Add latest fault state to motor status data structure

[mfussi66]

This PR aims to complement the work done to address: robotology/community#561 , in which the feature request involved communicating the latest fault to the yarpmotorgui.
To address it, the error code is stored in the free space of the struct motor_status_t, right after the diagnostics function sends the error towards the yarplogger.
The data is retrieved by embObjMotionControl upon periodical request of the yarpmotorgui, whenever a fault is detected.

The fault states that can be communicated are the following:

  {eoerror_value_MC_motor_external_fault,  "MC: exernal fault button pressed."},
    {eoerror_value_MC_motor_overcurrent,     "MC: overcurrent. The motor has been turned off to prevent it from being damaged by an impulsive spike of current. par16 = ID of joint."},
    {eoerror_value_MC_motor_i2t_limit,       "MC: i2t limit exceeded. The motor has been turned off to prevent it from being damaged by overheating due to a continuous high current. par16 = ID of joint."},
    {eoerror_value_MC_motor_hallsensors,     "MC: 2FOC hall sensors fault. Invalid sequence in motor Hall effect sensors, please check motor hall cable connections. par16 = ID of joint."},
    {eoerror_value_MC_motor_can_invalid_prot,"MC: 2FOC CAN invalid protocol. The EMS and 2FOC firmware versions are incompatible, please update. par16 = ID of joint."},
    {eoerror_value_MC_motor_can_generic,     "MC: 2FOC CAN generic error. Errors happened in the CAN bus between the EMS and the 2FOC board. par16 = ID of joint."},
    {eoerror_value_MC_motor_can_no_answer,   "MC: 2FOC CAN no answer. The communication between the EMS and the 2FOC board has been lost for more than 50 ms. par16 = ID of joint."},
    {eoerror_value_MC_axis_torque_sens,      "MC: torque sensor timeout. The joint is in a compliant interaction mode or torque control mode, and data from torque sensor have been unavailable for more than 100 ms. par16 = ID of joint."},
    {eoerror_value_MC_aea_abs_enc_invalid,   "MC: AEA encoder invalid data. Hardware problem in the magnetic position sensor of the joint caused invalid position readings. par16 = AEA port (msb) and ID of joint (lsb)."},
    {eoerror_value_MC_aea_abs_enc_timeout,   "MC: AEA encoder timeout. No answer from the magnetic position sensor of the joint (cable broken?). par16 = AEA port (msb) and ID of joint (lsb)."},
    {eoerror_value_MC_aea_abs_enc_spikes,    "MC: AEA encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. par16 = AEA port (msb) and ID of joint (lsb)."},
    {eoerror_value_MC_motor_qencoder_dirty,  "MC: 2FOC quadrature encoder dirty. The number of thicks in a complete revolution of the motor was lower than expected, the optical disks need to be cleaned. In par64 0xFF is the mask of raw encoder value. par16 = ID of joint."},
    {eoerror_value_MC_motor_qencoder_index,  "MC: 2FOC quadrature encoder index broken. The reference special thick was not detected during a complete revolution of the motor, please check motor encoder cables. In par64 0xFF is the mask of raw encoder value. par16 = ID of joint."},
    {eoerror_value_MC_motor_qencoder_phase,  "MC: 2FOC quadrature encoder phase broken. The motor encoder is not counting even if the motor is moving, please check motor encoder cables. In par64 0xFF is the mask of raw encoder value. par16 = ID of joint."},
    {eoerror_value_MC_generic_error,         "MC: generic motor error (see 64 bit mask parameter)."},
    {eoerror_value_MC_motor_wrong_state,     "MC: 2FOC wrong state. The 2FOC motor controller is in a control state different from required by the EMS. In par64 0xF0 is the mask of requested state, 0x0F is the mask of actual state. par16 = ID of joint."},
    {eoerror_value_MC_joint_hard_limit,      "MC: hard limit reached. The joint position is outside its hardware boundaries. par16 = ID of joint."},
    {eoerror_value_MC_motor_qencoder_phase_disappeared, "MC: qenc error has disappeared, warning counter has been reset."}

which belong to the motion controller category.
Here below is a little demo of the yarpmotorgui displaying a hardware limit fault:

144848405-89251081-02d5-4b19-8fec-f6ed8a2310ff.mp4

Related PR:
icub-firmware-shared: robotology/icub-firmware-shared#52
icub-main: icub-main: robotology/icub-main#779

[marcoaccame]

Hi @mfussi66, welcome to your first PR in icub-firmware.

The code is nice and clean. I would however ask you to two further efforts so that your first PR is just perfect.

A. Make the code safer to run by adding a protection vs potential deferencing a NULL pointer by applying the changes in here:

- eOmc_motor_status_t *mstatus = NULL;
- mstatus = eo_entities_GetMotorStatus(eo_entities_GetHandle(), id);
- mstatus->mc_fault_state = descriptor.code;
+ eOmc_motor_status_t *mstatus = eo_entities_GetMotorStatus(eo_entities_GetHandle(), id);
+ if(NULL != mstatus)
+ {
+     mstatus->mc_fault_state = descriptor.code;
+ }

It may seem pedantic but if you look at the code which use eo_entities_GetMotorStatus() you will see that the resulting pointer is always checked vs NULL. See for instance:

icub-firmware/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c

Lines 2482 to 2485 in 9073802

	if(NULL != (mstatus = eo_entities_GetMotorStatus(eo_entities_GetHandle(), jId)))
	{
	MController_get_motor_state(jId, mstatus);
	}

or:

icub-firmware/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c

Lines 1928 to 1932 in 9073802

	eOmc_motor_status_t *ms = eo_entities_GetMotorStatus(eo_entities_GetHandle(), j);
	eOmc_joint_status_t *js = eo_entities_GetJointStatus(eo_entities_GetHandle(), j);
	uint64_t amo_measure = s_eo_theappencreader.amodiag.vals[j];
	uint64_t mot_current = (NULL != ms) ? ms->basic.mot_current : 0;

B. Do some tests on the iCubGenova02 robot.
The reason is that in you PR of icub-main you now ask to broadcast as regulars a data structure eOmc_motor_status_t which is 4 bytes bigger than the previous one eOmc_motor_status_basic_t.

Now, in iCubGenova02 and other similar robot we have a ems board which manages 12 joints/motors plus skin patches. The free space in the payload of the UDP packet going to yarprobotinterface is almost over. I think that 4*12 = 48 bytes are still OK, but ... better to test the code on a true robot anyway.

[marcoaccame]

Hi @mfussi66, I have approved the previous changes on the code as they were OK.

But you also need to do the tests on the robot so I have copied the request in here again

B. Do some tests on the iCubGenova02 robot.
The reason is that in you PR of icub-main you now ask to broadcast as regulars a data structure eOmc_motor_status_t which is 4 bytes bigger than the previous one eOmc_motor_status_basic_t.

Now, in iCubGenova02 and other similar robot we have a ems board which manages 12 joints/motors plus skin patches. The free space in the payload of the UDP packet going to yarprobotinterface is almost over. I think that 4*12 = 48 bytes are still OK, but ... better to test the code on a true robot anyway.

However, I also add another change request.

I have noticed, better late than never, that you set the value of eOmc_motor_status_t::mc_fault_state to be a given value of eOerror_code_t when you send up a diagnostics message but...

• You don't initialize the value of eOmc_motor_status_t::mc_fault_state to be a neutral value which in the EoError.h file is indicated to be extern const eOerror_code_t eoerror_code_dummy (equal to 0xffffffff). The default value for it at startup is 0 which unluckily correspond to be a error of category eoerror_category_System and value eoerror_value_SYS_unspecified which will print the string "SYS: unspecified code. It may be that EOtheErrorManager is still called with a NULL eOerrmanDescriptor_t* param". So, we need to initialize at startup the value for each eOmc_motor_status_t::mc_fault_state to be eoerror_code_dummy otherwise the GUI will see the above string even when nothing has happened.
• You don't reset the value of eOmc_motor_status_t::mc_fault_state when the error condition goes away. The reset value is eoerror_code_dummy.

I will show you where to put code which:

• sets the correct values of eOmc_motor_status_t::mc_fault_state at startup,
• clear the values of eOmc_motor_status_t::mc_fault_state when the error condition is not present anymore.

[marcoaccame]

Hi @mfussi66, the changes in the code seem OK.

I also know that your further tests on your setup and on iCubGenova02 were successful.

So, let me attach some results for you:

testfault-2021-12-14_12.59.25.mp4

Figure. Tests on a dedicated setup which demonstrates how the yarpmotorgui prints the error code. This one has small edit which always prints the fault message for debugging purposes.

Figure. Tests on iCubGenova02: By keeping the actual robotology/devel but w/ changes in embObjMotionControl.cpp to ask the boards to broadcast the fatter eOmc_motor_status_t and not eOmc_motor_status_basic_t anymore, and w/ the new binary on the ems boards of the forearm, we have the system still working. The launch of yarprobotinterface is OK and yarpmotorgui correctly shows the joints.