eive-obsw/mission/devices/GPSHyperionHandler.cpp

181 lines
6.3 KiB
C++
Raw Normal View History

2021-06-24 11:42:40 +02:00
#include "GPSHyperionHandler.h"
2020-12-22 00:32:11 +01:00
2022-01-18 11:41:19 +01:00
#include "devicedefinitions/GPSDefinitions.h"
2021-06-24 11:42:40 +02:00
#include "fsfw/datapool/PoolReadGuard.h"
2021-06-24 13:55:51 +02:00
#include "fsfw/timemanager/Clock.h"
2021-06-24 11:42:40 +02:00
2022-01-17 15:41:22 +01:00
#ifdef FSFW_OSAL_LINUX
2022-01-17 11:31:50 +01:00
#include <gps.h>
#include <libgpsmm.h>
2022-01-17 15:41:22 +01:00
#endif
2022-01-17 11:31:50 +01:00
#include <cmath>
2021-09-07 16:11:02 +02:00
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
#include <filesystem>
#include <fstream>
#endif
2022-01-11 10:55:26 +01:00
GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t parentId,
2022-01-18 11:41:19 +01:00
bool debugHyperionGps)
: ExtendedControllerBase(objectId, objects::NO_OBJECT),
gpsSet(this),
debugHyperionGps(debugHyperionGps) {}
2020-12-22 00:32:11 +01:00
2021-06-24 11:42:40 +02:00
GPSHyperionHandler::~GPSHyperionHandler() {}
2020-12-22 00:32:11 +01:00
void GPSHyperionHandler::performControlOperation() {
2022-01-17 15:41:22 +01:00
#ifdef FSFW_OSAL_LINUX
2022-01-18 11:41:19 +01:00
readGpsDataFromGpsd();
2022-01-17 15:41:22 +01:00
#endif
}
2022-01-18 11:47:29 +01:00
LocalPoolDataSetBase *GPSHyperionHandler::getDataSetHandle(sid_t sid) {
return &gpsSet;
}
ReturnValue_t GPSHyperionHandler::checkModeCommand(Mode_t mode, Submode_t submode,
2022-01-18 11:41:19 +01:00
uint32_t *msToReachTheMode) {
return HasReturnvaluesIF::RETURN_OK;
2020-12-22 00:32:11 +01:00
}
ReturnValue_t GPSHyperionHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
2022-01-18 11:41:19 +01:00
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;
2021-09-07 16:11:02 +02:00
}
2022-01-18 11:41:19 +01:00
}
return HasReturnvaluesIF::RETURN_OK;
2020-12-22 00:32:11 +01:00
}
2022-01-18 11:41:19 +01:00
ReturnValue_t GPSHyperionHandler::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}));
2022-01-18 11:51:14 +01:00
localDataPoolMap.emplace(GpsHyperion::SPEED, new PoolEntry<double>({0.0}));
2022-01-18 11:41:19 +01:00
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>());
2022-01-18 11:51:14 +01:00
localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry<uint8_t>());
2022-01-18 11:41:19 +01:00
localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry<uint8_t>());
poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), true, 2.0, false);
return HasReturnvaluesIF::RETURN_OK;
2020-12-22 00:32:11 +01:00
}
2022-01-18 11:41:19 +01:00
void GPSHyperionHandler::setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void *args) {
this->resetCallback = resetCallback;
resetCallbackArgs = args;
2021-09-07 16:11:02 +02:00
}
2021-09-16 17:24:34 +02:00
ReturnValue_t GPSHyperionHandler::initialize() {
2022-01-18 11:41:19 +01:00
ReturnValue_t result = ExtendedControllerBase::initialize();
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
2022-01-18 11:41:19 +01:00
}
return result;
2021-09-16 17:24:34 +02:00
}
2021-09-23 17:14:08 +02:00
ReturnValue_t GPSHyperionHandler::handleCommandMessage(CommandMessage *message) {
2022-01-18 11:41:19 +01:00
return ExtendedControllerBase::handleCommandMessage(message);
2021-09-23 17:14:08 +02:00
}
2022-01-17 11:31:50 +01:00
2022-01-17 15:41:22 +01:00
#ifdef FSFW_OSAL_LINUX
2022-01-17 11:31:50 +01:00
void GPSHyperionHandler::readGpsDataFromGpsd() {
2022-01-18 11:41:19 +01:00
// The data from the device will generally be read all at once. Therefore, we
// can set all field here
gpsmm gpsmm(GPSD_SHARED_MEMORY, 0);
gps_data_t *gps;
gps = gpsmm.read();
if (gps == nullptr) {
sif::warning << "Q7STestTask: Reading GPS data failed" << std::endl;
}
PoolReadGuard pg(&gpsSet);
if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
2022-01-17 11:31:50 +01:00
#if FSFW_VERBOSE_LEVEL >= 1
2022-01-18 11:41:19 +01:00
sif::warning << "GPSHyperionHandler::scanForReply: Reading dataset failed" << std::endl;
2022-01-17 11:31:50 +01:00
#endif
2022-01-18 11:41:19 +01:00
}
// Print messages
if ((gps->set & MODE_SET) != MODE_SET) {
// Could not even set mode
gpsSet.setValidity(false, true);
return;
}
if (gps->satellites_used > 0) {
gpsSet.setValidity(true, true);
}
gpsSet.satInUse.value = gps->satellites_used;
gpsSet.satInView.value = gps->satellites_visible;
// 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
gpsSet.fixMode = gps->fix.mode;
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);
}
gpsSet.unixSeconds.value = gps->fix.time.tv_sec;
timeval time = {};
time.tv_sec = gpsSet.unixSeconds.value;
time.tv_usec = gps->fix.time.tv_nsec / 1000;
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;
time_t timeRaw = gps->fix.time.tv_sec;
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;
std::cout << "Altitude(MSL): " << gps->fix.altMSL << std::endl;
std::cout << "Speed(m/s): " << gps->fix.speed << std::endl;
}
2022-01-17 11:31:50 +01:00
}
2022-01-17 15:41:22 +01:00
#endif