eive-obsw/linux/acs/GpsHyperionLinuxController.cpp

461 lines
16 KiB
C++
Raw Normal View History

2023-02-05 13:13:09 +01:00
#include "GpsHyperionLinuxController.h"
2022-01-26 17:59:31 +01:00
2023-02-06 18:12:41 +01:00
#include <fsfw/tasks/TaskFactory.h>
2022-04-28 10:50:39 +02:00
#include <fsfw/timemanager/Stopwatch.h>
2022-03-08 09:37:23 +01:00
#include "OBSWConfig.h"
2022-04-17 11:48:06 +02:00
#include "fsfw/FSFW.h"
2022-01-26 17:59:31 +01:00
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/timemanager/Clock.h"
#include "linux/utility/utility.h"
#include "mission/utility/compileTime.h"
2022-01-26 17:59:31 +01:00
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
#include <filesystem>
#include <fstream>
#endif
#include <cmath>
2022-04-07 10:02:56 +02:00
#include <ctime>
2022-01-26 17:59:31 +01:00
2023-02-05 13:13:09 +01:00
GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
2023-04-04 19:18:53 +02:00
bool enableHkSets, bool debugHyperionGps)
2023-04-04 20:59:25 +02:00
: ExtendedControllerBase(objectId),
gpsSet(this),
2023-08-04 10:50:19 +02:00
skyviewSet(this),
2023-04-04 20:59:25 +02:00
enableHkSets(enableHkSets),
2023-04-04 19:18:53 +02:00
debugHyperionGps(debugHyperionGps) {}
2022-01-26 17:59:31 +01:00
2023-02-05 13:13:09 +01:00
GpsHyperionLinuxController::~GpsHyperionLinuxController() {
2022-05-03 11:33:07 +02:00
gps_stream(&gps, WATCH_DISABLE, nullptr);
gps_close(&gps);
}
2022-01-26 17:59:31 +01:00
2023-08-04 15:54:03 +02:00
LocalPoolDataSetBase *GpsHyperionLinuxController::getDataSetHandle(sid_t sid) {
switch (sid.ownerSetId) {
case GpsHyperion::CORE_DATASET:
return &gpsSet;
case GpsHyperion::SKYVIEW_DATASET:
return &skyviewSet;
default:
return nullptr;
}
return nullptr;
}
2022-01-26 17:59:31 +01:00
2023-02-05 13:13:09 +01:00
ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
2022-01-26 17:59:31 +01:00
uint32_t *msToReachTheMode) {
2024-03-01 10:59:37 +01:00
if (mode == MODE_ON) {
maxTimeToReachFix.resetTimer();
} else if (mode == MODE_NORMAL) {
return HasModesIF::INVALID_MODE;
2022-03-03 10:12:59 +01:00
}
if (mode == MODE_OFF) {
2024-03-01 10:59:37 +01:00
maxTimeToReachFix.timeOut();
2024-03-01 11:31:01 +01:00
gainedNewFix.timeOut();
2023-02-07 10:03:51 +01:00
PoolReadGuard pg(&gpsSet);
gpsSet.setValidity(false, true);
2024-04-09 09:58:05 +02:00
// The ctrl is off, so it cannot detect the data from the devices.
handleFixChangedEvent(GpsHyperion::FixMode::NOT_SEEN);
gpsSet.fixMode.value = GpsHyperion::FixMode::NOT_SEEN;
oneShotSwitches.reset();
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-01-26 17:59:31 +01:00
}
2023-02-05 13:13:09 +01:00
ReturnValue_t GpsHyperionLinuxController::executeAction(ActionId_t actionId,
2022-01-26 17:59:31 +01:00
MessageQueueId_t commandedBy,
const uint8_t *data, size_t size) {
switch (actionId) {
2022-05-25 10:59:20 +02:00
case (GpsHyperion::TRIGGER_RESET_PIN_GNSS): {
2022-01-26 17:59:31 +01:00
if (resetCallback != nullptr) {
PoolReadGuard pg(&gpsSet);
// Set HK entries invalid
gpsSet.setValidity(false, true);
ReturnValue_t result = resetCallback(data, size, resetCallbackArgs);
if (result != returnvalue::OK) {
return result;
}
2022-01-26 17:59:31 +01:00
return HasActionsIF::EXECUTION_FINISHED;
}
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
}
return HasActionsIF::INVALID_ACTION_ID;
2022-01-26 17:59:31 +01:00
}
2023-02-05 13:13:09 +01:00
ReturnValue_t GpsHyperionLinuxController::initializeLocalDataPool(
2022-01-26 17:59:31 +01:00
localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(GpsHyperion::SPEED, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(GpsHyperion::YEAR, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(GpsHyperion::MONTH, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(GpsHyperion::DAY, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(GpsHyperion::HOURS, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(GpsHyperion::MINUTES, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(GpsHyperion::SECONDS, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry<uint32_t>());
localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry<uint8_t>());
2023-04-04 19:18:53 +02:00
poolManager.subscribeForRegularPeriodicPacket({gpsSet.getSid(), enableHkSets, 30.0});
2023-08-04 10:50:19 +02:00
localDataPoolMap.emplace(GpsHyperion::SKYVIEW_UNIX_SECONDS, new PoolEntry<double>());
localDataPoolMap.emplace(GpsHyperion::PRN_ID, new PoolEntry<int16_t>());
localDataPoolMap.emplace(GpsHyperion::AZIMUTH, new PoolEntry<int16_t>());
localDataPoolMap.emplace(GpsHyperion::ELEVATION, new PoolEntry<int16_t>());
localDataPoolMap.emplace(GpsHyperion::SIGNAL2NOISE, new PoolEntry<double>());
localDataPoolMap.emplace(GpsHyperion::USED, new PoolEntry<uint8_t>());
poolManager.subscribeForRegularPeriodicPacket({skyviewSet.getSid(), false, 120.0});
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-01-26 17:59:31 +01:00
}
2023-02-05 13:13:09 +01:00
void GpsHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t resetCallback,
2022-01-26 17:59:31 +01:00
void *args) {
this->resetCallback = resetCallback;
resetCallbackArgs = args;
}
2023-02-06 10:20:22 +01:00
ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) {
handleQueue();
poolManager.performHkOperation();
2023-03-30 17:16:59 +02:00
2023-02-06 10:20:22 +01:00
while (true) {
2023-02-14 11:18:51 +01:00
#if OBSW_THREAD_TRACING == 1
trace::threadTrace(opCounter, "GPS CTRL");
#endif
2023-02-06 18:12:41 +01:00
bool callAgainImmediately = readGpsDataFromGpsd();
if (not callAgainImmediately) {
2023-02-06 10:20:22 +01:00
handleQueue();
poolManager.performHkOperation();
TaskFactory::delayTask(250);
2023-02-06 10:20:22 +01:00
}
}
// Should never be reached.
return returnvalue::OK;
}
2023-02-05 13:13:09 +01:00
ReturnValue_t GpsHyperionLinuxController::initialize() {
2022-01-26 17:59:31 +01:00
ReturnValue_t result = ExtendedControllerBase::initialize();
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
2022-01-26 17:59:31 +01:00
return result;
}
2022-05-03 11:33:07 +02:00
auto openError = [&](const char *type, int error) {
// Opening failed
2022-05-03 11:33:07 +02:00
#if FSFW_VERBOSE_LEVEL >= 1
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type
<< " failed | Error " << error << " | " << gps_errstr(error) << std::endl;
2022-05-03 11:33:07 +02:00
#endif
};
if (readMode == ReadModes::SOCKET) {
int retval = gps_open("localhost", DEFAULT_GPSD_PORT, &gps);
if (retval != 0) {
openError("Socket", retval);
return ObjectManager::CHILD_INIT_FAILED;
2022-05-03 11:33:07 +02:00
}
2023-02-06 18:12:41 +01:00
gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr);
2022-05-03 11:33:07 +02:00
} else if (readMode == ReadModes::SHM) {
int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps);
if (retval != 0) {
openError("SHM", retval);
return ObjectManager::CHILD_INIT_FAILED;
2022-05-03 11:33:07 +02:00
}
}
2022-01-26 17:59:31 +01:00
return result;
}
2023-02-05 13:13:09 +01:00
ReturnValue_t GpsHyperionLinuxController::handleCommandMessage(CommandMessage *message) {
2022-01-26 17:59:31 +01:00
return ExtendedControllerBase::handleCommandMessage(message);
}
2023-02-06 10:30:17 +01:00
void GpsHyperionLinuxController::performControlOperation() {}
2022-05-02 23:52:53 +02:00
2023-02-06 10:20:22 +01:00
bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
2023-02-06 18:12:41 +01:00
auto readError = [&]() {
if (oneShotSwitches.gpsReadFailedSwitch) {
oneShotSwitches.gpsReadFailedSwitch = false;
2022-05-03 11:33:07 +02:00
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | "
"Error "
2023-02-06 18:12:41 +01:00
<< errno << " | " << gps_errstr(errno) << std::endl;
2022-05-02 23:52:53 +02:00
}
2022-05-03 00:55:01 +02:00
};
// GPS is off, no point in reading data from GPSD.
2023-02-08 13:40:03 +01:00
if (mode == MODE_OFF) {
return false;
}
2023-08-02 16:00:27 +02:00
unsigned int readIdx = 0;
2022-05-03 00:55:01 +02:00
if (readMode == ReadModes::SOCKET) {
2023-02-07 17:01:07 +01:00
// Poll the GPS.
2023-08-02 16:00:27 +02:00
while (gps_waiting(&gps, 0)) {
2023-08-02 13:40:51 +02:00
int retval = gps_read(&gps);
if (retval < 0) {
2023-02-06 18:12:41 +01:00
readError();
2023-02-06 10:39:19 +01:00
return false;
2022-05-03 00:55:01 +02:00
}
2023-08-02 16:00:27 +02:00
readIdx++;
if (readIdx >= 40) {
sif::warning << "GpsHyperionLinuxController: Received " << readIdx
<< " GPSD message consecutively" << std::endl;
break;
2022-05-03 00:55:01 +02:00
}
2023-08-02 16:00:27 +02:00
}
if (readIdx > 0) {
oneShotSwitches.gpsReadFailedSwitch = true;
handleGpsReadData();
2022-04-17 11:48:06 +02:00
}
2022-05-03 11:33:07 +02:00
} else if (readMode == ReadModes::SHM) {
2023-02-06 10:20:22 +01:00
sif::error << "GpsHyperionLinuxController::readGpsDataFromGpsdPermanentLoop: "
"SHM read not implemented"
<< std::endl;
2022-05-03 00:55:01 +02:00
}
2023-08-02 16:00:27 +02:00
return false;
2022-05-02 23:52:53 +02:00
}
2023-02-05 13:13:09 +01:00
ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
2023-02-23 16:36:17 +01:00
bool modeIsSet = true;
if (MODE_SET != (MODE_SET & gps.set)) {
if (mode != MODE_OFF) {
modeIsSet = false;
} else {
2024-04-09 09:58:05 +02:00
// GPS ctrl is off anyway, so do other handling
2023-02-23 16:36:17 +01:00
return returnvalue::FAILED;
}
}
2023-08-04 10:50:19 +02:00
ReturnValue_t result = handleCoreTelemetry(modeIsSet);
if (result != returnvalue::OK) {
return result;
}
result = handleSkyviewTelemetry();
return result;
}
2023-02-23 16:36:17 +01:00
2023-08-04 10:50:19 +02:00
ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
2022-01-26 17:59:31 +01:00
PoolReadGuard pg(&gpsSet);
2022-08-24 17:27:47 +02:00
if (pg.getReadResult() != returnvalue::OK) {
2022-01-26 17:59:31 +01:00
#if FSFW_VERBOSE_LEVEL >= 1
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading dataset failed" << std::endl;
#endif
2022-08-24 17:27:47 +02:00
return returnvalue::FAILED;
2022-01-26 17:59:31 +01:00
}
bool validFix = false;
uint8_t newFix = 0;
2023-02-23 16:36:17 +01:00
if (modeIsSet) {
// 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
if (gps.fix.mode == GpsHyperion::FixMode::FIX_2D or
gps.fix.mode == GpsHyperion::FixMode::FIX_3D) {
2023-02-23 16:36:17 +01:00
validFix = true;
2024-04-09 11:26:55 +02:00
maxTimeToReachFix.resetTimer();
2023-02-23 16:36:17 +01:00
}
newFix = gps.fix.mode;
2024-04-09 09:58:05 +02:00
if (not validFix) {
2024-03-13 16:08:21 +01:00
if (maxTimeToReachFix.hasTimedOut()) {
2023-02-23 16:36:17 +01:00
// We are supposed to be on and functioning, but no fix was found
2024-03-15 14:54:16 +01:00
// Set HK entries invalid
gpsSet.setValidity(false, true);
2024-04-09 09:58:05 +02:00
if (oneShotSwitches.cantGetFixSwitch) {
sif::warning << "GpsHyperionLinuxController: No fix detected in allowed "
<< maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
oneShotSwitches.cantGetFixSwitch = false;
// Try resetting the devices
if (resetCallback != nullptr) {
uint8_t chip = GpsHyperion::GnssChip::A_SIDE;
ReturnValue_t result = resetCallback(&chip, 1, resetCallbackArgs);
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);
}
2024-03-13 16:08:21 +01:00
}
}
2022-03-03 10:12:59 +01:00
}
}
2022-01-26 17:59:31 +01:00
}
if (gpsSet.fixMode.value != newFix) {
handleFixChangedEvent(newFix);
}
gpsSet.fixMode = newFix;
2023-02-23 16:36:17 +01:00
gpsSet.fixMode.setValid(modeIsSet);
2022-01-26 17:59:31 +01:00
2023-02-26 18:21:05 +01:00
// Only set on specific messages, so only set a valid flag to invalid
// if not set for more than a full message set (10 messages here)
2023-02-23 16:36:17 +01:00
if (SATELLITE_SET == (SATELLITE_SET & gps.set)) {
gpsSet.satInUse.value = gps.satellites_used;
gpsSet.satInView.value = gps.satellites_visible;
2023-02-26 18:21:05 +01:00
if (not gpsSet.satInUse.isValid()) {
gpsSet.satInUse.setValid(true);
gpsSet.satInView.setValid(true);
}
satNotSetCounter = 0;
} else {
2024-04-09 09:58:05 +02:00
if (satNotSetCounter < 10) {
satNotSetCounter++;
} else {
gpsSet.satInUse.value = 0;
2023-02-26 18:21:05 +01:00
gpsSet.satInUse.setValid(false);
2024-04-09 09:58:05 +02:00
gpsSet.satInView.value = 0;
2023-02-26 18:21:05 +01:00
gpsSet.satInView.setValid(false);
}
2023-02-23 16:36:17 +01:00
}
2022-01-26 17:59:31 +01:00
2023-02-26 18:21:05 +01:00
// LATLON is set for every message, no need for a counter
2023-02-07 10:12:28 +01:00
bool latValid = false;
2023-02-23 16:36:17 +01:00
bool longValid = false;
2024-04-09 09:58:05 +02:00
if (modeIsSet) {
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 >= GpsHyperion::FixMode::FIX_2D) {
latValid = true;
}
2023-02-23 16:36:17 +01:00
}
2022-01-26 17:59:31 +01:00
2024-04-09 09:58:05 +02:00
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 >= GpsHyperion::FixMode::FIX_2D) {
longValid = true;
}
2023-02-23 16:36:17 +01:00
}
2023-02-07 10:12:28 +01:00
}
2022-01-26 17:59:31 +01:00
}
2023-02-23 16:36:17 +01:00
gpsSet.latitude.setValid(latValid);
gpsSet.longitude.setValid(longValid);
2022-01-26 17:59:31 +01:00
2023-02-26 18:21:05 +01:00
// ALTITUDE is set for every message, no need for a counter
2023-02-07 10:12:28 +01:00
bool altitudeValid = false;
2024-04-09 09:58:05 +02:00
if (modeIsSet) {
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 == GpsHyperion::FixMode::FIX_3D) {
altitudeValid = true;
}
2023-02-07 10:12:28 +01:00
}
2022-01-26 17:59:31 +01:00
}
2023-02-07 10:12:28 +01:00
gpsSet.altitude.setValid(altitudeValid);
2022-01-26 17:59:31 +01:00
2023-02-26 18:21:05 +01:00
// SPEED is set for every message, no need for a counter
2023-02-23 16:36:17 +01:00
bool speedValid = false;
2024-04-09 09:58:05 +02:00
if (modeIsSet) {
if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) {
gpsSet.speed.value = gps.fix.speed;
speedValid = true;
}
2022-01-26 17:59:31 +01:00
}
2023-02-23 16:36:17 +01:00
gpsSet.speed.setValid(speedValid);
2022-01-26 17:59:31 +01:00
2023-02-26 18:21:05 +01:00
// TIME is set for every message, no need for a counter
2023-02-23 16:36:17 +01:00
bool timeValid = false;
2023-02-07 14:02:46 +01:00
if (TIME_SET == (TIME_SET & gps.set)) {
// To prevent totally incorrect times from being declared valid.
2023-03-21 17:37:39 +01:00
if (gpsSet.satInView.isValid() and gpsSet.satInView.value >= 1) {
timeValid = true;
}
2023-02-07 10:22:12 +01:00
timeval time = {};
2022-04-19 17:47:05 +02:00
#if LIBGPS_VERSION_MINOR <= 17
2023-02-07 10:22:12 +01:00
gpsSet.unixSeconds.value = std::floor(gps.fix.time);
double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value;
time.tv_usec = fractionalPart * 1000.0 * 1000.0;
2022-04-19 17:47:05 +02:00
#else
2023-02-07 10:22:12 +01:00
gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
time.tv_usec = gps.fix.time.tv_nsec / 1000;
2022-04-19 17:47:05 +02:00
#endif
2023-02-07 10:22:12 +01:00
time.tv_sec = gpsSet.unixSeconds.value;
// If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC
// and no time file available) we set it with the roughly valid time from the GPS.
// NTP might only work if the time difference between sys time and current time is not too
// large.
overwriteTimeIfNotSane(time, validFix);
Clock::TimeOfDay_t timeOfDay = {};
Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
gpsSet.year = timeOfDay.year;
gpsSet.month = timeOfDay.month;
gpsSet.day = timeOfDay.day;
gpsSet.hours = timeOfDay.hour;
gpsSet.minutes = timeOfDay.minute;
gpsSet.seconds = timeOfDay.second;
}
2023-02-23 16:36:17 +01:00
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);
2022-04-07 10:02:56 +02:00
2022-01-26 17:59:31 +01:00
if (debugHyperionGps) {
sif::info << "-- Hyperion GPS Data --" << std::endl;
2022-04-19 17:47:05 +02:00
#if LIBGPS_VERSION_MINOR <= 17
2023-02-07 10:12:28 +01:00
time_t timeRaw = gpsSet.unixSeconds.value;
2022-04-19 17:47:05 +02:00
#else
2022-05-03 11:33:07 +02:00
time_t timeRaw = gps.fix.time.tv_sec;
2022-04-19 17:47:05 +02:00
#endif
2022-01-26 17:59:31 +01:00
std::tm *time = gmtime(&timeRaw);
std::cout << "Time: " << std::put_time(time, "%c %Z") << std::endl;
2022-05-03 11:33:07 +02:00
std::cout << "Visible satellites: " << gps.satellites_visible << std::endl;
std::cout << "Satellites used: " << gps.satellites_used << std::endl;
std::cout << "Fix (0:Not Seen|1:No Fix|2:2D|3:3D): " << gps.fix.mode << std::endl;
std::cout << "Latitude: " << gps.fix.latitude << std::endl;
std::cout << "Longitude: " << gps.fix.longitude << std::endl;
2022-04-19 17:47:05 +02:00
#if LIBGPS_VERSION_MINOR <= 17
2022-05-03 11:33:07 +02:00
std::cout << "Altitude(MSL): " << gps.fix.altitude << std::endl;
2022-04-19 17:47:05 +02:00
#else
2022-05-03 11:33:07 +02:00
std::cout << "Altitude(MSL): " << gps.fix.altMSL << std::endl;
2022-04-19 17:47:05 +02:00
#endif
2022-05-03 11:33:07 +02:00
std::cout << "Speed(m/s): " << gps.fix.speed << std::endl;
2022-04-07 10:02:56 +02:00
std::time_t t = std::time(nullptr);
std::tm tm = *std::gmtime(&t);
std::cout << "C Time: " << std::put_time(&tm, "%c") << std::endl;
2022-01-26 17:59:31 +01:00
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-01-26 17:59:31 +01:00
}
2023-02-07 10:03:51 +01:00
2023-08-04 10:50:19 +02:00
ReturnValue_t GpsHyperionLinuxController::handleSkyviewTelemetry() {
PoolReadGuard pg(&skyviewSet);
if (pg.getReadResult() != returnvalue::OK) {
return returnvalue::FAILED;
}
skyviewSet.unixSeconds.value = gps.skyview_time;
for (int sat = 0; sat < GpsHyperion::MAX_SATELLITES; sat++) {
skyviewSet.prn_id.value[sat] = gps.skyview[sat].PRN;
skyviewSet.azimuth.value[sat] = gps.skyview[sat].azimuth;
skyviewSet.elevation.value[sat] = gps.skyview[sat].elevation;
skyviewSet.signal2noise.value[sat] = gps.skyview[sat].ss;
skyviewSet.used.value[sat] = gps.skyview[sat].used;
}
return returnvalue::OK;
}
2023-02-07 10:03:51 +01:00
void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool validFix) {
if (not timeInit and validFix) {
if (not utility::timeSanityCheck()) {
#if OBSW_VERBOSE_LEVEL >= 1
time_t timeRaw = time.tv_sec;
std::tm *timeTm = std::gmtime(&timeRaw);
sif::info << "Overwriting invalid system time from GPS data directly: "
<< std::put_time(timeTm, "%c %Z") << std::endl;
#endif
// For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
Clock::setClock(&time);
}
timeInit = true;
}
}
void GpsHyperionLinuxController::handleFixChangedEvent(uint8_t newFix) {
if (gainedNewFix.hasTimedOut()) {
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, newFix, fixChangeCounter);
fixChangeCounter = 0;
}
fixChangeCounter++;
gainedNewFix.resetTimer();
}