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

convert gps handler base class
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source 
- Is ExtendedControllerBase now
This commit is contained in:
Robin Müller 2022-01-11 10:16:26 +01:00
parent 372dca87ad
commit 1f5553b571
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
3 changed files with 86 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
bsp_q7s/core/ObjectFactory.cpp
View File

@ -577,7 +577,6 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI
auto gpsHandler0 = new GPSHyperionHandler(objects::GPS0_HANDLER, objects::UART_COM_IF,
uartCookieGps0, debugGps);
gpsHandler0->setResetPinTriggerFunction(gps::triggerGpioResetPin, &resetArgsGnss0);
gpsHandler0->setStartUpImmediately();
}
void ObjectFactory::createHeaterComponents() {

219
mission/devices/GPSHyperionHandler.cpp
View File

@ -4,8 +4,6 @@
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/timemanager/Clock.h"
#include "lwgps/lwgps.h"
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
#include <filesystem>
#include <fstream>
@ -13,48 +11,88 @@
GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication,
CookieIF *comCookie, bool debugHyperionGps):
DeviceHandlerBase(objectId, deviceCommunication, comCookie), gpsSet(this),
ExtendedControllerBase(objectId, objects::NO_OBJECT), gpsSet(this),
debugHyperionGps(debugHyperionGps) {
lwgps_init(&gpsData);
}
GPSHyperionHandler::~GPSHyperionHandler() {}
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;
}
}
void GPSHyperionHandler::performControlOperation() {
// 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
}
void GPSHyperionHandler::doShutDown() {
internalState = InternalStates::NONE;
commandExecuted = false;
setMode(_MODE_POWER_DOWN);
LocalPoolDataSetBase* GPSHyperionHandler::getDataSetHandle(sid_t sid) {
return nullptr;
}
ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
return NOTHING_TO_SEND;
ReturnValue_t GPSHyperionHandler::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t *msToReachTheMode) {
return HasReturnvaluesIF::RETURN_OK;
}
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) {
ReturnValue_t GPSHyperionHandler::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);
@ -69,99 +107,6 @@ ReturnValue_t GPSHyperionHandler::buildCommandFromCommand(
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}));
@ -180,35 +125,21 @@ ReturnValue_t GPSHyperionHandler::initializeLocalDataPool(
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 = DeviceHandlerBase::initialize();
ReturnValue_t result = ExtendedControllerBase::initialize();
if(result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
// Enable reply immediately for now
return updatePeriodicReply(true, GpsHyperion::GPS_REPLY);
return result;
}
ReturnValue_t GPSHyperionHandler::acceptExternalDeviceCommands() {
return DeviceHandlerBase::acceptExternalDeviceCommands();
ReturnValue_t GPSHyperionHandler::handleCommandMessage(CommandMessage *message) {
return ExtendedControllerBase::handleCommandMessage(message);
}

43
mission/devices/GPSHyperionHandler.h
View File

@ -3,6 +3,7 @@
#include "fsfw/FSFW.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "fsfw/controller/ExtendedControllerBase.h"
#include "devicedefinitions/GPSDefinitions.h"
#include "lwgps/lwgps.h"
@ -12,7 +13,7 @@
* Flight manual:
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/Hyperion_HT-GPS200
*/
class GPSHyperionHandler: public DeviceHandlerBase {
class GPSHyperionHandler: public ExtendedControllerBase {
public:
GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication,
@ -22,46 +23,24 @@ public:
using gpioResetFunction_t = ReturnValue_t (*) (void* args);
void setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void*args);
ReturnValue_t acceptExternalDeviceCommands() override;
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;
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;
};