more improvements for gps handler
This commit is contained in:
parent
e19c03f0d7
commit
bf16d6d86a
@ -172,20 +172,10 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
|
||||
return false;
|
||||
}
|
||||
oneShotSwitches.gpsReadFailedSwitch = true;
|
||||
// did not event get mode, nothing to see.
|
||||
if (MODE_SET != (MODE_SET & gps.set)) {
|
||||
if (mode != MODE_OFF) {
|
||||
if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
|
||||
sif::warning
|
||||
<< "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed "
|
||||
<< maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl;
|
||||
triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout);
|
||||
oneShotSwitches.cantGetFixSwitch = false;
|
||||
}
|
||||
// Mode is on, so do next read immediately
|
||||
return true;
|
||||
}
|
||||
// GPS device is off anyway, so do other handling
|
||||
ReturnValue_t result = handleGpsReadData();
|
||||
if (result == returnvalue::OK) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
noModeSetCntr = 0;
|
||||
@ -197,11 +187,26 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
|
||||
"SHM read not implemented"
|
||||
<< std::endl;
|
||||
}
|
||||
handleGpsReadData();
|
||||
return true;
|
||||
}
|
||||
|
||||
ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
||||
bool modeIsSet = true;
|
||||
if (MODE_SET != (MODE_SET & gps.set)) {
|
||||
if (mode != MODE_OFF) {
|
||||
if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
|
||||
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed "
|
||||
<< maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl;
|
||||
triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout);
|
||||
oneShotSwitches.cantGetFixSwitch = false;
|
||||
}
|
||||
modeIsSet = false;
|
||||
} else {
|
||||
// GPS device is off anyway, so do other handling
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
PoolReadGuard pg(&gpsSet);
|
||||
if (pg.getReadResult() != returnvalue::OK) {
|
||||
#if FSFW_VERBOSE_LEVEL >= 1
|
||||
@ -211,66 +216,80 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
||||
}
|
||||
|
||||
bool validFix = false;
|
||||
// 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
|
||||
if (gps.fix.mode == 2 or gps.fix.mode == 3) {
|
||||
validFix = true;
|
||||
}
|
||||
if (gpsSet.fixMode.value != gps.fix.mode) {
|
||||
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode);
|
||||
}
|
||||
gpsSet.fixMode.value = gps.fix.mode;
|
||||
if (gps.fix.mode == 0 or gps.fix.mode == 1) {
|
||||
if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
|
||||
// We are supposed to be on and functioning, but no fix was found
|
||||
if (mode == MODE_ON or mode == MODE_NORMAL) {
|
||||
mode = MODE_OFF;
|
||||
}
|
||||
modeCommanded = false;
|
||||
if (modeIsSet) {
|
||||
// 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
|
||||
if (gps.fix.mode == 2 or gps.fix.mode == 3) {
|
||||
validFix = true;
|
||||
}
|
||||
if (gpsSet.fixMode.value != gps.fix.mode) {
|
||||
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode);
|
||||
}
|
||||
gpsSet.fixMode.value = gps.fix.mode;
|
||||
if (gps.fix.mode == 0 or gps.fix.mode == 1) {
|
||||
if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
|
||||
// We are supposed to be on and functioning, but no fix was found
|
||||
if (mode == MODE_ON or mode == MODE_NORMAL) {
|
||||
mode = MODE_OFF;
|
||||
}
|
||||
modeCommanded = false;
|
||||
}
|
||||
}
|
||||
gpsSet.setValidity(false, true);
|
||||
} else if (gps.satellites_used > 0 && validFix && mode != MODE_OFF) {
|
||||
gpsSet.setValidity(true, true);
|
||||
}
|
||||
gpsSet.fixMode.setValid(modeIsSet);
|
||||
|
||||
gpsSet.satInUse.value = gps.satellites_used;
|
||||
gpsSet.satInView.value = gps.satellites_visible;
|
||||
bool satInfoValid = false;
|
||||
if (SATELLITE_SET == (SATELLITE_SET & gps.set)) {
|
||||
satInfoValid = true;
|
||||
gpsSet.satInUse.value = gps.satellites_used;
|
||||
gpsSet.satInView.value = gps.satellites_visible;
|
||||
}
|
||||
gpsSet.satInUse.setValid(satInfoValid);
|
||||
gpsSet.satInView.setValid(satInfoValid);
|
||||
|
||||
bool latValid = false;
|
||||
if (std::isfinite(gps.fix.latitude)) {
|
||||
// Negative latitude -> South direction
|
||||
gpsSet.latitude.value = gps.fix.latitude;
|
||||
if (gps.fix.mode >= 2) {
|
||||
latValid = true;
|
||||
bool longValid = false;
|
||||
if (LATLON_SET == (LATLON_SET & gps.set)) {
|
||||
if (std::isfinite(gps.fix.latitude)) {
|
||||
// Negative latitude -> South direction
|
||||
gpsSet.latitude.value = gps.fix.latitude;
|
||||
// As specified in gps.h: Only valid if mode >= 2
|
||||
if (gps.fix.mode >= 2) {
|
||||
latValid = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (std::isfinite(gps.fix.longitude)) {
|
||||
// Negative longitude -> West direction
|
||||
gpsSet.longitude.value = gps.fix.longitude;
|
||||
// As specified in gps.h: Only valid if mode >= 2
|
||||
if (gps.fix.mode >= 2) {
|
||||
longValid = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
gpsSet.latitude.setValid(latValid);
|
||||
|
||||
bool longValid = false;
|
||||
if (std::isfinite(gps.fix.longitude)) {
|
||||
// Negative longitude -> West direction
|
||||
gpsSet.longitude.value = gps.fix.longitude;
|
||||
if (gps.fix.mode >= 2) {
|
||||
longValid = true;
|
||||
}
|
||||
}
|
||||
gpsSet.latitude.setValid(longValid);
|
||||
gpsSet.longitude.setValid(longValid);
|
||||
|
||||
bool altitudeValid = false;
|
||||
if (std::isfinite(gps.fix.altitude)) {
|
||||
if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) {
|
||||
gpsSet.altitude.value = gps.fix.altitude;
|
||||
// As specified in gps.h: Only valid if mode == 3
|
||||
if (gps.fix.mode == 3) {
|
||||
altitudeValid = true;
|
||||
}
|
||||
}
|
||||
gpsSet.altitude.setValid(altitudeValid);
|
||||
|
||||
if (std::isfinite(gps.fix.speed)) {
|
||||
bool speedValid = false;
|
||||
if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) {
|
||||
gpsSet.speed.value = gps.fix.speed;
|
||||
} else {
|
||||
gpsSet.speed.setValid(false);
|
||||
speedValid = true;
|
||||
}
|
||||
gpsSet.speed.setValid(speedValid);
|
||||
|
||||
bool timeValid = false;
|
||||
if (TIME_SET == (TIME_SET & gps.set)) {
|
||||
timeValid = true;
|
||||
timeval time = {};
|
||||
#if LIBGPS_VERSION_MINOR <= 17
|
||||
gpsSet.unixSeconds.value = std::floor(gps.fix.time);
|
||||
@ -294,15 +313,14 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
||||
gpsSet.hours = timeOfDay.hour;
|
||||
gpsSet.minutes = timeOfDay.minute;
|
||||
gpsSet.seconds = timeOfDay.second;
|
||||
} else {
|
||||
gpsSet.unixSeconds.setValid(false);
|
||||
gpsSet.year.setValid(false);
|
||||
gpsSet.month.setValid(false);
|
||||
gpsSet.day.setValid(false);
|
||||
gpsSet.hours.setValid(false);
|
||||
gpsSet.minutes.setValid(false);
|
||||
gpsSet.seconds.setValid(false);
|
||||
}
|
||||
gpsSet.unixSeconds.setValid(timeValid);
|
||||
gpsSet.year.setValid(timeValid);
|
||||
gpsSet.month.setValid(timeValid);
|
||||
gpsSet.day.setValid(timeValid);
|
||||
gpsSet.hours.setValid(timeValid);
|
||||
gpsSet.minutes.setValid(timeValid);
|
||||
gpsSet.seconds.setValid(timeValid);
|
||||
|
||||
if (debugHyperionGps) {
|
||||
sif::info << "-- Hyperion GPS Data --" << std::endl;
|
||||
|
||||
@ -17,8 +17,8 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF {
|
||||
* @param maxNumberOfSequences
|
||||
* @param maxNumberOfTables
|
||||
* @param transmitterTimeout Maximum time the transmitter of the syrlinks
|
||||
* will be
|
||||
* enabled
|
||||
* will
|
||||
* be enabled
|
||||
*/
|
||||
ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables,
|
||||
uint32_t transmitterTimeout);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user