From 4d0f5a1db6e7a4f43669cc5a103c7903dfa59d9b Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 30 Apr 2021 18:36:42 -0700 Subject: [PATCH] AP_Periph: add support for multiple protocols on AP_Periph --- Tools/AP_Periph/AP_Periph.cpp | 14 +++++++- Tools/AP_Periph/AP_Periph.h | 36 ++++++++++++++++++++ Tools/AP_Periph/Parameters.cpp | 47 +++++++++++++++++++++++++- Tools/AP_Periph/Parameters.h | 14 ++++++-- Tools/AP_Periph/can.cpp | 61 +++++++++++++++++++++++++++++----- Tools/AP_Periph/wscript | 1 + 6 files changed, 160 insertions(+), 13 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 34fbe86127..3ef946e5b6 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -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(); diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 10203ae816..d8f8dd14e2 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -15,6 +15,13 @@ #include #include "../AP_Bootloader/app_comms.h" #include "hwing_esc.h" +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include +#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#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" { diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index b596dab43b..c8bdc00f2b 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -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 diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index a4e03a3ab1..0f54b76b1c 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -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 can_protocol[HAL_NUM_CAN_IFACES]; +#endif + #ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY AP_Int8 buzz_volume; #endif diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index e3c12d1eb4..b9b2d8c9aa 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -68,6 +68,10 @@ #include "i2c.h" #include +#if HAL_NUM_CAN_IFACES >= 2 +#include +#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 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= 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= 2 + bool has_uavcan_at_1MHz = false; + for (uint8_t i=0; i= 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), diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index 750b40ff0f..30bdbdbaac 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -22,6 +22,7 @@ def build(bld): 'AP_Math', 'AP_BoardConfig', 'AP_BattMonitor', + 'AP_CANManager', 'AP_Param', 'StorageManager', 'AP_FlashStorage',