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', + )