LukeMike
10 years ago
committed by
Andrew Tridgell
6 changed files with 240 additions and 3 deletions
@ -0,0 +1,171 @@
@@ -0,0 +1,171 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include <AP_HAL.h> |
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN |
||||
|
||||
#include "AP_InertialSensor_VRBRAIN.h" |
||||
|
||||
const extern AP_HAL::HAL& hal; |
||||
|
||||
#include <sys/types.h> |
||||
#include <sys/stat.h> |
||||
#include <fcntl.h> |
||||
#include <unistd.h> |
||||
|
||||
#include <drivers/drv_accel.h> |
||||
#include <drivers/drv_gyro.h> |
||||
#include <drivers/drv_hrt.h> |
||||
|
||||
#include <stdio.h> |
||||
|
||||
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<INS_MAX_INSTANCES; i++) { |
||||
if (_accel_fd[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
|
||||
|
||||
@ -0,0 +1,63 @@
@@ -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 <AP_HAL.h> |
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN |
||||
|
||||
#include <AP_Progmem.h> |
||||
#include "AP_InertialSensor.h" |
||||
#include <drivers/drv_accel.h> |
||||
#include <drivers/drv_gyro.h> |
||||
#include <uORB/uORB.h> |
||||
#include <uORB/topics/sensor_combined.h> |
||||
|
||||
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__
|
||||
Loading…
Reference in new issue