diff --git a/NUSense/Core/Inc/imu.h b/NUSense/Core/Inc/imu.h index fcf494f..1a93e1c 100644 --- a/NUSense/Core/Inc/imu.h +++ b/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. diff --git a/NUSense/Core/Src/imu.cpp b/NUSense/Core/Src/imu.cpp index 0a08fa3..0635933 100644 --- a/NUSense/Core/Src/imu.cpp +++ b/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. diff --git a/NUSense/Core/Src/main.cpp b/NUSense/Core/Src/main.cpp index 7459c8e..d8ad897 100644 --- a/NUSense/Core/Src/main.cpp +++ b/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()) { } diff --git a/NUSense/Core/Src/nusense/NUSenseIO.hpp b/NUSense/Core/Src/nusense/NUSenseIO.hpp index 3362102..5c13afa 100644 --- a/NUSense/Core/Src/nusense/NUSenseIO.hpp +++ b/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(); diff --git a/NUSense/Core/Src/nusense/NUSenseIO/startup.cpp b/NUSense/Core/Src/nusense/NUSenseIO/startup.cpp index 6149778..8fc5661 100644 --- a/NUSense/Core/Src/nusense/NUSenseIO/startup.cpp +++ b/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() { /*