mirror of https://github.com/ArduPilot/ardupilot
parent
9eaf7f5528
commit
4a80313fdd
|
@ -1478,6 +1478,24 @@ Compass::use_for_yaw(uint8_t i) const
|
||||||
return _use_for_yaw[Priority(i)] && _learn.get() != LEARN_INFLIGHT;
|
return _use_for_yaw[Priority(i)] && _learn.get() != LEARN_INFLIGHT;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
return the number of enabled sensors. Used to determine if
|
||||||
|
non-compass operation is desired
|
||||||
|
*/
|
||||||
|
uint8_t Compass::get_num_enabled(void) const
|
||||||
|
{
|
||||||
|
if (get_count() == 0) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
uint8_t count = 0;
|
||||||
|
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
if (use_for_yaw(i)) {
|
||||||
|
count++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return count;
|
||||||
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
Compass::set_use_for_yaw(uint8_t i, bool use)
|
Compass::set_use_for_yaw(uint8_t i, bool use)
|
||||||
{
|
{
|
||||||
|
|
|
@ -137,6 +137,9 @@ public:
|
||||||
// return the number of compass instances
|
// return the number of compass instances
|
||||||
uint8_t get_count(void) const { return _compass_count; }
|
uint8_t get_count(void) const { return _compass_count; }
|
||||||
|
|
||||||
|
// return the number of enabled sensors
|
||||||
|
uint8_t get_num_enabled(void) const;
|
||||||
|
|
||||||
/// Return the current field as a Vector3f in milligauss
|
/// Return the current field as a Vector3f in milligauss
|
||||||
const Vector3f &get_field(uint8_t i) const { return _get_state(Priority(i)).field; }
|
const Vector3f &get_field(uint8_t i) const { return _get_state(Priority(i)).field; }
|
||||||
const Vector3f &get_field(void) const { return get_field(0); }
|
const Vector3f &get_field(void) const { return get_field(0); }
|
||||||
|
|
Loading…
Reference in New Issue