fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 04:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

lib/sensor_calibration: check param save success and comment helpers

Browse Source
This commit is contained in:
Daniel Agar
2020-09-13 21:00:40 -04:00
parent ac732cdeba
commit 04214a347e
5 changed files with 78 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sensor_calibration/Accelerometer.cpp
+6 -5
View File
@@ -155,10 +155,11 @@ bool Accelerometer::ParametersSave()
{ {
if (_calibration_index >= 0) { if (_calibration_index >= 0) {
// save calibration // save calibration
SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); bool success = true;
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale); success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale);
// if (_external) { // if (_external) {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); // SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
@@ -167,7 +168,7 @@ bool Accelerometer::ParametersSave()
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
// } // }
return true; return success;
} }
return false; return false;
src/lib/sensor_calibration/Gyroscope.cpp
+5 -4
View File
@@ -151,9 +151,10 @@ bool Gyroscope::ParametersSave()
{ {
if (_calibration_index >= 0) { if (_calibration_index >= 0) {
// save calibration // save calibration
SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); bool success = true;
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
// if (_external) { // if (_external) {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); // SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
@@ -162,7 +163,7 @@ bool Gyroscope::ParametersSave()
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
// } // }
return true; return success;
} }
return false; return false;
src/lib/sensor_calibration/Magnetometer.cpp
+12 -11
View File
@@ -200,26 +200,27 @@ bool Magnetometer::ParametersSave()
{ {
if (_calibration_index >= 0) { if (_calibration_index >= 0) {
// save calibration // save calibration
SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); bool success = true;
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
Vector3f scale{_scale.diag()}; const Vector3f scale{_scale.diag()};
SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, scale); success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, scale);
Vector3f off_diag{_scale(0, 1), _scale(0, 2), _scale(1, 2)}; const Vector3f off_diag{_scale(0, 1), _scale(0, 2), _scale(1, 2)};
SetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index, off_diag); 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) { if (_external) {
SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
} else { } else {
SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
} }
return true; return success;
} }
return false; return false;
src/lib/sensor_calibration/Utilities.cpp
+4 -4
View File
@@ -90,7 +90,7 @@ int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8
return value; 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] {}; 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); 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) 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; 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; int ret = PX4_OK;
char str[20] {}; 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() Dcmf GetBoardRotation()
src/lib/sensor_calibration/Utilities.hpp
+51 -4
View File
@@ -38,16 +38,63 @@
namespace calibration 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); 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); 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); 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);
/**
* @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(); matrix::Dcmf GetBoardRotation();
} // namespace calibration } // namespace calibration