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

commander: Fix calibration feedback so that QGC picks up all error conditions

This commit is contained in:
Lorenz Meier 2015-04-26 17:33:45 +02:00
parent a7f88d97b8
commit 362672ece8
5 changed files with 35 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/commander/accelerometer_calibration.cpp
View File

@ -222,8 +222,10 @@ int do_accel_calibration(int mavlink_fd)
}
}
if (res != OK || active_sensors == 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
if (res != OK) {
if (active_sensors == 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
}
return ERROR;
}
@ -278,7 +280,7 @@ int do_accel_calibration(int mavlink_fd)
fd = open(str, 0);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] sensor does not exist");
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = ERROR;
} else {
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
@ -298,14 +300,17 @@ int do_accel_calibration(int mavlink_fd)
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
}
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
/* give this message enough time to propagate */
usleep(600000);
return res;
}

10
src/modules/commander/calibration_messages.h
View File

@ -57,10 +57,10 @@
#define CAL_QGC_ORIENTATION_DETECTED_MSG "[cal] %s orientation detected"
#define CAL_QGC_SIDE_DONE_MSG "[cal] %s side done, rotate to a different side"
#define CAL_ERROR_SENSOR_MSG "[cal] ERROR: failed reading sensor"
#define CAL_ERROR_RESET_CAL_MSG "[cal] ERROR: failed to reset calibration, sensor %u"
#define CAL_ERROR_APPLY_CAL_MSG "[cal] ERROR: failed to apply calibration, sensor %u"
#define CAL_ERROR_SET_PARAMS_MSG "[cal] ERROR: failed to set parameters, sensor %u"
#define CAL_ERROR_SAVE_PARAMS_MSG "[cal] ERROR: failed to save parameters"
#define CAL_ERROR_SENSOR_MSG "[cal] calibration failed: reading sensor"
#define CAL_ERROR_RESET_CAL_MSG "[cal] calibration failed: to reset, sensor %u"
#define CAL_ERROR_APPLY_CAL_MSG "[cal] calibration failed: to apply calibration, sensor %u"
#define CAL_ERROR_SET_PARAMS_MSG "[cal] calibration failed: to set parameters, sensor %u"
#define CAL_ERROR_SAVE_PARAMS_MSG "[cal] calibration failed: failed to save parameters"
#endif /* CALIBRATION_MESSAGES_H_ */

8
src/modules/commander/calibration_routines.cpp
View File

@ -324,7 +324,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
/* not still, reset still start time */
if (t_still != 0) {
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
sleep(3);
usleep(500000);
t_still = 0;
}
}
@ -412,7 +412,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd,
int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
if (sub_accel < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: No onboard accel found");
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
@ -427,7 +427,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd,
if (orientation_failures > 4) {
result = calibrate_return_error;
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: timeout waiting for orientation");
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
@ -488,7 +488,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd,
// Note that this side is complete
side_data_collected[orient] = true;
tune_neutral(true);
sleep(1);
usleep(500000);
}
if (sub_accel >= 0) {

13
src/modules/commander/gyro_calibration.cpp
View File

@ -298,10 +298,17 @@ int do_gyro_calibration(int mavlink_fd)
}
}
mavlink_log_info(mavlink_fd, res == OK ? CAL_QGC_DONE_MSG : CAL_QGC_FAILED_MSG, sensor_name);
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return res;
}

8
src/modules/commander/mag_calibration.cpp
View File

@ -158,6 +158,10 @@ int do_mag_calibration(int mavlink_fd)
case calibrate_return_ok:
/* auto-save to EEPROM */
result = param_save_default();
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (result == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
@ -173,8 +177,8 @@ int do_mag_calibration(int mavlink_fd)
}
}
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
/* give this message enough time to propagate */
usleep(600000);
return result;
}