From 04214a347e791c91efd4d4865a4003467a77c2fe Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 4 Sep 2020 09:52:44 -0400 Subject: [PATCH] lib/sensor_calibration: check param save success and comment helpers --- src/lib/sensor_calibration/Accelerometer.cpp | 11 ++-- src/lib/sensor_calibration/Gyroscope.cpp | 9 ++-- src/lib/sensor_calibration/Magnetometer.cpp | 23 ++++---- src/lib/sensor_calibration/Utilities.cpp | 8 +-- src/lib/sensor_calibration/Utilities.hpp | 55 ++++++++++++++++++-- 5 files changed, 78 insertions(+), 28 deletions(-) diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index 1221ba2642..fc5302c122 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -155,10 +155,11 @@ bool Accelerometer::ParametersSave() { if (_calibration_index >= 0) { // save calibration - SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); - SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); - SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); - SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale); + bool success = true; + success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); + success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); + success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); + success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale); // if (_external) { // SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); @@ -167,7 +168,7 @@ bool Accelerometer::ParametersSave() // SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // } - return true; + return success; } return false; diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index a1f29b3da7..8fa8d6b4c9 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -151,9 +151,10 @@ bool Gyroscope::ParametersSave() { if (_calibration_index >= 0) { // save calibration - SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); - SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); - SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); + bool success = true; + success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); + success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); + success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); // if (_external) { // SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); @@ -162,7 +163,7 @@ bool Gyroscope::ParametersSave() // SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // } - return true; + return success; } return false; diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index 69ed1b38c4..4baf6b46c1 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -200,26 +200,27 @@ bool Magnetometer::ParametersSave() { if (_calibration_index >= 0) { // save calibration - SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); - SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); - SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); + bool success = true; + success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); + success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); + success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); - Vector3f scale{_scale.diag()}; - SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, scale); + const Vector3f scale{_scale.diag()}; + success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, scale); - Vector3f off_diag{_scale(0, 1), _scale(0, 2), _scale(1, 2)}; - SetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index, off_diag); + const Vector3f off_diag{_scale(0, 1), _scale(0, 2), _scale(1, 2)}; + success &= SetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index, off_diag); - SetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index, _power_compensation); + success &= SetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index, _power_compensation); if (_external) { - SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); + success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); } else { - SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); + success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); } - return true; + return success; } return false; diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor_calibration/Utilities.cpp index 22f9368cca..67384a7e67 100644 --- a/src/lib/sensor_calibration/Utilities.cpp +++ b/src/lib/sensor_calibration/Utilities.cpp @@ -90,7 +90,7 @@ int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8 return value; } -int SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, int32_t value) +bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, int32_t value) { char str[20] {}; @@ -103,7 +103,7 @@ int SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t i PX4_ERR("failed to set %s = %d", str, value); } - return ret; + return ret == PX4_OK; } Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance) @@ -126,7 +126,7 @@ Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_t return values; } -int SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, Vector3f values) +bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, Vector3f values) { int ret = PX4_OK; char str[20] {}; @@ -143,7 +143,7 @@ int SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, } } - return ret; + return ret == PX4_OK; } Dcmf GetBoardRotation() diff --git a/src/lib/sensor_calibration/Utilities.hpp b/src/lib/sensor_calibration/Utilities.hpp index b7c0960aeb..ff53c9b898 100644 --- a/src/lib/sensor_calibration/Utilities.hpp +++ b/src/lib/sensor_calibration/Utilities.hpp @@ -38,16 +38,63 @@ namespace calibration { +/** + * @brief Find sensor's calibration index if it exists. + * + * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param device_id + * @return int8_t Valid calibration index on success, -1 otherwise + */ int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id); +/** + * @brief Get sensor calibration parameter value. + * + * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param cal_type Calibration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") + * @param instance + * @return int32_t The calibration value. + */ int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance); -int SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, int32_t value); +/** + * @brief Set a single calibration paramter. + * + * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param cal_type Calibration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") + * @param instance Calibration index (0 - 3) + * @param value int32_t parameter value + * @return true if the parameter name was valid and value saved successfully, false otherwise. + */ +bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, int32_t value); + +/** + * @brief Get the Calibration Params Vector 3f object + * + * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param cal_type Calibration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") + * @param instance Calibration index (0 - 3) + * @return matrix::Vector3f Vector of calibration values. + */ matrix::Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance); -int SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, - matrix::Vector3f values); -matrix::Dcmf GetBoardRotation(); +/** + * @brief Set the Calibration Params Vector 3f object + * + * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG") + * @param cal_type Calibration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO") + * @param instance Calibration index (0 - 3) + * @param values Vector of calibration values x, y, z. + * @return true if the parameter name was valid and all values saved successfully, false otherwise. + */ +bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, + matrix::Vector3f values); +/** + * @brief Get the overall board rotation, including level adjustment. + * + * @return matrix::Dcmf + */ +matrix::Dcmf GetBoardRotation(); } // namespace calibration