diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 163af7e6a2..d1bbac8333 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -419,7 +419,7 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact PSTR("Place vehicle %S and press any key.\n"), msg); // wait for user input - if (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; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h index f72eeffb4b..baf2d95284 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h @@ -7,7 +7,7 @@ /* Pure virtual interface class */ class AP_InertialSensor_UserInteract { public: - virtual uint8_t blocking_read() = 0; + virtual bool blocking_read() = 0; virtual void _printf_P(const prog_char *, ...) = 0; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp index 06e3c2ea36..ce7bdf9117 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp @@ -7,7 +7,7 @@ extern const AP_HAL::HAL& hal; -uint8_t AP_InertialSensor_UserInteract_MAVLink::blocking_read(void) +bool AP_InertialSensor_UserInteract_MAVLink::blocking_read(void) { uint32_t start_ms = hal.scheduler->millis(); /* Wait for a COMMAND_ACK message to be received from the ground station */ @@ -20,12 +20,12 @@ uint8_t AP_InertialSensor_UserInteract_MAVLink::blocking_read(void) mavlink_status_t status; if (mavlink_parse_char(_chan, c, &msg, &status)) { if (msg.msgid == MAVLINK_MSG_ID_COMMAND_ACK) { - return 0; + return true; } } } hal.console->println_P(PSTR("Timed out waiting for user response")); - return 1; + return false; } void AP_InertialSensor_UserInteract_MAVLink::_printf_P(const prog_char* fmt, ...) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h index 89fb07ea52..4fa3cb4ebf 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h @@ -14,7 +14,7 @@ public: AP_InertialSensor_UserInteract_MAVLink(mavlink_channel_t chan) : _chan(chan) {} - uint8_t blocking_read(); + bool blocking_read(); void _printf_P(const prog_char *, ...); private: mavlink_channel_t _chan; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp index 35e5a245cf..9aaf53c995 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp @@ -4,7 +4,8 @@ extern const AP_HAL::HAL& hal; -uint8_t AP_InertialSensor_UserInteractStream::blocking_read() { +bool AP_InertialSensor_UserInteractStream::blocking_read() +{ /* Wait for input to be available */ while(!_s->available()) { hal.scheduler->delay(20); @@ -15,7 +16,7 @@ uint8_t AP_InertialSensor_UserInteractStream::blocking_read() { while (_s->available()) { _s->read(); } - return ret; + return true; } void AP_InertialSensor_UserInteractStream::_printf_P( diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h index c645df2e11..d445846ef5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h @@ -14,7 +14,7 @@ public: AP_InertialSensor_UserInteractStream(AP_HAL::BetterStream *s) : _s(s) {} - uint8_t blocking_read(); + bool blocking_read(); void _printf_P(const prog_char *, ...); private: AP_HAL::BetterStream *_s;