Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include <AP_HAL/AP_HAL.h>
      17             : 
      18             : #if AP_MODULE_SUPPORTED
      19             : 
      20             : #include <AP_AHRS/AP_AHRS.h>
      21             : 
      22             : /*
      23             :   support for external modules
      24             :  */
      25             : 
      26             : #include <stdio.h>
      27             : #if defined(HAVE_LIBDL)
      28             : #include <dirent.h>
      29             : #include <dlfcn.h>
      30             : #endif
      31             : #include <AP_Module/AP_Module.h>
      32             : #include <AP_Module/AP_Module_Structures.h>
      33             : 
      34             : struct AP_Module::hook_list *AP_Module::hooks[NUM_HOOKS];
      35             : 
      36             : const char *AP_Module::hook_names[AP_Module::NUM_HOOKS] = {
      37             :     "ap_hook_setup_start",
      38             :     "ap_hook_setup_complete",
      39             :     "ap_hook_AHRS_update",
      40             :     "ap_hook_gyro_sample",
      41             :     "ap_hook_accel_sample",
      42             : };
      43             : 
      44             : /*
      45             :   scan a module for hook symbols
      46             :  */
      47           0 : void AP_Module::module_scan(const char *path)
      48             : {
      49             : #if defined(HAVE_LIBDL)
      50           0 :     void *m = dlopen(path, RTLD_NOW);
      51           0 :     if (m == nullptr) {
      52           0 :         printf("dlopen(%s) -> %s\n", path, dlerror());
      53           0 :         return;
      54             :     }
      55           0 :     uint8_t found_hooks = 0;
      56           0 :     for (uint16_t i=0; i<NUM_HOOKS; i++) {
      57           0 :         void *s = dlsym(m, hook_names[i]);
      58           0 :         if (s != nullptr) {
      59             :             // found a hook in this module, add it to the list
      60           0 :             struct hook_list *h = NEW_NOTHROW hook_list;
      61           0 :             if (h == nullptr) {
      62           0 :                 AP_HAL::panic("Failed to allocate hook for %s", hook_names[i]);
      63             :             }
      64           0 :             h->next = hooks[i];
      65           0 :             h->symbol = s;
      66           0 :             hooks[i] = h;
      67           0 :             found_hooks++;
      68             :         }
      69             :     }
      70           0 :     if (found_hooks == 0) {
      71             :         // we don't need this module
      72           0 :         dlclose(m);
      73             :     } else {
      74           0 :         printf("AP_Module: Loaded %u hooks from %s\n", (unsigned)found_hooks, path);
      75             :     }
      76             : #endif
      77             : }
      78             : 
      79             : /*
      80             :   initialise AP_Module, looking for shared libraries in the given module path
      81             : */
      82           0 : void AP_Module::init(const char *module_path)
      83             : {
      84             : #if AP_MODULE_SUPPORTED
      85             :     // scan through module directory looking for *.so
      86             :     DIR *d;
      87             :     struct dirent *de;
      88           0 :     d = opendir(module_path);
      89           0 :     if (d == nullptr) {
      90           0 :         return;
      91             :     }
      92           0 :     while ((de = readdir(d))) {
      93           0 :         const char *extension = strrchr(de->d_name, '.');
      94           0 :         if (extension == nullptr || strcmp(extension, ".so") != 0) {
      95           0 :             continue;
      96             :         }
      97           0 :         char *path = nullptr;
      98           0 :         if (asprintf(&path, "%s/%s", module_path, de->d_name) == -1) {
      99           0 :             continue;
     100             :         }
     101           0 :         module_scan(path);
     102           0 :         free(path);
     103             :     }
     104           0 :     closedir(d);
     105             : #endif
     106             : }
     107             : 
     108             : 
     109             : /*
     110             :   call any setup_start hooks
     111             : */
     112           0 : void AP_Module::call_hook_setup_start(void)
     113             : {
     114             : #if AP_MODULE_SUPPORTED
     115           0 :     uint64_t now = AP_HAL::micros64();
     116           0 :     for (const struct hook_list *h=hooks[HOOK_SETUP_START]; h; h=h->next) {
     117           0 :         ap_hook_setup_start_fn_t fn = reinterpret_cast<ap_hook_setup_start_fn_t>(h->symbol);
     118           0 :         fn(now);
     119             :     }
     120             : #endif
     121           0 : }
     122             : 
     123             : /*
     124             :   call any setup_complete hooks
     125             : */
     126           0 : void AP_Module::call_hook_setup_complete(void)
     127             : {
     128             : #if AP_MODULE_SUPPORTED
     129           0 :     uint64_t now = AP_HAL::micros64();
     130           0 :     for (const struct hook_list *h=hooks[HOOK_SETUP_COMPLETE]; h; h=h->next) {
     131           0 :         ap_hook_setup_complete_fn_t fn = reinterpret_cast<ap_hook_setup_complete_fn_t>(h->symbol);
     132           0 :         fn(now);
     133             :     }
     134             : #endif
     135           0 : }
     136             : 
     137             : /*
     138             :   call any AHRS_update hooks
     139             : */
     140           0 : void AP_Module::call_hook_AHRS_update(const AP_AHRS &ahrs)
     141             : {
     142             : #if AP_MODULE_SUPPORTED
     143           0 :     if (hooks[HOOK_AHRS_UPDATE] == nullptr) {
     144             :         // avoid filling in AHRS_state
     145           0 :         return;
     146             :     }
     147             : 
     148             :     /*
     149             :       construct AHRS_state structure
     150             :      */
     151           0 :     struct AHRS_state state {};
     152           0 :     state.structure_version = AHRS_state_version;
     153           0 :     state.time_us = AP_HAL::micros64();
     154             : 
     155           0 :     if (!ahrs.initialised()) {
     156           0 :         state.status = AHRS_STATUS_INITIALISING;
     157           0 :     } else if (ahrs.healthy()) {
     158           0 :         state.status = AHRS_STATUS_HEALTHY;
     159             :     } else {
     160           0 :         state.status = AHRS_STATUS_UNHEALTHY;
     161             :     }
     162             : 
     163           0 :     Quaternion q;
     164           0 :     q.from_rotation_matrix(ahrs.get_rotation_body_to_ned());
     165           0 :     state.quat[0] = q[0];
     166           0 :     state.quat[1] = q[1];
     167           0 :     state.quat[2] = q[2];
     168           0 :     state.quat[3] = q[3];
     169             : 
     170           0 :     state.eulers[0] = ahrs.get_roll_rad();
     171           0 :     state.eulers[1] = ahrs.get_pitch_rad();
     172           0 :     state.eulers[2] = ahrs.get_yaw_rad();
     173             : 
     174           0 :     Location loc;
     175           0 :     if (ahrs.get_origin(loc)) {
     176           0 :         state.origin.initialised = true;
     177           0 :         state.origin.latitude = loc.lat;
     178           0 :         state.origin.longitude = loc.lng;
     179           0 :         state.origin.altitude = loc.alt*0.01f;
     180             :     }
     181             : 
     182           0 :     if (ahrs.get_location(loc)) {
     183           0 :         state.position.available = true;
     184           0 :         state.position.latitude = loc.lat;
     185           0 :         state.position.longitude = loc.lng;
     186           0 :         state.position.altitude = loc.alt*0.01f;
     187             :     }
     188             :     
     189           0 :     Vector3f pos;
     190           0 :     if (ahrs.get_relative_position_NED_origin_float(pos)) {
     191           0 :         state.relative_position[0] = pos[0];
     192           0 :         state.relative_position[1] = pos[1];
     193           0 :         state.relative_position[2] = pos[2];
     194             :     }
     195             : 
     196           0 :     const Vector3f &gyro = ahrs.get_gyro();
     197           0 :     state.gyro_rate[0] = gyro[0];
     198           0 :     state.gyro_rate[1] = gyro[1];
     199           0 :     state.gyro_rate[2] = gyro[2];
     200             : 
     201           0 :     const Vector3f &accel_ef = ahrs.get_accel_ef();
     202           0 :     state.accel_ef[0] = accel_ef[0];
     203           0 :     state.accel_ef[1] = accel_ef[0];
     204           0 :     state.accel_ef[2] = accel_ef[0];
     205             : 
     206           0 :     state.primary_accel = ahrs.get_primary_accel_index();
     207           0 :     state.primary_gyro = ahrs.get_primary_gyro_index();
     208             : 
     209           0 :     const Vector3f &gyro_bias = ahrs.get_gyro_drift();
     210           0 :     state.gyro_bias[0] = gyro_bias[0];
     211           0 :     state.gyro_bias[1] = gyro_bias[1];
     212           0 :     state.gyro_bias[2] = gyro_bias[2];
     213             : 
     214           0 :     Vector3f vel;
     215           0 :     if (ahrs.get_velocity_NED(vel)) {
     216           0 :         state.velocity_ned[0] = vel.x;
     217           0 :         state.velocity_ned[1] = vel.y;
     218           0 :         state.velocity_ned[2] = vel.z;
     219             :     }
     220             :     
     221           0 :     for (const struct hook_list *h=hooks[HOOK_AHRS_UPDATE]; h; h=h->next) {
     222           0 :         ap_hook_AHRS_update_fn_t fn = reinterpret_cast<ap_hook_AHRS_update_fn_t>(h->symbol);
     223           0 :         fn(&state);
     224             :     }
     225             : #endif
     226             : }
     227             : 
     228             : 
     229             : /*
     230             :   call any gyro_sample hooks
     231             : */
     232           0 : void AP_Module::call_hook_gyro_sample(uint8_t instance, float dt, const Vector3f &gyro)
     233             : {
     234             : #if AP_MODULE_SUPPORTED
     235           0 :     if (hooks[HOOK_GYRO_SAMPLE] == nullptr) {
     236             :         // avoid filling in struct
     237           0 :         return;
     238             :     }
     239             : 
     240             :     /*
     241             :       construct gyro_sample structure
     242             :      */
     243           0 :     struct gyro_sample state {};
     244           0 :     state.structure_version = gyro_sample_version;
     245           0 :     state.time_us = AP_HAL::micros64();
     246           0 :     state.instance = instance;
     247           0 :     state.delta_time = dt;
     248           0 :     state.gyro[0] = gyro[0];
     249           0 :     state.gyro[1] = gyro[1];
     250           0 :     state.gyro[2] = gyro[2];
     251             : 
     252           0 :     for (const struct hook_list *h=hooks[HOOK_GYRO_SAMPLE]; h; h=h->next) {
     253           0 :         ap_hook_gyro_sample_fn_t fn = reinterpret_cast<ap_hook_gyro_sample_fn_t>(h->symbol);
     254           0 :         fn(&state);
     255             :     }
     256             : #endif
     257             : }
     258             : 
     259             : /*
     260             :   call any accel_sample hooks
     261             : */
     262           0 : void AP_Module::call_hook_accel_sample(uint8_t instance, float dt, const Vector3f &accel, bool fsync_set)
     263             : {
     264             : #if AP_MODULE_SUPPORTED
     265           0 :     if (hooks[HOOK_ACCEL_SAMPLE] == nullptr) {
     266             :         // avoid filling in struct
     267           0 :         return;
     268             :     }
     269             : 
     270             :     /*
     271             :       construct accel_sample structure
     272             :      */
     273           0 :     struct accel_sample state {};
     274           0 :     state.structure_version = accel_sample_version;
     275           0 :     state.time_us = AP_HAL::micros64();
     276           0 :     state.instance = instance;
     277           0 :     state.delta_time = dt;
     278           0 :     state.accel[0] = accel[0];
     279           0 :     state.accel[1] = accel[1];
     280           0 :     state.accel[2] = accel[2];
     281           0 :     state.fsync_set = fsync_set;
     282             : 
     283           0 :     for (const struct hook_list *h=hooks[HOOK_ACCEL_SAMPLE]; h; h=h->next) {
     284           0 :         ap_hook_accel_sample_fn_t fn = reinterpret_cast<ap_hook_accel_sample_fn_t>(h->symbol);
     285           0 :         fn(&state);
     286             :     }
     287             : #endif
     288             : }
     289             : 
     290             : #endif // AP_MODULE_SUPPORTED