controls algo setup by Lucien950 · Pull Request #1936 · UBCFormulaElectric/Consolidated-Firmware

Lucien950 · 2026-03-29T21:43:02Z

Changelist

Testing Done

Resolved Tickets

Lucien950

imma trust you on the equations
i don't really agree with internal state abuse bceause I think it makes the code very hard to follow, i would prefer to see that the functions are all pure

Lucien950 · 2026-03-29T22:22:29Z

+void TireModel::estimateSlipRatio(const float wheel_angular_velocity_radps)
+{
+    const float wheel_surface_speed_mps = wheel_angular_velocity_radps * WHEEL_RADIUS_M;
+    const float effective_wheel_speed_mps = std::cos(tire_outputs_.slip_angle_rad) * wheel_vel_x_mps_;


i imagine this is also a function of wheelSteeringAngleRad?

slip angle is a function of wheel steering angle, it is not directly a function of wheel angle

Aditya-Dhiman4 · 2026-04-03T06:42:36Z

+    [[nodiscard]] float estimateDrag_N(float v_x_mps) const;
+
+private:
+    static constexpr float FRONTAL_AREA_M2    = 0.94f;   // m^2 from aero team


We should just put all these constants into constants.hpp

Lucien950

just a first pass, I think there are some high level structural understandability issues with the code we should address before correctness

Lucien950 · 2026-04-05T21:45:33Z

+static constexpr float W_FX        = 2.0f / 3.0f; // Weight on per-wheel force tracking error
+static constexpr float W_MZ        = 1.0f / 3.0f; // Weight on yaw moment tracking error
+static constexpr int   MAX_ITER    = 8;            // Gauss-Newton iterations per control cycle
+static constexpr float SLIP_CLAMP  = 0.3f;         // Physical slip ratio bounds


is it expensive to calculate high slip?

Lucien950 · 2026-04-05T21:46:05Z

+static constexpr int FL = 0;
+static constexpr int FR = 1;
+static constexpr int RL = 2;
+static constexpr int RR = 3;


use unsigned integers

also i prefer the wheel_set datastructure over this

Lucien950 · 2026-04-05T21:51:04Z

@@ -0,0 +1,22 @@
+#pragma once
+
+namespace app::tv::datatypes::datatypes


why two namespace qualifiers?

Lucien950 · 2026-04-05T21:53:05Z

+[[nodiscard]] estimation::TireModel::StateInputs buildStateInputs(
+    const datatypes::datatypes::VehicleState& vs,
+    const float normal_load_N,
+    const float wheel_angular_velocity_radps)
+{
+    return {
+        .wheel_angular_velocity_radps = wheel_angular_velocity_radps,
+        .vehicle_velocity_x_mps       = vs.v_x_mps,
+        .vehicle_velocity_y_mps       = vs.v_y_mps,
+        .yaw_rate_radps               = vs.yaw_rate_radps,
+        .steering_angle_rad           = vs.steer_ang_rad,
+        .normal_load_N                = normal_load_N,
+    };
+}
+


you can just make a constructor for this

also this is a very trivial constructor

Lucien950 · 2026-04-05T21:53:25Z

+
+// ---- Construction ----
+
+TorqueAllocator::TorqueAllocator(estimation::TireModel::TirePressure pressure)


put this in the header please (optimization point)

Lucien950 · 2026-04-05T22:16:44Z

+    return std::atan2(v_y_mps, safeLongitudinalVelocity(v_x_mps));
+}
+
+float vehicleDynamics::estimateYawMoment_Nm(


i think this is a better yaw moment estimator than in the ta? why not use this?

oh yes I was thinking that but I didn't know how ur C++ generation was going to work so I left it here for now to make it easier

but yes I agree for our code it should go there

Lucien950 · 2026-04-05T22:17:05Z

+    const app::tv::datatypes::datatypes::wheel_set& longitudinal_forces_N,
+    const app::tv::datatypes::datatypes::wheel_set& lateral_forces_N,


also you can just go F_y_N and F_x_N

Lucien950

more

Lucien950 · 2026-04-05T22:28:19Z

@@ -34,8 +34,7 @@ namespace app::tv::controllers::dyrc
 *
 * @return The corrective yaw moment in Nm to apply on the vehicle
 */
-[[nodiscard]] inline float
-    computeYawMoment(const float r_actual_rad, const float steer_ang_rad, const float body_velx_mps);
+[[nodiscard]] float computeYawMoment(const float r_actual_rad, const float steer_ang_rad, const float body_velx_mps);


lowkey can we calculate yawacceleration

Lucien950 · 2026-04-05T22:36:36Z

+}
+
+datatypes::datatypes::wheel_set TorqueAllocator::optimize(
+    const datatypes::datatypes::wheel_set& des_f_x,


your resired F_x is one number, and it represents the desired body F_x

please do not specify individual wheel f_x references

because you actually don't know how to effectively allocate, that is the whole point of the optimizer

hmm, yeah it might be redundant rn because I optimize on Mz err but also Fx error with Mz taken into account. Note I also realized no constraint is placed on the sum of forces adding on the user quested force * 4. That is important I BELIEVE

Lucien950 · 2026-04-05T22:37:40Z

+    // Desired per-wheel longitudinal force from pedal request, projected along tire heading
+    const float per_wheel_tq = MAX_TORQUE_REQUEST_NM * pedal_percentage;
+    wheel_set des_f_x = {
+        .fl = per_wheel_tq * std::cos(acc_slip_angle.fl) * WHEEL_RADIUS_M ,
+        .fr = per_wheel_tq * std::cos(acc_slip_angle.fr) * WHEEL_RADIUS_M,
+        .rl = per_wheel_tq * std::cos(acc_slip_angle.rl) * WHEEL_RADIUS_M,
+        .rr = per_wheel_tq * std::cos(acc_slip_angle.rr) * WHEEL_RADIUS_M,
+    };
+
+    // Direct yaw rate control: corrective yaw moment
+    const float des_yaw_moment_nm = controllers::dyrc::computeYawMoment(
+        estimated_state.yaw_rate_radps, estimated_state.steer_ang_rad, estimated_state.v_x_mps);
+
+    // Distribute yaw moment into per-wheel force adjustment
+    // From Mz = (t/2) * (Fx_right - Fx_left), solve for delta Fx per side:
+    const float des_fx_delta = des_yaw_moment_nm / TRACK_WIDTH_m;
+    des_f_x.fl -= des_fx_delta;
+    des_f_x.fr += des_fx_delta;
+    des_f_x.rl -= des_fx_delta;
+    des_f_x.rr += des_fx_delta;


must change as due to optimizer interface comment

Lucien950 · 2026-04-05T22:53:49Z

+void unpackTireOutputs(
+    const estimation::TireModel::Outputs& fl,
+    const estimation::TireModel::Outputs& fr,
+    const estimation::TireModel::Outputs& rl,
+    const estimation::TireModel::Outputs& rr)
+{
+    acc_f_x = { .fl = fl.longitudinal_force_N, .fr = fr.longitudinal_force_N,
+                .rl = rl.longitudinal_force_N, .rr = rr.longitudinal_force_N };
+    acc_f_y = { .fl = fl.lateral_force_N, .fr = fr.lateral_force_N,
+                .rl = rl.lateral_force_N, .rr = rr.lateral_force_N };
+    acc_slip_ratio = { .fl = fl.slip_ratio, .fr = fr.slip_ratio,
+                       .rl = rl.slip_ratio, .rr = rr.slip_ratio };
+    acc_slip_angle = { .fl = fl.slip_angle_rad, .fr = fr.slip_angle_rad,
+                       .rl = rl.slip_angle_rad, .rr = rr.slip_angle_rad };
+}


can you unpack them purely

it makes it easier to keep track of data dependencies

uhhhhhh sure

Lucien950 · 2026-04-05T22:59:46Z

+            const float fl_omega, const float fr_omega,
+            const float rl_omega, const float rr_omega)


just pass the omegas in a wheels struct

Aditya-Dhiman4 · 2026-04-05T22:53:24Z

@Lucien950 Our particular H7 has really shit SIMD, the width is like 32 bits so we can only perform it with 16 bit or 8 bit values. Our main performance benefit would come from the CORDIC because you can do trig, trig inverse, and square root functions with vectors as well, but we dont have the SIMD capabilities to do add, sub, multiply with 32bit float vectors. In other versions of the H7 there is Helium SIMD which has a 128 bit width so we can conveniently do SIMD for that

Aditya-Dhiman4 · 2026-04-05T22:55:12Z

but also it may be still beneficial to vectorize this so we can easily port it to SIMD in the future

Aditya-Dhiman4 · 2026-04-05T22:57:27Z

+     * @return Per-wheel optimal slip ratios
+     */
+    [[nodiscard]] datatypes::datatypes::wheel_set optimize(
+        const datatypes::datatypes::wheel_set& des_f_x,


nit: can you hit a using datatypes = datatypes::datatypes for anywhere where this is used it just makes it a bit easier to read

its been changed to shared_datatypes.

Aditya-Dhiman4 · 2026-04-06T00:16:59Z

also arent I doing this i have an MR up for body velocity estimation and i can add yaw moment + yaw rate in the state as well

Lucien950 · 2026-04-08T22:06:16Z

force push :(((((((

Aditya-Dhiman4

pretty sick mostly small things

Aditya-Dhiman4 · 2026-04-10T05:47:05Z

+    /**
+     * Yaw moment distribution factor Kmz (page 57)
+     * Accounts for load transfer effect on yaw moment generation capacity
+     * @return Effective moment arm (m)
+     */
+    [[nodiscard]] constexpr float ACCELERATION_TERM_KMZ() const
+    {
+        return vd_constants::DIST_FRONT_AXLE_CG_m + (a_x_mps2 * vd_constants::DIST_HEIGHT_CG_m) / vd_constants::GRAVITY;
+    }
+    [[nodiscard]] constexpr float KMZ() const
+    {
+        const float LONG_ACCEL_TERM = ACCELERATION_TERM_KMZ();
+        return ((vd_constants::CAR_WEIGHT - (vd_constants::CAR_WEIGHT / vd_constants::WHEELBASE_m) * LONG_ACCEL_TERM)) /
+               ((vd_constants::CAR_WEIGHT / vd_constants::WHEELBASE_m) * LONG_ACCEL_TERM);
+    }
+    /**
+     * Moment scaling factor F (page 58)
+     * Relates torque differential to yaw moment through track width and effective radius
+     */
+    static constexpr float F = (vd_constants::TRACK_WIDTH_m / ((WHEEL_DIAMETER_IN / 2.0f) * IN_TO_M)) * GEAR_RATIO;


These were required for the Quintuna algo not Hexray, we should probably add a comment for future reference to prevent confusion

actually I'm confused as to why this is required at all, i'm not so sure about what these terms are

shayan gave these to me

ic we can remove if u want or keep for legacy/fallback

Aditya-Dhiman4 · 2026-04-10T05:49:17Z

+    constexpr std::size_t AY = 1;
+
+    constexpr float ESTIMATOR_DT_S        = 0.01f;  // Matches the 100 Hz control task.
+    constexpr float ESTIMATOR_YAW_INERTIA = 110.0f; // TODO: Replace with measured Hexray yaw inertia.


Should we put this into constants.hpp?

idk I didn't write this this all u

dawg i did not write this i have a separate PR for this

ok well use that then

Aditya-Dhiman4 · 2026-04-10T06:59:23Z

This is kinda hard to read lowkey grouping the functions properly would help

…d slip once coefficents are here.... testing TODO

Co-authored-by: Edwin <20777515+Lucien950@users.noreply.github.com>

…ke it a motor speed request

Aditya-Dhiman4 · 2026-04-20T07:24:07Z

+                                 } };
+    // bring it in
+    const auto [k_kappas, k_torque_max, k_torque_min] = update(state);
+    // std::cout << "DIH" << std::endl;


### Changelist  ### Testing Done  ### Resolved Tickets

Lucien950 assigned shayana18 Mar 29, 2026

Lucien950 added the Vehicle Controls Torque Vectoring, Regen, Brake/Drive Diffs label Mar 29, 2026

Lucien950 changed the title ~~controls algo~~ algo setup Mar 29, 2026

Lucien950 changed the title ~~algo setup~~ controls algo setup Mar 29, 2026

Lucien950 commented Mar 29, 2026

View reviewed changes

Aditya-Dhiman4 reviewed Apr 3, 2026

View reviewed changes

Lucien950 commented Apr 3, 2026

View reviewed changes

Comment thread firmware/hexray/VC/src/app/torque_vectoring/estimation/tire_model.cpp

Lucien950 commented Apr 5, 2026

View reviewed changes

Aditya-Dhiman4 reviewed Apr 5, 2026

View reviewed changes

Aditya-Dhiman4 reviewed Apr 6, 2026

View reviewed changes

shayana18 force-pushed the Shayan/algo_set_up branch from c6d491a to 880059a Compare April 8, 2026 21:43

Aditya-Dhiman4 requested changes Apr 10, 2026

View reviewed changes

Lucien950 marked this pull request as ready for review April 10, 2026 20:29

shayana18 and others added 15 commits April 19, 2026 17:53

base work for tire model done (pure slip).... will change for combine…

42cdbb8

…d slip once coefficents are here.... testing TODO

algo in progress changes

26f9b13

work in progress, most peices are there

7cd32b8

logic is there now

f5ef2f3

fahh

9bcb1ba

small changes

3600b82

a few more small changes

80aa912

kms hours

1e509fe

rotations

8e44ed2

edwin zheng forgot how to do rotation matrix

13ced94

template metaprogramming masturbation

c8a8020

W optimizer?

ffb80cd

nuclearize

4940e26

penis

9471b20

name

731d184

Aditya-Dhiman4 and others added 16 commits April 19, 2026 17:55

I didnt even know this was possible

479c14e

Co-authored-by: Edwin <20777515+Lucien950@users.noreply.github.com>

wow

3741ff9

L

a66fb4c

maybe everything is fucked idk man, love matlab

6baefc0

clean up matlab interface

ba10cbb

small mistake

32ceac5

matplotlib and others

f2d8942

fixed torque bounds plus multiplied speed request by gear ratio to ma…

2dd61e9

…ke it a motor speed request

tweaks for ta

a182398

WORKS?S???????

68b41ff

working optimizer

6c91f74

torque allocator W

b95da94

working for floats as well now

14cfe3f

better ig

ba6f1a5

turning tests

6a3cf82

consistent optimizer

3b10a21

Aditya-Dhiman4 force-pushed the Shayan/algo_set_up branch from aaca9a1 to 3b10a21 Compare April 20, 2026 06:14

Aditya-Dhiman4 reviewed Apr 20, 2026

View reviewed changes

Aditya-Dhiman4 and others added 4 commits April 20, 2026 11:28

6.2 implemented

da931f5

tire model :)

1328739

Optimizer converges, Fz temporarily removed

79d12c4

		@@ -0,0 +1,22 @@
		#pragma once

		namespace app::tv::datatypes::datatypes


		// ---- Construction ----

		TorqueAllocator::TorqueAllocator(estimation::TireModel::TirePressure pressure)

		const app::tv::datatypes::datatypes::wheel_set& longitudinal_forces_N,
		const app::tv::datatypes::datatypes::wheel_set& lateral_forces_N,

		const float fl_omega, const float fr_omega,
		const float rl_omega, const float rr_omega)

Uh oh!

Uh oh!

Conversation

Lucien950 commented Mar 29, 2026

Changelist

Testing Done

Resolved Tickets

Uh oh!

Lucien950 left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Lucien950 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Lucien950 Apr 5, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Lucien950 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Lucien950 left a comment •

edited

Loading

Lucien950 Apr 5, 2026 •

edited

Loading

Lucien950 Apr 10, 2026 •

edited

Loading