From e2afc9ea18c499300493981a94b2f4f5a270b77f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Nov 2012 19:10:30 +1100 Subject: [PATCH] INS: get check for send_msg the right way around! I broke this in my previous commit --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 40a34f0eb8..86544d2783 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -275,9 +275,9 @@ bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void break; } if (send_msg == NULL) { - send_msg(PSTR("USER: Place APM %S and press any key.\n"), msg); - }else{ Serial.printf_P(PSTR("USER: Place APM %S and press any key.\n"), msg); + }else{ + send_msg(PSTR("USER: Place APM %S and press any key.\n"), msg); } // wait for user input @@ -307,9 +307,9 @@ bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void // run the calibration routine if( _calibrate_accel(samples, new_offsets, new_scaling) ) { if (send_msg == NULL) { - send_msg(PSTR("Calibration successful\n")); - }else{ Serial.printf_P(PSTR("Calibration successful\n")); + }else{ + send_msg(PSTR("Calibration successful\n")); } // set and save calibration @@ -320,9 +320,9 @@ bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void } if (send_msg == NULL) { - send_msg(PSTR("Calibration failed\n")); - } else { Serial.printf_P(PSTR("Calibration failed\n")); + } else { + send_msg(PSTR("Calibration failed\n")); } // restore original scaling and offsets _accel_offset.set(orig_offset);