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.

This commit is contained in:
Jason Short 2012-05-27 09:21:20 -07:00
parent 56335e81e4
commit 5754f97433
2 changed files with 7 additions and 6 deletions

View File

@ -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);

View File

@ -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(&current_loc);
circle_WP = next_WP;
circle_angle = 0;
break;