aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: converted the APM1/Oilpan driver to new API

Browse Source
This commit is contained in:
Andrew Tridgell 2014-10-16 10:31:21 +11:00
parent dcef9bb3b8
commit 85686c22ec
2 changed files with 87 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

170
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -1,11 +1,16 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
#include "AP_InertialSensor_Oilpan.h"
#include <AP_ADC.h>
const extern AP_HAL::HAL& hal;
// this driver assumes an AP_ADC object has been declared globally
extern AP_ADC_ADS7844 apm1_adc;
// ADC channel mappings on for the APM Oilpan
// Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 };
@ -14,9 +19,6 @@ const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 };
const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] =
{ 1, -1, -1, 1, -1, -1 };
// ADC channel reading the gyro temperature
const uint8_t AP_InertialSensor_Oilpan::_gyro_temp_ch = 3;
// Maximum possible value returned by an offset-corrected sensor channel
const float AP_InertialSensor_Oilpan::_adc_constraint = 900;
@ -30,120 +32,94 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900;
#define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f)
#define OILPAN_RAW_GYRO_OFFSET 1658.0f
#define ToRad(x) radians(x) // *pi/180
// IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s,
// 0.8mV/ADC step => 0.8/3.33 = 0.4
// Tested values : 0.4026, ?, 0.4192
const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4f);
const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41f);
const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41f);
const float AP_InertialSensor_Oilpan::_gyro_gain_x = radians(0.4f);
const float AP_InertialSensor_Oilpan::_gyro_gain_y = radians(0.41f);
const float AP_InertialSensor_Oilpan::_gyro_gain_z = radians(0.41f);
/* ------ Public functions -------------------------------------------*/
AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) :
AP_InertialSensor(),
_adc(adc)
AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu)
{
}
uint16_t AP_InertialSensor_Oilpan::_init_sensor( Sample_rate sample_rate)
{
_adc->Init();
switch (sample_rate) {
case RATE_50HZ:
_sample_threshold = 20;
break;
case RATE_100HZ:
_sample_threshold = 10;
break;
case RATE_200HZ:
_sample_threshold = 5;
break;
}
#if defined(__AVR_ATmega1280__)
return AP_PRODUCT_ID_APM1_1280;
#else
return AP_PRODUCT_ID_APM1_2560;
#endif
}
bool AP_InertialSensor_Oilpan::update()
{
if (!wait_for_sample(100)) {
return false;
}
float adc_values[6];
Vector3f gyro_offset = _gyro_offset[0].get();
Vector3f accel_scale = _accel_scale[0].get();
Vector3f accel_offset = _accel_offset[0].get();
_delta_time_micros = _adc->Ch6(_sensors, adc_values);
_temp = _adc->Ch(_gyro_temp_ch);
_gyro[0] = Vector3f(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ),
_sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ),
_sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ));
_gyro[0].rotate(_board_orientation);
_gyro[0].x *= _gyro_gain_x;
_gyro[0].y *= _gyro_gain_y;
_gyro[0].z *= _gyro_gain_z;
_gyro[0] -= gyro_offset;
_previous_accel[0] = _accel[0];
_accel[0] = Vector3f(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
_accel[0].rotate(_board_orientation);
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] *= OILPAN_ACCEL_SCALE_1G;
_accel[0] -= accel_offset;
/*
* X = 1619.30 to 2445.69
* Y = 1609.45 to 2435.42
* Z = 1627.44 to 2434.82
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_Oilpan::detect(AP_InertialSensor &_imu,
AP_InertialSensor::Sample_rate sample_rate)
{
AP_InertialSensor_Oilpan *sensor = new AP_InertialSensor_Oilpan(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor(sample_rate)) {
delete sensor;
return NULL;
}
return sensor;
}
bool AP_InertialSensor_Oilpan::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
{
apm1_adc.Init();
switch (sample_rate) {
case AP_InertialSensor::RATE_50HZ:
_sample_threshold = 20;
break;
case AP_InertialSensor::RATE_100HZ:
_sample_threshold = 10;
break;
case AP_InertialSensor::RATE_200HZ:
_sample_threshold = 5;
break;
default:
// can't do this speed
return false;
}
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
return true;
}
float AP_InertialSensor_Oilpan::get_delta_time() const {
return _delta_time_micros * 1.0e-6;
}
/* ------ Private functions -------------------------------------------*/
// return the oilpan gyro drift rate in radian/s/s
float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
/*
copy data from ADC to frontend
*/
bool AP_InertialSensor_Oilpan::update()
{
// 3.0 degrees/second/minute
return ToRad(3.0/60);
float adc_values[6];
uint32_t now = hal.scheduler->micros();
apm1_adc.Ch6(_sensors, adc_values);
// copy gyros to frontend
Vector3f v;
v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x,
_sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y,
_sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z);
_rotate_and_offset_gyro(_gyro_instance, v, now);
// copy accels to frontend
v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
v *= OILPAN_ACCEL_SCALE_1G;
_rotate_and_offset_accel(_accel_instance, v, now);
return true;
}
// return true if a new sample is available
bool AP_InertialSensor_Oilpan::_sample_available()
bool AP_InertialSensor_Oilpan::_sample_available() const
{
return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0;
}
bool AP_InertialSensor_Oilpan::wait_for_sample(uint16_t timeout_ms)
{
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay_microseconds(100);
if (_sample_available()) {
return true;
}
}
return false;
return apm1_adc.num_samples_available(_sensors) >= _sample_threshold;
}
#endif // CONFIG_HAL_BOARD

43
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
View File

@ -3,50 +3,35 @@
#ifndef __AP_INERTIAL_SENSOR_OILPAN_H__
#define __AP_INERTIAL_SENSOR_OILPAN_H__
#include <stdint.h>
#include <AP_ADC.h>
#include <AP_Math.h>
#include <AP_HAL.h>
#include "AP_InertialSensor.h"
class AP_InertialSensor_Oilpan : public AP_InertialSensor
class AP_InertialSensor_Oilpan : public AP_InertialSensor_Backend
{
public:
AP_InertialSensor_Oilpan(AP_InertialSensor &imu);
AP_InertialSensor_Oilpan( AP_ADC * adc );
/* update accel and gyro state */
bool update();
/* Concrete implementation of AP_InertialSensor functions: */
bool update();
float get_delta_time() const;
float get_gyro_drift_rate();
// wait for a sample to be available, with timeout in milliseconds
bool wait_for_sample(uint16_t timeout_ms);
protected:
uint16_t _init_sensor(Sample_rate sample_rate);
bool gyro_sample_available(void) { return _sample_available(); }
bool accel_sample_available(void) { return _sample_available(); }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
private:
bool _sample_available();
AP_ADC * _adc;
float _temp;
uint32_t _delta_time_micros;
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
bool _sample_available() const;
static const uint8_t _sensors[6];
static const int8_t _sensor_signs[6];
static const uint8_t _gyro_temp_ch;
static const float _gyro_gain_x;
static const float _gyro_gain_y;
static const float _gyro_gain_z;
static const float _adc_constraint;
uint8_t _sample_threshold;
uint8_t _gyro_instance;
uint8_t _accel_instance;
};
#endif // __AP_INERTIAL_SENSOR_OILPAN_H__