From a73d31b0fbace6f0bb0d27888213a39a3bdad76d Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 26 May 2021 13:11:48 +0200
Subject: [PATCH] added a few more commands

---
 mission/devices/GyroADIS16507Handler.cpp      | 46 ++++++++++++++++++-
 mission/devices/GyroADIS16507Handler.h        |  2 +
 .../GyroADIS16507Definitions.h                |  9 ++++
 3 files changed, 56 insertions(+), 1 deletion(-)

diff --git a/mission/devices/GyroADIS16507Handler.cpp b/mission/devices/GyroADIS16507Handler.cpp
index 3c4639a0..88973f97 100644
--- a/mission/devices/GyroADIS16507Handler.cpp
+++ b/mission/devices/GyroADIS16507Handler.cpp
@@ -14,7 +14,7 @@
 GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
         object_id_t deviceCommunication, CookieIF * comCookie):
         DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
-        configDataset(this) {
+        configDataset(this), breakCountdown(0) {
 #if ADIS16507_DEBUG == 1
     debugDivider = new PeriodicOperationDivider(5);
 #endif
@@ -87,6 +87,37 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
         this->rawPacket = commandBuffer.data();
         break;
     }
+    case(ADIS16507::SELF_TEST_SENSORS): {
+        prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SENSOR_SELF_TEST, 0x00);
+        break;
+    }
+    case(ADIS16507::SELF_TEST_MEMORY): {
+        prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_TEST, 0x00);
+        break;
+    }
+    case(ADIS16507::UPDATE_NV_CONFIGURATION): {
+        prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
+        break;
+    }
+    case(ADIS16507::RESET_SENSOR_CONFIGURATION): {
+        prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FACTORY_CALIBRATION, 0x00);
+        break;
+    }
+    case(ADIS16507::SW_RESET): {
+        prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SOFTWARE_RESET, 0x00);
+        break;
+    }
+    case(ADIS16507::PRINT_CURRENT_CONFIGURATION): {
+#if OBSW_VERBOSE_LEVEL >= 1
+        PoolReadGuard pg(&configDataset);
+        sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4) <<
+                std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
+        sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x" <<
+                configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x" <<
+                configDataset.filterSetting.value << " | DEC_RATE: 0x" <<
+                configDataset.decRateReg.value << std::setfill(' ') << std::endl;
+#endif /* OBSW_VERBOSE_LEVEL >= 1 */
+    }
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
@@ -94,6 +125,8 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
 void GyroADIS16507Handler::fillCommandAndReplyMap() {
     insertInCommandAndReplyMap(ADIS16507::READ_SENSOR_DATA, 1, &primaryDataset);
     insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset);
+    insertInCommandAndReplyMap(ADIS16507::SELF_TEST_SENSORS, 1);
+    insertInCommandAndReplyMap(ADIS16507::SELF_TEST_MEMORY, 1);
 }
 
 ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize,
@@ -223,6 +256,17 @@ uint32_t GyroADIS16507Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t mode
     return 5000;
 }
 
+void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
+        uint8_t valueTwo) {
+    uint8_t secondReg = startReg + 1;
+    startReg |= ADIS16507::WRITE_MASK;
+    secondReg |= ADIS16507::WRITE_MASK;
+    commandBuffer[0] = startReg;
+    commandBuffer[1] = valueOne;
+    commandBuffer[2] = secondReg;
+    commandBuffer[3] = valueTwo;
+}
+
 void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {
     for(size_t idx = 0; idx < len; idx++) {
         commandBuffer[idx * 2] = regList[idx];
diff --git a/mission/devices/GyroADIS16507Handler.h b/mission/devices/GyroADIS16507Handler.h
index fbb9f7bf..a9a62567 100644
--- a/mission/devices/GyroADIS16507Handler.h
+++ b/mission/devices/GyroADIS16507Handler.h
@@ -65,6 +65,8 @@ private:
 #if ADIS16507_DEBUG == 1
     PeriodicOperationDivider* debugDivider;
 #endif
+    Countdown breakCountdown;
+    void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo);
 
     ReturnValue_t handleSensorData(const uint8_t* packet);
 };
diff --git a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h
index 1328c99e..9c675a2e 100644
--- a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h
+++ b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h
@@ -8,6 +8,7 @@
 namespace ADIS16507 {
 
 static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
+static constexpr uint8_t WRITE_MASK = 0b1000'0000;
 
 static constexpr uint32_t GYRO_RANGE = 125;
 static constexpr uint32_t ACCELEROMETER_RANGE = 392;
@@ -47,6 +48,14 @@ static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2;
 static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA;
 static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG;
 
+enum GlobCmds: uint8_t {
+    FACTORY_CALIBRATION = 0b0000'0010,
+    SENSOR_SELF_TEST = 0b0000'0100,
+    FLASH_MEMORY_UPDATE = 0b0000'1000,
+    FLASH_MEMORY_TEST = 0b0001'0000,
+    SOFTWARE_RESET = 0b1000'0000
+};
+
 enum PrimaryPoolIds: lp_id_t {
     ANG_VELOC_X,
     ANG_VELOC_Y,