Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: calculate next_turn_angle to prevent early WP completion

Browse Source 
this should help for ball drop and camera trigger with straight line
runs
This commit is contained in:
Andrew Tridgell 2014-06-05 09:35:09 +10:00
parent 5a1aa8dfe7
commit 55e7f9b1d6
3 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/ArduPlane.pde
View File

@ -533,13 +533,17 @@ static struct {
// initial pitch. Used to detect if nose is rising in a tail dragger
int16_t initial_pitch_cd;
// turn angle for next leg of mission
float next_turn_angle;
} auto_state = {
takeoff_complete : true,
land_complete : false,
takeoff_altitude_cm : 0,
takeoff_pitch_cd : 0,
highest_airspeed : 0,
initial_pitch_cd : 0
initial_pitch_cd : 0,
next_turn_angle : 90.0f
};
// true if we are in an auto-throttle mode, which means

2
ArduPlane/commands_logic.pde
View File

@ -413,7 +413,7 @@ static bool verify_nav_wp()
return false;
}
if (wp_distance <= nav_controller->turn_distance(g.waypoint_radius)) {
if (wp_distance <= nav_controller->turn_distance(g.waypoint_radius, auto_state.next_turn_angle)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)get_distance(current_loc, next_WP_loc));

25
ArduPlane/navigation.pde
View File

@ -225,6 +225,9 @@ static void update_fbwb_speed_height(void)
calc_nav_pitch();
}
/*
setup for a gradual glide slope to the next waypoint, if appropriate
*/
static void setup_glide_slope(void)
{
// establish the distance we are travelling to the next waypoint,
@ -266,8 +269,30 @@ static void setup_glide_slope(void)
offset_altitude_cm = 0;
break;
}
// calculate turn angle for next leg
setup_turn_angle();
}
/*
calculate the turn angle for the next leg of the mission
*/
static void setup_turn_angle(void)
{
int32_t next_ground_course_cd = mission.get_next_ground_course_cd(-1);
if (next_ground_course_cd == -1) {
// the mission library can't determine a turn angle, assume 90 degrees
auto_state.next_turn_angle = 90.0f;
} else {
// get the heading of the current leg
int32_t ground_course_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
// work out the angle we need to turn through
auto_state.next_turn_angle = wrap_180_cd(next_ground_course_cd - ground_course_cd) * 0.01f;
}
}
/*
return relative altitude in meters (relative to home)
*/