AP_RPM: add DroneCAN backend

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 <AP_Logger/AP_Logger.h>
 
@@ -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]);

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

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
+#if AP_RPM_DRONECAN_ENABLED || defined(HAL_PERIPH_ENABLE_RPM_STREAM)
-    // @Param: DC_SEND_ID
+    // @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

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[];
 
 };

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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_RPM_config.h"
+
+#if AP_RPM_DRONECAN_ENABLED
+
+#include "RPM_DroneCAN.h"
+#include <AP_BoardConfig/AP_BoardConfig.h>
+
+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

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 <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include "AP_RPM_config.h"
+
+#if AP_RPM_DRONECAN_ENABLED
+
+#include "RPM_Backend.h"
+#include <AP_DroneCAN/AP_DroneCAN.h>
+
+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