diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index a4e07e3ac0..402a47e870 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -620,8 +620,6 @@ private: // For example in a change altitude command, it is the altitude to change to. int32_t condition_value; - // Sometimes there is a second condition required: - int32_t condition_value2; // A starting value used to check the status of a conditional command. // For example in a delay command the condition_start records that start time for the delay uint32_t condition_start; @@ -824,6 +822,7 @@ private: bool verify_vtol_land(const AP_Mission::Mission_Command &cmd); void do_loiter_at_location(); void do_take_picture(); + bool verify_loiter_heading(bool init); void log_picture(); void exit_mission_callback(); void update_commands(void); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 1e47aba56f..0a342a8d13 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -417,14 +417,16 @@ void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd) set_next_WP(cmd.content.location); loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL; loiter_set_direction_wp(cmd); + condition_value = 1; // used to signify primary turns goal not yet met } void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd) { set_next_WP(cmd.content.location); // we set start_time_ms when we reach the waypoint - loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // units are seconds + loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // convert sec to ms loiter_set_direction_wp(cmd); + condition_value = 1; // used to signify primary time goal not yet met } void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd) @@ -471,9 +473,12 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) next_WP_loc.alt += home.alt; } - //I know I'm storing this twice -- I'm doing that on purpose -- - //see verify_loiter_alt() function + // used to signify primary turns goal not yet met when non-zero condition_value = next_WP_loc.alt; + if (condition_value == 0) { + // the value of 0 is used to signify it has been reached. Lets bump alt to 1 which is 10cm. Close enough! + condition_value = 1; + } //are lat and lon 0? if so, don't change the current wp lat/lon if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { @@ -481,9 +486,6 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) } //set loiter direction loiter_set_direction_wp(cmd); - - //must the plane be heading towards the next waypoint before breaking? - condition_value2 = LOWBYTE(cmd.p1); } /********************************************************************************/ @@ -614,32 +616,55 @@ bool Plane::verify_loiter_unlim() bool Plane::verify_loiter_time() { + bool result = false; // mission radius is always g.loiter_radius update_loiter(0); + if (loiter.start_time_ms == 0) { if (nav_controller->reached_loiter_target()) { // we've reached the target, start the timer loiter.start_time_ms = millis(); } - } else if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) { + } else if (condition_value != 0) { + // primary goal, loiter time + if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) { + // primary goal completed, initialize secondary heading goal + condition_value = 0; + result = verify_loiter_heading(true); + } + } else { + // secondary goal, loiter to heading + result = verify_loiter_heading(false); + } + + if (result) { gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER time complete"); - return true; } - return false; + return result; } bool Plane::verify_loiter_turns() { + bool result = false; uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1); update_loiter(radius); - if (loiter.sum_cd > loiter.total_cd) { - loiter.total_cd = 0; + if (condition_value != 0) { + // primary goal, loiter time + if (loiter.sum_cd > loiter.total_cd) { + // primary goal completed, initialize secondary heading goal + condition_value = 0; + result = verify_loiter_heading(true); + } + } else { + // secondary goal, loiter to heading + result = verify_loiter_heading(false); + } + + if (result) { gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER orbits complete"); - // clear the command queue; - return true; } - return false; + return result; } /* @@ -649,70 +674,26 @@ bool Plane::verify_loiter_turns() */ bool Plane::verify_loiter_to_alt() { - uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1); - update_loiter(radius); + bool result = false; + update_loiter(mission.get_current_nav_cmd().p1); //has target altitude been reached? if (condition_value != 0) { + // primary goal, loiter alt if (labs(condition_value - current_loc.alt) < 500) { - //Only have to reach the altitude once -- that's why I need - //this global condition variable. - // - //This is in case of altitude oscillation when still trying - //to reach the target heading. + // primary goal completed, initialize secondary heading goal condition_value = 0; - } else { - return false; + result = verify_loiter_heading(true); } + } else { + // secondary goal, loiter to heading + result = verify_loiter_heading(false); } - - //has target heading been reached? - if (condition_value2 != 0) { - //Get the lat/lon of next Nav waypoint after this one: - AP_Mission::Mission_Command next_nav_cmd; - if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, - next_nav_cmd)) { - //no next waypoint to shoot for -- go ahead and break out of loiter - return true; - } - if (get_distance(next_WP_loc, next_nav_cmd.content.location) < labs(g.loiter_radius)) { - /* Whenever next waypoint is within the loiter radius, - maintaining loiter would prevent us from ever pointing toward the next waypoint. - Hence break out of loiter immediately - */ - return true; - } - - // Bearing in radians - int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location); - - // get current heading. We should probably be using the ground - // course instead to improve the accuracy in wind - int32_t heading_cd = ahrs.yaw_sensor; - - int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); - - /* - Check to see if the the plane is heading toward the land - waypoint - We use 10 degrees of slop so that we can handle 100 - degrees/second of yaw - */ - if (labs(heading_err_cd) <= 1000) { - //Want to head in a straight line from _here_ to the next waypoint. - //DON'T want to head in a line from the center of the loiter to - //the next waypoint. - //Therefore: mark the "last" (next_wp_loc is about to be updated) - //wp lat/lon as the current location. - next_WP_loc = current_loc; - return true; - } else { - return false; - } - } - - return true; + if (result) { + gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER alt complete"); + } + return result; } bool Plane::verify_RTL() @@ -1032,3 +1013,51 @@ void Plane::exit_mission_callback() start_command(auto_rtl_command); } } + +bool Plane::verify_loiter_heading(bool init) +{ + //Get the lat/lon of next Nav waypoint after this one: + AP_Mission::Mission_Command next_nav_cmd; + if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, + next_nav_cmd)) { + //no next waypoint to shoot for -- go ahead and break out of loiter + return true; + } + + if (get_distance(next_WP_loc, next_nav_cmd.content.location) < labs(g.loiter_radius)) { + /* Whenever next waypoint is within the loiter radius, + maintaining loiter would prevent us from ever pointing toward the next waypoint. + Hence break out of loiter immediately + */ + return true; + } + + // Bearing in degrees + int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location); + + // get current heading. + int32_t heading_cd = gps.ground_course_cd(); + + int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); + + if (init) { + loiter.total_cd = wrap_360_cd(bearing_cd - heading_cd); + loiter.sum_cd = 0; + } + + /* + Check to see if the the plane is heading toward the land + waypoint. We use 20 degrees (+/-10 deg) of margin so that + we can handle 200 degrees/second of yaw. Allow turn count + to stop it too to ensure we don't loop around forever in + case high winds are forcing us beyond 200 deg/sec at this + particular moment. + */ + if (labs(heading_err_cd) <= 1000 || + loiter.sum_cd > loiter.total_cd) { + // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp + next_WP_loc = current_loc; + return true; + } + return false; +}