mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: Use return value of blocking_read to handle calibration timeouts
This commit is contained in:
parent
786661ca2b
commit
b4c0e1215b
|
@ -419,7 +419,10 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
|
||||||
PSTR("Place vehicle %S and press any key.\n"), msg);
|
PSTR("Place vehicle %S and press any key.\n"), msg);
|
||||||
|
|
||||||
// wait for user input
|
// wait for user input
|
||||||
interact->blocking_read();
|
if (interact->blocking_read()) {
|
||||||
|
//No need to use interact->printf_P for an error, blocking_read does this when it fails
|
||||||
|
goto failed;
|
||||||
|
}
|
||||||
|
|
||||||
// clear out any existing samples from ins
|
// clear out any existing samples from ins
|
||||||
update();
|
update();
|
||||||
|
|
|
@ -25,7 +25,7 @@ uint8_t AP_InertialSensor_UserInteract_MAVLink::blocking_read(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
hal.console->println_P(PSTR("Timed out waiting for user response"));
|
hal.console->println_P(PSTR("Timed out waiting for user response"));
|
||||||
return 0;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void AP_InertialSensor_UserInteract_MAVLink::_printf_P(const prog_char* fmt, ...)
|
void AP_InertialSensor_UserInteract_MAVLink::_printf_P(const prog_char* fmt, ...)
|
||||||
|
|
Loading…
Reference in New Issue