mirror of https://github.com/ArduPilot/ardupilot
AP_Math: Add functions to convert microseconds to/from nanoseconds
This commit is contained in:
parent
ca533a9d94
commit
d53911e3f6
|
@ -235,6 +235,7 @@ static inline float minf(float a, float b)
|
||||||
}
|
}
|
||||||
|
|
||||||
#define NSEC_PER_SEC 1000000000ULL
|
#define NSEC_PER_SEC 1000000000ULL
|
||||||
|
#define NSEC_PER_USEC 1000ULL
|
||||||
|
|
||||||
inline uint32_t hz_to_nsec(uint32_t freq)
|
inline uint32_t hz_to_nsec(uint32_t freq)
|
||||||
{
|
{
|
||||||
|
@ -246,6 +247,16 @@ inline uint32_t nsec_to_hz(uint32_t usec)
|
||||||
return NSEC_PER_SEC / usec;
|
return NSEC_PER_SEC / usec;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
inline uint32_t usec_to_nsec(uint32_t usec)
|
||||||
|
{
|
||||||
|
return usec * NSEC_PER_USEC;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline uint32_t nsec_to_usec(uint32_t nsec)
|
||||||
|
{
|
||||||
|
return nsec / NSEC_PER_USEC;
|
||||||
|
}
|
||||||
|
|
||||||
#undef INLINE
|
#undef INLINE
|
||||||
#endif // AP_MATH_H
|
#endif // AP_MATH_H
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue