eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

added back old GPS handler
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller 2022-01-26 17:59:31 +01:00
parent bf73a2e442
commit 5c535784e9
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
22 changed files with 487 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
bsp_q7s/boardconfig/busConf.h
View File

@ -21,12 +21,12 @@ static constexpr char UIO_PTME[] = "/dev/uio1";
static constexpr int MAP_ID_PTME_CONFIG = 3;
namespace uiomapids {
static const int PTME_VC0 = 0;
static const int PTME_VC1 = 1;
static const int PTME_VC2 = 2;
static const int PTME_VC3 = 3;
static const int PTME_CONFIG = 4;
}
static const int PTME_VC0 = 0;
static const int PTME_VC1 = 1;
static const int PTME_VC2 = 2;
static const int PTME_VC3 = 3;
static const int PTME_CONFIG = 4;
} // namespace uiomapids
namespace gpioNames {
static constexpr char GYRO_0_ADIS_CS[] = "gyro_0_adis_chip_select";

4
bsp_q7s/core/ObjectFactory.cpp
View File

@ -45,7 +45,7 @@
#include "linux/devices/devicedefinitions/SusDefinitions.h"
#include "mission/core/GenericFactory.h"
#include "mission/devices/ACUHandler.h"
#include "mission/devices/GPSHyperionHandler.h"
#include "mission/devices/GPSHyperionLinuxController.h"
#include "mission/devices/GyroADIS1650XHandler.h"
#include "mission/devices/HeaterHandler.h"
#include "mission/devices/IMTQHandler.h"
@ -78,10 +78,10 @@
#include <linux/obc/PapbVcInterface.h>
#include <linux/obc/PdecHandler.h>
#include <linux/obc/Ptme.h>
#include <linux/obc/PtmeAxiConfig.h>
#include <linux/obc/PtmeConfig.h>
#include <linux/obc/PtmeRateSetter.h>
#include <linux/obc/TxRateSetterIF.h>
#include <linux/obc/PtmeAxiConfig.h>
ResetArgs resetArgsGnss0;
ResetArgs resetArgsGnss1;

1
linux/obc/PapbVcInterface.cpp
View File

@ -1,5 +1,6 @@
#include <fsfw_hal/linux/uio/UioMapper.h>
#include <linux/obc/PapbVcInterface.h>
#include "fsfw/serviceinterface/ServiceInterface.h"
PapbVcInterface::PapbVcInterface(LinuxLibgpioIF* gpioComIF, gpioId_t papbBusyId,

1
linux/obc/PapbVcInterface.h
View File

@ -3,6 +3,7 @@
#include <fsfw_hal/common/gpio/gpioDefinitions.h>
#include <fsfw_hal/linux/gpio/LinuxLibgpioIF.h>
#include "OBSWConfig.h"
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
#include "linux/obc/VcInterfaceIF.h"

5
linux/obc/PdecHandler.cpp
View File

@ -1,9 +1,12 @@
#include "PdecHandler.h"
#include <fcntl.h>
#include <sys/mman.h>
#include <cstring>
#include <sstream>
#include "OBSWConfig.h"
#include "PdecHandler.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serviceinterface/ServiceInterface.h"

1
linux/obc/Ptme.cpp
View File

@ -2,6 +2,7 @@
#include <linux/obc/Ptme.h>
#include <sys/mman.h>
#include <unistd.h>
#include "PtmeConfig.h"
#include "fsfw/serviceinterface/ServiceInterface.h"

9
linux/obc/PtmeAxiConfig.cpp
View File

@ -1,17 +1,17 @@
#include "PtmeAxiConfig.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw_hal/linux/uio/UioMapper.h"
PtmeAxiConfig::PtmeAxiConfig(object_id_t objectId, std::string configAxiUio, int mapNum) :
SystemObject(objectId), configAxiUio(configAxiUio), mapNum(mapNum) {
PtmeAxiConfig::PtmeAxiConfig(object_id_t objectId, std::string configAxiUio, int mapNum)
: SystemObject(objectId), configAxiUio(configAxiUio), mapNum(mapNum) {
mutex = MutexFactory::instance()->createMutex();
if (mutex == nullptr) {
sif::warning << "Failed to create mutex" << std::endl;
}
}
PtmeAxiConfig::~PtmeAxiConfig() {
}
PtmeAxiConfig::~PtmeAxiConfig() {}
ReturnValue_t PtmeAxiConfig::initialize() {
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
@ -38,4 +38,3 @@ ReturnValue_t PtmeAxiConfig::writeCaduRateReg(uint8_t rateVal) {
}
return HasReturnvaluesIF::RETURN_OK;
}

8
linux/obc/PtmeAxiConfig.h
View File

@ -2,9 +2,10 @@
#define LINUX_OBC_PTMEAXICONFIG_H_
#include <string>
#include "fsfw/ipc/MutexIF.h"
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
#include "fsfw/objectmanager/SystemObject.h"
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
/**
* @brief Class providing low level access to the configuration interface of the PTME.
@ -12,7 +13,7 @@
* @author J. Meier
*/
class PtmeAxiConfig : public SystemObject {
public:
public:
/**
* @brief Constructor
* @param configAxiUio Device file of UIO belonging to the AXI configuration interface.
@ -24,7 +25,7 @@ public:
virtual ReturnValue_t initialize() override;
ReturnValue_t writeCaduRateReg(uint8_t rateVal);
private:
private:
// Address of register storing the bitrate configuration parameter
static const uint32_t CADU_BITRATE_REG = 0x0;
@ -36,7 +37,6 @@ private:
uint32_t mutexTimeout = 20;
uint32_t* baseAddress = nullptr;
};
#endif /* LINUX_OBC_PTMEAXICONFIG_H_ */

3
linux/obc/PtmeRateSetter.h
View File

@ -2,10 +2,10 @@
#define LINUX_OBC_PTMERATESETTER_H_
#include "TxRateSetterIF.h"
#include "fsfw/objectmanager/SystemObject.h"
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
#include "linux/obc/PtmeAxiConfig.h"
#include "linux/obc/PtmeConfig.h"
#include "fsfw/objectmanager/SystemObject.h"
/**
* @brief Class to set the downlink bit rate by writing to the AXI configuration interface of the
@ -31,7 +31,6 @@ class PtmeRateSetter : public TxRateSetterIF, public SystemObject, public HasRet
virtual ReturnValue_t setRate(uint32_t bitRate);
private:
static const uint8_t INTERFACE_ID = CLASS_ID::RATE_SETTER;
//! [EXPORT] : [COMMENT] The commanded rate is not supported by the current FPGA design

2
mission/devices/CMakeLists.txt
View File

@ -1,5 +1,5 @@
target_sources(${TARGET_NAME} PUBLIC
GPSHyperionHandler.cpp
GPSHyperionLinuxController.cpp
GomspaceDeviceHandler.cpp
Tmp1075Handler.cpp
PCDUHandler.cpp

251
mission/devices/GPSHyperionHandler.cpp
View File

@ -3,41 +3,58 @@
#include "devicedefinitions/GPSDefinitions.h"
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/timemanager/Clock.h"
#include <cmath>
#include "lwgps/lwgps.h"
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
#include <filesystem>
#include <fstream>
#endif
GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t parentId,
bool debugHyperionGps)
: ExtendedControllerBase(objectId, objects::NO_OBJECT),
GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication,
CookieIF *comCookie, bool debugHyperionGps)
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
gpsSet(this),
myGpsmm(GPSD_SHARED_MEMORY, nullptr),
debugHyperionGps(debugHyperionGps) {}
debugHyperionGps(debugHyperionGps) {
lwgps_init(&gpsData);
}
GPSHyperionHandler::~GPSHyperionHandler() {}
void GPSHyperionHandler::performControlOperation() {
#ifdef FSFW_OSAL_LINUX
readGpsDataFromGpsd();
#endif
void GPSHyperionHandler::doStartUp() {
if (internalState == InternalStates::NONE) {
commandExecuted = false;
internalState = InternalStates::WAIT_FIRST_MESSAGE;
}
if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
if (commandExecuted) {
internalState = InternalStates::IDLE;
setMode(MODE_ON);
commandExecuted = false;
}
}
}
LocalPoolDataSetBase *GPSHyperionHandler::getDataSetHandle(sid_t sid) {
return &gpsSet;
void GPSHyperionHandler::doShutDown() {
internalState = InternalStates::NONE;
commandExecuted = false;
setMode(_MODE_POWER_DOWN);
}
ReturnValue_t GPSHyperionHandler::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t *msToReachTheMode) {
return HasReturnvaluesIF::RETURN_OK;
ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
return NOTHING_TO_SEND;
}
ReturnValue_t GPSHyperionHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t *data, size_t size) {
switch (actionId) {
ReturnValue_t GPSHyperionHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
return NOTHING_TO_SEND;
}
ReturnValue_t GPSHyperionHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
// By default, send nothing
rawPacketLen = 0;
switch (deviceCommand) {
case (GpsHyperion::TRIGGER_RESET_PIN): {
if (resetCallback != nullptr) {
PoolReadGuard pg(&gpsSet);
@ -52,12 +69,97 @@ ReturnValue_t GPSHyperionHandler::executeAction(ActionId_t actionId, MessageQueu
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GPSHyperionHandler::scanForReply(const uint8_t *start, size_t len,
DeviceCommandId_t *foundId, size_t *foundLen) {
// Pass data to GPS library
if (len > 0) {
// sif::debug << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl;
if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
// TODO: Check whether data is valid by checking whether NMEA start string is valid?
commandExecuted = true;
}
int result = lwgps_process(&gpsData, start, len);
if (result != 1) {
sif::warning << "GPSHandler::scanForReply: Issue processing GPS data with lwgps" << std::endl;
} else {
// The data from the device will generally be read all at once. Therefore, we
// can set all field here
PoolReadGuard pg(&gpsSet);
if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1
sif::warning << "GPSHyperionHandler::scanForReply: Reading dataset failed" << std::endl;
#endif
}
// Print messages
if (gpsData.is_valid) {
// Set all entries valid now, set invalid on case basis if values are sanitized
gpsSet.setValidity(true, true);
}
// Negative latitude -> South direction
gpsSet.latitude.value = gpsData.latitude;
// Negative longitude -> West direction
gpsSet.longitude.value = gpsData.longitude;
if (gpsData.altitude > 600000.0 or gpsData.altitude < 400000.0) {
gpsSet.altitude.setValid(false);
} else {
gpsSet.altitude.setValid(true);
gpsSet.altitude.value = gpsData.altitude;
}
gpsSet.fixMode.value = gpsData.fix_mode;
gpsSet.satInUse.value = gpsData.sats_in_use;
Clock::TimeOfDay_t timeStruct = {};
timeStruct.day = gpsData.date;
timeStruct.hour = gpsData.hours;
timeStruct.minute = gpsData.minutes;
timeStruct.month = gpsData.month;
timeStruct.second = gpsData.seconds;
// Convert two-digit year to full year (AD)
timeStruct.year = gpsData.year + 2000;
timeval timeval = {};
Clock::convertTimeOfDayToTimeval(&timeStruct, &timeval);
gpsSet.year = timeStruct.year;
gpsSet.month = gpsData.month;
gpsSet.day = gpsData.date;
gpsSet.hours = gpsData.hours;
gpsSet.minutes = gpsData.minutes;
gpsSet.seconds = gpsData.seconds;
gpsSet.unixSeconds = timeval.tv_sec;
if (debugHyperionGps) {
sif::info << "GPS Data" << std::endl;
printf("Valid status: %d\n", gpsData.is_valid);
printf("Latitude: %f degrees\n", gpsData.latitude);
printf("Longitude: %f degrees\n", gpsData.longitude);
printf("Altitude: %f meters\n", gpsData.altitude);
}
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
std::string filename = "/mnt/sd0/gps_log.txt";
std::ofstream gpsFile;
if (not std::filesystem::exists(filename)) {
gpsFile.open(filename, std::ofstream::out);
}
gpsFile.open(filename, std::ofstream::out | std::ofstream::app);
gpsFile.write("\n", 1);
gpsFile.write(reinterpret_cast<const char *>(start), len);
#endif
}
*foundLen = len;
*foundId = GpsHyperion::GPS_REPLY;
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GPSHyperionHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
return HasReturnvaluesIF::RETURN_OK;
}
uint32_t GPSHyperionHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { return 5000; }
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}));
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>());
@ -66,113 +168,36 @@ ReturnValue_t GPSHyperionHandler::initializeLocalDataPool(localpool::DataPool &l
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(), true, 2.0, false);
return HasReturnvaluesIF::RETURN_OK;
}
void GPSHyperionHandler::fillCommandAndReplyMap() {
// Reply length does not matter, packets should always arrive periodically
insertInReplyMap(GpsHyperion::GPS_REPLY, 4, &gpsSet, 0, true);
insertInCommandMap(GpsHyperion::TRIGGER_RESET_PIN);
}
void GPSHyperionHandler::modeChanged() { internalState = InternalStates::NONE; }
void GPSHyperionHandler::setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void *args) {
this->resetCallback = resetCallback;
resetCallbackArgs = args;
}
void GPSHyperionHandler::debugInterface(uint8_t positionTracker, object_id_t objectId,
uint32_t parameter) {}
ReturnValue_t GPSHyperionHandler::initialize() {
ReturnValue_t result = ExtendedControllerBase::initialize();
ReturnValue_t result = DeviceHandlerBase::initialize();
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
return result;
// Enable reply immediately for now
return updatePeriodicReply(true, GpsHyperion::GPS_REPLY);
}
ReturnValue_t GPSHyperionHandler::handleCommandMessage(CommandMessage *message) {
return ExtendedControllerBase::handleCommandMessage(message);
ReturnValue_t GPSHyperionHandler::acceptExternalDeviceCommands() {
return DeviceHandlerBase::acceptExternalDeviceCommands();
}
#ifdef FSFW_OSAL_LINUX
void GPSHyperionHandler::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;
debugHyperionGps = true;
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

55
mission/devices/GPSHyperionHandler.h
View File

@ -3,13 +3,8 @@
#include "devicedefinitions/GPSDefinitions.h"
#include "fsfw/FSFW.h"
#include "fsfw/controller/ExtendedControllerBase.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#ifdef FSFW_OSAL_LINUX
#include <gps.h>
#include <libgpsmm.h>
#endif
#include "lwgps/lwgps.h"
/**
* @brief Device handler for the Hyperion HT-GPS200 device
@ -17,36 +12,50 @@
* Flight manual:
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/Hyperion_HT-GPS200
*/
class GPSHyperionHandler : public ExtendedControllerBase {
class GPSHyperionHandler : public DeviceHandlerBase {
public:
GPSHyperionHandler(object_id_t objectId, object_id_t parentId, bool debugHyperionGps = false);
GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
bool debugHyperionGps = false);
virtual ~GPSHyperionHandler();
using gpioResetFunction_t = ReturnValue_t (*)(void* args);
using gpioResetFunction_t = ReturnValue_t (*)(void *args);
void setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void *args);
ReturnValue_t acceptExternalDeviceCommands() override;
void setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void* args);
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
void performControlOperation() override;
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t* msToReachTheMode) override;
ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) override;
ReturnValue_t initialize() override;
protected:
gpioResetFunction_t resetCallback = nullptr;
void* resetCallbackArgs = nullptr;
void *resetCallbackArgs = nullptr;
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) override;
enum class InternalStates { NONE, WAIT_FIRST_MESSAGE, IDLE };
InternalStates internalState = InternalStates::NONE;
bool commandExecuted = false;
/* DeviceHandlerBase overrides */
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override;
void doStartUp() override;
void doShutDown() override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen) override;
ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
size_t *foundLen) override;
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
void fillCommandAndReplyMap() override;
void modeChanged() override;
uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) override;
virtual void debugInterface(uint8_t positionTracker = 0, object_id_t objectId = 0,
uint32_t parameter = 0) override;
private:
lwgps_t gpsData = {};
GpsPrimaryDataset gpsSet;
gpsmm myGpsmm;
bool debugHyperionGps = false;
void readGpsDataFromGpsd();
};
#endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */

178
mission/devices/GPSHyperionLinuxController.cpp Normal file
View File

@ -0,0 +1,178 @@
#include "GPSHyperionLinuxController.h"
#include <cmath>
#include "devicedefinitions/GPSDefinitions.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
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>());
poolManager.subscribeForPeriodicPacket(gpsSet.getSid(), true, 2.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() {
// 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;
debugHyperionGps = true;
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

55
mission/devices/GPSHyperionLinuxController.h Normal file
View File

@ -0,0 +1,55 @@
#ifndef MISSION_DEVICES_GPSHYPERIONHANDLER_H_
#define MISSION_DEVICES_GPSHYPERIONHANDLER_H_
#include "devicedefinitions/GPSDefinitions.h"
#include "fsfw/FSFW.h"
#include "fsfw/controller/ExtendedControllerBase.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#ifdef FSFW_OSAL_LINUX
#include <gps.h>
#include <libgpsmm.h>
#endif
/**
* @brief Device handler for the Hyperion HT-GPS200 device
* @details
* Flight manual:
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/Hyperion_HT-GPS200
* This device handler can only be used on Linux system where the gpsd daemon with shared memory
* export is running.
*/
class GPSHyperionLinuxController : public ExtendedControllerBase {
public:
GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId,
bool debugHyperionGps = false);
virtual ~GPSHyperionLinuxController();
using gpioResetFunction_t = ReturnValue_t (*)(void* args);
void setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void* args);
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
void performControlOperation() override;
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t* msToReachTheMode) override;
ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) override;
ReturnValue_t initialize() override;
protected:
gpioResetFunction_t resetCallback = nullptr;
void* resetCallbackArgs = nullptr;
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) override;
private:
GpsPrimaryDataset gpsSet;
gpsmm myGpsmm;
bool debugHyperionGps = false;
void readGpsDataFromGpsd();
};
#endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */

4
mission/devices/GomspaceDeviceHandler.cpp
View File

@ -187,8 +187,8 @@ void GomspaceDeviceHandler::setNormalDatapoolEntriesInvalid() {}
ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* commandData,
size_t commandDataLen) {
ReturnValue_t result = setParamCacher.deSerialize(&commandData, &commandDataLen,
SerializeIF::Endianness::BIG);
ReturnValue_t result =
setParamCacher.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG);
if (result != HasReturnvaluesIF::RETURN_OK) {
sif::error << "GomspaceDeviceHandler: Failed to deserialize set parameter "
"message"

2
mission/devices/GomspaceDeviceHandler.h
View File

@ -126,7 +126,7 @@ class GomspaceDeviceHandler : public DeviceHandlerBase {
* execution
* @return
*/
virtual ReturnValue_t setParamCallback(SetParamMessageUnpacker& unpacker, bool afterExecution);
virtual ReturnValue_t setParamCallback(SetParamMessageUnpacker &unpacker, bool afterExecution);
/**
* @brief Function to generate the command to get a parameter from a

8
mission/devices/GyroADIS1650XHandler.cpp
View File

@ -55,7 +55,7 @@ void GyroADIS1650XHandler::doStartUp() {
}
if (internalState == InternalState::IDLE) {
if(goToNormalMode) {
if (goToNormalMode) {
setMode(MODE_NORMAL);
} else {
setMode(MODE_ON);
@ -211,7 +211,7 @@ ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or
((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) {
#if OBSW_VERBOSE_LEVEL >= 1
if(warningSwitch) {
if (warningSwitch) {
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
<< readProdId << std::endl;
}
@ -327,9 +327,7 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
return HasReturnvaluesIF::RETURN_OK;
}
uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
return 6000;
}
uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
uint8_t valueTwo) {

5
mission/devices/PDU1Handler.cpp
View File

@ -73,10 +73,11 @@ void PDU1Handler::assignChannelHookFunction(GOMSPACE::ChannelSwitchHook hook, vo
this->hookArgs = args;
}
ReturnValue_t PDU1Handler::setParamCallback(SetParamMessageUnpacker &unpacker, bool afterExecution) {
ReturnValue_t PDU1Handler::setParamCallback(SetParamMessageUnpacker &unpacker,
bool afterExecution) {
using namespace PDU1;
GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU1;
if(not afterExecution) {
if (not afterExecution) {
return HasReturnvaluesIF::RETURN_OK;
}
if (channelSwitchHook != nullptr and unpacker.getParameterSize() == 1) {

5
mission/devices/PDU2Handler.cpp
View File

@ -427,10 +427,11 @@ void PDU2Handler::printHkTable() {
<< std::endl;
}
ReturnValue_t PDU2Handler::setParamCallback(SetParamMessageUnpacker &unpacker, bool afterExecution) {
ReturnValue_t PDU2Handler::setParamCallback(SetParamMessageUnpacker &unpacker,
bool afterExecution) {
using namespace PDU2;
GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU2;
if(not afterExecution) {
if (not afterExecution) {
return HasReturnvaluesIF::RETURN_OK;
}
if (channelSwitchHook != nullptr and unpacker.getParameterSize() == 1) {

2
mission/devices/devicedefinitions/GPSDefinitions.h
View File

@ -55,7 +55,7 @@ class GpsPrimaryDataset : public StaticLocalDataSet<18> {
lp_var_t<uint32_t>(sid.objectId, GpsHyperion::UNIX_SECONDS, this);
private:
friend class GPSHyperionHandler;
friend class GPSHyperionLinuxController;
GpsPrimaryDataset(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, GpsHyperion::DATASET_ID) {}
};