diff --git a/libraries/AP_Module/AP_Module.cpp b/libraries/AP_Module/AP_Module.cpp
new file mode 100644
index 0000000000..940c1c8a82
--- /dev/null
+++ b/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 .
+ */
+
+/*
+ support for external modules
+ */
+
+#include
+#include
+#if defined(HAVE_LIBDL)
+#include
+#endif
+#include
+#include
+
+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; inext = 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(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(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(h->symbol);
+ fn(&state);
+ }
+}
diff --git a/libraries/AP_Module/AP_Module.h b/libraries/AP_Module/AP_Module.h
new file mode 100644
index 0000000000..9c5b5b7963
--- /dev/null
+++ b/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 .
+ */
+
+/*
+ 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
+#include
+
+
+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);
+};
diff --git a/libraries/AP_Module/AP_Module_Structures.h b/libraries/AP_Module/AP_Module_Structures.h
new file mode 100644
index 0000000000..b12f70b534
--- /dev/null
+++ b/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
+#include
+
+#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
+
diff --git a/libraries/AP_Module/examples/ModuleTest/Makefile b/libraries/AP_Module/examples/ModuleTest/Makefile
new file mode 100644
index 0000000000..f5daf25151
--- /dev/null
+++ b/libraries/AP_Module/examples/ModuleTest/Makefile
@@ -0,0 +1 @@
+include ../../../../mk/apm.mk
diff --git a/libraries/AP_Module/examples/ModuleTest/ModuleTest.cpp b/libraries/AP_Module/examples/ModuleTest/ModuleTest.cpp
new file mode 100644
index 0000000000..52cce249dc
--- /dev/null
+++ b/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
+#include
+#include
+#include
+
+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();
diff --git a/libraries/AP_Module/examples/ModuleTest/module/Makefile b/libraries/AP_Module/examples/ModuleTest/module/Makefile
new file mode 100644
index 0000000000..8e3dedcc56
--- /dev/null
+++ b/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
diff --git a/libraries/AP_Module/examples/ModuleTest/module/moduletest.c b/libraries/AP_Module/examples/ModuleTest/module/moduletest.c
new file mode 100644
index 0000000000..24acc32d5a
--- /dev/null
+++ b/libraries/AP_Module/examples/ModuleTest/module/moduletest.c
@@ -0,0 +1,37 @@
+/*
+ very simple example module
+ */
+
+#include
+#include
+#include
+#include
+
+#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]));
+}
diff --git a/libraries/AP_Module/examples/ModuleTest/wscript b/libraries/AP_Module/examples/ModuleTest/wscript
new file mode 100644
index 0000000000..719ec151ba
--- /dev/null
+++ b/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',
+ )