From 6a93148b9249d03672f4036f0c88baf849f7523a Mon Sep 17 00:00:00 2001 From: LukeMike Date: Fri, 14 Nov 2014 15:08:39 +0100 Subject: [PATCH] AP_InertialSensor: added library for VRBRAIN Inertial Sensor --- .../AP_InertialSensor/AP_InertialSensor.cpp | 4 +- .../AP_InertialSensor/AP_InertialSensor.h | 1 + .../AP_InertialSensor_PX4.cpp | 2 +- .../AP_InertialSensor/AP_InertialSensor_PX4.h | 2 +- .../AP_InertialSensor_VRBRAIN.cpp | 171 ++++++++++++++++++ .../AP_InertialSensor_VRBRAIN.h | 63 +++++++ 6 files changed, 240 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp create 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 17155715e1..856338ea48 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -338,8 +338,10 @@ 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 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN +#elif HAL_INS_DEFAULT == HAL_INS_PX4 _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 d8d3d58b93..1096462b73 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -293,6 +293,7 @@ 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 c36e126414..659cc2a0fd 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 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #include "AP_InertialSensor_PX4.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h index 262608baaa..c83043dddb 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 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #include #include "AP_InertialSensor.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp new file mode 100644 index 0000000000..ce269f9535 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp @@ -0,0 +1,171 @@ +/// -*- 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]; + _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]; + _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 new file mode 100644 index 0000000000..bbedc0ab3d --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.h @@ -0,0 +1,63 @@ +/// -*- 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__