From 018e766a3fecceef1b87bb21d9cf87c6634ef8b5 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 9 Aug 2019 11:42:00 -0700 Subject: [PATCH] Sub: NAV_Delay variable timers to be all unsigned and labeled as _ms --- ArduSub/Sub.h | 4 ++-- ArduSub/commands_logic.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index efae5a19c0..2f89fca3e0 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -324,8 +324,8 @@ private: uint32_t loiter_time; // How long have we been loitering - The start time in millis // Delay the next navigation command - int32_t nav_delay_time_max; // used for delaying the navigation commands - uint32_t nav_delay_time_start; + uint32_t nav_delay_time_max_ms; // used for delaying the navigation commands + uint32_t nav_delay_time_start_ms; // Battery Sensors AP_BattMonitor battery{MASK_LOG_CURRENT, diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index d1149137fb..6e7dcddcc3 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -473,16 +473,16 @@ void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) // do_nav_delay - Delay the next navigation command void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd) { - nav_delay_time_start = AP_HAL::millis(); + nav_delay_time_start_ms = AP_HAL::millis(); if (cmd.content.nav_delay.seconds > 0) { // relative delay - nav_delay_time_max = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds + 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 = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); + 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",(unsigned int)(nav_delay_time_max/1000)); + gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec",nav_delay_time_max_ms/1000); } #if NAV_GUIDED == ENABLED @@ -657,8 +657,8 @@ bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) // verify_nav_delay - check if we have waited long enough bool Sub::verify_nav_delay(const AP_Mission::Mission_Command& cmd) { - if (AP_HAL::millis() - nav_delay_time_start > (uint32_t)MAX(nav_delay_time_max, 0)) { - nav_delay_time_max = 0; + if (AP_HAL::millis() - nav_delay_time_start_ms > nav_delay_time_max_ms) { + nav_delay_time_max_ms = 0; return true; } return false;