Copter: move report_compass out of the CLI

This function is also used by compassmot.  Having it strictly part of
the CLI causes a compile error when the CLI is disabled
This commit is contained in:
Randy Mackay 2014-03-05 12:01:45 +09:00
parent 46c8206d0e
commit 4f963e7795
1 changed files with 40 additions and 39 deletions

View File

@ -162,45 +162,6 @@ static void report_ins()
print_blanks(2); print_blanks(2);
} }
static void report_compass()
{
cliSerial->printf_P(PSTR("Compass\n"));
print_divider();
print_enabled(g.compass_enabled);
// mag declination
cliSerial->printf_P(PSTR("Mag Dec: %4.4f\n"),
degrees(compass.get_declination()));
Vector3f offsets = compass.get_offsets();
// mag offsets
cliSerial->printf_P(PSTR("Mag off: %4.4f, %4.4f, %4.4f\n"),
offsets.x,
offsets.y,
offsets.z);
// motor compensation
cliSerial->print_P(PSTR("Motor Comp: "));
if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
cliSerial->print_P(PSTR("Off\n"));
}else{
if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) {
cliSerial->print_P(PSTR("Throttle"));
}
if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) {
cliSerial->print_P(PSTR("Current"));
}
Vector3f motor_compensation = compass.get_motor_compensation();
cliSerial->printf_P(PSTR("\nComp Vec: %4.2f, %4.2f, %4.2f\n"),
motor_compensation.x,
motor_compensation.y,
motor_compensation.z);
}
print_blanks(1);
}
static void report_flight_modes() static void report_flight_modes()
{ {
cliSerial->printf_P(PSTR("Flight modes\n")); cliSerial->printf_P(PSTR("Flight modes\n"));
@ -279,6 +240,46 @@ print_gyro_offsets(void)
#endif // CLI_ENABLED #endif // CLI_ENABLED
// report_compass - displays compass information. Also called by compassmot.pde
static void report_compass()
{
cliSerial->printf_P(PSTR("Compass\n"));
print_divider();
print_enabled(g.compass_enabled);
// mag declination
cliSerial->printf_P(PSTR("Mag Dec: %4.4f\n"),
degrees(compass.get_declination()));
Vector3f offsets = compass.get_offsets();
// mag offsets
cliSerial->printf_P(PSTR("Mag off: %4.4f, %4.4f, %4.4f\n"),
offsets.x,
offsets.y,
offsets.z);
// motor compensation
cliSerial->print_P(PSTR("Motor Comp: "));
if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
cliSerial->print_P(PSTR("Off\n"));
}else{
if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) {
cliSerial->print_P(PSTR("Throttle"));
}
if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) {
cliSerial->print_P(PSTR("Current"));
}
Vector3f motor_compensation = compass.get_motor_compensation();
cliSerial->printf_P(PSTR("\nComp Vec: %4.2f, %4.2f, %4.2f\n"),
motor_compensation.x,
motor_compensation.y,
motor_compensation.z);
}
print_blanks(1);
}
static void static void
print_blanks(int16_t num) print_blanks(int16_t num)
{ {