From 5754f9743367e0b6b5a677a2e7c306d08cb52915 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 27 May 2012 09:21:20 -0700 Subject: [PATCH] reversed circle_WP calculation so the next_WP refers to the destination around the circle and circle_WP refers to the center. The intent is to make it easier to see via ground station. --- ArduCopter/ArduCopter.pde | 4 ++-- ArduCopter/system.pde | 9 +++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 154542753f..fa8c716427 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2226,8 +2226,8 @@ static void update_nav_wp() if (circle_angle > 6.28318531) circle_angle -= 6.28318531; - circle_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp); - circle_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle)); + next_WP.lng = circle_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp); + next_WP.lat = circle_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle)); // calc the lat and long error to the target calc_location_error(&circle_WP); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 3d0aee089c..2ace5945fb 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -140,13 +140,13 @@ static void init_ardupilot() pinMode(COPTER_LED_6, OUTPUT); //Motor or Aux LED pinMode(COPTER_LED_7, OUTPUT); //Motor or GPS LED pinMode(COPTER_LED_8, OUTPUT); //Motor or GPS LED - - if ( !bitRead(g.copter_leds_mode, 3) ){ + + if ( !bitRead(g.copter_leds_mode, 3) ){ piezo_beep(); } - + #endif - + // load parameters from EEPROM load_parameters(); @@ -453,6 +453,7 @@ static void set_mode(byte mode) roll_pitch_mode = CIRCLE_RP; throttle_mode = CIRCLE_THR; set_next_WP(¤t_loc); + circle_WP = next_WP; circle_angle = 0; break;