Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

commander: sensor calibration check parameter save success

Browse Source
This commit is contained in:
Daniel Agar
2020-09-04 09:55:46 -04:00
parent 04214a347e
commit d70e183f5b
3 changed files with 109 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+35 -17
View File
@@ -340,10 +340,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return PX4_ERROR;
} else if (active_sensors > MAX_ACCEL_SENS) {
calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", active_sensors,
MAX_ACCEL_SENS);
}
/* measure and calculate offsets & scales */
@@ -357,6 +353,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
const Dcmf board_rotation = calibration::GetBoardRotation();
const Dcmf board_rotation_t = board_rotation.transpose();
bool param_save = false;
bool failed = true;
for (unsigned i = 0; i < MAX_ACCEL_SENS; i++) {
if (i < active_sensors) {
// calculate offsets
@@ -410,16 +409,25 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
}
// save all calibrations including empty slots
calibrations[i].ParametersSave();
if (calibrations[i].ParametersSave()) {
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
}
}
param_notify_changes();
if (param_save) {
param_notify_changes();
}
/* if there is a any preflight-check system response, let the barrage of messages through */
px4_usleep(200000);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
px4_usleep(600000); /* give this message enough time to propagate */
return PX4_OK;
if (!failed) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
return PX4_OK;
}
}
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
@@ -431,7 +439,8 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
#if !defined(CONSTRAINED_FLASH)
PX4_INFO("Accelerometer quick calibration");
bool success = false;
bool param_save = false;
bool failed = true;
// sensor thermal corrections (optional)
uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
@@ -536,16 +545,25 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
} else {
calibration.set_offset(offset);
success = true;
}
if (calibration.ParametersSave()) {
calibration.PrintStatus();
param_save = true;
failed = false;
calibration.ParametersSave();
calibration.PrintStatus();
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "calibration save failed");
break;
}
}
}
}
if (success) {
if (param_save) {
param_notify_changes();
}
if (!failed) {
return PX4_OK;
}
src/modules/commander/gyro_calibration.cpp
+22 -23
View File
@@ -286,9 +286,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
}
if (res == PX4_OK) {
/* set offset parameters to new values */
bool failed = false;
// set offset parameters to new values
bool param_save = false;
bool failed = true;
for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) {
@@ -304,29 +304,28 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
}
calibration.set_calibration_index(uorb_index);
calibration.ParametersSave();
if (calibration.ParametersSave()) {
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
}
}
if (failed) {
calibration_log_critical(mavlink_log_pub, "ERROR: failed to set offset params");
res = PX4_ERROR;
if (param_save) {
param_notify_changes();
}
if (!failed) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
return PX4_OK;
}
}
param_notify_changes();
/* if there is a any preflight-check system response, let the barrage of messages through */
px4_usleep(200000);
if (res == PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
px4_usleep(600000);
return res;
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
return PX4_ERROR;
}
src/modules/commander/mag_calibration.cpp
+52 -19
View File
@@ -857,6 +857,9 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
}
if (result == calibrate_return_ok) {
bool param_save = false;
bool failed = true;
for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
auto &current_cal = worker_data.calibration[cur_mag];
@@ -882,20 +885,35 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
}
current_cal.set_calibration_index(cur_mag);
current_cal.ParametersSave();
if (current_cal.ParametersSave()) {
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
}
}
// reset the learned EKF mag in-flight bias offsets which have been learned for the previous
// sensor calibration and will be invalidated by a new sensor calibration
for (int axis = 0; axis < 3; axis++) {
char axis_char = 'X' + axis;
char str[20] {};
sprintf(str, "EKF2_MAGBIAS_%c", axis_char);
float offset = 0.f;
param_set_no_notification(param_find(str), &offset);
if (param_save) {
// reset the learned EKF mag in-flight bias offsets which have been learned for the previous
// sensor calibration and will be invalidated by a new sensor calibration
for (int axis = 0; axis < 3; axis++) {
char axis_char = 'X' + axis;
char str[20] {};
sprintf(str, "EKF2_MAGBIAS_%c", axis_char);
float offset = 0.f;
param_set_no_notification(param_find(str), &offset);
}
param_notify_changes();
}
param_notify_changes();
if (failed) {
result = calibrate_return_error;
}
}
return result;
@@ -925,7 +943,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
if (!mag_earth_available) {
calibration_log_critical(mavlink_log_pub, "GPS required for mag quick cal");
return calibrate_return_error;
return PX4_ERROR;
} else {
@@ -943,7 +961,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
if (hrt_elapsed_time(&attitude.timestamp) > 1_s) {
calibration_log_critical(mavlink_log_pub, "attitude required for mag quick cal");
return calibrate_return_error;
return PX4_ERROR;
}
calibration_log_critical(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees",
@@ -954,6 +972,9 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
const Vector3f expected_field = Dcmf(euler).transpose() * mag_earth_pred;
bool param_save = false;
bool failed = true;
for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
uORB::Subscription mag_sub{ORB_ID(sensor_mag), cur_mag};
sensor_mag_s mag{};
@@ -970,18 +991,30 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field);
cal.set_offset(offset);
cal.PrintStatus();
// save new calibration
cal.ParametersSave();
if (cal.ParametersSave()) {
cal.PrintStatus();
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
}
}
}
param_notify_changes();
if (param_save) {
param_notify_changes();
}
calibration_log_info(mavlink_log_pub, "Mag quick calibration finished");
return calibrate_return_ok;
if (!failed) {
calibration_log_info(mavlink_log_pub, "Mag quick calibration finished");
return PX4_OK;
}
}
return calibrate_return_error;
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
return PX4_ERROR;
}