mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
AP_InertialSensor: fixed example build
This commit is contained in:
parent
57d3609d00
commit
7ea18d5a61
@ -101,6 +101,26 @@ void loop(void)
|
||||
}
|
||||
}
|
||||
|
||||
static void setup_printf_P(const prog_char_t *fmt, ...)
|
||||
{
|
||||
va_list arg_list;
|
||||
va_start(arg_list, fmt);
|
||||
Serial.vprintf_P(fmt, arg_list);
|
||||
va_end(arg_list);
|
||||
}
|
||||
|
||||
static void setup_wait_key(void)
|
||||
{
|
||||
// wait for user input
|
||||
while (!Serial.available()) {
|
||||
delay(20);
|
||||
}
|
||||
// clear input buffer
|
||||
while( Serial.available() ) {
|
||||
Serial.read();
|
||||
}
|
||||
}
|
||||
|
||||
void run_calibration()
|
||||
{
|
||||
// clear off any other characters (like line feeds,etc)
|
||||
@ -108,7 +128,7 @@ void run_calibration()
|
||||
Serial.read();
|
||||
}
|
||||
|
||||
ins.calibrate_accel(delay, NULL);
|
||||
ins.calibrate_accel(delay, NULL, setup_printf_P, setup_wait_key);
|
||||
}
|
||||
|
||||
void display_offsets_and_scaling()
|
||||
@ -197,4 +217,4 @@ void run_test()
|
||||
while( Serial.available() ) {
|
||||
Serial.read();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user