void X11Renderer::Run(void) {
VideoData *frameBuf;
int lastTimestampMs = 0;
while (!Shutdown) {
uint64_t renderStartUs = getTimeUs();
frameBuf = m_pFrameQueue->GetReadPtr();
if (!frameBuf) {
break;
}
RenderFrameInt(frameBuf);
m_pFrameQueue->ReadDone();
if (lastTimestampMs) {
// consider sleep between frames only if queue not getting full
if (m_pFrameQueue->GetCnt() < FRAME_QUEUE_LEN / 2) {
int elapsedUs = (int)(getTimeUs() - renderStartUs);
int timestampDiffUs = (frameBuf->GetTimestamp() - lastTimestampMs) * 1000;
int sleepTimeUs = timestampDiffUs - elapsedUs;
if (sleepTimeUs > 0 && sleepTimeUs < 200000) {
//REND_DBG("X11Renderer::Run usleep: %d, elapsed: %d, timestampdiff: %d", sleepTimeUs, elapsedUs, timestampDiffUs);
usleep(sleepTimeUs);
} else {
REND_DBG("X11Renderer::Run DONT usleep: %d, elapsed: %d, timestampdiff: %d", timestampDiffUs, elapsedUs, timestampDiffUs);
}
} else {
REND_DBG("X11Renderer::Run DONT usleep: queue %d", m_pFrameQueue->GetCnt());
}
}
lastTimestampMs = frameBuf->GetTimestamp();
}
REND_DBG("Exit X11Renderer::Run (%s)", m_pTitle)
}
void pathManagerRuntime(void) {
#if DEBUG
// char str[16];
// sprintf(&str,"%f",pmData.time);
// UART1_SendString(&str);
#endif
requestGPSInfo();
copyGPSData();
// Update status LED
if (led_bright == 0) going_up = true;
else if (led_bright == 255) going_up = false;
if (going_up) led_bright++;
else led_bright--;
setLEDBrightness(led_bright);
if (returnHome){
interchip_send_buffer.pm_data.targetWaypoint = -1;
} else {
interchip_send_buffer.pm_data.targetWaypoint = path[currentIndex]->id;
}
//Check for new uplink command data
checkAMData();
float position[3];
float heading;
//Get the position of the plane (in meters)
getCoordinates(gps_data.longitude,gps_data.latitude,(float*)&position);
position[2] = gps_data.altitude;
heading = (float)gps_data.heading;
if (returnHome || (pathCount - currentIndex < 1 && pathCount >= 0)){
interchip_send_buffer.pm_data.sp_Heading = lastKnownHeadingHome;
} else if (followPath && pathCount - currentIndex >= 1 && interchip_send_buffer.pm_data.positionFix > 0) {
currentIndex = followWaypoints(path[currentIndex], (float*)position, heading, (int*)&interchip_send_buffer.pm_data.sp_Heading);
}
if (interchip_send_buffer.pm_data.positionFix >= 1){
lastKnownHeadingHome = calculateHeadingHome(home, (float*)position, heading);
}
if (getTimeUs() - interchip_last_send_time >= INTERCHIP_SEND_INTERVAL_US){
interchip_last_send_time = getTimeUs();
interchip_send_buffer.pm_data.interchip_error_count = getInterchipErrorCount();
sendInterchipData();
}
}
bool audioClock::setTimeUs(uint64_t clk)
{
uint64_t curTime=getTimeUs();
int64_t delta=(int64_t)clk-(int64_t)curTime;
if(labs((long int)delta)<2000) return true;
printf("[audioClock] Drift detected :%"PRIu64" vs %"PRIu64", delta=%"PRId64"\n",curTime,clk,delta);
_nbSamples=0;
_baseClock=clk;
return true;
}
void NamedStopWatch::Stop() {
if (!mStartUSec) {
return;
}
uint64_t stopUSec = getTimeUs();
if (stopUSec > mStartUSec) {
++mNumCalls;
mTotalUSec += stopUSec - mStartUSec;
if (mNumCalls % mLoggingPeriodInFrames == 0) {
const float mSec = TotalUSec() * 1.0E-3f / NumCalls();
LOGE("%s: %f ms", Name().c_str(), mSec);
}
}
mStartUSec = 0;
}
void NamedStopWatch::Start() {
mStartUSec = getTimeUs();
}
