eive-obsw/mission/controller/AcsController.cpp

#include "AcsController.h"

#include <fsfw/datapool/PoolReadGuard.h>

#include "mission/devices/torquer.h"

AcsController::AcsController(object_id_t objectId)
    : ExtendedControllerBase(objectId),
      sensorProcessing(&acsParameters),
      navigation(&acsParameters),
      actuatorCmd(&acsParameters),
      guidance(&acsParameters),
      safeCtrl(&acsParameters),
      detumble(&acsParameters),
      ptgCtrl(&acsParameters),
      detumbleCounter{0},
      mgmDataRaw(this),
      mgmDataProcessed(this),
      susDataRaw(this),
      susDataProcessed(this),
      gyrDataRaw(this),
      gyrDataProcessed(this),
      gpsDataProcessed(this),
      mekfData(this),
      ctrlValData(this),
      actuatorCmdData(this) {}

ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
  return returnvalue::OK;
}

void AcsController::performControlOperation() {
  switch (internalState) {
    case InternalState::STARTUP: {
      initialCountdown.resetTimer();
      internalState = InternalState::INITIAL_DELAY;
      return;
    }
    case InternalState::INITIAL_DELAY: {
      if (initialCountdown.hasTimedOut()) {
        internalState = InternalState::READY;
      }
      return;
    }
    case InternalState::READY: {
      if (mode != MODE_OFF) {
        switch (submode) {
          case SUBMODE_SAFE:
            performSafe();
            break;
          case SUBMODE_DETUMBLE:
            performDetumble();
            break;
          case SUBMODE_PTG_TARGET:
          case SUBMODE_PTG_NADIR:
          case SUBMODE_PTG_INERTIAL:
            performPointingCtrl();
            break;
        }
      }
      break;
    }
    default:
      break;
  }

  {
    PoolReadGuard pg(&mgmDataRaw);
    if (pg.getReadResult() == returnvalue::OK) {
      copyMgmData();
    }
  }
  {
    PoolReadGuard pg(&susDataRaw);
    if (pg.getReadResult() == returnvalue::OK) {
      copySusData();
    }
  }
  {
    PoolReadGuard pg(&gyrDataRaw);
    if (pg.getReadResult() == returnvalue::OK) {
      copyGyrData();
    }
  }

  // DEBUG : REMOVE AFTER COMPLETION
  mode = MODE_ON;
  submode = SUBMODE_SAFE;
  // DEBUG END
}

void AcsController::performSafe() {
  //	Concept: SAFE MODE WITH MEKF
  //	-do the sensor processing, maybe is does make more sense do call this class function in
  //	another place since we have to do it for every mode regardless of safe or not

  ACS::SensorValues sensorValues;

  timeval now;
  Clock::getClock_timeval(&now);

  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
  ReturnValue_t validMekf;
  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
                     &mekfData, &validMekf);

  // Give desired satellite rate and sun direction to align
  double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
  guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
  //	IF MEKF is working
  double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
  bool magMomMtqValid = false;
  if (validMekf == returnvalue::OK) {
    safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
                      mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
                      susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
                      mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
                      sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
  } else {
    safeCtrl.safeNoMekf(
        now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
        susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
        mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
        mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
        sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
  }

  double dipolCmdUnits[3] = {0, 0, 0};
  actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);

  {
    PoolReadGuard pg(&ctrlValData);
    if (pg.getReadResult() == returnvalue::OK) {
      double zeroQuat[4] = {0, 0, 0, 0};
      std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double));
      ctrlValData.tgtQuat.setValid(false);
      std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double));
      ctrlValData.errQuat.setValid(false);
      ctrlValData.errAng.value = errAng;
      ctrlValData.errAng.setValid(true);
      ctrlValData.setValidity(true, false);
    }
  }

  //	Detumble check and switch
  if (mekfData.satRotRateMekf.isValid() &&
      VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
          acsParameters.detumbleParameter.omegaDetumbleStart) {
    detumbleCounter++;
  } else if (gyrDataProcessed.gyrVecTot.isValid() &&
             VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
                 acsParameters.detumbleParameter.omegaDetumbleStart) {
    detumbleCounter++;
  } else {
    detumbleCounter = 0;
  }
  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
    submode = SUBMODE_DETUMBLE;
    detumbleCounter = 0;
    triggerEvent(SAFE_RATE_VIOLATION);
  }

  {
    PoolReadGuard pg(&actuatorCmdData);
    if (pg.getReadResult() == returnvalue::OK) {
      int32_t zeroVec[4] = {0, 0, 0, 0};
      std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t));
      actuatorCmdData.rwTargetTorque.setValid(false);
      std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
      actuatorCmdData.rwTargetSpeed.setValid(false);
      std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t));
      actuatorCmdData.mtqTargetDipole.setValid(true);
      actuatorCmdData.setValidity(true, false);
    }
  }
  //  {
  //    PoolReadGuard pg(&dipoleSet);
  //    MutexGuard mg(torquer::lazyLock());
  //    torquer::NEW_ACTUATION_FLAG = true;
  //    dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration);
  //  }
}

void AcsController::performDetumble() {
  ACS::SensorValues sensorValues;

  timeval now;
  Clock::getClock_timeval(&now);

  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
  ReturnValue_t validMekf;
  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
                     &mekfData, &validMekf);

  double magMomMtq[3] = {0, 0, 0};
  detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
                   mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
                   mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
  double dipolCmdUnits[3] = {0, 0, 0};
  actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);

  if (mekfData.satRotRateMekf.isValid() &&
      VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
          acsParameters.detumbleParameter.omegaDetumbleEnd) {
    detumbleCounter++;
  } else if (gyrDataProcessed.gyrVecTot.isValid() &&
             VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
                 acsParameters.detumbleParameter.omegaDetumbleEnd) {
    detumbleCounter++;
  } else {
    detumbleCounter = 0;
  }
  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
    submode = SUBMODE_SAFE;
    detumbleCounter = 0;
  }

  int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
  for (int i = 0; i < 3; ++i) {
    cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]);
  }
  {
    PoolReadGuard pg(&actuatorCmdData);
    if (pg.getReadResult() == returnvalue::OK) {
      int32_t zeroVec[4] = {0, 0, 0, 0};
      std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
      actuatorCmdData.rwTargetTorque.setValid(false);
      std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
      actuatorCmdData.rwTargetSpeed.setValid(false);
      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
      actuatorCmdData.mtqTargetDipole.setValid(true);
      actuatorCmdData.setValidity(true, false);
    }
  }
  //  {
  //	  PoolReadGuard pg(&dipoleSet);
  //	  MutexGuard mg(torquer::lazyLock());
  //	  torquer::NEW_ACTUATION_FLAG = true;
  //	  dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
  // torqueDuration);
  //  }
}

void AcsController::performPointingCtrl() {
  ACS::SensorValues sensorValues;

  timeval now;
  Clock::getClock_timeval(&now);

  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
  ReturnValue_t validMekf;
  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
                     &mekfData, &validMekf);

  double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
  guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate);
  double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
         deltaRate[3] = {0, 0, 0};  // ToDo: check if pointer needed
  guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
  double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
  double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
  ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
  double rwTrqNs[4] = {0, 0, 0, 0};
  ptgCtrl.ptgNullspace(
      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
  double cmdSpeedRws[4] = {0, 0, 0, 0};  // Should be given to the actuator reaction wheel as input
  actuatorCmd.cmdSpeedToRws(
      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
      rwTrqNs, cmdSpeedRws);
  double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
  ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
                          mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),
                          &(sensorValues.rw2Set.currSpeed.value),
                          &(sensorValues.rw3Set.currSpeed.value),
                          &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
  actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);

  int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
  for (int i = 0; i < 3; ++i) {
    cmdDipolUnitsInt[i] = std::round(dipolUnits[i]);
  }
  int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0};
  for (int i = 0; i < 4; ++i) {
    cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]);
  }

  {
    PoolReadGuard pg(&actuatorCmdData);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
      std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t));
      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
      actuatorCmdData.setValidity(true, true);
    }
  }
  //  {
  //	  PoolReadGuard pg(&dipoleSet);
  //	  MutexGuard mg(torquer::lazyLock());
  //	  torquer::NEW_ACTUATION_FLAG = true;
  //	  dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
  // torqueDuration);
  //  }
}

ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                     LocalDataPoolManager &poolManager) {
  // MGM Raw
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
  poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0});
  // MGM Processed
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer);
  localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf);
  poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0});
  // SUS Raw
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw);
  poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0});
  // SUS Processed
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk);
  poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), false, 5.0});
  // GYR Raw
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw);
  poolManager.subscribeForRegularPeriodicPacket({gyrDataRaw.getSid(), false, 5.0});
  // GYR Processed
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot);
  poolManager.subscribeForRegularPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0});
  // GPS Processed
  localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
  localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
  poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
  // MEKF
  localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
  localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
  poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0});
  // Ctrl Values
  localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
  localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
  localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
  poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
  // Actuator CMD
  localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
  localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
  localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
  poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0});
  return returnvalue::OK;
}

LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
  switch (sid.ownerSetId) {
    case acsctrl::MGM_SENSOR_DATA:
      return &mgmDataRaw;
    case acsctrl::MGM_PROCESSED_DATA:
      return &mgmDataProcessed;
    case acsctrl::SUS_SENSOR_DATA:
      return &susDataRaw;
    case acsctrl::SUS_PROCESSED_DATA:
      return &susDataProcessed;
    case acsctrl::GYR_SENSOR_DATA:
      return &gyrDataRaw;
    case acsctrl::GYR_PROCESSED_DATA:
      return &gyrDataProcessed;
    case acsctrl::GPS_PROCESSED_DATA:
      return &gpsDataProcessed;
    case acsctrl::MEKF_DATA:
      return &mekfData;
    case acsctrl::CTRL_VAL_DATA:
      return &ctrlValData;
    case acsctrl::ACTUATOR_CMD_DATA:
      return &actuatorCmdData;
    default:
      return nullptr;
  }
  return nullptr;
}

ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
                                              uint32_t *msToReachTheMode) {
  if (mode == MODE_OFF) {
    if (submode == SUBMODE_NONE) {
      return returnvalue::OK;
    } else {
      return INVALID_SUBMODE;
    }
  } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
    if ((submode > 6) || (submode < 2)) {
      return INVALID_SUBMODE;
    } else {
      return returnvalue::OK;
    }
  }
  return INVALID_MODE;
}

void AcsController::modeChanged(Mode_t mode, Submode_t submode) {}

void AcsController::announceMode(bool recursive) {}

void AcsController::copyMgmData() {
  ACS::SensorValues sensorValues;
  {
    PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value,
                  3 * sizeof(float));
      mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.mgm1Rm3100Set);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value,
                  3 * sizeof(float));
      mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.mgm2Lis3Set);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value,
                  3 * sizeof(float));
      mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.mgm3Rm3100Set);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value,
                  3 + sizeof(float));
      mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.imtqMgmSet);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value,
                  3 * sizeof(float));
      mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid());
      mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value;
      mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid());
    }
  }
}

void AcsController::copySusData() {
  ACS::SensorValues sensorValues;
  {
    PoolReadGuard pg(&sensorValues.susSets[0]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[1]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[2]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[3]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[4]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[5]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[6]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[7]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[8]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[9]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[10]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[11]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid());
    }
  }
}

void AcsController::copyGyrData() {
  ACS::SensorValues sensorValues;
  {
    PoolReadGuard pg(&sensorValues.gyr0AdisSet);
    if (pg.getReadResult() == returnvalue::OK) {
      gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value;
      gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value;
      gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value;
      gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() &&
                                   sensorValues.gyr0AdisSet.angVelocY.isValid() &&
                                   sensorValues.gyr0AdisSet.angVelocZ.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.gyr1L3gSet);
    if (pg.getReadResult() == returnvalue::OK) {
      gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value;
      gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value;
      gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value;
      gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() &&
                                 sensorValues.gyr1L3gSet.angVelocY.isValid() &&
                                 sensorValues.gyr1L3gSet.angVelocZ.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.gyr2AdisSet);
    if (pg.getReadResult() == returnvalue::OK) {
      gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value;
      gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value;
      gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value;
      gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() &&
                                   sensorValues.gyr2AdisSet.angVelocY.isValid() &&
                                   sensorValues.gyr2AdisSet.angVelocZ.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.gyr3L3gSet);
    if (pg.getReadResult() == returnvalue::OK) {
      gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value;
      gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value;
      gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value;
      gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() &&
                                 sensorValues.gyr3L3gSet.angVelocY.isValid() &&
                                 sensorValues.gyr3L3gSet.angVelocZ.isValid());
    }
  }
}