You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
109 lines
4.3 KiB
109 lines
4.3 KiB
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
|
|
/// @file AP_IMU_Oilpan.h |
|
/// @brief IMU driver for the APM oilpan |
|
|
|
#ifndef AP_IMU_Oilpan_h |
|
#define AP_IMU_Oilpan_h |
|
|
|
|
|
#include "../AP_Common/AP_Common.h" |
|
#include "../AP_Math/AP_Math.h" |
|
#include "../AP_ADC/AP_ADC.h" |
|
#include <inttypes.h> |
|
|
|
#include "IMU.h" |
|
|
|
class AP_IMU_Oilpan : public IMU |
|
{ |
|
|
|
public: |
|
/// Constructor |
|
/// |
|
/// Saves the ADC pointer and constructs the calibration data variable. |
|
/// |
|
/// @param adc Pointer to the AP_ADC instance that is connected to the gyro and accelerometer. |
|
/// @param key The AP_Var::key value we will use when loading/saving calibration data. |
|
/// |
|
AP_IMU_Oilpan(AP_ADC *adc, AP_Var::Key key) : |
|
_adc(adc), |
|
_sensor_cal(key, PSTR("IMU_SENSOR_CAL")) |
|
// _sensor_cal(key, PSTR("IMU_SENSOR_CAL"), AP_Var::k_flag_no_auto_load) |
|
{} |
|
|
|
/// Do warm or cold start. |
|
/// |
|
/// @note For a partial-warmstart where e.g. the accelerometer calibration should be preserved |
|
/// but the gyro cal needs to be re-performed, start with ::init(WARM_START) to load the |
|
/// previous calibration settings, then force a re-calibration of the gyro with ::init_gyro. |
|
/// |
|
/// @param style Selects the initialisation style. |
|
/// COLD_START performs calibration of both the accelerometer and gyro. |
|
/// WARM_START loads accelerometer and gyro calibration from a previous cold start. |
|
/// |
|
virtual void init(Start_style style = COLD_START, void (*callback)(unsigned long t) = delay); |
|
|
|
virtual void save(); |
|
virtual void init_accel(void (*callback)(unsigned long t) = delay); |
|
virtual void init_gyro(void (*callback)(unsigned long t) = delay); |
|
virtual bool update(void); |
|
|
|
// for jason |
|
float gx() { return _sensor_cal[0]; } |
|
float gy() { return _sensor_cal[1]; } |
|
float gz() { return _sensor_cal[2]; } |
|
float ax() { return _sensor_cal[3]; } |
|
float ay() { return _sensor_cal[4]; } |
|
float az() { return _sensor_cal[5]; } |
|
|
|
void ax(const float v) { _sensor_cal[3] = v; } |
|
void ay(const float v) { _sensor_cal[4] = v; } |
|
void az(const float v) { _sensor_cal[5] = v; } |
|
|
|
|
|
private: |
|
AP_ADC *_adc; ///< ADC that we use for reading sensors |
|
AP_VarA<float,6> _sensor_cal; ///< Calibrated sensor offsets |
|
|
|
virtual void _init_accel(void (*callback)(unsigned long t)); ///< no-save implementation |
|
virtual void _init_gyro(void (*callback)(unsigned long t)); ///< no-save implementation |
|
|
|
float _sensor_in(uint8_t channel, uint16_t adc_value, int temperature); |
|
float _sensor_compensation(uint8_t channel, int temp) const; |
|
|
|
// constants |
|
static const uint8_t _sensors[6]; ///< ADC channel mappings for the sensors |
|
static const int8_t _sensor_signs[6]; ///< ADC result sign adjustment for sensors |
|
static const uint8_t _gyro_temp_ch = 3; ///< ADC channel reading the gyro temperature |
|
static const float _gyro_temp_curve[3][3]; ///< Temperature compensation curve for the gyro |
|
|
|
// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step |
|
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 |
|
// Tested value : 418 |
|
// |
|
static const float _gravity = 423.8; ///< 1G in the raw data coming from the accelerometer |
|
// Value based on actual sample data from 20 boards |
|
|
|
static const float _accel_scale = 9.80665 / 423.8; ///< would like to use _gravity here, but cannot |
|
|
|
// IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 |
|
// Tested values : 0.4026, ?, 0.4192 |
|
// |
|
static const float _gyro_gain_x = ToRad(0.4); // X axis Gyro gain |
|
static const float _gyro_gain_y = ToRad(0.41); // Y axis Gyro gain |
|
static const float _gyro_gain_z = ToRad(0.41); // Z axis Gyro gain |
|
|
|
// Maximum possible value returned by an offset-corrected sensor channel |
|
// |
|
static const float _adc_constraint = 900; |
|
|
|
// Gyro and Accelerometer calibration criterial |
|
// |
|
static const float _gyro_total_cal_change = 4.0; // Experimentally derived - allows for some minor motion |
|
static const float _gyro_max_cal_offset = 320.0; |
|
static const float _accel_total_cal_change = 4.0; |
|
static const float _accel_max_cal_offset = 250.0; |
|
|
|
}; |
|
|
|
#endif
|
|
|