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

AP_InertialSensor: VRBRAIN added 3th inertial sensor

This commit is contained in:
LukeMike 2014-07-14 12:53:59 +02:00 committed by Andrew Tridgell
parent dd1cab3ab8
commit 85fc72c57c
2 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -14,7 +14,7 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
#define INS_MAX_INSTANCES 3 #define INS_MAX_INSTANCES 3
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#define INS_MAX_INSTANCES 2 #define INS_MAX_INSTANCES 3
#else #else
#define INS_MAX_INSTANCES 1 #define INS_MAX_INSTANCES 1
#endif #endif

18
libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp
View File

@ -22,17 +22,27 @@ uint16_t AP_InertialSensor_VRBRAIN::_init_sensor( Sample_rate sample_rate )
// assumes max 2 instances // assumes max 2 instances
_accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY); _accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY);
_accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY); _accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY);
_accel_fd[2] = open(ACCEL_DEVICE_PATH "2", O_RDONLY);
_gyro_fd[0] = open(GYRO_DEVICE_PATH, O_RDONLY); _gyro_fd[0] = open(GYRO_DEVICE_PATH, O_RDONLY);
_gyro_fd[1] = open(GYRO_DEVICE_PATH "1", O_RDONLY); _gyro_fd[1] = open(GYRO_DEVICE_PATH "1", O_RDONLY);
_gyro_fd[2] = open(GYRO_DEVICE_PATH "2", O_RDONLY);
if (_accel_fd[0] < 0) { _num_accel_instances = 0;
_num_gyro_instances = 0;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_fd[i] >= 0) {
_num_accel_instances = i+1;
}
if (_gyro_fd[i] >= 0) {
_num_gyro_instances = i+1;
}
}
if (_num_accel_instances == 0) {
hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH); hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
} }
if (_gyro_fd[0] < 0) { if (_num_gyro_instances == 0) {
hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH); hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
} }
_num_accel_instances = _accel_fd[1] >= 0?2:1;
_num_gyro_instances = _gyro_fd[1] >= 0?2:1;
switch (sample_rate) { switch (sample_rate) {
case RATE_50HZ: case RATE_50HZ: