aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: fix consistent check for less than three compasses

Browse Source 
Also use vector functions where available
Use get_field_milligause instead of just get_field
This commit is contained in:
Randy Mackay 2015-09-15 15:54:26 +09:00
parent 87bbf1a487
commit fff5ec09eb
2 changed files with 16 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Compass/Compass.cpp
View File

@ -782,57 +782,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();
float primary_mag_field_mag = primary_mag_field.length();
Vector3f primary_mag_field = get_field_milligauss();
Vector3f primary_mag_field_norm;
if (!is_zero(primary_mag_field_mag)) {
primary_mag_field_norm = primary_mag_field / primary_mag_field_mag;
if (!primary_mag_field.is_zero()) {
primary_mag_field_norm = primary_mag_field.normalized();
} else {
return false;
}
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;
if (!is_zero(primary_mag_field_xy_mag)) {
primary_mag_field_xy_norm = primary_mag_field_xy / primary_mag_field_xy_mag;
if (!primary_mag_field_xy.is_zero()) {
primary_mag_field_xy_norm = primary_mag_field_xy.normalized();
} else {
return false;
}
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if ((1<<i) & mask) {
Vector3f mag_field = get_field(i);
float mag_field_mag = mag_field.length();
for (uint8_t i=0; i<get_count(); i++) {
if (use_for_yaw(i)) {
Vector3f mag_field = get_field_milligauss(i);
Vector3f mag_field_norm;
if (!is_zero(mag_field_mag)) {
mag_field_norm = mag_field / mag_field_mag;
if (!mag_field.is_zero()) {
mag_field_norm = mag_field.normalized();
} else {
return false;
}
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;
if (!is_zero(mag_field_xy_mag)) {
mag_field_xy_norm = mag_field_xy / mag_field_xy_mag;
if (!mag_field_xy.is_zero()) {
mag_field_xy_norm = mag_field_xy.normalized();
} else {
return false;
}
@ -858,3 +843,4 @@ bool Compass::consistent(uint8_t mask) const
}
return true;
}

5
libraries/AP_Compass/Compass.h
View File

@ -170,9 +170,8 @@ public:
void send_mag_cal_progress(mavlink_channel_t chan);
void send_mag_cal_report(mavlink_channel_t chan);
uint8_t get_use_mask() const;
bool consistent(uint8_t mask) const;
bool consistent() const { return consistent(get_use_mask()); }
// check if the compasses are pointing in the same direction
bool consistent() const;
/// Return the health of a compass
bool healthy(uint8_t i) const { return _state[i].healthy; }