From 0b54d5764cd6122714ff8fe9eb36dbef07f2abfa Mon Sep 17 00:00:00 2001 From: Eugene Shamaev Date: Sun, 2 Apr 2017 17:56:37 +0300 Subject: [PATCH] AP_BoardConfig: support for CAN and UAVCAN at board level --- libraries/AP_BoardConfig/AP_BoardConfig.cpp | 29 ++++--- libraries/AP_BoardConfig/AP_BoardConfig.h | 65 ++++++++++++--- libraries/AP_BoardConfig/canbus.cpp | 59 ++++++++++++++ libraries/AP_BoardConfig/px4_drivers.cpp | 88 +++++++++++---------- 4 files changed, 177 insertions(+), 64 deletions(-) create mode 100644 libraries/AP_BoardConfig/canbus.cpp diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index d930f94d52..74686d0461 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -30,6 +30,10 @@ #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) @@ -97,7 +101,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { // @User: Advanced AP_GROUPINFO("SER2_RTSCTS", 2, AP_BoardConfig, px4.ser2_rtscts, 2), #endif - + #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN // @Param: SAFETYENABLE // @DisplayName: Enable use of safety arming switch @@ -126,12 +130,9 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { AP_GROUPINFO("SERIAL_NUM", 5, AP_BoardConfig, vehicleSerialNumber, 0), #if HAL_WITH_UAVCAN - // @Param: CAN_ENABLE - // @DisplayName: Enable use of UAVCAN devices - // @Description: Enabling this option on a Pixhawk enables UAVCAN devices. Note that this uses about 25k of memory - // @Values: 0:Disabled,1:Enabled,2:Dynamic ID/Update - // @User: Advanced - AP_GROUPINFO("CAN_ENABLE", 6, AP_BoardConfig, px4.can_enable, 0), + // @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 @@ -176,16 +177,26 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { AP_GROUPINFO("IO_ENABLE", 10, AP_BoardConfig, px4.io_enable, 1), #endif #endif - + 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 eeac9c55ea..5719e90d74 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -13,25 +13,44 @@ #define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1) // query if node identification is in progress #define UAVCAN_IOCS_HARDPOINT_SET _UAVCAN_IOC(10) // control hardpoint (e.g. OpenGrab EPM) -#define UAVCAN_NODE_FILE "/dev/uavcan/esc" // design flaw of uavcan driver, this should be /dev/uavcan/node one day - #endif extern "C" typedef int (*main_fn_t)(int argc, char **); -class AP_BoardConfig -{ +class AP_BoardConfig { public: // constructor AP_BoardConfig(void) { - AP_Param::setup_object_defaults(this, var_info); + AP_Param::setup_object_defaults(this, var_info); }; void init(void); 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 + #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN // public method to start a driver static bool px4_start_driver(main_fn_t main_function, const char *name, const char *arguments); @@ -70,18 +89,35 @@ public: return px4_configured_board; } #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; AP_Int8 safety_enable; AP_Int32 ignore_safety_channels; -#if HAL_WITH_UAVCAN - AP_Int8 can_enable; -#endif #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 AP_Int8 ser1_rtscts; AP_Int8 ser2_rtscts; @@ -92,7 +128,12 @@ private: } px4; 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); @@ -106,11 +147,11 @@ private: void px4_setup_px4io(void); void px4_tone_alarm(const char *tone_string); bool spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag = 0x80); - + #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 void px4_autodetect(void); #endif - + #endif // HAL_BOARD_PX4 || HAL_BOARD_VRBRAIN // target temperarure for IMU in Celsius, or -1 to disable diff --git a/libraries/AP_BoardConfig/canbus.cpp b/libraries/AP_BoardConfig/canbus.cpp new file mode 100644 index 0000000000..2ff24f1fcb --- /dev/null +++ b/libraries/AP_BoardConfig/canbus.cpp @@ -0,0 +1,59 @@ +/* + * 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 . +*/ + +#include +#include +#include "AP_BoardConfig.h" + +#if HAL_WITH_UAVCAN +#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 + // @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), + + // @Param: BITRATE + // @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), + + // @Param: DEBUG + // @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), + + // @Param: UC_EN + // @DisplayName: Enable use of UAVCAN devices + // @Description: Enabling this option starts UAVCAN protocol. + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("UC_EN", 4, AP_BoardConfig::CAN_var_info, _uavcan_enable, 1), + + // @Group: UC_ + // @Path: ../AP_UAVCAN/AP_UAVCAN.cpp + AP_SUBGROUPPTR(_uavcan, "UC_", 5, AP_BoardConfig::CAN_var_info, AP_UAVCAN), + + AP_GROUPEND +}; +#endif diff --git a/libraries/AP_BoardConfig/px4_drivers.cpp b/libraries/AP_BoardConfig/px4_drivers.cpp index d995435b80..283add393c 100644 --- a/libraries/AP_BoardConfig/px4_drivers.cpp +++ b/libraries/AP_BoardConfig/px4_drivers.cpp @@ -16,7 +16,6 @@ * AP_BoardConfig - px4 driver loading and setup */ - #include #include "AP_BoardConfig.h" @@ -31,6 +30,11 @@ #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; @@ -48,7 +52,6 @@ extern "C" { int tone_alarm_main(int, char **); }; - #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* this is needed for the code to wait for CAN startup @@ -59,7 +62,6 @@ extern "C" { #define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1) // query if node identification is in progress #endif - /* setup PWM pins */ @@ -89,7 +91,7 @@ void AP_BoardConfig::px4_setup_pwm() } } if (i == ARRAY_SIZE(mode_table)) { - hal.console->printf("RCOutput: invalid BRD_PWM_COUNT %u\n", mode_parm); + hal.console->printf("RCOutput: invalid BRD_PWM_COUNT %u\n", mode_parm); } else { int fd = open("/dev/px4fmu", 0); if (fd == -1) { @@ -97,7 +99,7 @@ void AP_BoardConfig::px4_setup_pwm() } if (ioctl(fd, PWM_SERVO_SET_MODE, mode_table[i].mode_value) != 0) { hal.console->printf("RCOutput: unable to setup AUX PWM with BRD_PWM_COUNT %u\n", mode_parm); - } + } close(fd); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 if (mode_table[i].num_gpios < 2) { @@ -109,7 +111,6 @@ void AP_BoardConfig::px4_setup_pwm() } } - /* setup flow control on UARTs */ @@ -171,7 +172,6 @@ void AP_BoardConfig::px4_setup_safety() } } - /* setup SBUS */ @@ -204,46 +204,47 @@ void AP_BoardConfig::px4_setup_sbus(void) #endif } - /* setup CANBUS drivers */ void AP_BoardConfig::px4_setup_canbus(void) { #if HAL_WITH_UAVCAN - if (px4.can_enable >= 1) { - // give time for other drivers to fully start before we start - // canbus. This prevents a race where a canbus mag comes up - // before the hmc5883 - hal.scheduler->delay(500); - if (px4_start_driver(uavcan_main, "uavcan", "start")) { - hal.console->printf("UAVCAN: started\n"); - // give some time for CAN bus initialisation - hal.scheduler->delay(2000); - } else { - hal.console->printf("UAVCAN: failed to start\n"); + if (_var_info_can._can_enable >= 1) { + if(hal.can_mgr == nullptr) + { + const_cast (hal).can_mgr = new PX4::PX4CANManager; } - // give time for canbus drivers to register themselves - hal.scheduler->delay(2000); - } - if (px4.can_enable >= 2) { - if (px4_start_driver(uavcan_main, "uavcan", "start fw")) { - uint32_t start_wait_ms = AP_HAL::millis(); - int fd = open("/dev/uavcan/esc", 0); // design flaw of uavcan driver, this should be /dev/uavcan/node one day - if (fd == -1) { - AP_HAL::panic("Configuration invalid - unable to open /dev/uavcan/esc"); - } - // delay startup, UAVCAN still discovering nodes - while (ioctl(fd, UAVCAN_IOCG_NODEID_INPROGRESS,0) == OK && - AP_HAL::millis() - start_wait_ms < 7000) { - hal.scheduler->delay(500); + 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"); + } } - hal.console->printf("UAVCAN: node discovery complete\n"); - close(fd); } } -#endif // CONFIG_HAL_BOARD && !CONFIG_ARCH_BOARD_PX4FMU_V1 +#endif } extern "C" int waitpid(pid_t, int *, int); @@ -269,7 +270,7 @@ bool AP_BoardConfig::px4_start_driver(main_fn_t main_function, const char *name, printf("Starting driver %s %s\n", name, arguments); pid_t pid; - + if (task_spawn(&pid, name, main_function, nullptr, nullptr, args, nullptr) != 0) { free(s); @@ -297,7 +298,7 @@ void AP_BoardConfig::px4_setup_drivers(void) sensor brownout on boot */ if (px4_start_driver(fmu_main, "fmu", "sensor_reset 20")) { - printf("FMUv4 sensor reset complete\n"); + printf("FMUv4 sensor reset complete\n"); } #endif @@ -305,7 +306,7 @@ void AP_BoardConfig::px4_setup_drivers(void) printf("Old drivers no longer supported\n"); px4.board_type = PX4_BOARD_AUTO; } - + // run board auto-detection px4_autodetect(); @@ -313,7 +314,7 @@ void AP_BoardConfig::px4_setup_drivers(void) px4.board_type == PX4_BOARD_PIXHAWK2) { _imu_target_temperature.set_default(60); } - + px4_configured_board = (enum px4_board_type)px4.board_type.get(); switch (px4_configured_board) { @@ -473,7 +474,7 @@ void AP_BoardConfig::px4_autodetect(void) // user has chosen a board type return; } - + #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) // only one choice px4.board_type.set(PX4_BOARD_PX4V1); @@ -499,6 +500,7 @@ void AP_BoardConfig::px4_autodetect(void) spi_check_register(HAL_INS_ICM20608_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) || spi_check_register(HAL_INS_ICM20608_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) || spi_check_register(HAL_INS_MPU9250_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250))) { + // classic or upgraded Pixhawk1 px4.board_type.set(PX4_BOARD_PIXHAWK); hal.console->printf("Detected Pixhawk\n"); @@ -507,13 +509,13 @@ void AP_BoardConfig::px4_autodetect(void) } #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4) // only one choice - px4.board_type.set_and_notify(PX4_BOARD_PIXRACER); + px4.board_type.set_and_notify(PX4_BOARD_PIXRACER); hal.console->printf("Detected Pixracer\n"); #elif defined(CONFIG_ARCH_BOARD_AEROFC_V1) px4.board_type.set_and_notify(PX4_BOARD_AEROFC); hal.console->printf("Detected Aero FC\n"); #endif - + } /*