mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
update - See Arducopter for implementation details
git-svn-id: https://arducopter.googlecode.com/svn/trunk@985 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
4ef6bd673f
commit
efa2c199f9
@ -54,64 +54,68 @@ const float AP_IMU::_gyro_temp_curve[3][3] = {
|
||||
{1665,0,0}
|
||||
}; // To Do - make additional constructors to pass this in.
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
|
||||
AP_IMU::AP_IMU(AP_ADC * adc)
|
||||
: _adc(adc)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_IMU::quick_init(uint16_t *_offset_address)
|
||||
AP_IMU::set_health(float health)
|
||||
{
|
||||
// We read the imu offsets from EEPROM for a quick air restart
|
||||
eeprom_read_block((void*)&_adc_offset, _offset_address, sizeof(_adc_offset));
|
||||
|
||||
_health += constrain(health, 0, 1);
|
||||
}
|
||||
|
||||
float
|
||||
AP_IMU::get_health(void)
|
||||
{
|
||||
return _health;
|
||||
}
|
||||
|
||||
void
|
||||
AP_IMU::init(void)
|
||||
{
|
||||
init_gyro();
|
||||
init_accel();
|
||||
}
|
||||
/**************************************************/
|
||||
void
|
||||
AP_IMU::read_offsets(void)
|
||||
AP_IMU::init_gyro(void)
|
||||
{
|
||||
|
||||
float temp;
|
||||
int flashcount = 0;
|
||||
int tc_temp = _adc->Ch(_gyro_temp_ch);
|
||||
delay(500);
|
||||
Serial.println("Init Gyro");
|
||||
|
||||
for(int c = 0; c < 200; c++)
|
||||
{
|
||||
for(int c = 0; c < 200; c++){
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
digitalWrite(C_LED_PIN, HIGH);
|
||||
delay(20);
|
||||
|
||||
for (int i = 0; i < 6; i++)
|
||||
_adc_in[i] = _adc->Ch(_sensors[i]);
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
|
||||
digitalWrite(A_LED_PIN, HIGH);
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
delay(20);
|
||||
}
|
||||
|
||||
for(int i = 0; i < 200; i++){ // We take some readings...
|
||||
for (int j = 0; j < 6; j++) {
|
||||
for(int i = 0; i < 200; i++){
|
||||
for (int j = 0; j <= 2; j++){
|
||||
_adc_in[j] = _adc->Ch(_sensors[j]);
|
||||
if (j < 3) {
|
||||
_adc_in[j] -= gyro_temp_comp(j, tc_temp); // Subtract temp compensated typical gyro bias
|
||||
} else {
|
||||
_adc_in[j] -= 2025;
|
||||
}
|
||||
|
||||
// Subtract temp compensated typical gyro bias
|
||||
_adc_in[j] -= gyro_temp_comp(j, tc_temp);
|
||||
|
||||
// filter
|
||||
_adc_offset[j] = _adc_offset[j] * 0.9 + _adc_in[j] * 0.1;
|
||||
|
||||
}
|
||||
|
||||
delay(20);
|
||||
if(flashcount == 5) {
|
||||
Serial.print("*");
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
digitalWrite(C_LED_PIN, HIGH);
|
||||
}
|
||||
|
||||
if(flashcount >= 10) {
|
||||
flashcount = 0;
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
@ -121,39 +125,48 @@ AP_IMU::read_offsets(void)
|
||||
}
|
||||
|
||||
_adc_offset[5] += GRAVITY * _sensor_signs[5];
|
||||
|
||||
save_gyro_eeprom();
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
|
||||
void
|
||||
AP_IMU::init(uint16_t *_offset_address)
|
||||
AP_IMU::init_accel(void) // 3, 4, 5
|
||||
{
|
||||
float temp;
|
||||
int flashcount = 0;
|
||||
delay(500);
|
||||
|
||||
read_offsets();
|
||||
|
||||
// Save offset values to EEPROM for use in an air restart, gyro only restart
|
||||
eeprom_write_block((const void *)&_adc_offset, _offset_address, sizeof(_adc_offset));
|
||||
Serial.println("Init Accel");
|
||||
|
||||
for(int c = 0; c < 200; c++){
|
||||
for (int i = 0; i < 6; i++)
|
||||
_adc_in[i] = _adc->Ch(_sensors[i]);
|
||||
}
|
||||
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_IMU::gyro_init(uint16_t *_offset_address)
|
||||
{
|
||||
float temp[6];
|
||||
read_offsets();
|
||||
// We read the imu offsets from EEPROM to reuse the saved accel values.
|
||||
// This way we do not need the IMU to be level during calibration.
|
||||
eeprom_read_block((void*)&temp, _offset_address, sizeof(_adc_offset));
|
||||
_adc_offset[3] = temp[3];
|
||||
_adc_offset[4] = temp[4];
|
||||
_adc_offset[5] = temp[5];
|
||||
|
||||
// Save offset values to EEPROM for use in an air restart
|
||||
eeprom_write_block((const void *)&_adc_offset, _offset_address, sizeof(_adc_offset));
|
||||
for(int i = 0; i < 200; i++){ // We take some readings...
|
||||
for (int j = 3; j <= 5; j++){
|
||||
_adc_in[j] = _adc->Ch(_sensors[j]);
|
||||
_adc_in[j] -= 2025;
|
||||
_adc_offset[j] = _adc_offset[j] * 0.9 + _adc_in[j] * 0.1;
|
||||
}
|
||||
|
||||
delay(20);
|
||||
if(flashcount == 5) {
|
||||
Serial.print("*");
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
digitalWrite(C_LED_PIN, HIGH);
|
||||
}
|
||||
|
||||
if(flashcount >= 10) {
|
||||
flashcount = 0;
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
digitalWrite(A_LED_PIN, HIGH);
|
||||
}
|
||||
flashcount++;
|
||||
}
|
||||
_adc_offset[5] += GRAVITY * _sensor_signs[5];
|
||||
save_accel_eeprom();
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
// Returns the temperature compensated raw gyro value
|
||||
@ -165,6 +178,7 @@ AP_IMU::gyro_temp_comp(int i, int temp) const
|
||||
//------------------------------------------------------------------------
|
||||
return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp;
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
Vector3f
|
||||
AP_IMU::get_gyro(void)
|
||||
@ -215,3 +229,69 @@ AP_IMU::get_accel(void)
|
||||
|
||||
return _accel_vector;
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void
|
||||
AP_IMU::load_gyro_eeprom(void)
|
||||
{
|
||||
_adc_offset[0] = read_EE_float(_address );
|
||||
_adc_offset[1] = read_EE_float(_address + 4);
|
||||
_adc_offset[2] = read_EE_float(_address + 8);
|
||||
}
|
||||
|
||||
void
|
||||
AP_IMU::save_gyro_eeprom(void)
|
||||
{
|
||||
write_EE_float(_adc_offset[0], _address);
|
||||
write_EE_float(_adc_offset[1], _address + 4);
|
||||
write_EE_float(_adc_offset[2], _address + 8);
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void
|
||||
AP_IMU::load_accel_eeprom(void)
|
||||
{
|
||||
_adc_offset[3] = read_EE_float(_address + 12);
|
||||
_adc_offset[4] = read_EE_float(_address + 16);
|
||||
_adc_offset[5] = read_EE_float(_address + 20);
|
||||
}
|
||||
|
||||
void
|
||||
AP_IMU::save_accel_eeprom(void)
|
||||
{
|
||||
write_EE_float(_adc_offset[3], _address + 12);
|
||||
write_EE_float(_adc_offset[4], _address + 16);
|
||||
write_EE_float(_adc_offset[5], _address + 20);
|
||||
}
|
||||
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
float
|
||||
AP_IMU::read_EE_float(int address)
|
||||
{
|
||||
union {
|
||||
byte bytes[4];
|
||||
float value;
|
||||
} _floatOut;
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
_floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
|
||||
return _floatOut.value;
|
||||
}
|
||||
|
||||
void
|
||||
AP_IMU::write_EE_float(float value, int address)
|
||||
{
|
||||
union {
|
||||
byte bytes[4];
|
||||
float value;
|
||||
} _floatIn;
|
||||
|
||||
_floatIn.value = value;
|
||||
for (int i = 0; i < 4; i++)
|
||||
eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
|
||||
}
|
||||
|
||||
|
||||
@ -11,20 +11,33 @@
|
||||
|
||||
class AP_IMU
|
||||
{
|
||||
|
||||
public:
|
||||
// Constructors
|
||||
AP_IMU(AP_ADC * adc); // Default Constructor
|
||||
AP_IMU(AP_ADC *adc, uint16_t address) :
|
||||
_adc(adc),
|
||||
_address(address)
|
||||
{}
|
||||
|
||||
// Methods
|
||||
void quick_init(uint16_t *_offset_address); // For air start
|
||||
void init(uint16_t *_offset_address); // For calibration (includes accels)
|
||||
void gyro_init(uint16_t *_offset_address); // Read gyro offsets, use stored accel offsets
|
||||
void init(void); // inits both
|
||||
void init_accel(void); // just Accels
|
||||
void init_gyro(void); // just gyros
|
||||
|
||||
void load_gyro_eeprom(void);
|
||||
void save_gyro_eeprom(void);
|
||||
void load_accel_eeprom(void);
|
||||
void save_accel_eeprom(void);
|
||||
|
||||
// raw ADC values - called by DCM
|
||||
Vector3f get_gyro(void); // Radians/second
|
||||
Vector3f get_accel(void); // meters/seconds squared
|
||||
|
||||
void set_health(float health);
|
||||
float get_health(void);
|
||||
|
||||
// Members
|
||||
uint8_t gyro_sat_count;
|
||||
uint8_t adc_constraints;
|
||||
uint8_t adc_constraints; // a check of how many times we get non-sensical values
|
||||
|
||||
private:
|
||||
// Methods
|
||||
@ -32,13 +45,17 @@ private:
|
||||
float gyro_temp_comp(int i, int temp) const;
|
||||
|
||||
// members
|
||||
//uint16_t _offset_address; // EEPROM start address for saving/retrieving offsets
|
||||
uint16_t _address; // EEPROM start address for saving/retrieving offsets
|
||||
float _adc_in[6]; // array that store the 6 ADC channels used by IMU
|
||||
float _adc_offset[6]; // Array that store the Offset of the gyros and accelerometers
|
||||
float _health;
|
||||
Vector3f _accel_vector; // Store the acceleration in a vector
|
||||
Vector3f _gyro_vector; // Store the gyros turn rate in a vector
|
||||
AP_ADC * _adc; // Analog to digital converter pointer
|
||||
|
||||
float read_EE_float(int address);
|
||||
void write_EE_float(float value, int address);
|
||||
|
||||
// constants
|
||||
static const uint8_t _sensors[6];
|
||||
static const int _sensor_signs[9];
|
||||
|
||||
Loading…
Reference in New Issue
Block a user