diff --git a/linux/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp index fa1f8f02..63050791 100644 --- a/linux/devices/GPSHyperionLinuxController.cpp +++ b/linux/devices/GPSHyperionLinuxController.cpp @@ -28,6 +28,14 @@ LocalPoolDataSetBase *GPSHyperionLinuxController::getDataSetHandle(sid_t sid) { ReturnValue_t GPSHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) { + if(not modeCommanded) { + if(mode == MODE_ON or mode == MODE_OFF) { + // 10 minutes time to reach fix + *msToReachTheMode = 600000; + maxTimeToReachFix.resetTimer(); + modeCommanded = true; + } + } return HasReturnvaluesIF::RETURN_OK; } @@ -114,6 +122,13 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() { // 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) { + if(modeCommanded and maxTimeToReachFix.hasTimedOut()) { + // We are supposed to be on and functioning, but not fix was found + if(mode == MODE_ON or mode == MODE_NORMAL) { + mode = MODE_OFF; + } + modeCommanded = false; + } gpsSet.setValidity(false, true); } else if (gps->satellites_used > 0) { gpsSet.setValidity(true, true); diff --git a/linux/devices/GPSHyperionLinuxController.h b/linux/devices/GPSHyperionLinuxController.h index 3615cebd..b8ab028d 100644 --- a/linux/devices/GPSHyperionLinuxController.h +++ b/linux/devices/GPSHyperionLinuxController.h @@ -21,6 +21,7 @@ */ class GPSHyperionLinuxController : public ExtendedControllerBase { public: + static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 600; GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId, bool debugHyperionGps = false); virtual ~GPSHyperionLinuxController(); @@ -46,6 +47,8 @@ class GPSHyperionLinuxController : public ExtendedControllerBase { private: GpsPrimaryDataset gpsSet; + Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); + bool modeCommanded = true; gpsmm myGpsmm; bool debugHyperionGps = false; diff --git a/mission/system/AcsBoardAssembly.cpp b/mission/system/AcsBoardAssembly.cpp index b3ef24c6..49190470 100644 --- a/mission/system/AcsBoardAssembly.cpp +++ b/mission/system/AcsBoardAssembly.cpp @@ -38,11 +38,32 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) helper.mgm2SideBMode = childrenMap[helper.mgm2Lis3IdSideB].mode; helper.mgm3SideBMode = childrenMap[helper.mgm3Rm3100IdSideB].mode; helper.gpsMode = childrenMap[helper.gpsId].mode; - if (mode == DeviceHandlerIF::MODE_NORMAL) { - handleNormalModeCmd(submode); - } else if (mode == MODE_ON) { - } else { + if (state == States::MODE_COMMANDING) { + if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) { + powerStateMachine(submode); + handleNormalOrOnModeCmd(mode, submode); + } else { + modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF); + modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF); + modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF); + modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF); + modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF); + modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF); + modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF); + modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF); + modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::GPS].setMode(MODE_OFF); + modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE); + } } + HybridIterator tableIter(modeTable.begin(), modeTable.end()); executeTable(tableIter); return result; @@ -116,12 +137,11 @@ bool AcsBoardAssembly::isUseable(object_id_t object, Mode_t mode) { return false; } -ReturnValue_t AcsBoardAssembly::handleNormalModeCmd(Submode_t submode) { +ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) { ReturnValue_t result = RETURN_OK; Mode_t tgtMode = DeviceHandlerIF::MODE_NORMAL; - powerStateMachine(submode); - if (state == States::MODE_COMMANDING) { - auto cmdSeq = [&](object_id_t objectId, ModeTableIdx tableIdx) { + auto cmdSeq = [&](object_id_t objectId, ModeTableIdx tableIdx) { + if(tgtMode == DeviceHandlerIF::MODE_NORMAL) { if (isUseable(objectId, mode)) { if (helper.gyro0SideAMode != MODE_OFF) { modeTable[tableIdx].setMode(tgtMode); @@ -132,53 +152,61 @@ ReturnValue_t AcsBoardAssembly::handleNormalModeCmd(Submode_t submode) { modeTable[tableIdx].setSubmode(SUBMODE_NONE); } } - }; - switch (submode) { - case (A_SIDE): { - cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A); - cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A); - cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A); - cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A); - modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF); - modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE); - modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF); - modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE); - modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF); - modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE); - modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF); - modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE); - return result; - } - case (B_SIDE): { - cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B); - cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B); - cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B); - cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B); - modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF); - modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE); - modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF); - modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE); - modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF); - modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE); - modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF); - modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE); - return result; - } - case (DUAL_MODE): { - cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A); - cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A); - cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B); - cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B); - cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A); - cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A); - cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B); - cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B); - return result; - } - default: { - sif::error << "AcsBoardAssembly::handleNormalModeCmd: Unknown submode" << std::endl; + } else if(tgtMode == MODE_ON) { + if (isUseable(objectId, mode)) { + modeTable[tableIdx].setMode(MODE_ON); + modeTable[tableIdx].setSubmode(SUBMODE_NONE); } } + }; + switch (submode) { + case (A_SIDE): { + cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A); + cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A); + cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A); + cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A); + cmdSeq(helper.gpsId, ModeTableIdx::GPS); + modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF); + modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF); + modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF); + modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF); + modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE); + return result; + } + case (B_SIDE): { + cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B); + cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B); + cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B); + cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B); + cmdSeq(helper.gpsId, ModeTableIdx::GPS); + modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF); + modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF); + modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF); + modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE); + modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF); + modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE); + return result; + } + case (DUAL_MODE): { + cmdSeq(helper.gpsId, ModeTableIdx::GPS); + cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A); + cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A); + cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B); + cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B); + cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A); + cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A); + cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B); + cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B); + return result; + } + default: { + sif::error << "AcsBoardAssembly::handleNormalModeCmd: Unknown submode" << std::endl; + } } return result; } @@ -244,6 +272,6 @@ void AcsBoardAssembly::powerStateMachine(Submode_t submode) { state = States::SWITCHING_POWER; } if (state == States::SWITCHING_POWER) { - // TODO: Could check for a timeout (temporal or cycles) here and resent command + // TODO: Could check for a timeout (temporal or cycles) here and resend command } } diff --git a/mission/system/AcsBoardAssembly.h b/mission/system/AcsBoardAssembly.h index 6ff63dd5..63136393 100644 --- a/mission/system/AcsBoardAssembly.h +++ b/mission/system/AcsBoardAssembly.h @@ -91,7 +91,7 @@ class AcsBoardAssembly : public AssemblyBase { * @return */ bool isUseable(object_id_t object, Mode_t mode); - ReturnValue_t handleNormalModeCmd(Submode_t submode); + ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode); void powerStateMachine(Submode_t submode); };