AP_Periph: move RC IN parameters to sub-object

This commit is contained in:
Peter Barker 2023-08-16 08:13:02 +10:00 committed by Peter Barker
parent 312ca017a0
commit 9fb872cdd7
5 changed files with 83 additions and 60 deletions

View File

@ -33,6 +33,7 @@
#include <AP_ESC_Telem/AP_ESC_Telem.h> #include <AP_ESC_Telem/AP_ESC_Telem.h>
#endif #endif
#include <AP_RCProtocol/AP_RCProtocol_config.h> #include <AP_RCProtocol/AP_RCProtocol_config.h>
#include "rc_in.h"
#include <AP_NMEA_Output/AP_NMEA_Output.h> #include <AP_NMEA_Output/AP_NMEA_Output.h>
#if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS)) #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS))
@ -286,6 +287,7 @@ public:
bool rcin_initialised; bool rcin_initialised;
uint32_t rcin_last_sent_RCInput_ms; uint32_t rcin_last_sent_RCInput_ms;
const char *rcin_rc_protocol; // protocol currently being decoded const char *rcin_rc_protocol; // protocol currently being decoded
Parameters_RCIN g_rcin;
#endif #endif
#if AP_TEMPERATURE_SENSOR_ENABLED #if AP_TEMPERATURE_SENSOR_ENABLED

View File

@ -16,14 +16,6 @@ extern const AP_HAL::HAL &hal;
#define AP_PERIPH_RANGEFINDER_PORT_DEFAULT 3 #define AP_PERIPH_RANGEFINDER_PORT_DEFAULT 3
#endif #endif
#ifndef AP_PERIPH_RC1_PORT_DEFAULT
#define AP_PERIPH_RC1_PORT_DEFAULT -1
#endif
#ifndef AP_PERIPH_RC1_PORT_OPTIONS_DEFAULT
#define AP_PERIPH_RC1_PORT_OPTIONS_DEFAULT 0
#endif
#ifndef HAL_PERIPH_GPS_PORT_DEFAULT #ifndef HAL_PERIPH_GPS_PORT_DEFAULT
#define HAL_PERIPH_GPS_PORT_DEFAULT 3 #define HAL_PERIPH_GPS_PORT_DEFAULT 3
#endif #endif
@ -572,41 +564,10 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
#endif #endif
#ifdef HAL_PERIPH_ENABLE_RCIN #ifdef HAL_PERIPH_ENABLE_RCIN
// RC_PROTOCOLS copied from RC_Channel/RC_Channels_Varinfo.h // @Group: RC
// @Param: RC_PROTOCOLS // @Path: rc_in.cpp
// @DisplayName: RC protocols enabled GOBJECT(g_rcin, "RC", Parameters_RCIN),
// @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. #endif
// @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),
// @Param: RC1_PORT_OPTIONS
// @DisplayName: RC input port serial options
// @Description: This is the serial port number where SERIALx_PROTOCOL will be set to RC input.
// @Description: Control over UART options. The InvertRX option controls invert of the receive pin. The InvertTX option controls invert of the transmit pin. The HalfDuplex option controls half-duplex (onewire) mode, where both transmit and receive is done on the transmit wire. The Swap option allows the RX and TX pins to be swapped on STM32F7 based boards.
// @Bitmask: 0:InvertRX, 1:InvertTX, 2:HalfDuplex, 3:SwapTXRX, 4: RX_PullDown, 5: RX_PullUp, 6: TX_PullDown, 7: TX_PullUp, 8: RX_NoDMA, 9: TX_NoDMA, 10: Don't forward mavlink to/from, 11: DisableFIFO, 12: Ignore Streamrate
GSCALAR(rcin1_port_options, "RC1_PORT_OPTIONS", AP_PERIPH_RC1_PORT_OPTIONS_DEFAULT),
// @RebootRequired: True
#endif // HAL_PERIPH_ENABLE_RCIN
AP_VAREND AP_VAREND
}; };

View File

@ -78,10 +78,7 @@ public:
k_param_esc_serial_port1, k_param_esc_serial_port1,
k_param_networking, k_param_networking,
k_param_rpm_sensor, k_param_rpm_sensor,
k_param_rcin_protocols, k_param_g_rcin,
k_param_rcin_rate_hz,
k_param_rcin1_port,
k_param_rcin1_port_options,
}; };
AP_Int16 format_version; AP_Int16 format_version;
@ -115,13 +112,6 @@ public:
AP_Int16 rangefinder_max_rate; AP_Int16 rangefinder_max_rate;
#endif #endif
#ifdef HAL_PERIPH_ENABLE_RCIN
AP_Int32 rcin_protocols;
AP_Int8 rcin_rate_hz;
AP_Int8 rcin1_port;
AP_Int16 rcin1_port_options;
#endif
#if HAL_PROXIMITY_ENABLED #if HAL_PROXIMITY_ENABLED
AP_Int32 proximity_baud; AP_Int32 proximity_baud;
AP_Int8 proximity_port; AP_Int8 proximity_port;

View File

@ -17,35 +17,87 @@
#ifdef HAL_PERIPH_ENABLE_RCIN #ifdef HAL_PERIPH_ENABLE_RCIN
#ifndef AP_PERIPH_RC1_PORT_DEFAULT
#define AP_PERIPH_RC1_PORT_DEFAULT -1
#endif
#ifndef AP_PERIPH_RC1_PORT_OPTIONS_DEFAULT
#define AP_PERIPH_RC1_PORT_OPTIONS_DEFAULT 0
#endif
#include <AP_RCProtocol/AP_RCProtocol.h> #include <AP_RCProtocol/AP_RCProtocol.h>
#include "AP_Periph.h" #include "AP_Periph.h"
#include <dronecan_msgs.h> #include <dronecan_msgs.h>
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
const AP_Param::GroupInfo Parameters_RCIN::var_info[] {
// 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
AP_GROUPINFO("_PROTOCOLS", 1, Parameters_RCIN, rcin_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
AP_GROUPINFO("_MSGRATE", 2, Parameters_RCIN, rcin_rate_hz, 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
AP_GROUPINFO("_PORT", 3, Parameters_RCIN, rcin1_port, AP_PERIPH_RC1_PORT_DEFAULT),
// @Param: RC1_PORT_OPTIONS
// @DisplayName: RC input port serial options
// @Description: This is the serial port number where SERIALx_PROTOCOL will be set to RC input.
// @Description: Control over UART options. The InvertRX option controls invert of the receive pin. The InvertTX option controls invert of the transmit pin. The HalfDuplex option controls half-duplex (onewire) mode, where both transmit and receive is done on the transmit wire. The Swap option allows the RX and TX pins to be swapped on STM32F7 based boards.
// @Bitmask: 0:InvertRX, 1:InvertTX, 2:HalfDuplex, 3:SwapTXRX, 4: RX_PullDown, 5: RX_PullUp, 6: TX_PullDown, 7: TX_PullUp, 8: RX_NoDMA, 9: TX_NoDMA, 10: Don't forward mavlink to/from, 11: DisableFIFO, 12: Ignore Streamrate
AP_GROUPINFO("1_PORT_OPTIONS", 4, Parameters_RCIN, rcin1_port_options, AP_PERIPH_RC1_PORT_OPTIONS_DEFAULT),
// @RebootRequired: True
AP_GROUPEND
};
Parameters_RCIN::Parameters_RCIN(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
void AP_Periph_FW::rcin_init() void AP_Periph_FW::rcin_init()
{ {
if (g.rcin1_port == 0) { if (g_rcin.rcin1_port == 0) {
return; return;
} }
// init uart for serial RC // init uart for serial RC
auto *uart = hal.serial(g.rcin1_port); auto *uart = hal.serial(g_rcin.rcin1_port);
if (uart == nullptr) { if (uart == nullptr) {
return; return;
} }
uart->set_options(g.rcin1_port_options); uart->set_options(g_rcin.rcin1_port_options);
serial_manager.set_protocol_and_baud( serial_manager.set_protocol_and_baud(
g.rcin1_port, g_rcin.rcin1_port,
AP_SerialManager::SerialProtocol_RCIN, AP_SerialManager::SerialProtocol_RCIN,
115200 // baud doesn't matter; RC Protocol autobauds 115200 // baud doesn't matter; RC Protocol autobauds
); );
auto &rc = AP::RC(); auto &rc = AP::RC();
rc.init(); rc.init();
rc.set_rc_protocols(g.rcin_protocols); rc.set_rc_protocols(g_rcin.rcin_protocols);
rc.add_uart(uart); rc.add_uart(uart);
rcin_initialised = true; rcin_initialised = true;
@ -70,7 +122,7 @@ void AP_Periph_FW::rcin_update()
} }
// decimate the input to a parameterized rate // decimate the input to a parameterized rate
const uint8_t rate_hz = g.rcin_rate_hz; const uint8_t rate_hz = g_rcin.rcin_rate_hz;
if (rate_hz == 0) { if (rate_hz == 0) {
return; return;
} }

18
Tools/AP_Periph/rc_in.h Normal file
View File

@ -0,0 +1,18 @@
#pragma once
#ifdef HAL_PERIPH_ENABLE_RCIN
class Parameters_RCIN {
public:
Parameters_RCIN(void);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
AP_Int32 rcin_protocols;
AP_Int8 rcin_rate_hz;
AP_Int8 rcin1_port;
AP_Int16 rcin1_port_options;
};
#endif