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

AP_InertialSensor: use PX4 library for VRBRAIN boards.

This commit is contained in:
Emile Castelnuovo 2014-12-30 12:01:24 +01:00 committed by Andrew Tridgell
parent 25818f08a3
commit dae32984f1
6 changed files with 7 additions and 249 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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

1
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -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"

7
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -1,7 +1,7 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h>
#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;
}

2
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -4,7 +4,7 @@
#define __AP_INERTIAL_SENSOR_PX4_H__
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <AP_Progmem.h>
#include "AP_InertialSensor.h"

179
libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp
View File

@ -1,179 +0,0 @@
/// -*- 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];
// 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

63
libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.h
View File

@ -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 <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__