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 }
void Copter::report_optflow() { #if OPTFLOW == ENABLED cliSerial->printf("OptFlow\n"); print_divider(); print_enabled(optflow.enabled()); print_blanks(2); #endif // OPTFLOW == ENABLED }
void Copter::report_batt_monitor() { cliSerial->printf("\nBatt Mon:\n"); print_divider(); if (battery.num_instances() == 0) { print_enabled(false); } else if (!battery.has_current()) { cliSerial->printf("volts"); } else { cliSerial->printf("volts and cur"); } print_blanks(2); }
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); } } } }
// report_compass - displays compass information. Also called by compassmot.pde void Copter::report_compass() { cliSerial->printf("Compass\n"); print_divider(); print_enabled(g.compass_enabled); // mag declination cliSerial->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 cliSerial->printf("Mag%d off: %4.4f, %4.4f, %4.4f\n", (int)i, (double)offsets.x, (double)offsets.y, (double)offsets.z); } // motor compensation cliSerial->print("Motor Comp: "); if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) { cliSerial->print("Off\n"); }else{ if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) { cliSerial->print("Throttle"); } if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) { cliSerial->print("Current"); } Vector3f motor_compensation; for (uint8_t i=0; i<compass.get_count(); i++) { motor_compensation = compass.get_motor_compensation(i); cliSerial->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); }
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); }
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); }