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); }