Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: bug fix for conditional_distance command 

Due to a race condition, the wp_distance was not being updated before
the conditional_distance
This commit is contained in:
Randy Mackay 2014-04-17 22:22:05 +09:00
parent 55d926b055
commit 817c893f21
2 changed files with 29 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/commands_logic.pde
View File

@ -745,6 +745,8 @@ static bool verify_change_alt()
static bool verify_within_distance() static bool verify_within_distance()
{ {
// update distance calculation
calc_wp_distance();
if (wp_distance < max(condition_value,0)) { if (wp_distance < max(condition_value,0)) {
condition_value = 0; condition_value = 0;
return true; return true;

46
ArduCopter/navigation.pde
View File

@ -24,25 +24,47 @@ static void calc_position(){
} }
} }
// calc_distance_and_bearing - calculate distance and direction to waypoints for reporting and autopilot decisions // calc_distance_and_bearing - calculate distance and bearing to next waypoint and home
static void calc_distance_and_bearing() static void calc_distance_and_bearing()
{ {
Vector3f curr = inertial_nav.get_position(); calc_wp_distance();
calc_wp_bearing();
calc_home_distance_and_bearing();
}
// calc_wp_distance - calculate distance to next waypoint for reporting and autopilot decisions
static void calc_wp_distance()
{
// get target from loiter or wpinav controller // get target from loiter or wpinav controller
if (control_mode == LOITER || control_mode == CIRCLE) { if (control_mode == LOITER || control_mode == CIRCLE) {
wp_distance = wp_nav.get_loiter_distance_to_target(); wp_distance = wp_nav.get_loiter_distance_to_target();
wp_bearing = wp_nav.get_loiter_bearing_to_target();
}else if (control_mode == AUTO) { }else if (control_mode == AUTO) {
wp_distance = wp_nav.get_wp_distance_to_destination(); wp_distance = wp_nav.get_wp_distance_to_destination();
wp_bearing = wp_nav.get_wp_bearing_to_destination();
}else{ }else{
wp_distance = 0; wp_distance = 0;
}
}
// calc_wp_bearing - calculate bearing to next waypoint for reporting and autopilot decisions
static void calc_wp_bearing()
{
// get target from loiter or wpinav controller
if (control_mode == LOITER || control_mode == CIRCLE) {
wp_bearing = wp_nav.get_loiter_bearing_to_target();
} else if (control_mode == AUTO) {
wp_bearing = wp_nav.get_wp_bearing_to_destination();
} else {
wp_bearing = 0; wp_bearing = 0;
} }
}
// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
static void calc_home_distance_and_bearing()
{
Vector3f curr = inertial_nav.get_position();
// calculate home distance and bearing // calculate home distance and bearing
if(GPS_ok()) { if (GPS_ok()) {
home_distance = pythagorous2(curr.x, curr.y); home_distance = pythagorous2(curr.x, curr.y);
home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0)); home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0));
@ -60,17 +82,3 @@ static void run_autopilot()
mission.update(); mission.update();
} }
} }
// Keeps old data out of our calculation / logs
static void reset_nav_params(void)
{
// Will be set by new command
wp_bearing = 0;
// Will be set by new command
wp_distance = 0;
// Will be set by nav or loiter controllers
lon_error = 0;
lat_error = 0;
}