From dae32984f1e0b8548b0bc99085c785cd430045bc Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Tue, 30 Dec 2014 12:01:24 +0100 Subject: [PATCH] AP_InertialSensor: use PX4 library for VRBRAIN boards. --- .../AP_InertialSensor/AP_InertialSensor.cpp | 4 +- .../AP_InertialSensor/AP_InertialSensor.h | 1 - .../AP_InertialSensor_PX4.cpp | 7 +- .../AP_InertialSensor/AP_InertialSensor_PX4.h | 2 +- .../AP_InertialSensor_VRBRAIN.cpp | 179 ------------------ .../AP_InertialSensor_VRBRAIN.h | 63 ------ 6 files changed, 7 insertions(+), 249 deletions(-) delete mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp delete mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 856338ea48..17155715e1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -338,10 +338,8 @@ AP_InertialSensor::_detect_backends(void) _add_backend(AP_InertialSensor_HIL::detect); #elif HAL_INS_DEFAULT == HAL_INS_MPU6000 _add_backend(AP_InertialSensor_MPU6000::detect); -#elif HAL_INS_DEFAULT == HAL_INS_PX4 +#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN _add_backend(AP_InertialSensor_PX4::detect); -#elif HAL_INS_DEFAULT == HAL_INS_VRBRAIN - _add_backend(AP_InertialSensor_VRBRAIN::detect); #elif HAL_INS_DEFAULT == HAL_INS_OILPAN _add_backend(AP_InertialSensor_Oilpan::detect); #elif HAL_INS_DEFAULT == HAL_INS_MPU9250 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 1096462b73..d8d3d58b93 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -293,7 +293,6 @@ private: #include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor_MPU6000.h" #include "AP_InertialSensor_PX4.h" -#include "AP_InertialSensor_VRBRAIN.h" #include "AP_InertialSensor_Oilpan.h" #include "AP_InertialSensor_MPU9250.h" #include "AP_InertialSensor_L3G4200D.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index 659cc2a0fd..bc8a5232f2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -1,7 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #include "AP_InertialSensor_PX4.h" @@ -72,12 +72,15 @@ bool AP_InertialSensor_PX4::_init_sensor(void) _default_filter_hz = _default_filter(); _set_filter_frequency(_imu.get_filter()); +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + _product_id = AP_PRODUCT_ID_VRBRAIN; +#else #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) _product_id = AP_PRODUCT_ID_PX4_V2; #else _product_id = AP_PRODUCT_ID_PX4; #endif - +#endif return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h index c83043dddb..262608baaa 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -4,7 +4,7 @@ #define __AP_INERTIAL_SENSOR_PX4_H__ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #include #include "AP_InertialSensor.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp deleted file mode 100644 index 9e74088c85..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp +++ /dev/null @@ -1,179 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include "AP_InertialSensor_VRBRAIN.h" - -const extern AP_HAL::HAL& hal; - -#include -#include -#include -#include - -#include -#include -#include - -#include - -AP_InertialSensor_VRBRAIN::AP_InertialSensor_VRBRAIN(AP_InertialSensor &imu) : - AP_InertialSensor_Backend(imu), - _last_get_sample_timestamp(0) -{ -} - -/* - detect the sensor - */ -AP_InertialSensor_Backend *AP_InertialSensor_VRBRAIN::detect(AP_InertialSensor &_imu) -{ - AP_InertialSensor_VRBRAIN *sensor = new AP_InertialSensor_VRBRAIN(_imu); - if (sensor == NULL) { - return NULL; - } - if (!sensor->_init_sensor()) { - delete sensor; - return NULL; - } - return sensor; -} - -bool AP_InertialSensor_VRBRAIN::_init_sensor(void) -{ - // assumes max 3 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); - - _num_accel_instances = 0; - _num_gyro_instances = 0; - for (uint8_t i=0; i= 0) { - _num_accel_instances = i+1; - _accel_instance[i] = _imu.register_accel(); - } - if (_gyro_fd[i] >= 0) { - _num_gyro_instances = i+1; - _gyro_instance[i] = _imu.register_gyro(); - } - } - if (_num_accel_instances == 0) { - return false; - } - if (_num_gyro_instances == 0) { - return false; - } - - _default_filter_hz = _default_filter(); - _set_filter_frequency(_imu.get_filter()); - - - - - return AP_PRODUCT_ID_VRBRAIN; - - - return true; -} - -/* - set the filter frequency - */ -void AP_InertialSensor_VRBRAIN::_set_filter_frequency(uint8_t filter_hz) -{ - if (filter_hz == 0) { - filter_hz = _default_filter_hz; - } - for (uint8_t i=0; i<_num_gyro_instances; i++) { - ioctl(_gyro_fd[i], GYROIOCSLOWPASS, filter_hz); - } - for (uint8_t i=0; i<_num_accel_instances; i++) { - ioctl(_accel_fd[i], ACCELIOCSLOWPASS, filter_hz); - } -} - -bool AP_InertialSensor_VRBRAIN::update(void) -{ - // get the latest sample from the sensor drivers - _get_sample(); - - for (uint8_t k=0; k<_num_accel_instances; k++) { - Vector3f accel = _accel_in[k]; - // calling _rotate_and_offset_accel sets the sensor healthy, - // so we only want to do this if we have new data from it - if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) { - _rotate_and_offset_accel(_accel_instance[k], accel); - _last_accel_update_timestamp[k] = _last_accel_timestamp[k]; - } - } - - for (uint8_t k=0; k<_num_gyro_instances; k++) { - Vector3f gyro = _gyro_in[k]; - // calling _rotate_and_offset_accel sets the sensor healthy, - // so we only want to do this if we have new data from it - if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) { - _rotate_and_offset_gyro(_gyro_instance[k], gyro); - _last_gyro_update_timestamp[k] = _last_gyro_timestamp[k]; - } - } - - if (_last_filter_hz != _imu.get_filter()) { - _set_filter_frequency(_imu.get_filter()); - _last_filter_hz = _imu.get_filter(); - } - - return true; -} - -void AP_InertialSensor_VRBRAIN::_get_sample(void) -{ - for (uint8_t i=0; i<_num_accel_instances; i++) { - struct accel_report accel_report; - while (_accel_fd[i] != -1 && - ::read(_accel_fd[i], &accel_report, sizeof(accel_report)) == sizeof(accel_report) && - accel_report.timestamp != _last_accel_timestamp[i]) { - _accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z); - _last_accel_timestamp[i] = accel_report.timestamp; - } - } - for (uint8_t i=0; i<_num_gyro_instances; i++) { - struct gyro_report gyro_report; - while (_gyro_fd[i] != -1 && - ::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) && - gyro_report.timestamp != _last_gyro_timestamp[i]) { - _gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z); - _last_gyro_timestamp[i] = gyro_report.timestamp; - } - } - _last_get_sample_timestamp = hal.scheduler->micros64(); -} - -bool AP_InertialSensor_VRBRAIN::gyro_sample_available(void) -{ - _get_sample(); - for (uint8_t i=0; i<_num_gyro_instances; i++) { - if (_last_gyro_timestamp[i] != _last_gyro_update_timestamp[i]) { - return true; - } - } - return false; -} - -bool AP_InertialSensor_VRBRAIN::accel_sample_available(void) -{ - _get_sample(); - for (uint8_t i=0; i<_num_accel_instances; i++) { - if (_last_accel_timestamp[i] != _last_accel_update_timestamp[i]) { - return true; - } - } - return false; -} - -#endif // CONFIG_HAL_BOARD - diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.h b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.h deleted file mode 100644 index bbedc0ab3d..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.h +++ /dev/null @@ -1,63 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_INERTIAL_SENSOR_VRBRAIN_H__ -#define __AP_INERTIAL_SENSOR_VRBRAIN_H__ - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include -#include "AP_InertialSensor.h" -#include -#include -#include -#include - -class AP_InertialSensor_VRBRAIN : public AP_InertialSensor_Backend -{ -public: - - AP_InertialSensor_VRBRAIN(AP_InertialSensor &imu); - - /* update accel and gyro state */ - bool update(); - - // detect the sensor - static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); - - bool gyro_sample_available(void); - bool accel_sample_available(void); - -private: - bool _init_sensor(void); - void _get_sample(void); - bool _sample_available(void); - Vector3f _accel_in[INS_MAX_INSTANCES]; - Vector3f _gyro_in[INS_MAX_INSTANCES]; - uint64_t _last_accel_timestamp[INS_MAX_INSTANCES]; - uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES]; - uint64_t _last_accel_update_timestamp[INS_MAX_INSTANCES]; - uint64_t _last_gyro_update_timestamp[INS_MAX_INSTANCES]; - uint64_t _last_get_sample_timestamp; - uint64_t _last_sample_timestamp; - - // support for updating filter at runtime - uint8_t _last_filter_hz; - uint8_t _default_filter_hz; - - void _set_filter_frequency(uint8_t filter_hz); - - // accelerometer and gyro driver handles - uint8_t _num_accel_instances; - uint8_t _num_gyro_instances; - - int _accel_fd[INS_MAX_INSTANCES]; - int _gyro_fd[INS_MAX_INSTANCES]; - - // indexes in frontend object. Note that these could be different - // from the backend indexes - uint8_t _accel_instance[INS_MAX_INSTANCES]; - uint8_t _gyro_instance[INS_MAX_INSTANCES]; -}; -#endif // CONFIG_HAL_BOARD -#endif // __AP_INERTIAL_SENSOR_VRBRAIN_H__