diff --git a/examples/DataCollection.cpp b/examples/DataCollection.cpp index 9c05877..f85b316 100644 --- a/examples/DataCollection.cpp +++ b/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(); diff --git a/include/RotationSensor.h b/include/RotationSensor.h new file mode 100644 index 0000000..5682fd4 --- /dev/null +++ b/include/RotationSensor.h @@ -0,0 +1,38 @@ +#ifndef ROTATIONSENSOR_H +#define ROTATIONSENSOR_H + +#include +#include +#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 diff --git a/include/RotationVectors.h b/include/RotationVectors.h new file mode 100644 index 0000000..39c48a6 --- /dev/null +++ b/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(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 + explicit RotationVector(const T& rv) + { + i = rv.i; + j = rv.j; + k = rv.k; + real = rv.real; + } +}; + +#endif //ROTATIONVECTORS_H diff --git a/include/Sensor.h b/include/Sensor.h index fc4050d..c8ed651 100644 --- a/include/Sensor.h +++ b/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 */ diff --git a/include/SensorData.h b/include/SensorData.h index 7c127a8..526e447 100644 --- a/include/SensorData.h +++ b/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(); diff --git a/library.json b/library.json index d7e6e44..42ecf4d 100644 --- a/library.json +++ b/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", diff --git a/src/AnalogSensor.cpp b/src/AnalogSensor.cpp index 64d5f26..fa958dc 100644 --- a/src/AnalogSensor.cpp +++ b/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 packer; packer.pack(analogRead(pin)); packer.deepCopyTo(buf); + SensorData sensorData = SensorData(id, 1); sensorData.setMsg(buf, sizeof(int)); return sensorData; } diff --git a/src/DigitalSensor.cpp b/src/DigitalSensor.cpp index 9546f72..0351c53 100644 --- a/src/DigitalSensor.cpp +++ b/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; } diff --git a/src/RVC.cpp b/src/RVC.cpp index 40a19b2..f70829c 100644 --- a/src/RVC.cpp +++ b/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(); 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: diff --git a/src/RotationSensor.cpp b/src/RotationSensor.cpp new file mode 100644 index 0000000..1e611a5 --- /dev/null +++ b/src/RotationSensor.cpp @@ -0,0 +1,141 @@ +#include "RotationSensor.h" +#include + +#define BUFFER_SIZE 8 + +RotationSensor::RotationSensor( + const ReservedIDs id, const bool criticality, const uint32_t readInterval, Adafruit_BNO08x* imu, + const sh2_SensorId_t report +) +{ + this->id = id; + this->criticality = criticality; + this->readInterval = readInterval; + this->imu = imu; + this->report = report; + sensorValue = new sh2_SensorValue_t; + ypr = new euler_t; +} + +RotationSensor::~RotationSensor() +{ + delete sensorValue; + delete ypr; +} + +void RotationSensor::begin(HardwareSerial* serial) +{ + constexpr uint16_t millisToMicros = 1000; + imu->begin_UART(serial, static_cast(id)); + imu->enableReport(report, readInterval * millisToMicros); +} + +Health RotationSensor::healthCheck() const +{ + if (imu != nullptr) + { + return HEALTHY; + } + if (criticality) + { + return CRITICAL; + } + return UNRESPONSIVE; +} // Unsure if this is best health check? + +bool RotationSensor::ready() +{ + if (millis() - lastRead >= readInterval) + { + return imu->getSensorEvent(sensorValue); + } + return false; +} + +float RotationSensor::updateAngle(const float newAngle, const float prevAngle) +{ + const float normNewAngle = fmod(newAngle, D_180); + const float normPrevAngle = fmod(prevAngle, D_360); + float delta = normNewAngle - normPrevAngle; + if (delta > D_180) + { + delta -= D_360; + } else if (delta < -D_180) + { + delta += D_360; + } + return fmod(normPrevAngle + delta, D_360); // Don't allow more than 1 full rotation +} + +void RotationSensor::updateYPR(euler_t* ypr, const RotationVector& rv) +{ + const auto [roll, pitch, yaw] = quaternionToEuler(rv.real, rv.i, rv.j, rv.k); + ypr->roll = updateAngle(roll, ypr->roll); + ypr->pitch = updateAngle(pitch, ypr->pitch); + ypr->yaw = updateAngle(yaw, ypr->yaw); +} + + +void RotationSensor::setMsg(SensorData* sensorData, uint8_t* msgIndex, const float value, const RVCSubIDs subSensorId) +{ + uint8_t buf[sizeof(float) + sizeof(RVCSubIDs)]; + BufferPacker packer; + packer.pack(subSensorId); + packer.pack(value); + packer.deepCopyTo(buf); + sensorData->setMsg(buf, sizeof(float) + sizeof(RVCSubIDs), *msgIndex); + (*msgIndex)++; +} + +SensorData RotationSensor::read() +{ + lastRead = millis(); + // Defaulted to "empty"" state in case a switch on sensorValue.sensorId isn't matched + SensorData sensorData; + + if (sensorValue->sensorId == report) // This is only necessary if the bno0x is used for multiple sensor reports + { + sensorData = SensorData(id, 3); + const RotationVector rv(sensorValue->un.gameRotationVector); + updateYPR(ypr, rv); + uint8_t msgIndex = 0; + setMsg(&sensorData, &msgIndex, ypr->roll, RVCSubIDs::RollId); + setMsg(&sensorData, &msgIndex, ypr->pitch, RVCSubIDs::PitchId); + setMsg(&sensorData, &msgIndex, ypr->yaw, RVCSubIDs::YawId); + } + return sensorData; +} + +void RotationSensor::printValue(const char label[], const float value, const char units[]) +{ + Serial.print(label); + Serial.print(": "); + Serial.print(value); + Serial.print(" "); + Serial.println(units); +} + +void RotationSensor::debugPrint(const CAN_message_t& canMsg) const +{ + Serial.println("Rotation Sensor CAN Message:"); + Serial.print("Timestamp: "); + Serial.println(canMsg.timestamp); + BufferPacker unpacker(canMsg.buf); + const RVCSubIDs id = unpacker.unpack(); + const float value = unpacker.unpack(); + switch (id) + { + case RVCSubIDs::RollId: + printValue("Roll", value, "deg"); + break; + case RVCSubIDs::PitchId: + printValue("Pitch", value, "deg"); + break; + case RVCSubIDs::YawId: + printValue("Yaw", value, "deg"); + break; + default: + break; + } + Serial.println(); +} diff --git a/src/RotationVectors.cpp b/src/RotationVectors.cpp new file mode 100644 index 0000000..f248e07 --- /dev/null +++ b/src/RotationVectors.cpp @@ -0,0 +1,65 @@ +#include "RotationVectors.h" + +#include +#include "Arduino.h" + +float quaternionToRoll(const float q0, const float q1, const float q2, const float q3) +{ + // Combined Sine of Roll and Cosine of Pitch + const float sinRcosP = 2.0f * (q0 * q1 + q2 * q3); + // Cosine of Roll and Cosine of Pitch + const float cosRcosP = 1.0f - 2.0f * (q1 * q1 + q2 * q2); + // Roll (x-axis) in degrees + return atan2f(sinRcosP, cosRcosP) * TO_DEG; +} + +float quaternionToPitch(const float q0, const float q1, const float q2, const float q3) +{ + // Sine of Pitch + const float sinP = 2.0f * (q0 * q2 - q3 * q1); + // Floating Point Rounding Check + if (fabsf(sinP) >= 1) + { + // Clamp pitch (y-axis) to ± 90 degrees + return static_cast(copysign(M_PI / 2, sinP)) * TO_DEG; + } + // Pitch (y-axis) in degrees + return asinf(sinP) * TO_DEG; +} + +float quaternionToYaw(const float q0, const float q1, const float q2, const float q3) +{ + // Combined Sine of Yaw and Cosine of Pitch + const float sinYcosP = 2.0f * (q0 * q3 + q1 * q2); + // Cosine of Yaw and Cosine of Pitch + const float cosYcosP = 1.0f - 2.0f * (q2 * q2 + q3 * q3); + // Yaw (z-axis) in degrees + return atan2f(sinYcosP, cosYcosP) * TO_DEG; +} + +euler_t quaternionToEuler(const float qr, const float qi, const float qj, const float qk) +{ + // Normalize quaternion + + // The magnitude of the quaternion (Euclidean norm) + const float norm = 1.0f / sqrtf(qr * qr + qi * qi + qj * qj + qk * qk); + // Real scalar part of the quaternion + const float q0 = qr * norm; + // Imaginary vector part of the quaternion + const float q1 = qi * norm; + // Imaginary vector part of the quaternion + const float q2 = qj * norm; + // Imaginary vector part of the quaternion + const float q3 = qk * norm; + + // Compute Euler Angles + + // X-axis + const float roll = quaternionToRoll(q0, q1, q2, q3); + // Y-axis + const float pitch = quaternionToPitch(q0, q1, q2, q3); + // Z-axis + const float yaw = quaternionToYaw(q0, q1, q2, q3); + + return euler_t { yaw, pitch, roll }; +} diff --git a/src/Sensor.cpp b/src/Sensor.cpp index 30ce711..c5213ca 100644 --- a/src/Sensor.cpp +++ b/src/Sensor.cpp @@ -2,8 +2,6 @@ #define NUM_BITS 7 -ReservedIDs Sensor::getId() const { return id; } - bool Sensor::isCritical() const { return criticality; } bool Sensor::ready() { return millis() - lastRead >= readInterval; } diff --git a/src/SensorData.cpp b/src/SensorData.cpp index ab90610..e330238 100644 --- a/src/SensorData.cpp +++ b/src/SensorData.cpp @@ -1,6 +1,6 @@ #include "SensorData.h" -SensorData::SensorData(const ReservedIDs id, const size_t msgCount) +SensorData::SensorData(const uint32_t id, const size_t msgCount) { this->id = id; this->msgCount = msgCount;