297 lines
10 KiB
C++
297 lines
10 KiB
C++
#include "GPSHyperionLinuxController.h"
|
|
|
|
#include <fsfw/timemanager/Stopwatch.h>
|
|
|
|
#include "OBSWConfig.h"
|
|
#include "fsfw/FSFW.h"
|
|
#include "fsfw/datapool/PoolReadGuard.h"
|
|
#include "fsfw/timemanager/Clock.h"
|
|
#include "linux/utility/utility.h"
|
|
#include "mission/utility/compileTime.h"
|
|
|
|
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
|
|
#include <filesystem>
|
|
#include <fstream>
|
|
#endif
|
|
#include <cmath>
|
|
#include <ctime>
|
|
|
|
GPSHyperionLinuxController::GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId,
|
|
bool debugHyperionGps)
|
|
: ExtendedControllerBase(objectId, objects::NO_OBJECT),
|
|
gpsSet(this),
|
|
debugHyperionGps(debugHyperionGps) {
|
|
timeUpdateCd.resetTimer();
|
|
}
|
|
|
|
GPSHyperionLinuxController::~GPSHyperionLinuxController() {}
|
|
|
|
void GPSHyperionLinuxController::performControlOperation() {
|
|
#ifdef FSFW_OSAL_LINUX
|
|
readGpsDataFromGpsd();
|
|
#endif
|
|
}
|
|
|
|
LocalPoolDataSetBase *GPSHyperionLinuxController::getDataSetHandle(sid_t sid) { return &gpsSet; }
|
|
|
|
ReturnValue_t GPSHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
|
|
uint32_t *msToReachTheMode) {
|
|
if (not modeCommanded) {
|
|
if (mode == MODE_ON or mode == MODE_OFF) {
|
|
gpsNotOpenSwitch = true;
|
|
// 5h time to reach fix
|
|
*msToReachTheMode = MAX_SECONDS_TO_REACH_FIX;
|
|
maxTimeToReachFix.resetTimer();
|
|
modeCommanded = true;
|
|
} else if (mode == MODE_NORMAL) {
|
|
return HasModesIF::INVALID_MODE;
|
|
}
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t GPSHyperionLinuxController::executeAction(ActionId_t actionId,
|
|
MessageQueueId_t commandedBy,
|
|
const uint8_t *data, size_t size) {
|
|
switch (actionId) {
|
|
case (GpsHyperion::TRIGGER_RESET_PIN): {
|
|
if (resetCallback != nullptr) {
|
|
PoolReadGuard pg(&gpsSet);
|
|
// Set HK entries invalid
|
|
gpsSet.setValidity(false, true);
|
|
resetCallback(resetCallbackArgs);
|
|
return HasActionsIF::EXECUTION_FINISHED;
|
|
}
|
|
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
|
}
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t GPSHyperionLinuxController::initializeLocalDataPool(
|
|
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>());
|
|
poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), false, 30.0, false);
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
void GPSHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t resetCallback,
|
|
void *args) {
|
|
this->resetCallback = resetCallback;
|
|
resetCallbackArgs = args;
|
|
}
|
|
|
|
ReturnValue_t GPSHyperionLinuxController::initialize() {
|
|
ReturnValue_t result = ExtendedControllerBase::initialize();
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t GPSHyperionLinuxController::handleCommandMessage(CommandMessage *message) {
|
|
return ExtendedControllerBase::handleCommandMessage(message);
|
|
}
|
|
|
|
#ifdef FSFW_OSAL_LINUX
|
|
void GPSHyperionLinuxController::readGpsDataFromGpsd() {
|
|
gpsmm gpsmm("localhost", DEFAULT_GPSD_PORT);
|
|
// The data from the device will generally be read all at once. Therefore, we
|
|
// can set all field here
|
|
if (not gpsmm.is_open()) {
|
|
if (gpsNotOpenSwitch) {
|
|
// Opening failed
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
|
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM failed | "
|
|
<< "Error " << errno << " | " << gps_errstr(errno) << std::endl;
|
|
#endif
|
|
|
|
gpsNotOpenSwitch = false;
|
|
}
|
|
return;
|
|
}
|
|
// Stopwatch watch;
|
|
gps_data_t *gps = nullptr;
|
|
gps = gpsmm.stream(WATCH_ENABLE | WATCH_JSON);
|
|
if (gps == nullptr) {
|
|
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd:: Setting GPSD watch "
|
|
"policy failed"
|
|
<< std::endl;
|
|
}
|
|
while (gpsmm.waiting(2000)) {
|
|
gps = gpsmm.read();
|
|
if (gps == nullptr) {
|
|
if (gpsReadFailedSwitch) {
|
|
gpsReadFailedSwitch = false;
|
|
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed"
|
|
<< std::endl;
|
|
}
|
|
return;
|
|
}
|
|
if (MODE_SET != (MODE_SET & gps->set)) {
|
|
if (noModeSetCntr >= 0) {
|
|
noModeSetCntr++;
|
|
}
|
|
if (noModeSetCntr == 10) {
|
|
// TODO: Trigger event here
|
|
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be "
|
|
"read for 10 consecutive reads"
|
|
<< std::endl;
|
|
noModeSetCntr = -1;
|
|
}
|
|
return;
|
|
} else {
|
|
noModeSetCntr = 0;
|
|
}
|
|
}
|
|
gps = gpsmm.stream(WATCH_DISABLE);
|
|
PoolReadGuard pg(&gpsSet);
|
|
if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
|
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading dataset failed" << std::endl;
|
|
#endif
|
|
return;
|
|
}
|
|
|
|
bool validFix = false;
|
|
static_cast<void>(validFix);
|
|
// 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
|
|
int newFixMode = gps->fix.mode;
|
|
if (newFixMode == 2 or newFixMode == 3) {
|
|
validFix = true;
|
|
}
|
|
if (gpsSet.fixMode.value != newFixMode) {
|
|
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFixMode);
|
|
}
|
|
gpsSet.fixMode.value = newFixMode;
|
|
if (gps->fix.mode == 0 or gps->fix.mode == 1) {
|
|
if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
|
|
// We are supposed to be on and functioning, but not 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) {
|
|
gpsSet.setValidity(true, true);
|
|
}
|
|
|
|
gpsSet.satInUse.value = gps->satellites_used;
|
|
gpsSet.satInView.value = gps->satellites_visible;
|
|
|
|
if (std::isfinite(gps->fix.latitude)) {
|
|
// Negative latitude -> South direction
|
|
gpsSet.latitude.value = gps->fix.latitude;
|
|
} else {
|
|
gpsSet.latitude.setValid(false);
|
|
}
|
|
|
|
if (std::isfinite(gps->fix.longitude)) {
|
|
// Negative longitude -> West direction
|
|
gpsSet.longitude.value = gps->fix.longitude;
|
|
} else {
|
|
gpsSet.longitude.setValid(false);
|
|
}
|
|
|
|
if (std::isfinite(gps->fix.altitude)) {
|
|
gpsSet.altitude.value = gps->fix.altitude;
|
|
} else {
|
|
gpsSet.altitude.setValid(false);
|
|
}
|
|
|
|
if (std::isfinite(gps->fix.speed)) {
|
|
gpsSet.speed.value = gps->fix.speed;
|
|
} else {
|
|
gpsSet.speed.setValid(false);
|
|
}
|
|
|
|
#if LIBGPS_VERSION_MINOR <= 17
|
|
gpsSet.unixSeconds.value = gps->fix.time;
|
|
#else
|
|
gpsSet.unixSeconds.value = gps->fix.time.tv_sec;
|
|
#endif
|
|
timeval time = {};
|
|
time.tv_sec = gpsSet.unixSeconds.value;
|
|
#if LIBGPS_VERSION_MINOR <= 17
|
|
double fractionalPart = gps->fix.time - std::floor(gps->fix.time);
|
|
time.tv_usec = fractionalPart * 1000.0 * 1000.0;
|
|
#else
|
|
time.tv_usec = gps->fix.time.tv_nsec / 1000;
|
|
#endif
|
|
std::time_t t = std::time(nullptr);
|
|
if (time.tv_sec == t) {
|
|
timeIsConstantCounter++;
|
|
} else {
|
|
timeIsConstantCounter = 0;
|
|
}
|
|
if (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 << "Setting 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 = false;
|
|
}
|
|
// If the received time does not change anymore for whatever reason, do not set it here
|
|
// to avoid stale times. Also, don't do it too often often to avoid jumping times
|
|
if (timeIsConstantCounter < 20 and timeUpdateCd.hasTimedOut()) {
|
|
// Update the system time here for now. NTP seems to be unable to do so for whatever reason.
|
|
// Further tests have shown that the time seems to be set by NTPD after some time..
|
|
// Clock::setClock(&time);
|
|
timeUpdateCd.resetTimer();
|
|
}
|
|
|
|
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;
|
|
if (debugHyperionGps) {
|
|
sif::info << "-- Hyperion GPS Data --" << std::endl;
|
|
#if LIBGPS_VERSION_MINOR <= 17
|
|
time_t timeRaw = gps->fix.time;
|
|
#else
|
|
time_t timeRaw = gps->fix.time.tv_sec;
|
|
#endif
|
|
std::tm *time = gmtime(&timeRaw);
|
|
std::cout << "Time: " << std::put_time(time, "%c %Z") << std::endl;
|
|
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;
|
|
#if LIBGPS_VERSION_MINOR <= 17
|
|
std::cout << "Altitude(MSL): " << gps->fix.altitude << std::endl;
|
|
#else
|
|
std::cout << "Altitude(MSL): " << gps->fix.altMSL << std::endl;
|
|
#endif
|
|
std::cout << "Speed(m/s): " << gps->fix.speed << std::endl;
|
|
std::time_t t = std::time(nullptr);
|
|
std::tm tm = *std::gmtime(&t);
|
|
std::cout << "C Time: " << std::put_time(&tm, "%c") << std::endl;
|
|
}
|
|
}
|
|
#endif
|