Merge branch 'develop' into eggert/acs

2023-01-20 10:50:43 +01:00 · 2023-01-20 10:50:43 +01:00 · bd7a38ac2b
commit bd7a38ac2b
parent abee554454 e66567eb94
13 changed files with 106 additions and 50 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@ -19,9 +19,18 @@ list yields a list of all related PRs for each release.

 ## Changed

+- The ACS Controller Gyro Sets (raw and processed) and the MEKF dataset are diagnostics now.
 - Bumped FSFW for Service 11 improvement which includes size and CRC check for contained TC
 - Syrlinks module now always included for both EM and FM

+## Fixed
+
+- `GyroADIS1650XHandler`: Updated handler to determine correct dynamic range from `RANG_MDL`
+  register readout. This is because ADIS16505-3BMLZ devices are used on the ACS board and the
+  previous range setting was wrong. Also fixed a small error properly set internal state
+  on shut-down.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/342
+
 # [v1.19.0] 10.01.2023

 ## Changed
--- a/bsp_q7s/obsw.cpp
+++ b/bsp_q7s/obsw.cpp
@ -1,13 +1,13 @@
 #include "obsw.h"

+#include <pwd.h>
+#include <sys/types.h>
+#include <unistd.h>
+
 #include <filesystem>
 #include <fstream>
 #include <iostream>

-#include <unistd.h>
-#include <sys/types.h>
-#include <pwd.h>
-
 #include "OBSWConfig.h"
 #include "commonConfig.h"
 #include "core/scheduling.h"
@ -44,8 +44,8 @@ int obsw::obsw() {

  const char* homedir = nullptr;
  homedir = getenv("HOME");
-  if(homedir == nullptr) {
-	  homedir = getpwuid(getuid())->pw_dir;
+  if (homedir == nullptr) {
+    homedir = getpwuid(getuid())->pw_dir;
  }
  std::filesystem::path bootDelayFile = std::filesystem::path(homedir) / "boot_delay_secs.txt";
  // Init delay handling.
--- a/generators/events/translateEvents.cpp
+++ b/generators/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
 * @brief    Auto-generated event translation file. Contains 240 translations.
 * @details
- * Generated on: 2023-01-18 14:34:15
+ * Generated on: 2023-01-18 16:08:56
 */
 #include "translateEvents.h"

--- a/generators/objects/translateObjects.cpp
+++ b/generators/objects/translateObjects.cpp
@ -2,7 +2,7 @@
 * @brief  Auto-generated object translation file.
 * @details
 * Contains 150 translations.
- * Generated on: 2023-01-18 14:34:15
+ * Generated on: 2023-01-18 16:08:56
 */
 #include "translateObjects.h"

--- a/linux/fsfwconfig/events/translateEvents.cpp
+++ b/linux/fsfwconfig/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
 * @brief    Auto-generated event translation file. Contains 240 translations.
 * @details
- * Generated on: 2023-01-18 14:34:15
+ * Generated on: 2023-01-18 16:08:56
 */
 #include "translateEvents.h"

--- a/linux/fsfwconfig/objects/translateObjects.cpp
+++ b/linux/fsfwconfig/objects/translateObjects.cpp
@ -2,7 +2,7 @@
 * @brief  Auto-generated object translation file.
 * @details
 * Contains 150 translations.
- * Generated on: 2023-01-18 14:34:15
+ * Generated on: 2023-01-18 16:08:56
 */
 #include "translateObjects.h"

--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@ -407,14 +407,14 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool& localD
  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});
+  poolManager.subscribeForDiagPeriodicPacket({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});
+  poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0});
  // GPS Processed
  localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
  localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
@ -424,7 +424,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool& localD
  // MEKF
  localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
  localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
-  poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0});
+  poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), false, 5.0});
  // Ctrl Values
  localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
  localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
--- a/mission/devices/GyroADIS1650XHandler.cpp
+++ b/mission/devices/GyroADIS1650XHandler.cpp
@ -63,6 +63,7 @@ void GyroADIS1650XHandler::doStartUp() {

 void GyroADIS1650XHandler::doShutDown() {
  commandExecuted = false;
+  internalState = InternalState::STARTUP;
  setMode(_MODE_POWER_DOWN);
 }

@ -99,12 +100,13 @@ ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t de
  switch (deviceCommand) {
    case (ADIS1650X::READ_OUT_CONFIG): {
      this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
-      uint8_t regList[5] = {};
+      uint8_t regList[6] = {};
      regList[0] = ADIS1650X::DIAG_STAT_REG;
      regList[1] = ADIS1650X::FILTER_CTRL_REG;
-      regList[2] = ADIS1650X::MSC_CTRL_REG;
-      regList[3] = ADIS1650X::DEC_RATE_REG;
-      regList[4] = ADIS1650X::PROD_ID_REG;
+      regList[2] = ADIS1650X::RANG_MDL_REG;
+      regList[3] = ADIS1650X::MSC_CTRL_REG;
+      regList[4] = ADIS1650X::DEC_RATE_REG;
+      regList[5] = ADIS1650X::PROD_ID_REG;
      prepareReadCommand(regList, sizeof(regList));
      this->rawPacket = commandBuffer.data();
      break;
@ -203,9 +205,10 @@ ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t re

 ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                         const uint8_t *packet) {
+  using namespace ADIS1650X;
  switch (id) {
    case (ADIS1650X::READ_OUT_CONFIG): {
-      uint16_t readProdId = packet[10] << 8 | packet[11];
+      uint16_t readProdId = packet[12] << 8 | packet[13];
      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
@ -220,8 +223,32 @@ ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
      PoolReadGuard rg(&configDataset);
      configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
      configDataset.filterSetting.value = packet[4] << 8 | packet[5];
-      configDataset.mscCtrlReg.value = packet[6] << 8 | packet[7];
-      configDataset.decRateReg.value = packet[8] << 8 | packet[9];
+      uint16_t rangMdlRaw = packet[6] << 8 | packet[7];
+      ADIS1650X::RangMdlBitfield bitfield =
+          static_cast<ADIS1650X::RangMdlBitfield>((rangMdlRaw >> 2) & 0b11);
+      switch (bitfield) {
+        case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): {
+          rangeMultiplicator = RANGE_1BMLZ;
+          break;
+        }
+        case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): {
+          rangeMultiplicator = RANGE_2BMLZ;
+          break;
+        }
+        case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): {
+          rangeMultiplicator = RANGE_3BMLZ;
+          break;
+        }
+        case (RangMdlBitfield::RESERVED): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+          sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
+#endif
+          break;
+        }
+      }
+      configDataset.rangMdl.value = rangMdlRaw;
+      configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9];
+      configDataset.decRateReg.value = packet[10] << 8 | packet[11];
      configDataset.setValidity(true, true);
      if (internalState == InternalState::CONFIG) {
        commandExecuted = true;
@ -272,13 +299,13 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
        PoolReadGuard pg(&primaryDataset);
        int16_t angVelocXRaw = packet[4] << 8 | packet[5];
        primaryDataset.angVelocX.value =
-            static_cast<float>(angVelocXRaw) / INT16_MAX * ADIS1650X::GYRO_RANGE;
+            static_cast<float>(angVelocXRaw) / static_cast<float>(INT16_MAX) * rangeMultiplicator;
        int16_t angVelocYRaw = packet[6] << 8 | packet[7];
        primaryDataset.angVelocY.value =
-            static_cast<float>(angVelocYRaw) / INT16_MAX * ADIS1650X::GYRO_RANGE;
+            static_cast<float>(angVelocYRaw) / static_cast<float>(INT16_MAX) * rangeMultiplicator;
        int16_t angVelocZRaw = packet[8] << 8 | packet[9];
        primaryDataset.angVelocZ.value =
-            static_cast<float>(angVelocZRaw) / INT16_MAX * ADIS1650X::GYRO_RANGE;
+            static_cast<float>(angVelocZRaw) / static_cast<float>(INT16_MAX) * rangeMultiplicator;

        float accelScaling = 0;
        if (adisType == ADIS1650X::Type::ADIS16507) {
--- a/mission/devices/GyroADIS1650XHandler.h
+++ b/mission/devices/GyroADIS1650XHandler.h
@ -46,6 +46,8 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
  ADIS1650X::Type adisType;
  AdisGyroPrimaryDataset primaryDataset;
  AdisGyroConfigDataset configDataset;
+  double rangeMultiplicator = ADIS1650X::RANGE_UNSET;
+
  bool goToNormalMode = false;
  bool warningSwitch = true;

--- a/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h
+++ b/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h
@ -13,7 +13,19 @@ enum class Type { ADIS16505, ADIS16507 };
 static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
 static constexpr uint8_t WRITE_MASK = 0b1000'0000;

-static constexpr uint32_t GYRO_RANGE = 125;
+// Ranges in deg / s
+static constexpr double RANGE_UNSET = 0.0;
+static constexpr double RANGE_1BMLZ = 125.0;
+static constexpr double RANGE_2BMLZ = 500.0;
+static constexpr double RANGE_3BMLZ = 2000.0;
+
+enum RangMdlBitfield {
+  RANGE_125_1BMLZ = 0b00,
+  RANGE_500_2BMLZ = 0b01,
+  RESERVED = 0b10,
+  RANGE_2000_3BMLZ = 0b11
+};
+
 static constexpr uint32_t ACCELEROMETER_RANGE_16507 = 392;
 static constexpr float ACCELEROMETER_RANGE_16505 = 78.4;

@ -33,6 +45,7 @@ static constexpr dur_millis_t SELF_TEST_BREAK = 24;

 static constexpr uint8_t DIAG_STAT_REG = 0x02;
 static constexpr uint8_t FILTER_CTRL_REG = 0x5c;
+static constexpr uint8_t RANG_MDL_REG = 0x5e;
 static constexpr uint8_t MSC_CTRL_REG = 0x60;
 static constexpr uint8_t DEC_RATE_REG = 0x64;
 static constexpr uint8_t GLOB_CMD = 0x68;
@ -54,7 +67,7 @@ static constexpr uint16_t BURST_SEL_BIT = 1 << 8;
 static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7;
 static constexpr uint16_t POINT_PERCUSSION_COMPENSATION_BIT = 1 << 6;

-static constexpr size_t CONFIG_READOUT_SIZE = 10 + 2;
+static constexpr size_t CONFIG_READOUT_SIZE = 12 + 2;
 static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2;

 static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA;
@ -78,6 +91,7 @@ enum PrimaryPoolIds : lp_id_t {
  TEMPERATURE,
  DIAG_STAT_REGISTER,
  FILTER_SETTINGS,
+  RANG_MDL,
  MSC_CTRL_REGISTER,
  DEC_RATE_REGISTER,
 };
@ -130,6 +144,7 @@ class AdisGyroConfigDataset : public StaticLocalDataSet<5> {

  lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER);
  lp_var_t<uint8_t> filterSetting = lp_var_t<uint8_t>(sid.objectId, ADIS1650X::FILTER_SETTINGS);
+  lp_var_t<uint16_t> rangMdl = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::RANG_MDL);
  lp_var_t<uint16_t> mscCtrlReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER);
  lp_var_t<uint16_t> decRateReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DEC_RATE_REGISTER);

--- a/mission/system/objects/definitions.h
+++ b/mission/system/objects/definitions.h
@ -18,7 +18,7 @@ enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 };

 namespace payload {

-enum Modes { NONE = 0, SUPV_ONLY = 1, MPSOC_STREAM = 2, CAM_STREAM = 3, EARTH_OBSV = 4, SCEX = 5 };
+enum Mode { OFF = 0, SUPV_ONLY = 1, MPSOC_STREAM = 2, CAM_STREAM = 3, EARTH_OBSV = 4, SCEX = 5 };

 namespace ploc {

--- a/mission/system/tree/payloadModeTree.cpp
+++ b/mission/system/tree/payloadModeTree.cpp
@ -27,53 +27,56 @@ static const auto OFF = HasModesIF::MODE_OFF;
 static const auto ON = HasModesIF::MODE_ON;
 static const auto NML = DeviceHandlerIF::MODE_NORMAL;

-auto PL_SEQUENCE_OFF = std::make_pair(OFF << 24, FixedArrayList<ModeListEntry, 3>());
-auto PL_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList<ModeListEntry, 1>());
-auto PL_TABLE_OFF_TRANS_0 = std::make_pair((OFF << 24) | 2, FixedArrayList<ModeListEntry, 5>());
-auto PL_TABLE_OFF_TRANS_1 = std::make_pair((OFF << 24) | 3, FixedArrayList<ModeListEntry, 1>());
+auto PL_SEQUENCE_OFF = std::make_pair(payload::Mode::OFF << 24, FixedArrayList<ModeListEntry, 3>());
+auto PL_TABLE_OFF_TGT =
+    std::make_pair((payload::Mode::OFF << 24) | 1, FixedArrayList<ModeListEntry, 1>());
+auto PL_TABLE_OFF_TRANS_0 =
+    std::make_pair((payload::Mode::OFF << 24) | 2, FixedArrayList<ModeListEntry, 5>());
+auto PL_TABLE_OFF_TRANS_1 =
+    std::make_pair((payload::Mode::OFF << 24) | 3, FixedArrayList<ModeListEntry, 1>());

 auto PL_SEQUENCE_MPSOC_STREAM =
-    std::make_pair(payload::Modes::MPSOC_STREAM << 24, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(payload::Mode::MPSOC_STREAM << 24, FixedArrayList<ModeListEntry, 3>());
 auto PL_TABLE_MPSOC_STREAM_TGT =
-    std::make_pair((payload::Modes::MPSOC_STREAM << 24) | 1, FixedArrayList<ModeListEntry, 5>());
+    std::make_pair((payload::Mode::MPSOC_STREAM << 24) | 1, FixedArrayList<ModeListEntry, 5>());
 auto PL_TABLE_MPSOC_STREAM_TRANS_0 =
-    std::make_pair((payload::Modes::MPSOC_STREAM << 24) | 2, FixedArrayList<ModeListEntry, 4>());
+    std::make_pair((payload::Mode::MPSOC_STREAM << 24) | 2, FixedArrayList<ModeListEntry, 4>());
 auto PL_TABLE_MPSOC_STREAM_TRANS_1 =
-    std::make_pair((payload::Modes::MPSOC_STREAM << 24) | 3, FixedArrayList<ModeListEntry, 2>());
+    std::make_pair((payload::Mode::MPSOC_STREAM << 24) | 3, FixedArrayList<ModeListEntry, 2>());

 auto PL_SEQUENCE_CAM_STREAM =
-    std::make_pair(payload::Modes::CAM_STREAM << 24, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(payload::Mode::CAM_STREAM << 24, FixedArrayList<ModeListEntry, 3>());
 auto PL_TABLE_CAM_STREAM_TGT =
-    std::make_pair((payload::Modes::CAM_STREAM << 24) | 1, FixedArrayList<ModeListEntry, 2>());
+    std::make_pair((payload::Mode::CAM_STREAM << 24) | 1, FixedArrayList<ModeListEntry, 2>());
 auto PL_TABLE_CAM_STREAM_TRANS_0 =
-    std::make_pair((payload::Modes::CAM_STREAM << 24) | 2, FixedArrayList<ModeListEntry, 4>());
+    std::make_pair((payload::Mode::CAM_STREAM << 24) | 2, FixedArrayList<ModeListEntry, 4>());
 auto PL_TABLE_CAM_STREAM_TRANS_1 =
-    std::make_pair((payload::Modes::CAM_STREAM << 24) | 3, FixedArrayList<ModeListEntry, 2>());
+    std::make_pair((payload::Mode::CAM_STREAM << 24) | 3, FixedArrayList<ModeListEntry, 2>());

 auto PL_SEQUENCE_SUPV_ONLY =
-    std::make_pair(payload::Modes::SUPV_ONLY << 24, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(payload::Mode::SUPV_ONLY << 24, FixedArrayList<ModeListEntry, 3>());
 auto PL_TABLE_SUPV_ONLY_TGT =
-    std::make_pair((payload::Modes::SUPV_ONLY << 24) | 1, FixedArrayList<ModeListEntry, 5>());
+    std::make_pair((payload::Mode::SUPV_ONLY << 24) | 1, FixedArrayList<ModeListEntry, 5>());
 auto PL_TABLE_SUPV_ONLY_TRANS_0 =
-    std::make_pair((payload::Modes::SUPV_ONLY << 24) | 2, FixedArrayList<ModeListEntry, 5>());
+    std::make_pair((payload::Mode::SUPV_ONLY << 24) | 2, FixedArrayList<ModeListEntry, 5>());
 auto PL_TABLE_SUPV_ONLY_TRANS_1 =
-    std::make_pair((payload::Modes::SUPV_ONLY << 24) | 3, FixedArrayList<ModeListEntry, 5>());
+    std::make_pair((payload::Mode::SUPV_ONLY << 24) | 3, FixedArrayList<ModeListEntry, 5>());

 auto PL_SEQUENCE_EARTH_OBSV =
-    std::make_pair(payload::Modes::EARTH_OBSV << 24, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(payload::Mode::EARTH_OBSV << 24, FixedArrayList<ModeListEntry, 3>());
 auto PL_TABLE_EARTH_OBSV_TGT =
-    std::make_pair((payload::Modes::EARTH_OBSV << 24) | 1, FixedArrayList<ModeListEntry, 5>());
+    std::make_pair((payload::Mode::EARTH_OBSV << 24) | 1, FixedArrayList<ModeListEntry, 5>());
 auto PL_TABLE_EARTH_OBSV_TRANS_0 =
-    std::make_pair((payload::Modes::EARTH_OBSV << 24) | 2, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair((payload::Mode::EARTH_OBSV << 24) | 2, FixedArrayList<ModeListEntry, 3>());
 auto PL_TABLE_EARTH_OBSV_TRANS_1 =
-    std::make_pair((payload::Modes::EARTH_OBSV << 24) | 3, FixedArrayList<ModeListEntry, 2>());
+    std::make_pair((payload::Mode::EARTH_OBSV << 24) | 3, FixedArrayList<ModeListEntry, 2>());

 auto PL_SEQUENCE_SCEX =
-    std::make_pair(payload::Modes::SCEX << 24, FixedArrayList<ModeListEntry, 2>());
+    std::make_pair(payload::Mode::SCEX << 24, FixedArrayList<ModeListEntry, 2>());
 auto PL_TABLE_SCEX_TGT =
-    std::make_pair((payload::Modes::SCEX << 24) | 1, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair((payload::Mode::SCEX << 24) | 1, FixedArrayList<ModeListEntry, 1>());
 auto PL_TABLE_SCEX_TRANS_0 =
-    std::make_pair((payload::Modes::SCEX << 24) | 2, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair((payload::Mode::SCEX << 24) | 2, FixedArrayList<ModeListEntry, 1>());

 void satsystem::pl::init() {
  ModeListEntry entry;
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 43e6f0cefa2fd5dfe1f885ca9b818e1486dda568
+Subproject commit f3c0b7567aec22db02a07d76548617b8d163fb29