Improve the AMO encoder diagnostic to make the user aware of specific AMO encoder problem

[MSECode]

Following the works done in #535 and collected in this EPIC: https://github.com/icub-tech-iit/study-encoders/issues/21 we have decided to exploit the information made available by the diagnostic bytes of the AMO encoder, i.e. the ones called status_0 and status_1, to make the user more aware of what is actually happing with the encoder when the problems taken into account during the previous issues happen.

The diagnostic is fully described by these 2 tables available in the AMO datasheet.

[MSECode]

I've started the work at this branch: https://github.com/MSECode/icub-firmware/commits/feature/updateAMOEncDiagnostic/

@valegagge, I've a doubt about using the embot library to check which bits of the diagnostic byte are set to 1. However, in order to do so we need to include embot_core_binary.h in EOappEncodersReader.c. Do u think that this might be an issue or is it ok?
Moreover, what is now just missing is:

• adding the specific errors to the error class: eoerror_category_HardWare and decide which we wanna consider as blocking error and which as just warning
• identify thresholds for stopping the joint if the number of diagnostic fails increases too much
• update the diagnostic in icub-main
• decide if we wanna add some fields to this enum hal_spiencoder_diagnostic_type_t or create a new one for the specific AMO failures

[valegagge]

Hi @MSECode,

regarding the including of embot_core_binary.h at first check it seems ok, but i don't know if it creates a inclusion loop.

adding the specific errors to the error class: eoerror_category_HardWare and decide which we wanna consider as blocking error and which as just warning

Here, since you carried on the AMO study, you could suggest something.

identify thresholds for stopping the joint if the number of diagnostic fails increases too much

Currently, if there is a very critical case, I'd like to add a fault.

decide if we wanna add some fields to this enum hal_spiencoder_diagnostic_type_t or create a new one for the specific AMO failures

You can suggest a solution that best fits the fw and icub-main if such errors are shared with the diagnostic at high level.

[MSECode]

Now, just to test if we can correctly read the different diagnostic bits I've added some debug lines, which are sent to the yarprobointerface every 10 seconds if we have errors. Doing so, I've run a test with the joint with the threeEncoders that was moving between -180 and +180 degrees in position mode using the AMO encoder only calibrated with calibration type 12.
At a certain point I've moved the encoder boards slightly far from the magnetic disk and this is a part of the output I got:

[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=535s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[DEBUG] from BOARD 10.0.1.2 (three-encoders-setup) time=535s 209m 77u :  src LOCAL, adr 0,(code 0x04000003, par16 0x0000 par64 0x0000000000000000) -> DEBUG: tag03  Poor level or clipping in both master and nonius
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=536s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=537s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[ERROR] from BOARD 10.0.1.2 (three-encoders-setup) time=537s 396m 146u :  MC: hard limit reached. The joint position is outside its hardware boundaries. (Joint=three_encoders_joint (NIB=0))
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=538s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=539s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=540s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=541s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=542s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[ERROR] from BOARD 10.0.1.2 (three-encoders-setup) time=542s 397m 144u :  MC: hard limit reached. The joint position is outside its hardware boundaries. (Joint=three_encoders_joint (NIB=0))
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=543s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[ERROR] positionMoveRaw: skipping command because  BOARD three-encoders-setup (IP 10.0.1.2)   joint  0  is not in VOCAB_CM_POSITION mode
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=544s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=545s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[DEBUG] from BOARD 10.0.1.2 (three-encoders-setup) time=545s 210m 73u :  src LOCAL, adr 0,(code 0x04000007, par16 0x0000 par64 0x0000000000000008) -> DEBUG: tag07  All error bits are risen in the par64 value
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=546s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=547s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[ERROR] from BOARD 10.0.1.2 (three-encoders-setup) time=547s 398m 144u :  MC: hard limit reached. The joint position is outside its hardware boundaries. (Joint=three_encoders_joint (NIB=0))
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=548s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=549s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=550s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=551s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=552s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[ERROR] from BOARD 10.0.1.2 (three-encoders-setup) time=552s 399m 144u :  MC: hard limit reached. The joint position is outside its hardware boundaries. (Joint=three_encoders_joint (NIB=0))
[WARNING] from BOARD 10.0.1.2 (three-encoders-setup) time=553s 158m 92u :  MC: Absolute encoder has spikes. There is impulsive noise in the measures of the magnetic position sensor of the joint. (Joint=three_encoders_joint (NIB=0), encoderPort=0, encoderType=AMO)
[ERROR] positionMoveRaw: skipping command because  BOARD three-encoders-setup (IP 10.0.1.2)   joint  0  is not in VOCAB_CM_POSITION mode

Among the WARNING messages you can see the DEBUG messages we used for checking if the bit of the diagnostic are correctly captured.

I've written the code so that if you see:

• DEBUG: tag01, DEBUG: tag02 and DEBUG: tag03 --> these are related to the errors of status 0
• DEBUG: tag07 --> related to errors of status 1. In par64 you have the hex number for the bits that are set to 1.

[MSECode]

Navigating the code, I've found that a similar work was previously done with these functions: https://github.com/robotology/icub-firmware/blob/master/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c#L1984-L2143

However, I suppose that the code in those lines it is not currently used since I think that can be enabled by the flag that we have in icub-main: eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.config.mode.
Anyway, I was thinking to add the part of my code that rise different errors depending on the values in the registries status 0 and status 1` to those functions.
I thought to that since those methods are already well organized and managed. On top of that we can or remove the flag in icub-main or re-enable the AMO diagnostic.
What do u think @valegagge ? Does it make sense to you or should I proceed differently.
Once that is done, I just need to add the error and warning messages and the threshold but the part that reads the bits of the registry values is almost complete.