From a7921a273b98959f43227f19f4eb205d71186c2f Mon Sep 17 00:00:00 2001 From: Eugene Shamaev Date: Sat, 6 May 2017 12:12:54 +0300 Subject: [PATCH] AP_BoardConfig: move of CAN parameters into separate group and support of several interfaces --- libraries/AP_BoardConfig/AP_BoardConfig.cpp | 21 --- libraries/AP_BoardConfig/AP_BoardConfig.h | 49 +----- .../AP_BoardConfig/AP_BoardConfig_CAN.cpp | 162 ++++++++++++++++++ libraries/AP_BoardConfig/AP_BoardConfig_CAN.h | 100 +++++++++++ libraries/AP_BoardConfig/canbus.cpp | 39 +++-- libraries/AP_BoardConfig/px4_drivers.cpp | 52 ------ 6 files changed, 286 insertions(+), 137 deletions(-) create mode 100644 libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp create mode 100644 libraries/AP_BoardConfig/AP_BoardConfig_CAN.h diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index 91efad7164..1f32bbf64d 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -16,7 +16,6 @@ * AP_BoardConfig - board specific configuration */ - #include #include #include @@ -32,10 +31,6 @@ #include #endif -#if HAL_WITH_UAVCAN -#include -#endif - #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 # define BOARD_SAFETY_ENABLE_DEFAULT 1 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) @@ -131,12 +126,6 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { // @User: Standard AP_GROUPINFO("SERIAL_NUM", 5, AP_BoardConfig, vehicleSerialNumber, 0), -#if HAL_WITH_UAVCAN - // @Group: CAN_ - // @Path: ../AP_BoardConfig/canbus.cpp - AP_SUBGROUPINFO(_var_info_can, "CAN_", 6, AP_BoardConfig, AP_BoardConfig::CAN_var_info), -#endif - #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN // @Param: SAFETY_MASK // @DisplayName: Channels to which ignore the safety switch state @@ -183,22 +172,12 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { AP_GROUPEND }; -#if HAL_WITH_UAVCAN -int8_t AP_BoardConfig::_st_can_enable; -int8_t AP_BoardConfig::_st_can_debug; -#endif - void AP_BoardConfig::init() { #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN px4_setup(); #endif -#if HAL_WITH_UAVCAN - _st_can_enable = (int8_t) _var_info_can._can_enable; - _st_can_debug = (int8_t) _var_info_can._can_debug; -#endif - #if HAL_HAVE_IMU_HEATER // let the HAL know the target temperature. We pass a pointer as // we want the user to be able to change the parameter without diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index f2708f123b..b27798e144 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -20,30 +20,9 @@ public: static const struct AP_Param::GroupInfo var_info[]; -#if HAL_WITH_UAVCAN - class CAN_var_info { - friend class AP_BoardConfig; - - public: - CAN_var_info() : _uavcan(nullptr) - { - AP_Param::setup_object_defaults(this, var_info); - } - static const struct AP_Param::GroupInfo var_info[]; - - private: - AP_Int8 _can_enable; - AP_Int8 _can_debug; - AP_Int32 _can_bitrate; - - AP_Int8 _uavcan_enable; - - AP_UAVCAN *_uavcan; - }; -#endif - // notify user of a fatal startup error related to available sensors. static void sensor_config_error(const char *reason); + // permit other libraries (in particular, GCS_MAVLink) to detect // that we're never going to boot properly: static bool in_sensor_config_error(void) { return _in_sensor_config_error; } @@ -87,29 +66,9 @@ public: } #endif - static int8_t get_can_enable() - { -#if HAL_WITH_UAVCAN - return _st_can_enable; -#else - return 0; -#endif - } - static int8_t get_can_debug() - { -#if HAL_WITH_UAVCAN - return _st_can_debug; -#else - return 0; -#endif - } private: AP_Int16 vehicleSerialNumber; -#if HAL_WITH_UAVCAN - CAN_var_info _var_info_can; -#endif - #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN struct { AP_Int8 pwm_count; @@ -126,11 +85,6 @@ private: static enum px4_board_type px4_configured_board; -#if HAL_WITH_UAVCAN - static int8_t _st_can_enable; - static int8_t _st_can_debug; -#endif - void px4_drivers_start(void); void px4_setup(void); void px4_setup_pwm(void); @@ -138,7 +92,6 @@ private: void px4_setup_safety_mask(void); void px4_setup_uart(void); void px4_setup_sbus(void); - void px4_setup_canbus(void); void px4_setup_drivers(void); void px4_setup_peripherals(void); void px4_setup_px4io(void); diff --git a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp new file mode 100644 index 0000000000..a173258bbc --- /dev/null +++ b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp @@ -0,0 +1,162 @@ + /* + 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 . + */ +/* + * AP_BoardConfig_CAN - board specific configuration for CAN interface + */ + +#include +#include +#include "AP_BoardConfig_CAN.h" + +#if HAL_WITH_UAVCAN + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#include +#include +#include +#include + +#include +#endif + +#include + +extern const AP_HAL::HAL& hal; + +// table of user settable parameters +const AP_Param::GroupInfo AP_BoardConfig_CAN::var_info[] = { + +#if MAX_NUMBER_OF_CAN_INTERFACES > 0 + // @Group: P1_ + // @Path: ../AP_BoardConfig/canbus.cpp + AP_SUBGROUPINFO(_var_info_can[0], "P1_", 1, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_if_var_info), +#endif + +#if MAX_NUMBER_OF_CAN_INTERFACES > 1 + // @Group: P2_ + // @Path: ../AP_BoardConfig/canbus.cpp + AP_SUBGROUPINFO(_var_info_can[1], "P2_", 2, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_if_var_info), +#endif + +#if MAX_NUMBER_OF_CAN_INTERFACES > 2 + // @Group: P3_ + // @Path: ../AP_BoardConfig/canbus.cpp + AP_SUBGROUPINFO(_var_info_can[2], "P3_", 3, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_if_var_info), +#endif + +#if MAX_NUMBER_OF_CAN_DRIVERS > 0 + // @Group: D1_ + // @Path: ../AP_BoardConfig/canbus.cpp + AP_SUBGROUPINFO(_var_info_can_protocol[0], "D1_", 4, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_driver_var_info), +#endif + +#if MAX_NUMBER_OF_CAN_DRIVERS > 1 + // @Group: D2_ + // @Path: ../AP_BoardConfig/canbus.cpp + AP_SUBGROUPINFO(_var_info_can_protocol[1], "D2_", 5, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_driver_var_info), +#endif + +#if MAX_NUMBER_OF_CAN_DRIVERS > 2 + // @Group: D3_ + // @Path: ../AP_BoardConfig/canbus.cpp + AP_SUBGROUPINFO(_var_info_can_protocol[2], "D3_", 6, AP_BoardConfig_CAN, AP_BoardConfig_CAN::CAN_driver_var_info), +#endif + + AP_GROUPEND +}; + +int8_t AP_BoardConfig_CAN::_st_driver_number[MAX_NUMBER_OF_CAN_INTERFACES]; +int8_t AP_BoardConfig_CAN::_st_can_debug[MAX_NUMBER_OF_CAN_INTERFACES]; + +void AP_BoardConfig_CAN::init() +{ + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) + { + _st_driver_number[i] = (int8_t) _var_info_can[i]._driver_number; + _st_can_debug[i] = (int8_t) _var_info_can[i]._can_debug; + } + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + px4_setup_canbus(); +#endif + +} + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +/* + setup CANBUS drivers + */ +void AP_BoardConfig_CAN::px4_setup_canbus(void) +{ + // Create all drivers that we need + bool initret = true; + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) { + uint8_t drv_num = _var_info_can[i]._driver_number; + + if (drv_num != 0 && drv_num <= MAX_NUMBER_OF_CAN_DRIVERS) { + if (hal.can_mgr[drv_num - 1] == nullptr) { + const_cast (hal).can_mgr[drv_num - 1] = new PX4::PX4CANManager; + } + + if (hal.can_mgr[drv_num - 1] != nullptr) { + initret &= hal.can_mgr[drv_num - 1]->begin(_var_info_can[i]._can_bitrate, i); + } else { + printf("Failed to initialize can interface %d\n\r", i + 1); + } + } + } + + bool any_uavcan_present = false; + + if (initret) { + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) { + if (hal.can_mgr[i] != nullptr) { + hal.can_mgr[i]->initialized(true); + printf("can_mgr %d initialized well\n\r", i + 1); + + if (_var_info_can_protocol[i]._protocol == UAVCAN_PROTOCOL_ENABLE) { + _var_info_can_protocol[i]._uavcan = new AP_UAVCAN; + + if (_var_info_can_protocol[i]._uavcan != nullptr) + { + AP_Param::load_object_from_eeprom(_var_info_can_protocol[i]._uavcan, AP_UAVCAN::var_info); + + hal.can_mgr[i]->set_UAVCAN(_var_info_can_protocol[i]._uavcan); + _var_info_can_protocol[i]._uavcan->set_parent_can_mgr(hal.can_mgr[i]); + + if (_var_info_can_protocol[i]._uavcan->try_init() == true) { + any_uavcan_present = true; + } else { + printf("Failed to initialize uavcan interface %d\n\r", i + 1); + } + } else { + AP_HAL::panic("Failed to allocate uavcan %d\n\r", i + 1); + } + } + } + } + + if (any_uavcan_present) { + // start UAVCAN working thread + hal.scheduler->create_uavcan_thread(); + + // Delay for magnetometer and barometer discovery + hal.scheduler->delay(2000); + } + } +} +#endif +#endif + diff --git a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h new file mode 100644 index 0000000000..827ef5cab5 --- /dev/null +++ b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h @@ -0,0 +1,100 @@ +#pragma once + +#include +#include "AP_BoardConfig.h" +#include +#include +#include + +#if HAL_WITH_UAVCAN +#define UAVCAN_PROTOCOL_ENABLE 1 + +class AP_BoardConfig_CAN { +public: + // constructor + AP_BoardConfig_CAN(void) + { + AP_Param::setup_object_defaults(this, var_info); + }; + + void init(void); + + static const struct AP_Param::GroupInfo var_info[]; + + class CAN_if_var_info { + friend class AP_BoardConfig_CAN; + + public: + CAN_if_var_info() + { + AP_Param::setup_object_defaults(this, var_info); + } + + static const struct AP_Param::GroupInfo var_info[]; + + private: + AP_Int8 _driver_number; + AP_Int8 _can_debug; + AP_Int32 _can_bitrate; + }; + + class CAN_driver_var_info { + friend class AP_BoardConfig_CAN; + + public: + CAN_driver_var_info() : + _uavcan(nullptr) + { + AP_Param::setup_object_defaults(this, var_info); + } + static const struct AP_Param::GroupInfo var_info[]; + + private: + AP_Int8 _protocol; + + AP_UAVCAN* _uavcan; + }; + + // returns number of enabled CAN interfaces + static int8_t get_can_num_ifaces(void) + { + uint8_t ret = 0; + + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) { + if (_st_driver_number[i]) { + ret++; + } + } + + return ret; + } + + static int8_t get_can_debug(uint8_t i) + { + if (i < MAX_NUMBER_OF_CAN_INTERFACES) { + return _st_can_debug[i]; + } + return 0; + } + + // return maximum level of debug + static int8_t get_can_debug(void) + { + uint8_t ret = 0; + for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) { + uint8_t dbg = get_can_debug(i); + ret = (dbg > ret) ? dbg : ret; + } + return ret; + } + + CAN_if_var_info _var_info_can[MAX_NUMBER_OF_CAN_INTERFACES]; + CAN_driver_var_info _var_info_can_protocol[MAX_NUMBER_OF_CAN_DRIVERS]; + + static int8_t _st_driver_number[MAX_NUMBER_OF_CAN_INTERFACES]; + static int8_t _st_can_debug[MAX_NUMBER_OF_CAN_INTERFACES]; +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + void px4_setup_canbus(void); +#endif // HAL_BOARD_PX4 || HAL_BOARD_VRBRAIN +}; +#endif diff --git a/libraries/AP_BoardConfig/canbus.cpp b/libraries/AP_BoardConfig/canbus.cpp index 2ff24f1fcb..8607c169e3 100644 --- a/libraries/AP_BoardConfig/canbus.cpp +++ b/libraries/AP_BoardConfig/canbus.cpp @@ -18,41 +18,48 @@ #include "AP_BoardConfig.h" #if HAL_WITH_UAVCAN +#include "AP_BoardConfig_CAN.h" #include // table of user settable CAN bus parameters -const AP_Param::GroupInfo AP_BoardConfig::CAN_var_info::var_info[] = { - // @Param: ENABLE - // @DisplayName: Enable use of CAN buses +const AP_Param::GroupInfo AP_BoardConfig_CAN::CAN_if_var_info::var_info[] = { + // @Param: DRIVER + // @DisplayName: Index of virtual driver to be used with physical CAN interface // @Description: Enabling this option enables use of CAN buses. - // @Values: 0:Disabled,1:Enabled first channel,2:Enabled both channels - // @User: Advanced - AP_GROUPINFO_FLAGS("ENABLE", 1, AP_BoardConfig::CAN_var_info, _can_enable, 0, AP_PARAM_FLAG_ENABLE), + // @Values: 0:Disabled,1:First driver,2:Second driver + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO_FLAGS("DRIVER", 1, AP_BoardConfig_CAN::CAN_if_var_info, _driver_number, 0, AP_PARAM_FLAG_ENABLE), // @Param: BITRATE - // @DisplayName: Bitrate of CAN interface + // @DisplayName: Bitrate of CAN interface // @Description: Bit rate can be set up to from 10000 to 1000000 // @Range: 10000 1000000 // @User: Advanced - AP_GROUPINFO("BITRATE", 2, AP_BoardConfig::CAN_var_info, _can_bitrate, 1000000), + AP_GROUPINFO("BITRATE", 2, AP_BoardConfig_CAN::CAN_if_var_info, _can_bitrate, 1000000), // @Param: DEBUG - // @DisplayName: Level of debug for CAN devices + // @DisplayName: Level of debug for CAN devices // @Description: Enabling this option will provide debug messages // @Values: 0:Disabled,1:Major messages,2:All messages // @User: Advanced - AP_GROUPINFO("DEBUG", 3, AP_BoardConfig::CAN_var_info, _can_debug, 2), + AP_GROUPINFO("DEBUG", 3, AP_BoardConfig_CAN::CAN_if_var_info, _can_debug, 2), - // @Param: UC_EN - // @DisplayName: Enable use of UAVCAN devices - // @Description: Enabling this option starts UAVCAN protocol. - // @Values: 0:Disabled,1:Enabled + AP_GROUPEND +}; + +const AP_Param::GroupInfo AP_BoardConfig_CAN::CAN_driver_var_info::var_info[] = { + // @Param: PROTOCOL + // @DisplayName: Enable use of specific protocol over virtual driver + // @Description: Enabling this option starts selected protocol that will use this virtual driver + // @Values: 0:Disabled,1:UAVCAN // @User: Advanced - AP_GROUPINFO("UC_EN", 4, AP_BoardConfig::CAN_var_info, _uavcan_enable, 1), + // @RebootRequired: True + AP_GROUPINFO("PROTOCOL", 1, AP_BoardConfig_CAN::CAN_driver_var_info, _protocol, UAVCAN_PROTOCOL_ENABLE), // @Group: UC_ // @Path: ../AP_UAVCAN/AP_UAVCAN.cpp - AP_SUBGROUPPTR(_uavcan, "UC_", 5, AP_BoardConfig::CAN_var_info, AP_UAVCAN), + AP_SUBGROUPPTR(_uavcan, "UC_", 2, AP_BoardConfig_CAN::CAN_driver_var_info, AP_UAVCAN), AP_GROUPEND }; diff --git a/libraries/AP_BoardConfig/px4_drivers.cpp b/libraries/AP_BoardConfig/px4_drivers.cpp index f52654bb7e..19e03071bb 100644 --- a/libraries/AP_BoardConfig/px4_drivers.cpp +++ b/libraries/AP_BoardConfig/px4_drivers.cpp @@ -29,11 +29,6 @@ #include #include -#if HAL_WITH_UAVCAN -#include -#include -#endif - extern const AP_HAL::HAL& hal; AP_BoardConfig::px4_board_type AP_BoardConfig::px4_configured_board; @@ -42,9 +37,6 @@ AP_BoardConfig::px4_board_type AP_BoardConfig::px4_configured_board; declare driver main entry points */ extern "C" { -#if HAL_WITH_UAVCAN - int uavcan_main(int, char **); -#endif int fmu_main(int, char **); int px4io_main(int, char **); int adc_main(int, char **); @@ -191,49 +183,6 @@ void AP_BoardConfig::px4_setup_sbus(void) #endif } -/* - setup CANBUS drivers - */ -void AP_BoardConfig::px4_setup_canbus(void) -{ -#if HAL_WITH_UAVCAN - if (_var_info_can._can_enable >= 1) { - if(hal.can_mgr == nullptr) - { - const_cast (hal).can_mgr = new PX4::PX4CANManager; - } - - if(hal.can_mgr != nullptr) - { - if(_var_info_can._uavcan_enable > 0) - { - _var_info_can._uavcan = new AP_UAVCAN; - if(_var_info_can._uavcan != nullptr) - { - AP_Param::load_object_from_eeprom(_var_info_can._uavcan, AP_UAVCAN::var_info); - - hal.can_mgr->set_UAVCAN(_var_info_can._uavcan); - - bool initret = hal.can_mgr->begin(_var_info_can._can_bitrate, _var_info_can._can_enable); - if (!initret) { - hal.console->printf("Failed to initialize can_mgr\n\r"); - } else { - hal.console->printf("can_mgr initialized well\n\r"); - - // start UAVCAN working thread - hal.scheduler->create_uavcan_thread(); - } - } else - { - _var_info_can._uavcan_enable.set(0); - hal.console->printf("AP_UAVCAN failed to allocate\n\r"); - } - } - } - } -#endif -} - extern "C" int waitpid(pid_t, int *, int); /* @@ -552,7 +501,6 @@ void AP_BoardConfig::px4_setup() px4_setup_uart(); px4_setup_sbus(); px4_setup_drivers(); - px4_setup_canbus(); } #endif // HAL_BOARD_PX4