AP_Math: Add function to convert frequency to/from microseconds

This commit is contained in:
José Roberto de Souza 2015-12-07 14:19:50 -02:00 committed by Andrew Tridgell
parent 38575dd87a
commit d74bd533c8
1 changed files with 11 additions and 0 deletions

View File

@ -249,6 +249,7 @@ static inline auto MAX(const A &one, const B &two) -> decltype(one > two ? one :
#define NSEC_PER_SEC 1000000000ULL
#define NSEC_PER_USEC 1000ULL
#define USEC_PER_SEC 1000000ULL
inline uint32_t hz_to_nsec(uint32_t freq)
{
@ -270,6 +271,16 @@ inline uint32_t nsec_to_usec(uint32_t nsec)
return nsec / NSEC_PER_USEC;
}
inline uint32_t hz_to_usec(uint32_t freq)
{
return USEC_PER_SEC / freq;
}
inline uint32_t usec_to_hz(uint32_t usec)
{
return USEC_PER_SEC / usec;
}
#undef INLINE
#endif // AP_MATH_H