diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index fc5302c122..66a8a536b2 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -48,21 +48,42 @@ Accelerometer::Accelerometer() Reset(); } -Accelerometer::Accelerometer(uint32_t device_id) +Accelerometer::Accelerometer(uint32_t device_id, bool external) { Reset(); - set_device_id(device_id); + set_device_id(device_id, external); } -void Accelerometer::set_device_id(uint32_t device_id) +void Accelerometer::set_device_id(uint32_t device_id, bool external) { - if (_device_id != device_id) { + if (_device_id != device_id || _external != external) { + set_external(external); _device_id = device_id; ParametersUpdate(); SensorCorrectionsUpdate(true); } } +void Accelerometer::set_external(bool external) +{ + // update priority default appropriately if not set + if (_calibration_index < 0 || _priority < 0) { + if ((_priority < 0) || (_priority > 100)) { + _priority = external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + + } else if (!_external && external && (_priority == DEFAULT_PRIORITY)) { + // internal -> external + _priority = DEFAULT_EXTERNAL_PRIORITY; + + } else if (_external && !external && (_priority == DEFAULT_EXTERNAL_PRIORITY)) { + // external -> internal + _priority = DEFAULT_PRIORITY; + } + } + + _external = external; +} + void Accelerometer::SensorCorrectionsUpdate(bool force) { // check if the selected sensor has updated diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index 59e0a87993..f24ad538ae 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -53,15 +53,15 @@ public: static constexpr const char *SensorString() { return "ACC"; } Accelerometer(); - explicit Accelerometer(uint32_t device_id); + explicit Accelerometer(uint32_t device_id, bool external = false); ~Accelerometer() = default; void PrintStatus(); void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } - void set_device_id(uint32_t device_id); - void set_external(bool external = true) { _external = external; } + void set_device_id(uint32_t device_id, bool external = false); + void set_external(bool external = true); void set_offset(const matrix::Vector3f &offset) { _offset = offset; } void set_scale(const matrix::Vector3f &scale) { _scale = scale; } @@ -95,7 +95,7 @@ private: int8_t _calibration_index{-1}; uint32_t _device_id{0}; - int32_t _priority{DEFAULT_PRIORITY}; + int32_t _priority{-1}; bool _external{false}; }; diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index 8fa8d6b4c9..669e3fb581 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -48,21 +48,42 @@ Gyroscope::Gyroscope() Reset(); } -Gyroscope::Gyroscope(uint32_t device_id) +Gyroscope::Gyroscope(uint32_t device_id, bool external) { Reset(); - set_device_id(device_id); + set_device_id(device_id, external); } -void Gyroscope::set_device_id(uint32_t device_id) +void Gyroscope::set_device_id(uint32_t device_id, bool external) { - if (_device_id != device_id) { + if (_device_id != device_id || _external != external) { + set_external(external); _device_id = device_id; ParametersUpdate(); SensorCorrectionsUpdate(true); } } +void Gyroscope::set_external(bool external) +{ + // update priority default appropriately if not set + if (_calibration_index < 0 || _priority < 0) { + if ((_priority < 0) || (_priority > 100)) { + _priority = external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + + } else if (!_external && external && (_priority == DEFAULT_PRIORITY)) { + // internal -> external + _priority = DEFAULT_EXTERNAL_PRIORITY; + + } else if (_external && !external && (_priority == DEFAULT_EXTERNAL_PRIORITY)) { + // external -> internal + _priority = DEFAULT_PRIORITY; + } + } + + _external = external; +} + void Gyroscope::SensorCorrectionsUpdate(bool force) { // check if the selected sensor has updated diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 907a2f5789..e25707a185 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -53,15 +53,15 @@ public: static constexpr const char *SensorString() { return "GYRO"; } Gyroscope(); - explicit Gyroscope(uint32_t device_id); + explicit Gyroscope(uint32_t device_id, bool external = false); ~Gyroscope() = default; void PrintStatus(); void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } - void set_device_id(uint32_t device_id); - void set_external(bool external = true) { _external = external; } + void set_device_id(uint32_t device_id, bool external = false); + void set_external(bool external = true); void set_offset(const matrix::Vector3f &offset) { _offset = offset; } uint32_t device_id() const { return _device_id; } @@ -93,7 +93,7 @@ private: int8_t _calibration_index{-1}; uint32_t _device_id{0}; - int32_t _priority{DEFAULT_PRIORITY}; + int32_t _priority{-1}; bool _external{false}; }; diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index 4baf6b46c1..401fe7ed8e 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -50,14 +50,14 @@ Magnetometer::Magnetometer() Magnetometer::Magnetometer(uint32_t device_id, bool external) { - set_external(external); Reset(); - set_device_id(device_id); + set_device_id(device_id, external); } -void Magnetometer::set_device_id(uint32_t device_id) +void Magnetometer::set_device_id(uint32_t device_id, bool external) { - if (_device_id != device_id) { + if (_device_id != device_id || _external != external) { + set_external(external); _device_id = device_id; ParametersUpdate(); } @@ -66,7 +66,7 @@ void Magnetometer::set_device_id(uint32_t device_id) void Magnetometer::set_external(bool external) { // update priority default appropriately if not set - if (_calibration_index < 0) { + if (_calibration_index < 0 || _priority < 0) { if ((_priority < 0) || (_priority > 100)) { _priority = external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 3984c99c46..5cbafcee77 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -61,7 +61,7 @@ public: void PrintStatus(); void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } - void set_device_id(uint32_t device_id); + void set_device_id(uint32_t device_id, bool external = false); void set_external(bool external = true); void set_offset(const matrix::Vector3f &offset) { _offset = offset; } void set_scale(const matrix::Vector3f &scale); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 76619baf4b..464af4e2d7 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -451,8 +451,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma uORB::SubscriptionData mag_sub{ORB_ID(sensor_mag), cur_mag}; if (mag_sub.advertised() && (mag_sub.get().device_id != 0) && (mag_sub.get().timestamp > 0)) { - worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id); - worker_data.calibration[cur_mag].set_external(mag_sub.get().is_external); + worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id, mag_sub.get().is_external); } // reset calibration index to match uORB numbering diff --git a/src/modules/sensors/sensor_params_acc0.c b/src/modules/sensors/sensor_params_acc0.c index 8843fefaba..f7dae6fbe4 100644 --- a/src/modules/sensors/sensor_params_acc0.c +++ b/src/modules/sensors/sensor_params_acc0.c @@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_ACC0_ID, 0); /** * Accelerometer 0 priority. * + * @value -1 Uninitialized * @value 0 Disabled * @value 1 Min * @value 25 Low @@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_ACC0_ID, 0); * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_ACC0_PRIO, 50); +PARAM_DEFINE_INT32(CAL_ACC0_PRIO, -1); /** * Accelerometer X-axis offset diff --git a/src/modules/sensors/sensor_params_acc1.c b/src/modules/sensors/sensor_params_acc1.c index 96130686e2..b814bd55ba 100644 --- a/src/modules/sensors/sensor_params_acc1.c +++ b/src/modules/sensors/sensor_params_acc1.c @@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_ACC1_ID, 0); /** * Accelerometer 1 priority. * + * @value -1 Uninitialized * @value 0 Disabled * @value 1 Min * @value 25 Low @@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_ACC1_ID, 0); * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_ACC1_PRIO, 50); +PARAM_DEFINE_INT32(CAL_ACC1_PRIO, -1); /** * Accelerometer X-axis offset diff --git a/src/modules/sensors/sensor_params_acc2.c b/src/modules/sensors/sensor_params_acc2.c index da1873ca61..c298e08cd8 100644 --- a/src/modules/sensors/sensor_params_acc2.c +++ b/src/modules/sensors/sensor_params_acc2.c @@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_ACC2_ID, 0); /** * Accelerometer 2 priority. * + * @value -1 Uninitialized * @value 0 Disabled * @value 1 Min * @value 25 Low @@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_ACC2_ID, 0); * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_ACC2_PRIO, 50); +PARAM_DEFINE_INT32(CAL_ACC2_PRIO, -1); /** * Accelerometer X-axis offset diff --git a/src/modules/sensors/sensor_params_gyro0.c b/src/modules/sensors/sensor_params_gyro0.c index 20b2c99e0a..ff3176f9c2 100644 --- a/src/modules/sensors/sensor_params_gyro0.c +++ b/src/modules/sensors/sensor_params_gyro0.c @@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0); /** * Gyro 0 priority. * + * @value -1 Uninitialized * @value 0 Disabled * @value 1 Min * @value 25 Low @@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0); * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, 50); +PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, -1); /** * Gyro X-axis offset diff --git a/src/modules/sensors/sensor_params_gyro1.c b/src/modules/sensors/sensor_params_gyro1.c index 2b30fa51a7..925c5a13fd 100644 --- a/src/modules/sensors/sensor_params_gyro1.c +++ b/src/modules/sensors/sensor_params_gyro1.c @@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0); /** * Gyro 1 priority. * + * @value -1 Uninitialized * @value 0 Disabled * @value 1 Min * @value 25 Low @@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0); * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, 50); +PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, -1); /** * Gyro X-axis offset diff --git a/src/modules/sensors/sensor_params_gyro2.c b/src/modules/sensors/sensor_params_gyro2.c index d2608942f1..e662822b4c 100644 --- a/src/modules/sensors/sensor_params_gyro2.c +++ b/src/modules/sensors/sensor_params_gyro2.c @@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0); /** * Gyro 2 priority. * + * @value -1 Uninitialized * @value 0 Disabled * @value 1 Min * @value 25 Low @@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0); * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, 50); +PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, -1); /** * Gyro X-axis offset diff --git a/src/modules/sensors/sensor_params_mag0.c b/src/modules/sensors/sensor_params_mag0.c index 8bf3611f3b..0d47608da9 100644 --- a/src/modules/sensors/sensor_params_mag0.c +++ b/src/modules/sensors/sensor_params_mag0.c @@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0); /** * Mag 0 priority. * + * @value -1 Uninitialized * @value 0 Disabled * @value 1 Min * @value 25 Low @@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0); * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG0_PRIO, 50); +PARAM_DEFINE_INT32(CAL_MAG0_PRIO, -1); /** * Rotation of magnetometer 0 relative to airframe. diff --git a/src/modules/sensors/sensor_params_mag1.c b/src/modules/sensors/sensor_params_mag1.c index 405aeb4ded..ee8a5058dd 100644 --- a/src/modules/sensors/sensor_params_mag1.c +++ b/src/modules/sensors/sensor_params_mag1.c @@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_MAG1_ID, 0); /** * Mag 1 priority. * + * @value -1 Uninitialized * @value 0 Disabled * @value 1 Min * @value 25 Low @@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_MAG1_ID, 0); * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG1_PRIO, 50); +PARAM_DEFINE_INT32(CAL_MAG1_PRIO, -1); /** * Rotation of magnetometer 1 relative to airframe. diff --git a/src/modules/sensors/sensor_params_mag2.c b/src/modules/sensors/sensor_params_mag2.c index e8bb1a0bee..75a36d6d02 100644 --- a/src/modules/sensors/sensor_params_mag2.c +++ b/src/modules/sensors/sensor_params_mag2.c @@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_MAG2_ID, 0); /** * Mag 2 priority. * + * @value -1 Uninitialized * @value 0 Disabled * @value 1 Min * @value 25 Low @@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_MAG2_ID, 0); * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG2_PRIO, 50); +PARAM_DEFINE_INT32(CAL_MAG2_PRIO, -1); /** * Rotation of magnetometer 2 relative to airframe. diff --git a/src/modules/sensors/sensor_params_mag3.c b/src/modules/sensors/sensor_params_mag3.c index 91ba9cd95a..b7c0c6552e 100644 --- a/src/modules/sensors/sensor_params_mag3.c +++ b/src/modules/sensors/sensor_params_mag3.c @@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_MAG3_ID, 0); /** * Mag 3 priority. * + * @value -1 Uninitialized * @value 0 Disabled * @value 1 Min * @value 25 Low @@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_MAG3_ID, 0); * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG3_PRIO, 50); +PARAM_DEFINE_INT32(CAL_MAG3_PRIO, -1); /** * Rotation of magnetometer 3 relative to airframe. diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 18e430d5a2..99fa4d1bdf 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -224,8 +224,7 @@ void VehicleMagnetometer::Run() updated[uorb_index] = true; if (_calibration[uorb_index].device_id() != report.device_id) { - _calibration[uorb_index].set_external(report.is_external); - _calibration[uorb_index].set_device_id(report.device_id); + _calibration[uorb_index].set_device_id(report.device_id, report.is_external); _priority[uorb_index] = _calibration[uorb_index].priority(); }