Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: add support for multiple protocols on AP_Periph

This commit is contained in:
Tom Pittenger 2021-04-30 18:36:42 -07:00 committed by Andrew Tridgell
parent 5e4efe9f57
commit 4d0f5a1db6
6 changed files with 160 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
Tools/AP_Periph/AP_Periph.cpp
View File

@ -68,7 +68,12 @@ AP_Periph_FW::AP_Periph_FW()
#if HAL_LOGGING_ENABLED
: logger(g.log_bitmask)
#endif
{}
{
if (_singleton != nullptr) {
AP_HAL::panic("AP_Periph_FW must be singleton");
}
_singleton = this;
}
#if HAL_LOGGING_ENABLED
const struct LogStructure AP_Periph_FW::log_structure[] = {
@ -450,4 +455,11 @@ void AP_Periph_FW::prepare_reboot()
hal.scheduler->delay(40);
}
AP_Periph_FW *AP_Periph_FW::_singleton;
AP_Periph_FW& AP::periph()
{
return *AP_Periph_FW::get_singleton();
}
AP_HAL_MAIN();

36
Tools/AP_Periph/AP_Periph.h
View File

@ -15,6 +15,13 @@
#include <AP_MSP/msp.h>
#include "../AP_Bootloader/app_comms.h"
#include "hwing_esc.h"
#include <AP_CANManager/AP_CANManager.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <AP_HAL_ChibiOS/CANIface.h>
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_HAL_SITL/CANSocketIface.h>
#endif
#if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY)
#define AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY
@ -55,6 +62,16 @@ class AP_Periph_FW {
public:
AP_Periph_FW();
CLASS_NO_COPY(AP_Periph_FW);
static AP_Periph_FW* get_singleton()
{
if (_singleton == nullptr) {
AP_HAL::panic("AP_Periph_FW used before allocation.");
}
return _singleton;
}
void init();
void update();
@ -76,6 +93,12 @@ public:
void check_for_serial_reboot_cmd(const int8_t serial_index);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
static ChibiOS::CANIface* can_iface_periph[HAL_NUM_CAN_IFACES];
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
static HALSITL::CANIface* can_iface_periph[HAL_NUM_CAN_IFACES];
#endif
AP_SerialManager serial_manager;
#ifdef HAL_PERIPH_ENABLE_GPS
@ -100,6 +123,12 @@ public:
} battery;
#endif
#if HAL_NUM_CAN_IFACES >= 2
// This allows you to change the protocol and it continues to use the one at boot.
// Without this, changing away from UAVCAN causes loss of comms and you can't
// change the rest of your params or veryofy it suceeded.
AP_CANManager::Driver_Type can_protocol_cached[HAL_NUM_CAN_IFACES];
#endif
#ifdef HAL_PERIPH_ENABLE_MSP
struct {
@ -191,10 +220,17 @@ public:
uint32_t last_baro_update_ms;
uint32_t last_airspeed_update_ms;
static AP_Periph_FW *_singleton;
// show stack as DEBUG msgs
void show_stack_free();
};
namespace AP
{
AP_Periph_FW& periph();
}
extern AP_Periph_FW periph;
extern "C" {

47
Tools/AP_Periph/Parameters.cpp
View File

@ -35,6 +35,7 @@ extern const AP_HAL::HAL &hal;
*/
#define GSCALAR(v, name, def) { periph.g.v.vtype, name, Parameters::k_param_ ## v, &periph.g.v, {def_value : def} }
#define GARRAY(v, index, name, def) { periph.g.v[index].vtype, name, Parameters::k_param_ ## v ## index, &periph.g.v[index], {def_value : def} }
#define ASCALAR(v, name, def) { periph.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&periph.aparm.v, {def_value : def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &periph.g.v, {group_info : class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&periph.v, {group_info : class::var_info} }
@ -61,7 +62,51 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
// @Range: 10000 1000000
// @User: Advanced
// @RebootRequired: True
GSCALAR(can_baudrate, "CAN_BAUDRATE", 1000000),
GARRAY(can_baudrate, 0, "CAN_BAUDRATE", 1000000),
#if HAL_NUM_CAN_IFACES >= 2
// @Param: CAN_PROTOCOL
// @DisplayName: Enable use of specific protocol to be used on this port
// @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN
// @Values: 0:Disabled,1:UAVCAN,3:ToshibaCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN,9:PacketDigital
// @User: Advanced
// @RebootRequired: True
GARRAY(can_protocol, 0, "CAN_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN),
// @Param: CAN2_BAUDRATE
// @DisplayName: Bitrate of CAN2 interface
// @Description: Bit rate can be set up to from 10000 to 1000000
// @Range: 10000 1000000
// @User: Advanced
// @RebootRequired: True
GARRAY(can_baudrate, 1, "CAN2_BAUDRATE", 1000000),
// @Param: CAN2_PROTOCOL
// @DisplayName: Enable use of specific protocol to be used on this port
// @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN
// @Values: 0:Disabled,1:UAVCAN,3:ToshibaCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN,9:PacketDigital
// @User: Advanced
// @RebootRequired: True
GARRAY(can_protocol, 1, "CAN2_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN),
#endif
#if HAL_NUM_CAN_IFACES >= 3
// @Param: CAN3_BAUDRATE
// @DisplayName: Bitrate of CAN3 interface
// @Description: Bit rate can be set up to from 10000 to 1000000
// @Range: 10000 1000000
// @User: Advanced
// @RebootRequired: True
GARRAY(can_baudrate, 2, "CAN3_BAUDRATE", 1000000),
// @Param: CAN3_PROTOCOL
// @DisplayName: Enable use of specific protocol to be used on this port
// @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN
// @Values: 0:Disabled,1:UAVCAN,3:ToshibaCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN,9:PacketDigital
// @User: Advanced
// @RebootRequired: True
GARRAY(can_protocol, 2, "CAN3_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN),
#endif
#if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT)
// @Param: FLASH_BOOTLOADER

14
Tools/AP_Periph/Parameters.h
View File

@ -15,7 +15,7 @@ public:
k_param_gps,
k_param_compass,
k_param_can_node,
k_param_can_baudrate,
k_param_can_baudrate0,
k_param_baro,
k_param_buzz_volume,
k_param_led_brightness,
@ -40,11 +40,21 @@ public:
k_param_esc_pwm_type,
k_param_logger,
k_param_log_bitmask,
k_param_can_baudrate1,
k_param_can_baudrate2,
k_param_can_protocol0,
k_param_can_protocol1,
k_param_can_protocol2,
};
AP_Int16 format_version;
AP_Int16 can_node;
AP_Int32 can_baudrate;
AP_Int32 can_baudrate[HAL_NUM_CAN_IFACES];
#if HAL_NUM_CAN_IFACES >= 2
AP_Enum<AP_CANManager::Driver_Type> can_protocol[HAL_NUM_CAN_IFACES];
#endif
#ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY
AP_Int8 buzz_volume;
#endif

61
Tools/AP_Periph/can.cpp
View File

@ -68,6 +68,10 @@
#include "i2c.h"
#include <utility>
#if HAL_NUM_CAN_IFACES >= 2
#include <AP_CANManager/AP_CANSensor.h>
#endif
extern const AP_HAL::HAL &hal;
extern AP_Periph_FW periph;
@ -95,11 +99,16 @@ static uint8_t transfer_id;
#define CAN_PROBE_CONTINUOUS 0
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
static ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
static HALSITL::CANIface can_iface[HAL_NUM_CAN_IFACES];
#ifndef AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz
#define AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz 1
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
ChibiOS::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
HALSITL::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
#endif
/*
* Variables used for dynamic node ID allocation.
* RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
@ -980,7 +989,12 @@ static void processTx(void)
bool sent_ok = false;
const uint64_t deadline = AP_HAL::native_micros64() + 1000000;
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
sent_ok |= (can_iface[i].send(txmsg, deadline, 0) > 0);
#if HAL_NUM_CAN_IFACES >= 2
if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) {
continue;
}
#endif
sent_ok |= (AP::periph().can_iface_periph[i]->send(txmsg, deadline, 0) > 0);
}
if (sent_ok) {
canardPopTxQueue(&canard);
@ -1004,9 +1018,14 @@ static void processRx(void)
while (true) {
bool got_pkt = false;
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
#if HAL_NUM_CAN_IFACES >= 2
if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) {
continue;
}
#endif
bool read_select = true;
bool write_select = false;
can_iface[i].select(read_select, write_select, nullptr, 0);
AP::periph().can_iface_periph[i]->select(read_select, write_select, nullptr, 0);
if (!read_select) {
continue;
}
@ -1015,7 +1034,7 @@ static void processRx(void)
//palToggleLine(HAL_GPIO_PIN_LED);
uint64_t timestamp;
AP_HAL::CANIface::CanIOFlags flags;
can_iface[i].receive(rxmsg, timestamp, flags);
AP::periph().can_iface_periph[i]->receive(rxmsg, timestamp, flags);
memcpy(rx_frame.data, rxmsg.data, 8);
rx_frame.data_len = rxmsg.dlc;
rx_frame.id = rxmsg.id;
@ -1253,8 +1272,32 @@ void AP_Periph_FW::can_start()
periph.g.flash_bootloader.set_and_save_ifchanged(0);
#endif
for (int8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
can_iface[i].init(1000000, AP_HAL::CANIface::NormalMode);
#if AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz && HAL_NUM_CAN_IFACES >= 2
bool has_uavcan_at_1MHz = false;
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
if (g.can_protocol[i] == AP_CANManager::Driver_Type_UAVCAN && g.can_baudrate[i] == 1000000) {
has_uavcan_at_1MHz = true;
}
}
if (!has_uavcan_at_1MHz) {
g.can_protocol[0].set_and_save(AP_CANManager::Driver_Type_UAVCAN);
g.can_baudrate[0].set_and_save(1000000);
}
#endif // HAL_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz
for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
can_iface_periph[i] = new ChibiOS::CANIface();
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
can_iface_periph[i] = new HALSITL::CANIface();
#endif
#if HAL_NUM_CAN_IFACES >= 2
can_protocol_cached[i] = g.can_protocol[i];
CANSensor::set_periph(i, can_protocol_cached[i], can_iface_periph[i]);
#endif
if (can_iface_periph[i] != nullptr) {
can_iface_periph[i]->init(g.can_baudrate[i], AP_HAL::CANIface::NormalMode);
}
}
canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool),

1
Tools/AP_Periph/wscript
View File

@ -22,6 +22,7 @@ def build(bld):
'AP_Math',
'AP_BoardConfig',
'AP_BattMonitor',
'AP_CANManager',
'AP_Param',
'StorageManager',
'AP_FlashStorage',