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

AP_BoardConfig: move of CAN parameters into separate group and support of several interfaces

This commit is contained in:
Eugene Shamaev 2017-05-06 12:12:54 +03:00 committed by Francisco Ferreira
parent 36655310b3
commit a7921a273b
6 changed files with 286 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_BoardConfig/AP_BoardConfig.cpp
View File

@ -16,7 +16,6 @@
* AP_BoardConfig - board specific configuration
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include <GCS_MAVLink/GCS.h>
@ -32,10 +31,6 @@
#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)
@ -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

49
libraries/AP_BoardConfig/AP_BoardConfig.h
View File

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

162
libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
* AP_BoardConfig_CAN - board specific configuration for CAN interface
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include "AP_BoardConfig_CAN.h"
#if HAL_WITH_UAVCAN
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <AP_HAL_PX4/CAN.h>
#endif
#include <AP_UAVCAN/AP_UAVCAN.h>
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 <AP_HAL::HAL&> (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

100
libraries/AP_BoardConfig/AP_BoardConfig_CAN.h Normal file
View File

@ -0,0 +1,100 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_BoardConfig.h"
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <sys/ioctl.h>
#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

39
libraries/AP_BoardConfig/canbus.cpp
View File

@ -18,41 +18,48 @@
#include "AP_BoardConfig.h"
#if HAL_WITH_UAVCAN
#include "AP_BoardConfig_CAN.h"
#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
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
};

52
libraries/AP_BoardConfig/px4_drivers.cpp
View File

@ -29,11 +29,6 @@
#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;
@ -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 <AP_HAL::HAL&> (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