WIP: SensorHub

.travis.yml

@@ -84,3 +84,5 @@ matrix:
       env: CI_BUILD_TARGET="px4-v4"
     - compiler: "gcc"
       env: CI_BUILD_TARGET="sitltest"
+    - compiler: "gcc"
+      env: CI_BUILD_TARGET="sitltest-shub"

ArduCopter/Copter.cpp

@@ -22,6 +22,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
  */
 Copter::Copter(void) :
     DataFlash{FIRMWARE_STRING, g.log_bitmask},
+    gps_serial_protocol(AP_SerialManager::SerialProtocol_GPS),
     flight_modes(&g.flight_mode1),
     mission(ahrs, 
             FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &),
@@ -104,6 +105,9 @@ Copter::Copter(void) :
     in_mavlink_delay(false),
     gcs_out_of_time(false),
     param_loader(var_info)
+#if HAL_SENSORHUB_ENABLED
+    ,shub(AP_SensorHub::init_instance())
+#endif
 {
     memset(&current_loc, 0, sizeof(current_loc));
 

ArduCopter/Copter.h

@@ -132,6 +132,10 @@
 #include <SITL/SITL.h>
 #endif
 
+#if HAL_SENSORHUB_ENABLED
+#include <AP_SensorHub/AP_SensorHub.h>
+#endif
+
 
 class Copter : public AP_HAL::HAL::Callbacks {
 public:
@@ -193,6 +197,7 @@ class Copter : public AP_HAL::HAL::Callbacks {
     DataFlash_Class DataFlash;
 
     AP_GPS gps;
+    AP_SerialManager::SerialProtocol gps_serial_protocol;
 
     // flight modes convenience array
     AP_Int8 *flight_modes;
@@ -220,6 +225,11 @@ class Copter : public AP_HAL::HAL::Callbacks {
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     SITL::SITL sitl;
+
+    AP_Baro baro_sitl;
+    Compass compass_sitl;
+    AP_InertialSensor ins_sitl;
+    AP_GPS gps_sitl;
 #endif
 
     // Mission library
@@ -641,7 +651,11 @@ class Copter : public AP_HAL::HAL::Callbacks {
 
     // set when we are upgrading parameters from 3.4
     bool upgrading_frame_params;
-
+
+#if HAL_SENSORHUB_ENABLED
+    AP_SensorHub *shub;
+#endif
+
     static const AP_Scheduler::Task scheduler_tasks[];
     static const AP_Param::Info var_info[];
     static const struct LogStructure log_structure[];
@@ -1160,6 +1174,10 @@ class Copter : public AP_HAL::HAL::Callbacks {
     void dataflash_periodic(void);
     void accel_cal_update(void);
 
+#if HAL_SENSORHUB_ENABLED
+    void setup_shub();
+#endif
+
 public:
     void mavlink_delay_cb();
     void failsafe_check();

ArduCopter/Parameters.cpp

@@ -752,6 +752,10 @@ const AP_Param::Info Copter::var_info[] = {
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     GOBJECT(sitl, "SIM_", SITL::SITL),
+    GOBJECT(baro_sitl, "SG_", AP_Baro),
+    GOBJECT(compass_sitl, "SC_", Compass),
+    GOBJECT(ins_sitl, "SI_", AP_InertialSensor),
+    GOBJECT(gps_sitl, "SP_", AP_GPS),
 #endif
 
     // @Group: GND_

ArduCopter/Parameters.h

@@ -368,6 +368,11 @@ class Parameters {
 
         // 254,255: reserved
 
+        k_param_baro_sitl = 300,
+        k_param_compass_sitl,
+        k_param_ins_sitl,
+        k_param_gps_sitl,
+
         // the k_param_* space is 9-bits in size
         // 511: reserved
     };

ArduCopter/system.cpp

@@ -119,6 +119,10 @@ void Copter::init_ardupilot()
     // initialise serial ports
     serial_manager.init();
 
+#if HAL_SENSORHUB_ENABLED
+    setup_shub();
+#endif
+
     // setup first port early to allow BoardConfig to report errors
     gcs_chan[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
 
@@ -209,7 +213,8 @@ void Copter::init_ardupilot()
 
     // Do GPS init
     gps.set_log_gps_bit(MASK_LOG_GPS);
-    gps.init(serial_manager);
+
+    gps.init(serial_manager, gps_serial_protocol);
 
     init_compass();
 
@@ -333,6 +338,10 @@ void Copter::init_ardupilot()
 
     // flag that initialisation has completed
     ap.initialised = true;
+
+#if HAL_SENSORHUB_ENABLED
+    shub->start();
+#endif
 }
 
 
@@ -722,3 +731,14 @@ void Copter::allocate_motors(void)
     // upgrade parameters. This must be done after allocating the objects
     convert_pid_parameters();
 }
+
+#if HAL_SENSORHUB_ENABLED
+void Copter::setup_shub()
+{
+#if SENSORHUB_MODE == SENSORHUB_SINK
+    gps_serial_protocol = AP_SerialManager::SerialProtocol_SENSORHUB;
+#endif
+
+    shub->init(serial_manager);
+}
+#endif

SensorHub/APM_Config.h

@@ -0,0 +1 @@
+#pragma once

SensorHub/Parameters.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+
+class Parameters {
+public:
+    static const uint16_t k_format_version = 1;
+    static const uint16_t k_software_type = 5;
+
+    enum {
+        k_param_format_version = 0,
+        k_param_software_type,
+        k_param_scheduler,
+        k_param_gps,
+        k_param_barometer,
+        k_param_compass,
+        k_param_ins,
+        k_param_serial_manager,
+        k_param_BoardConfig,
+    };
+
+    AP_Int16 format_version;
+    AP_Int16 software_type;
+};
+
+extern const AP_Param::Info var_info[];

SensorHub/SensorHub.cpp

@@ -0,0 +1,131 @@
+#include "SensorHub.h"
+#include <AP_GPS/GPS_Backend.h>
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+#if HAL_SENSORHUB_ENABLED
+
+SensorHub_Source sensorhub_source;
+
+#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(SensorHub_Source, &sensorhub_source, func, rate_hz, max_time_micros)
+#define GSCALAR(v, name, def) { sensorhub_source.g.v.vtype, name, Parameters::k_param_ ## v, &sensorhub_source.g.v, {def_value : def} }
+#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&sensorhub_source.v, {group_info : class::var_info} }
+
+const AP_Scheduler::Task SensorHub_Source::scheduler_tasks[] = {
+    SCHED_TASK(update_barometer, 100, 100),
+    SCHED_TASK(update_gps,       50, 200)
+};
+
+const AP_Param::Info SensorHub_Source::var_info[] = {
+    // @Param: FORMAT_VERSION
+    // @DisplayName: Eeprom format version number
+    // @Description: This value is incremented when changes are made to the eeprom format
+    // @User: Advanced
+    GSCALAR(format_version,         "FORMAT_VERSION", 0),
+
+    // @Param: SYSID_SW_TYPE
+    // @DisplayName: Software Type
+    // @Description: This is used by the ground station to recognise the software type (eg ArduPlane vs ArduCopter)
+    // @Values: 0:ArduPlane,4:AntennaTracker,10:Copter,20:Rover
+    // @User: Advanced
+    // @ReadOnly: True
+    GSCALAR(software_type,          "SYSID_SW_TYPE",  Parameters::k_software_type),
+
+
+    GOBJECT(compass, "COMPASS_", Compass),
+    GOBJECT(ins, "INS_", AP_InertialSensor),
+    GOBJECT(BoardConfig,            "BRD_",       AP_BoardConfig),
+    GOBJECT(barometer, "GND_", AP_Baro),
+    GOBJECT(gps, "GPS_", AP_GPS),
+    GOBJECT(scheduler, "SCHED_", AP_Scheduler),
+    AP_VAREND
+};
+
+SensorHub_Source::SensorHub_Source(void) :
+    shub(AP_SensorHub::init_instance())
+{}
+
+void SensorHub_Source::update_barometer()
+{
+    barometer.update();
+}
+
+void SensorHub_Source::update_gps()
+{
+    gps.update();
+}
+
+void SensorHub_Source::load_parameters()
+{
+    if (!g.format_version.load() ||
+        g.format_version != Parameters::k_format_version) {
+
+        // erase all parameters
+        hal.console->printf("Firmware change: erasing EEPROM...\n");
+        AP_Param::erase_all();
+
+        // save the current format version
+        g.format_version.set_and_save(Parameters::k_format_version);
+        hal.console->printf("done.\n");
+    }
+
+    uint32_t before = AP_HAL::micros();
+    // Load all auto-loaded EEPROM variables
+    AP_Param::load_all();
+    hal.console->printf("load_all took %luus\n", (unsigned long)(AP_HAL::micros() - before));
+}
+void SensorHub_Source::setup()
+{
+    AP_Param::setup_sketch_defaults();
+    serial_manager.init_console();
+    serial_manager.init();
+    load_parameters();
+    BoardConfig.init();
+    shub->init(serial_manager);
+
+    barometer.init();
+    barometer.calibrate();
+
+    compass.init();
+
+    gps.init(serial_manager, AP_SerialManager::SerialProtocol_GPS);
+    gps.update();
+
+    bool gpsInitialized = false;
+    do {
+        for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {
+            auto driver = gps.get_driver(i);
+            if (driver) {
+                driver->setSensorHub(shub);
+                gpsInitialized = true;
+                hal.console->printf("Initialized GPS\n");
+            }
+        }
+        gps.update();
+        hal.scheduler->delay(1);
+    } while (!gpsInitialized);
+
+    ins.init(scheduler.get_loop_rate_hz());
+    shub->start();
+
+    hal.console->printf("Initializing Scheduler...\n");
+    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
+}
+
+void SensorHub_Source::loop()
+{
+    ins.wait_for_sample();
+    ins.update();
+
+    auto timer = AP_HAL::micros();
+    scheduler.tick();
+    auto time_available = (timer + MAIN_LOOP_MICROS) - AP_HAL::micros();
+    scheduler.run(time_available > MAIN_LOOP_MICROS ? 0u : time_available);
+}
+
+AP_HAL_MAIN_CALLBACKS(&sensorhub_source);
+#else
+void setup() {}
+void loop() {}
+AP_HAL_MAIN();
+#endif

SensorHub/SensorHub.h

@@ -0,0 +1,65 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Param/AP_Param.h>
+#include <StorageManager/StorageManager.h>
+#include <AP_GPS/AP_GPS.h>
+#include <AP_Baro/AP_Baro.h>
+#include <AP_Compass/AP_Compass.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
+#include <AP_AccelCal/AP_AccelCal.h>
+#include <AP_AHRS/AP_AHRS.h>
+
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <AP_Declination/AP_Declination.h>
+#include <DataFlash/DataFlash.h>
+#include <AP_Scheduler/AP_Scheduler.h>
+
+#include <AP_Vehicle/AP_Vehicle.h>
+
+#include <AP_BoardConfig/AP_BoardConfig.h>
+
+#include <AP_SensorHub/AP_SensorHub.h>
+
+#include "defines.h"
+#include "config.h"
+#include "Parameters.h"
+
+#if HAL_SENSORHUB_ENABLED
+class SensorHub_Source : public AP_HAL::HAL::Callbacks {
+public:
+    friend class Parameters;
+
+    SensorHub_Source(void);
+
+    void setup() override;
+    void loop() override;
+
+private:
+    Parameters g;
+    AP_Scheduler scheduler;
+    AP_GPS gps;
+    AP_Baro barometer;
+    Compass compass;
+    AP_InertialSensor ins;
+
+    AP_SerialManager serial_manager;
+    AP_BoardConfig BoardConfig;
+
+    AP_Param param_loader{var_info};
+
+    static const AP_Param::Info var_info[];
+    static const AP_Scheduler::Task scheduler_tasks[];
+
+    AP_SensorHub *shub;
+
+public:
+    void update_barometer();
+    void update_gps();
+    void load_parameters();
+};
+
+extern SensorHub_Source sensorhub_source;
+#endif

SensorHub/config.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include "defines.h"
+#include "APM_Config.h"
+
+// run at 400Hz on all systems
+# define MAIN_LOOP_RATE    400
+# define MAIN_LOOP_SECONDS 0.0025f
+# define MAIN_LOOP_MICROS  2500

SensorHub/defines.h

@@ -0,0 +1 @@
+#pragma once