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

AP_InertialSensor: auto-failover to working gyro and accel

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-09 20:02:04 +11:00
parent c44d8b45ce
commit cba0cb963a
3 changed files with 31 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -94,35 +94,37 @@ public:
/// @returns vector of rotational rates in radians/sec
///
const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; }
const Vector3f &get_gyro(void) const { return get_gyro(0); }
const Vector3f &get_gyro(void) const { return get_gyro(_get_primary_gyro()); }
virtual void set_gyro(const Vector3f &gyro) {}
// set gyro offsets in radians/sec
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); }
const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_get_primary_gyro()); }
/// Fetch the current accelerometer values
///
/// @returns vector of current accelerations in m/s/s
///
const Vector3f &get_accel(uint8_t i) const { return _accel[i]; }
const Vector3f &get_accel(void) const { return get_accel(0); }
const Vector3f &get_accel(void) const { return get_accel(_get_primary_accel()); }
virtual void set_accel(const Vector3f &accel) {}
// multi-device interface
virtual bool get_gyro_health(uint8_t instance) const;
bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); }
virtual uint8_t get_gyro_count(void) const { return 1; };
virtual bool get_accel_health(uint8_t instance) const;
bool get_accel_health(void) const { return get_accel_health(_get_primary_accel()); }
virtual uint8_t get_accel_count(void) const { return 1; };
// get accel offsets in m/s/s
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); }
const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_get_primary_accel()); }
// get 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); }
const Vector3f &get_accel_scale(void) const { return get_accel_scale(_get_primary_accel()); }
/* Update the sensor data, so that getters are nonblocking.
* Returns a bool of whether data was updated or not.
@ -157,10 +159,13 @@ public:
}
virtual uint16_t error_count(void) const { return 0; }
virtual bool healthy(void) const { return true; }
virtual bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
protected:
virtual uint8_t _get_primary_gyro(void) const { return 0; }
virtual uint8_t _get_primary_accel(void) const { return 0; }
// sensor specific init to be overwritten by descendant classes
virtual uint16_t _init_sensor( Sample_rate sample_rate ) = 0;

17
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -123,6 +123,7 @@ bool AP_InertialSensor_PX4::get_accel_health(uint8_t k) const
return true;
}
uint8_t AP_InertialSensor_PX4::get_accel_count(void) const
{
return _num_accel_instances;
@ -232,5 +233,21 @@ bool AP_InertialSensor_PX4::healthy(void) const
return get_gyro_health(0) && get_accel_health(0);
}
uint8_t AP_InertialSensor_PX4::_get_primary_gyro(void) const
{
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (get_gyro_health(i)) return i;
}
return 0;
}
uint8_t AP_InertialSensor_PX4::_get_primary_accel(void) const
{
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (get_accel_health(i)) return i;
}
return 0;
}
#endif // CONFIG_HAL_BOARD

3
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -38,6 +38,9 @@ public:
uint8_t get_accel_count(void) const;
private:
uint8_t _get_primary_gyro(void) const;
uint8_t _get_primary_accel(void) const;
uint16_t _init_sensor( Sample_rate sample_rate );
void _get_sample(void);
bool _sample_available(void);