fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-09 08:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensor_calibration: refactor and centralize calibration slot logic

Browse Source 
- centralize logic for selecting a preferred calibration slot
   - automatically use existing calibration slot if it exists, otherwise
find first available slot, with a preference for a requested index
 - existing commander calibration methods rewrite all calibration slots
to match current sensor ordering
This commit is contained in:
Daniel Agar
2022-01-11 11:56:40 -05:00
parent d1304e1ceb
commit bb1177d504
18 changed files with 202 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sensor_calibration/Accelerometer.cpp
+36 -4
View File
@@ -144,15 +144,31 @@ void Accelerometer::set_rotation(Rotation rotation)
_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
}
bool Accelerometer::set_calibration_index(int calibration_index)
{
if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
_calibration_index = calibration_index;
return true;
}
return false;
}
void Accelerometer::ParametersUpdate()
{
if (_device_id == 0) {
return;
}
_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
ParametersLoad();
if (_calibration_index == -1) {
// no saved calibration available
Reset();
} else {
ParametersLoad();
}
}
bool Accelerometer::ParametersLoad()
@@ -236,9 +252,25 @@ void Accelerometer::Reset()
_calibration_count = 0;
}
bool Accelerometer::ParametersSave()
bool Accelerometer::ParametersSave(int desired_calibration_index, bool force)
{
if (_calibration_index >= 0) {
if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
_calibration_index = desired_calibration_index;
} else if (!force || (_calibration_index < 0)
|| (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) {
// ensure we have a valid calibration slot (matching existing or first available slot)
int8_t calibration_index_prev = _calibration_index;
_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
calibration_index_prev, _calibration_index);
}
}
if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
// save calibration
bool success = true;
success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);