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

AP_InertialSensor: use gyro sample count on L3G4200D

Browse Source 
this gives more even timing in ArduCopter
This commit is contained in:
Andrew Tridgell 2013-10-08 19:19:11 +11:00
parent 6444b0bddd
commit 5d685385eb
2 changed files with 11 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -86,7 +86,6 @@ const extern AP_HAL::HAL& hal;
// constructor
AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() :
AP_InertialSensor(),
_sample_available(false),
_accel_filter_x(800, 10),
_accel_filter_y(800, 10),
_accel_filter_z(800, 10),
@ -102,15 +101,18 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate )
case RATE_50HZ:
_default_filter_hz = 10;
_sample_period_usec = (1000*1000) / 50;
_gyro_samples_needed = 16;
break;
case RATE_100HZ:
_default_filter_hz = 20;
_sample_period_usec = (1000*1000) / 100;
_gyro_samples_needed = 8;
break;
case RATE_200HZ:
default:
_default_filter_hz = 20;
_sample_period_usec = (1000*1000) / 200;
_gyro_samples_needed = 4;
break;
}
@ -237,6 +239,7 @@ bool AP_InertialSensor_L3G4200D::update(void)
hal.scheduler->suspend_timer_procs();
_accel = _accel_filtered;
_gyro = _gyro_filtered;
_gyro_samples_available = 0;
hal.scheduler->resume_timer_procs();
// add offsets and rotation
@ -262,9 +265,6 @@ bool AP_InertialSensor_L3G4200D::update(void)
_last_filter_hz = _mpu6000_filter;
}
_sample_available = false;
_last_sample_time = hal.scheduler->micros();
return true;
}
@ -333,35 +333,32 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
_gyro_filtered = Vector3f(_gyro_filter_x.apply(buffer[i][0]),
_gyro_filter_y.apply(-buffer[i][1]),
_gyro_filter_z.apply(-buffer[i][2]));
_gyro_samples_available++;
}
}
}
if (num_samples_available > 0) {
_sample_available = true;
}
// give back i2c semaphore
i2c_sem->give();
}
bool AP_InertialSensor_L3G4200D::sample_available(void)
{
if (!_sample_available) {
return false;
}
return ((hal.scheduler->micros() - _last_sample_time) >= _sample_period_usec);
return (_gyro_samples_available >= _gyro_samples_needed);
}
bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms)
{
if (sample_available()) {
_last_sample_time = hal.scheduler->micros();
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay_microseconds(100);
_accumulate();
if (sample_available()) {
_last_sample_time = hal.scheduler->micros();
return true;
}
}

3
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -32,7 +32,8 @@ private:
Vector3f _gyro_filtered;
uint32_t _sample_period_usec;
uint32_t _last_sample_time;
volatile bool _sample_available;
volatile uint32_t _gyro_samples_available;
volatile uint8_t _gyro_samples_needed;
// support for updating filter at runtime
uint8_t _last_filter_hz;