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

HAL_ChibiOS: fixed handling of 16bit timer wrap

Browse Source 
this fixes #12948
This commit is contained in:
Andrew Tridgell 2019-12-08 22:47:00 +11:00 committed by Randy Mackay
parent d965cd7a69
commit 35dd28ec88
1 changed files with 54 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
View File

@ -13,60 +13,84 @@
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// High Resolution Timer
/*
High Resolution Timer code. This provides support for 32 and 64 bit
returns for micros and millis functions
*/
#include "ch.h"
#include "hal.h"
#include "hrt.h"
#include <stdint.h>
static uint64_t timer_base_us64;
#if CH_CFG_ST_RESOLUTION == 16
static uint32_t timer_base_us32;
#pragma GCC optimize("O2")
/*
we have 4 possible configurations of boards, made up of boards that
have the following properties:
CH_CFG_ST_RESOLUTION = 16 or 32
CH_CFG_ST_FREQUENCY = 1000 or 1000000
To keep as much code in common as possible we create a function
system_time_u32_us() for all boards which gives system time since
boot in microseconds, and which wraps at 0xFFFFFFFF.
On top of this base function we build get_systime_us32() which has
the same property, but which also maintains timer_base_us64 to allow
for micros64()
*/
#if CH_CFG_ST_FREQUENCY != 1000000U && CH_CFG_ST_FREQUENCY != 1000U
#error "unsupported tick frequency"
#endif
static uint32_t timer_base_ms;
static volatile systime_t last_systime;
#if CH_CFG_ST_RESOLUTION == 16
static uint32_t get_systime_us32(void)
static uint32_t system_time_u32_us(void)
{
systime_t now = chVTGetSystemTimeX();
#if CH_CFG_ST_FREQUENCY != 1000000
now *= (1000000UL / CH_CFG_ST_FREQUENCY);
#if CH_CFG_ST_FREQUENCY == 1000U
now *= 1000U;
#endif
if (now < last_systime) {
uint32_t last_u32 = timer_base_us32;
timer_base_us32 += (uint32_t)TIME_MAX_SYSTIME;
if (timer_base_us32 < last_u32) {
timer_base_us64 += ((uint32_t)-1);
timer_base_ms += ((uint32_t)-1)/1000;
}
}
static systime_t last_systime;
static uint32_t timer_base_us32;
uint16_t dt = now - last_systime;
last_systime = now;
return timer_base_us32 + (uint32_t)now;
timer_base_us32 += dt;
return timer_base_us32;
}
#elif CH_CFG_ST_RESOLUTION == 32
static uint32_t get_systime_us32(void)
static uint32_t system_time_u32_us(void)
{
systime_t now = chVTGetSystemTimeX();
#if CH_CFG_ST_FREQUENCY != 1000000
now *= (1000000UL / CH_CFG_ST_FREQUENCY);
#if CH_CFG_ST_FREQUENCY == 1000U
now *= 1000U;
#endif
if (now < last_systime) {
timer_base_us64 += TIME_MAX_SYSTIME;
timer_base_ms += TIME_MAX_SYSTIME/1000;
}
last_systime = now;
return now;
}
#else
#error "unsupported timer resolution"
#endif
// offset for micros64()
static uint64_t timer_base_us64;
static uint32_t get_systime_us32(void)
{
static uint32_t last_us32;
uint32_t now = system_time_u32_us();
if (now < last_us32) {
const uint64_t dt_us = 0x100000000ULL;
timer_base_us64 += dt_us;
}
last_us32 = now;
return now;
}
/*
we use chSysGetStatusAndLockX() to prevent an interrupt while
allowing this call from any context
for the exposed functions we use chSysGetStatusAndLockX() to prevent
an interrupt changing the globals while allowing this call from any
context
*/
uint64_t hrt_micros64()
@ -88,9 +112,5 @@ uint32_t hrt_micros32()
uint32_t hrt_millis32()
{
syssts_t sts = chSysGetStatusAndLockX();
uint32_t now = get_systime_us32();
uint32_t ret = (now / 1000U) + timer_base_ms;
chSysRestoreStatusX(sts);
return ret;
return (uint32_t)(hrt_micros64() / 1000U);
}