2016-02-17 21:25:16 -04:00
|
|
|
#pragma once
|
2014-01-19 21:57:39 -04:00
|
|
|
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
2016-06-23 20:48:29 -03:00
|
|
|
#include <sys/ioctl.h>
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
|
|
|
|
|
|
|
#define _UAVCAN_IOCBASE (0x4000) // IOCTL base for module UAVCAN
|
|
|
|
#define _UAVCAN_IOC(_n) (_IOC(_UAVCAN_IOCBASE, _n))
|
|
|
|
|
|
|
|
#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
|
2014-01-19 21:57:39 -04:00
|
|
|
|
2016-08-03 01:18:36 -03:00
|
|
|
extern "C" typedef int (*main_fn_t)(int argc, char **);
|
|
|
|
|
2014-01-19 21:57:39 -04:00
|
|
|
class AP_BoardConfig
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
// constructor
|
|
|
|
AP_BoardConfig(void)
|
|
|
|
{
|
|
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
|
|
};
|
|
|
|
|
|
|
|
void init(void);
|
|
|
|
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
|
2016-08-10 10:48:50 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
2016-08-03 01:46:11 -03:00
|
|
|
// public method to start a driver
|
|
|
|
static bool px4_start_driver(main_fn_t main_function, const char *name, const char *arguments);
|
2016-08-10 10:48:50 -03:00
|
|
|
|
2016-08-03 04:56:04 -03:00
|
|
|
// valid types for BRD_TYPE
|
|
|
|
enum px4_board_type {
|
2016-08-10 10:48:50 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
2016-08-03 04:56:04 -03:00
|
|
|
PX4_BOARD_AUTO = 0,
|
|
|
|
PX4_BOARD_PX4V1 = 1,
|
|
|
|
PX4_BOARD_PIXHAWK = 2,
|
|
|
|
PX4_BOARD_PIXHAWK2 = 3,
|
|
|
|
PX4_BOARD_PIXRACER = 4,
|
|
|
|
PX4_BOARD_PHMINI = 5,
|
|
|
|
PX4_BOARD_PH2SLIM = 6,
|
2016-11-10 22:13:59 -04:00
|
|
|
PX4_BOARD_OLDDRIVERS = 100,
|
2016-08-10 10:48:50 -03:00
|
|
|
#endif
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
|
|
VRX_BOARD_BRAIN51 = 7,
|
|
|
|
VRX_BOARD_BRAIN52 = 8,
|
|
|
|
VRX_BOARD_UBRAIN51 = 9,
|
|
|
|
VRX_BOARD_UBRAIN52 = 10,
|
|
|
|
VRX_BOARD_CORE10 = 11,
|
|
|
|
VRX_BOARD_BRAIN54 = 12,
|
|
|
|
#endif
|
2016-08-03 04:56:04 -03:00
|
|
|
};
|
2016-08-03 01:46:11 -03:00
|
|
|
#endif
|
2016-10-20 21:53:34 -03:00
|
|
|
|
|
|
|
// set default value for BRD_SAFETY_MASK
|
|
|
|
void set_default_safety_ignore_mask(uint16_t mask);
|
2016-11-02 21:40:21 -03:00
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
|
|
static enum px4_board_type get_board_type(void) {
|
|
|
|
return px4_configured_board;
|
|
|
|
}
|
|
|
|
#endif
|
2016-10-20 21:53:34 -03:00
|
|
|
|
2014-01-19 21:57:39 -04:00
|
|
|
private:
|
2015-04-29 23:02:27 -03:00
|
|
|
AP_Int16 vehicleSerialNumber;
|
|
|
|
|
2016-07-05 12:50:34 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
2016-08-02 18:41:24 -03:00
|
|
|
struct {
|
|
|
|
AP_Int8 pwm_count;
|
|
|
|
AP_Int8 safety_enable;
|
|
|
|
AP_Int32 ignore_safety_channels;
|
2015-11-06 02:10:08 -04:00
|
|
|
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
|
2016-08-02 18:41:24 -03:00
|
|
|
AP_Int8 can_enable;
|
2014-02-09 21:57:19 -04:00
|
|
|
#endif
|
2016-08-02 18:41:24 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
|
|
|
AP_Int8 ser1_rtscts;
|
|
|
|
AP_Int8 ser2_rtscts;
|
|
|
|
AP_Int8 sbus_out_rate;
|
2014-03-31 14:53:16 -03:00
|
|
|
#endif
|
2016-08-03 04:56:04 -03:00
|
|
|
AP_Int8 board_type;
|
2016-08-02 18:41:24 -03:00
|
|
|
} px4;
|
2016-11-02 21:40:21 -03:00
|
|
|
|
|
|
|
static enum px4_board_type px4_configured_board;
|
|
|
|
|
2016-08-02 18:41:24 -03:00
|
|
|
void px4_drivers_start(void);
|
2016-08-02 19:34:24 -03:00
|
|
|
void px4_setup(void);
|
|
|
|
void px4_setup_pwm(void);
|
|
|
|
void px4_setup_safety(void);
|
2016-10-20 21:53:34 -03:00
|
|
|
void px4_setup_safety_mask(void);
|
2016-08-02 19:34:24 -03:00
|
|
|
void px4_setup_uart(void);
|
|
|
|
void px4_setup_sbus(void);
|
|
|
|
void px4_setup_canbus(void);
|
2016-08-03 01:18:36 -03:00
|
|
|
void px4_setup_drivers(void);
|
2016-11-02 23:16:44 -03:00
|
|
|
void px4_setup_peripherals(void);
|
2016-11-03 04:15:45 -03:00
|
|
|
void px4_setup_px4io(void);
|
|
|
|
void px4_tone_alarm(const char *tone_string);
|
2016-08-03 01:18:36 -03:00
|
|
|
void px4_sensor_error(const char *reason);
|
2016-11-10 00:29:26 -04:00
|
|
|
bool spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag = 0x80);
|
2016-08-03 01:18:36 -03:00
|
|
|
|
2016-08-10 10:48:50 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
2016-11-10 00:29:26 -04:00
|
|
|
void px4_autodetect(void);
|
2016-08-03 01:18:36 -03:00
|
|
|
void px4_start_common_sensors(void);
|
|
|
|
void px4_start_fmuv1_sensors(void);
|
|
|
|
void px4_start_fmuv2_sensors(void);
|
2016-08-10 10:48:50 -03:00
|
|
|
#endif
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
|
|
void vrx_start_common_sensors(void);
|
|
|
|
void vrx_start_brain51_sensors(void);
|
|
|
|
void vrx_start_brain52_sensors(void);
|
|
|
|
void vrx_start_ubrain51_sensors(void);
|
|
|
|
void vrx_start_ubrain52_sensors(void);
|
|
|
|
void vrx_start_core10_sensors(void);
|
|
|
|
void vrx_start_brain54_sensors(void);
|
|
|
|
#endif
|
|
|
|
|
2016-08-02 18:41:24 -03:00
|
|
|
#endif // HAL_BOARD_PX4 || HAL_BOARD_VRBRAIN
|
2016-06-15 05:28:18 -03:00
|
|
|
|
|
|
|
// target temperarure for IMU in Celsius, or -1 to disable
|
|
|
|
AP_Int8 _imu_target_temperature;
|
2014-01-19 21:57:39 -04:00
|
|
|
};
|