bool INDI::GPS::updateProperties() { INDI::DefaultDevice::updateProperties(); if (isConnected()) { // Update GPS and send values to client IPState state = updateGPS(); LocationNP.s = state; defineNumber(&LocationNP); TimeTP.s = state; defineText(&TimeTP); RefreshSP.s = state; defineSwitch(&RefreshSP); defineNumber(&PeriodNP); if (state != IPS_OK) { if (state == IPS_BUSY) DEBUG(INDI::Logger::DBG_SESSION, "GPS fix is in progress..."); timerID = SetTimer(POLLMS); } else if (PeriodN[0].value > 0) timerID = SetTimer(PeriodN[0].value); } else { deleteProperty(LocationNP.name); deleteProperty(TimeTP.name); deleteProperty(RefreshSP.name); deleteProperty(PeriodNP.name); if (timerID > 0) { RemoveTimer(timerID); timerID = -1; } } return true; }
void INDI::GPS::TimerHit() { if (!isConnected()) { timerID = SetTimer(POLLMS); return; } IPState state = updateGPS(); LocationNP.s = state; TimeTP.s = state; RefreshSP.s = state; switch (state) { // Ok case IPS_OK: IDSetNumber(&LocationNP, nullptr); IDSetText(&TimeTP, nullptr); // We got data OK, but if we are required to update once in a while, we'll call it. if (PeriodN[0].value > 0) timerID = SetTimer(PeriodN[0].value*1000); return; break; // GPS fix is in progress or alert case IPS_ALERT: IDSetNumber(&LocationNP, nullptr); IDSetText(&TimeTP, nullptr); break; default: break; } timerID = SetTimer(POLLMS); }
void System::updateSensors() { anemometer.update(); updateGPS(); }
int main(int argc, char *argv[]) { int done=0; double gpsUpdateRate, imuUpdateRate, eulerUpdateRate; long int gpsUpdateCount, imuUpdateCount, eulerUpdateCount, kalmanUpdateCount; double lastTime; // Program begins with the usual logging of data and so on... captureQuitSignal(); shared = (Q_SHARED * )attach_memory(QBOATShareKey, sizeof(Q_SHARED)); sharedDIAG = (DIAG_SHARED *)attach_memory(DIAGShareKey, sizeof(DIAG_SHARED)); createDirectory(); sleep(1); openKalmanlogfile(); fprintf(stderr,"\nExtended Kalman Filter v 1.0 for the Q-boat (based on the Heli INS by Srik)"); initEverything(); lastTime = get_time(); // Now begin main loop... while (!done) { // Run a loop to check if we've received new data from the sensors... if((get_time()-lastTime)>=1.0) { fprintf(stderr,"\nGPS %dHz, IMU %d Hz, EUL %d Hz, KAL %dHz, LastTime %f",gpsUpdateCount, imuUpdateCount, eulerUpdateCount,kalmanUpdateCount,lastTime); gpsUpdateCount = 0; imuUpdateCount = 0; eulerUpdateCount = 0; kalmanUpdateCount = 0; lastTime = get_time(); } if(shared->SensorData.lastEulerKalmanTime<shared->SensorData.eulerUpdateTime) { ++eulerUpdateCount; if(!init_compass) compassInit(); else updateCompass(); } #ifdef __USE3DMG_DATA__ if(shared->SensorData.lastImuKalmanTime<shared->SensorData.imuUpdateTime) { if(!init_imu) imuInit(); else updateImu(); ++imuUpdateCount; } #endif #ifdef __USEXBOW_DATA__ if(shared->SensorData.lastImuKalmanTime<sharedDIAG->xbowData.lastXbowTime) { if(!init_imu) imuInit(); else updateImu(); ++imuUpdateCount; } #endif if(shared->SensorData.lastGpsKalmanTime<shared->SensorData.gpsUpdateTime) { if(!init_gps) gpsInit(); else updateGPS(); ++gpsUpdateCount; } if(!init_kf) { if(init_gps && init_compass && init_imu) kalmanInit(); } // Update the State... if(init_kf && (get_time() - shared->kalmanData.lastKalmanUpdateTime)>=0.01) { updateState(); ++kalmanUpdateCount; } usleep(50); } }