Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

Rover: Adding LOITER_TIME and LOITER_TIME_UNLIM functionality

Browse Source
This commit is contained in:
Grant Morphett 2017-03-09 20:14:55 +11:00
parent 79a0709054
commit ecc89f0b7e
4 changed files with 159 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
APMrover2/Rover.h
View File

@ -360,9 +360,10 @@ private:
static const LogStructure log_structure[];
// Loiter control
uint16_t loiter_time_max; // How long we should loiter at the nav_waypoint (time in seconds)
uint32_t loiter_time; // How long have we been loitering - The start time in millis
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
bool previously_reached_wp; // set to true if we have EVER reached the waypoint
float distance_past_wp; // record the distance we have gone past the wp
private:
@ -495,6 +496,10 @@ private:
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
void do_within_distance(const AP_Mission::Mission_Command& cmd);
void do_change_speed(const AP_Mission::Mission_Command& cmd);
@ -502,6 +507,8 @@ private:
void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
void do_digicam_control(const AP_Mission::Mission_Command& cmd);
void init_capabilities(void);
bool in_stationary_loiter(void);
void set_loiter_active(const AP_Mission::Mission_Command& cmd);
public:
bool print_log_menu(void);

30
APMrover2/Steering.cpp
View File

@ -81,6 +81,24 @@ bool Rover::use_pivot_steering(void)
}
/*
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 == 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;
}
/*
calculate the throtte for auto-throttle modes
*/
@ -88,7 +106,7 @@ void Rover::calc_throttle(float target_speed)
{
// If not autostarting OR we are loitering at a waypoint
// then set the throttle to minimum
if (!auto_check_trigger() || ((loiter_time > 0) && (control_mode == AUTO))) {
if (!auto_check_trigger() || in_stationary_loiter()) {
channel_throttle->servo_out = g.throttle_min.get();
return;
}
@ -166,7 +184,13 @@ void Rover::calc_lateral_acceleration()
{
switch (control_mode) {
case AUTO:
nav_controller->update_waypoint(prev_WP, next_WP);
// If we have reached the waypoint previously navigate
// back to it from our current position
if (previously_reached_wp && (loiter_duration > 0)) {
nav_controller->update_waypoint(current_loc, next_WP);
} else {
nav_controller->update_waypoint(prev_WP, next_WP);
}
break;
case RTL:
@ -199,7 +223,7 @@ void Rover::calc_lateral_acceleration()
void Rover::calc_nav_steer()
{
// check to see if the rover is loitering
if ((loiter_time > 0) && (control_mode == AUTO)) {
if (in_stationary_loiter()) {
channel_steer->servo_out = 0;
return;
}

147
APMrover2/commands_logic.cpp
View File

@ -31,6 +31,14 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
do_RTL();
break;
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
do_loiter_unlimited(cmd);
break;
case MAV_CMD_NAV_LOITER_TIME:
do_loiter_time(cmd);
break;
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
do_wait_delay(cmd);
@ -153,21 +161,38 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited(cmd);
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time(cmd);
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_SET_ROI:
return true;
default:
if (cmd.id > MAV_CMD_CONDITION_LAST) {
// this is a command that doesn't require verify
return true;
}
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command"));
// error message
gcs_send_text_fmt(PSTR("Skipping invalid cmd #%i"), cmd.id);
// return true if we do not recognize the command so that we move on to the next command
return true;
}
return false;
}
/********************************************************************************/
@ -183,62 +208,111 @@ void Rover::do_RTL(void)
void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
// 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_time = 0;
loiter_start_time = 0;
// this is the delay, stored in seconds
loiter_time_max = abs(cmd.p1);
loiter_duration = cmd.p1;
// this is the distance we travel past the waypoint - not there yet so 0 initially
distance_past_wp = 0;
set_next_WP(cmd.content.location);
Location cmdloc = cmd.content.location;
set_next_WP(cmdloc);
}
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_loiter_time - initiate loitering at a point for a given time period
// if the vehicle is moved off the loiter point (i.e. a boat in a current)
// 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);
}
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
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 need to loiter at this waypoint
if (loiter_time_max > 0) {
if (loiter_time == 0) { // check if we are just starting loiter
gcs_send_text_fmt(PSTR("Reached Waypoint #%i - Loiter for %i seconds"),
// 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_fmt(PSTR("Reached waypoint #%i. Loiter for %i seconds"),
(unsigned)cmd.index,
(unsigned)loiter_time_max);
(unsigned)loiter_duration);
// record the current time i.e. start timer
loiter_time = millis();
loiter_start_time = millis();
previously_reached_wp = true;
}
distance_past_wp = wp_distance;
// Check if we have loiter long enough
if (((millis() - loiter_time) / 1000) < loiter_time_max) {
if (((millis() - loiter_start_time) / 1000) < loiter_duration) {
return false;
}
} else {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
gcs_send_text_fmt(PSTR("Reached waypoint #%i. Distance %um"),
(unsigned)cmd.index,
(unsigned)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;
}
// have we gone past the waypoint?
// We should always go through the waypoint i.e. the above code
// first before we go past it.
if (location_passed_point(current_loc, prev_WP, next_WP)) {
// check if we have gone futher past the wp then last time and output new message if we have
if ((uint32_t)distance_past_wp != (uint32_t)get_distance(current_loc, next_WP)) {
distance_past_wp = get_distance(current_loc, next_WP);
gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
// 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) {
gcs_send_text_fmt(PSTR("Reached waypoint #%i. Loiter for %i seconds"),
(unsigned)cmd.index,
(unsigned)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 distance to the WP has changed and output new message if it has
float dist_to_wp = get_distance(current_loc, next_WP);
if ((uint32_t)distance_past_wp != (uint32_t)dist_to_wp) {
distance_past_wp = dist_to_wp;
gcs_send_text_fmt(PSTR("Passed waypoint #%i. Distance %um"),
(unsigned)cmd.index,
(unsigned)distance_past_wp);
}
// Check if we need to loiter at this waypoint
if (loiter_time_max > 0) {
if (((millis() - loiter_time) / 1000) < loiter_time_max) {
if (loiter_duration > 0) {
if (((millis() - loiter_start_time) / 1000) < loiter_duration) {
return false;
}
}
distance_past_wp = 0;
// set loiter_duration to 0 so we know we aren't or have finished loitering
loiter_duration = 0;
return true;
}
@ -264,6 +338,27 @@ bool Rover::verify_RTL()
return false;
}
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;
}
// verify_loiter_time - check if we have loitered long enough
bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
{
bool result = verify_nav_wp(cmd);
if (result) {
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Finished active loiter\n"));
// if we have finished active loitering - turn it off
active_loiter = false;
}
return result;
}
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/

2
APMrover2/system.cpp
View File

@ -282,7 +282,7 @@ void Rover::set_mode(enum mode mode)
// If we are changing out of AUTO mode reset the loiter timer
if (control_mode == AUTO)
loiter_time = 0;
loiter_start_time = 0;
control_mode = mode;
throttle_last = 0;