forked from Archive/PX4-Autopilot
sensors: sensor_preflight_imu -> sensors_status_imu and run continuously
- inconsistency checks now run continuously instead of only preflight - keep inconsistencies for all sensors - add per sensor data validator state as overall health flag
This commit is contained in:
Daniel Agar
GitHub
parent
d4ecf24bf2
commit
60d613ea04
|
|
@ -862,6 +862,7 @@ void statusFTDI() {
|
|||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro_fifo"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_mag"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensors_status_imu"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener px4io_status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener system_power"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_air_data"'
|
||||
|
|
@ -934,6 +935,7 @@ void statusSEGGER() {
|
|||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro_fifo"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_mag"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensors_status_imu"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener px4io_status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener system_power"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_air_data"'
|
||||
|
|
|
|||
|
|
@ -146,7 +146,7 @@ def get_gps_check_fail_flags(estimator_status: dict) -> dict:
|
|||
return gps_fail_flags
|
||||
|
||||
|
||||
def magnetic_field_estimates_from_status(estimator_states: dict) -> Tuple[float, float, float]:
|
||||
def magnetic_field_estimates_from_states(estimator_states: dict) -> Tuple[float, float, float]:
|
||||
"""
|
||||
|
||||
:param estimator_states:
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@ import numpy as np
|
|||
from matplotlib.backends.backend_pdf import PdfPages
|
||||
from pyulog import ULog
|
||||
|
||||
from analysis.post_processing import magnetic_field_estimates_from_status, get_estimator_check_flags
|
||||
from analysis.post_processing import magnetic_field_estimates_from_states, get_estimator_check_flags
|
||||
from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \
|
||||
CheckFlagsPlot
|
||||
from analysis.detectors import PreconditionError
|
||||
|
|
@ -34,6 +34,12 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
|||
except:
|
||||
raise PreconditionError('could not find estimator_status data')
|
||||
|
||||
try:
|
||||
estimator_states = ulog.get_dataset('estimator_states').data
|
||||
print('found estimator_states data')
|
||||
except:
|
||||
raise PreconditionError('could not find estimator_states data')
|
||||
|
||||
try:
|
||||
estimator_innovations = ulog.get_dataset('estimator_innovations').data
|
||||
estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').data
|
||||
|
|
@ -45,12 +51,6 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
|||
except:
|
||||
raise PreconditionError('could not find innovation data')
|
||||
|
||||
try:
|
||||
sensor_preflight_imu = ulog.get_dataset('sensor_preflight_imu').data
|
||||
print('found sensor_preflight data')
|
||||
except:
|
||||
raise PreconditionError('could not find sensor_preflight_imu data')
|
||||
|
||||
control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)
|
||||
|
||||
status_time = 1e-6 * estimator_status['timestamp']
|
||||
|
|
@ -60,17 +60,6 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
|||
|
||||
with PdfPages(output_plot_filename) as pdf_pages:
|
||||
|
||||
# plot IMU consistency data
|
||||
if ('accel_inconsistency_m_s_s' in sensor_preflight_imu.keys()) and (
|
||||
'gyro_inconsistency_rad_s' in sensor_preflight_imu.keys()):
|
||||
data_plot = TimeSeriesPlot(
|
||||
sensor_preflight_imu, [['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']],
|
||||
x_labels=['data index', 'data index'],
|
||||
y_labels=['acceleration (m/s/s)', 'angular rate (rad/s)'],
|
||||
plot_title='IMU Consistency Check Levels', pdf_handle=pdf_pages)
|
||||
data_plot.save()
|
||||
data_plot.close()
|
||||
|
||||
# vertical velocity and position innovations
|
||||
data_plot = InnovationPlot(
|
||||
innovation_data, [('gps_vpos', 'var_gps_vpos'),
|
||||
|
|
@ -292,10 +281,10 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
|||
data_plot.close()
|
||||
|
||||
# Plot the earth frame magnetic field estimates
|
||||
declination, field_strength, inclination = magnetic_field_estimates_from_status(
|
||||
estimator_status)
|
||||
declination, field_strength, inclination = magnetic_field_estimates_from_states(
|
||||
estimator_states)
|
||||
data_plot = CheckFlagsPlot(
|
||||
1e-6 * estimator_status['timestamp'],
|
||||
1e-6 * estimator_states['timestamp'],
|
||||
{'strength': field_strength, 'declination': declination, 'inclination': inclination},
|
||||
[['declination'], ['inclination'], ['strength']],
|
||||
x_label='time (sec)', y_labels=['declination (deg)', 'inclination (deg)',
|
||||
|
|
@ -307,7 +296,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
|||
|
||||
# Plot the body frame magnetic field estimates
|
||||
data_plot = CheckFlagsPlot(
|
||||
1e-6 * estimator_status['timestamp'], estimator_status,
|
||||
1e-6 * estimator_states['timestamp'], estimator_states,
|
||||
[['states[19]'], ['states[20]'], ['states[21]']],
|
||||
x_label='time (sec)', y_labels=['X (Gauss)', 'Y (Gauss)', 'Z (Gauss)'],
|
||||
plot_title='Magnetometer Bias Estimates', annotate=False, pdf_handle=pdf_pages)
|
||||
|
|
@ -316,7 +305,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
|||
|
||||
# Plot the EKF wind estimates
|
||||
data_plot = CheckFlagsPlot(
|
||||
1e-6 * estimator_status['timestamp'], estimator_status,
|
||||
1e-6 * estimator_states['timestamp'], estimator_states,
|
||||
[['states[22]'], ['states[23]']], x_label='time (sec)',
|
||||
y_labels=['North (m/s)', 'East (m/s)'], plot_title='Wind Velocity Estimates',
|
||||
annotate=False, pdf_handle=pdf_pages)
|
||||
|
|
|
|||
|
|
@ -120,9 +120,9 @@ set(msg_files
|
|||
sensor_gyro.msg
|
||||
sensor_gyro_fifo.msg
|
||||
sensor_mag.msg
|
||||
sensor_preflight_imu.msg
|
||||
sensor_preflight_mag.msg
|
||||
sensor_selection.msg
|
||||
sensors_status_imu.msg
|
||||
subsystem_info.msg
|
||||
system_power.msg
|
||||
task_stack_info.msg
|
||||
|
|
|
|||
|
|
@ -1,7 +0,0 @@
|
|||
#
|
||||
# Pre-flight sensor check metrics. These will be zero if the vehicle only has one sensor.
|
||||
# The topic will not be updated when the vehicle is armed
|
||||
#
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s).
|
||||
float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s).
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
#
|
||||
# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated.
|
||||
#
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint32 accel_device_id_primary # current primary accel device id for reference
|
||||
|
||||
uint32[3] accel_device_ids
|
||||
float32[3] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and primary in (m/s/s).
|
||||
bool[3] accel_healthy
|
||||
|
||||
|
||||
uint32 gyro_device_id_primary # current primary gyro device id for reference
|
||||
|
||||
uint32[3] gyro_device_ids
|
||||
float32[3] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and primary in (rad/s).
|
||||
bool[3] gyro_healthy
|
||||
|
|
@ -144,7 +144,7 @@ rtps:
|
|||
id: 67
|
||||
- msg: sensor_mag
|
||||
id: 68
|
||||
- msg: sensor_preflight_imu
|
||||
- msg: sensors_status_imu
|
||||
id: 69
|
||||
- msg: sensor_selection
|
||||
id: 70
|
||||
|
|
|
|||
|
|
@ -38,53 +38,88 @@
|
|||
#include <lib/parameters/param.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/sensor_preflight_imu.h>
|
||||
#include <uORB/topics/sensors_status_imu.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
|
||||
bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
||||
const bool report_status)
|
||||
{
|
||||
float test_limit = 1.0f; // pass limit re-used for each test
|
||||
|
||||
// Get sensor_preflight data if available and exit with a fail recorded if not
|
||||
uORB::SubscriptionData<sensor_preflight_imu_s> sensors_sub{ORB_ID(sensor_preflight_imu)};
|
||||
sensors_sub.update();
|
||||
const sensor_preflight_imu_s &sensors = sensors_sub.get();
|
||||
uORB::SubscriptionData<sensors_status_imu_s> sensors_status_imu_sub{ORB_ID(sensors_status_imu)};
|
||||
const sensors_status_imu_s &imu = sensors_status_imu_sub.get();
|
||||
|
||||
// Use the difference between IMU's to detect a bad calibration.
|
||||
// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
|
||||
param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
|
||||
float accel_test_limit = 1.f;
|
||||
param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit);
|
||||
|
||||
if (sensors.accel_inconsistency_m_s_s > test_limit) {
|
||||
if (report_status) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accels inconsistent - Check Cal");
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
|
||||
}
|
||||
for (unsigned i = 0; i < (sizeof(imu.accel_inconsistency_m_s_s) / sizeof(imu.accel_inconsistency_m_s_s[0])); i++) {
|
||||
if (imu.accel_device_ids[i] != 0) {
|
||||
if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, imu.accel_healthy[i], status);
|
||||
|
||||
return false;
|
||||
} else {
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, imu.accel_healthy[i], status);
|
||||
}
|
||||
|
||||
} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) {
|
||||
if (report_status) {
|
||||
mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accels inconsistent - Check Cal");
|
||||
const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i];
|
||||
|
||||
if (accel_inconsistency_m_s_s > accel_test_limit) {
|
||||
if (report_status) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u inconsistent - Check Cal", i);
|
||||
|
||||
if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
|
||||
|
||||
} else {
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
|
||||
} else if (accel_inconsistency_m_s_s > accel_test_limit * 0.8f) {
|
||||
if (report_status) {
|
||||
mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accel %u inconsistent - Check Cal", i);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
|
||||
param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
|
||||
float gyro_test_limit = 1.f;
|
||||
param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit);
|
||||
|
||||
if (sensors.gyro_inconsistency_rad_s > test_limit) {
|
||||
if (report_status) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyros inconsistent - Check Cal");
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
|
||||
}
|
||||
for (unsigned i = 0; i < (sizeof(imu.gyro_inconsistency_rad_s) / sizeof(imu.gyro_inconsistency_rad_s[0])); i++) {
|
||||
if (imu.gyro_device_ids[i] != 0) {
|
||||
if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, imu.accel_healthy[i], status);
|
||||
|
||||
return false;
|
||||
} else {
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, imu.accel_healthy[i], status);
|
||||
}
|
||||
|
||||
} else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) {
|
||||
if (report_status) {
|
||||
mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyros inconsistent - Check Cal");
|
||||
const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i];
|
||||
|
||||
if (gyro_inconsistency_rad_s > gyro_test_limit) {
|
||||
if (report_status) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u inconsistent - Check Cal", i);
|
||||
|
||||
if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
|
||||
|
||||
} else {
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
|
||||
} else if (gyro_inconsistency_rad_s > gyro_test_limit * 0.5f) {
|
||||
if (report_status) {
|
||||
mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyro %u inconsistent - Check Cal", i);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -80,7 +80,7 @@ void LoggedTopics::add_default_topics()
|
|||
add_topic("safety");
|
||||
add_topic("sensor_combined");
|
||||
add_topic("sensor_correction");
|
||||
add_topic("sensor_preflight_imu", 200);
|
||||
add_topic("sensors_status_imu", 200);
|
||||
add_topic("sensor_preflight_mag", 500);
|
||||
add_topic("sensor_selection");
|
||||
add_topic("system_power", 500);
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@
|
|||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_preflight_imu.h>
|
||||
#include <uORB/topics/sensors_status_imu.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_imu.h>
|
||||
|
|
@ -115,7 +115,6 @@ private:
|
|||
hrt_abstime _sensor_combined_prev_timestamp{0};
|
||||
|
||||
sensor_combined_s _sensor_combined{};
|
||||
sensor_preflight_imu_s _sensor_preflight_imu{};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[3] {
|
||||
{this, ORB_ID(vehicle_imu), 0},
|
||||
|
|
@ -130,7 +129,6 @@ private:
|
|||
|
||||
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
|
||||
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
|
||||
uORB::Publication<sensor_preflight_imu_s> _sensor_preflight_imu_pub{ORB_ID(sensor_preflight_imu)};
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
|
|
@ -584,16 +582,6 @@ void Sensors::Run()
|
|||
_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
|
||||
_sensor_pub.publish(_sensor_combined);
|
||||
_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
|
||||
|
||||
// If the the vehicle is disarmed calculate the length of the maximum difference between
|
||||
// IMU units as a consistency metric and publish to the sensor preflight topic
|
||||
if (!_armed) {
|
||||
_voted_sensors_update.calcAccelInconsistency(_sensor_preflight_imu);
|
||||
_voted_sensors_update.calcGyroInconsistency(_sensor_preflight_imu);
|
||||
|
||||
_sensor_preflight_imu.timestamp = hrt_absolute_time();
|
||||
_sensor_preflight_imu_pub.publish(_sensor_preflight_imu);
|
||||
}
|
||||
}
|
||||
|
||||
// keep adding sensors as long as we are not armed,
|
||||
|
|
@ -725,7 +713,7 @@ The provided functionality includes:
|
|||
- Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or
|
||||
on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the
|
||||
sensor drivers must already be running when `sensors` is started.
|
||||
- Do preflight sensor consistency checks and publish the `sensor_preflight_imu` topic.
|
||||
- Do sensor consistency checks and publish the `sensors_status_imu` topic.
|
||||
|
||||
### Implementation
|
||||
It runs in its own thread and polls on the currently selected gyro topic.
|
||||
|
|
|
|||
|
|
@ -187,8 +187,8 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
|
|||
_accel.voter.get_best(hrt_absolute_time(), &accel_best_index);
|
||||
_gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index);
|
||||
|
||||
checkFailover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC);
|
||||
checkFailover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
|
||||
checkFailover(_accel, "Accel");
|
||||
checkFailover(_gyro, "Gyro");
|
||||
|
||||
// write data for the best sensor to output variables
|
||||
if ((accel_best_index >= 0) && (gyro_best_index >= 0)) {
|
||||
|
|
@ -229,7 +229,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
|
|||
}
|
||||
}
|
||||
|
||||
bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type)
|
||||
bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name)
|
||||
{
|
||||
if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) {
|
||||
|
||||
|
|
@ -261,34 +261,6 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na
|
|||
|
||||
// reduce priority of failed sensor to the minimum
|
||||
sensor.priority[failover_index] = 1;
|
||||
|
||||
int ctr_valid = 0;
|
||||
|
||||
for (uint8_t i = 0; i < sensor.subscription_count; i++) {
|
||||
if (sensor.priority[i] > 1) {
|
||||
ctr_valid++;
|
||||
}
|
||||
}
|
||||
|
||||
if (ctr_valid < 2) {
|
||||
if (ctr_valid == 0) {
|
||||
// Zero valid sensors remain! Set even the primary sensor health to false
|
||||
_info.subsystem_type = type;
|
||||
|
||||
} else if (ctr_valid == 1) {
|
||||
// One valid sensor remains, set secondary sensor health to false
|
||||
if (type == subsystem_info_s::SUBSYSTEM_TYPE_GYRO) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; }
|
||||
|
||||
if (type == subsystem_info_s::SUBSYSTEM_TYPE_ACC) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
|
||||
}
|
||||
|
||||
_info.timestamp = hrt_absolute_time();
|
||||
_info.present = true;
|
||||
_info.enabled = true;
|
||||
_info.ok = false;
|
||||
|
||||
_info_pub.publish(_info);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -357,8 +329,41 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
|
|||
_selection.timestamp = hrt_absolute_time();
|
||||
_sensor_selection_pub.publish(_selection);
|
||||
_selection_changed = false;
|
||||
|
||||
for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) {
|
||||
_accel_diff[sensor_index].zero();
|
||||
}
|
||||
|
||||
for (int sensor_index = 0; sensor_index < GYRO_COUNT_MAX; sensor_index++) {
|
||||
_gyro_diff[sensor_index].zero();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
calcAccelInconsistency();
|
||||
calcGyroInconsistency();
|
||||
|
||||
sensors_status_imu_s status{};
|
||||
status.accel_device_id_primary = _selection.accel_device_id;
|
||||
status.gyro_device_id_primary = _selection.gyro_device_id;
|
||||
|
||||
for (int i = 0; i < SENSOR_COUNT_MAX; i++) {
|
||||
if (_accel_device_id[i] != 0) {
|
||||
status.accel_device_ids[i] = _accel_device_id[i];
|
||||
status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm();
|
||||
status.accel_healthy[i] = (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
|
||||
}
|
||||
|
||||
if (_gyro_device_id[i] != 0) {
|
||||
status.gyro_device_ids[i] = _gyro_device_id[i];
|
||||
status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm();
|
||||
status.gyro_healthy[i] = (_gyro.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
status.timestamp = hrt_absolute_time();
|
||||
_sensors_status_imu_pub.publish(status);
|
||||
}
|
||||
|
||||
void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
|
||||
|
|
@ -369,100 +374,34 @@ void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
|
|||
}
|
||||
}
|
||||
|
||||
void VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_imu_s &preflt)
|
||||
void VotedSensorsUpdate::calcAccelInconsistency()
|
||||
{
|
||||
float accel_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
|
||||
const Vector3f primary_accel{_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2};
|
||||
|
||||
// Check each sensor against the primary
|
||||
for (int sensor_index = 0; sensor_index < _accel.subscription_count; sensor_index++) {
|
||||
|
||||
for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) {
|
||||
// check that the sensor we are checking against is not the same as the primary
|
||||
if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) {
|
||||
if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0)
|
||||
&& (_accel_device_id[sensor_index] != _selection.accel_device_id)) {
|
||||
|
||||
float accel_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison agains the primary
|
||||
|
||||
// calculate accel_diff_sum_sq for the specified sensor against the primary
|
||||
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
|
||||
_accel_diff[axis_index][check_index] = 0.95f * _accel_diff[axis_index][check_index] + 0.05f *
|
||||
(_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2[axis_index] -
|
||||
_last_sensor_data[sensor_index].accelerometer_m_s2[axis_index]);
|
||||
accel_diff_sum_sq += _accel_diff[axis_index][check_index] * _accel_diff[axis_index][check_index];
|
||||
|
||||
}
|
||||
|
||||
// capture the largest sum value
|
||||
if (accel_diff_sum_sq > accel_diff_sum_max_sq) {
|
||||
accel_diff_sum_max_sq = accel_diff_sum_sq;
|
||||
|
||||
}
|
||||
|
||||
// increment the check index
|
||||
check_index++;
|
||||
const Vector3f current_accel{_last_sensor_data[sensor_index].accelerometer_m_s2};
|
||||
_accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (primary_accel - current_accel);
|
||||
}
|
||||
|
||||
// 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.accel_inconsistency_m_s_s = 0.0f;
|
||||
|
||||
} else {
|
||||
// get the vector length of the largest difference and write to the combined sensor struct
|
||||
preflt.accel_inconsistency_m_s_s = sqrtf(accel_diff_sum_max_sq);
|
||||
}
|
||||
}
|
||||
|
||||
void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_imu_s &preflt)
|
||||
void VotedSensorsUpdate::calcGyroInconsistency()
|
||||
{
|
||||
float gyro_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
|
||||
const Vector3f primary_gyro{_last_sensor_data[_gyro.last_best_vote].gyro_rad};
|
||||
|
||||
// Check each sensor against the primary
|
||||
for (int sensor_index = 0; sensor_index < _gyro.subscription_count; sensor_index++) {
|
||||
|
||||
for (int sensor_index = 0; sensor_index < GYRO_COUNT_MAX; sensor_index++) {
|
||||
// check that the sensor we are checking against is not the same as the primary
|
||||
if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) {
|
||||
if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0)
|
||||
&& (_gyro_device_id[sensor_index] != _selection.gyro_device_id)) {
|
||||
|
||||
float gyro_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary
|
||||
|
||||
// calculate gyro_diff_sum_sq for the specified sensor against the primary
|
||||
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
|
||||
_gyro_diff[axis_index][check_index] = 0.95f * _gyro_diff[axis_index][check_index] + 0.05f *
|
||||
(_last_sensor_data[_gyro.last_best_vote].gyro_rad[axis_index] -
|
||||
_last_sensor_data[sensor_index].gyro_rad[axis_index]);
|
||||
gyro_diff_sum_sq += _gyro_diff[axis_index][check_index] * _gyro_diff[axis_index][check_index];
|
||||
|
||||
}
|
||||
|
||||
// capture the largest sum value
|
||||
if (gyro_diff_sum_sq > gyro_diff_sum_max_sq) {
|
||||
gyro_diff_sum_max_sq = gyro_diff_sum_sq;
|
||||
|
||||
}
|
||||
|
||||
// increment the check index
|
||||
check_index++;
|
||||
const Vector3f current_gyro{_last_sensor_data[sensor_index].gyro_rad};
|
||||
_gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (primary_gyro - current_gyro);
|
||||
}
|
||||
|
||||
// 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.gyro_inconsistency_rad_s = 0.0f;
|
||||
|
||||
} else {
|
||||
// get the vector length of the largest difference and write to the combined sensor struct
|
||||
preflt.gyro_inconsistency_rad_s = sqrtf(gyro_diff_sum_max_sq);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,11 +53,10 @@
|
|||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/sensor_preflight_imu.h>
|
||||
#include <uORB/topics/sensors_status_imu.h>
|
||||
#include <uORB/topics/sensor_selection.h>
|
||||
#include <uORB/topics/vehicle_imu.h>
|
||||
#include <uORB/topics/vehicle_imu_status.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
|
||||
namespace sensors
|
||||
{
|
||||
|
|
@ -111,16 +110,6 @@ public:
|
|||
*/
|
||||
void setRelativeTimestamps(sensor_combined_s &raw);
|
||||
|
||||
/**
|
||||
* Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
|
||||
*/
|
||||
void calcAccelInconsistency(sensor_preflight_imu_s &preflt);
|
||||
|
||||
/**
|
||||
* Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
|
||||
*/
|
||||
void calcGyroInconsistency(sensor_preflight_imu_s &preflt);
|
||||
|
||||
private:
|
||||
|
||||
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
|
||||
|
|
@ -157,7 +146,17 @@ private:
|
|||
* Check & handle failover of a sensor
|
||||
* @return true if a switch occured (could be for a non-critical reason)
|
||||
*/
|
||||
bool checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type);
|
||||
bool checkFailover(SensorData &sensor, const char *sensor_name);
|
||||
|
||||
/**
|
||||
* Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
|
||||
*/
|
||||
void calcAccelInconsistency();
|
||||
|
||||
/**
|
||||
* Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
|
||||
*/
|
||||
void calcGyroInconsistency();
|
||||
|
||||
SensorData _accel{ORB_ID::sensor_accel};
|
||||
SensorData _gyro{ORB_ID::sensor_gyro};
|
||||
|
|
@ -166,7 +165,7 @@ private:
|
|||
orb_advert_t _mavlink_log_pub{nullptr};
|
||||
|
||||
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
|
||||
uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */
|
||||
uORB::Publication<sensors_status_imu_s> _sensors_status_imu_pub{ORB_ID(sensors_status_imu)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[3];
|
||||
uORB::SubscriptionMultiArray<vehicle_imu_status_s, ACCEL_COUNT_MAX> _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status};
|
||||
|
|
@ -177,8 +176,8 @@ private:
|
|||
|
||||
bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */
|
||||
|
||||
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) */
|
||||
matrix::Vector3f _accel_diff[ACCEL_COUNT_MAX] {}; /**< filtered accel differences between IMU units (m/s/s) */
|
||||
matrix::Vector3f _gyro_diff[GYRO_COUNT_MAX] {}; /**< filtered gyro differences between IMU uinits (rad/s) */
|
||||
|
||||
uint32_t _accel_device_id[SENSOR_COUNT_MAX] {}; /**< accel driver device id for each uorb instance */
|
||||
uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {}; /**< gyro driver device id for each uorb instance */
|
||||
|
|
@ -186,7 +185,6 @@ private:
|
|||
uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */
|
||||
|
||||
sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */
|
||||
subsystem_info_s _info {}; /**< subsystem info publication */
|
||||
};
|
||||
|
||||
} /* namespace sensors */
|
||||
|
|
|
|||
Loading…
Reference in New Issue