eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 119 Wiki Activity

some cleanup
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2024-04-09 09:58:05 +02:00
parent f2f856e227
commit 2057ab9c10
1 changed files with 53 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
linux/acs/GpsHyperionLinuxController.cpp
View File

@ -54,7 +54,7 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
gainedNewFix.timeOut(); gainedNewFix.timeOut();
PoolReadGuard pg(&gpsSet); PoolReadGuard pg(&gpsSet);
gpsSet.setValidity(false, true); gpsSet.setValidity(false, true);
// There can't be a fix with a device that is off. // The ctrl is off, so it cannot detect the data from the devices.
handleFixChangedEvent(GpsHyperion::FixMode::NOT_SEEN); handleFixChangedEvent(GpsHyperion::FixMode::NOT_SEEN);
gpsSet.fixMode.value = GpsHyperion::FixMode::NOT_SEEN; gpsSet.fixMode.value = GpsHyperion::FixMode::NOT_SEEN;
oneShotSwitches.reset(); oneShotSwitches.reset();
@ -215,15 +215,9 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
bool modeIsSet = true; bool modeIsSet = true;
if (MODE_SET != (MODE_SET & gps.set)) { if (MODE_SET != (MODE_SET & gps.set)) {
if (mode != MODE_OFF) { if (mode != MODE_OFF) {
if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
sif::warning << "GpsHyperionLinuxController: No mode could be set in allowed "
<< maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
oneShotSwitches.cantGetFixSwitch = false;
}
modeIsSet = false; modeIsSet = false;
} else { } else {
// GPS device is off anyway, so do other handling // GPS ctrl is off anyway, so do other handling
return returnvalue::FAILED; return returnvalue::FAILED;
} }
} }
@ -253,21 +247,28 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
validFix = true; validFix = true;
} }
newFix = gps.fix.mode; newFix = gps.fix.mode;
if (newFix == GpsHyperion::FixMode::NOT_SEEN or newFix == GpsHyperion::FixMode::NO_FIX) { if (not validFix) {
if (maxTimeToReachFix.hasTimedOut()) { if (maxTimeToReachFix.hasTimedOut()) {
// We are supposed to be on and functioning, but no fix was found // We are supposed to be on and functioning, but no fix was found
// Set HK entries invalid // Set HK entries invalid
gpsSet.setValidity(false, true); gpsSet.setValidity(false, true);
if (resetCallback != nullptr) { if (oneShotSwitches.cantGetFixSwitch) {
uint8_t chip = GpsHyperion::GnssChip::A_SIDE; sif::warning << "GpsHyperionLinuxController: No fix detected in allowed "
ReturnValue_t result = resetCallback(&chip, 1, resetCallbackArgs); << maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
if (result != returnvalue::OK) { triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
triggerEvent(GpsHyperion::RESET_FAIL, chip); oneShotSwitches.cantGetFixSwitch = false;
} // Try resetting the devices
chip = GpsHyperion::GnssChip::B_SIDE; if (resetCallback != nullptr) {
result = resetCallback(&chip, 1, resetCallbackArgs); uint8_t chip = GpsHyperion::GnssChip::A_SIDE;
if (result != returnvalue::OK) { ReturnValue_t result = resetCallback(&chip, 1, resetCallbackArgs);
triggerEvent(GpsHyperion::RESET_FAIL, chip); if (result != returnvalue::OK) {
triggerEvent(GpsHyperion::RESET_FAIL, chip);
}
chip = GpsHyperion::GnssChip::B_SIDE;
result = resetCallback(&chip, 1, resetCallbackArgs);
if (result != returnvalue::OK) {
triggerEvent(GpsHyperion::RESET_FAIL, chip);
}
} }
} }
} }
@ -290,9 +291,12 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
} }
satNotSetCounter = 0; satNotSetCounter = 0;
} else { } else {
satNotSetCounter++; if (satNotSetCounter < 10) {
if (gpsSet.satInUse.isValid() and satNotSetCounter >= 10) { satNotSetCounter++;
} else {
gpsSet.satInUse.value = 0;
gpsSet.satInUse.setValid(false); gpsSet.satInUse.setValid(false);
gpsSet.satInView.value = 0;
gpsSet.satInView.setValid(false); gpsSet.satInView.setValid(false);
} }
} }
@ -300,22 +304,24 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
// LATLON is set for every message, no need for a counter // LATLON is set for every message, no need for a counter
bool latValid = false; bool latValid = false;
bool longValid = false; bool longValid = false;
if (LATLON_SET == (LATLON_SET & gps.set)) { if (modeIsSet) {
if (std::isfinite(gps.fix.latitude)) { if (LATLON_SET == (LATLON_SET & gps.set)) {
// Negative latitude -> South direction if (std::isfinite(gps.fix.latitude)) {
gpsSet.latitude.value = gps.fix.latitude; // Negative latitude -> South direction
// As specified in gps.h: Only valid if mode >= 2 gpsSet.latitude.value = gps.fix.latitude;
if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) { // As specified in gps.h: Only valid if mode >= 2
latValid = true; if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) {
latValid = true;
}
} }
}
if (std::isfinite(gps.fix.longitude)) { if (std::isfinite(gps.fix.longitude)) {
// Negative longitude -> West direction // Negative longitude -> West direction
gpsSet.longitude.value = gps.fix.longitude; gpsSet.longitude.value = gps.fix.longitude;
// As specified in gps.h: Only valid if mode >= 2 // As specified in gps.h: Only valid if mode >= 2
if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) { if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) {
longValid = true; longValid = true;
}
} }
} }
} }
@ -324,20 +330,24 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
// ALTITUDE is set for every message, no need for a counter // ALTITUDE is set for every message, no need for a counter
bool altitudeValid = false; bool altitudeValid = false;
if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) { if (modeIsSet) {
gpsSet.altitude.value = gps.fix.altitude; if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) {
// As specified in gps.h: Only valid if mode == 3 gpsSet.altitude.value = gps.fix.altitude;
if (gps.fix.mode == GpsHyperion::FixMode::FIX_3D) { // As specified in gps.h: Only valid if mode == 3
altitudeValid = true; if (gps.fix.mode == GpsHyperion::FixMode::FIX_3D) {
altitudeValid = true;
}
} }
} }
gpsSet.altitude.setValid(altitudeValid); gpsSet.altitude.setValid(altitudeValid);
// SPEED is set for every message, no need for a counter // SPEED is set for every message, no need for a counter
bool speedValid = false; bool speedValid = false;
if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) { if (modeIsSet) {
gpsSet.speed.value = gps.fix.speed; if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) {
speedValid = true; gpsSet.speed.value = gps.fix.speed;
speedValid = true;
}
} }
gpsSet.speed.setValid(speedValid); gpsSet.speed.setValid(speedValid);