diff --git a/can_bus/quadruna/VC/VC_tx.json b/can_bus/quadruna/VC/VC_tx.json index dfce441bdb..40062b1578 100644 --- a/can_bus/quadruna/VC/VC_tx.json +++ b/can_bus/quadruna/VC/VC_tx.json @@ -496,6 +496,12 @@ "min": -150, "max": 150, "unit": "km/h" + }, + "VehicleVelocityCalculated": { + "resolution": 0.01, + "min": -150, + "max": 150, + "unit": "km/h" } } }, @@ -691,10 +697,10 @@ } } }, - "ImuLinearAcceleration": { + "ImuData": { "msg_id": 225, "cycle_time": 100, - "description": "Linear acceleration in x, y and z direction from the IMU", + "description": "Linear Acceleration and Angular Velocity in x, y and z direction from the IMU", "signals": { "ImuAccelerationX": { "resolution": 0.1, @@ -713,6 +719,24 @@ "min": 0, "max": 20, "unit": "m/s" + }, + "ImuAngularVelocityRoll": { + "resolution": 0.1, + "min": -360, + "max": 360, + "unit": "deg/s" + }, + "ImuAngularVelocityPitch": { + "resolution": 0.1, + "min": -360, + "max": 360, + "unit": "deg/s" + }, + "ImuAngularVelocityYaw": { + "resolution": 0.1, + "min": -360, + "max": 360, + "unit": "deg/s" } } }, diff --git a/firmware/quadruna/VC/src/app/app_sbgEllipse.c b/firmware/quadruna/VC/src/app/app_sbgEllipse.c index afd9c44825..2a30ae0752 100644 --- a/firmware/quadruna/VC/src/app/app_sbgEllipse.c +++ b/firmware/quadruna/VC/src/app/app_sbgEllipse.c @@ -1,9 +1,11 @@ #include #include "app_sbgEllipse.h" #include "app_canTx.h" +#include "app_canRx.h" #include "app_utils.h" #include "app_units.h" -#include "io_sbgEllipse.h" +#include "app_vehicleDynamicsConstants.h" +#include "io_log.h" void app_sbgEllipse_broadcast() { @@ -21,10 +23,15 @@ void app_sbgEllipse_broadcast() // const uint32_t timestamp_us = io_sbgEllipse_getTimestampUs(); // app_canTx_VC_EllipseTimestamp_set(timestamp_us); - // Velocity EKF - const float ekf_vel_N = io_sbgEllipse_getEkfNavVelocityData()->north; - const float ekf_vel_E = io_sbgEllipse_getEkfNavVelocityData()->east; - const float ekf_vel_D = io_sbgEllipse_getEkfNavVelocityData()->down; + VelocityData velocity_calculated; + + // calculating velocity data based on wheel speed + app_sbgEllipse_calculateVelocity(&velocity_calculated); + + // EKF + float ekf_vel_N = io_sbgEllipse_getEkfNavVelocityData()->north; + float ekf_vel_E = io_sbgEllipse_getEkfNavVelocityData()->east; + float ekf_vel_D = io_sbgEllipse_getEkfNavVelocityData()->down; const float ekf_vel_N_accuracy = io_sbgEllipse_getEkfNavVelocityData()->north_std_dev; const float ekf_vel_E_accuracy = io_sbgEllipse_getEkfNavVelocityData()->east_std_dev; @@ -38,9 +45,21 @@ void app_sbgEllipse_broadcast() app_canTx_VC_VelocityEastAccuracy_set(ekf_vel_E_accuracy); app_canTx_VC_VelocityDownAccuracy_set(ekf_vel_D_accuracy); - const float vehicle_velocity = sqrtf(SQUARE(ekf_vel_N) + SQUARE(ekf_vel_E) + SQUARE(ekf_vel_D)); + const float vehicle_velocity = sqrtf(SQUARE(ekf_vel_N) + SQUARE(ekf_vel_E) + SQUARE(ekf_vel_D)); + const float vehicle_velocity_calculated = MPS_TO_KMH(velocity_calculated.north); + + uint32_t ekf_sol_mode = io_sbgEllipse_getEkfSolutionMode(); + + if (ekf_sol_mode < NUM_VC_EKF_STATUS_CHOICES) + { + app_canTx_VC_EkfSolutionMode_set((VcEkfStatus)ekf_sol_mode); + } - app_canTx_VC_VehicleVelocity_set(MPS_TO_KMH(vehicle_velocity)); + // determines when to use calculated or gps velocity, will be externed later + // bool use_calculated_velocity = ekf_sol_mode == POSITION; + + app_canTx_VC_VehicleVelocity_set(vehicle_velocity); + app_canTx_VC_VehicleVelocityCalculated_set(vehicle_velocity_calculated); // Position EKF // const double ekf_pos_lat = io_sbgEllipse_getEkfNavPositionData()->latitude; @@ -76,3 +95,25 @@ void app_sbgEllipse_broadcast() app_canTx_VC_EulerAnglesPitch_set(euler_pitch); app_canTx_VC_EulerAnglesYaw_set(euler_yaw); } + +void app_sbgEllipse_calculateVelocity(VelocityData *velocity) +{ + // These velocity calculations are not going to be super accurate because it + // currently does not compute a proper relative y-axis velocity because no yaw rate + + const float rightMotorRPM = (float)-app_canRx_INVR_MotorSpeed_get(); + const float leftMotorRPM = (float)app_canRx_INVL_MotorSpeed_get(); + + float leftWheelVelocity = MOTOR_RPM_TO_KMH(leftMotorRPM); + float rightWheelVelocity = MOTOR_RPM_TO_KMH(rightMotorRPM); + + float velocityX = (leftWheelVelocity + rightWheelVelocity) / 2.0f; + + // This is technically velocity in the x-axis as it is relative + velocity->north = velocityX; + + // This is technically velocity in the y-axis as it is relative + velocity->east = 0; + + velocity->down = 0; +} \ No newline at end of file diff --git a/firmware/quadruna/VC/src/app/app_sbgEllipse.h b/firmware/quadruna/VC/src/app/app_sbgEllipse.h index 0686ee44a3..2a2f2de3ce 100644 --- a/firmware/quadruna/VC/src/app/app_sbgEllipse.h +++ b/firmware/quadruna/VC/src/app/app_sbgEllipse.h @@ -1,7 +1,14 @@ #pragma once #include +#include "io_sbgEllipse.h" + /* * Broadcast sensor outputs over CAN. */ void app_sbgEllipse_broadcast(void); + +/* + * Calculate vehicle velocity based on motor RPM + */ +void app_sbgEllipse_calculateVelocity(VelocityData *velocity); \ No newline at end of file diff --git a/firmware/quadruna/VC/src/app/states/app_allStates.c b/firmware/quadruna/VC/src/app/states/app_allStates.c index a237d930c9..29ae1713f9 100644 --- a/firmware/quadruna/VC/src/app/states/app_allStates.c +++ b/firmware/quadruna/VC/src/app/states/app_allStates.c @@ -50,6 +50,21 @@ void app_allStates_runOnTick100Hz(void) app_canTx_VC_ImuAccelerationZ_set(lin_accel_z); } + float angular_velocity_roll = 0.0f; + float angular_velocity_pitch = 0.0f; + float angular_velocity_yaw = 0.0f; + + bool has_ang_vel_roll = io_imu_getAngularVelocityRoll(&angular_velocity_roll); + bool has_ang_vel_pitch = io_imu_getAngularVelocityPitch(&angular_velocity_pitch); + bool has_ang_vel_yaw = io_imu_getAngularVelocityYaw(&angular_velocity_yaw); + + if (has_ang_vel_roll && has_ang_vel_pitch && has_ang_vel_yaw) + { + app_canTx_VC_ImuAngularVelocityRoll_set(angular_velocity_roll); + app_canTx_VC_ImuAngularVelocityPitch_set(angular_velocity_pitch); + app_canTx_VC_ImuAngularVelocityYaw_set(angular_velocity_yaw); + } + app_heartbeatMonitor_checkIn(&hb_monitor); if (heartbeat_cycles <= IGNORE_HEARTBEAT_CYCLES) // TODO make this part of the heartbeat monitor diff --git a/firmware/quadruna/VC/src/io/io_imu.c b/firmware/quadruna/VC/src/io/io_imu.c index 91d1ee1c52..f1e8908f1d 100644 --- a/firmware/quadruna/VC/src/io/io_imu.c +++ b/firmware/quadruna/VC/src/io/io_imu.c @@ -3,8 +3,10 @@ #include #include -// Default sensitivity for LSM6DSM is 0.061 mg/digit -const float SENSITIVITY = 0.061f; +// Default accelerometer sensitivity for LSM6DSM is 0.061 mg/digit +const float ACCEL_SENSITIVITY = 0.061f; +// Default gyroscope sensitivity for LSM6DSM is 8.75 mdeg/digit +const float GYRO_SENSITIVITY = 8.75f; extern I2C_HandleTypeDef hi2c2; static I2cInterface imu = { .i2c_handle = &hi2c2, .target_address = 0x6B, .timeout_ms = 100 }; @@ -13,8 +15,9 @@ bool io_imu_init() { if (hw_i2c_isTargetReady(&imu)) { - uint8_t buffer = 0x40; // turn on accelerometer to normal mode - return hw_i2c_memWrite(&imu, 0x10, &buffer, 1); + uint8_t buffer = 0x40; // turn on accelerometer/gyroscope to normal mode + bool activated_imu_sensors = hw_i2c_memWrite(&imu, 0x10, &buffer, 1) && hw_i2c_memWrite(&imu, 0x11, &buffer, 1); + return activated_imu_sensors; } return false; } @@ -31,7 +34,7 @@ bool io_imu_getLinearAccelerationX(float *x_acceleration) // Convert raw value to acceleration in m/s^2 int16_t x_raw = (int16_t)(x_data[1] << 8 | x_data[0]); - *x_acceleration = x_raw * SENSITIVITY * 9.81f / 1000.0f; + *x_acceleration = x_raw * ACCEL_SENSITIVITY * 9.81f / 1000.0f; return true; } @@ -47,7 +50,7 @@ bool io_imu_getLinearAccelerationY(float *y_acceleration) // Convert raw value to acceleration in m/s^2 int16_t y_raw = (int16_t)(y_data[1] << 8 | y_data[0]); - *y_acceleration = y_raw * SENSITIVITY * 9.81f / 1000.0f; + *y_acceleration = y_raw * ACCEL_SENSITIVITY * 9.81f / 1000.0f; return true; } @@ -63,6 +66,54 @@ bool io_imu_getLinearAccelerationZ(float *z_acceleration) // Convert raw value to acceleration in m/s^2 and subtract force of gravity int16_t z_raw = (int16_t)(z_data[1] << 8 | z_data[0]); - *z_acceleration = z_raw * SENSITIVITY * 9.81f / 1000.0f - 9.81f; + *z_acceleration = z_raw * ACCEL_SENSITIVITY * 9.81f / 1000.0f - 9.81f; + return true; +} + +bool io_imu_getAngularVelocityRoll(float *roll_velocity) +{ + uint8_t roll_data[2]; + bool is_read_successful = hw_i2c_memRead(&imu, 0x22, roll_data, 2); + + if (!is_read_successful) + { + return false; + } + + // Convert raw value to angular velocity (degrees per second or radians per second as required) + int16_t roll_raw = (int16_t)(roll_data[1] << 8 | roll_data[0]); + *roll_velocity = roll_raw * GYRO_SENSITIVITY / 1000.0f; + return true; +} + +bool io_imu_getAngularVelocityPitch(float *pitch_velocity) +{ + uint8_t pitch_data[2]; + bool is_read_successful = hw_i2c_memRead(&imu, 0x24, pitch_data, 2); + + if (!is_read_successful) + { + return false; + } + + // Convert raw value to angular velocity (degrees per second or radians per second as required) + int16_t pitch_raw = (int16_t)(pitch_data[1] << 8 | pitch_data[0]); + *pitch_velocity = pitch_raw * GYRO_SENSITIVITY / 1000.0f; + return true; +} + +bool io_imu_getAngularVelocityYaw(float *yaw_velocity) +{ + uint8_t yaw_data[2]; + bool is_read_successful = hw_i2c_memRead(&imu, 0x26, yaw_data, 2); + + if (!is_read_successful) + { + return false; + } + + // Convert raw value to angular velocity (degrees per second or radians per second as required) + int16_t yaw_raw = (int16_t)(yaw_data[1] << 8 | yaw_data[0]); + *yaw_velocity = yaw_raw * GYRO_SENSITIVITY / 1000.0f; return true; } diff --git a/firmware/quadruna/VC/src/io/io_imu.h b/firmware/quadruna/VC/src/io/io_imu.h index c6b497a944..340a8beb6e 100644 --- a/firmware/quadruna/VC/src/io/io_imu.h +++ b/firmware/quadruna/VC/src/io/io_imu.h @@ -29,3 +29,24 @@ bool io_imu_getLinearAccelerationY(float *y_acceleration); * @return if successful read */ bool io_imu_getLinearAccelerationZ(float *z_acceleration); + +/** + * @brief Gets the angular velocity in the roll direction + * @param roll_velocity Pointer to store the roll angular velocity + * @return True if the read is successful, false otherwise + */ +bool io_imu_getAngularVelocityRoll(float *roll_velocity); + +/** + * @brief Gets the angular velocity in the pitch direction + * @param pitch_velocity Pointer to store the pitch angular velocity + * @return True if the read is successful, false otherwise + */ +bool io_imu_getAngularVelocityPitch(float *pitch_velocity); + +/** + * @brief Gets the angular velocity in the yaw direction + * @param yaw_velocity Pointer to store the yaw angular velocity + * @return True if the read is successful, false otherwise + */ +bool io_imu_getAngularVelocityYaw(float *yaw_velocity); \ No newline at end of file diff --git a/firmware/quadruna/VC/src/io/io_sbgEllipse.c b/firmware/quadruna/VC/src/io/io_sbgEllipse.c index c5e8df5245..0088076a42 100644 --- a/firmware/quadruna/VC/src/io/io_sbgEllipse.c +++ b/firmware/quadruna/VC/src/io/io_sbgEllipse.c @@ -10,6 +10,8 @@ #include "sbgECom.h" #include "interfaces/sbgInterfaceSerial.h" #include "app_canTx.h" +#include "app_canUtils.h" + #include "io_time.h" #include "io_log.h" @@ -211,23 +213,9 @@ static void io_sbgEllipse_processMsg_status(const SbgBinaryLogData *log_data) static void io_sbgEllipse_processMsg_EkfNavVelandPos(const SbgBinaryLogData *log_data) { // TODO: uncomment after initial testing, if this occurs skip reading data - // app_canAlerts_VC_Fault_SBGModeFault_set(sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status) != - // SBG_ECOM_SOL_MODE_NAV_POSITION); - - app_canTx_VC_EkfSolutionMode_set((VcEkfStatus)sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status)); - - // uint32_t status = log_data->ekfNavData.status; - - // if (sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status) != SBG_ECOM_SOL_MODE_NAV_POSITION) - // { - // uint32_t status = log_data->ekfNavData.status; - // bool is_velocity_valid = (status & SBG_ECOM_SOL_VELOCITY_VALID) != 0; - // bool is_position_valid = (status & SBG_ECOM_SOL_POSITION_VALID) != 0; - - // if (!is_velocity_valid & !is_position_valid) - // return; - // } + // obtaining ekf solution mode from sbg ellipse and setting it in status field + sensor_data.ekf_solution_status = sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status); // velocity data in m/s sensor_data.ekf_nav_data.velocity.north = log_data->ekfNavData.velocity[0]; @@ -318,6 +306,11 @@ uint32_t io_sbgEllipse_getOverflowCount(void) return sbg_queue_overflow_count; } +uint32_t io_sbgEllipse_getEkfSolutionMode(void) +{ + return sensor_data.ekf_solution_status; +} + Vector3 *io_sbgEllipse_getImuAccelerations() { return &sensor_data.imu_data.acceleration; diff --git a/firmware/quadruna/VC/src/io/io_sbgEllipse.h b/firmware/quadruna/VC/src/io/io_sbgEllipse.h index 2ff387cc8a..653909e7ad 100644 --- a/firmware/quadruna/VC/src/io/io_sbgEllipse.h +++ b/firmware/quadruna/VC/src/io/io_sbgEllipse.h @@ -75,6 +75,7 @@ typedef struct EkfEulerPacketData ekf_euler_data; StatusPacketData status_data; EkfNavPacketData ekf_nav_data; + uint32_t ekf_solution_status; } SensorData; #ifdef TARGET_EMBEDDED @@ -114,6 +115,12 @@ uint32_t io_sbgEllipse_getComStatus(void); */ uint32_t io_sbgEllipse_getOverflowCount(void); +/* + * Get EKF Solution Mode Status + * @return the ekf solution mode + */ +uint32_t io_sbgEllipse_getEkfSolutionMode(void); + /** * Get the IMU accelerations as a struct pointer with fields: * - float x: Forward acceleration in m/s^2 diff --git a/firmware/quadruna/VC/test/test_velocityCalculation.cpp b/firmware/quadruna/VC/test/test_velocityCalculation.cpp new file mode 100644 index 0000000000..e7ff155518 --- /dev/null +++ b/firmware/quadruna/VC/test/test_velocityCalculation.cpp @@ -0,0 +1,90 @@ +#include "test_vcBaseStateMachineTest.h" + +extern "C" +{ +#include +#include "app_units.h" +#include "app_sbgEllipse.h" +#include "io_sbgEllipse.h" +} + +class VelocityCalculationTest : public VcBaseStateMachineTest +{ +}; + +TEST_F(VelocityCalculationTest, velocity_calculation_when_rpms_same) +{ + TearDown(); + SetUp(); + + app_canRx_INVR_MotorSpeed_update(-MOTOR_KMH_TO_RPM(10.0f)); + app_canRx_INVL_MotorSpeed_update(MOTOR_KMH_TO_RPM(10.0f)); + + VelocityData velocity; + + app_sbgEllipse_calculateVelocity(&velocity); + + float expectedVelocityKMH = 10.0f; + + ASSERT_NEAR(velocity.north, expectedVelocityKMH, 0.1f); +} + +TEST_F(VelocityCalculationTest, velocity_calculation_when_rpms_zero) +{ + app_canRx_INVR_MotorSpeed_update(-MOTOR_KMH_TO_RPM(0.0f)); + app_canRx_INVL_MotorSpeed_update(MOTOR_KMH_TO_RPM(0.0f)); + + VelocityData velocity; + + app_sbgEllipse_calculateVelocity(&velocity); + + ASSERT_NEAR(velocity.north, 0.0f, 0.1f); +} + +TEST_F(VelocityCalculationTest, velocity_calculation_when_rpms_diff) +{ + app_canRx_INVR_MotorSpeed_update(-MOTOR_KMH_TO_RPM(20.0f)); + app_canRx_INVL_MotorSpeed_update(MOTOR_KMH_TO_RPM(10.0f)); + + VelocityData velocity; + + app_sbgEllipse_calculateVelocity(&velocity); + + ASSERT_NEAR(velocity.north, 15.0f, 0.1f); +} + +TEST_F(VelocityCalculationTest, velocity_calculation_when_one_rpm_neg) +{ + app_canRx_INVR_MotorSpeed_update(-MOTOR_KMH_TO_RPM(4.0f)); + app_canRx_INVL_MotorSpeed_update(-MOTOR_KMH_TO_RPM(2.0f)); + + VelocityData velocity; + + app_sbgEllipse_calculateVelocity(&velocity); + + ASSERT_NEAR(velocity.north, 1.0f, 0.1f); +} + +TEST_F(VelocityCalculationTest, velocity_calculation_when_rpms_neg) +{ + app_canRx_INVR_MotorSpeed_update(MOTOR_KMH_TO_RPM(5.0f)); + app_canRx_INVL_MotorSpeed_update(-MOTOR_KMH_TO_RPM(5.0f)); + + VelocityData velocity; + + app_sbgEllipse_calculateVelocity(&velocity); + + ASSERT_NEAR(velocity.north, -5.0f, 0.1f); +} + +TEST_F(VelocityCalculationTest, velocity_calculation_precision) +{ + app_canRx_INVR_MotorSpeed_update(-MOTOR_KMH_TO_RPM(59.9f)); + app_canRx_INVL_MotorSpeed_update(MOTOR_KMH_TO_RPM(60.1f)); + + VelocityData velocity; + + app_sbgEllipse_calculateVelocity(&velocity); + + ASSERT_NEAR(velocity.north, 60.0f, 0.1f); +} \ No newline at end of file diff --git a/firmware/quadruna/VC/test/vehicle_dynamics/test_regen.cpp b/firmware/quadruna/VC/test/vehicle_dynamics/test_regen.cpp index eddac1ac5a..b6fc30da75 100644 --- a/firmware/quadruna/VC/test/vehicle_dynamics/test_regen.cpp +++ b/firmware/quadruna/VC/test/vehicle_dynamics/test_regen.cpp @@ -219,8 +219,19 @@ TEST_F(TestRegen, regular_run_regen_and_switch_disable_during_drive_state) float torque_lim_Nm = -(POWER_TO_TORQUE_CONVERSION_FACTOR * inputs.power_max_kW) / (inputs.motor_speed_left_rpm * cl + inputs.motor_speed_right_rpm * cr + SMALL_EPSILON); - float expected_left_torque_request = torque_lim_Nm * cl; - float expected_right_torque_request = torque_lim_Nm * cr; + float torque_left_Nm = torque_lim_Nm * cl; + float torque_right_Nm = torque_lim_Nm * cr; + + float torque_negative_max_Nm = fminf(torque_left_Nm, torque_right_Nm); + + float scale = 1; + if (torque_negative_max_Nm < MAX_REGEN_Nm) + { + scale *= MAX_REGEN_Nm / torque_negative_max_Nm; + } + + float expected_left_torque_request = torque_left_Nm * scale; + float expected_right_torque_request = torque_right_Nm * scale; LetTimePass(10); diff --git a/firmware/shared/src/app/app_units.h b/firmware/shared/src/app/app_units.h index 05a5091c63..ed1c3a35a7 100644 --- a/firmware/shared/src/app/app_units.h +++ b/firmware/shared/src/app/app_units.h @@ -18,7 +18,7 @@ #define DEG_TO_RAD(degrees) ((degrees) * M_PI_F / 180.0f) #define RPM_TO_RADS(rpm) ((rpm) * M_PI_F / 30.0f) -#define WHEEL_DIAMETER_IN (15.0f) +#define WHEEL_DIAMETER_IN (18.0f) #define GEAR_RATIO (4.124f) #define MOTOR_RPM_TO_KMH(rpm) ((rpm) * (float)WHEEL_DIAMETER_IN * M_PI_F * INCH_TO_KM * MIN_TO_HOUR / GEAR_RATIO) #define MOTOR_KMH_TO_RPM(kmh) \