mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 00:28:30 -04:00
AP_Periph: added support for SERIAL_OPTIONS
allows for options and flow control per serial port
This commit is contained in:
parent
6c2a7b8cfd
commit
83cfcd841b
@ -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);
|
||||
|
@ -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))
|
||||
@ -332,6 +333,10 @@ public:
|
||||
BattBalance battery_balance;
|
||||
#endif
|
||||
|
||||
#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS
|
||||
SerialOptions serial_options;
|
||||
#endif
|
||||
|
||||
#if AP_TEMPERATURE_SENSOR_ENABLED
|
||||
AP_TemperatureSensor temperature_sensor;
|
||||
#endif
|
||||
|
@ -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_
|
||||
|
@ -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;
|
||||
|
110
Tools/AP_Periph/serial_options.cpp
Normal file
110
Tools/AP_Periph/serial_options.cpp
Normal 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
|
30
Tools/AP_Periph/serial_options.h
Normal file
30
Tools/AP_Periph/serial_options.h
Normal 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
|
47
Tools/AP_Periph/serial_options_dev.cpp
Normal file
47
Tools/AP_Periph/serial_options_dev.cpp
Normal 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
|
Loading…
Reference in New Issue
Block a user