Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: use C implemention of div1000

Browse Source
This commit is contained in:
Andrew Tridgell 2023-12-20 14:26:48 +11:00
parent 49c3536ca7
commit 4a59097b5e
2 changed files with 5 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,6 +25,8 @@
#pragma GCC optimize("O2")
#include "../../../AP_Math/div1000.h"
/*
we have 4 possible configurations of boards, made up of boards that
have the following properties:
@ -139,9 +141,9 @@ uint32_t hrt_micros32()
#endif
}
uint32_t hrt_millis64()
uint64_t hrt_millis64()
{
return _hrt_div1000(hrt_micros64());
return uint64_div1000(hrt_micros64());
}
uint32_t hrt_millis32()

33
libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h
View File

@ -13,38 +13,7 @@ uint32_t hrt_micros32(void);
uint32_t hrt_millis32(void);
uint32_t hrt_millis32I(void); // from locked context
uint32_t hrt_millis32_from_ISR(void); // from an ISR
uint32_t hrt_millis64(void);
/*
thanks to:
https://0x414b.com/2021/04/16/arm-division.html
*/
static inline uint64_t _hrt_umul64x64hi(uint32_t b, uint32_t a, uint32_t d, uint32_t c)
{
__asm__ ("\n\
umull r4, r5, %[b], %[d] \n\
umull %[d], r4, %[a], %[d] \n\
adds r5, %[d] \n\
umull %[d], %[a], %[a], %[c] \n\
adcs r4, %[d] \n\
adc %[a], #0 \n\
umull %[c], %[b], %[b], %[c] \n\
adds r5, %[c] \n\
adcs %[b], r4 \n\
adc %[a], #0 \n\
" : [a] "+r" (a), [b] "+r" (b), [c] "+r" (c), [d] "+r" (d) : : "r4", "r5");
return (uint64_t) a << 32 | b;
}
/*
return x / 1000
faster than the gcc implementation using _hrt_umul64x64hi() by about 3x
*/
static inline uint64_t _hrt_div1000(uint64_t x)
{
x >>= 3U;
return _hrt_umul64x64hi((uint32_t)x, x >> 32U, 0xe353f7cfU, 0x20c49ba5U) >> 4U;
}
uint64_t hrt_millis64(void);
#ifdef __cplusplus
}