AP_Module: added external module hook library

this allows for external modules to be called at defined hook
locations in ArduPilot. The initial example is a module that consumes
the AHRS state, but this can be generalised to a wide variety of hooks

libraries/AP_Module/AP_Module.cpp

@@ -0,0 +1,194 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  support for external modules
+ */
+
+#include <stdio.h>
+#include <dirent.h>
+#if defined(HAVE_LIBDL)
+#include <dlfcn.h>
+#endif
+#include <AP_Module/AP_Module.h>
+#include <AP_Module/AP_Module_Structures.h>
+
+struct AP_Module::hook_list *AP_Module::hooks[NUM_HOOKS];
+
+const char *AP_Module::hook_names[AP_Module::NUM_HOOKS] = {
+    "hook_setup_start",
+    "hook_setup_complete",
+    "hook_AHRS_update",
+};
+
+/*
+  scan a module for hook symbols
+ */
+void AP_Module::module_scan(const char *path)
+{
+#if defined(HAVE_LIBDL)
+    void *m = dlopen(path, RTLD_NOW);
+    if (m == nullptr) {
+        printf("dlopen(%s) -> %s\n", path, dlerror());
+        return;
+    }
+    bool found_hook = false;
+    for (uint16_t i=0; i<NUM_HOOKS; i++) {
+        void *s = dlsym(m, hook_names[i]);
+        if (s != nullptr) {
+            // found a hook in this module, add it to the list
+            struct hook_list *h = new hook_list;
+            if (h == nullptr) {
+                AP_HAL::panic("Failed to allocate hook for %s", hook_names[i]);
+            }
+            h->next = hooks[i];
+            h->symbol = s;
+            hooks[i] = h;
+            found_hook = true;
+        }
+    }
+    if (!found_hook) {
+        // we don't need this module
+        dlclose(m);
+    }
+#endif
+}
+
+/*
+  initialise AP_Module, looking for shared libraries in the given module path
+*/
+void AP_Module::init(const char *module_path)
+{
+    // scan through module directory looking for *.so
+    DIR *d;
+    struct dirent *de;
+    d = opendir(module_path);
+    if (d == nullptr) {
+        return;
+    }
+    while ((de = readdir(d))) {
+        const char *extension = strrchr(de->d_name, '.');
+        if (extension == nullptr || strcmp(extension, ".so") != 0) {
+            continue;
+        }
+        char *path = nullptr;
+        if (asprintf(&path, "%s/%s", module_path, de->d_name) == -1) {
+            continue;
+        }
+        module_scan(path);
+        free(path);
+    }
+    closedir(d);
+}
+
+
+/*
+  call any setup_start hooks
+*/
+void AP_Module::call_hook_setup_start(void)
+{
+    uint64_t now = AP_HAL::micros64();
+    for (const struct hook_list *h=hooks[HOOK_SETUP_START]; h; h=h->next) {
+        hook_setup_start_fn_t fn = reinterpret_cast<hook_setup_start_fn_t>(h->symbol);
+        fn(now);
+    }
+}
+
+/*
+  call any setup_complete hooks
+*/
+void AP_Module::call_hook_setup_complete(void)
+{
+    uint64_t now = AP_HAL::micros64();
+    for (const struct hook_list *h=hooks[HOOK_SETUP_COMPLETE]; h; h=h->next) {
+        hook_setup_complete_fn_t fn = reinterpret_cast<hook_setup_complete_fn_t>(h->symbol);
+        fn(now);
+    }    
+}
+
+/*
+  call any AHRS_update hooks
+*/
+void AP_Module::call_hook_AHRS_update(const AP_AHRS_NavEKF &ahrs)
+{
+    if (hooks[HOOK_AHRS_UPDATE] == nullptr) {
+        // avoid filling in AHRS_state
+        return;
+    }
+
+    /*
+      construct AHRS_state structure
+     */
+    struct AHRS_state state {};
+    state.structure_version = AHRS_state_version;
+    state.time_us = AP_HAL::micros64();
+
+    if (!ahrs.initialised()) {
+        state.status = AHRS_STATUS_INITIALISING;
+    } else if (ahrs.healthy()) {
+        state.status = AHRS_STATUS_HEALTHY;
+    } else {
+        state.status = AHRS_STATUS_UNHEALTHY;
+    }
+
+    Quaternion q;
+    q.from_rotation_matrix(ahrs.get_rotation_body_to_ned());
+    state.quat[0] = q[0];
+    state.quat[1] = q[1];
+    state.quat[2] = q[2];
+    state.quat[3] = q[3];
+
+    state.eulers[0] = ahrs.roll;
+    state.eulers[1] = ahrs.pitch;
+    state.eulers[2] = ahrs.yaw;
+
+    Location loc;
+    if (ahrs.get_origin(loc)) {
+        state.origin.initialised = true;
+        state.origin.latitude = loc.lat;
+        state.origin.longitude = loc.lng;
+        state.origin.altitude = loc.alt*0.01f;
+    }
+
+    if (ahrs.get_position(loc)) {
+        state.position.available = true;
+        state.position.latitude = loc.lat;
+        state.position.longitude = loc.lng;
+        state.position.altitude = loc.alt*0.01f;
+    }
+    
+    Vector3f pos;
+    if (ahrs.get_relative_position_NED(pos)) {
+        state.relative_position[0] = pos[0];
+        state.relative_position[1] = pos[1];
+        state.relative_position[2] = pos[2];
+    }
+
+    const Vector3f &gyro = ahrs.get_gyro();
+    state.gyro_rate[0] = gyro[0];
+    state.gyro_rate[1] = gyro[1];
+    state.gyro_rate[2] = gyro[2];
+
+    const Vector3f &accel_ef = ahrs.get_accel_ef();
+    state.accel_ef[0] = accel_ef[0];
+    state.accel_ef[1] = accel_ef[0];
+    state.accel_ef[2] = accel_ef[0];
+    
+    for (const struct hook_list *h=hooks[HOOK_AHRS_UPDATE]; h; h=h->next) {
+        hook_AHRS_update_fn_t fn = reinterpret_cast<hook_AHRS_update_fn_t>(h->symbol);
+        fn(&state);
+    }    
+}

libraries/AP_Module/AP_Module.h

@@ -0,0 +1,76 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  support for external modules
+
+  ******************************************************************
+  PLEASE NOTE: module hooks are called synchronously from
+  ArduPilot. They must not block or make any IO calls. If anything
+  could take more than a few 10s of microseconds then you must defer
+  it to another thread. Modules are responsible for their own thread
+  handling
+  ******************************************************************
+
+ */
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_AHRS/AP_AHRS.h>
+
+
+class AP_Module {
+public:
+
+    // initialise AP_Module, looking for shared libraries in the given module path
+    static void init(const char *module_path);
+    
+    // call any setup_start hooks
+    static void call_hook_setup_start(void);
+
+    // call any setup_complete hooks
+    static void call_hook_setup_complete(void);
+    
+    // call any AHRS_update hooks
+    static void call_hook_AHRS_update(const AP_AHRS_NavEKF &ahrs);
+
+    
+private:
+
+    enum ModuleHooks {
+        HOOK_SETUP_START    = 0,
+        HOOK_SETUP_COMPLETE,
+        HOOK_AHRS_UPDATE,
+        NUM_HOOKS
+    };
+
+    // singly linked list per hook
+    struct hook_list {
+        struct hook_list *next;
+        void *symbol; // from dlsym()
+    };
+
+    // currently installed hooks
+    static struct hook_list *hooks[NUM_HOOKS];
+
+    // table giving the name of the hooks in the external
+    // modules. These are passed to dlsym(). The table order must
+    // match the ModuleHooks enum
+    static const char *hook_names[NUM_HOOKS];
+    
+    // scan a module for hooks
+    static void module_scan(const char *path);
+};

libraries/AP_Module/AP_Module_Structures.h

@@ -0,0 +1,100 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+  this defines data structures for public module interfaces in
+  ArduPilot. 
+
+  These structures are designed to not depend on other headers inside
+  ArduPilot, although they do depend on the general ABI of the
+  platform, and thus can depend on compilation options to some extent
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define AHRS_state_version 1
+
+enum AHRS_status {
+    AHRS_STATUS_INITIALISING    = 0,
+    AHRS_STATUS_UNHEALTHY       = 1,
+    AHRS_STATUS_HEALTHY         = 2
+};
+
+/*
+  export the attitude and position of the vehicle.
+ */
+struct AHRS_state {
+    // version of this structure (AHRS_state_version)
+    uint32_t structure_version;
+
+    // time since boot in microseconds
+    uint64_t time_us;
+    
+    // status of AHRS solution
+    enum AHRS_status status;
+    
+    // quaternion attitude, first element is length scalar. Same
+    // conventions as AP_Math/quaternion.h
+    float quat[4];
+
+    // euler angles in radians. Order is roll, pitch, yaw
+    float eulers[3];
+    
+    // global origin
+    struct {
+        // true when origin has been initialised with a global position
+        bool initialised;
+
+        // latitude and longitude in degrees * 10^7 (approx 1cm resolution)
+        int32_t latitude;
+        int32_t longitude;
+
+        // altitude AMSL in meters, positive up
+        float altitude;
+    } origin;
+
+    // global position
+    struct {
+        // true when we have a global position
+        bool available;
+
+        // latitude and longitude in degrees * 10^7 (approx 1cm resolution)
+        int32_t latitude;
+        int32_t longitude;
+
+        // altitude AMSL in meters, positive up
+        float altitude;
+    } position;
+    
+    // NED relative position in meters. Relative to origin
+    float relative_position[3];
+
+    // current rotational rates in radians/second in body frame
+    // order is roll, pitch, yaw
+    float gyro_rate[3];
+
+    // current earth frame acceleration estimate, including
+    // gravitational forces, m/s/s order is NED
+    float accel_ef[3];
+};
+
+
+/*
+  prototypes for hook functions
+ */
+typedef void (*hook_setup_start_fn_t)(uint64_t);
+void hook_setup_start(uint64_t time_us);
+    
+typedef void (*hook_setup_complete_fn_t)(uint64_t);
+void hook_setup_complete(uint64_t time_us);
+    
+typedef void (*hook_AHRS_update_fn_t)(const struct AHRS_state *);
+void hook_AHRS_update(const struct AHRS_state *state);
+
+#ifdef __cplusplus
+}
+#endif
+

libraries/AP_Module/examples/ModuleTest/Makefile

@@ -0,0 +1 @@
+include ../../../../mk/apm.mk

libraries/AP_Module/examples/ModuleTest/ModuleTest.cpp

@@ -0,0 +1,38 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+//
+// Simple test for the AP_AHRS interface
+//
+
+#include <AP_ADC/AP_ADC.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Module/AP_Module.h>
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+// sensor declaration
+AP_InertialSensor ins;
+AP_GPS gps;
+AP_Baro baro;
+AP_SerialManager serial_manager;
+
+// choose which AHRS system to use
+AP_AHRS_DCM  ahrs(ins, baro, gps);
+
+void setup(void)
+{
+    serial_manager.init();
+    ins.init(100);
+    baro.init();
+    ahrs.init();
+
+    gps.init(NULL, serial_manager);
+}
+
+void loop(void)
+{
+    ahrs.update();
+}
+
+AP_HAL_MAIN();

libraries/AP_Module/examples/ModuleTest/module/Makefile

@@ -0,0 +1,13 @@
+
+# minimal makefile setup for ARM linux targets
+
+CC=arm-linux-gnueabihf-gcc
+CFLAGS=-Wall -fPIC -g -shared
+
+all: moduletest.so
+
+moduletest.so: moduletest.c
+	$(CC) $(CFLAGS) -o moduletest.so moduletest.c -ldl
+
+clean:
+	rm -f moduletest.so

libraries/AP_Module/examples/ModuleTest/module/moduletest.c

@@ -0,0 +1,37 @@
+/*
+  very simple example module
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <math.h>
+
+#include "../../../AP_Module_Structures.h"
+
+void hook_setup_start(uint64_t time_us)
+{
+    printf("setup_start called\n");
+}
+
+void hook_setup_complete(uint64_t time_us)
+{
+    printf("setup_complete called\n");
+}
+
+
+#define degrees(x) (x * 180.0 / M_PI)
+
+void hook_AHRS_update(const struct AHRS_state *state)
+{
+    static uint64_t last_print_us;
+    if (state->time_us - last_print_us < 1000000UL) {
+        return;
+    }
+    last_print_us = state->time_us;
+    // print euler angles once per second
+    printf("AHRS_update (%.1f,%.1f,%.1f)\n",
+           degrees(state->eulers[0]),
+           degrees(state->eulers[1]),
+           degrees(state->eulers[2]));
+}

libraries/AP_Module/examples/ModuleTest/wscript

@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_example(
+        use='ap',
+    )