moved gps handler and deployment handler

2022-02-23 19:08:12 +01:00
parent c83020d2c1
commit 10e2600415
7 changed files with 371 additions and 7 deletions
--- a/linux/devices/GPSHyperionLinuxController.cpp
+++ b/linux/devices/GPSHyperionLinuxController.cpp
@ -0,0 +1,177 @@
+#include "GPSHyperionLinuxController.h"
+
+#include "fsfw/datapool/PoolReadGuard.h"
+#include "fsfw/timemanager/Clock.h"
+
+#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
+#include <filesystem>
+#include <fstream>
+#endif
+#include <cmath>
+
+GPSHyperionLinuxController::GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId,
+                                                       bool debugHyperionGps)
+    : ExtendedControllerBase(objectId, objects::NO_OBJECT),
+      gpsSet(this),
+      myGpsmm(GPSD_SHARED_MEMORY, nullptr),
+      debugHyperionGps(debugHyperionGps) {}
+
+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) {
+  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>());
+#if OBSW_ENABLE_PERIODIC_HK == 1
+  poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), true, 2.0, false);
+#endif
+  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() {
+  // The data from the device will generally be read all at once. Therefore, we
+  // can set all field here
+  if (not myGpsmm.is_open()) {
+    // Opening failed
+#if FSFW_VERBOSE_LEVEL >= 1
+    sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM failed" << std::endl;
+#endif
+  }
+  gps_data_t *gps = nullptr;
+  gps = myGpsmm.read();
+  if (gps == nullptr) {
+    sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" << std::endl;
+  }
+  PoolReadGuard pg(&gpsSet);
+  if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+    sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading dataset failed" << std::endl;
+#endif
+  }
+
+  // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
+  gpsSet.fixMode.value = gps->fix.mode;
+  if (gps->fix.mode == 0 or gps->fix.mode == 1) {
+    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);
+  }
+
+  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;
+  }
+}
+#endif