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

Rover: add support for NAV_DELAY misison item

Browse Source
This commit is contained in:
Tom Pittenger 2019-08-09 11:42:19 -07:00 committed by Tom Pittenger
parent 466681a94e
commit 1bb332382c
2 changed files with 47 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
APMrover2/mode.h
View File

@ -302,6 +302,8 @@ private:
bool 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_guided_enable(const AP_Mission::Mission_Command& cmd);
void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd); void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd);
void do_nav_delay(const AP_Mission::Mission_Command& cmd);
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
bool verify_RTL(); bool verify_RTL();
bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd); bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
@ -346,6 +348,11 @@ private:
// For example in a delay command the condition_start records that start time for the delay // For example in a delay command the condition_start records that start time for the delay
int32_t condition_start; int32_t condition_start;
// Delay the next navigation command
uint32_t nav_delay_time_max_ms; // used for delaying the navigation commands
uint32_t nav_delay_time_start_ms;
AutoSubMode nav_delay_submode_backup; // back up sub_mode
}; };

40
APMrover2/mode_auto.cpp
View File

@ -348,6 +348,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
do_nav_set_yaw_speed(cmd); do_nav_set_yaw_speed(cmd);
break; break;
case MAV_CMD_NAV_DELAY: // 93 Delay the next navigation command
do_nav_delay(cmd);
break;
// Conditional commands // Conditional commands
case MAV_CMD_CONDITION_DELAY: case MAV_CMD_CONDITION_DELAY:
do_wait_delay(cmd); do_wait_delay(cmd);
@ -472,6 +476,9 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_GUIDED_ENABLE: case MAV_CMD_NAV_GUIDED_ENABLE:
return verify_nav_guided_enable(cmd); return verify_nav_guided_enable(cmd);
case MAV_CMD_NAV_DELAY:
return verify_nav_delay(cmd);
case MAV_CMD_CONDITION_DELAY: case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay(); return verify_wait_delay();
@ -538,6 +545,27 @@ bool ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_sto
return true; return true;
} }
// do_nav_delay - Delay the next navigation command
void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
{
nav_delay_time_start_ms = millis();
// act like we're just doing a heading hold that we instantly achieve.
// the behavior of this will put a boat into loiter, else stop
nav_delay_submode_backup = _submode;
_submode = Auto_HeadingAndSpeed;
_reached_heading = true;
if (cmd.content.nav_delay.seconds > 0) {
// relative delay
nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
} else {
// absolute delay to utc time
nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0);
}
gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec",nav_delay_time_max_ms/1000);
}
// start guided within auto to allow external navigation system to control vehicle // start guided within auto to allow external navigation system to control vehicle
void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
{ {
@ -601,6 +629,18 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
} }
} }
// verify_nav_delay - check if we have waited long enough
bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
{
if (millis() - nav_delay_time_start_ms > nav_delay_time_max_ms) {
nav_delay_time_max_ms = 0;
_submode = nav_delay_submode_backup;
return true;
}
return false;
}
bool ModeAuto::verify_RTL() bool ModeAuto::verify_RTL()
{ {
return reached_destination(); return reached_destination();