From dff29a5015a5d52106affdc1717a40502ecd9ff1 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 14 Mar 2024 00:58:46 +0000 Subject: [PATCH] AP_RPM: add DroneCAN backend --- libraries/AP_RPM/AP_RPM.cpp | 6 ++ libraries/AP_RPM/AP_RPM.h | 3 + libraries/AP_RPM/AP_RPM_Params.cpp | 11 ++-- libraries/AP_RPM/AP_RPM_Params.h | 3 +- libraries/AP_RPM/AP_RPM_config.h | 7 +++ libraries/AP_RPM/RPM_DroneCAN.cpp | 88 ++++++++++++++++++++++++++++++ libraries/AP_RPM/RPM_DroneCAN.h | 52 ++++++++++++++++++ 7 files changed, 163 insertions(+), 7 deletions(-) create mode 100644 libraries/AP_RPM/RPM_DroneCAN.cpp create mode 100644 libraries/AP_RPM/RPM_DroneCAN.h diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index adb9bc62cd..228e845294 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -24,6 +24,7 @@ #include "RPM_Generator.h" #include "RPM_HarmonicNotch.h" #include "RPM_ESC_Telem.h" +#include "RPM_DroneCAN.h" #include @@ -99,6 +100,11 @@ void AP_RPM::init(void) drivers[i] = new AP_RPM_HarmonicNotch(*this, i, state[i]); break; #endif // AP_RPM_HARMONICNOTCH_ENABLED +#if AP_RPM_DRONECAN_ENABLED + case RPM_TYPE_DRONECAN: + drivers[i] = new AP_RPM_DroneCAN(*this, i, state[i]); + break; +#endif // AP_RPM_DRONECAN_ENABLED #if AP_RPM_SIM_ENABLED case RPM_TYPE_SITL: drivers[i] = new AP_RPM_SITL(*this, i, state[i]); diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h index bb5b5b4612..e2643527cf 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/libraries/AP_RPM/AP_RPM.h @@ -54,6 +54,9 @@ public: #if AP_RPM_GENERATOR_ENABLED RPM_TYPE_GENERATOR = 6, #endif +#if AP_RPM_DRONECAN_ENABLED + RPM_TYPE_DRONECAN = 7, +#endif #if AP_RPM_SIM_ENABLED RPM_TYPE_SITL = 10, #endif diff --git a/libraries/AP_RPM/AP_RPM_Params.cpp b/libraries/AP_RPM/AP_RPM_Params.cpp index 1d9ed86d64..dc6f9bb116 100644 --- a/libraries/AP_RPM/AP_RPM_Params.cpp +++ b/libraries/AP_RPM/AP_RPM_Params.cpp @@ -20,7 +20,7 @@ const AP_Param::GroupInfo AP_RPM_Params::var_info[] = { // @Param: TYPE // @DisplayName: RPM type // @Description: What type of RPM sensor is connected - // @Values: 0:None,1:Not Used,2:GPIO,3:EFI,4:Harmonic Notch,5:ESC Telemetry Motors Bitmask,6:Generator + // @Values: 0:None,1:Not Used,2:GPIO,3:EFI,4:Harmonic Notch,5:ESC Telemetry Motors Bitmask,6:Generator,7:DroneCAN // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_RPM_Params, type, 0, AP_PARAM_FLAG_ENABLE), // Note, 1 was previously for type = PWM. This has been removed from docs to make setup less confusing for users. @@ -78,14 +78,15 @@ const AP_Param::GroupInfo AP_RPM_Params::var_info[] = { AP_GROUPINFO("ESC_INDEX", 8, AP_RPM_Params, esc_telem_outbound_index, 0), #endif -#ifdef HAL_PERIPH_ENABLE_RPM_STREAM - // @Param: DC_SEND_ID +#if AP_RPM_DRONECAN_ENABLED || defined(HAL_PERIPH_ENABLE_RPM_STREAM) + // @Param: DC_ID // @DisplayName: DroneCAN Sensor ID - // @Description: DroneCAN sensor ID to send as on AP-Periph -1 disables + // @Description: DroneCAN sensor ID to assign to this backend + // @Description{AP_Periph}: DroneCAN sensor ID to send as on AP-Periph -1 disables // @Range: -1 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("DC_SEND_ID", 9, AP_RPM_Params, dronecan_sensor_id, -1), + AP_GROUPINFO("DC_ID", 9, AP_RPM_Params, dronecan_sensor_id, -1), #endif AP_GROUPEND diff --git a/libraries/AP_RPM/AP_RPM_Params.h b/libraries/AP_RPM/AP_RPM_Params.h index 194fb464c3..5d2f6a7cf9 100644 --- a/libraries/AP_RPM/AP_RPM_Params.h +++ b/libraries/AP_RPM/AP_RPM_Params.h @@ -33,10 +33,9 @@ public: #if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED AP_Int8 esc_telem_outbound_index; #endif -#ifdef HAL_PERIPH_ENABLE_RPM_STREAM +#if AP_RPM_DRONECAN_ENABLED || defined(HAL_PERIPH_ENABLE_RPM_STREAM) AP_Int8 dronecan_sensor_id; #endif - static const struct AP_Param::GroupInfo var_info[]; }; diff --git a/libraries/AP_RPM/AP_RPM_config.h b/libraries/AP_RPM/AP_RPM_config.h index e07aee1b3d..03f7868cef 100644 --- a/libraries/AP_RPM/AP_RPM_config.h +++ b/libraries/AP_RPM/AP_RPM_config.h @@ -46,3 +46,10 @@ #ifndef AP_RPM_ESC_TELEM_OUTBOUND_ENABLED #define AP_RPM_ESC_TELEM_OUTBOUND_ENABLED AP_RPM_BACKEND_DEFAULT_ENABLED && HAL_WITH_ESC_TELEM #endif + +#ifndef AP_RPM_DRONECAN_ENABLED +#define AP_RPM_DRONECAN_ENABLED AP_RPM_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS +#endif +#if AP_RPM_DRONECAN_ENABLED && !HAL_ENABLE_DRONECAN_DRIVERS + #error AP_RPM_DRONECAN_ENABLED requires HAL_ENABLE_DRONECAN_DRIVERS +#endif diff --git a/libraries/AP_RPM/RPM_DroneCAN.cpp b/libraries/AP_RPM/RPM_DroneCAN.cpp new file mode 100644 index 0000000000..4657ff7714 --- /dev/null +++ b/libraries/AP_RPM/RPM_DroneCAN.cpp @@ -0,0 +1,88 @@ +/* + 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 . + */ + +#include "AP_RPM_config.h" + +#if AP_RPM_DRONECAN_ENABLED + +#include "RPM_DroneCAN.h" +#include + +AP_RPM_DroneCAN* AP_RPM_DroneCAN::_drivers[]; +uint8_t AP_RPM_DroneCAN::_driver_instance; +HAL_Semaphore AP_RPM_DroneCAN::_driver_sem; + +AP_RPM_DroneCAN::AP_RPM_DroneCAN(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state) : + AP_RPM_Backend(_ap_rpm, instance, _state) +{ + // Register self in static driver list + WITH_SEMAPHORE(_driver_sem); + _drivers[_driver_instance] = this; + _driver_instance++; +} + +// Subscribe to incoming rpm messages +void AP_RPM_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +{ + if (ap_dronecan == nullptr) { + return; + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_rpm, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("rpm_sub"); + } +} + +// Receive new CAN message +void AP_RPM_DroneCAN::handle_rpm(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_rpm_RPM &msg) +{ + WITH_SEMAPHORE(_driver_sem); + + for (uint8_t i = 0; i < _driver_instance; i++) { + if (_drivers[i] == nullptr) { + continue; + } + // Find params for this instance + const uint8_t instance = _drivers[i]->state.instance; + const AP_RPM_Params& params = _drivers[i]->ap_rpm._params[instance]; + + if (params.dronecan_sensor_id == msg.sensor_id) { + // Driver loaded and looking for this ID, add reading + _drivers[i]->last_reading_ms = AP_HAL::millis(); + _drivers[i]->rpm = msg.rpm * params.scaling; + + const bool heathy = (msg.flags & DRONECAN_SENSORS_RPM_RPM_FLAGS_UNHEALTHY) == 0; + _drivers[i]->signal_quality = heathy ? 0.5 : 0.0; + } + } +} + +void AP_RPM_DroneCAN::update(void) +{ + WITH_SEMAPHORE(_driver_sem); + + // Update state from temporay variables + state.last_reading_ms = last_reading_ms; + state.signal_quality = signal_quality; + state.rate_rpm = rpm; + + // assume we get readings at at least 1Hz, otherwise reset quality to zero + if ((AP_HAL::millis() - state.last_reading_ms) > 1000) { + state.signal_quality = 0; + state.rate_rpm = 0; + } +} + +#endif // AP_RPM_DRONECAN_ENABLED diff --git a/libraries/AP_RPM/RPM_DroneCAN.h b/libraries/AP_RPM/RPM_DroneCAN.h new file mode 100644 index 0000000000..c9d8e8c0c5 --- /dev/null +++ b/libraries/AP_RPM/RPM_DroneCAN.h @@ -0,0 +1,52 @@ +/* + 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 . + */ +#pragma once + +#include "AP_RPM_config.h" + +#if AP_RPM_DRONECAN_ENABLED + +#include "RPM_Backend.h" +#include + +class AP_RPM_DroneCAN : public AP_RPM_Backend +{ +public: + AP_RPM_DroneCAN(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state); + + // Subscribe to incoming rpm messages + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + + // update state + void update(void) override; + +private: + + // Receive new CAN message + static void handle_rpm(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_rpm_RPM &msg); + + // Temporay variables used to update main state in update call + float rpm; + uint32_t last_reading_ms; + float signal_quality; + + // Static list of drivers + static AP_RPM_DroneCAN *_drivers[RPM_MAX_INSTANCES]; + static uint8_t _driver_instance; + static HAL_Semaphore _driver_sem; + +}; + +#endif // AP_RPM_DRONECAN_ENABLED