From 4cc236b8a7d63a4594f44580501b8259d01b06a0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 9 Mar 2018 13:15:08 -0300 Subject: [PATCH] Sub: use AP_RTC Sub: AP_GPS now sets the system time directly Sub: use AP_RTC for delays in missions --- ArduSub/ArduSub.cpp | 3 --- ArduSub/Sub.h | 2 -- ArduSub/commands.cpp | 23 ----------------------- ArduSub/commands_logic.cpp | 4 +++- ArduSub/defines.h | 2 +- 5 files changed, 4 insertions(+), 30 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 49b34c015d..0dedf063e3 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -309,9 +309,6 @@ void Sub::update_GPS(void) } if (gps_updated) { - // set system time if necessary - set_system_time_from_GPS(); - #if CAMERA == ENABLED camera.update(); #endif diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index ca10912c1f..4cfb9b6434 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -234,7 +234,6 @@ private: uint8_t compass_mot : 1; // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // true if we are currently performing the motors test uint8_t initialised : 1; // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes - uint8_t system_time_set : 1; // true if the system time has been set from the GPS uint8_t gps_base_pos_set : 1; // true when the gps base position has been set (used for RTK gps only) uint8_t at_bottom : 1; // true if we are at the bottom uint8_t at_surface : 1; // true if we are at the surface @@ -519,7 +518,6 @@ private: bool set_home_to_current_location(bool lock); bool set_home(const Location& loc, bool lock); bool far_from_EKF_origin(const Location& loc); - void set_system_time_from_GPS(); void exit_mission(); bool verify_loiter_unlimited(); bool verify_loiter_time(); diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index 6a984655d8..7eece9d7cc 100644 --- a/ArduSub/commands.cpp +++ b/ArduSub/commands.cpp @@ -112,26 +112,3 @@ bool Sub::far_from_EKF_origin(const Location& loc) // close enough to origin return false; } - -// checks if we should update ahrs/RTL home position from GPS -void Sub::set_system_time_from_GPS() -{ - // exit immediately if system time already set - if (ap.system_time_set) { - return; - } - - // if we have a 3d lock and valid location - if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { - uint64_t gps_timestamp = gps.time_epoch_usec(); - - // set system clock for log timestamps - hal.util->set_system_clock(gps_timestamp); - - // update signing timestamp - GCS_MAVLINK::update_signing_timestamp(gps_timestamp); - - ap.system_time_set = true; - Log_Write_Event(DATA_SYSTEM_TIME_SET); - } -} diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 691fc6d3ca..69895cb9ec 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -1,5 +1,7 @@ #include "Sub.h" +#include + static enum AutoSurfaceState auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION; // start_command - this function will be called when the ap_mission lib wishes to start a new command @@ -526,7 +528,7 @@ void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd) nav_delay_time_max = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds } else { // absolute delay to utc time - nav_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); + 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); } gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec",(unsigned int)(nav_delay_time_max/1000)); } diff --git a/ArduSub/defines.h b/ArduSub/defines.h index c3313ab317..63e46248ef 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -140,7 +140,7 @@ enum LoggingParameters { // DATA - event logging #define DATA_AP_STATE 7 -#define DATA_SYSTEM_TIME_SET 8 +// 8 was DATA_SYSTEM_TIME_SET #define DATA_ARMED 10 #define DATA_DISARMED 11 #define DATA_LOST_GPS 19