mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 18:38:28 -04:00
Compass: catch the case of a user enabling a compass mid-flight
if a compass has not been initialised at startup we can't enable it mid-flight, as we don't have the orientation
This commit is contained in:
parent
3fe5b3151b
commit
f50f4357ca
@ -247,12 +247,20 @@ AP_Compass_HMC5843::init()
|
||||
return false;
|
||||
}
|
||||
|
||||
_initialised = true;
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
// Read Sensor data
|
||||
bool AP_Compass_HMC5843::read()
|
||||
{
|
||||
if (!_initialised) {
|
||||
// someone has tried to enable a compass for the first time
|
||||
// mid-flight .... we can't do that yet (especially as we won't
|
||||
// have the right orientation!)
|
||||
return false;
|
||||
}
|
||||
if (!healthy) {
|
||||
if (millis() < _retry_time) {
|
||||
return false;
|
||||
|
@ -48,6 +48,7 @@ class AP_Compass_HMC5843 : public Compass
|
||||
{
|
||||
private:
|
||||
float calibration[3];
|
||||
bool _initialised;
|
||||
virtual bool read_raw(void);
|
||||
uint8_t _base_config;
|
||||
virtual bool re_initialise(void);
|
||||
|
Loading…
Reference in New Issue
Block a user