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

Rover: handle AR_WPNav failing to set desired location

This commit is contained in:
Peter Barker 2019-05-09 10:54:40 +10:00 committed by Randy Mackay
parent 20152dbdb7
commit 40f3f414cd
6 changed files with 62 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
APMrover2/GCS_Mavlink.cpp
View File

@ -511,8 +511,7 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
}
// make any new wp uploaded instant (in case we are already in Guided mode)
rover.mode_guided.set_desired_location(cmd.content.location);
return true;
return rover.mode_guided.set_desired_location(cmd.content.location);
}
void GCS_MAVLINK_Rover::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
@ -824,7 +823,10 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
// set guided mode targets
if (!pos_ignore) {
// consume position target
rover.mode_guided.set_desired_location(target_loc);
if (!rover.mode_guided.set_desired_location(target_loc)) {
// GCS will need to monitor desired location to
// see if they are having an effect.
}
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
@ -926,7 +928,10 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
// set guided mode targets
if (!pos_ignore) {
// consume position target
rover.mode_guided.set_desired_location(target_loc);
if (!rover.mode_guided.set_desired_location(target_loc)) {
// GCS will just need to look at desired location
// outputs to see if it having an effect.
}
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);

8
APMrover2/mode.cpp
View File

@ -196,13 +196,17 @@ float Mode::get_desired_lat_accel() const
}
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
bool Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd);
if (!g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
return false;
}
// initialise distance
_distance_to_destination = g2.wp_nav.get_distance_to_destination();
_reached_destination = false;
return true;
}
// set desired heading and speed

8
APMrover2/mode.h
View File

@ -106,7 +106,7 @@ public:
// set desired location (used in Guided, Auto)
// next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn)
virtual void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN);
virtual bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED;
// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
virtual bool reached_destination() const { return true; }
@ -253,7 +253,7 @@ public:
float get_distance_to_destination() const override;
// set desired location
void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override;
bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
bool reached_destination() const override;
// heading and speed control
@ -294,7 +294,7 @@ private:
bool verify_command(const AP_Mission::Mission_Command& cmd);
void do_RTL(void);
void do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination);
bool do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination);
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd);
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
@ -367,7 +367,7 @@ public:
bool reached_destination() const override;
// set desired location, heading and speed
void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override;
bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
// set desired heading-delta, turn-rate and speed

48
APMrover2/mode_auto.cpp
View File

@ -11,12 +11,14 @@ bool ModeAuto::_enter()
return false;
}
// init location target
if (!g2.wp_nav.set_desired_location(rover.current_loc)) {
return false;
}
// initialise waypoint speed
g2.wp_nav.set_desired_speed_to_default();
// init location target
g2.wp_nav.set_desired_location(rover.current_loc);
// other initialisation
auto_triggered = false;
@ -130,12 +132,16 @@ float ModeAuto::get_distance_to_destination() const
}
// set desired location to drive to
void ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
bool ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
// call parent
Mode::set_desired_location(destination, next_leg_bearing_cd);
if (!Mode::set_desired_location(destination, next_leg_bearing_cd)) {
return false;
}
_submode = Auto_WP;
return true;
}
// return true if vehicle has reached or even passed destination
@ -299,8 +305,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
switch (cmd.id) {
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp(cmd, false);
break;
return do_nav_wp(cmd, false);
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
@ -308,8 +313,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
case MAV_CMD_NAV_LOITER_TIME: // Loiter for specified time
do_nav_wp(cmd, true);
break;
return do_nav_wp(cmd, true);
case MAV_CMD_NAV_GUIDED_ENABLE: // accept navigation commands from external nav computer
do_nav_guided_enable(cmd);
@ -481,17 +485,8 @@ void ModeAuto::do_RTL(void)
start_RTL();
}
void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
bool ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
{
// just starting so we haven't previously reached the waypoint
previously_reached_wp = false;
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_start_time = 0;
// this is the delay, stored in seconds
loiter_duration = cmd.p1;
// get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot)
// in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point
float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN;
@ -502,7 +497,20 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_sto
// retrieve and sanitize target location
Location cmdloc = cmd.content.location;
cmdloc.sanitize(rover.current_loc);
set_desired_location(cmdloc, next_leg_bearing_cd);
if (!set_desired_location(cmdloc, next_leg_bearing_cd)) {
return false;
}
// just starting so we haven't previously reached the waypoint
previously_reached_wp = false;
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_start_time = 0;
// this is the delay, stored in seconds
loiter_duration = cmd.p1;
return true;
}
// start guided within auto to allow external navigation system to control vehicle

4
APMrover2/mode_guided.cpp
View File

@ -146,7 +146,7 @@ bool ModeGuided::reached_destination() const
}
// set desired location
void ModeGuided::set_desired_location(const struct Location& destination,
bool ModeGuided::set_desired_location(const struct Location& destination,
float next_leg_bearing_cd)
{
if (g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
@ -154,7 +154,9 @@ void ModeGuided::set_desired_location(const struct Location& destination,
// handle guided specific initialisation and logging
_guided_mode = ModeGuided::Guided_WP;
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f));
return true;
}
return false;
}
// set desired attitude

20
APMrover2/mode_rtl.cpp
View File

@ -8,6 +8,18 @@ bool ModeRTL::_enter()
return false;
}
// set target to the closest rally point or home
#if AP_RALLY == ENABLED
if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) {
return false;
}
#else
// set destination
if (!g2.wp_nav.set_desired_location(rover.home)) {
return false;
}
#endif
// initialise waypoint speed
if (is_positive(g2.rtl_speed)) {
g2.wp_nav.set_desired_speed(g2.rtl_speed);
@ -15,14 +27,6 @@ bool ModeRTL::_enter()
g2.wp_nav.set_desired_speed_to_default();
}
// set target to the closest rally point or home
#if AP_RALLY == ENABLED
g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt));
#else
// set destination
g2.wp_nav.set_desired_location(rover.home);
#endif
sent_notification = false;
return true;