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

INS: add USE parameters

This commit is contained in:
Randy Mackay 2015-08-01 15:40:40 +09:00
parent affbd67c43
commit e5615ec349
2 changed files with 51 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -265,6 +265,30 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("ACCEL_FILTER", 19, AP_InertialSensor, _accel_filter_cutoff, DEFAULT_ACCEL_FILTER),
// @Param: USE
// @DisplayName: Use first IMU for attitude, velocity and position estimates
// @Description: Use first IMU for attitude, velocity and position estimates
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("USE", 20, AP_InertialSensor, _use[0], 1),
#if INS_MAX_INSTANCES > 2
// @Param: USE2
// @DisplayName: Use second IMU for attitude, velocity and position estimates
// @Description: Use second IMU for attitude, velocity and position estimates
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("USE2", 21, AP_InertialSensor, _use[1], 1),
#endif
#if INS_MAX_INSTANCES > 3
// @Param: USE3
// @DisplayName: Use third IMU for attitude, velocity and position estimates
// @Description: Use third IMU for attitude, velocity and position estimates
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("USE3", 21, AP_InertialSensor, _use[2], 0),
#endif
/*
NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully
@ -680,6 +704,16 @@ bool AP_InertialSensor::gyro_calibrated_ok_all() const
return (get_gyro_count() > 0);
}
// return true if gyro instance should be used (must be healthy and have it's use parameter set to 1)
bool AP_InertialSensor::use_gyro(uint8_t instance) const
{
if (instance >= INS_MAX_INSTANCES) {
return false;
}
return (get_gyro_health(instance) && _use[instance]);
}
// get_accel_health_all - return true if all accels are healthy
bool AP_InertialSensor::get_accel_health_all(void) const
{
@ -784,6 +818,16 @@ bool AP_InertialSensor::accel_calibrated_ok_all() const
return true;
}
// return true if accel instance should be used (must be healthy and have it's use parameter set to 1)
bool AP_InertialSensor::use_accel(uint8_t instance) const
{
if (instance >= INS_MAX_INSTANCES) {
return false;
}
return (get_accel_health(instance) && _use[instance]);
}
void
AP_InertialSensor::_init_gyro()
{
@ -1181,13 +1225,13 @@ void AP_InertialSensor::update(void)
// set primary to first healthy accel and gyro
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i]) {
if (_gyro_healthy[i] && _use[i]) {
_primary_gyro = i;
break;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i]) {
if (_accel_healthy[i] && _use[i]) {
_primary_accel = i;
break;
}

5
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -144,12 +144,14 @@ public:
uint8_t get_gyro_count(void) const { return _gyro_count; }
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;
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); }
bool get_accel_health_all(void) const;
uint8_t get_accel_count(void) const { return _accel_count; };
bool accel_calibrated_ok_all() const;
bool use_accel(uint8_t instance) const;
// get accel offsets in m/s/s
const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
@ -304,6 +306,9 @@ private:
AP_Int8 _accel_filter_cutoff;
AP_Int8 _gyro_filter_cutoff;
// use for attitude, velocity, position estimates
AP_Int8 _use[INS_MAX_INSTANCES];
// board orientation from AHRS
enum Rotation _board_orientation;