int8_t Sub::test_optflow(uint8_t argc, const Menu::arg *argv)
{
#if OPTFLOW == ENABLED
if(optflow.enabled()) {
cliSerial->printf("dev id: %d\t",(int)optflow.device_id());
print_hit_enter();
while(1) {
delay(200);
optflow.update();
const Vector2f& flowRate = optflow.flowRate();
cliSerial->printf("flowX : %7.4f\t flowY : %7.4f\t flow qual : %d\n",
(double)flowRate.x,
(double)flowRate.y,
(int)optflow.quality());
if(cliSerial->available() > 0) {
return (0);
}
}
} else {
cliSerial->printf("OptFlow: ");
print_enabled(false);
}
return (0);
#else
return (0);
#endif // OPTFLOW == ENABLED
}
int8_t Rover::test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
// read the radio to set trims
// ---------------------------
trim_radio();
while(1){
delay(20);
read_radio();
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
// write out the servo PWM values
// ------------------------------
set_servos();
cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
channel_steer->get_control_in(),
g.rc_2.get_control_in(),
channel_throttle->get_control_in(),
g.rc_4.get_control_in(),
g.rc_5.get_control_in(),
g.rc_6.get_control_in(),
g.rc_7.get_control_in(),
g.rc_8.get_control_in());
if(cliSerial->available() > 0){
return (0);
}
}
}
/*
* test the rangefinders
*/
int8_t Sub::test_rangefinder(uint8_t argc, const Menu::arg *argv)
{
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.init();
cliSerial->printf("RangeFinder: %d devices detected\n", rangefinder.num_sensors());
print_hit_enter();
while (1) {
hal.scheduler->delay(100);
rangefinder.update();
for (uint8_t i=0; i<rangefinder.num_sensors(); i++) {
cliSerial->printf("Dev%d: status %d distance_cm %d\n",
(int)i,
(int)rangefinder.status(i),
(int)rangefinder.distance_cm(i));
}
if (cliSerial->available() > 0) {
return (0);
}
}
#endif
return (0);
}
int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
while(1) {
hal.scheduler->delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
cliSerial->printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
(int)channel_roll->radio_in,
(int)channel_pitch->radio_in,
(int)channel_throttle->radio_in,
(int)channel_rudder->radio_in,
(int)g.rc_5.radio_in,
(int)g.rc_6.radio_in,
(int)g.rc_7.radio_in,
(int)g.rc_8.radio_in);
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Sub::test_ins(uint8_t argc, const Menu::arg *argv)
{
Vector3f gyro, accel;
print_hit_enter();
cliSerial->printf("INS\n");
delay(1000);
ahrs.init();
ins.init(scheduler.get_loop_rate_hz());
cliSerial->printf("...done\n");
delay(50);
while(1) {
ins.update();
gyro = ins.get_gyro();
accel = ins.get_accel();
float test = accel.length() / GRAVITY_MSS;
cliSerial->printf("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %7.4f \n",
(double)accel.x, (double)accel.y, (double)accel.z,
(double)gyro.x, (double)gyro.y, (double)gyro.z,
(double)test);
delay(40);
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Rover::test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.cpp file
// ----------------------------------------------------------
read_radio();
cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
channel_steer->get_radio_in(),
g.rc_2.get_radio_in(),
channel_throttle->get_radio_in(),
g.rc_4.get_radio_in(),
g.rc_5.get_radio_in(),
g.rc_6.get_radio_in(),
g.rc_7.get_radio_in(),
g.rc_8.get_radio_in());
if(cliSerial->available() > 0){
return (0);
}
}
}
int8_t Rover::test_gps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
uint32_t last_message_time_ms = 0;
while(1) {
delay(100);
gps.update();
if (gps.last_message_time_ms() != last_message_time_ms) {
last_message_time_ms = gps.last_message_time_ms();
const Location &loc = gps.location();
cliSerial->printf("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n",
(long)loc.lat,
(long)loc.lng,
(long)loc.alt/100,
(int)gps.num_sats());
} else {
cliSerial->print(".");
}
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Plane::test_pressure(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf_P(PSTR("Uncalibrated relative airpressure\n"));
print_hit_enter();
init_barometer();
while(1) {
hal.scheduler->delay(100);
barometer.update();
if (!barometer.healthy()) {
cliSerial->println_P(PSTR("not healthy"));
} else {
cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"),
(double)barometer.get_altitude(),
(double)barometer.get_pressure(),
(double)barometer.get_temperature());
}
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test = 0;
print_hit_enter();
for(int i = 0; i < 50; i++){
delay(20);
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
cliSerial->println("Unplug battery, throttle in neutral, turn off radio.");
while(channel_throttle->get_control_in() > 0){
delay(20);
read_radio();
}
while(1){
delay(20);
read_radio();
if(channel_throttle->get_control_in() > 0){
cliSerial->printf("THROTTLE CHANGED %d \n", channel_throttle->get_control_in());
fail_test++;
}
if (oldSwitchPosition != readSwitch()){
cliSerial->print("CONTROL MODE CHANGED: ");
print_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(throttle_failsafe_active()) {
cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", channel_throttle->get_radio_in());
print_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(fail_test > 0){
return (0);
}
if(cliSerial->available() > 0){
cliSerial->println("LOS caused no change in APM.");
return (0);
}
}
}
int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test;
print_hit_enter();
for(int16_t i = 0; i < 50; i++) {
hal.scheduler->delay(20);
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
cliSerial->printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(channel_throttle->control_in > 0) {
hal.scheduler->delay(20);
read_radio();
}
while(1) {
hal.scheduler->delay(20);
read_radio();
if(channel_throttle->control_in > 0) {
cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)channel_throttle->control_in);
fail_test++;
}
if(oldSwitchPosition != readSwitch()) {
cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(rc_failsafe_active()) {
cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)channel_throttle->radio_in);
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(fail_test > 0) {
return (0);
}
if(cliSerial->available() > 0) {
cliSerial->printf_P(PSTR("LOS caused no change in APM.\n"));
return (0);
}
}
}
int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv)
{
//cliSerial->printf_P(PSTR("Calibrating."));
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate);
ahrs.reset();
print_hit_enter();
hal.scheduler->delay(1000);
uint8_t counter = 0;
while(1) {
hal.scheduler->delay(20);
if (micros() - fast_loopTimer_us > 19000UL) {
fast_loopTimer_us = micros();
// INS
// ---
ahrs.update();
if(g.compass_enabled) {
counter++;
if(counter == 5) {
compass.read();
counter = 0;
}
}
// We are using the INS
// ---------------------
Vector3f gyros = ins.get_gyro();
Vector3f accels = ins.get_accel();
cliSerial->printf_P(PSTR("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n"),
(int)ahrs.roll_sensor / 100,
(int)ahrs.pitch_sensor / 100,
(uint16_t)ahrs.yaw_sensor / 100,
(double)gyros.x, (double)gyros.y, (double)gyros.z,
(double)accels.x, (double)accels.y, (double)accels.z);
}
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Plane::test_xbee(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
cliSerial->printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
while(1) {
if (hal.uartC->available())
hal.uartC->write(hal.uartC->read());
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Plane::test_adc(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
apm1_adc.Init();
hal.scheduler->delay(1000);
cliSerial->printf_P(PSTR("ADC\n"));
hal.scheduler->delay(1000);
while(1) {
for (int8_t i=0; i<9; i++) cliSerial->printf_P(PSTR("%.1f\t"),apm1_adc.Ch(i));
cliSerial->println();
hal.scheduler->delay(100);
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv)
{
//cliSerial->print("Calibrating.");
ahrs.init();
ahrs.set_fly_forward(true);
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
print_hit_enter();
delay(1000);
uint8_t medium_loopCounter = 0;
while(1){
ins.wait_for_sample();
ahrs.update();
if(g.compass_enabled) {
medium_loopCounter++;
if(medium_loopCounter >= 5){
compass.read();
medium_loopCounter = 0;
}
}
// We are using the IMU
// ---------------------
Vector3f gyros = ins.get_gyro();
Vector3f accels = ins.get_accel();
cliSerial->printf("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n",
(int)ahrs.roll_sensor / 100,
(int)ahrs.pitch_sensor / 100,
(uint16_t)ahrs.yaw_sensor / 100,
(double)gyros.x, (double)gyros.y, (double)gyros.z,
(double)accels.x, (double)accels.y, (double)accels.z);
if(cliSerial->available() > 0){
return (0);
}
}
}
int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
cliSerial->print("Control CH ");
cliSerial->println(MODE_CHANNEL, BASE_DEC);
while(1){
delay(20);
uint8_t switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
cliSerial->printf("Position %d\n", switchPosition);
oldSwitchPosition = switchPosition;
}
if(cliSerial->available() > 0){
return (0);
}
}
}
int8_t Plane::test_modeswitch(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
cliSerial->printf_P(PSTR("Control CH "));
cliSerial->println(FLIGHT_MODE_CHANNEL, BASE_DEC);
while(1) {
hal.scheduler->delay(20);
uint8_t switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition) {
cliSerial->printf_P(PSTR("Position %d\n"), (int)switchPosition);
oldSwitchPosition = switchPosition;
}
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Plane::test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
while(1) {
cliSerial->printf_P(PSTR("Relay on\n"));
relay.on(0);
hal.scheduler->delay(3000);
if(cliSerial->available() > 0) {
return (0);
}
cliSerial->printf_P(PSTR("Relay off\n"));
relay.off(0);
hal.scheduler->delay(3000);
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Rover::test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
cliSerial->println("Relay on");
relay.on(0);
delay(3000);
if(cliSerial->available() > 0){
return (0);
}
cliSerial->println("Relay off");
relay.off(0);
delay(3000);
if(cliSerial->available() > 0){
return (0);
}
}
}
int8_t Sub::test_baro(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
init_barometer(true);
while(1) {
delay(100);
read_barometer();
if (!barometer.healthy()) {
cliSerial->println("not healthy");
} else {
cliSerial->printf("Alt: %0.2fm, Raw: %f Temperature: %.1f\n",
(double)(baro_alt / 100.0f),
(double)barometer.get_pressure(),
(double)barometer.get_temperature());
}
if(cliSerial->available() > 0) {
return (0);
}
}
return 0;
}
int8_t Plane::test_airspeed(uint8_t argc, const Menu::arg *argv)
{
if (!airspeed.enabled()) {
cliSerial->printf_P(PSTR("airspeed: "));
print_enabled(false);
return (0);
}else{
print_hit_enter();
zero_airspeed(false);
cliSerial->printf_P(PSTR("airspeed: "));
print_enabled(true);
while(1) {
hal.scheduler->delay(20);
read_airspeed();
cliSerial->printf_P(PSTR("%.1f m/s\n"), (double)airspeed.get_airspeed());
if(cliSerial->available() > 0) {
return (0);
}
}
}
}
/*
* test the rangefinders
*/
int8_t Sub::test_sonar(uint8_t argc, const Menu::arg *argv)
{
#if CONFIG_SONAR == ENABLED
sonar.init();
cliSerial->printf("RangeFinder: %d devices detected\n", sonar.num_sensors());
print_hit_enter();
while(1) {
delay(100);
sonar.update();
cliSerial->printf("Primary: status %d distance_cm %d \n", (int)sonar.status(), sonar.distance_cm());
cliSerial->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n",
(int)sonar._type[0], (int)sonar.status(0), sonar.distance_cm(0), (int)sonar._type[1], (int)sonar.status(1), sonar.distance_cm(1));
if(cliSerial->available() > 0) {
return (0);
}
}
#endif
return (0);
}
int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
// read the radio to set trims
// ---------------------------
trim_radio();
while(1) {
hal.scheduler->delay(20);
read_radio();
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
channel_throttle->calc_pwm();
channel_rudder->calc_pwm();
// write out the servo PWM values
// ------------------------------
set_servos();
cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
(int)channel_roll->control_in,
(int)channel_pitch->control_in,
(int)channel_throttle->control_in,
(int)channel_rudder->control_in,
(int)g.rc_5.control_in,
(int)g.rc_6.control_in,
(int)g.rc_7.control_in,
(int)g.rc_8.control_in);
if(cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(20);
// New radio frame? (we could use also if((millis()- timer) > 20)
if (hal.rcin->new_input()) {
cliSerial->print("CH:");
for(int i = 0; i < 8; i++){
cliSerial->print(hal.rcin->read(i)); // Print channel values
cliSerial->print(",");
hal.rcout->write(i, hal.rcin->read(i)); // Copy input to Servos
}
cliSerial->println();
}
if (cliSerial->available() > 0){
return (0);
}
}
return 0;
}
int8_t Plane::test_passthru(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
hal.scheduler->delay(1000);
while(1) {
hal.scheduler->delay(20);
// New radio frame? (we could use also if((millis()- timer) > 20)
if (hal.rcin->new_input()) {
cliSerial->print_P(PSTR("CH:"));
for(int16_t i = 0; i < 8; i++) {
cliSerial->print(hal.rcin->read(i)); // Print channel values
print_comma();
servo_write(i, hal.rcin->read(i)); // Copy input to Servos
}
cliSerial->println();
}
if (cliSerial->available() > 0) {
return (0);
}
}
return 0;
}
int8_t Sub::test_compass(uint8_t argc, const Menu::arg *argv)
{
uint8_t delta_ms_fast_loop;
uint8_t medium_loopCounter = 0;
if (!g.compass_enabled) {
cliSerial->printf("Compass: "******"Compass initialisation failed!");
return 0;
}
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_compass(&compass);
#if OPTFLOW == ENABLED
ahrs.set_optflow(&optflow);
#endif
report_compass();
// we need the AHRS initialised for this test
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
int16_t counter = 0;
float heading = 0;
print_hit_enter();
while(1) {
delay(20);
if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.0f; // used by DCM integrator
fast_loopTimer = millis();
// INS
// ---
ahrs.update();
medium_loopCounter++;
if(medium_loopCounter == 5) {
if (compass.read()) {
// Calculate heading
const Matrix3f &m = ahrs.get_rotation_body_to_ned();
heading = compass.calculate_heading(m);
compass.learn_offsets();
}
medium_loopCounter = 0;
}
counter++;
if (counter>20) {
if (compass.healthy()) {
const Vector3f &mag_ofs = compass.get_offsets();
const Vector3f &mag = compass.get_field();
cliSerial->printf("Heading: %d, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
(int)(wrap_360_cd(ToDeg(heading) * 100)) /100,
(double)mag.x,
(double)mag.y,
(double)mag.z,
(double)mag_ofs.x,
(double)mag_ofs.y,
(double)mag_ofs.z);
} else {
cliSerial->println("compass not healthy");
}
counter=0;
}
}
if (cliSerial->available() > 0) {
break;
}
}
// save offsets. This allows you to get sane offset values using
// the CLI before you go flying.
cliSerial->println("saving offsets");
compass.save_offsets();
return (0);
}
int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv)
{
init_sonar();
delay(20);
sonar.update();
if (sonar.status() == RangeFinder::RangeFinder_NotConnected) {
cliSerial->println("WARNING: Sonar is not enabled");
}
print_hit_enter();
float sonar_dist_cm_min = 0.0f;
float sonar_dist_cm_max = 0.0f;
float voltage_min=0.0f, voltage_max = 0.0f;
float sonar2_dist_cm_min = 0.0f;
float sonar2_dist_cm_max = 0.0f;
float voltage2_min=0.0f, voltage2_max = 0.0f;
uint32_t last_print = 0;
while (true) {
delay(20);
sonar.update();
uint32_t now = millis();
float dist_cm = sonar.distance_cm(0);
float voltage = sonar.voltage_mv(0);
if (is_zero(sonar_dist_cm_min)) {
sonar_dist_cm_min = dist_cm;
voltage_min = voltage;
}
sonar_dist_cm_max = MAX(sonar_dist_cm_max, dist_cm);
sonar_dist_cm_min = MIN(sonar_dist_cm_min, dist_cm);
voltage_min = MIN(voltage_min, voltage);
voltage_max = MAX(voltage_max, voltage);
dist_cm = sonar.distance_cm(1);
voltage = sonar.voltage_mv(1);
if (is_zero(sonar2_dist_cm_min)) {
sonar2_dist_cm_min = dist_cm;
voltage2_min = voltage;
}
sonar2_dist_cm_max = MAX(sonar2_dist_cm_max, dist_cm);
sonar2_dist_cm_min = MIN(sonar2_dist_cm_min, dist_cm);
voltage2_min = MIN(voltage2_min, voltage);
voltage2_max = MAX(voltage2_max, voltage);
if (now - last_print >= 200) {
cliSerial->printf("sonar1 dist=%.1f:%.1fcm volt1=%.2f:%.2f sonar2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n",
(double)sonar_dist_cm_min,
(double)sonar_dist_cm_max,
(double)voltage_min,
(double)voltage_max,
(double)sonar2_dist_cm_min,
(double)sonar2_dist_cm_max,
(double)voltage2_min,
(double)voltage2_max);
voltage_min = voltage_max = 0.0f;
voltage2_min = voltage2_max = 0.0f;
sonar_dist_cm_min = sonar_dist_cm_max = 0.0f;
sonar2_dist_cm_min = sonar2_dist_cm_max = 0.0f;
last_print = now;
}
if (cliSerial->available() > 0) {
break;
}
}
return (0);
}
int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
{
if (!g.compass_enabled) {
cliSerial->print("Compass: "******"Compass initialisation failed!");
return 0;
}
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_compass(&compass);
// we need the AHRS initialised for this test
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
int counter = 0;
float heading = 0;
print_hit_enter();
uint8_t medium_loopCounter = 0;
while(1) {
ins.wait_for_sample();
ahrs.update();
medium_loopCounter++;
if(medium_loopCounter >= 5){
if (compass.read()) {
// Calculate heading
Matrix3f m = ahrs.get_rotation_body_to_ned();
heading = compass.calculate_heading(m);
compass.learn_offsets();
}
medium_loopCounter = 0;
}
counter++;
if (counter>20) {
if (compass.healthy()) {
const Vector3f mag_ofs = compass.get_offsets();
const Vector3f mag = compass.get_field();
cliSerial->printf("Heading: %f, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
(double)(wrap_360_cd(ToDeg(heading) * 100)) /100,
(double)mag.x, (double)mag.y, (double)mag.z,
(double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);
} else {
cliSerial->println("compass not healthy");
}
counter=0;
}
if (cliSerial->available() > 0) {
break;
}
}
// save offsets. This allows you to get sane offset values using
// the CLI before you go flying.
cliSerial->println("saving offsets");
compass.save_offsets();
return (0);
}
int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
{
if (!g.compass_enabled) {
cliSerial->printf_P(PSTR("Compass: "******"Compass initialisation failed!"));
return 0;
}
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
ahrs.set_compass(&compass);
// we need the AHRS initialised for this test
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate);
ahrs.reset();
uint16_t counter = 0;
float heading = 0;
print_hit_enter();
while(1) {
hal.scheduler->delay(20);
if (micros() - fast_loopTimer_us > 19000UL) {
fast_loopTimer_us = micros();
// INS
// ---
ahrs.update();
if(counter % 5 == 0) {
if (compass.read()) {
// Calculate heading
const Matrix3f &m = ahrs.get_dcm_matrix();
heading = compass.calculate_heading(m);
compass.learn_offsets();
}
}
counter++;
if (counter>20) {
if (compass.healthy()) {
const Vector3f &mag_ofs = compass.get_offsets_milligauss();
const Vector3f &mag = compass.get_field_milligauss();
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,
(double)mag.x, (double)mag.y, (double)mag.z,
(double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);
} else {
cliSerial->println_P(PSTR("compass not healthy"));
}
counter=0;
}
}
if (cliSerial->available() > 0) {
break;
}
}
// save offsets. This allows you to get sane offset values using
// the CLI before you go flying.
cliSerial->println_P(PSTR("saving offsets"));
compass.save_offsets();
return (0);
}
