// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #if CLI_ENABLED == ENABLED // These are function definitions so the Menu can be constructed before the functions // are defined below. Order matters to the compiler. #if HIL_MODE == HIL_MODE_DISABLED static int8_t test_baro(uint8_t argc, const Menu::arg *argv); #endif static int8_t test_compass(uint8_t argc, const Menu::arg *argv); static int8_t test_ins(uint8_t argc, const Menu::arg *argv); static int8_t test_optflow(uint8_t argc, const Menu::arg *argv); static int8_t test_relay(uint8_t argc, const Menu::arg *argv); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN static int8_t test_shell(uint8_t argc, const Menu::arg *argv); #endif #if HIL_MODE == HIL_MODE_DISABLED static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); #endif // Creates a constant array of structs representing menu options // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Coommon for implementation details const struct Menu::command test_menu_commands[] PROGMEM = { #if HIL_MODE == HIL_MODE_DISABLED {"baro", test_baro}, #endif {"compass", test_compass}, {"ins", test_ins}, {"optflow", test_optflow}, {"relay", test_relay}, #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN {"shell", test_shell}, #endif #if HIL_MODE == HIL_MODE_DISABLED {"sonar", test_sonar}, #endif }; // A Macro to create the Menu MENU(test_menu, "test", test_menu_commands); static int8_t test_mode(uint8_t argc, const Menu::arg *argv) { test_menu.run(); return 0; } #if HIL_MODE == HIL_MODE_DISABLED static int8_t test_baro(uint8_t argc, const Menu::arg *argv) { int32_t alt; print_hit_enter(); init_barometer(true); while(1) { delay(100); alt = read_barometer(); if (!barometer.healthy) { cliSerial->println_P(PSTR("not healthy")); } else { cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"), alt / 100.0, barometer.get_pressure(), barometer.get_temperature()); } if(cliSerial->available() > 0) { return (0); } } return 0; } #endif static int8_t 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_P(PSTR("Compass: ")); print_enabled(false); return (0); } if (!compass.init()) { cliSerial->println_P(PSTR("Compass initialisation failed!")); return 0; } ahrs.init(); ahrs.set_fly_forward(true); ahrs.set_compass(&compass); report_compass(); // we need the AHRS initialised for this test ins.init(AP_InertialSensor::COLD_START, ins_sample_rate); 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.f; // 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_dcm_matrix(); 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_P(PSTR("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), (wrap_360_cd(ToDeg(heading) * 100)) /100, mag.x, mag.y, mag.z, mag_ofs.x, mag_ofs.y, 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); } static int8_t test_ins(uint8_t argc, const Menu::arg *argv) { Vector3f gyro, accel; print_hit_enter(); cliSerial->printf_P(PSTR("INS\n")); delay(1000); ahrs.init(); ins.init(AP_InertialSensor::COLD_START, ins_sample_rate); cliSerial->printf_P(PSTR("...done\n")); delay(50); while(1) { ins.update(); gyro = ins.get_gyro(); accel = ins.get_accel(); float test = accel.length() / GRAVITY_MSS; cliSerial->printf_P(PSTR("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %7.4f \n"), accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z, test); delay(40); if(cliSerial->available() > 0) { return (0); } } } static int8_t test_optflow(uint8_t argc, const Menu::arg *argv) { #if OPTFLOW == ENABLED if(g.optflow_enabled) { cliSerial->printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); print_hit_enter(); while(1) { delay(200); optflow.update(); cliSerial->printf_P(PSTR("dx:%d\t dy:%d\t squal:%d\n"), optflow.dx, optflow.dy, optflow.surface_quality); if(cliSerial->available() > 0) { return (0); } } } else { cliSerial->printf_P(PSTR("OptFlow: ")); print_enabled(false); } return (0); #else return (0); #endif // OPTFLOW == ENABLED } static int8_t test_relay(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); delay(1000); while(1) { cliSerial->printf_P(PSTR("Relay on\n")); relay.on(0); delay(3000); if(cliSerial->available() > 0) { return (0); } cliSerial->printf_P(PSTR("Relay off\n")); relay.off(0); delay(3000); if(cliSerial->available() > 0) { return (0); } } } #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN /* * run a debug shell */ static int8_t test_shell(uint8_t argc, const Menu::arg *argv) { hal.util->run_debug_shell(cliSerial); return 0; } #endif #if HIL_MODE == HIL_MODE_DISABLED /* * test the sonar */ static int8_t test_sonar(uint8_t argc, const Menu::arg *argv) { #if CONFIG_SONAR == ENABLED if(!sonar.healthy()) { cliSerial->printf_P(PSTR("Sonar disabled\n")); return (0); } // make sure sonar is initialised init_sonar(); print_hit_enter(); while(1) { delay(100); sonar.update(); cliSerial->printf_P(PSTR("Sonar: %d cm\n"), sonar.distance_cm()); if(cliSerial->available() > 0) { return (0); } } #endif return (0); } #endif static void print_hit_enter() { cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n")); } #endif // CLI_ENABLED