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

AP_InertialSensor: generalise the accel/gyro calibration for N sensors

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-09 09:50:12 +11:00
parent db400ffa51
commit a0688a69d4
11 changed files with 309 additions and 272 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

302
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -38,7 +38,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
// @Description: Accelerometer scaling of Z axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
AP_GROUPINFO("ACCSCAL", 1, AP_InertialSensor, _accel_scale, 0),
AP_GROUPINFO("ACCSCAL", 1, AP_InertialSensor, _accel_scale[0], 0),
// @Param: ACCOFFS_X
// @DisplayName: Accelerometer offsets of X axis
@ -60,7 +60,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
// @Units: m/s/s
// @Range: -300 300
// @User: Advanced
AP_GROUPINFO("ACCOFFS", 2, AP_InertialSensor, _accel_offset, 0),
AP_GROUPINFO("ACCOFFS", 2, AP_InertialSensor, _accel_offset[0], 0),
// @Param: GYROFFS_X
// @DisplayName: Gyro offsets of X axis
@ -79,7 +79,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
// @Description: Gyro sensor offsets of Z axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_GROUPINFO("GYROFFS", 3, AP_InertialSensor, _gyro_offset, 0),
AP_GROUPINFO("GYROFFS", 3, AP_InertialSensor, _gyro_offset[0], 0),
// @Param: MPU6K_FILTER
// @DisplayName: MPU6000 filter frequency
@ -89,6 +89,12 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("MPU6K_FILTER", 4, AP_InertialSensor, _mpu6000_filter, 0),
#if INS_MAX_INSTANCES > 1
AP_GROUPINFO("ACC2SCAL", 5, AP_InertialSensor, _accel_scale[1], 0),
AP_GROUPINFO("ACC2OFFS", 6, AP_InertialSensor, _accel_offset[1], 0),
AP_GROUPINFO("GYR2OFFS", 7, AP_InertialSensor, _gyro_offset[1], 0),
#endif
AP_GROUPEND
};
@ -106,10 +112,10 @@ AP_InertialSensor::init( Start_style style,
_product_id = _init_sensor(sample_rate);
// check scaling
Vector3f accel_scale = _accel_scale.get();
if( accel_scale.x == 0 && accel_scale.y == 0 && accel_scale.z == 0 ) {
accel_scale.x = accel_scale.y = accel_scale.z = 1.0;
_accel_scale.set(accel_scale);
for (uint8_t i=0; i<get_accel_count(); i++) {
if (_accel_scale[i].get().is_zero()) {
_accel_scale[i].set(Vector3f(1,1,1));
}
}
if (WARM_START != style) {
@ -122,9 +128,11 @@ AP_InertialSensor::init( Start_style style,
void AP_InertialSensor::_save_parameters()
{
_product_id.save();
_accel_scale.save();
_accel_offset.save();
_gyro_offset.save();
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_scale[i].save();
_accel_offset[i].save();
_gyro_offset[i].save();
}
}
void
@ -139,9 +147,10 @@ AP_InertialSensor::init_gyro()
void
AP_InertialSensor::_init_gyro()
{
Vector3f last_average, best_avg;
Vector3f ins_gyro;
float best_diff = 0;
uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES);
Vector3f last_average[num_gyros], best_avg[num_gyros];
float best_diff[num_gyros];
bool converged[num_gyros];
// cold start
hal.scheduler->delay(100);
@ -151,68 +160,90 @@ AP_InertialSensor::_init_gyro()
AP_Notify::flags.initialising = true;
// remove existing gyro offsets
_gyro_offset = Vector3f(0,0,0);
for (uint8_t k=0; k<num_gyros; k++) {
_gyro_offset[k] = Vector3f(0,0,0);
best_diff[k] = 0;
converged[k] = false;
}
for(int8_t c = 0; c < 25; c++) {
hal.scheduler->delay(20);
update();
ins_gyro = get_gyro();
}
// the strategy is to average 200 points over 1 second, then do it
// again and see if the 2nd average is within a small margin of
// the first
last_average.zero();
for (uint8_t k=0; k<num_gyros; k++) {
last_average[k].zero();
}
uint8_t num_converged = 0;
// we try to get a good calibration estimate for up to 10 seconds
// if the gyros are stable, we should get it in 2 seconds
for (int16_t j = 0; j <= 10; j++) {
Vector3f gyro_sum, gyro_avg, gyro_diff;
float diff_norm;
for (int16_t j = 0; j <= 10 && num_converged < num_gyros; j++) {
Vector3f gyro_sum[num_gyros], gyro_avg[num_gyros], gyro_diff[num_gyros];
float diff_norm[num_gyros];
uint8_t i;
hal.console->printf_P(PSTR("*"));
gyro_sum.zero();
for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k].zero();
}
for (i=0; i<200; i++) {
update();
ins_gyro = get_gyro();
gyro_sum += ins_gyro;
for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k] += get_gyro(k);
}
hal.scheduler->delay(5);
}
gyro_avg = gyro_sum / i;
gyro_diff = last_average - gyro_avg;
diff_norm = gyro_diff.length();
if (j == 0) {
best_diff = diff_norm;
best_avg = gyro_avg;
} else if (gyro_diff.length() < ToRad(0.04)) {
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average = (gyro_avg * 0.5) + (last_average * 0.5);
_gyro_offset = last_average;
// stop flashing leds
AP_Notify::flags.initialising = false;
// all done
return;
} else if (diff_norm < best_diff) {
best_diff = diff_norm;
best_avg = (gyro_avg * 0.5) + (last_average * 0.5);
for (uint8_t k=0; k<num_gyros; k++) {
gyro_avg[k] = gyro_sum[k] / i;
gyro_diff[k] = last_average[k] - gyro_avg[k];
diff_norm[k] = gyro_diff[k].length();
}
for (uint8_t k=0; k<num_gyros; k++) {
if (converged[k]) continue;
if (j == 0) {
best_diff[k] = diff_norm[k];
best_avg[k] = gyro_avg[k];
} else if (gyro_diff[k].length() < ToRad(0.04f)) {
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
_gyro_offset[k] = last_average[k];
converged[k] = true;
num_converged++;
} else if (diff_norm[k] < best_diff[k]) {
best_diff[k] = diff_norm[k];
best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
}
last_average[k] = gyro_avg[k];
}
last_average = gyro_avg;
}
// stop flashing leds
AP_Notify::flags.initialising = false;
if (num_converged == num_gyros) {
// all OK
return;
}
// we've kept the user waiting long enough - use the best pair we
// found so far
hal.console->printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
_gyro_offset = best_avg;
hal.console->println();
for (uint8_t k=0; k<num_gyros; k++) {
if (!converged[k]) {
hal.console->printf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"),
(unsigned)k, ToDeg(best_diff[k]));
_gyro_offset[k] = best_avg[k];
}
}
}
@ -228,12 +259,12 @@ AP_InertialSensor::init_accel()
void
AP_InertialSensor::_init_accel()
{
int8_t flashcount = 0;
Vector3f ins_accel;
Vector3f prev;
Vector3f accel_offset;
float total_change;
float max_offset;
uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
uint8_t flashcount = 0;
Vector3f prev[num_accels];
Vector3f accel_offset[num_accels];
float total_change[num_accels];
float max_offset[num_accels];
// cold start
hal.scheduler->delay(100);
@ -244,34 +275,38 @@ AP_InertialSensor::_init_accel()
AP_Notify::flags.initialising = true;
// clear accelerometer offsets and scaling
_accel_offset = Vector3f(0,0,0);
_accel_scale = Vector3f(1,1,1);
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k] = Vector3f(0,0,0);
_accel_scale[k] = Vector3f(1,1,1);
// initialise accel offsets to a large value the first time
// this will force us to calibrate accels at least twice
accel_offset = Vector3f(500, 500, 500);
// initialise accel offsets to a large value the first time
// this will force us to calibrate accels at least twice
accel_offset[k] = Vector3f(500, 500, 500);
}
// loop until we calculate acceptable offsets
do {
while (true) {
// get latest accelerometer values
update();
ins_accel = get_accel();
// store old offsets
prev = accel_offset;
for (uint8_t k=0; k<num_accels; k++) {
// store old offsets
prev[k] = accel_offset[k];
// get new offsets
accel_offset = ins_accel;
// get new offsets
accel_offset[k] = get_accel(k);
}
// We take some readings...
for(int8_t i = 0; i < 50; i++) {
hal.scheduler->delay(20);
update();
ins_accel = get_accel();
// low pass filter the offsets
accel_offset = accel_offset * 0.9 + ins_accel * 0.1;
for (uint8_t k=0; k<num_accels; k++) {
accel_offset[k] = accel_offset[k] * 0.9f + get_accel(k) * 0.1f;
}
// display some output to the user
if(flashcount >= 10) {
@ -281,18 +316,35 @@ AP_InertialSensor::_init_accel()
flashcount++;
}
// null gravity from the Z accel
accel_offset.z += GRAVITY_MSS;
for (uint8_t k=0; k<num_accels; k++) {
// null gravity from the Z accel
accel_offset[k].z += GRAVITY_MSS;
total_change = fabsf(prev.x - accel_offset.x) + fabsf(prev.y - accel_offset.y) + fabsf(prev.z - accel_offset.z);
max_offset = (accel_offset.x > accel_offset.y) ? accel_offset.x : accel_offset.y;
max_offset = (max_offset > accel_offset.z) ? max_offset : accel_offset.z;
total_change[k] =
fabsf(prev[k].x - accel_offset[k].x) +
fabsf(prev[k].y - accel_offset[k].y) +
fabsf(prev[k].z - accel_offset[k].z);
max_offset[k] = (accel_offset[k].x > accel_offset[k].y) ? accel_offset[k].x : accel_offset[k].y;
max_offset[k] = (max_offset[k] > accel_offset[k].z) ? max_offset[k] : accel_offset[k].z;
}
uint8_t num_converged = 0;
for (uint8_t k=0; k<num_accels; k++) {
if (total_change[k] <= AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE &&
max_offset[k] <= AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET) {
num_converged++;
}
}
if (num_converged == num_accels) break;
hal.scheduler->delay(500);
} while ( total_change > AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE || max_offset > AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET);
}
// set the global accel offsets
_accel_offset = accel_offset;
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k] = accel_offset[k];
}
// stop flashing the leds
AP_Notify::flags.initialising = false;
@ -312,22 +364,26 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
float &trim_roll,
float &trim_pitch)
{
Vector3f samples[6];
Vector3f new_offsets;
Vector3f new_scaling;
Vector3f orig_offset;
Vector3f orig_scale;
uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
Vector3f samples[num_accels][6];
Vector3f new_offsets[num_accels];
Vector3f new_scaling[num_accels];
Vector3f orig_offset[num_accels];
Vector3f orig_scale[num_accels];
uint8_t num_ok = 0;
// backup original offsets and scaling
orig_offset = _accel_offset.get();
orig_scale = _accel_scale.get();
for (uint8_t k=0; k<num_accels; k++) {
// backup original offsets and scaling
orig_offset[k] = _accel_offset[k].get();
orig_scale[k] = _accel_scale[k].get();
// clear accelerometer offsets and scaling
_accel_offset = Vector3f(0,0,0);
_accel_scale = Vector3f(1,1,1);
// clear accelerometer offsets and scaling
_accel_offset[k] = Vector3f(0,0,0);
_accel_scale[k] = Vector3f(1,1,1);
}
// capture data from 6 positions
for (int8_t i=0; i<6; i++) {
for (uint8_t i=0; i<6; i++) {
const prog_char_t *msg;
// display message to user
@ -362,49 +418,65 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
update();
// average 32 samples
samples[i] = Vector3f();
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] = Vector3f();
}
uint8_t num_samples = 0;
while (num_samples < 32) {
if (!wait_for_sample(1000)) {
interact->printf_P(PSTR("Failed to get INS sample\n"));
return false;
goto failed;
}
// read samples from ins
update();
// capture sample
samples[i] += get_accel();
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] += get_accel(k);
}
hal.scheduler->delay(10);
num_samples++;
}
samples[i] /= num_samples;
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] /= num_samples;
}
}
// run the calibration routine
bool success = _calibrate_accel(samples, new_offsets, new_scaling);
for (uint8_t k=0; k<num_accels; k++) {
bool success = _calibrate_accel(samples[k], new_offsets[k], new_scaling[k]);
interact->printf_P(PSTR("Offsets: %.2f %.2f %.2f\n"),
new_offsets.x, new_offsets.y, new_offsets.z);
interact->printf_P(PSTR("Scaling: %.2f %.2f %.2f\n"),
new_scaling.x, new_scaling.y, new_scaling.z);
interact->printf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
new_offsets[k].x, new_offsets[k].y, new_offsets[k].z);
interact->printf_P(PSTR("Scaling[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
new_scaling[k].x, new_scaling[k].y, new_scaling[k].z);
if (success) num_ok++;
}
if (success) {
if (num_ok == num_accels) {
interact->printf_P(PSTR("Calibration successful\n"));
// set and save calibration
_accel_offset.set(new_offsets);
_accel_scale.set(new_scaling);
for (uint8_t k=0; k<num_accels; k++) {
// set and save calibration
_accel_offset[k].set(new_offsets[k]);
_accel_scale[k].set(new_scaling[k]);
}
_save_parameters();
// calculate the trims as well and pass back to caller
_calculate_trim(samples[0], trim_roll, trim_pitch);
// calculate the trims as well from primary accels and pass back to caller
_calculate_trim(samples[0][0], trim_roll, trim_pitch);
return true;
}
failed:
interact->printf_P(PSTR("Calibration FAILED\n"));
// restore original scaling and offsets
_accel_offset.set(orig_offset);
_accel_scale.set(orig_scale);
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k].set(orig_offset[k]);
_accel_scale[k].set(orig_scale[k]);
}
return false;
}
@ -412,7 +484,7 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
/// @note this should not be called while flying because it reads from the eeprom which can be slow
bool AP_InertialSensor::calibrated()
{
return _accel_offset.load();
return _accel_offset[0].load();
}
// _calibrate_model - perform low level accel calibration
@ -421,7 +493,7 @@ bool AP_InertialSensor::calibrated()
// accel_scale are output from the calibration routine
// returns true if successful
bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6],
Vector3f& accel_offsets, Vector3f& accel_scale )
Vector3f& accel_offsets, Vector3f& accel_scale )
{
int16_t i;
int16_t num_iterations = 0;
@ -565,8 +637,8 @@ void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float
void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch)
{
// scale sample and apply offsets
Vector3f accel_scale = _accel_scale.get();
Vector3f accel_offsets = _accel_offset.get();
Vector3f accel_scale = _accel_scale[0].get();
Vector3f accel_offsets = _accel_offset[0].get();
Vector3f scaled_accels_x( accel_sample.x * accel_scale.x - accel_offsets.x,
0,
accel_sample.z * accel_scale.z - accel_offsets.z );
@ -591,7 +663,7 @@ void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll,
/**
default versions of multi-device accessor functions
*/
bool AP_InertialSensor::get_gyro_instance_health(uint8_t instance) const
bool AP_InertialSensor::get_gyro_health(uint8_t instance) const
{
if (instance != 0) {
return false;
@ -599,16 +671,7 @@ bool AP_InertialSensor::get_gyro_instance_health(uint8_t instance) const
return healthy();
}
bool AP_InertialSensor::get_gyro_instance(uint8_t instance, Vector3f &gyro) const
{
if (instance != 0) {
return false;
}
gyro = get_gyro();
return true;
}
bool AP_InertialSensor::get_accel_instance_health(uint8_t instance) const
bool AP_InertialSensor::get_accel_health(uint8_t instance) const
{
if (instance != 0) {
return false;
@ -616,14 +679,5 @@ bool AP_InertialSensor::get_accel_instance_health(uint8_t instance) const
return healthy();
}
bool AP_InertialSensor::get_accel_instance(uint8_t instance, Vector3f &accel) const
{
if (instance != 0) {
return false;
}
accel = get_accel();
return true;
}
#endif // __AVR_ATmega1280__

45
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -7,6 +7,16 @@
#define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0f
#define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0f
/**
maximum number of INS instances available on this platform. If more
than 1 then redundent sensors may be available
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#define INS_MAX_INSTANCES 2
#else
#define INS_MAX_INSTANCES 1
#endif
#include <stdint.h>
#include <AP_HAL.h>
#include <AP_Math.h>
@ -83,35 +93,36 @@ public:
///
/// @returns vector of rotational rates in radians/sec
///
const Vector3f &get_gyro(void) const { return _gyro; }
const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; }
const Vector3f &get_gyro(void) const { return get_gyro(0); }
virtual void set_gyro(const Vector3f &gyro) {}
// set gyro offsets in radians/sec
Vector3f get_gyro_offsets(void) { return _gyro_offset; }
void set_gyro_offsets(Vector3f offsets) { _gyro_offset.set(offsets); }
const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(0); }
/// Fetch the current accelerometer values
///
/// @returns vector of current accelerations in m/s/s
///
const Vector3f &get_accel(void) const { return _accel; }
const Vector3f &get_accel(uint8_t i) const { return _accel[i]; }
const Vector3f &get_accel(void) const { return get_accel(0); }
virtual void set_accel(const Vector3f &accel) {}
// multi-device interface
virtual bool get_gyro_instance_health(uint8_t instance) const;
virtual bool get_gyro_health(uint8_t instance) const;
virtual uint8_t get_gyro_count(void) const { return 1; };
virtual bool get_gyro_instance(uint8_t instance, Vector3f &gyro) const;
virtual bool get_accel_instance_health(uint8_t instance) const;
virtual bool get_accel_health(uint8_t instance) const;
virtual uint8_t get_accel_count(void) const { return 1; };
virtual bool get_accel_instance(uint8_t instance, Vector3f &accel) const;
// get accel offsets in m/s/s
Vector3f get_accel_offsets() { return _accel_offset; }
void set_accel_offsets(Vector3f offsets) { _accel_offset.set(offsets); }
const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(0); }
// get accel scale
Vector3f get_accel_scale() { return _accel_scale; }
const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; }
const Vector3f &get_accel_scale(void) const { return get_accel_scale(0); }
/* Update the sensor data, so that getters are nonblocking.
* Returns a bool of whether data was updated or not.
@ -175,21 +186,21 @@ protected:
void _save_parameters();
// Most recent accelerometer reading obtained by ::update
Vector3f _accel;
Vector3f _accel[INS_MAX_INSTANCES];
// previous accelerometer reading obtained by ::update
Vector3f _previous_accel;
Vector3f _previous_accel[INS_MAX_INSTANCES];
// Most recent gyro reading obtained by ::update
Vector3f _gyro;
Vector3f _gyro[INS_MAX_INSTANCES];
// product id
AP_Int16 _product_id;
// accelerometer scaling and offsets
AP_Vector3f _accel_scale;
AP_Vector3f _accel_offset;
AP_Vector3f _gyro_offset;
AP_Vector3f _accel_scale[INS_MAX_INSTANCES];
AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
AP_Vector3f _gyro_offset[INS_MAX_INSTANCES];
// filtering frequency (0 means default)
AP_Int8 _mpu6000_filter;

26
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -178,7 +178,7 @@ bool AP_InertialSensor_Flymaple::update(void)
if (!wait_for_sample(100)) {
return false;
}
Vector3f accel_scale = _accel_scale.get();
Vector3f accel_scale = _accel_scale[0].get();
// Not really needed since Flymaple _accumulate runs in the main thread
hal.scheduler->suspend_timer_procs();
@ -188,33 +188,33 @@ bool AP_InertialSensor_Flymaple::update(void)
_delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
_last_update_usec = _last_gyro_timestamp;
_previous_accel = _accel;
_previous_accel[0] = _accel[0];
_accel = _accel_filtered;
_accel[0] = _accel_filtered;
_accel_samples = 0;
_gyro = _gyro_filtered;
_gyro[0] = _gyro_filtered;
_gyro_samples = 0;
hal.scheduler->resume_timer_procs();
// add offsets and rotation
_accel.rotate(_board_orientation);
_accel[0].rotate(_board_orientation);
// Adjust for chip scaling to get m/s/s
_accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
_accel[0] *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
// Now the calibration scale factor
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
_gyro.rotate(_board_orientation);
_gyro[0].rotate(_board_orientation);
// Adjust for chip scaling to get radians/sec
_gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_gyro -= _gyro_offset;
_gyro[0] *= FLYMAPLE_GYRO_SCALE_R_S;
_gyro[0] -= _gyro_offset[0];
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);

12
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -5,7 +5,7 @@
const extern AP_HAL::HAL& hal;
AP_InertialSensor_HIL::AP_InertialSensor_HIL() : AP_InertialSensor() {
_accel.z = -GRAVITY_MSS;
_accel[0] = Vector3f(0, 0, -GRAVITY_MSS);
}
uint16_t AP_InertialSensor_HIL::_init_sensor( Sample_rate sample_rate ) {
@ -66,14 +66,14 @@ bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
void AP_InertialSensor_HIL::set_accel(const Vector3f &accel)
{
_previous_accel = _accel;
_accel = accel;
_previous_accel[0] = _accel[0];
_accel[0] = accel;
_last_accel_usec = hal.scheduler->micros();
}
void AP_InertialSensor_HIL::set_gyro(const Vector3f &gyro)
{
_gyro = gyro;
_gyro[0] = gyro;
_last_gyro_usec = hal.scheduler->micros();
}
@ -91,8 +91,8 @@ bool AP_InertialSensor_HIL::healthy(void) const
// gyros have not updated
return false;
}
if (fabs(_accel.x) > 30 && fabs(_accel.y) > 30 && fabs(_accel.z) > 30 &&
(_previous_accel - _accel).length() < 0.01f) {
if (fabs(_accel[0].x) > 30 && fabs(_accel[0].y) > 30 && fabs(_accel[0].z) > 30 &&
(_previous_accel[0] - _accel[0]).length() < 0.01f) {
// unchanging accel, large in all 3 axes. This is a likely
// accelerometer failure
return false;

26
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -234,33 +234,33 @@ bool AP_InertialSensor_L3G4200D::update(void)
if (!wait_for_sample(1000)) {
return false;
}
Vector3f accel_scale = _accel_scale.get();
Vector3f accel_scale = _accel_scale[0].get();
_previous_accel = _accel;
_previous_accel[0] = _accel[0];
hal.scheduler->suspend_timer_procs();
_accel = _accel_filtered;
_gyro = _gyro_filtered;
_accel[0] = _accel_filtered;
_gyro[0] = _gyro_filtered;
_gyro_samples_available = 0;
hal.scheduler->resume_timer_procs();
// add offsets and rotation
_accel.rotate(_board_orientation);
_accel[0].rotate(_board_orientation);
// Adjust for chip scaling to get m/s/s
_accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
_accel[0] *= ADXL345_ACCELEROMETER_SCALE_M_S;
// Now the calibration scale factor
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
_gyro.rotate(_board_orientation);
_gyro[0].rotate(_board_orientation);
// Adjust for chip scaling to get radians/sec
_gyro *= L3G4200D_GYRO_SCALE_R_S;
_gyro -= _gyro_offset;
_gyro[0] *= L3G4200D_GYRO_SCALE_R_S;
_gyro[0] -= _gyro_offset[0];
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);

26
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -255,30 +255,30 @@ bool AP_InertialSensor_MPU6000::update( void )
return false;
}
_previous_accel = _accel;
_previous_accel[0] = _accel[0];
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs();
_gyro = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
_accel = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z);
_gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
_accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z);
_num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
_sum_count = 0;
hal.scheduler->resume_timer_procs();
_gyro.rotate(_board_orientation);
_gyro *= _gyro_scale / _num_samples;
_gyro -= _gyro_offset;
_gyro[0].rotate(_board_orientation);
_gyro[0] *= _gyro_scale / _num_samples;
_gyro[0] -= _gyro_offset[0];
_accel.rotate(_board_orientation);
_accel *= MPU6000_ACCEL_SCALE_1G / _num_samples;
_accel[0].rotate(_board_orientation);
_accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples;
Vector3f accel_scale = _accel_scale.get();
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
Vector3f accel_scale = _accel_scale[0].get();
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
if (_last_filter_hz != _mpu6000_filter) {
if (_spi_sem->take(10)) {

43
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -72,34 +72,33 @@ uint16_t AP_InertialSensor_Oilpan::_init_sensor( Sample_rate sample_rate)
bool AP_InertialSensor_Oilpan::update()
{
float adc_values[6];
Vector3f gyro_offset = _gyro_offset.get();
Vector3f accel_scale = _accel_scale.get();
Vector3f accel_offset = _accel_offset.get();
Vector3f gyro_offset = _gyro_offset[0].get();
Vector3f accel_scale = _accel_scale[0].get();
Vector3f accel_offset = _accel_offset[0].get();
_delta_time_micros = _adc->Ch6(_sensors, adc_values);
_temp = _adc->Ch(_gyro_temp_ch);
_gyro = Vector3f(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ),
_sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ),
_sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ));
_gyro.rotate(_board_orientation);
_gyro.x *= _gyro_gain_x;
_gyro.y *= _gyro_gain_y;
_gyro.z *= _gyro_gain_z;
_gyro -= gyro_offset;
_gyro[0] = Vector3f(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ),
_sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ),
_sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ));
_gyro[0].rotate(_board_orientation);
_gyro[0].x *= _gyro_gain_x;
_gyro[0].y *= _gyro_gain_y;
_gyro[0].z *= _gyro_gain_z;
_gyro[0] -= gyro_offset;
_previous_accel = _accel;
_previous_accel[0] = _accel[0];
_accel = Vector3f(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
_accel.rotate(_board_orientation);
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel *= OILPAN_ACCEL_SCALE_1G;
_accel -= accel_offset;
_accel[0] = Vector3f(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
_accel[0].rotate(_board_orientation);
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] *= OILPAN_ACCEL_SCALE_1G;
_accel[0] -= accel_offset;
/*
* X = 1619.30 to 2445.69

72
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -29,12 +29,8 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
if (_gyro_fd[0] < 0) {
hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
}
if (_accel_fd[1] >= 0) {
_num_accel_instances = 2;
}
if (_gyro_fd[1] >= 0) {
_num_gyro_instances = 2;
}
_num_accel_instances = _accel_fd[1] >= 0?2:1;
_num_gyro_instances = _gyro_fd[1] >= 0?2:1;
switch (sample_rate) {
case RATE_50HZ:
@ -69,7 +65,7 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
if (filter_hz == 0) {
filter_hz = _default_filter_hz;
}
for (uint8_t i=0; i<PX4_MAX_INS_INSTANCES; i++) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
ioctl(_gyro_fd[i], GYROIOCSLOWPASS, filter_hz);
ioctl(_accel_fd[i], ACCELIOCSLOWPASS, filter_hz);
}
@ -78,7 +74,7 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
// multi-device interface
bool AP_InertialSensor_PX4::get_gyro_instance_health(uint8_t instance) const
bool AP_InertialSensor_PX4::get_gyro_health(uint8_t instance) const
{
if (instance >= _num_gyro_instances) {
return false;
@ -102,20 +98,9 @@ uint8_t AP_InertialSensor_PX4::get_gyro_count(void) const
return _num_gyro_instances;
}
bool AP_InertialSensor_PX4::get_gyro_instance(uint8_t instance, Vector3f &gyro) const
bool AP_InertialSensor_PX4::get_accel_health(uint8_t k) const
{
if (instance >= _num_gyro_instances) {
return false;
}
gyro = _gyro_in[instance];
gyro.rotate(_board_orientation);
gyro -= _gyro_offset;
return true;
}
bool AP_InertialSensor_PX4::get_accel_instance_health(uint8_t instance) const
{
if (instance >= _num_accel_instances) {
if (k >= _num_accel_instances) {
return false;
}
if (_sample_time_usec == 0) {
@ -125,12 +110,12 @@ bool AP_InertialSensor_PX4::get_accel_instance_health(uint8_t instance) const
}
uint64_t tnow = hrt_absolute_time();
if ((tnow - _last_accel_timestamp[instance]) > 2*_sample_time_usec) {
if ((tnow - _last_accel_timestamp[k]) > 2*_sample_time_usec) {
// accels have not updated
return false;
}
if (fabsf(_accel.x) > 30 && fabsf(_accel.y) > 30 && fabsf(_accel.z) > 30 &&
(_previous_accels[instance] - _accel_in[instance]).length() < 0.01f) {
if (fabsf(_accel[k].x) > 30 && fabsf(_accel[k].y) > 30 && fabsf(_accel[k].z) > 30 &&
(_previous_accel[k] - _accel[k]).length() < 0.01f) {
// unchanging accel, large in all 3 axes. This is a likely
// accelerometer failure of the LSM303d
return false;
@ -143,29 +128,27 @@ uint8_t AP_InertialSensor_PX4::get_accel_count(void) const
return _num_accel_instances;
}
bool AP_InertialSensor_PX4::get_accel_instance(uint8_t instance, Vector3f &accel) const
{
if (instance >= _num_accel_instances) {
return false;
}
accel = _accel_in[instance];
accel.rotate(_board_orientation);
accel.x *= _accel_scale.get().x;
accel.y *= _accel_scale.get().y;
accel.z *= _accel_scale.get().z;
accel -= _accel_offset;
return true;
}
bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
_get_sample();
_previous_accel = _accel;
get_accel_instance(0, _accel);
get_gyro_instance(0, _gyro);
for (uint8_t k=0; k<_num_accel_instances; k++) {
_previous_accel[k] = _accel[k];
_accel[k] = _accel_in[k];
_accel[k].rotate(_board_orientation);
_accel[k].x *= _accel_scale[k].get().x;
_accel[k].y *= _accel_scale[k].get().y;
_accel[k].z *= _accel_scale[k].get().z;
_accel[k] -= _accel_offset[k];
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
_gyro[k] = _gyro_in[k];
_gyro[k].rotate(_board_orientation);
_gyro[k] -= _gyro_offset[k];
}
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
@ -190,17 +173,16 @@ float AP_InertialSensor_PX4::get_gyro_drift_rate(void)
void AP_InertialSensor_PX4::_get_sample(void)
{
for (uint8_t i=0; i<PX4_MAX_INS_INSTANCES; i++) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
struct accel_report accel_report;
while (_accel_fd[i] != -1 &&
::read(_accel_fd[i], &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
accel_report.timestamp != _last_accel_timestamp[i]) {
_previous_accels[i] = _accel_in[i];
_accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z);
_last_accel_timestamp[i] = accel_report.timestamp;
}
}
for (uint8_t i=0; i<PX4_MAX_INS_INSTANCES; i++) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
struct gyro_report gyro_report;
while (_gyro_fd[i] != -1 &&
::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
@ -247,7 +229,7 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
*/
bool AP_InertialSensor_PX4::healthy(void) const
{
return get_gyro_instance_health(0) && get_accel_instance_health(0);
return get_gyro_health(0) && get_accel_health(0);
}
#endif // CONFIG_HAL_BOARD

25
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -13,8 +13,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#define PX4_MAX_INS_INSTANCES 2
class AP_InertialSensor_PX4 : public AP_InertialSensor
{
public:
@ -33,22 +31,20 @@ public:
bool healthy(void) const;
// multi-device interface
bool get_gyro_instance_health(uint8_t instance) const;
bool get_gyro_health(uint8_t instance) const;
uint8_t get_gyro_count(void) const;
bool get_gyro_instance(uint8_t instance, Vector3f &gyro) const;
bool get_accel_instance_health(uint8_t instance) const;
bool get_accel_health(uint8_t instance) const;
uint8_t get_accel_count(void) const;
bool get_accel_instance(uint8_t instance, Vector3f &accel) const;
private:
uint16_t _init_sensor( Sample_rate sample_rate );
void _get_sample(void);
bool _sample_available(void);
Vector3f _accel_in[PX4_MAX_INS_INSTANCES];
Vector3f _gyro_in[PX4_MAX_INS_INSTANCES];
uint64_t _last_accel_timestamp[PX4_MAX_INS_INSTANCES];
uint64_t _last_gyro_timestamp[PX4_MAX_INS_INSTANCES];
Vector3f _accel_in[INS_MAX_INSTANCES];
Vector3f _gyro_in[INS_MAX_INSTANCES];
uint64_t _last_accel_timestamp[INS_MAX_INSTANCES];
uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES];
uint64_t _last_sample_timestamp;
uint32_t _sample_time_usec;
bool _have_sample_available;
@ -59,16 +55,11 @@ private:
void _set_filter_frequency(uint8_t filter_hz);
Vector3f _accel_data[PX4_MAX_INS_INSTANCES];
Vector3f _gyro_data[PX4_MAX_INS_INSTANCES];
Vector3f _previous_accels[PX4_MAX_INS_INSTANCES];
// accelerometer and gyro driver handles
uint8_t _num_accel_instances;
uint8_t _num_gyro_instances;
int _accel_fd[PX4_MAX_INS_INSTANCES];
int _gyro_fd[PX4_MAX_INS_INSTANCES];
int _accel_fd[INS_MAX_INSTANCES];
int _gyro_fd[INS_MAX_INSTANCES];
};
#endif
#endif // __AP_INERTIAL_SENSOR_PX4_H__

2
libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde
View File

@ -170,7 +170,7 @@ void run_test()
while( !hal.console->available() ) {
// wait until we have a sample
ins.wait_for_sample();
ins.wait_for_sample(1000);
// read samples from ins
ins.update();

2
libraries/AP_InertialSensor/examples/OilPan/OilPan.pde
View File

@ -174,7 +174,7 @@ void run_test()
while( !hal.console->available() ) {
// wait until we have a sample
ins.wait_for_sample();
ins.wait_for_sample(1000);
// read samples from ins
ins.update();