int clock_gettime(clockid_t unused, struct timespec *tp)
{
(void) unused;
uint64_t m = micros64();
tp->tv_sec = m / 1000000;
tp->tv_nsec = (m % 1000000) * 1000;
return 0;
}
int _gettimeofday_r(struct _reent* unused, struct timeval *tp, void *tzp)
{
(void) unused;
(void) tzp;
if (tp)
{
if (!timeshift64_is_set)
tune_timeshift64(sntp_get_current_timestamp() * 1000000ULL);
uint64_t currentTime_us = timeshift64 + micros64();
tp->tv_sec = currentTime_us / 1000000ULL;
tp->tv_usec = currentTime_us % 1000000ULL;
}
return 0;
}
/*
main program
*/
int main(int argc, const char *argv[])
{
sock.set_broadcast();
printf("Starting DSP code\n");
send_storage();
ardupilot_start();
while (true) {
uint64_t now = micros64();
if (now - last_get_storage_us > 1000*1000) {
printf("tick t=%.6f\n", now*1.0e-6f);
ardupilot_heartbeat();
get_storage();
last_get_storage_us = now;
}
socket_check();
usleep(5000);
}
}
uint32_t micros()
{
return micros64() & 0xFFFFFFFF;
}
uint64_t millis64()
{
return micros64() / 1000;
}
/**********************************************************************
name :
function :
**********************************************************************/
uint32_t micros( void )
{
return micros64();
}
void tune_timeshift64 (uint64_t now_us)
{
timeshift64 = now_us - micros64();
timeshift64_is_set = true;
}
