forked from Archive/PX4-Autopilot
sensors: Calculate and publish magnetometer inconsistency
This commit is contained in:
parent
c7097085fa
commit
60a68d30c7
|
@ -621,8 +621,8 @@ Sensors::run()
|
|||
|
||||
/* advertise the sensor_preflight topic and make the initial publication */
|
||||
preflt.accel_inconsistency_m_s_s = 0.0f;
|
||||
|
||||
preflt.gyro_inconsistency_rad_s = 0.0f;
|
||||
preflt.mag_inconsistency_ga = 0.0f;
|
||||
|
||||
_sensor_preflight = orb_advertise(ORB_ID(sensor_preflight), &preflt);
|
||||
|
||||
|
@ -686,6 +686,7 @@ Sensors::run()
|
|||
if (!_armed) {
|
||||
_voted_sensors_update.calc_accel_inconsistency(preflt);
|
||||
_voted_sensors_update.calc_gyro_inconsistency(preflt);
|
||||
_voted_sensors_update.calc_mag_inconsistency(preflt);
|
||||
orb_publish(ORB_ID(sensor_preflight), _sensor_preflight, &preflt);
|
||||
|
||||
}
|
||||
|
|
|
@ -62,6 +62,7 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_en
|
|||
memset(&_last_baro_timestamp, 0, sizeof(_last_baro_timestamp));
|
||||
memset(&_accel_diff, 0, sizeof(_accel_diff));
|
||||
memset(&_gyro_diff, 0, sizeof(_gyro_diff));
|
||||
memset(&_mag_diff, 0, sizeof(_mag_diff));
|
||||
|
||||
// initialise the corrections
|
||||
memset(&_corrections, 0, sizeof(_corrections));
|
||||
|
@ -1210,4 +1211,51 @@ void VotedSensorsUpdate::calc_gyro_inconsistency(sensor_preflight_s &preflt)
|
|||
}
|
||||
}
|
||||
|
||||
void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt)
|
||||
{
|
||||
float mag_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
|
||||
unsigned check_index = 0; // the number of sensors the primary has been checked against
|
||||
|
||||
// Check each sensor against the primary
|
||||
for (unsigned sensor_index = 0; sensor_index < _mag.subscription_count; sensor_index++) {
|
||||
|
||||
// check that the sensor we are checking against is not the same as the primary
|
||||
if ((_mag.priority[sensor_index] > 0) && (sensor_index != _mag.last_best_vote)) {
|
||||
|
||||
float mag_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary
|
||||
|
||||
// calculate mag_diff_sum_sq for the specified sensor against the primary
|
||||
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
|
||||
_mag_diff[axis_index][check_index] = 0.95f * _mag_diff[axis_index][check_index] + 0.05f *
|
||||
(_last_sensor_data[_mag.last_best_vote].magnetometer_ga[axis_index] -
|
||||
_last_sensor_data[sensor_index].magnetometer_ga[axis_index]);
|
||||
mag_diff_sum_sq += _mag_diff[axis_index][check_index] * _mag_diff[axis_index][check_index];
|
||||
|
||||
}
|
||||
|
||||
// capture the largest sum value
|
||||
if (mag_diff_sum_sq > mag_diff_sum_max_sq) {
|
||||
mag_diff_sum_max_sq = mag_diff_sum_sq;
|
||||
|
||||
}
|
||||
|
||||
// increment the check index
|
||||
check_index++;
|
||||
}
|
||||
|
||||
// check to see if the maximum number of checks has been reached and break
|
||||
if (check_index >= 2) {
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// skip check if less than 2 sensors
|
||||
if (check_index < 1) {
|
||||
preflt.mag_inconsistency_ga = 0.0f;
|
||||
|
||||
} else {
|
||||
// get the vector length of the largest difference and write to the combined sensor struct
|
||||
preflt.mag_inconsistency_ga = sqrtf(mag_diff_sum_max_sq);
|
||||
}
|
||||
}
|
|
@ -142,7 +142,12 @@ public:
|
|||
/**
|
||||
* Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
|
||||
*/
|
||||
void calc_gyro_inconsistency(struct sensor_preflight_s &preflt);
|
||||
void calc_gyro_inconsistency(sensor_preflight_s &preflt);
|
||||
|
||||
/**
|
||||
* Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
|
||||
*/
|
||||
void calc_mag_inconsistency(sensor_preflight_s &preflt);
|
||||
|
||||
private:
|
||||
|
||||
|
@ -263,6 +268,7 @@ private:
|
|||
|
||||
float _accel_diff[3][2]; /**< filtered accel differences between IMU units (m/s/s) */
|
||||
float _gyro_diff[3][2]; /**< filtered gyro differences between IMU uinits (rad/s) */
|
||||
float _mag_diff[3][2]; /**< filtered mag differences between sensor instances (Ga) */
|
||||
|
||||
/* sensor thermal compensation */
|
||||
TemperatureCompensation _temperature_compensation;
|
||||
|
|
Loading…
Reference in New Issue