Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: auto-select healthy compass

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-09 20:01:42 +11:00
parent 61f564d7c9
commit c44d8b45ce
3 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -144,4 +144,12 @@ void AP_Compass_PX4::accumulate(void)
} }
} }
uint8_t AP_Compass_PX4::_get_primary(void) const
{
for (uint8_t i=0; i<_num_instances; i++) {
if (_healthy[i]) return i;
}
return 0;
}
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

1
libraries/AP_Compass/AP_Compass_PX4.h
View File

@ -20,6 +20,7 @@ public:
uint8_t get_count(void) const { return _num_instances; } uint8_t get_count(void) const { return _num_instances; }
private: private:
uint8_t _get_primary(void) const;
uint8_t _num_instances; uint8_t _num_instances;
int _mag_fd[COMPASS_MAX_INSTANCES]; int _mag_fd[COMPASS_MAX_INSTANCES];
Vector3f _sum[COMPASS_MAX_INSTANCES]; Vector3f _sum[COMPASS_MAX_INSTANCES];

8
libraries/AP_Compass/Compass.h
View File

@ -100,11 +100,11 @@ public:
/// Return the current field as a Vector3f /// Return the current field as a Vector3f
const Vector3f &get_field(uint8_t i) const { return _field[i]; } const Vector3f &get_field(uint8_t i) const { return _field[i]; }
const Vector3f &get_field(void) const { return get_field(0); } const Vector3f &get_field(void) const { return get_field(_get_primary()); }
/// Return the health of a compass /// Return the health of a compass
bool healthy(uint8_t i) const { return _healthy[i]; } bool healthy(uint8_t i) const { return _healthy[i]; }
bool healthy(void) const { return healthy(0); } bool healthy(void) const { return healthy(_get_primary()); }
/// set the current field as a Vector3f /// set the current field as a Vector3f
void set_field(const Vector3f &field) { _field[0] = field; } void set_field(const Vector3f &field) { _field[0] = field; }
@ -114,7 +114,7 @@ public:
/// @returns The current compass offsets. /// @returns The current compass offsets.
/// ///
const Vector3f &get_offsets(uint8_t i) const { return _offset[i]; } const Vector3f &get_offsets(uint8_t i) const { return _offset[i]; }
const Vector3f &get_offsets(void) const { return get_offsets(0); } const Vector3f &get_offsets(void) const { return get_offsets(_get_primary()); }
/// Sets the initial location used to get declination /// Sets the initial location used to get declination
/// ///
@ -217,6 +217,8 @@ public:
AP_Int8 _learn; ///<enable calibration learning AP_Int8 _learn; ///<enable calibration learning
protected: protected:
virtual uint8_t _get_primary(void) const { return 0; }
bool _healthy[COMPASS_MAX_INSTANCES]; bool _healthy[COMPASS_MAX_INSTANCES];
Vector3f _field[COMPASS_MAX_INSTANCES]; ///< magnetic field strength Vector3f _field[COMPASS_MAX_INSTANCES]; ///< magnetic field strength