Browse Source

AP_InertialSensor: enable get_accel_count() and get_gyro_count() APIs for HIL

master
Andrew Tridgell 11 years ago
parent
commit
6732d6c79b
  1. 16
      libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
  2. 2
      libraries/AP_InertialSensor/AP_InertialSensor_HIL.h

16
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@ -90,3 +90,19 @@ bool AP_InertialSensor_HIL::get_accel_health(uint8_t instance) const @@ -90,3 +90,19 @@ bool AP_InertialSensor_HIL::get_accel_health(uint8_t instance) const
return (hal.scheduler->micros() - _last_accel_usec[instance]) < 40000;
}
uint8_t AP_InertialSensor_HIL::get_gyro_count(void) const
{
if (get_gyro_health(1)) {
return 2;
}
return 1;
}
uint8_t AP_InertialSensor_HIL::get_accel_count(void) const
{
if (get_accel_health(1)) {
return 2;
}
return 1;
}

2
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h

@ -21,6 +21,8 @@ public: @@ -21,6 +21,8 @@ public:
void set_gyro(uint8_t instance, const Vector3f &gyro);
bool get_gyro_health(uint8_t instance) const;
bool get_accel_health(uint8_t instance) const;
uint8_t get_gyro_count(void) const;
uint8_t get_accel_count(void) const;
private:
bool _sample_available();

Loading…
Cancel
Save