Imu health reporting and preflight checks improvements

[dusan19]

Main issues:

1. Gyro, accel, mag and baro health is evaluated by different modules (sensors, commander) and each of them set the health bitmask inconsistently, furthermore each of them overrides the health reported by the other.
2. Preflight checks do not consider sensor failures reported by the sensors module

More detailed explanation and problem examples:

1. Sensors health bitmask is set in commander::status.onboard_control_sensors_health. It is set by preflight tests function, and by the health reported on the subsystem_info topic by sensors module.
   Preflight checks set the bit in the health bitmask that matches the sensor instance. For example SUBSYSTEM_TYPE_GYRO bit is set for gyro 0, and SUBSYSTEM_TYPE_GYRO2 for gyro 1. On the other hand sensors module sets the bits considering the number of healthy sensors. Which means that in case of a gyro 0 failure, commander would set SUBSYSTEM_TYPE_GYRO to false, and sensors would set SUBSYSTEM_TYPE_GYRO2 to false. That behavior is not consistent.

• Solution: sensors module should also set the health of a specific sensor according to its index and not to the number of healthy sensors. This way we know which sensor failed by looking at the vehicle_status message. Especially useful when using external sensors, to be able to know which one to replace.
  Furthermore, the health reported by different modules should be passed through a "logical and" function to get the final health. If any module reports a sensor failure, the sensor is flagged as failed.

2. Preflight checks do not consider health reported by sensors module (no data, stale data, timeout, high error count, high error density).
   I could reproduce the following scenario: I set the required number of healthy sensors to 2 instead of default 1, enabled 2 mags, and had stale data on the second mag. The checks passed even though the second mag was broken.

• Solution: use the health reported by the sensors module together with the existing health checks in preflight tests.

Other fixes and improvements:

• Commander runs preflight tests every 2 seconds or when the health message is received, if the vehicle is unarmed. This way we have up to date health checks, and flight readiness, which can be reported over mavlink to the base station such that the user is informed about the system state and failures all the time and not only when arming is requested. Or be used otherwise.

• I encountered a bug where I had 1 mag enabled but got mag sensors inconsistency failure. This happened because mag 2 was disabled but still had priority set to 100 instead of zero. Mag enabled is initalized to true and overridden by the parameter setting inVotedSensorsUpdate::parametersUpdate(), but only if this sensors uorb instance was published. Due to race conditions this didnt happen before the initial parameter update, but after mag poll was called, so in VotedSensorsUpdate::magPoll since the enable was true, the priority 100 was assigned to the sensor and it was checked in the inconsistency test.
  Solution: sensors enabled array is initialized to false. (priority is already 0 initially, and so should the enabled flags be).

[julianoes]

@dusan19 thanks for the pull request. I will try to review it as soon as possible!

[dusan19]

Maybe we should give commander some time to initialize and subscribe to subsystem_info since this is published on change thus failures might get dropped
for instance:
if (_sensors_health != new_sensors_health && hrt_absolute_time > 10_s) {

[dagar]

In general I think we'd be better to push the specifics of the sensor checks to the sensor module itself and then only publish a higher level summary. Then in commander arming and failsafes can act on this simple information at the state machine level. Sensors would also be in a good position to continuously publish more of the health checks for logging purposes.

[dusan19]

@dagar I agree, preflight checks in commander shouldn't subscribe to different sensors topics published by the drivers, but only to the relevant topics from the sensors module.
I will push an another PR today, with the proposed changes. Also I found some related bugs in the sensors module and the commander and will include those fixes as well

[dusan19]

check out #13025

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[LorenzMeier]

Unfortunately this is stale and I'm closing it accordingly. We still would want to get this functionality in, but based on the recent improvements that Daniel did on the estimatior.