From 867492d93ee8e249e9dd7e56c8235fecf7adba6a Mon Sep 17 00:00:00 2001 From: btoone Date: Sun, 13 Oct 2024 23:40:41 -0600 Subject: [PATCH 01/11] Minor cleanup --- include/RotationSensor.h | 2 +- src/RotationVectors.cpp | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/include/RotationSensor.h b/include/RotationSensor.h index ba7635d..a4edbd7 100644 --- a/include/RotationSensor.h +++ b/include/RotationSensor.h @@ -16,7 +16,7 @@ class RotationSensor final : public Sensor { static void printValue(const char label[], float value, const char units[]); public: RotationSensor( - uint32_t id, bool criticality, uint32_t readInterval, Adafruit_BNO08x* imu, + ReservedIDs id, bool criticality, uint32_t readInterval, Adafruit_BNO08x* imu, sh2_SensorId_t report = SH2_GAME_ROTATION_VECTOR ); ~RotationSensor() override; diff --git a/src/RotationVectors.cpp b/src/RotationVectors.cpp index 4d70bb0..dfd293c 100644 --- a/src/RotationVectors.cpp +++ b/src/RotationVectors.cpp @@ -1,10 +1,9 @@ #include "RotationVectors.h" -constexpr float D_180 = 180.0f; -constexpr float D_360 = 360.0f; - +#define D_180 180.0f +#define D_360 360.0f /** Multiply with F_RAD_TO_DEG to convert from radians to degrees */ -constexpr float F_RAD_TO_DEG = static_cast(RAD_TO_DEG); +#define F_RAD_TO_DEG static_cast(RAD_TO_DEG); float quaternionToRoll(const float q0, const float q1, const float q2, const float q3) { From 87ff043e8e2bca763ddccec6404da9ebff48d50f Mon Sep 17 00:00:00 2001 From: btoone Date: Sun, 13 Oct 2024 23:40:47 -0600 Subject: [PATCH 02/11] Minor cleanup --- include/RotationVectors.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/RotationVectors.h b/include/RotationVectors.h index 129a252..c1b318a 100644 --- a/include/RotationVectors.h +++ b/include/RotationVectors.h @@ -1,8 +1,6 @@ #ifndef ROTATIONVECTORS_H #define ROTATIONVECTORS_H -#include - /** Struct containing Euler angles in Degrees */ struct euler_t { From 09158d2e452c2de07fc369710b706408a792dbde Mon Sep 17 00:00:00 2001 From: btoone Date: Mon, 14 Oct 2024 00:14:42 -0600 Subject: [PATCH 03/11] Using new Utils and EnumClass --- include/RotationSensor.h | 2 +- src/RVC.cpp | 4 +- src/RotationSensor.cpp | 157 +++++++++++++++++++++++++++++++++++++++ src/RotationVectors.cpp | 16 ++-- src/SensorData.cpp | 2 +- 5 files changed, 170 insertions(+), 11 deletions(-) create mode 100644 src/RotationSensor.cpp diff --git a/include/RotationSensor.h b/include/RotationSensor.h index a4edbd7..d861e60 100644 --- a/include/RotationSensor.h +++ b/include/RotationSensor.h @@ -12,7 +12,7 @@ class RotationSensor final : public Sensor { 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, uint8_t subSensorId); + static void setMsg(SensorData* sensorData, uint8_t* msgIndex, float value, RVCSubIDs subSensorId); static void printValue(const char label[], float value, const char units[]); public: RotationSensor( diff --git a/src/RVC.cpp b/src/RVC.cpp index aeaea25..5088d90 100644 --- a/src/RVC.cpp +++ b/src/RVC.cpp @@ -51,7 +51,7 @@ SensorData RVC::read() // rvc->read is called in ready() check setMsg(&sensorData, &msgIndex, heading->x_accel, RVCSubIDs::X_Accel); setMsg(&sensorData, &msgIndex, heading->y_accel, RVCSubIDs::Y_Accel); - setMsg(&sensorData, &msgIndex, heading->z_accel, Z_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); @@ -74,7 +74,7 @@ void RVC::debugPrint(const CAN_message_t& canMsg) const Serial.print("Timestamp: "); Serial.println(canMsg.timestamp); BufferPacker unpacker(canMsg.buf); - const RVCSubIDs id = unpacker.unpack(); + const uint8_t id = unpacker.unpack(); const float value = unpacker.unpack(); switch (id) { diff --git a/src/RotationSensor.cpp b/src/RotationSensor.cpp new file mode 100644 index 0000000..d4ea24b --- /dev/null +++ b/src/RotationSensor.cpp @@ -0,0 +1,157 @@ +#include "RotationSensor.h" +#include + +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; +} + +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(uint8_t)]; + BufferPacker packer; + packer.pack(subSensorId); + packer.pack(value); + packer.deepCopyTo(buf); + sensorData->setMsg(buf, sizeof(float) + sizeof(uint8_t), *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) // TODO: Remove multiple options after testing which Rotation Vector is best + { + RotationVector rv; + + switch (sensorValue->sensorId) + { + case SH2_ROTATION_VECTOR: + /// Sensor fusion of Accelerometer, Gyroscope, and Magnetometer + /// Referenced to Gravity *and* Magnetic North + + rv = RotationVector(sensorValue->un.rotationVector); + break; + case SH2_GAME_ROTATION_VECTOR: + /// Sensor fusion of Accelerometer and Gyroscope + /// Referenced to Gravity + /// Avoids sudden jumps due to Magnetometer based corrections + + rv = RotationVector(sensorValue->un.gameRotationVector); + break; + case SH2_GEOMAGNETIC_ROTATION_VECTOR: + /// Sensor fusion of Accelerometer and Magnetometer + + rv = RotationVector(sensorValue->un.geoMagRotationVector); + break; + case SH2_GYRO_INTEGRATED_RV: + /// Sensor fusion of Accelerometer and Gyroscope + /// Made for rotation? + + rv = RotationVector(sensorValue->un.gyroIntegratedRV); + break; + default: + // A missing match will lead to an "empty" sensorData with no values + return sensorData; + } + sensorData = SensorData(id, 3); + + updateYPR(ypr, rv); + + uint8_t msgIndex = 0; // used for prependId and message index + + setMsg(&sensorData, &msgIndex, ypr->roll, RVCSubIDs::Roll); + setMsg(&sensorData, &msgIndex, ypr->pitch, RVCSubIDs::Pitch); + setMsg(&sensorData, &msgIndex, ypr->yaw, RVCSubIDs::Yaw); + } + 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::Roll: + printValue("Roll", value, "deg"); + break; + case RVCSubIDs::Pitch: + printValue("Pitch", value, "deg"); + break; + case RVCSubIDs::Yaw: + printValue("Yaw", value, "deg"); + break; + default: + break; + } + Serial.println(); +} diff --git a/src/RotationVectors.cpp b/src/RotationVectors.cpp index dfd293c..9596d79 100644 --- a/src/RotationVectors.cpp +++ b/src/RotationVectors.cpp @@ -1,9 +1,11 @@ #include "RotationVectors.h" +#include + #define D_180 180.0f #define D_360 360.0f /** Multiply with F_RAD_TO_DEG to convert from radians to degrees */ -#define F_RAD_TO_DEG static_cast(RAD_TO_DEG); +#define TO_DEG static_cast(180.0 / M_PI) float quaternionToRoll(const float q0, const float q1, const float q2, const float q3) { @@ -12,7 +14,7 @@ float quaternionToRoll(const float q0, const float q1, const float q2, const flo // Cosine of Roll and Cosine of Pitch const float cosRcosP = 1.0f - 2.0f * (q1 * q1 + q2 * q2); // Roll (x-axis) in degrees - return atan2(sinRcosP, cosRcosP) * F_RAD_TO_DEG; + return atan2f(sinRcosP, cosRcosP) * TO_DEG; } float quaternionToPitch(const float q0, const float q1, const float q2, const float q3) @@ -20,13 +22,13 @@ float quaternionToPitch(const float q0, const float q1, const float q2, const fl // Sine of Pitch const float sinP = 2.0f * (q0 * q2 - q3 * q1); // Floating Point Rounding Check - if (fabs(sinP) >= 1) + if (fabsf(sinP) >= 1) { // Clamp pitch (y-axis) to ± 90 degrees - return static_cast(copysign(M_PI / 2, sinP)) * F_RAD_TO_DEG; + return static_cast(copysign(M_PI / 2, sinP)) * TO_DEG; } // Pitch (y-axis) in degrees - return asin(sinP) * F_RAD_TO_DEG; + return asinf(sinP) * TO_DEG; } float quaternionToYaw(const float q0, const float q1, const float q2, const float q3) @@ -36,7 +38,7 @@ float quaternionToYaw(const float q0, const float q1, const float q2, const floa // Cosine of Yaw and Cosine of Pitch const float cosYcosP = 1.0f - 2.0f * (q2 * q2 + q3 * q3); // Yaw (z-axis) in degrees - return atan2(sinYcosP, cosYcosP) * F_RAD_TO_DEG; + return atan2f(sinYcosP, cosYcosP) * TO_DEG; } euler_t quaternionToEuler(const float qr, const float qi, const float qj, const float qk) @@ -44,7 +46,7 @@ euler_t quaternionToEuler(const float qr, const float qi, const float qj, const // Normalize quaternion // The magnitude of the quaternion (Euclidean norm) - const float norm = 1.0f / sqrt(qr * qr + qi * qi + qj * qj + qk * qk); + 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 diff --git a/src/SensorData.cpp b/src/SensorData.cpp index 485f82b..ab90610 100644 --- a/src/SensorData.cpp +++ b/src/SensorData.cpp @@ -49,7 +49,7 @@ CAN_message_t* SensorData::getMsgs() const { return msgs; } CAN_message_t SensorData::newMsg(const uint8_t* buf, const size_t len) const { CAN_message_t msg; - msg.id = id; + msg.id = static_cast(id); const size_t maxLen = len > 8 ? 8 : len; // Len can't exceed 8 msg.len = maxLen; for (size_t i = 0; i < maxLen; i++) From 99ceb2ac476b85010da6fb93528b0b1abacdb39e Mon Sep 17 00:00:00 2001 From: btoone Date: Mon, 14 Oct 2024 00:58:53 -0600 Subject: [PATCH 04/11] Fix for buffer packer oversizing --- src/RVC.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RVC.cpp b/src/RVC.cpp index 5088d90..86ef74a 100644 --- a/src/RVC.cpp +++ b/src/RVC.cpp @@ -73,8 +73,8 @@ void RVC::debugPrint(const CAN_message_t& canMsg) const Serial.println("RVC CAN Message:"); Serial.print("Timestamp: "); Serial.println(canMsg.timestamp); - BufferPacker unpacker(canMsg.buf); - const uint8_t id = unpacker.unpack(); + BufferPacker unpacker(canMsg.buf); + const RVCSubIDs id = unpacker.unpack(); const float value = unpacker.unpack(); switch (id) { From 9a553d0c6dce4ec2cd9d149bd621ff405b19cd77 Mon Sep 17 00:00:00 2001 From: btoone Date: Mon, 14 Oct 2024 00:59:30 -0600 Subject: [PATCH 05/11] Fix for angles that are massively overflowed (1.01234e30) --- src/RotationVectors.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/RotationVectors.cpp b/src/RotationVectors.cpp index 9596d79..3300f26 100644 --- a/src/RotationVectors.cpp +++ b/src/RotationVectors.cpp @@ -1,9 +1,11 @@ #include "RotationVectors.h" #include +#include "Arduino.h" #define D_180 180.0f #define D_360 360.0f +#define D_720 720.0f /** Multiply with F_RAD_TO_DEG to convert from radians to degrees */ #define TO_DEG static_cast(180.0 / M_PI) @@ -70,6 +72,7 @@ euler_t quaternionToEuler(const float qr, const float qi, const float qj, const float normalize180(float angle) { + angle = fmod(angle, D_720); // Clamp angle to within 2 rotations while (angle > D_180) { angle -= D_360; @@ -96,7 +99,9 @@ float normalize360(float angle) float updateAngle(const float newAngle, const float prevAngle) { - float delta = normalize180(newAngle) - normalize180(prevAngle); + const float normNewAngle = normalize180(newAngle); + const float normPrevAngle = normalize180(prevAngle); + float delta = normNewAngle - normPrevAngle; if (delta > D_180) { delta -= D_360; @@ -104,5 +109,5 @@ float updateAngle(const float newAngle, const float prevAngle) { delta += D_360; } - return normalize360(prevAngle + delta); + return normalize360(normPrevAngle + delta); } From 0b5d2b6e1ddbe0db458477b832bd79cf90778661 Mon Sep 17 00:00:00 2001 From: btoone Date: Mon, 14 Oct 2024 01:36:06 -0600 Subject: [PATCH 06/11] Working implementation of rotation sensor. Rotation around the x arrow on the sensor results in cleanest angle change (to the yaw value). We can remove the use of other values for speed sake if we care. --- include/RotationSensor.h | 8 +++++ include/RotationVectors.h | 19 +++--------- src/RotationSensor.cpp | 64 ++++++++++++++------------------------- src/RotationVectors.cpp | 48 ----------------------------- 4 files changed, 36 insertions(+), 103 deletions(-) diff --git a/include/RotationSensor.h b/include/RotationSensor.h index d861e60..5682fd4 100644 --- a/include/RotationSensor.h +++ b/include/RotationSensor.h @@ -14,6 +14,14 @@ class RotationSensor final : public Sensor { 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, diff --git a/include/RotationVectors.h b/include/RotationVectors.h index c1b318a..39c48a6 100644 --- a/include/RotationVectors.h +++ b/include/RotationVectors.h @@ -1,6 +1,11 @@ #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 { @@ -36,20 +41,6 @@ float quaternionToYaw(float q0, float q1, float q2, float q3); */ euler_t quaternionToEuler(float qr, float qi, float qj, float qk); -/** @return an angle normalized between [-180, 180] */ -float normalize180(float angle); - -/** @return an angle normalized between [-360, 360] */ -float normalize360(float angle); - -/** - * 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] - */ -float updateAngle(float newAngle, float prevAngle); /** Quaternion representation of an absolute rotation vector */ struct RotationVector diff --git a/src/RotationSensor.cpp b/src/RotationSensor.cpp index d4ea24b..7cec83a 100644 --- a/src/RotationSensor.cpp +++ b/src/RotationSensor.cpp @@ -50,6 +50,21 @@ bool RotationSensor::ready() 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); @@ -58,14 +73,15 @@ void RotationSensor::updateYPR(euler_t* ypr, const RotationVector& rv) 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(uint8_t)]; - BufferPacker packer; + uint8_t buf[sizeof(float) + sizeof(RVCSubIDs)]; + BufferPacker packer; packer.pack(subSensorId); packer.pack(value); packer.deepCopyTo(buf); - sensorData->setMsg(buf, sizeof(float) + sizeof(uint8_t), *msgIndex); + sensorData->setMsg(buf, sizeof(float) + sizeof(RVCSubIDs), *msgIndex); (*msgIndex)++; } @@ -75,46 +91,12 @@ SensorData RotationSensor::read() // Defaulted to "empty"" state in case a switch on sensorValue.sensorId isn't matched SensorData sensorData; - if (sensorValue->sensorId == report) // TODO: Remove multiple options after testing which Rotation Vector is best + if (sensorValue->sensorId == report) // This is only necessary if the bno0x is used for multiple sensor reports { - RotationVector rv; - - switch (sensorValue->sensorId) - { - case SH2_ROTATION_VECTOR: - /// Sensor fusion of Accelerometer, Gyroscope, and Magnetometer - /// Referenced to Gravity *and* Magnetic North - - rv = RotationVector(sensorValue->un.rotationVector); - break; - case SH2_GAME_ROTATION_VECTOR: - /// Sensor fusion of Accelerometer and Gyroscope - /// Referenced to Gravity - /// Avoids sudden jumps due to Magnetometer based corrections - - rv = RotationVector(sensorValue->un.gameRotationVector); - break; - case SH2_GEOMAGNETIC_ROTATION_VECTOR: - /// Sensor fusion of Accelerometer and Magnetometer - - rv = RotationVector(sensorValue->un.geoMagRotationVector); - break; - case SH2_GYRO_INTEGRATED_RV: - /// Sensor fusion of Accelerometer and Gyroscope - /// Made for rotation? - - rv = RotationVector(sensorValue->un.gyroIntegratedRV); - break; - default: - // A missing match will lead to an "empty" sensorData with no values - return sensorData; - } sensorData = SensorData(id, 3); - + const RotationVector rv(sensorValue->un.gameRotationVector); updateYPR(ypr, rv); - - uint8_t msgIndex = 0; // used for prependId and message index - + uint8_t msgIndex = 0; setMsg(&sensorData, &msgIndex, ypr->roll, RVCSubIDs::Roll); setMsg(&sensorData, &msgIndex, ypr->pitch, RVCSubIDs::Pitch); setMsg(&sensorData, &msgIndex, ypr->yaw, RVCSubIDs::Yaw); @@ -136,7 +118,7 @@ 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); + BufferPacker unpacker(canMsg.buf); const RVCSubIDs id = unpacker.unpack(); const float value = unpacker.unpack(); switch (id) diff --git a/src/RotationVectors.cpp b/src/RotationVectors.cpp index 3300f26..f248e07 100644 --- a/src/RotationVectors.cpp +++ b/src/RotationVectors.cpp @@ -3,12 +3,6 @@ #include #include "Arduino.h" -#define D_180 180.0f -#define D_360 360.0f -#define D_720 720.0f -/** Multiply with F_RAD_TO_DEG to convert from radians to degrees */ -#define TO_DEG static_cast(180.0 / M_PI) - float quaternionToRoll(const float q0, const float q1, const float q2, const float q3) { // Combined Sine of Roll and Cosine of Pitch @@ -69,45 +63,3 @@ euler_t quaternionToEuler(const float qr, const float qi, const float qj, const return euler_t { yaw, pitch, roll }; } - -float normalize180(float angle) -{ - angle = fmod(angle, D_720); // Clamp angle to within 2 rotations - while (angle > D_180) - { - angle -= D_360; - } - while (angle < -D_180) - { - angle += D_360; - } - return angle; -} - -float normalize360(float angle) -{ - while (angle > D_360) - { - angle -= D_180; - } - while (angle < -D_360) - { - angle += D_180; - } - return angle; -} - -float updateAngle(const float newAngle, const float prevAngle) -{ - const float normNewAngle = normalize180(newAngle); - const float normPrevAngle = normalize180(prevAngle); - float delta = normNewAngle - normPrevAngle; - if (delta > D_180) - { - delta -= D_360; - } else if (delta < -D_180) - { - delta += D_360; - } - return normalize360(normPrevAngle + delta); -} From ad4efb9b451b84176734c934d4c9e36d95ff8924 Mon Sep 17 00:00:00 2001 From: btoone Date: Mon, 14 Oct 2024 17:39:24 -0600 Subject: [PATCH 07/11] LibDeps fix --- library.json | 14 ++++++++++++++ 1 file changed, 14 insertions(+) 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", From d6288e3dba1f03f6eab6c516691ebb249097c601 Mon Sep 17 00:00:00 2001 From: btoone Date: Mon, 14 Oct 2024 18:21:10 -0600 Subject: [PATCH 08/11] Renamed enum usage --- examples/DataCollection.cpp | 10 +++++----- include/Sensor.h | 4 +--- include/SensorData.h | 4 ++-- src/AnalogSensor.cpp | 2 +- src/DigitalSensor.cpp | 2 +- src/RVC.cpp | 24 ++++++++++++------------ src/Sensor.cpp | 2 -- src/SensorData.cpp | 2 +- 8 files changed, 23 insertions(+), 27 deletions(-) diff --git a/examples/DataCollection.cpp b/examples/DataCollection.cpp index 9c05877..922cbae 100644 --- a/examples/DataCollection.cpp +++ b/examples/DataCollection.cpp @@ -23,11 +23,11 @@ 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); +AnalogSensor throttle1 = AnalogSensor(ReservedIDs::Throttle1PositionId, R_DAMPER_CRITICALITY, R_DAMPER_PIN, DAMPER_INTERVAL); +AnalogSensor throttle2 = AnalogSensor(ReservedIDs::Throttle2PositionId, L_DAMPER_CRITICALITY, L_DAMPER_PIN, DAMPER_INTERVAL); +DigitalSensor startSwitch = DigitalSensor(ReservedIDs::StartSwitchId, SWITCH_CRITICALITY, SWITCH_PIN, SWITCH_INTERVAL); Adafruit_BNO08x_RVC bno; -RVC rvc = RVC(ReservedIDs::RVC, RVC_CRITICALITY, RVC_INTERVAL, &bno); +RVC rvc = RVC(ReservedIDs::RVCId, RVC_CRITICALITY, RVC_INTERVAL, &bno); Sensor* sensors[] = { &throttle1, @@ -46,7 +46,7 @@ void setup() { 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/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/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/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; From 8d778d4b6280cf284b4bc1951fc9713b360944d3 Mon Sep 17 00:00:00 2001 From: btoone Date: Mon, 14 Oct 2024 18:24:01 -0600 Subject: [PATCH 09/11] Example update --- examples/DataCollection.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/DataCollection.cpp b/examples/DataCollection.cpp index 922cbae..ab4f8b0 100644 --- a/examples/DataCollection.cpp +++ b/examples/DataCollection.cpp @@ -8,13 +8,13 @@ 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,8 +23,8 @@ constexpr uint32_t SWITCH_INTERVAL = 100; constexpr bool RVC_CRITICALITY = false; constexpr uint32_t RVC_INTERVAL = 100; -AnalogSensor throttle1 = AnalogSensor(ReservedIDs::Throttle1PositionId, R_DAMPER_CRITICALITY, R_DAMPER_PIN, DAMPER_INTERVAL); -AnalogSensor throttle2 = AnalogSensor(ReservedIDs::Throttle2PositionId, L_DAMPER_CRITICALITY, L_DAMPER_PIN, DAMPER_INTERVAL); +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 bno; RVC rvc = RVC(ReservedIDs::RVCId, RVC_CRITICALITY, RVC_INTERVAL, &bno); From 29443cf2f84eef98f5397dca0e1feba8b715ba59 Mon Sep 17 00:00:00 2001 From: btoone Date: Mon, 14 Oct 2024 18:26:58 -0600 Subject: [PATCH 10/11] Example update --- examples/DataCollection.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/examples/DataCollection.cpp b/examples/DataCollection.cpp index ab4f8b0..f85b316 100644 --- a/examples/DataCollection.cpp +++ b/examples/DataCollection.cpp @@ -5,6 +5,8 @@ #include "AnalogSensor.h" #include "DigitalSensor.h" #include "RVC.h" +#include "RotationSensor.h" + constexpr uint32_t BAUD_RATE = 115200; @@ -23,24 +25,35 @@ constexpr uint32_t SWITCH_INTERVAL = 100; constexpr bool RVC_CRITICALITY = false; constexpr uint32_t RVC_INTERVAL = 100; +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 bno; -RVC rvc = RVC(ReservedIDs::RVCId, RVC_CRITICALITY, RVC_INTERVAL, &bno); +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() { From bf2905cc08bb6c6ff2856fa997edd75b1a45556f Mon Sep 17 00:00:00 2001 From: btoone Date: Mon, 14 Oct 2024 19:56:03 -0600 Subject: [PATCH 11/11] buffer packer fix --- src/RotationSensor.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/RotationSensor.cpp b/src/RotationSensor.cpp index 7cec83a..1e611a5 100644 --- a/src/RotationSensor.cpp +++ b/src/RotationSensor.cpp @@ -1,6 +1,8 @@ #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 @@ -97,9 +99,9 @@ SensorData RotationSensor::read() const RotationVector rv(sensorValue->un.gameRotationVector); updateYPR(ypr, rv); uint8_t msgIndex = 0; - setMsg(&sensorData, &msgIndex, ypr->roll, RVCSubIDs::Roll); - setMsg(&sensorData, &msgIndex, ypr->pitch, RVCSubIDs::Pitch); - setMsg(&sensorData, &msgIndex, ypr->yaw, RVCSubIDs::Yaw); + setMsg(&sensorData, &msgIndex, ypr->roll, RVCSubIDs::RollId); + setMsg(&sensorData, &msgIndex, ypr->pitch, RVCSubIDs::PitchId); + setMsg(&sensorData, &msgIndex, ypr->yaw, RVCSubIDs::YawId); } return sensorData; } @@ -118,18 +120,18 @@ 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); + BufferPacker unpacker(canMsg.buf); const RVCSubIDs id = unpacker.unpack(); const float value = unpacker.unpack(); switch (id) { - 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: