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

AP_BoardConfig: support for CAN and UAVCAN at board level

This commit is contained in:
Eugene Shamaev 2017-04-02 17:56:37 +03:00 committed by Francisco Ferreira
parent dfe9b0e6fc
commit 0b54d5764c
4 changed files with 177 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_BoardConfig/AP_BoardConfig.cpp
View File

@ -30,6 +30,10 @@
#include <drivers/drv_sbus.h>
#endif
#if HAL_WITH_UAVCAN
#include <AP_UAVCAN/AP_UAVCAN.h>
#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

65
libraries/AP_BoardConfig/AP_BoardConfig.h
View File

@ -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

59
libraries/AP_BoardConfig/canbus.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include "AP_BoardConfig.h"
#if HAL_WITH_UAVCAN
#include <AP_UAVCAN/AP_UAVCAN.h>
// 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

88
libraries/AP_BoardConfig/px4_drivers.cpp
View File

@ -16,7 +16,6 @@
* AP_BoardConfig - px4 driver loading and setup
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_BoardConfig.h"
@ -31,6 +30,11 @@
#include <nuttx/arch.h>
#include <spawn.h>
#if HAL_WITH_UAVCAN
#include <AP_HAL_PX4/CAN.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#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 <AP_HAL::HAL&> (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
}
/*