Rotation sensor Implementation

examples/DataCollection.cpp

@@ -5,16 +5,18 @@
 #include "AnalogSensor.h"
 #include "DigitalSensor.h"
 #include "RVC.h"
+#include "RotationSensor.h"
+
 
 constexpr uint32_t BAUD_RATE = 115200;
 
-constexpr bool R_DAMPER_CRITICALITY = false;
-constexpr uint8_t R_DAMPER_PIN = 22;
+constexpr bool THROTTLE1_CRITICALITY = false;
+constexpr uint8_t THROTTLE1_PIN = 22;
 
-constexpr bool L_DAMPER_CRITICALITY = false;
-constexpr uint8_t L_DAMPER_PIN = 23;
+constexpr bool THROTTLE2_CRITICALITY = false;
+constexpr uint8_t THROTTLE2_PIN = 23;
 
-constexpr uint32_t DAMPER_INTERVAL = 100;
+constexpr uint32_t THROTTLE_INTERVAL = 100;
 
 constexpr bool SWITCH_CRITICALITY = true;
 constexpr uint8_t SWITCH_PIN = 38;
@@ -23,30 +25,41 @@ constexpr uint32_t SWITCH_INTERVAL = 100;
 constexpr bool RVC_CRITICALITY = false;
 constexpr uint32_t RVC_INTERVAL = 100;
 
-AnalogSensor throttle1 = AnalogSensor(ReservedIDs::Throttle1Position, R_DAMPER_CRITICALITY, R_DAMPER_PIN, DAMPER_INTERVAL);
-AnalogSensor throttle2 = AnalogSensor(ReservedIDs::Throttle2Position, L_DAMPER_CRITICALITY, L_DAMPER_PIN, DAMPER_INTERVAL);
-DigitalSensor startSwitch = DigitalSensor(ReservedIDs::StartSwitch, SWITCH_CRITICALITY, SWITCH_PIN, SWITCH_INTERVAL);
-Adafruit_BNO08x_RVC bno;
-RVC rvc = RVC(ReservedIDs::RVC, RVC_CRITICALITY, RVC_INTERVAL, &bno);
+constexpr bool STEERING_WHEEL_CRITICALITY = false;
+constexpr uint32_t STEERING_WHEEL_INTERVAL = 100;
+
+AnalogSensor throttle1 = AnalogSensor(ReservedIDs::Throttle1PositionId, THROTTLE1_CRITICALITY, THROTTLE1_PIN, THROTTLE_INTERVAL);
+AnalogSensor throttle2 = AnalogSensor(ReservedIDs::Throttle2PositionId, THROTTLE2_CRITICALITY, THROTTLE2_PIN, THROTTLE_INTERVAL);
+DigitalSensor startSwitch = DigitalSensor(ReservedIDs::StartSwitchId, SWITCH_CRITICALITY, SWITCH_PIN, SWITCH_INTERVAL);
+Adafruit_BNO08x_RVC bno1;
+RVC rvc = RVC(ReservedIDs::RVCId, RVC_CRITICALITY, RVC_INTERVAL, &bno1);
+// rvc cannot share a sensor with rotationsensor
+Adafruit_BNO08x bno2;
+RotationSensor steeringWheel = RotationSensor(
+    ReservedIDs::SteeringWheelAngleId, STEERING_WHEEL_CRITICALITY, STEERING_WHEEL_INTERVAL, &bno2
+);
 
 Sensor* sensors[] = {
     &throttle1,
     &throttle2,
     &startSwitch,
     &rvc,
+    &steeringWheel
 };
 size_t numSensors = sizeof(sensors) / sizeof(sensors[0]);
 
 void setup() {
     Serial.begin(BAUD_RATE);
     Serial1.begin(BAUD_RATE);
+    Serial2.begin(BAUD_RATE);
     rvc.begin(&Serial1);
+    steeringWheel.begin(&Serial2);
 }
 
 void loop() {
     for (size_t sensorIndex = 0; sensorIndex < numSensors; sensorIndex++)
     {
-        if (Sensor* sensor = sensors[sensorIndex]; sensor->healthCheck() && sensor->ready())
+        if (Sensor* sensor = sensors[sensorIndex]; sensor->ready())
         {
             const SensorData data = sensor->read();
             const size_t msgCount = data.getMsgCount();

include/RotationSensor.h

@@ -0,0 +1,38 @@
+#ifndef ROTATIONSENSOR_H
+#define ROTATIONSENSOR_H
+
+#include <Arduino.h>
+#include <Adafruit_BNO08x.h>
+#include "Sensor.h"
+#include "RotationVectors.h"
+
+class RotationSensor final : public Sensor {
+    Adafruit_BNO08x* imu = nullptr;
+    sh2_SensorId_t report = 0;
+    sh2_SensorValue_t* sensorValue = nullptr;
+    euler_t* ypr = nullptr;
+    static void updateYPR(euler_t* ypr, const RotationVector& rv);
+    static void setMsg(SensorData* sensorData, uint8_t* msgIndex, float value, RVCSubIDs subSensorId);
+    static void printValue(const char label[], float value, const char units[]);
+    /**
+     * Gets the change in theta between two angles in degrees and applies it
+     * The angle is wrapped across the -180/180 degree boundary and normalized to [-360, 360].
+     * @param newAngle expected to be in range [-180, 180]
+     * @param prevAngle expected to be in range [-360, 360]
+     * @return the result angle in range [-360, 360]
+     */
+    static float updateAngle(float newAngle, float prevAngle);
+public:
+    RotationSensor(
+        ReservedIDs id, bool criticality, uint32_t readInterval, Adafruit_BNO08x* imu,
+        sh2_SensorId_t report = SH2_GAME_ROTATION_VECTOR
+    );
+    ~RotationSensor() override;
+    void begin(HardwareSerial* serial);
+    Health healthCheck() const override;
+    bool ready() override;
+    SensorData read() override;
+    void debugPrint(const CAN_message_t& canMsg) const override;
+};
+
+#endif //ROTATIONSENSOR_H

include/RotationVectors.h

@@ -0,0 +1,67 @@
+#ifndef ROTATIONVECTORS_H
+#define ROTATIONVECTORS_H
+
+#define D_180 180.0f
+#define D_360 360.0f
+/** Multiply with F_RAD_TO_DEG to convert from radians to degrees */
+#define TO_DEG static_cast<float>(180.0 / M_PI)
+
+/** Struct containing Euler angles in Degrees */
+struct euler_t
+{
+    // The x-axis angle
+    float roll;
+    // The y-axis angle
+    float pitch;
+    // The z-axis angle
+    float yaw;
+};
+
+/**
+ * Calculates the roll (x-axis) angle from a normalized quaternion
+ * @return the roll
+ */
+float quaternionToRoll(float q0, float q1, float q2, float q3);
+
+/**
+ * Calculates the pitch (y-axis) angle from a normalized quaternion
+ * @return the pitch
+ */
+float quaternionToPitch(float q0, float q1, float q2, float q3);
+
+/**
+ * Calculates the yaw (z-axis) angle from a normalized quaternion
+ * @return the yaw
+ */
+float quaternionToYaw(float q0, float q1, float q2, float q3);
+
+/**
+ * Calculates the Euler angles from an unnormalized quaternion.
+ * @return the Euler angles
+ */
+euler_t quaternionToEuler(float qr, float qi, float qj, float qk);
+
+
+/** Quaternion representation of an absolute rotation vector */
+struct RotationVector
+{
+    float i = 0.0f;
+    float j = 0.0f;
+    float k = 0.0f;
+    float real = 0.0f;
+    RotationVector() = default;
+    /**
+     * @tparam T Any class that has implements floats: i, j, k, and real
+     * @param rv rotation vector to create from
+     */
+    template<typename T>
+    explicit RotationVector(const T& rv)
+    {
+        i = rv.i;
+        j = rv.j;
+        k = rv.k;
+        real = rv.real;
+    }
+};
+
+#endif //ROTATIONVECTORS_H

include/Sensor.h

@@ -28,7 +28,7 @@ class Sensor
 {
 protected:
     /** the sensor id */
-    ReservedIDs id = ReservedIDs::INVALID;
+    uint32_t id = ReservedIDs::INVALIDId;
     /** the sensor criticality */
     bool criticality = false;
     /** the interval a sensor should read at */
@@ -37,8 +37,6 @@ class Sensor
     uint32_t lastRead = 0;
 public:
     virtual ~Sensor() = default;
-    /** @return the sensor id */
-    [[nodiscard]] ReservedIDs getId() const;
     /** @return the sensor criticality */
     [[nodiscard]] bool isCritical() const;
     /** @return the health of a sensor object */

include/SensorData.h

@@ -7,13 +7,13 @@
 
 class SensorData
 {
-    ReservedIDs id = ReservedIDs::INVALID;
+    uint32_t id = ReservedIDs::INVALIDId;
     size_t msgCount = 0;
     CAN_message_t* msgs = nullptr;
     CAN_message_t newMsg(const uint8_t* buf, size_t len) const;
 public:
     SensorData() = default;
-    SensorData(ReservedIDs id, size_t msgCount);
+    SensorData(uint32_t id, size_t msgCount);
     SensorData(const SensorData& other);
     SensorData& operator=(const SensorData& other);
     ~SensorData();

library.json

@@ -25,6 +25,10 @@
       "name": "FlexCAN_T4",
       "version": "https://github.com/tonton81/FlexCAN_T4.git"
     },
+    {
+      "name": "Adafruit_BusIO",
+      "version": "https://github.com/adafruit/Adafruit_BusIO.git"
+    },
     {
       "name": "Adafruit_BNO08x_RVC",
       "version": "https://github.com/adafruit/Adafruit_BNO08x_RVC.git"
@@ -33,9 +37,19 @@
       "name": "Adafruit_BNO08x",
       "version": "https://github.com/adafruit/Adafruit_BNO08x.git"
     },
+    {
+      "name": "Adafruit_Sensor",
+      "version": "https://github.com/adafruit/Adafruit_Sensor.git"
+    },
     {
       "name": "Utils",
       "version": "https://github.com/BYU-Racing/Utils.git"
+    },
+    {
+      "name": "Wire"
+    },
+    {
+      "name": "SPI"
     }
   ],
   "frameworks": "arduino",

src/AnalogSensor.cpp

@@ -19,11 +19,11 @@ Health AnalogSensor::healthCheck() const
 SensorData AnalogSensor::read()
 {
     lastRead = millis();
-    SensorData sensorData = SensorData(id, 1);
     uint8_t buf[sizeof(int)] = {};
     BufferPacker<sizeof(int)> packer;
     packer.pack(analogRead(pin));
     packer.deepCopyTo(buf);
+    SensorData sensorData = SensorData(id, 1);
     sensorData.setMsg(buf, sizeof(int));
     return sensorData;
 }

src/DigitalSensor.cpp

@@ -27,8 +27,8 @@ Health DigitalSensor::healthCheck() const
 SensorData DigitalSensor::read()
 {
     lastRead = millis();
-    SensorData sensorData = SensorData(id, 1);
     const uint8_t buf[sizeof(uint8_t)] = { digitalRead(pin) };
+    SensorData sensorData = SensorData(id, 1);
     sensorData.setMsg(buf, sizeof(uint8_t));
     return sensorData;
 }

src/RVC.cpp

@@ -47,12 +47,12 @@ SensorData RVC::read()
     lastRead = millis();
     SensorData sensorData = SensorData(id, rvcMsgCount);
     uint8_t msgIndex = 0;
-    setMsg(&sensorData, &msgIndex, heading->x_accel, RVCSubIDs::X_Accel);
-    setMsg(&sensorData, &msgIndex, heading->y_accel, RVCSubIDs::Y_Accel);
-    setMsg(&sensorData, &msgIndex, heading->z_accel, RVCSubIDs::Z_Accel);
-    setMsg(&sensorData, &msgIndex, heading->roll, RVCSubIDs::Roll);
-    setMsg(&sensorData, &msgIndex, heading->pitch, RVCSubIDs::Pitch);
-    setMsg(&sensorData, &msgIndex, heading->yaw, RVCSubIDs::Yaw);
+    setMsg(&sensorData, &msgIndex, heading->x_accel, RVCSubIDs::X_AccelId);
+    setMsg(&sensorData, &msgIndex, heading->y_accel, RVCSubIDs::Y_AccelId);
+    setMsg(&sensorData, &msgIndex, heading->z_accel, RVCSubIDs::Z_AccelId);
+    setMsg(&sensorData, &msgIndex, heading->roll, RVCSubIDs::RollId);
+    setMsg(&sensorData, &msgIndex, heading->pitch, RVCSubIDs::PitchId);
+    setMsg(&sensorData, &msgIndex, heading->yaw, RVCSubIDs::YawId);
     return sensorData;
 }
 
@@ -75,22 +75,22 @@ void RVC::debugPrint(const CAN_message_t& canMsg) const
     const float value = unpacker.unpack<float>();
     switch (id)
     {
-    case RVCSubIDs::X_Accel:
+    case RVCSubIDs::X_AccelId:
         printValue("X Acceleration", value, "m/s^2");
         break;
-    case RVCSubIDs::Y_Accel:
+    case RVCSubIDs::Y_AccelId:
         printValue("Y Acceleration", value, "m/s^2");
         break;
-    case RVCSubIDs::Z_Accel:
+    case RVCSubIDs::Z_AccelId:
         printValue("Z Acceleration", value, "m/s^2");
         break;
-    case RVCSubIDs::Roll:
+    case RVCSubIDs::RollId:
         printValue("Roll", value, "deg");
         break;
-    case RVCSubIDs::Pitch:
+    case RVCSubIDs::PitchId:
         printValue("Pitch", value, "deg");
         break;
-    case RVCSubIDs::Yaw:
+    case RVCSubIDs::YawId:
         printValue("Yaw", value, "deg");
         break;
     default: