From 85fc72c57c36f3cc1f6dfa6b109a00fb984e5746 Mon Sep 17 00:00:00 2001 From: LukeMike Date: Mon, 14 Jul 2014 12:53:59 +0200 Subject: [PATCH] AP_InertialSensor: VRBRAIN added 3th inertial sensor --- .../AP_InertialSensor/AP_InertialSensor.h | 2 +- .../AP_InertialSensor_VRBRAIN.cpp | 18 ++++++++++++++---- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 9d065d19a1..1caa071c7b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -14,7 +14,7 @@ #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 #elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#define INS_MAX_INSTANCES 2 +#define INS_MAX_INSTANCES 3 #else #define INS_MAX_INSTANCES 1 #endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp index d6c43cc2f8..da0938cb2a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp @@ -22,17 +22,27 @@ uint16_t AP_InertialSensor_VRBRAIN::_init_sensor( Sample_rate sample_rate ) // assumes max 2 instances _accel_fd[0] = open(ACCEL_DEVICE_PATH, 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[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= 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); } - if (_gyro_fd[0] < 0) { + if (_num_gyro_instances == 0) { 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) { case RATE_50HZ: