From 7238916d035d6550d8a614d865da90e97cb2c706 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 13:37:18 +0200 Subject: [PATCH] Sensors: Reload calibration whenever a new sensor instance is detected --- src/modules/sensors/sensors.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 60e1a9aabb..61eab348d7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2020,6 +2020,11 @@ Sensors::task_main() * do subscriptions */ + int gcount_prev = _gyro_count; + int mcount_prev = _mag_count; + int acount_prev = _accel_count; + int bcount_prev = _baro_count; + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], &raw.gyro_priority[0], &raw.gyro_errcount[0]); @@ -2032,6 +2037,15 @@ Sensors::task_main() _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], &raw.baro_priority[0], &raw.baro_errcount[0]); + if (gcount_prev != _gyro_count || + mcount_prev != _mag_count || + acount_prev != _accel_count || + bcount_prev != _baro_count) { + + /* reload calibration params */ + parameter_update_poll(true); + } + _rc_sub = orb_subscribe(ORB_ID(input_rc)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));