From 7c8794b0bd41f9d06d0944e7951f9454600c3fe7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 29 Dec 2021 10:08:26 +1100
Subject: [PATCH] AP_InertialSensor: fixed INS_generic example

useful for driver development
---
 .../examples/INS_generic/INS_generic.cpp             | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp
index 55ad771623..d8f7b17498 100644
--- a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp
+++ b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp
@@ -6,6 +6,7 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
 #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
+#include <AP_Logger/AP_Logger.h>
 
 const AP_HAL::HAL &hal = AP_HAL::get_HAL();
 
@@ -19,6 +20,8 @@ static void run_test();
 
 // board specific config
 static AP_BoardConfig BoardConfig;
+static AP_Int32 log_bitmask;
+static AP_Logger logger{log_bitmask};
 
 void setup(void);
 void loop(void);
@@ -28,6 +31,7 @@ void setup(void)
     // setup any board specific drivers
     BoardConfig.init();
 
+    hal.console->begin(115200);
     hal.console->printf("AP_InertialSensor startup...\n");
 
     ins.init(100);
@@ -57,6 +61,7 @@ void loop(void)
 
     // wait for user input
     while (!hal.console->available()) {
+        EXPECT_DELAY_MS(20);
         hal.scheduler->delay(20);
     }
 
@@ -72,7 +77,7 @@ void loop(void)
             run_test();
         }
 
-        if (user_input == 'r' || user_input == 'R') {
+        if (user_input == 'r') {
             hal.scheduler->reboot(false);
         }
     }
@@ -110,6 +115,7 @@ static void run_test()
 
     // flush any user input
     while (hal.console->available()) {
+        EXPECT_DELAY_MS(20);
         hal.console->read();
     }
 
@@ -118,6 +124,8 @@ static void run_test()
 
     // loop as long as user does not press a key
     while (!hal.console->available()) {
+        EXPECT_DELAY_MS(10);
+
         // wait until we have a sample
         ins.wait_for_sample();
 
@@ -163,7 +171,7 @@ static void run_test()
                 state = 'u';
             }
 
-            hal.console->printf("   Gyro (%c) : X:%6.2f Y:%6.2f Z:%6.2f\n",
+            hal.console->printf("   Gyro (%c) : X:%6.2f Y:%6.2f Z:%6.2f",
                                 state, (double)gyro.x, (double)gyro.y, (double)gyro.z);
             auto temp = ins.get_temperature(ii);
             hal.console->printf("   t:%6.2f\n", (double)temp);