dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

lib/sensor_calibration: check param save success and comment helpers

This commit is contained in:
Daniel Agar 2020-09-04 09:52:44 -04:00
parent ac732cdeba
commit 04214a347e
5 changed files with 78 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/lib/sensor_calibration/Accelerometer.cpp
View File

@ -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;

9
src/lib/sensor_calibration/Gyroscope.cpp
View File

@ -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;

23
src/lib/sensor_calibration/Magnetometer.cpp
View File

@ -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;

8
src/lib/sensor_calibration/Utilities.cpp
View File

@ -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()

55
src/lib/sensor_calibration/Utilities.hpp
View File

@ -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);
/**
* @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