From 2e18eebc6f3e301a840a62c9101621e93cd7b995 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 14 Mar 2024 00:07:27 +0000 Subject: [PATCH 1/6] AP_Periph: add support for streaming dedicated RPM message --- Tools/AP_Periph/AP_Periph.h | 9 ++++++ Tools/AP_Periph/Parameters.cpp | 11 +++++++ Tools/AP_Periph/Parameters.h | 5 ++++ Tools/AP_Periph/can.cpp | 3 ++ Tools/AP_Periph/rpm.cpp | 55 ++++++++++++++++++++++++++++++++++ 5 files changed, 83 insertions(+) create mode 100644 Tools/AP_Periph/rpm.cpp diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index eff5ccd6b0464..b6db8f6a11c2c 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/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) { } diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 2876836170670..61e42c29f342f 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/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 }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 9d51c97cb0488..d68e38f7b4ecc 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/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]; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 80ae6fbc4fc9c..1b5607cdab160 100644 --- a/Tools/AP_Periph/can.cpp +++ b/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(); diff --git a/Tools/AP_Periph/rpm.cpp b/Tools/AP_Periph/rpm.cpp new file mode 100644 index 0000000000000..5a39866013b7e --- /dev/null +++ b/Tools/AP_Periph/rpm.cpp @@ -0,0 +1,55 @@ +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_RPM_STREAM + +#include + +// 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 From e144a474cc8cda4a12ed40043fbaa0e3b731489f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 14 Mar 2024 00:09:20 +0000 Subject: [PATCH 2/6] AP_RPM: add DC_SEND_ID for periph RPM stream --- libraries/AP_RPM/AP_RPM.cpp | 12 ++++++++++++ libraries/AP_RPM/AP_RPM.h | 5 +++++ libraries/AP_RPM/AP_RPM_Params.cpp | 10 ++++++++++ libraries/AP_RPM/AP_RPM_Params.h | 3 +++ 4 files changed, 30 insertions(+) diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index ef0061553eaa7..adb9bc62cd2d8 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -304,6 +304,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; diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h index 6a6b0e4817570..bb5b5b4612202 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/libraries/AP_RPM/AP_RPM.h @@ -104,6 +104,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); diff --git a/libraries/AP_RPM/AP_RPM_Params.cpp b/libraries/AP_RPM/AP_RPM_Params.cpp index 1d58d41eb693b..1d9ed86d6434d 100644 --- a/libraries/AP_RPM/AP_RPM_Params.cpp +++ b/libraries/AP_RPM/AP_RPM_Params.cpp @@ -78,6 +78,16 @@ 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 + // @DisplayName: DroneCAN Sensor ID + // @Description: 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), +#endif + AP_GROUPEND }; diff --git a/libraries/AP_RPM/AP_RPM_Params.h b/libraries/AP_RPM/AP_RPM_Params.h index 591bd76e7fdaa..194fb464c3733 100644 --- a/libraries/AP_RPM/AP_RPM_Params.h +++ b/libraries/AP_RPM/AP_RPM_Params.h @@ -33,6 +33,9 @@ class AP_RPM_Params { #if AP_RPM_ESC_TELEM_OUTBOUND_ENABLED AP_Int8 esc_telem_outbound_index; #endif +#ifdef HAL_PERIPH_ENABLE_RPM_STREAM + AP_Int8 dronecan_sensor_id; +#endif static const struct AP_Param::GroupInfo var_info[]; From 308528f235b850e39032df08778ca623841ea4a4 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 14 Mar 2024 00:09:49 +0000 Subject: [PATCH 3/6] Tools: ardupilotwaf: boards: enable `HAL_PERIPH_ENABLE_RPM_STREAM` on `sitl_periph_universal` --- Tools/ardupilotwaf/boards.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 9b6824fc67679..ed4b653a49054 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/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, From b11c50f5dda9ba34936a4ce375c6338a7fb806db Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 14 Mar 2024 00:58:46 +0000 Subject: [PATCH 4/6] 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 adb9bc62cd2d8..228e845294c66 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 bb5b5b4612202..e2643527cf161 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/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 diff --git a/libraries/AP_RPM/AP_RPM_Params.cpp b/libraries/AP_RPM/AP_RPM_Params.cpp index 1d9ed86d6434d..dc6f9bb116e41 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 194fb464c3733..5d2f6a7cf925a 100644 --- a/libraries/AP_RPM/AP_RPM_Params.h +++ b/libraries/AP_RPM/AP_RPM_Params.h @@ -33,10 +33,9 @@ class AP_RPM_Params { #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 e07aee1b3dd06..03f7868ceface 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 0000000000000..4657ff7714be6 --- /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 0000000000000..c9d8e8c0c511a --- /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 From e274f6609ece2aec91aa198e1dee5fa9e36b3c16 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 14 Mar 2024 00:59:02 +0000 Subject: [PATCH 5/6] AP_DroneCAN: call RPM subscribe --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index b72899bf72cae..67c629b241616 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -61,6 +61,8 @@ #include +#include + 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); From f6b26e69273a362b13b107dc42a6a2570d186264 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 1 May 2024 01:45:16 +0100 Subject: [PATCH 6/6] DroneCAN: DSDL: add RPM message --- modules/DroneCAN/DSDL | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneCAN/DSDL b/modules/DroneCAN/DSDL index eb370f198f17e..993be80a62ec9 160000 --- a/modules/DroneCAN/DSDL +++ b/modules/DroneCAN/DSDL @@ -1 +1 @@ -Subproject commit eb370f198f17e561706d6eaddb02cb06d9e91cf6 +Subproject commit 993be80a62ec957c01fb41115b83663959a49f46