diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 34ec6bba84..47c1d80d94 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -163,6 +163,10 @@ void AP_Periph_FW::init() battery.lib.init(); #endif +#ifdef HAL_PERIPH_ENABLE_RCIN + rcin_init(); +#endif + #if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_RC_OUT) hal.rcout->init(); #endif @@ -457,6 +461,10 @@ void AP_Periph_FW::update() } #endif +#ifdef HAL_PERIPH_ENABLE_RCIN + rcin_update(); +#endif + static uint32_t fiftyhz_last_update_ms; if (now - fiftyhz_last_update_ms >= 20) { // update at 50Hz diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index aaa4c3ed9c..649b787467 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -32,6 +32,7 @@ #if HAL_WITH_ESC_TELEM #include #endif +#include #include #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS)) @@ -278,6 +279,15 @@ public: void rcout_handle_safety_state(uint8_t safety_state); #endif +#ifdef HAL_PERIPH_ENABLE_RCIN + void rcin_init(); + void rcin_update(); + void can_send_RCInput(uint8_t quality, uint16_t *values, uint8_t nvalues, bool in_failsafe, bool quality_valid); + bool rcin_initialised; + uint32_t rcin_last_sent_RCInput_ms; + const char *rcin_rc_protocol; // protocol currently being decoded +#endif + #if AP_TEMPERATURE_SENSOR_ENABLED AP_TemperatureSensor temperature_sensor; #endif diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index c10c43db93..d064e96f30 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -16,6 +16,10 @@ extern const AP_HAL::HAL &hal; #define AP_PERIPH_RANGEFINDER_PORT_DEFAULT 3 #endif +#ifndef AP_PERIPH_RC1_PORT_DEFAULT +#define AP_PERIPH_RC1_PORT_DEFAULT -1 +#endif + #ifndef HAL_PERIPH_GPS_PORT_DEFAULT #define HAL_PERIPH_GPS_PORT_DEFAULT 3 #endif @@ -563,6 +567,35 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(rpm_sensor, "RPM", AP_RPM), #endif +#ifdef HAL_PERIPH_ENABLE_RCIN + // RC_PROTOCOLS copied from RC_Channel/RC_Channels_Varinfo.h + // @Param: RC_PROTOCOLS + // @DisplayName: RC protocols enabled + // @Description: Bitmask of enabled RC protocols. Allows narrowing the protocol detection to only specific types of RC receivers which can avoid issues with incorrect detection. Set to 1 to enable all protocols. + // @User: Advanced + // @Bitmask: 0:All,1:PPM,2:IBUS,3:SBUS,4:SBUS_NI,5:DSM,6:SUMD,7:SRXL,8:SRXL2,9:CRSF,10:ST24,11:FPORT,12:FPORT2,13:FastSBUS + GSCALAR(rcin_protocols, "RC_PROTOCOLS", 1), + + // RC_PROTOCOLS copied from RC_Channel/RC_Channels_Varinfo.h + // @Param: RC_MSGRATE + // @DisplayName: DroneCAN RC Message rate + // @Description: Rate at which RC input is sent via DroneCAN + // @User: Advanced + // @Increment: 1 + // @Range: 0 255 + // @Units: Hz + GSCALAR(rcin_rate_hz, "RC_MSGRATE", 50), + + // @Param: RC1_PORT + // @DisplayName: RC input port + // @Description: This is the serial port number where SERIALx_PROTOCOL will be set to RC input. + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + // @RebootRequired: True + GSCALAR(rcin1_port, "RC1_PORT", AP_PERIPH_RC1_PORT_DEFAULT), +#endif // HAL_PERIPH_ENABLE_RCIN + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index f9e3eedb1e..af0c2942b0 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -78,6 +78,9 @@ public: k_param_esc_serial_port1, k_param_networking, k_param_rpm_sensor, + k_param_rcin_protocols, + k_param_rcin_rate_hz, + k_param_rcin1_port, }; AP_Int16 format_version; @@ -111,6 +114,12 @@ public: AP_Int16 rangefinder_max_rate; #endif +#ifdef HAL_PERIPH_ENABLE_RCIN + AP_Int32 rcin_protocols; + AP_Int8 rcin_rate_hz; + AP_Int8 rcin1_port; +#endif + #if HAL_PROXIMITY_ENABLED AP_Int32 proximity_baud; AP_Int8 proximity_port; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 1122b823bb..abc7b54e51 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -2771,7 +2771,6 @@ void AP_Periph_FW::can_efi_update(void) } #endif // HAL_PERIPH_ENABLE_EFI - // printf to CAN LogMessage for debugging void can_printf(const char *fmt, ...) { diff --git a/Tools/AP_Periph/rc_in.cpp b/Tools/AP_Periph/rc_in.cpp new file mode 100644 index 0000000000..bcca79936e --- /dev/null +++ b/Tools/AP_Periph/rc_in.cpp @@ -0,0 +1,126 @@ +/* + 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 + +#ifdef HAL_PERIPH_ENABLE_RCIN + +#include +#include "AP_Periph.h" +#include + +extern const AP_HAL::HAL &hal; + +void AP_Periph_FW::rcin_init() +{ + if (g.rcin1_port == 0) { + return; + } + + // init uart for serial RC + auto *uart = hal.serial(g.rcin1_port); + if (uart == nullptr) { + return; + } + + serial_manager.set_protocol_and_baud( + g.rcin1_port, + AP_SerialManager::SerialProtocol_RCIN, + 115200 // baud doesn't matter; RC Protocol autobauds + ); + + auto &rc = AP::RC(); + rc.init(); + rc.set_rc_protocols(g.rcin_protocols); + rc.add_uart(uart); + + rcin_initialised = true; +} + +void AP_Periph_FW::rcin_update() +{ + if (!rcin_initialised) { + return; + } + + auto &rc = AP::RC(); + if (!rc.new_input()) { + return; + } + + // log discovered protocols: + auto new_rc_protocol = rc.protocol_name(); + if (new_rc_protocol != rcin_rc_protocol) { + can_printf("Decoding (%s)", new_rc_protocol); + rcin_rc_protocol = new_rc_protocol; + } + + // decimate the input to a parameterized rate + const uint8_t rate_hz = g.rcin_rate_hz; + if (rate_hz == 0) { + return; + } + + const auto now_ms = AP_HAL::millis(); + const auto interval_ms = 1000U / rate_hz; + if (now_ms - rcin_last_sent_RCInput_ms < interval_ms) { + return; + } + rcin_last_sent_RCInput_ms = now_ms; + + // extract data and send CAN packet: + const uint8_t num_channels = rc.num_channels(); + uint16_t channels[MAX_RCIN_CHANNELS]; + rc.read(channels, num_channels); + const int16_t rssi = rc.get_RSSI(); + + can_send_RCInput((uint8_t)rssi, channels, num_channels, rc.failsafe_active(), rssi > 0 && rssi <256); +} + +/* + send an RCInput CAN message + */ +void AP_Periph_FW::can_send_RCInput(uint8_t quality, uint16_t *values, uint8_t nvalues, bool in_failsafe, bool quality_valid) +{ + uint16_t status = 0; + if (quality_valid) { + status |= DRONECAN_SENSORS_RC_RCINPUT_STATUS_QUALITY_VALID; + } + if (in_failsafe) { + status |= DRONECAN_SENSORS_RC_RCINPUT_STATUS_FAILSAFE; + } + + // assemble packet + dronecan_sensors_rc_RCInput pkt {}; + pkt.quality = quality; + pkt.status = status; + pkt.rcin.len = nvalues; + for (uint8_t i=0; i