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

Copter: Add support for MAV_CMD_NAV_DELAY

Browse Source
This commit is contained in:
Niti Rohilla 2016-04-12 11:40:09 +05:30 committed by Randy Mackay
parent d771017c25
commit 55f66b7696
2 changed files with 38 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/Copter.h
View File

@ -379,6 +379,10 @@ private:
uint32_t brake_timeout_start;
uint32_t brake_timeout_ms;
// Delay the next navigation command
int32_t delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.)
uint32_t delay_time_start;
// Flip
Vector3f flip_orig_attitude; // original copter attitude before flip
@ -1014,6 +1018,7 @@ private:
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
#endif
void do_nav_delay(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_yaw(const AP_Mission::Mission_Command& cmd);
@ -1037,8 +1042,9 @@ private:
#if NAV_GUIDED == ENABLED
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
#endif
void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
void log_init(void);
void run_cli(AP_HAL::UARTDriver *port);

31
ArduCopter/commands_logic.cpp
View File

@ -53,6 +53,10 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
break;
#endif
case MAV_CMD_NAV_DELAY: // 94 Delay the next navigation command
do_nav_delay(cmd);
break;
//
// conditional commands
//
@ -212,6 +216,9 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
return verify_nav_guided_enable(cmd);
#endif
case MAV_CMD_NAV_DELAY:
return verify_nav_delay(cmd);
///
/// conditional commands
///
@ -514,6 +521,20 @@ void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
}
#endif // NAV_GUIDED
// do_nav_delay - Delay the next navigation command
void Copter::do_nav_delay(const AP_Mission::Mission_Command& cmd)
{
delay_time_start = millis();
if (cmd.content.nav_delay.seconds > 0) {
// relative delay
delay_time_max = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
} else {
// absolute delay to utc time
delay_time_max = hal.util->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_fmt(MAV_SEVERITY_INFO, "Delaying %u sec",(unsigned int)(delay_time_max/1000));
}
#if PARACHUTE == ENABLED
// do_parachute - configure or release parachute
@ -738,6 +759,16 @@ bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
}
#endif // NAV_GUIDED
// verify_nav_delay - check if we have waited long enough
bool Copter::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
{
if (millis() - delay_time_start > (uint32_t)MAX(delay_time_max,0)) {
delay_time_max = 0;
return true;
}
return false;
}
/********************************************************************************/
// Condition (May) commands