Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: allow HIL sensors to support multiple instances

Browse Source 
this allows log replay to test both sets of sensors on a Pixhawk log
This commit is contained in:
Andrew Tridgell 2014-02-23 08:16:33 +11:00
parent 735c6449a1
commit 0e18079c47
3 changed files with 24 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -11,7 +11,7 @@
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
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#define INS_MAX_INSTANCES 2
#else
#define INS_MAX_INSTANCES 1
@ -96,7 +96,7 @@ public:
///
const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; }
const Vector3f &get_gyro(void) const { return get_gyro(_get_primary_gyro()); }
virtual void set_gyro(const Vector3f &gyro) {}
virtual void set_gyro(uint8_t instance, const Vector3f &gyro) {}
// set gyro offsets in radians/sec
const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
@ -108,7 +108,7 @@ public:
///
const Vector3f &get_accel(uint8_t i) const { return _accel[i]; }
const Vector3f &get_accel(void) const { return get_accel(_get_primary_accel()); }
virtual void set_accel(const Vector3f &accel) {}
virtual void set_accel(uint8_t instance, const Vector3f &accel) {}
// multi-device interface
virtual bool get_gyro_health(uint8_t instance) const { return true; }

40
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -67,38 +67,26 @@ bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
return false;
}
void AP_InertialSensor_HIL::set_accel(const Vector3f &accel)
void AP_InertialSensor_HIL::set_accel(uint8_t instance, const Vector3f &accel)
{
_previous_accel[0] = _accel[0];
_accel[0] = accel;
_last_accel_usec = hal.scheduler->micros();
_previous_accel[instance] = _accel[instance];
_accel[instance] = accel;
_last_accel_usec[instance] = hal.scheduler->micros();
}
void AP_InertialSensor_HIL::set_gyro(const Vector3f &gyro)
void AP_InertialSensor_HIL::set_gyro(uint8_t instance, const Vector3f &gyro)
{
_gyro[0] = gyro;
_last_gyro_usec = hal.scheduler->micros();
_gyro[instance] = gyro;
_last_gyro_usec[instance] = hal.scheduler->micros();
}
/**
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_HIL::healthy(void) const
bool AP_InertialSensor_HIL::get_gyro_health(uint8_t instance) const
{
uint32_t tnow = hal.scheduler->micros();
if ((tnow - _last_accel_usec) > 40000) {
// accels have not updated
return false;
return (hal.scheduler->micros() - _last_gyro_usec[instance]) < 40000;
}
if ((tnow - _last_gyro_usec) > 40000) {
// gyros have not updated
return false;
}
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;
}
return true;
bool AP_InertialSensor_HIL::get_accel_health(uint8_t instance) const
{
return (hal.scheduler->micros() - _last_accel_usec[instance]) < 40000;
}

11
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

@ -17,9 +17,10 @@ public:
float get_delta_time() const;
float get_gyro_drift_rate();
bool wait_for_sample(uint16_t timeout_ms);
void set_accel(const Vector3f &accel);
void set_gyro(const Vector3f &gyro);
bool healthy(void) const;
void set_accel(uint8_t instance, const Vector3f &accel);
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;
private:
bool _sample_available();
@ -27,8 +28,8 @@ private:
float _sample_period_ms;
uint32_t _last_update_ms;
uint32_t _delta_time_usec;
uint32_t _last_accel_usec;
uint32_t _last_gyro_usec;
uint32_t _last_accel_usec[INS_MAX_INSTANCES];
uint32_t _last_gyro_usec[INS_MAX_INSTANCES];
};
#endif // __AP_INERTIAL_SENSOR_STUB_H__