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

AP_InertialSensor: make PX4 healthy call _get_sample() 

this prevents a false positive during times like arming where we are
not reading the sensors
This commit is contained in:
Andrew Tridgell 2013-11-10 15:35:28 +11:00
parent 2af69b7131
commit f10a4b04ae
6 changed files with 29 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -140,7 +140,7 @@ public:
}
virtual uint16_t error_count(void) const { return 0; }
virtual bool healthy(void) const { return true; }
virtual bool healthy(void) { return true; }
protected:

2
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -80,7 +80,7 @@ void AP_InertialSensor_HIL::set_gyro(const Vector3f &gyro)
/**
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_HIL::healthy(void) const
bool AP_InertialSensor_HIL::healthy(void)
{
uint32_t tnow = hal.scheduler->micros();
if ((tnow - _last_accel_usec) > 40000) {

2
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

@ -20,7 +20,7 @@ public:
bool wait_for_sample(uint16_t timeout_ms);
void set_accel(const Vector3f &accel);
void set_gyro(const Vector3f &gyro);
bool healthy(void) const;
bool healthy(void);
protected:
uint16_t _init_sensor( Sample_rate sample_rate );

2
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -34,7 +34,7 @@ public:
float get_delta_time();
uint16_t error_count(void) const { return _error_count; }
bool healthy(void) const { return _error_count <= 4; }
bool healthy(void) { return _error_count <= 4; }
protected:
uint16_t _init_sensor( Sample_rate sample_rate );

19
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -131,6 +131,10 @@ void AP_InertialSensor_PX4::_get_sample(void)
struct accel_report accel_report;
struct gyro_report gyro_report;
if (_accel_fd == -1 || _gyro_fd == -1) {
return;
}
while (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
accel_report.timestamp != _last_accel_timestamp) {
_accel_in = Vector3f(accel_report.x, accel_report.y, accel_report.z);
@ -178,9 +182,22 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
/**
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_PX4::healthy(void) const
bool AP_InertialSensor_PX4::healthy(void)
{
if (_sample_time_usec == 0) {
// not initialised yet, show as healthy to prevent scary GCS
// warnings
return true;
}
uint64_t tnow = hrt_absolute_time();
if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec ||
(tnow - _last_gyro_timestamp) > 2*_sample_time_usec) {
// see if new samples are available
_get_sample();
tnow = hrt_absolute_time();
}
if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec) {
// accels have not updated
return false;

9
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -17,7 +17,12 @@ class AP_InertialSensor_PX4 : public AP_InertialSensor
{
public:
AP_InertialSensor_PX4() : AP_InertialSensor() {}
AP_InertialSensor_PX4() :
AP_InertialSensor(),
_sample_time_usec(0),
_accel_fd(-1),
_gyro_fd(-1)
{}
/* Concrete implementation of AP_InertialSensor functions: */
bool update();
@ -25,7 +30,7 @@ public:
float get_gyro_drift_rate();
bool sample_available();
bool wait_for_sample(uint16_t timeout_ms);
bool healthy(void) const;
bool healthy(void);
private:
uint16_t _init_sensor( Sample_rate sample_rate );