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

MPU6000: use expected sample time, not actual sample time 

this avoids problems in the scheduler turning up as errors in the DCM
calculation. The MPU6k produces samples with more reliable timing than
our scheduler does!
This commit is contained in:
Andrew Tridgell 2013-01-07 19:39:37 +11:00
parent e0197a3f7c
commit 82b036282b
2 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -185,7 +185,6 @@ int16_t AP_InertialSensor_MPU6000::_mpu6000_product_id = AP_PRODUCT_ID_NONE;
// variables to calculate time period over which a group of samples were collected // variables to calculate time period over which a group of samples were collected
static volatile uint32_t _delta_time_micros = 1; // time period overwhich samples were collected (initialise to non-zero number but will be overwritten on 2nd read in any case) static volatile uint32_t _delta_time_micros = 1; // time period overwhich samples were collected (initialise to non-zero number but will be overwritten on 2nd read in any case)
static volatile uint32_t _delta_time_start_micros = 0; // time we start collecting sample (reset on update)
static volatile uint32_t _last_sample_time_micros = 0; // time latest sample was collected static volatile uint32_t _last_sample_time_micros = 0; // time latest sample was collected
// DMP related static variables // DMP related static variables
@ -253,8 +252,7 @@ bool AP_InertialSensor_MPU6000::update( void )
_count = 0; _count = 0;
// record sample time // record sample time
_delta_time_micros = _last_sample_time_micros - _delta_time_start_micros; _delta_time_micros = count * _micros_per_sample;
_delta_time_start_micros = _last_sample_time_micros;
SREG = oldSREG; SREG = oldSREG;
count_scale = 1.0 / count; count_scale = 1.0 / count;
@ -391,15 +389,18 @@ void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
case RATE_50HZ: case RATE_50HZ:
rate = MPUREG_SMPLRT_50HZ; rate = MPUREG_SMPLRT_50HZ;
default_filter = BITS_DLPF_CFG_20HZ; default_filter = BITS_DLPF_CFG_20HZ;
_micros_per_sample = 20000;
break; break;
case RATE_100HZ: case RATE_100HZ:
rate = MPUREG_SMPLRT_100HZ; rate = MPUREG_SMPLRT_100HZ;
default_filter = BITS_DLPF_CFG_42HZ; default_filter = BITS_DLPF_CFG_42HZ;
_micros_per_sample = 10000;
break; break;
case RATE_200HZ: case RATE_200HZ:
default: default:
rate = MPUREG_SMPLRT_200HZ; rate = MPUREG_SMPLRT_200HZ;
default_filter = BITS_DLPF_CFG_42HZ; default_filter = BITS_DLPF_CFG_42HZ;
_micros_per_sample = 5000;
break; break;
} }

1
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -75,6 +75,7 @@ private:
// ensure we can't initialise twice // ensure we can't initialise twice
bool _initialised; bool _initialised;
static int16_t _mpu6000_product_id; static int16_t _mpu6000_product_id;
uint32_t _micros_per_sample;
// dmp related methods and parameters // dmp related methods and parameters
static void dmp_register_write(uint8_t bank, uint8_t address, uint8_t num_bytes, uint8_t data[]); // Method to write multiple bytes into dmp registers. Requires a "bank" static void dmp_register_write(uint8_t bank, uint8_t address, uint8_t num_bytes, uint8_t data[]); // Method to write multiple bytes into dmp registers. Requires a "bank"