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);
}
}
