mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: use compass get_{field,offsets}() functions
Both functions are equivalent, so we're going to simply use get_{field,offsets}() instead of get_{field,offsets}_milligauss().
This commit is contained in:
parent
7c725bb59e
commit
fb5320bb25
|
@ -124,7 +124,7 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
|
||||||
// store initial x,y,z compass values
|
// store initial x,y,z compass values
|
||||||
// initialise interference percentage
|
// initialise interference percentage
|
||||||
for (uint8_t i=0; i<compass.get_count(); i++) {
|
for (uint8_t i=0; i<compass.get_count(); i++) {
|
||||||
compass_base[i] = compass.get_field_milligauss(i);
|
compass_base[i] = compass.get_field(i);
|
||||||
interference_pct[i] = 0.0f;
|
interference_pct[i] = 0.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -167,7 +167,7 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
|
||||||
// if throttle is near zero, update base x,y,z values
|
// if throttle is near zero, update base x,y,z values
|
||||||
if (throttle_pct <= 0.0f) {
|
if (throttle_pct <= 0.0f) {
|
||||||
for (uint8_t i=0; i<compass.get_count(); i++) {
|
for (uint8_t i=0; i<compass.get_count(); i++) {
|
||||||
compass_base[i] = compass_base[i] * 0.99f + compass.get_field_milligauss(i) * 0.01f;
|
compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f;
|
||||||
}
|
}
|
||||||
|
|
||||||
// causing printing to happen as soon as throttle is lifted
|
// causing printing to happen as soon as throttle is lifted
|
||||||
|
@ -175,7 +175,7 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
|
||||||
|
|
||||||
// calculate diff from compass base and scale with throttle
|
// calculate diff from compass base and scale with throttle
|
||||||
for (uint8_t i=0; i<compass.get_count(); i++) {
|
for (uint8_t i=0; i<compass.get_count(); i++) {
|
||||||
motor_impact[i] = compass.get_field_milligauss(i) - compass_base[i];
|
motor_impact[i] = compass.get_field(i) - compass_base[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
// throttle based compensation
|
// throttle based compensation
|
||||||
|
|
|
@ -358,7 +358,7 @@ bool Copter::pre_arm_checks(bool display_failure)
|
||||||
}
|
}
|
||||||
|
|
||||||
// check for unreasonable compass offsets
|
// check for unreasonable compass offsets
|
||||||
Vector3f offsets = compass.get_offsets_milligauss();
|
Vector3f offsets = compass.get_offsets();
|
||||||
if(offsets.length() > COMPASS_OFFSETS_MAX) {
|
if(offsets.length() > COMPASS_OFFSETS_MAX) {
|
||||||
if (display_failure) {
|
if (display_failure) {
|
||||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Compass offsets too high"));
|
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Compass offsets too high"));
|
||||||
|
@ -367,7 +367,7 @@ bool Copter::pre_arm_checks(bool display_failure)
|
||||||
}
|
}
|
||||||
|
|
||||||
// check for unreasonable mag field length
|
// check for unreasonable mag field length
|
||||||
float mag_field = compass.get_field_milligauss().length();
|
float mag_field = compass.get_field().length();
|
||||||
if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) {
|
if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) {
|
||||||
if (display_failure) {
|
if (display_failure) {
|
||||||
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check mag field"));
|
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check mag field"));
|
||||||
|
|
|
@ -450,7 +450,7 @@ void Copter::report_compass()
|
||||||
// mag offsets
|
// mag offsets
|
||||||
Vector3f offsets;
|
Vector3f offsets;
|
||||||
for (uint8_t i=0; i<compass.get_count(); i++) {
|
for (uint8_t i=0; i<compass.get_count(); i++) {
|
||||||
offsets = compass.get_offsets_milligauss(i);
|
offsets = compass.get_offsets(i);
|
||||||
// mag offsets
|
// mag offsets
|
||||||
cliSerial->printf_P(PSTR("Mag%d off: %4.4f, %4.4f, %4.4f\n"),
|
cliSerial->printf_P(PSTR("Mag%d off: %4.4f, %4.4f, %4.4f\n"),
|
||||||
(int)i,
|
(int)i,
|
||||||
|
|
|
@ -116,8 +116,8 @@ int8_t Copter::test_compass(uint8_t argc, const Menu::arg *argv)
|
||||||
counter++;
|
counter++;
|
||||||
if (counter>20) {
|
if (counter>20) {
|
||||||
if (compass.healthy()) {
|
if (compass.healthy()) {
|
||||||
const Vector3f &mag_ofs = compass.get_offsets_milligauss();
|
const Vector3f &mag_ofs = compass.get_offsets();
|
||||||
const Vector3f &mag = compass.get_field_milligauss();
|
const Vector3f &mag = compass.get_field();
|
||||||
cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
|
cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
|
||||||
(wrap_360_cd(ToDeg(heading) * 100)) /100,
|
(wrap_360_cd(ToDeg(heading) * 100)) /100,
|
||||||
(double)mag.x,
|
(double)mag.x,
|
||||||
|
|
Loading…
Reference in New Issue