From 31f31eec7531ce58db8254f8c3aab69c46e8c765 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Wed, 15 Aug 2012 21:03:32 -0700 Subject: [PATCH] ACM : Test for bearing calc --- ArduCopter/test.pde | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 90ebc50bd3..f902e65858 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -19,7 +19,7 @@ static int8_t test_ins(uint8_t argc, const Menu::arg *argv); //static int8_t test_stab_d(uint8_t argc, const Menu::arg *argv); static int8_t test_battery(uint8_t argc, const Menu::arg *argv); //static int8_t test_toy(uint8_t argc, const Menu::arg *argv); -//static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv); +static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv); //static int8_t test_reverse(uint8_t argc, const Menu::arg *argv); static int8_t test_tuning(uint8_t argc, const Menu::arg *argv); static int8_t test_relay(uint8_t argc, const Menu::arg *argv); @@ -88,7 +88,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { // {"rawgps", test_rawgps}, // {"mission", test_mission}, //{"reverse", test_reverse}, - //{"wp", test_wp_nav}, + {"nav", test_wp_nav}, }; // A Macro to create the Menu @@ -1236,6 +1236,20 @@ test_optflow(uint8_t argc, const Menu::arg *argv) } +static int8_t +test_wp_nav(uint8_t argc, const Menu::arg *argv) +{ + current_loc.lat = 389539260; + current_loc.lng = -1199540200; + + next_WP.lat = 389538528; + next_WP.lng = -1199541248; + + // got 235;, should be 228 + navigate(); + Serial.printf("bear: %ld\n", target_bearing); +} + /* test the dataflash is working */