mirror of https://github.com/ArduPilot/ardupilot
82 lines
2.2 KiB
C++
82 lines
2.2 KiB
C++
#include "Copter.h"
|
|
|
|
// report_compass - displays compass information. Also called by compassmot.pde
|
|
void Copter::report_compass()
|
|
{
|
|
hal.console->printf("Compass\n");
|
|
print_divider();
|
|
|
|
print_enabled(AP::compass().enabled());
|
|
|
|
// mag declination
|
|
hal.console->printf("Mag Dec: %4.4f\n",
|
|
(double)degrees(compass.get_declination()));
|
|
|
|
// mag offsets
|
|
Vector3f offsets;
|
|
for (uint8_t i=0; i<compass.get_count(); i++) {
|
|
offsets = compass.get_offsets(i);
|
|
// mag offsets
|
|
hal.console->printf("Mag%d off: %4.4f, %4.4f, %4.4f\n",
|
|
(int)i,
|
|
(double)offsets.x,
|
|
(double)offsets.y,
|
|
(double)offsets.z);
|
|
}
|
|
|
|
// motor compensation
|
|
hal.console->printf("Motor Comp: ");
|
|
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
|
|
hal.console->printf("Off\n");
|
|
}else{
|
|
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) {
|
|
hal.console->printf("Throttle");
|
|
}
|
|
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) {
|
|
hal.console->printf("Current");
|
|
}
|
|
Vector3f motor_compensation;
|
|
for (uint8_t i=0; i<compass.get_count(); i++) {
|
|
motor_compensation = compass.get_motor_compensation(i);
|
|
hal.console->printf("\nComMot%d: %4.2f, %4.2f, %4.2f\n",
|
|
(int)i,
|
|
(double)motor_compensation.x,
|
|
(double)motor_compensation.y,
|
|
(double)motor_compensation.z);
|
|
}
|
|
}
|
|
print_blanks(1);
|
|
}
|
|
|
|
void Copter::print_blanks(int16_t num)
|
|
{
|
|
while(num > 0) {
|
|
num--;
|
|
hal.console->printf("\n");
|
|
}
|
|
}
|
|
|
|
void Copter::print_divider(void)
|
|
{
|
|
for (int i = 0; i < 40; i++) {
|
|
hal.console->printf("-");
|
|
}
|
|
hal.console->printf("\n");
|
|
}
|
|
|
|
void Copter::print_enabled(bool b)
|
|
{
|
|
if(b)
|
|
hal.console->printf("en");
|
|
else
|
|
hal.console->printf("dis");
|
|
hal.console->printf("abled\n");
|
|
}
|
|
|
|
void Copter::report_version()
|
|
{
|
|
hal.console->printf("FW Ver: %d\n",(int)(g.k_format_version));
|
|
print_divider();
|
|
print_blanks(2);
|
|
}
|