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

GPS Update #130

Merged
meierj merged 12 commits from mueller/gps-update into develop 2022-01-28 07:33:37 +01:00
Conversation 0 Commits 12 Files Changed 27 +521 -220
27 changed files with 521 additions and 220 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
bsp_q7s/boardconfig/busConf.h
View File

@ -26,7 +26,7 @@ namespace uiomapids {
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";

10
bsp_q7s/core/InitMission.cpp
View File

@ -117,7 +117,7 @@ void initmission::initTasks() {
#if OBSW_ADD_ACS_HANDLERS == 1
PeriodicTaskIF* acsCtrl = factory->createPeriodicTask(
"ACS_CTRL", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, missedDeadlineFunc);
"ACS_CTRL", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
result = acsCtrl->addComponent(objects::GPS_CONTROLLER);
if (result != HasReturnvaluesIF::RETURN_OK) {
initmission::printAddObjectError("ACS_CTRL", objects::GPS_CONTROLLER);
@ -136,10 +136,10 @@ void initmission::initTasks() {
#if OBSW_ADD_STAR_TRACKER == 1
PeriodicTaskIF* strImgLoaderTask = factory->createPeriodicTask(
"FILE_SYSTEM_TASK", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc);
"STR_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc);
result = strImgLoaderTask->addComponent(objects::STR_HELPER);
if (result != HasReturnvaluesIF::RETURN_OK) {
initmission::printAddObjectError("FILE_SYSTEM_TASK", objects::STR_HELPER);
initmission::printAddObjectError("STR_HELPER", objects::STR_HELPER);
}
#endif /* OBSW_ADD_STAR_TRACKER == 1 */
@ -208,7 +208,9 @@ void initmission::initTasks() {
#endif /* OBSW_ADD_STAR_TRACKER == 1 */
#endif
// acsCtrl->startTask();
#if OBSW_ADD_ACS_HANDLERS == 1
acsCtrl->startTask();
#endif
sif::info << "Tasks started.." << std::endl;
}

11
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;
@ -527,6 +527,10 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
auto adisHandler = new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF,
spiCookie, ADIS1650X::Type::ADIS16505);
adisHandler->setStartUpImmediately();
#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
adisHandler->setToGoToNormalModeImmediately();
#endif
// Gyro 1 Side A
spiCookie =
new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, spiDev, L3GD20H::MAX_BUFFER_SIZE,
@ -544,6 +548,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF,
spiCookie, ADIS1650X::Type::ADIS16505);
adisHandler->setStartUpImmediately();
#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
adisHandler->setToGoToNormalModeImmediately();
#endif
// Gyro 3 Side B
spiCookie =
new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, spiDev, L3GD20H::MAX_BUFFER_SIZE,

2
bsp_q7s/core/obsw.cpp
View File

@ -29,7 +29,7 @@ int obsw::obsw() {
if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME)) {
sif::warning << "File " << watchdog::RUNNING_FILE_NAME
<< " exists so the software might "
"already be running. Aborting.."
"already be running. Check if obsw systemd service has been stopped."
<< std::endl;
return OBSW_ALREADY_RUNNING;
}

2
fsfw

@ -1 +1 @@
Subproject commit faf7da2743dcd30d83c3ab2f7b4d85277878e636
Subproject commit cc7a3a5a342aa274ba85805ebdfef65224bbe80c

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;
}

4
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.
@ -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

253
mission/devices/GPSHyperionHandler.cpp
View File

@ -3,43 +3,58 @@
#include "devicedefinitions/GPSDefinitions.h"
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/timemanager/Clock.h"
#ifdef FSFW_OSAL_LINUX
#include <gps.h>
#include <libgpsmm.h>
#endif
#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),
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;
}
LocalPoolDataSetBase *GPSHyperionHandler::getDataSetHandle(sid_t sid) { return nullptr; }
ReturnValue_t GPSHyperionHandler::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t *msToReachTheMode) {
return HasReturnvaluesIF::RETURN_OK;
if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
if (commandExecuted) {
internalState = InternalStates::IDLE;
setMode(MODE_ON);
commandExecuted = false;
}
}
}
ReturnValue_t GPSHyperionHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t *data, size_t size) {
switch (actionId) {
void GPSHyperionHandler::doShutDown() {
internalState = InternalStates::NONE;
commandExecuted = false;
setMode(_MODE_POWER_DOWN);
}
ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
return NOTHING_TO_SEND;
}
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);
@ -54,6 +69,92 @@ 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}));
@ -72,105 +173,31 @@ ReturnValue_t GPSHyperionHandler::initializeLocalDataPool(localpool::DataPool &l
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
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) {
#if FSFW_VERBOSE_LEVEL >= 1
sif::warning << "GPSHyperionHandler::scanForReply: Reading dataset failed" << std::endl;
#endif
}
// 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;
}
}
#endif

39
mission/devices/GPSHyperionHandler.h
View File

@ -3,8 +3,8 @@
#include "devicedefinitions/GPSDefinitions.h"
#include "fsfw/FSFW.h"
#include "fsfw/controller/ExtendedControllerBase.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "lwgps/lwgps.h"
/**
* @brief Device handler for the Hyperion HT-GPS200 device
@ -12,35 +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);
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 acceptExternalDeviceCommands() override;
ReturnValue_t initialize() override;
protected:
gpioResetFunction_t resetCallback = nullptr;
void *resetCallbackArgs = nullptr;
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;
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"

17
mission/devices/GyroADIS1650XHandler.cpp
View File

@ -36,6 +36,7 @@ void GyroADIS1650XHandler::doStartUp() {
// Initial 310 ms start up time after power-up
if (internalState == InternalState::STARTUP) {
if (not commandExecuted) {
warningSwitch = true;
breakCountdown.setTimeout(ADIS1650X::START_UP_TIME);
commandExecuted = true;
}
@ -54,8 +55,11 @@ void GyroADIS1650XHandler::doStartUp() {
}
if (internalState == InternalState::IDLE) {
if (goToNormalMode) {
setMode(MODE_NORMAL);
// setMode(MODE_ON);
} else {
setMode(MODE_ON);
}
}
}
@ -81,7 +85,7 @@ ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId
break;
}
default: {
/* Might be a configuration error. */
// Might be a configuration error
sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
"Unknown internal state!"
<< std::endl;
@ -207,8 +211,11 @@ 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) {
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
<< readProdId << std::endl;
}
warningSwitch = false;
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
@ -320,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 10000;
}
uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
uint8_t valueTwo) {
@ -479,4 +484,6 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *
return HasReturnvaluesIF::RETURN_OK;
}
void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */

4
mission/devices/GyroADIS1650XHandler.h
View File

@ -23,6 +23,8 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
ADIS1650X::Type type);
void setToGoToNormalModeImmediately();
// DeviceHandlerBase abstract function implementation
void doStartUp() override;
void doShutDown() override;
@ -43,6 +45,8 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
ADIS1650X::Type adisType;
AdisGyroPrimaryDataset primaryDataset;
AdisGyroConfigDataset configDataset;
bool goToNormalMode = false;
bool warningSwitch = true;
enum class InternalState { STARTUP, CONFIG, IDLE };

3
mission/devices/PDU1Handler.cpp
View File

@ -73,7 +73,8 @@ 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) {

3
mission/devices/PDU2Handler.cpp
View File

@ -427,7 +427,8 @@ 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) {

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) {}
};

2
tmtc

@ -1 +1 @@
Subproject commit 598635ee4fb1eb246980564ae9f3a0feb1f4da30
Subproject commit 57398383ae81d7cc851fc36a6332d4946d3e17ce