mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: MPU6000: remove dead code for !FAST_SAMPLING
The code with ifdef for !FAST_SAMPLING is bit rotting and the example
for AP_InertialSensor is currently broken for this case. Instead of
adding more ifdefs, remove the legacy implementation for !FAST_SAMPLING
since we don't support it anymore.
Reported by Grant:
/home/grant/3dr/ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp:
In member function 'void AP_InertialSensor_MPU6000::_accumulate(uint8_t*,
uint8_t)':
/home/grant/3dr/ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp:776:20:
error: no match for 'operator+=' (operand types are 'Vector3l {aka
Vector3<long int>}' and 'Vector3f {aka Vector3<float>}')
_accel_sum += accel;
/home/grant/3dr/ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp:777:19:
error: no match for 'operator+=' (operand types are 'Vector3l {aka
Vector3<long int>}' and 'Vector3f {aka Vector3<float>}')
_gyro_sum += gyro;
This commit is contained in:
Lucas De Marchi
Andrew Tridgell
parent
75add595a0
commit
4480956c68
|
|
@ -441,15 +441,9 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
|
|||
_bus_sem(NULL),
|
||||
_last_accel_filter_hz(-1),
|
||||
_last_gyro_filter_hz(-1),
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
_accel_filter(1000, 15),
|
||||
_gyro_filter(1000, 15),
|
||||
_temp_filter(1000, 1),
|
||||
#else
|
||||
_sample_count(0),
|
||||
_accel_sum(),
|
||||
_gyro_sum(),
|
||||
#endif
|
||||
_sum_count(0),
|
||||
_samples(NULL)
|
||||
{
|
||||
|
|
@ -548,31 +542,8 @@ void AP_InertialSensor_MPU6000::start()
|
|||
_samples = new uint8_t[max_samples * MPU6000_SAMPLE_SIZE];
|
||||
hal.scheduler->delay(1);
|
||||
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
_sample_count = 1;
|
||||
#else
|
||||
// sample rate and filtering
|
||||
// to minimise the effects of aliasing we choose a filter
|
||||
// that is less than half of the sample rate
|
||||
switch (_imu.get_sample_rate()) {
|
||||
case AP_InertialSensor::RATE_50HZ:
|
||||
// this is used for plane and rover, where noise resistance is
|
||||
// more important than update rate. Tests on an aerobatic plane
|
||||
// show that 10Hz is fine, and makes it very noise resistant
|
||||
_sample_count = 4;
|
||||
break;
|
||||
case AP_InertialSensor::RATE_100HZ:
|
||||
_sample_count = 2;
|
||||
break;
|
||||
case AP_InertialSensor::RATE_200HZ:
|
||||
_sample_count = 1;
|
||||
break;
|
||||
default:
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
// disable sensor filtering
|
||||
_set_filter_register(256);
|
||||
|
||||
|
|
@ -582,13 +553,7 @@ void AP_InertialSensor_MPU6000::start()
|
|||
// So we have to set it to 7 to have a 1kHz sampling
|
||||
// rate on the gyro
|
||||
_register_write(MPUREG_SMPLRT_DIV, 7);
|
||||
#else
|
||||
_set_filter_register(_accel_filter_cutoff());
|
||||
|
||||
// set sample rate to 200Hz, and use _sample_divider to give
|
||||
// the requested rate to the application
|
||||
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);
|
||||
#endif
|
||||
hal.scheduler->delay(1);
|
||||
|
||||
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
|
||||
|
|
@ -642,33 +607,16 @@ void AP_InertialSensor_MPU6000::start()
|
|||
*/
|
||||
bool AP_InertialSensor_MPU6000::update( void )
|
||||
{
|
||||
#if !MPU6000_FAST_SAMPLING
|
||||
if (_sum_count < _sample_count) {
|
||||
// we don't have enough samples yet
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
// we have a full set of samples
|
||||
uint16_t num_samples;
|
||||
Vector3f accel, gyro;
|
||||
float temp;
|
||||
|
||||
hal.scheduler->suspend_timer_procs();
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
gyro = _gyro_filtered;
|
||||
accel = _accel_filtered;
|
||||
temp = _temp_filtered;
|
||||
num_samples = 1;
|
||||
#else
|
||||
gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
|
||||
accel(_accel_sum.x, _accel_sum.y, _accel_sum.z);
|
||||
temp = _temp_sum;
|
||||
num_samples = _sum_count;
|
||||
_accel_sum.zero();
|
||||
_gyro_sum.zero();
|
||||
_temp_sum = 0;
|
||||
#endif
|
||||
_sum_count = 0;
|
||||
hal.scheduler->resume_timer_procs();
|
||||
|
||||
|
|
@ -682,7 +630,6 @@ bool AP_InertialSensor_MPU6000::update( void )
|
|||
/* give the temperature to the control loop in order to keep it constant*/
|
||||
hal.util->set_imu_temp(temp);
|
||||
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
|
||||
_accel_filter.set_cutoff_frequency(1000, _accel_filter_cutoff());
|
||||
_last_accel_filter_hz = _accel_filter_cutoff();
|
||||
|
|
@ -692,17 +639,6 @@ bool AP_InertialSensor_MPU6000::update( void )
|
|||
_gyro_filter.set_cutoff_frequency(1000, _gyro_filter_cutoff());
|
||||
_last_gyro_filter_hz = _gyro_filter_cutoff();
|
||||
}
|
||||
#else
|
||||
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
|
||||
if (_bus_sem->take(10)) {
|
||||
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
|
||||
_set_filter_register(_accel_filter_cutoff());
|
||||
_bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
|
||||
_bus_sem->give();
|
||||
_last_accel_filter_hz = _accel_filter_cutoff();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -783,25 +719,10 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
|
|||
|
||||
_notify_new_accel_raw_sample(_accel_instance, accel);
|
||||
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
_accel_filtered = _accel_filter.apply(accel);
|
||||
_gyro_filtered = _gyro_filter.apply(gyro);
|
||||
_temp_filtered = _temp_filter.apply(temp);
|
||||
#else
|
||||
_accel_sum += accel;
|
||||
_gyro_sum += gyro;
|
||||
_temp_sum += temp;
|
||||
#endif
|
||||
_sum_count++;
|
||||
|
||||
#if !MPU6000_FAST_SAMPLING
|
||||
if (_sum_count == 0) {
|
||||
// rollover - v unlikely
|
||||
_accel_sum.zero();
|
||||
_gyro_sum.zero();
|
||||
_temp_sum = 0;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -4,9 +4,13 @@
|
|||
#define __AP_INERTIAL_SENSOR_MPU6000_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Progmem/AP_Progmem.h>
|
||||
#include <Filter/Filter.h>
|
||||
#include <Filter/LowPassFilter2p.h>
|
||||
#include <Filter/LowPassFilter.h>
|
||||
|
||||
#include "AP_InertialSensor.h"
|
||||
#include "AuxiliaryBus.h"
|
||||
|
|
@ -14,19 +18,6 @@
|
|||
// enable debug to see a register dump on startup
|
||||
#define MPU6000_DEBUG 0
|
||||
|
||||
// on fast CPUs we sample at 1kHz and use a software filter
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
#define MPU6000_FAST_SAMPLING 1
|
||||
#else
|
||||
#define MPU6000_FAST_SAMPLING 0
|
||||
#endif
|
||||
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
#include <Filter/Filter.h>
|
||||
#include <Filter/LowPassFilter2p.h>
|
||||
#include <Filter/LowPassFilter.h>
|
||||
#endif
|
||||
|
||||
#define MPU6000_SAMPLE_SIZE 14
|
||||
#define MPU6000_MAX_FIFO_SAMPLES 3
|
||||
#define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE)
|
||||
|
|
@ -125,7 +116,6 @@ private:
|
|||
// how many hardware samples before we report a sample to the caller
|
||||
uint8_t _sample_count;
|
||||
|
||||
#if MPU6000_FAST_SAMPLING
|
||||
Vector3f _accel_filtered;
|
||||
Vector3f _gyro_filtered;
|
||||
float _temp_filtered;
|
||||
|
|
@ -134,14 +124,7 @@ private:
|
|||
LowPassFilter2pVector3f _accel_filter;
|
||||
LowPassFilter2pVector3f _gyro_filter;
|
||||
LowPassFilter2pFloat _temp_filter;
|
||||
#else
|
||||
// accumulation in timer - must be read with timer disabled
|
||||
// the sum of the values since last read
|
||||
Vector3l _accel_sum;
|
||||
Vector3l _gyro_sum;
|
||||
int32_t _temp_sum;
|
||||
|
||||
#endif
|
||||
volatile uint16_t _sum_count;
|
||||
bool _fifo_mode;
|
||||
uint8_t *_samples = nullptr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue