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

Rover: remove active-loiter and set-auto-wp

Browse Source 
these are now handled by auto mode
This commit is contained in:
Randy Mackay 2017-08-02 15:56:31 +09:00
parent 974453607e
commit 1b19ee865d
4 changed files with 26 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
APMrover2/Rover.h
View File

@ -385,7 +385,6 @@ private:
// Loiter control
uint16_t loiter_duration; // How long we should loiter at the nav_waypoint (time in seconds)
uint32_t loiter_start_time; // How long have we been loitering - The start time in millis
bool active_loiter; // TRUE if we actively return to the loitering waypoint if we drift off
float distance_past_wp; // record the distance we have gone past the wp
bool previously_reached_wp; // set to true if we have EVER reached the waypoint
@ -479,12 +478,10 @@ private:
void load_parameters(void);
bool use_pivot_steering(float yaw_error_cd);
void set_servos(void);
void set_auto_WP(const struct Location& loc);
void update_home_from_EKF();
bool set_home_to_current_location(bool lock);
bool set_home(const Location& loc, bool lock);
void set_system_time_from_GPS();
void restart_nav();
void exit_mission();
void do_RTL(void);
bool verify_RTL();
@ -539,7 +536,7 @@ private:
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_at_dest);
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
@ -563,7 +560,6 @@ private:
bool motor_active();
void update_home();
void accel_cal_update(void);
bool in_stationary_loiter(void);
void crash_check();
#if ADVANCED_FAILSAFE == ENABLED
void afs_fs_check(void);

18
APMrover2/Steering.cpp
View File

@ -30,24 +30,6 @@ bool Rover::use_pivot_steering(float yaw_error_cd)
return pivot_steering_active;
}
/*
test if we are loitering AND should be stopped at a waypoint
*/
bool Rover::in_stationary_loiter()
{
// Confirm we are in AUTO mode and need to loiter for a time period
if ((loiter_start_time > 0) && (control_mode == &mode_auto)) {
// Check if active loiter is enabled AND we are outside the waypoint loiter radius
// then the vehicle still needs to move so return false
if (active_loiter && (wp_distance > g.waypoint_radius)) {
return false;
}
return true;
}
return false;
}
/*****************************************
Set the flight control servos based on the current calculated values
*****************************************/

34
APMrover2/commands.cpp
View File

@ -1,31 +1,4 @@
#include "Rover.h"
/*
* set_auto_WP - sets the target location the vehicle should drive to in Auto mode
*/
void Rover::set_auto_WP(const struct Location& loc)
{
// copy the current WP into the OldWP slot
// ---------------------------------------
prev_WP = next_WP;
// Load the next_WP slot
// ---------------------
next_WP = loc;
// are we already past the waypoint? This happens when we jump
// waypoints, and it can cause us to skip a waypoint. If we are
// past the waypoint when we start on a leg, then use the current
// location as the previous waypoint, to prevent immediately
// considering the waypoint complete
if (location_passed_point(current_loc, prev_WP, next_WP)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Resetting previous WP");
prev_WP = current_loc;
}
// this is handy for the groundstation
wp_totalDistance = get_distance(current_loc, next_WP);
wp_distance = wp_totalDistance;
}
// checks if we should update ahrs home position from the EKF's position
void Rover::update_home_from_EKF()
@ -134,13 +107,6 @@ void Rover::set_system_time_from_GPS()
}
}
void Rover::restart_nav()
{
g.pidSpeedThrottle.reset_I();
prev_WP = current_loc;
mission.start_or_resume();
}
/*
update home location from GPS
this is called as long as we have 3D lock and the arming switch is

100
APMrover2/commands_logic.cpp
View File

@ -17,7 +17,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
switch (cmd.id) {
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp(cmd);
do_nav_wp(cmd, false);
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
@ -122,10 +122,9 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
}
// exit_mission - callback function called from ap-mission when the mission has completed
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
void Rover::exit_mission()
{
gcs().send_text(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO. Setting HOLD");
gcs().send_text(MAV_SEVERITY_NOTICE, "Mission Complete");
set_mode(mode_hold, MODE_REASON_MISSION_END);
}
@ -208,7 +207,7 @@ void Rover::do_RTL(void)
set_mode(mode_rtl, MODE_REASON_MISSION_COMMAND);
}
void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_at_dest)
{
// just starting so we haven't previously reached the waypoint
previously_reached_wp = false;
@ -224,14 +223,12 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_auto_WP(cmdloc);
mode_auto.set_desired_location(cmdloc, stay_active_at_dest);
}
void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
active_loiter = true;
do_nav_wp(cmd);
loiter_duration = 100; // an arbitrary large loiter time
do_nav_wp(cmd, true);
}
// do_loiter_time - initiate loitering at a point for a given time period
@ -239,8 +236,7 @@ void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
// then the vehicle will actively return to the loiter coords.
void Rover::do_loiter_time(const AP_Mission::Mission_Command& cmd)
{
active_loiter = true;
do_nav_wp(cmd);
do_nav_wp(cmd, true);
}
// do_set_yaw_speed - turn to a specified heading and achieve and given speed
@ -266,77 +262,35 @@ void Rover::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
/********************************************************************************/
bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
// Have we reached the waypoint i.e. are we within waypoint_radius of the waypoint
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
// check if we are loitering at this waypoint - the message sent to the GCS is different
if (loiter_duration > 0) {
// Check if this is the first time we have reached the waypoint
if (!previously_reached_wp) {
gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
static_cast<uint32_t>(cmd.index),
static_cast<uint32_t>(loiter_duration));
// record the current time i.e. start timer
loiter_start_time = millis();
previously_reached_wp = true;
}
distance_past_wp = wp_distance;
// Check if we have loiter long enough
if (((millis() - loiter_start_time) / 1000) < loiter_duration) {
return false;
}
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u. Distance %dm",
static_cast<uint32_t>(cmd.index),
static_cast<int32_t>(fabsf(get_distance(current_loc, next_WP))));
}
// set loiter_duration to 0 so we know we aren't or have finished loitering
loiter_duration = 0;
return true;
// exit immediately if we haven't reached the destination
if (!mode_auto.reached_destination()) {
return false;
}
// have we gone past the waypoint?
// We should always go through the waypoint i.e. the above code
// first before we go past it but sometimes we don't.
// OR have we reached the waypoint previously be we aren't actively loitering
// This second check is required for when we roll past the waypoint radius
if (location_passed_point(current_loc, prev_WP, next_WP) ||
(!active_loiter && previously_reached_wp)) {
// As we have passed the waypoint navigation needs to be done from current location
prev_WP = current_loc;
// Check if this is the first time we have reached the waypoint even though we have gone past it
if (!previously_reached_wp) {
// Check if this is the first time we have noticed reaching the waypoint
if (!previously_reached_wp) {
previously_reached_wp = true;
// check if we are loitering at this waypoint - the message sent to the GCS is different
if (loiter_duration > 0) {
// send message including loiter time
gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
static_cast<uint32_t>(cmd.index),
static_cast<uint32_t>(loiter_duration));
// record the current time i.e. start timer
loiter_start_time = millis();
previously_reached_wp = true;
distance_past_wp = wp_distance;
} else {
// send simpler message to GCS
gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u", static_cast<uint32_t>(cmd.index));
}
// check if distance to the WP has changed and output new message if it has
const float dist_to_wp = get_distance(current_loc, next_WP);
if (!is_equal(distance_past_wp, dist_to_wp)) {
distance_past_wp = dist_to_wp;
gcs().send_text(MAV_SEVERITY_INFO, "Passed waypoint #%u. Distance %dm",
static_cast<uint32_t>(cmd.index),
static_cast<int32_t>(fabsf(distance_past_wp)));
}
// Check if we need to loiter at this waypoint
if (loiter_duration > 0) {
if (((millis() - loiter_start_time) / 1000) < loiter_duration) {
return false;
}
}
// set loiter_duration to 0 so we know we aren't or have finished loitering
loiter_duration = 0;
return true;
}
return false;
// Check if we have loitered long enough
if (loiter_duration == 0) {
return true;
} else {
return (((millis() - loiter_start_time) / 1000) >= loiter_duration);
}
}
bool Rover::verify_RTL()
@ -360,8 +314,6 @@ bool Rover::verify_RTL()
bool Rover::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
// Continually set loiter start time to now so it never finishes
loiter_start_time += millis();
verify_nav_wp(cmd);
return false;
}
@ -372,8 +324,6 @@ bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
const bool result = verify_nav_wp(cmd);
if (result) {
gcs().send_text(MAV_SEVERITY_WARNING, "Finished active loiter\n");
// if we have finished active loitering - turn it off
active_loiter = false;
}
return result;
}