RPM: Send on Periph and receive on vehicle

Tools/AP_Periph/AP_Periph.h

@@ -91,6 +91,10 @@
     #endif
 #endif
 
+#if defined(HAL_PERIPH_ENABLE_RPM_STREAM) && !defined(HAL_PERIPH_ENABLE_RPM)
+    #error "HAL_PERIPH_ENABLE_RPM_STREAM requires HAL_PERIPH_ENABLE_RPM"
+#endif
+
 #ifndef AP_PERIPH_SAFETY_SWITCH_ENABLED
 #define AP_PERIPH_SAFETY_SWITCH_ENABLED defined(HAL_PERIPH_ENABLE_RC_OUT)
 #endif
@@ -229,7 +233,12 @@ class AP_Periph_FW {
 #ifdef HAL_PERIPH_ENABLE_RPM
     AP_RPM rpm_sensor;
     uint32_t rpm_last_update_ms;
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+    void rpm_sensor_send();
+    uint32_t rpm_last_send_ms;
+    uint8_t rpm_last_sent_index;
 #endif
+#endif // HAL_PERIPH_ENABLE_RPM
 
 #ifdef HAL_PERIPH_ENABLE_BATTERY
     void handle_battery_failsafe(const char* type_str, const int8_t action) { }

Tools/AP_Periph/Parameters.cpp

@@ -692,6 +692,17 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @User: Standard
     GSCALAR(options, "OPTIONS", AP_PERIPH_PROBE_CONTINUOUS),
 
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+    // @Param: RPM_MSG_RATE
+    // @DisplayName: RPM sensor message rate
+    // @Description: This is the rate RPM sensor data is sent in Hz. Zero means no send. Each sensor with a set ID is sent in turn.
+    // @Units: Hz
+    // @Range: 0 200
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(rpm_msg_rate, "RPM_MSG_RATE", 0),
+#endif
+
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -94,6 +94,7 @@ class Parameters {
         k_param_rangefinder_baud1,
         k_param_rangefinder_port1,
         k_param_options,
+        k_param_rpm_msg_rate,
     };
 
     AP_Int16 format_version;
@@ -212,6 +213,10 @@ class Parameters {
     AP_Int8 temperature_msg_rate;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+    AP_Int16 rpm_msg_rate;
+#endif
+
 #if HAL_CANFD_SUPPORTED
     AP_Int8 can_fdmode;
     AP_Int8 can_fdbaudrate[HAL_NUM_CAN_IFACES];

Tools/AP_Periph/can.cpp

@@ -1848,6 +1848,9 @@ void AP_Periph_FW::can_update()
     #endif
 #ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE
         temperature_sensor_update();
+#endif
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+        rpm_sensor_send();
 #endif
     }
     const uint32_t now_us = AP_HAL::micros();

Tools/AP_Periph/rpm.cpp

@@ -0,0 +1,55 @@
+#include "AP_Periph.h"
+
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+
+#include <dronecan_msgs.h>
+
+// Send rpm message occasionally
+void AP_Periph_FW::rpm_sensor_send(void)
+{
+    if (g.rpm_msg_rate <= 0) {
+        return;
+    }
+
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - rpm_last_send_ms < (1000U / g.rpm_msg_rate)) {
+        return;
+    }
+    rpm_last_send_ms = now_ms;
+
+    {
+        const uint8_t num_sensors = rpm_sensor.num_sensors();
+        for (uint8_t i = 0; i < num_sensors; i++) {
+            // Send each sensor in turn
+            const uint8_t index = (rpm_last_sent_index + 1 + i) % num_sensors;
+
+            const int8_t sensor_id = rpm_sensor.get_dronecan_sensor_id(index);
+            if (sensor_id < 0) {
+                // disabled or not configured to send
+                continue;
+            }
+
+            dronecan_sensors_rpm_RPM pkt {};
+            pkt.sensor_id = sensor_id;
+
+            // Get rpm and set health flag
+            if (!rpm_sensor.get_rpm(index, pkt.rpm)) {
+                pkt.flags |= DRONECAN_SENSORS_RPM_RPM_FLAGS_UNHEALTHY;
+            }
+
+            uint8_t buffer[DRONECAN_SENSORS_RPM_RPM_MAX_SIZE] {};
+            const uint16_t total_size = dronecan_sensors_rpm_RPM_encode(&pkt, buffer, !canfdout());
+
+            canard_broadcast(DRONECAN_SENSORS_RPM_RPM_SIGNATURE,
+                             DRONECAN_SENSORS_RPM_RPM_ID,
+                             CANARD_TRANSFER_PRIORITY_LOW,
+                             &buffer[0],
+                             total_size);
+
+            rpm_last_sent_index = index;
+            break;
+        }
+    }
+}
+
+#endif // HAL_PERIPH_ENABLE_RPM_STREAM

Tools/ardupilotwaf/boards.py

@@ -915,6 +915,7 @@ def configure_env(self, cfg, env):
             HAL_PERIPH_ENABLE_BATTERY = 1,
             HAL_PERIPH_ENABLE_EFI = 1,
             HAL_PERIPH_ENABLE_RPM = 1,
+            HAL_PERIPH_ENABLE_RPM_STREAM = 1,
             HAL_PERIPH_ENABLE_RC_OUT = 1,
             HAL_PERIPH_ENABLE_ADSB = 1,
             HAL_PERIPH_ENABLE_SERIAL_OPTIONS = 1,

libraries/AP_DroneCAN/AP_DroneCAN.cpp

@@ -61,6 +61,8 @@
 
 #include <AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h>
 
+#include <AP_RPM/RPM_DroneCAN.h>
+
 extern const AP_HAL::HAL& hal;
 
 // setup default pool size
@@ -396,6 +398,9 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
 #if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
     AP_TemperatureSensor_DroneCAN::subscribe_msgs(this);
 #endif
+#if AP_RPM_DRONECAN_ENABLED
+    AP_RPM_DroneCAN::subscribe_msgs(this);
+#endif
 
     act_out_array.set_timeout_ms(5);
     act_out_array.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);

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]);
@@ -304,6 +310,18 @@ void AP_RPM::Log_RPM() const
 }
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+// Return the sensor id to use for streaming over DroneCAN, negative number disables
+int8_t AP_RPM::get_dronecan_sensor_id(uint8_t instance) const
+{
+    if (!enabled(instance)) {
+        return -1;
+    }
+    return _params[instance].dronecan_sensor_id;
+}
+#endif
+
+
 // singleton instance
 AP_RPM *AP_RPM::_singleton;
 

libraries/AP_RPM/AP_RPM.h

@@ -54,6 +54,9 @@ class AP_RPM
 #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
@@ -104,6 +107,11 @@ class AP_RPM
     // check settings are valid
     bool arming_checks(size_t buflen, char *buffer) const;
 
+#ifdef HAL_PERIPH_ENABLE_RPM_STREAM
+    // Return the sensor id to use for streaming over DroneCAN, negative number disables
+    int8_t get_dronecan_sensor_id(uint8_t instance) const;
+#endif
+
 private:
     void convert_params(void);
 

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,6 +78,17 @@ const AP_Param::GroupInfo AP_RPM_Params::var_info[] = {
     AP_GROUPINFO("ESC_INDEX", 8, AP_RPM_Params, esc_telem_outbound_index, 0),
 #endif
 
+#if AP_RPM_DRONECAN_ENABLED || defined(HAL_PERIPH_ENABLE_RPM_STREAM)
+    // @Param: DC_ID
+    // @DisplayName: DroneCAN Sensor ID
+    // @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_ID", 9, AP_RPM_Params, dronecan_sensor_id, -1),
+#endif
+
     AP_GROUPEND
 };
 

libraries/AP_RPM/AP_RPM_Params.h

@@ -33,7 +33,9 @@ class AP_RPM_Params {
 #if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED
     AP_Int8  esc_telem_outbound_index;
 #endif
-
+#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