AP_Periph: added support for SERIAL_OPTIONS

allows for options and flow control per serial port
This commit is contained in:
Andrew Tridgell 2023-12-20 07:40:30 +11:00 committed by Tom Pittenger
parent 6c2a7b8cfd
commit 83cfcd841b
7 changed files with 203 additions and 0 deletions

View File

@ -140,6 +140,10 @@ void AP_Periph_FW::init()
node_stats.init();
#endif
#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS
serial_options.init();
#endif
#ifdef HAL_PERIPH_ENABLE_GPS
if (gps.get_type(0) != AP_GPS::GPS_Type::GPS_TYPE_NONE && g.gps_port >= 0) {
serial_manager.set_protocol_and_baud(g.gps_port, AP_SerialManager::SerialProtocol_GPS, AP_SERIALMANAGER_GPS_BAUD);

View File

@ -36,6 +36,7 @@
#include "rc_in.h"
#include "batt_balance.h"
#include "networking.h"
#include "serial_options.h"
#include <AP_NMEA_Output/AP_NMEA_Output.h>
#if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS))
@ -331,6 +332,10 @@ public:
void batt_balance_update();
BattBalance battery_balance;
#endif
#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS
SerialOptions serial_options;
#endif
#if AP_TEMPERATURE_SENSOR_ENABLED
AP_TemperatureSensor temperature_sensor;

View File

@ -610,6 +610,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
GOBJECT(battery_balance, "BAL", BattBalance),
#endif
#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS
// @Group: UART
// @Path: serial_options.cpp
GOBJECT(serial_options, "UART", SerialOptions),
#endif
// NOTE: sim parameters should go last
#if AP_SIM_ENABLED
// @Group: SIM_

View File

@ -88,6 +88,7 @@ public:
k_param_can_terminate0,
k_param_can_terminate1,
k_param_can_terminate2,
k_param_serial_options,
};
AP_Int16 format_version;

View File

@ -0,0 +1,110 @@
/*
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/>.
*/
/*
serial options support, for serial over DroneCAN
*/
#include "AP_Periph.h"
#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS
#include "serial_options.h"
#include <AP_SerialManager/AP_SerialManager_config.h>
extern const AP_HAL::HAL &hal;
const AP_Param::GroupInfo SerialOptions::var_info[] {
#if HAL_HAVE_SERIAL0
// @Group: 0_
// @Path: serial_options_dev.cpp
AP_SUBGROUPINFO(devs[0], "0_", 1, SerialOptions, SerialOptionsDev),
#endif
#if HAL_HAVE_SERIAL1
// @Group: 1_
// @Path: serial_options_dev.cpp
AP_SUBGROUPINFO(devs[1], "1_", 2, SerialOptions, SerialOptionsDev),
#endif
#if HAL_HAVE_SERIAL2
// @Group: 2_
// @Path: serial_options_dev.cpp
AP_SUBGROUPINFO(devs[2], "2_", 3, SerialOptions, SerialOptionsDev),
#endif
#if HAL_HAVE_SERIAL3
// @Group: 3_
// @Path: serial_options_dev.cpp
AP_SUBGROUPINFO(devs[3], "3_", 4, SerialOptions, SerialOptionsDev),
#endif
#if HAL_HAVE_SERIAL4
// @Group: 4_
// @Path: serial_options_dev.cpp
AP_SUBGROUPINFO(devs[4], "4_", 5, SerialOptions, SerialOptionsDev),
#endif
#if HAL_HAVE_SERIAL5
// @Group: 5_
// @Path: serial_options_dev.cpp
AP_SUBGROUPINFO(devs[5], "5_", 6, SerialOptions, SerialOptionsDev),
#endif
#if HAL_HAVE_SERIAL6
// @Group: 6_
// @Path: serial_options_dev.cpp
AP_SUBGROUPINFO(devs[6], "6_", 7, SerialOptions, SerialOptionsDev),
#endif
#if HAL_HAVE_SERIAL7
// @Group: 7_
// @Path: serial_options_dev.cpp
AP_SUBGROUPINFO(devs[7], "7_", 8, SerialOptions, SerialOptionsDev),
#endif
#if HAL_HAVE_SERIAL8
// @Group: 8_
// @Path: serial_options_dev.cpp
AP_SUBGROUPINFO(devs[8], "8_", 9, SerialOptions, SerialOptionsDev),
#endif
#if HAL_HAVE_SERIAL9
// @Group: 9_
// @Path: serial_options_dev.cpp
AP_SUBGROUPINFO(devs[9], "9_", 10, SerialOptions, SerialOptionsDev),
#endif
AP_GROUPEND
};
SerialOptions::SerialOptions(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
void SerialOptions::init(void)
{
for (uint8_t i=0; i<ARRAY_SIZE(devs); i++) {
auto *uart = hal.serial(i);
if (uart != nullptr) {
auto &d = devs[i];
uart->set_options(d.options);
uart->set_flow_control(AP_HAL::UARTDriver::flow_control(d.rtscts.get()));
}
}
}
#endif // HAL_PERIPH_ENABLE_SERIAL_OPTIONS

View File

@ -0,0 +1,30 @@
#pragma once
#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS
#ifndef HAL_UART_NUM_SERIAL_PORTS
#define HAL_UART_NUM_SERIAL_PORTS AP_HAL::HAL::num_serial
#endif
class SerialOptionsDev {
public:
SerialOptionsDev(void);
static const struct AP_Param::GroupInfo var_info[];
AP_Int32 options;
AP_Int8 rtscts;
};
class SerialOptions {
public:
friend class AP_Periph_FW;
SerialOptions(void);
void init(void);
static const struct AP_Param::GroupInfo var_info[];
private:
SerialOptionsDev devs[HAL_UART_NUM_SERIAL_PORTS];
};
#endif // HAL_PERIPH_ENABLE_SERIAL_OPTIONS

View File

@ -0,0 +1,47 @@
/*
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/>.
*/
/*
serial options support, for serial over DroneCAN
*/
#include "AP_Periph.h"
#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS
#include "serial_options.h"
SerialOptionsDev::SerialOptionsDev(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
const AP_Param::GroupInfo SerialOptionsDev::var_info[] {
// @Param: OPTIONS
// @DisplayName: Serial options
// @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("OPTIONS", 1, SerialOptionsDev, options, 0),
// @Param: RTSCTS
// @DisplayName: Serial1 flow control
// @Description: Enable flow control. You must have the RTS and CTS pins available on the port. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.
// @Values: 0:Disabled,1:Enabled,2:Auto
AP_GROUPINFO("RTSCTS", 2, SerialOptionsDev, rtscts, float(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE)),
AP_GROUPEND
};
#endif // HAL_PERIPH_ENABLE_SERIAL_OPTIONS