Browse Source

Accel calibration: Show better error message if cal fails

sbg
Lorenz Meier 10 years ago
parent
commit
32b9354783
  1. 12
      src/modules/commander/accelerometer_calibration.cpp

12
src/modules/commander/accelerometer_calibration.cpp

@ -559,6 +559,7 @@ int do_level_calibration(int mavlink_fd) { @@ -559,6 +559,7 @@ int do_level_calibration(int mavlink_fd) {
const unsigned cal_time = 5;
const unsigned cal_hz = 100;
const unsigned settle_time = 30;
bool success = false;
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
@ -597,7 +598,15 @@ int do_level_calibration(int mavlink_fd) { @@ -597,7 +598,15 @@ int do_level_calibration(int mavlink_fd) {
start = hrt_absolute_time();
// average attitude for 5 seconds
while(hrt_elapsed_time(&start) < cal_time * 1000000) {
poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
int pollret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
if (pollret <= 0) {
// attitude estimator is not running
mavlink_and_console_log_critical(mavlink_fd, "attitude estimator not running - check system boot");
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
goto out;
}
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
roll_mean += att.roll;
pitch_mean += att.pitch;
@ -606,7 +615,6 @@ int do_level_calibration(int mavlink_fd) { @@ -606,7 +615,6 @@ int do_level_calibration(int mavlink_fd) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
bool success = false;
if (counter > (cal_time * cal_hz / 2 )) {
roll_mean /= counter;
pitch_mean /= counter;

Loading…
Cancel
Save