Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL: use AP_RTC 

Also add a define as to whether set_time / get_time can be used
This commit is contained in:
Peter Barker 2018-02-13 23:39:37 -02:00 committed by Andrew Tridgell
parent 00153f513c
commit f5eb281efd
6 changed files with 32 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_HAL/AP_HAL_Boards.h
View File

@ -216,6 +216,10 @@
#define HAL_WITH_IO_MCU 0
#endif
#ifndef HAL_HAVE_GETTIME_SETTIME
#define HAL_HAVE_GETTIME_SETTIME 0
#endif
// this is used as a general mechanism to make a 'small' build by
// dropping little used features. We use this to allow us to keep
// FMUv2 going for as long as possible

99
libraries/AP_HAL/Util.cpp
View File

@ -60,96 +60,31 @@ int AP_HAL::Util::vsnprintf(char* str, size_t size, const char *format, va_list
return ret;
}
uint64_t AP_HAL::Util::get_system_clock_ms() const
uint64_t AP_HAL::Util::get_hw_rtc() const
{
#if defined(__APPLE__) && defined(__MACH__)
struct timeval ts;
gettimeofday(&ts, nullptr);
return ((long long)((ts.tv_sec * 1000) + (ts.tv_usec / 1000)));
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
return ST2MS(chVTGetSystemTime());
#else
return ((long long)((ts.tv_sec * 1000000) + ts.tv_usec));
#elif HAL_HAVE_GETTIME_SETTIME
struct timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
const uint64_t seconds = ts.tv_sec;
const uint64_t nanoseconds = ts.tv_nsec;
return (seconds * 1000ULL + nanoseconds/1000000ULL);
return (seconds * 1000000ULL + nanoseconds/1000ULL);
#endif
// no HW clock (or not one worth bothering with)
return 0;
}
void AP_HAL::Util::set_hw_rtc(uint64_t time_utc_usec)
{
#if HAL_HAVE_GETTIME_SETTIME
timespec ts;
ts.tv_sec = time_utc_usec/1000000ULL;
ts.tv_nsec = (time_utc_usec % 1000000ULL) * 1000ULL;
clock_settime(CLOCK_REALTIME, &ts);
#endif
}
void AP_HAL::Util::get_system_clock_utc(int32_t &hour, int32_t &min, int32_t &sec, int32_t &ms) const
{
// get time of day in ms
uint64_t time_ms = get_system_clock_ms();
// separate time into ms, sec, min, hour and days but all expressed in milliseconds
ms = time_ms % 1000;
uint32_t sec_ms = (time_ms % (60 * 1000)) - ms;
uint32_t min_ms = (time_ms % (60 * 60 * 1000)) - sec_ms - ms;
uint32_t hour_ms = (time_ms % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms;
// convert times as milliseconds into appropriate units
sec = sec_ms / 1000;
min = min_ms / (60 * 1000);
hour = hour_ms / (60 * 60 * 1000);
}
// get milliseconds from now to a target time of day expressed as hour, min, sec, ms
// match starts from first value that is not -1. I.e. specifying hour=-1, minutes=10 will ignore the hour and return time until 10 minutes past 12am (utc)
uint32_t AP_HAL::Util::get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms) const
{
// determine highest value specified (0=none, 1=ms, 2=sec, 3=min, 4=hour)
int8_t largest_element = 0;
if (hour != -1) {
largest_element = 4;
} else if (min != -1) {
largest_element = 3;
} else if (sec != -1) {
largest_element = 2;
} else if (ms != -1) {
largest_element = 1;
} else {
// exit immediately if no time specified
return 0;
}
// get start_time_ms as h, m, s, ms
int32_t curr_hour, curr_min, curr_sec, curr_ms;
get_system_clock_utc(curr_hour, curr_min, curr_sec, curr_ms);
int32_t total_delay_ms = 0;
// calculate ms to target
if (largest_element >= 1) {
total_delay_ms += ms - curr_ms;
}
if (largest_element == 1 && total_delay_ms < 0) {
return static_cast<uint32_t>(total_delay_ms += 1000);
}
// calculate sec to target
if (largest_element >= 2) {
total_delay_ms += (sec - curr_sec)*1000;
}
if (largest_element == 2 && total_delay_ms < 0) {
return static_cast<uint32_t>(total_delay_ms += (60*1000));
}
// calculate min to target
if (largest_element >= 3) {
total_delay_ms += (min - curr_min)*60*1000;
}
if (largest_element == 3 && total_delay_ms < 0) {
return static_cast<uint32_t>(total_delay_ms += (60*60*1000));
}
// calculate hours to target
if (largest_element >= 4) {
total_delay_ms += (hour - curr_hour)*60*60*1000;
}
if (largest_element == 4 && total_delay_ms < 0) {
return static_cast<uint32_t>(total_delay_ms += (24*60*60*1000));
}
// total delay in milliseconds
return static_cast<uint32_t>(total_delay_ms);
}

16
libraries/AP_HAL/Util.h
View File

@ -42,21 +42,14 @@ public:
virtual enum safety_state safety_switch_state(void) { return SAFETY_NONE; }
/*
set system clock in UTC microseconds
set HW RTC in UTC microseconds
*/
virtual void set_system_clock(uint64_t time_utc_usec) {}
virtual void set_hw_rtc(uint64_t time_utc_usec);
/*
get system clock in UTC milliseconds
get system clock in UTC microseconds
*/
uint64_t get_system_clock_ms() const;
/*
get system time in UTC hours, minutes, seconds and milliseconds
*/
void get_system_clock_utc(int32_t &hour, int32_t &min, int32_t &sec, int32_t &ms) const;
uint32_t get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms) const;
virtual uint64_t get_hw_rtc() const;
/*
get system identifier (eg. serial number)
@ -124,4 +117,5 @@ protected:
// values until the vehicle code has fully started
bool soft_armed = false;
uint64_t capabilities = 0;
};

2
libraries/AP_HAL/board/linux.h
View File

@ -364,6 +364,8 @@
#define HAL_COMPASS_LIS3MDL_NAME "lis3mdl"
#define HAL_OPTFLOW_PX4FLOW_I2C_BUS 0
#define HAL_HAVE_GETTIME_SETTIME 1
#else
#error "no Linux board subtype set"
#endif

2
libraries/AP_HAL/board/px4.h
View File

@ -10,6 +10,8 @@
#define HAL_BARO_DEFAULT HAL_BARO_PX4
#define HAL_COMPASS_DEFAULT HAL_COMPASS_PX4
#define HAL_HAVE_GETTIME_SETTIME 1
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_PX4_V1
#define HAL_STORAGE_SIZE 8192

2
libraries/AP_HAL/board/vrbrain.h
View File

@ -10,6 +10,8 @@
#define HAL_BARO_DEFAULT HAL_BARO_VRBRAIN
#define HAL_COMPASS_DEFAULT HAL_COMPASS_VRBRAIN
#define HAL_HAVE_GETTIME_SETTIME 1
#ifdef CONFIG_ARCH_BOARD_VRBRAIN_V45
#define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_VRBRAIN_V45
#define HAL_STORAGE_SIZE 8192