Implement POSTs

NUSense/Core/Inc/imu.h

@@ -109,6 +109,7 @@ namespace nusense {
         /* Common Defines */
         const uint8_t IMU_READ  = 0x80;
         const uint8_t IMU_WRITE = 0x00;
+        const uint8_t IMU_DEVICE_ID = 0x98;
         enum class Address : uint8_t {
             /* Command Defines */ /* ADR Register Function */
             // Self Test
@@ -662,6 +663,14 @@ namespace nusense {
          */
         void init();
 
+        /*
+         * @brief   performs post-startup tests (POSTs)
+         * @note    reads WHOAMI 1-byte register, asserts equal to 0x98
+         * @param   none
+         * @return  true if everything ok, false if not
+         */
+        bool post();
+
         /*
          * @brief   writes a byte to a register.
          * @note    uses polling, should only be used for beginning.

NUSense/Core/Src/imu.cpp

@@ -98,6 +98,23 @@ namespace nusense {
         */
     }
 
+    /*
+     * @brief   performs post-startup tests (POSTs)
+     * @note    reads WHOAMI 1-byte register, asserts equal to 0x98
+     * @param   none
+     * @return  true if everything ok, false if not
+     */
+    bool IMU::post() {
+        uint8_t who_am_i = 0;
+        read_reg(Address::WHO_AM_I, &who_am_i);
+
+        if (who_am_i != IMU_DEVICE_ID) {
+            return false;
+        }
+
+        return true;
+    }
+
     /*
      * @brief   writes a byte to a register.
      * @note    uses polling, should only be used for beginning.

NUSense/Core/Src/main.cpp

@@ -88,6 +88,11 @@ int main(void) {
 #ifdef RUN_MAIN
     nusense::NUSenseIO nusenseIO;
 
+    bool posted_successfully = nusenseIO.post();
+    if (!posted_successfully) {
+        return -1;
+    }
+
     // Wait for the initial handshake message from NUC
     while (!nusenseIO.handshake_received()) {
     }

NUSense/Core/Src/nusense/NUSenseIO.hpp

@@ -114,6 +114,9 @@ namespace nusense {
                              dynamixel::Chain(ports[5], 5)})
             , imu() {}
 
+        /// @brief   Does basic sanity checks to ensure subsystems exist and communication is working
+        bool post();
+
         /// @brief   Begins the ports and sets the servos up with indirect addresses, etc.
         /// @note    Is loosely inspired by startup() in NUbots/NUbots OpenCR HardwareIO.
         void startup();

NUSense/Core/Src/nusense/NUSenseIO/startup.cpp

@@ -1,6 +1,14 @@
 #include "../NUSenseIO.hpp"
 namespace nusense {
 
+    bool NUSenseIO::post() {
+        if (!this->imu.post()) {
+            return false;
+        }
+
+        return true;
+    }
+
     void NUSenseIO::startup() {
 
         /*