aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: add advance track call to actually move towards waypoint

Browse Source 
Also added unrelated check that waypoint is valid before updating
waypoint distance.
This commit is contained in:
Randy Mackay 2013-01-28 15:59:55 +09:00 committed by rmackay9
parent 766ccdf6f8
commit 53ab1d5d9b
1 changed files with 39 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
ArduCopter/navigation.pde
View File

@ -157,21 +157,31 @@ static void calc_velocity_and_position(){
//****************************************************************
static void calc_distance_and_bearing()
{
// waypoint distance from plane in cm
// ---------------------------------------
wp_distance = get_distance_cm(&current_loc, &next_WP);
home_distance = get_distance_cm(&current_loc, &home);
// waypoint distance (in cm) and bearaing from plane
if( waypoint_valid(next_WP) ) {
wp_distance = get_distance_cm(&current_loc, &next_WP);
wp_bearing = get_bearing_cd(&current_loc, &next_WP);
}else{
wp_distance = 0;
wp_bearing = 0;
}
// wp_bearing is bearing to next waypoint
// --------------------------------------------
wp_bearing = get_bearing_cd(&current_loc, &next_WP);
home_bearing = get_bearing_cd(&current_loc, &home);
// calculate home distance and bearing
if( ap.home_is_set ) {
home_distance = get_distance_cm(&current_loc, &home);
home_bearing = get_bearing_cd(&current_loc, &home);
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing();
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing();
}else{
home_distance = 0;
home_bearing = 0;
}
// bearing to target (used when yaw_mode = YAW_LOOK_AT_LOCATION)
yaw_look_at_WP_bearing = get_bearing_cd(&current_loc, &yaw_look_at_WP);
// calculate bearing to target (used when yaw_mode = YAW_LOOK_AT_LOCATION)
if( waypoint_valid(yaw_look_at_WP) ) {
yaw_look_at_WP_bearing = get_bearing_cd(&current_loc, &yaw_look_at_WP);
}
}
static void calc_location_error(struct Location *next_loc)
@ -361,6 +371,10 @@ static void update_nav_mode()
break;
case NAV_WP_INAV:
// move forward on the waypoint
// To-Do: slew up the speed to the max waypoint speed instead of immediately jumping to max
wpinav_advance_track_desired(g.waypoint_speed_max, 0.1f);
// run the navigation controller
get_wpinav_pos(0.1f);
break;
}
@ -677,6 +691,16 @@ static int32_t get_yaw_slew(int32_t current_yaw, int32_t desired_yaw, int16_t de
return wrap_360(current_yaw + constrain(wrap_180(desired_yaw - current_yaw), -deg_per_sec, deg_per_sec));
}
// valid_waypoint - checks if a waypoint has been initialised or not
static bool waypoint_valid(Location &wp)
{
if( wp.lat != 0 || wp.lng != 0 ) {
return true;
}else{
return false;
}
}
////////////////////////////////////////////////////
// Loiter controller using inertial nav
////////////////////////////////////////////////////
@ -933,6 +957,9 @@ wpinav_advance_track_desired(float velocity_cms, float dt)
if( wpinav_track_desired > track_desired_max ) {
wpinav_track_desired = track_desired_max;
}
if( wpinav_track_desired > wpinav_track_length ) {
wpinav_track_desired = wpinav_track_length;
}
// recalculate the desired position
float track_length_pct = wpinav_track_desired/wpinav_track_length;