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

bugfix for GPS handler #368

Merged
muellerr merged 11 commits from gps_bugfix into develop 2023-02-06 18:25:53 +01:00
Conversation 0 Commits 11 Files Changed 8 +120 -99
8 changed files with 120 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
CHANGELOG.md
View File

@ -17,6 +17,13 @@ change warranting a new major release:
# [unreleased]
## Fixed
- `GpsHyperionLinuxController`: Fix `gpsd` polling by continuously calling `gps_read` in one cycle
until it does not have any data left anymore. Also, the data is now polled in a permanent loop,
where controller handling is done on 0.2 second timeouts.
PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/368
# [v1.24.0] 2023-02-03
- eive-tmtc v2.10.0

4
bsp_q7s/core/ObjectFactory.cpp
View File

@ -23,7 +23,7 @@
#include "linux/boardtest/UartTestClass.h"
#include "linux/callbacks/gpioCallbacks.h"
#include "linux/csp/CspComIF.h"
#include "linux/devices/GPSHyperionLinuxController.h"
#include "linux/devices/GpsHyperionLinuxController.h"
#include "linux/devices/ScexUartReader.h"
#include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h"
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
@ -474,7 +474,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
RESET_ARGS_GNSS.gpioComIF = gpioComIF;
RESET_ARGS_GNSS.waitPeriodMs = 100;
auto gpsCtrl =
new GPSHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT, debugGps);
new GpsHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT, debugGps);
gpsCtrl->setResetPinTriggerFunction(gps::triggerGpioResetPin, &RESET_ARGS_GNSS);
AcsBoardHelper acsBoardHelper = AcsBoardHelper(

78
bsp_q7s/core/scheduling.cpp
View File

@ -173,54 +173,25 @@ void scheduling::initTasks() {
}
#endif
PeriodicTaskIF* acsCtrlTask = factory->createPeriodicTask(
"ACS_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
#if OBSW_ADD_GPS_CTRL == 1
result = acsCtrlTask->addComponent(objects::GPS_CONTROLLER);
PeriodicTaskIF* gpsTask = factory->createPeriodicTask(
"GPS_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
result = gpsTask->addComponent(objects::GPS_CONTROLLER);
if (result != returnvalue::OK) {
scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER);
}
#endif /* OBSW_ADD_GPS_CTRL */
PeriodicTaskIF* acsSysTask = factory->createPeriodicTask(
"SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
static_cast<void>(acsSysTask);
// To be removed soon because it will be part of the ACS PST.
#if OBSW_ADD_ACS_CTRL == 1
acsCtrlTask->addComponent(objects::ACS_CONTROLLER);
acsSysTask->addComponent(objects::ACS_CONTROLLER);
if (result != returnvalue::OK) {
scheduling::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER);
}
#endif
#if OBSW_Q7S_EM == 1
acsCtrlTask->addComponent(objects::MGM_0_LIS3_HANDLER);
acsCtrlTask->addComponent(objects::MGM_1_RM3100_HANDLER);
acsCtrlTask->addComponent(objects::MGM_2_LIS3_HANDLER);
acsCtrlTask->addComponent(objects::MGM_3_RM3100_HANDLER);
acsCtrlTask->addComponent(objects::IMTQ_HANDLER);
acsCtrlTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF);
acsCtrlTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
acsCtrlTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB);
acsCtrlTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB);
acsCtrlTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB);
acsCtrlTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB);
acsCtrlTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF);
acsCtrlTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF);
acsCtrlTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF);
acsCtrlTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF);
acsCtrlTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
acsCtrlTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB);
acsCtrlTask->addComponent(objects::GYRO_0_ADIS_HANDLER);
acsCtrlTask->addComponent(objects::GYRO_1_L3G_HANDLER);
acsCtrlTask->addComponent(objects::GYRO_2_ADIS_HANDLER);
acsCtrlTask->addComponent(objects::GYRO_3_L3G_HANDLER);
acsCtrlTask->addComponent(objects::GPS_CONTROLLER);
acsCtrlTask->addComponent(objects::STAR_TRACKER);
acsCtrlTask->addComponent(objects::RW1);
acsCtrlTask->addComponent(objects::RW2);
acsCtrlTask->addComponent(objects::RW3);
acsCtrlTask->addComponent(objects::RW4);
#endif
PeriodicTaskIF* acsSysTask = factory->createPeriodicTask(
"SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
static_cast<void>(acsSysTask);
#if OBSW_ADD_ACS_BOARD == 1
result = acsSysTask->addComponent(objects::ACS_BOARD_ASS);
if (result != returnvalue::OK) {
@ -244,6 +215,35 @@ void scheduling::initTasks() {
scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM);
}
#if OBSW_Q7S_EM == 1
acsSysTask->addComponent(objects::MGM_0_LIS3_HANDLER);
acsSysTask->addComponent(objects::MGM_1_RM3100_HANDLER);
acsSysTask->addComponent(objects::MGM_2_LIS3_HANDLER);
acsSysTask->addComponent(objects::MGM_3_RM3100_HANDLER);
acsSysTask->addComponent(objects::IMTQ_HANDLER);
acsSysTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF);
acsSysTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
acsSysTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB);
acsSysTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB);
acsSysTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB);
acsSysTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB);
acsSysTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF);
acsSysTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF);
acsSysTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF);
acsSysTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF);
acsSysTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
acsSysTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB);
acsSysTask->addComponent(objects::GYRO_0_ADIS_HANDLER);
acsSysTask->addComponent(objects::GYRO_1_L3G_HANDLER);
acsSysTask->addComponent(objects::GYRO_2_ADIS_HANDLER);
acsSysTask->addComponent(objects::GYRO_3_L3G_HANDLER);
acsSysTask->addComponent(objects::GPS_CONTROLLER);
acsSysTask->addComponent(objects::STAR_TRACKER);
acsSysTask->addComponent(objects::RW1);
acsSysTask->addComponent(objects::RW2);
acsSysTask->addComponent(objects::RW3);
acsSysTask->addComponent(objects::RW4);
#endif
#if OBSW_ADD_RTD_DEVICES == 1
PeriodicTaskIF* tcsPollingTask = factory->createPeriodicTask(
"TCS_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc);
@ -414,7 +414,9 @@ void scheduling::initTasks() {
strHelperTask->startTask();
#endif /* OBSW_ADD_STAR_TRACKER == 1 */
acsCtrlTask->startTask();
#if OBSW_ADD_GPS_CTRL == 1
gpsTask->startTask();
#endif
acsSysTask->startTask();
#if OBSW_ADD_RTD_DEVICES == 1
tcsPollingTask->startTask();

4
linux/ObjectFactory.cpp
View File

@ -121,8 +121,8 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo
fdir = new SusFdir(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
susHandlers[5]->setCustomFdir(fdir);
spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, SUS::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE,
spi::SUS_MAX1227_SPI_FREQ);
spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, SUS::MAX_CMD_SIZE,
spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
susHandlers[6] =
new SusHandler(objects::SUS_6_R_LOC_XFYBZM_PT_XF, 6, objects::SPI_MAIN_COM_IF, spiCookie);
fdir = new SusFdir(objects::SUS_6_R_LOC_XFYBZM_PT_XF);

2
linux/devices/CMakeLists.txt
View File

@ -1,5 +1,5 @@
if(EIVE_BUILD_GPSD_GPS_HANDLER)
target_sources(${OBSW_NAME} PRIVATE GPSHyperionLinuxController.cpp)
target_sources(${OBSW_NAME} PRIVATE GpsHyperionLinuxController.cpp)
endif()
target_sources(

109
linux/devices/GPSHyperionLinuxController.cpp → linux/devices/GpsHyperionLinuxController.cpp
View File

@ -1,5 +1,6 @@
#include "GPSHyperionLinuxController.h"
#include "GpsHyperionLinuxController.h"
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>
#include "OBSWConfig.h"
@ -16,26 +17,20 @@
#include <cmath>
#include <ctime>
GPSHyperionLinuxController::GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId,
GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
bool debugHyperionGps)
: ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {
timeUpdateCd.resetTimer();
}
GPSHyperionLinuxController::~GPSHyperionLinuxController() {
GpsHyperionLinuxController::~GpsHyperionLinuxController() {
gps_stream(&gps, WATCH_DISABLE, nullptr);
gps_close(&gps);
}
void GPSHyperionLinuxController::performControlOperation() {
#ifdef FSFW_OSAL_LINUX
readGpsDataFromGpsd();
#endif
}
LocalPoolDataSetBase *GpsHyperionLinuxController::getDataSetHandle(sid_t sid) { return &gpsSet; }
LocalPoolDataSetBase *GPSHyperionLinuxController::getDataSetHandle(sid_t sid) { return &gpsSet; }
ReturnValue_t GPSHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t *msToReachTheMode) {
if (not modeCommanded) {
if (mode == MODE_ON or mode == MODE_OFF) {
@ -54,7 +49,7 @@ ReturnValue_t GPSHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
return returnvalue::OK;
}
ReturnValue_t GPSHyperionLinuxController::executeAction(ActionId_t actionId,
ReturnValue_t GpsHyperionLinuxController::executeAction(ActionId_t actionId,
MessageQueueId_t commandedBy,
const uint8_t *data, size_t size) {
switch (actionId) {
@ -72,7 +67,7 @@ ReturnValue_t GPSHyperionLinuxController::executeAction(ActionId_t actionId,
return returnvalue::OK;
}
ReturnValue_t GPSHyperionLinuxController::initializeLocalDataPool(
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}));
@ -92,13 +87,27 @@ ReturnValue_t GPSHyperionLinuxController::initializeLocalDataPool(
return returnvalue::OK;
}
void GPSHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t resetCallback,
void GpsHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t resetCallback,
void *args) {
this->resetCallback = resetCallback;
resetCallbackArgs = args;
}
ReturnValue_t GPSHyperionLinuxController::initialize() {
ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) {
handleQueue();
poolManager.performHkOperation();
while (true) {
bool callAgainImmediately = readGpsDataFromGpsd();
if (not callAgainImmediately) {
handleQueue();
poolManager.performHkOperation();
}
}
// Should never be reached.
return returnvalue::OK;
}
ReturnValue_t GpsHyperionLinuxController::initialize() {
ReturnValue_t result = ExtendedControllerBase::initialize();
if (result != returnvalue::OK) {
return result;
@ -118,6 +127,7 @@ ReturnValue_t GPSHyperionLinuxController::initialize() {
if (retval != 0) {
openError("Socket", retval);
}
gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr);
} else if (readMode == ReadModes::SHM) {
int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps);
if (retval != 0) {
@ -127,57 +137,58 @@ ReturnValue_t GPSHyperionLinuxController::initialize() {
return result;
}
ReturnValue_t GPSHyperionLinuxController::handleCommandMessage(CommandMessage *message) {
ReturnValue_t GpsHyperionLinuxController::handleCommandMessage(CommandMessage *message) {
return ExtendedControllerBase::handleCommandMessage(message);
}
#ifdef FSFW_OSAL_LINUX
void GpsHyperionLinuxController::performControlOperation() {}
void GPSHyperionLinuxController::readGpsDataFromGpsd() {
auto readError = [&](int error) {
bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
auto readError = [&]() {
if (gpsReadFailedSwitch) {
gpsReadFailedSwitch = false;
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | "
"Error "
<< error << " | " << gps_errstr(error) << std::endl;
<< errno << " | " << gps_errstr(errno) << std::endl;
}
};
currentClientBuf = gps_data(&gps);
if (readMode == ReadModes::SOCKET) {
gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr);
// Exit if no data is seen in 2 seconds (should not happen)
if (not gps_waiting(&gps, 2000000)) {
return;
}
int result = gps_read(&gps);
if (result == -1) {
readError(result);
return;
}
if (MODE_SET != (MODE_SET & gps.set)) {
if (noModeSetCntr >= 0) {
noModeSetCntr++;
// Perform other necessary handling if not data seen for 0.2 seconds.
if (gps_waiting(&gps, 200000)) {
if (-1 == gps_read(&gps)) {
readError();
return false;
}
if (noModeSetCntr == 10) {
// TODO: Trigger event here
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be "
"read for 10 consecutive reads"
<< std::endl;
noModeSetCntr = -1;
if (MODE_SET != (MODE_SET & gps.set)) {
if (mode == MODE_ON) {
if (noModeSetCntr >= 0) {
noModeSetCntr++;
}
if (noModeSetCntr == 10) {
// TODO: Trigger event here
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be "
"read for 10 consecutive reads"
<< std::endl;
noModeSetCntr = -1;
}
// did not event get mode, nothing to see.
return false;
}
}
noModeSetCntr = 0;
} else {
return false;
}
noModeSetCntr = 0;
} else if (readMode == ReadModes::SHM) {
int result = gps_read(&gps);
if (result == -1) {
readError(result);
return;
}
sif::error << "GpsHyperionLinuxController::readGpsDataFromGpsdPermanentLoop: "
"SHM read not implemented"
<< std::endl;
}
handleGpsRead();
handleGpsReadData();
return true;
}
ReturnValue_t GPSHyperionLinuxController::handleGpsRead() {
ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
PoolReadGuard pg(&gpsSet);
if (pg.getReadResult() != returnvalue::OK) {
#if FSFW_VERBOSE_LEVEL >= 1
@ -314,5 +325,3 @@ ReturnValue_t GPSHyperionLinuxController::handleGpsRead() {
}
return returnvalue::OK;
}
#endif

13
linux/devices/GPSHyperionLinuxController.h → linux/devices/GpsHyperionLinuxController.h
View File

@ -20,18 +20,19 @@
* This device handler can only be used on Linux system where the gpsd daemon with shared memory
* export is running.
*/
class GPSHyperionLinuxController : public ExtendedControllerBase {
class GpsHyperionLinuxController : public ExtendedControllerBase {
public:
static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 60 * 5;
enum ReadModes { SHM = 0, SOCKET = 1 };
GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId,
GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
bool debugHyperionGps = false);
virtual ~GPSHyperionLinuxController();
virtual ~GpsHyperionLinuxController();
using gpioResetFunction_t = ReturnValue_t (*)(const uint8_t* actionData, size_t len, void* args);
ReturnValue_t performOperation(uint8_t opCode) override;
void setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void* args);
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
void performControlOperation() override;
@ -49,7 +50,7 @@ class GPSHyperionLinuxController : public ExtendedControllerBase {
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) override;
ReturnValue_t handleGpsRead();
ReturnValue_t handleGpsReadData();
private:
GpsPrimaryDataset gpsSet;
@ -66,7 +67,9 @@ class GPSHyperionLinuxController : public ExtendedControllerBase {
uint32_t timeIsConstantCounter = 0;
Countdown timeUpdateCd = Countdown(60);
void readGpsDataFromGpsd();
// Returns true if the function should be called again or false if other
// controller handling can be done.
bool readGpsDataFromGpsd();
};
#endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */

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

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