From 078d90132c9a33b802d6b23f01f8b45d24327efc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 9 Mar 2018 13:14:51 -0300 Subject: [PATCH] Copter: use AP_RTC MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: patrickelectric@gmail.com Copter: AP_GPS now sets the system time directly Copter: use AP_RTC for mission delays Copter: Remove ap_t bits counter (Patrick José Pereira) Change bits in ap_t to bool and add a static assert --- ArduCopter/ArduCopter.cpp | 3 --- ArduCopter/Copter.h | 9 +++++---- ArduCopter/commands.cpp | 23 ----------------------- ArduCopter/defines.h | 2 +- ArduCopter/mode_auto.cpp | 2 +- 5 files changed, 7 insertions(+), 32 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index d90ab1d0ae..a613699eae 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -473,9 +473,6 @@ void Copter::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/ArduCopter/Copter.h b/ArduCopter/Copter.h index 18b1c18e0e..5a12422241 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -323,7 +323,7 @@ private: uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete) uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock - uint8_t system_time_set : 1; // 16 // true if the system time has been set from the GPS + uint8_t system_time_set_unused : 1; // 16 // true if the system time has been set from the GPS uint8_t gps_glitching : 1; // 17 // true if GPS glitching is affecting navigation accuracy uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use uint8_t motor_emergency_stop : 1; // 21 // motor estop switch, shuts off motors when enabled @@ -340,6 +340,8 @@ private: ap_t ap; + static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t"); + // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, control_mode_t control_mode; @@ -376,7 +378,7 @@ private: // intertial nav alt when we armed float arming_altitude_m; - + // board specific config AP_BoardConfig BoardConfig; @@ -622,7 +624,7 @@ private: // set when we are upgrading parameters from 3.4 bool upgrading_frame_params; - + static const AP_Scheduler::Task scheduler_tasks[]; static const AP_Param::Info var_info[]; static const struct LogStructure log_structure[]; @@ -709,7 +711,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(); // compassmot.cpp MAV_RESULT mavlink_compassmot(mavlink_channel_t chan); diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index 3dd838cad2..5faf1c677a 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -124,26 +124,3 @@ bool Copter::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 Copter::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) { - // set system clock for log timestamps - uint64_t gps_timestamp = gps.time_epoch_usec(); - - 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/ArduCopter/defines.h b/ArduCopter/defines.h index 4e553bef1b..fb610bb12a 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -334,7 +334,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_INIT_SIMPLE_BEARING 9 #define DATA_ARMED 10 #define DATA_DISARMED 11 diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 6764504adf..bea75456ca 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1310,7 +1310,7 @@ void Copter::ModeAuto::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)); }