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

InertialSensor: add GYR_CAL to control when gyro calibration occurs

Browse Source
This commit is contained in:
Randy Mackay 2015-09-17 15:42:41 +09:00
parent 038ad31f81
commit ec82aa68c4
2 changed files with 21 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -303,6 +303,13 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
AP_GROUPINFO("STILL_THRESH", 23, AP_InertialSensor, _still_threshold, DEFAULT_STILL_THRESH),
#endif
// @Param: GYR_CAL
// @DisplayName: Gyro Calibration scheme
// @Description: Conrols when automatic gyro calibration is performed
// @Values: 0:Never, 1:Start-up only, 2:Start-up and first arming (Copter only)
// @User: Advanced
AP_GROUPINFO("GYR_CAL", 24, AP_InertialSensor, _gyro_cal_timing, 1),
/*
NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully
@ -339,6 +346,7 @@ AP_InertialSensor::AP_InertialSensor() :
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_error_count[i] = 0;
_gyro_error_count[i] = 0;
_gyro_cal_ok[i] = true;
#if INS_VIBRATION_CHECK
_accel_clip_count[i] = 0;
#endif
@ -424,8 +432,7 @@ AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id)
}
void
AP_InertialSensor::init( Start_style style,
Sample_rate sample_rate)
AP_InertialSensor::init(Sample_rate sample_rate)
{
// remember the sample rate
_sample_rate = sample_rate;
@ -442,8 +449,8 @@ AP_InertialSensor::init( Start_style style,
}
}
if (WARM_START != style) {
// do cold-start calibration for gyro only
// calibrate gyros unless gyro calibration has been disabled
if (gyro_calibration_timing() != GYRO_CAL_NEVER) {
_init_gyro();
}

23
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -56,11 +56,6 @@ public:
AP_InertialSensor();
static AP_InertialSensor *get_instance();
enum Start_style {
COLD_START = 0,
WARM_START
};
// the rate that updates will be available to the application
enum Sample_rate {
RATE_50HZ = 50,
@ -69,21 +64,21 @@ public:
RATE_400HZ = 400
};
enum Gyro_Calibration_Timing {
GYRO_CAL_NEVER = 0,
GYRO_CAL_STARTUP_ONLY = 1,
GYRO_CAL_STARTUP_AND_FIRST_BOOT = 2
};
/// Perform startup initialisation.
///
/// Called to initialise the state of the IMU.
///
/// For COLD_START, implementations using real sensors can assume
/// that the airframe is stationary and nominally oriented.
///
/// For WARM_START, no assumptions should be made about the
/// orientation or motion of the airframe. Calibration should be
/// as for the previous COLD_START call.
/// Gyros will be calibrated unless INS_GYRO_CAL is zero
///
/// @param style The initialisation startup style.
///
void init( Start_style style,
Sample_rate sample_rate);
void init(Sample_rate sample_rate);
/// Register a new gyro/accel driver, allocating an instance
/// number
@ -147,6 +142,7 @@ public:
bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
bool gyro_calibrated_ok_all() const;
bool use_gyro(uint8_t instance) const;
Gyro_Calibration_Timing gyro_calibration_timing() { return (Gyro_Calibration_Timing)_gyro_cal_timing.get(); }
bool get_accel_health(uint8_t instance) const { return (instance<_accel_count) ? _accel_healthy[instance] : false; }
bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
@ -317,6 +313,7 @@ private:
// filtering frequency (0 means default)
AP_Int8 _accel_filter_cutoff;
AP_Int8 _gyro_filter_cutoff;
AP_Int8 _gyro_cal_timing;
// use for attitude, velocity, position estimates
AP_Int8 _use[INS_MAX_INSTANCES];