static void s_eo_currents_watchdog_CheckI2T(uint8_t motor, int16_t value)
{
//change sign to check absolute value
if (value < 0)
value = -value;
uint32_t averageCurrent = s_eo_currents_watchdog_averageCalc_addValue( motor, value);
if(!s_eo_currents_watchdog_averageCalc_collectDataIsCompleted(motor))
{
return;
}
// 1) calculate Ep
float Ep_aux= (averageCurrent*averageCurrent) - s_eo_currents_watchdog.nominalCurrent2[motor];
s_eo_currents_watchdog.accomulatorEp[motor] += Ep_aux;
if(s_eo_currents_watchdog.accomulatorEp[motor] < 0) //Ep could not be smaller than zero
{
s_eo_currents_watchdog.accomulatorEp[motor] = 0;
}
// 2) check if current Ep is bigger than threshold then rais fault
if( s_eo_currents_watchdog.accomulatorEp[motor] > s_eo_currents_watchdog.I2T_threshold[motor])
{
s_eo_currents_watchdog.motorinI2Tfault[motor] = eobool_true;
MController_motor_raise_fault_i2t(motor);
}
else
{
//I need to raise I2T fault until the current system energy is not littler than I2T threshold divided 2. (we decided so....)
if(s_eo_currents_watchdog.motorinI2Tfault[motor])
{
if(s_eo_currents_watchdog.accomulatorEp[motor] > (s_eo_currents_watchdog.I2T_threshold[motor]/2))
{
MController_motor_raise_fault_i2t(motor);
}
else
{
eOerrmanDescriptor_t errdes = {0};
errdes.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag00);
errdes.par16 = motor;
errdes.sourcedevice = eo_errman_sourcedevice_localboard;
errdes.sourceaddress = 0;
char str[100];
snprintf(str, sizeof(str), "Ep < I2T/2: now it is possible put in idle the motor");
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, str, NULL, &errdes);
s_eo_currents_watchdog.motorinI2Tfault[motor] = eobool_false;
}
}
}
// 3) reset average data
s_eo_currents_watchdog_averageCalc_reset(motor);
}
static void s_eo_currents_watchdog_CheckI2T(uint8_t motor, int16_t value)
{
// apply a simple LOW-PASS filter and if the value is above a threshold signal the error
// IMPLEMENTATION OF LOW-PASSFILTER TO CHECK I2T INSIDE 2FOC FW
/*
if (!MotorConfig.has_tsens)
{
//static const long I2T_LIMIT = ((long)(sMaxCurrent/100)*(long)(sMaxCurrent/100))*(UDEF_I2T_LIMIT*UDEF_I2T_LIMIT);
long I2 = __builtin_mulss(I2Tdata.IQMeasured,I2Tdata.IQMeasured);
I2 -= I2Tacc;
I2 >>= 15;
I2Tacc += I2;
if (I2Tacc > sI2Tlimit)
{
//The temperature grew too much. Protect!
SysError.I2TFailure = TRUE;
FaultConditionsHandler();
}
}
*/
//int32_t i2 = (int32_t) (value * value);
//s_eo_currents_watchdog.filter_reg[motor] += (float) (i2 - s_eo_currents_watchdog.filter_reg[joint]) / FILTER_WINDOW;
//change sign to check absolute value
if (value < 0)
value = -value;
uint32_t averageCurrent = s_eo_currents_watchdog_averageCalc_addValue( motor, value);
if(!s_eo_currents_watchdog_averageCalc_collectDataIsCompleted(motor))
{
return;
}
// 1) calculate Ep
s_eo_currents_watchdog.accomulatorEp[motor] += (averageCurrent*averageCurrent) - s_eo_currents_watchdog.nominalCurrent2[motor];
// 2) check if current Ep is bigger than threshold
if( s_eo_currents_watchdog.accomulatorEp[motor] > s_eo_currents_watchdog.I2T_threshold[motor])
{
//signal the I2T error to EOemsController
uint32_t current_state = eo_mcserv_GetMotorFaultMask(eo_mcserv_GetHandle(),motor);
if((current_state & MOTOR_I2T_LIMIT_FAULT) == 0) //I2T fault bit not set
{
//simulate the CANframe used by 2FOC to signal the status
uint64_t fault_mask = (((uint64_t)(current_state | MOTOR_I2T_LIMIT_FAULT)) << 32) & 0xFFFFFFFF00000000; //adding the error to the current state
fault_mask |= icubCanProto_controlmode_hwFault; //setting the hard fault of the motor
eo_motor_set_motor_status( eo_motors_GetHandle(), motor, (uint8_t*)&fault_mask);
}
}
// 3) reset average data
s_eo_currents_watchdog_averageCalc_reset(motor);
}
