From 9012c538fb4ba179fd69022abbd4579fc67becca Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 12 Mar 2015 20:08:49 +0900 Subject: [PATCH] InertialNav: remove example sketch --- .../AP_InertialNav_test.pde | 98 ------------------- .../examples/AP_InertialNav_test/Makefile | 1 - .../AP_InertialNav_test/nocore.infoflag | 0 .../AP_InertialNav_test/norelax.inoflag | 0 4 files changed, 99 deletions(-) delete mode 100644 libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde delete mode 100644 libraries/AP_InertialNav/examples/AP_InertialNav_test/Makefile delete mode 100644 libraries/AP_InertialNav/examples/AP_InertialNav_test/nocore.infoflag delete mode 100644 libraries/AP_InertialNav/examples/AP_InertialNav_test/norelax.inoflag diff --git a/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde b/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde deleted file mode 100644 index 58f3bb3b04..0000000000 --- a/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde +++ /dev/null @@ -1,98 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include -#include -#include // ArduPilot Mega Vector/Matrix math Library -#include -#include -#include - -#include // ArduPilot GPS library -#include // GPS glitch protection library -#include // ArduPilot Mega Analog to Digital Converter Library -#include -#include // ArduPilot Mega Barometer Library -#include // Baro glitch protection library -#include -#include // ArduPilot Mega Magnetometer Library -#include -#include // ArduPilot Mega Inertial Sensor (accel & gyro) Library -#include -#include -#include -#include -#include // PID library -#include // P library -#include // ArduPilot general purpose FIFO buffer -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // battery monitor library -#include // Serial manager library - -const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; - -AP_InertialSensor ins; -static AP_SerialManager serial_manager; - -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 -AP_ADC_ADS7844 apm1_adc; -#endif - -AP_Baro baro; - -AP_GPS gps; -GPS_Glitch gps_glitch(gps); -Baro_Glitch baro_glitch(baro); - -AP_Compass_HMC5843 compass; -AP_AHRS_DCM ahrs(ins, baro, gps); - -AP_InertialNav inertialnav(ahrs, baro, gps_glitch, baro_glitch); - -uint32_t last_update; - -void setup(void) -{ - hal.console->println_P(PSTR("AP_InertialNav test startup...")); - - serial_manager.init(); - gps.init(NULL, serial_manager); - - ins.init(AP_InertialSensor::COLD_START, - AP_InertialSensor::RATE_100HZ); - - ahrs.set_compass(&compass); - - last_update = hal.scheduler->millis(); - - inertialnav.init(); - inertialnav.set_velocity_xy(0,0); - inertialnav.setup_home_position(); -} - -void loop(void) -{ - hal.scheduler->delay(20); - gps.update(); - ahrs.update(); - uint32_t currtime = hal.scheduler->millis(); - float dt = (currtime - last_update) / 1000.0f; - last_update = currtime; - inertialnav.update(dt); - - float dx = inertialnav.get_latitude_diff(); - float dy = inertialnav.get_longitude_diff(); - - hal.console->printf_P( - PSTR("inertial nav pos: (%f,%f)\r\n"), - dx, dy); -} - -AP_HAL_MAIN(); diff --git a/libraries/AP_InertialNav/examples/AP_InertialNav_test/Makefile b/libraries/AP_InertialNav/examples/AP_InertialNav_test/Makefile deleted file mode 100644 index f5daf25151..0000000000 --- a/libraries/AP_InertialNav/examples/AP_InertialNav_test/Makefile +++ /dev/null @@ -1 +0,0 @@ -include ../../../../mk/apm.mk diff --git a/libraries/AP_InertialNav/examples/AP_InertialNav_test/nocore.infoflag b/libraries/AP_InertialNav/examples/AP_InertialNav_test/nocore.infoflag deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/libraries/AP_InertialNav/examples/AP_InertialNav_test/norelax.inoflag b/libraries/AP_InertialNav/examples/AP_InertialNav_test/norelax.inoflag deleted file mode 100644 index e69de29bb2..0000000000