AP_AHRS: fix example

libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp

@@ -10,6 +10,8 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Baro/AP_Baro.h>
+#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
+#include <AP_Vehicle/AP_Vehicle.h>
 
 void setup();
 void loop();
@@ -17,20 +19,24 @@ void loop();
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
 
-static AP_BoardConfig board_config;
-static AP_InertialSensor ins;
-
-static Compass compass;
-
-static AP_GPS gps;
-static AP_Baro barometer;
 static AP_SerialManager serial_manager;
 AP_Int32 logger_bitmask;
 static AP_Logger logger{logger_bitmask};
 
-class DummyVehicle {
+class DummyVehicle : public AP_Vehicle {
 public:
     AP_AHRS_NavEKF ahrs{AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
+    bool set_mode(const uint8_t new_mode, const ModeReason reason) override { return true; };
+    uint8_t get_mode() const override { return 1; };
+    void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) override {};
+    void init_ardupilot() override {};
+    void load_parameters() override {};
+    void init() {
+        BoardConfig.init();
+        ins.init(100);
+        ahrs.init();
+    }
+
 };
 
 static DummyVehicle vehicle;
@@ -41,19 +47,16 @@ AP_AHRS_NavEKF &ahrs = vehicle.ahrs;
 
 void setup(void)
 {
-    board_config.init();
+    vehicle.init();
-    ins.init(100);
-    ahrs.init();
     serial_manager.init();
-
+    AP::compass().init();
-    compass.init();
+    if(AP::compass().read()) {
-    if(compass.read()) {
         hal.console->printf("Enabling compass\n");
-        ahrs.set_compass(&compass);
+        ahrs.set_compass(&AP::compass());
     } else {
         hal.console->printf("No compass detected\n");
     }
-    gps.init(serial_manager);
+    AP::gps().init(serial_manager);
 }
 
 void loop(void)
@@ -70,8 +73,8 @@ void loop(void)
     last_t = now;
 
     if (now - last_compass > 100 * 1000UL &&
-        compass.read()) {
+        AP::compass().read()) {
-        heading = compass.calculate_heading(ahrs.get_rotation_body_to_ned());
+        heading = AP::compass().calculate_heading(ahrs.get_rotation_body_to_ned());
         // read compass at 10Hz
         last_compass = now;
     }
@@ -90,7 +93,7 @@ void loop(void)
                 (double)ToDeg(drift.x),
                 (double)ToDeg(drift.y),
                 (double)ToDeg(drift.z),
-                (double)(compass.use_for_yaw() ? ToDeg(heading) : 0.0f),
+                (double)(AP::compass().use_for_yaw() ? ToDeg(heading) : 0.0f),
                 (double)((1.0e6f * counter) / (now-last_print)));
         last_print = now;
         counter = 0;