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

AP_Compass: fix consistent check for less than three compasses

Browse Source 
Also use vector functions where available
This commit is contained in:
Randy Mackay 2015-09-15 15:54:26 +09:00
parent 2f4f76d17a
commit 72ab60a19e
2 changed files with 14 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_Compass/Compass.cpp
View File

@ -637,57 +637,42 @@ void Compass::motor_compensation_type(const uint8_t comp_type)
} }
} }
uint8_t Compass::get_use_mask() const bool Compass::consistent() const
{
uint8_t ret = 0;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if (use_for_yaw(i)) {
ret |= 1 << i;
}
}
return ret;
}
bool Compass::consistent(uint8_t mask) const
{ {
Vector3f primary_mag_field = get_field(); Vector3f primary_mag_field = get_field();
float primary_mag_field_mag = primary_mag_field.length();
Vector3f primary_mag_field_norm; Vector3f primary_mag_field_norm;
if (!is_zero(primary_mag_field_mag)) { if (!primary_mag_field.is_zero()) {
primary_mag_field_norm = primary_mag_field / primary_mag_field_mag; primary_mag_field_norm = primary_mag_field.normalized();
} else { } else {
return false; return false;
} }
Vector2f primary_mag_field_xy = Vector2f(primary_mag_field.x,primary_mag_field.y); Vector2f primary_mag_field_xy = Vector2f(primary_mag_field.x,primary_mag_field.y);
float primary_mag_field_xy_mag = primary_mag_field_xy.length();
Vector2f primary_mag_field_xy_norm; Vector2f primary_mag_field_xy_norm;
if (!is_zero(primary_mag_field_xy_mag)) { if (!primary_mag_field_xy.is_zero()) {
primary_mag_field_xy_norm = primary_mag_field_xy / primary_mag_field_xy_mag; primary_mag_field_xy_norm = primary_mag_field_xy.normalized();
} else { } else {
return false; return false;
} }
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<get_count(); i++) {
if ((1<<i) & mask) { if (use_for_yaw(i)) {
Vector3f mag_field = get_field(i); Vector3f mag_field = get_field(i);
float mag_field_mag = mag_field.length();
Vector3f mag_field_norm; Vector3f mag_field_norm;
if (!is_zero(mag_field_mag)) { if (!mag_field.is_zero()) {
mag_field_norm = mag_field / mag_field_mag; mag_field_norm = mag_field.normalized();
} else { } else {
return false; return false;
} }
Vector2f mag_field_xy = Vector2f(mag_field.x,mag_field.y); Vector2f mag_field_xy = Vector2f(mag_field.x,mag_field.y);
float mag_field_xy_mag = mag_field_xy.length();
Vector2f mag_field_xy_norm; Vector2f mag_field_xy_norm;
if (!is_zero(mag_field_xy_mag)) { if (!mag_field_xy.is_zero()) {
mag_field_xy_norm = mag_field_xy / mag_field_xy_mag; mag_field_xy_norm = mag_field_xy.normalized();
} else { } else {
return false; return false;
} }
@ -713,3 +698,4 @@ bool Compass::consistent(uint8_t mask) const
} }
return true; return true;
} }

5
libraries/AP_Compass/Compass.h
View File

@ -110,9 +110,8 @@ public:
const Vector3f &get_field(uint8_t i) const { return _state[i].field; } const Vector3f &get_field(uint8_t i) const { return _state[i].field; }
const Vector3f &get_field(void) const { return get_field(get_primary()); } const Vector3f &get_field(void) const { return get_field(get_primary()); }
uint8_t get_use_mask() const; // check if the compasses are pointing in the same direction
bool consistent(uint8_t mask) const; bool consistent() const;
bool consistent() const { return consistent(get_use_mask()); }
/// Return the health of a compass /// Return the health of a compass
bool healthy(uint8_t i) const { return _state[i].healthy; } bool healthy(uint8_t i) const { return _state[i].healthy; }