Fsm integration by guytonde · Pull Request #14 · lhr-solar/PS-VehicleControlUnit

guytonde · 2026-04-11T22:33:29Z

integration of fsm

gemini-code-assist

Code Review

This pull request implements a major refactoring of the VCU firmware, introducing a new Finite State Machine for motor control, a centralized fault management system, and CAN message watchdogs. It also adds tasks for input processing and status broadcasting, supported by auto-generated lookup tables. Feedback identifies several critical bugs and logic errors, including the use of incorrect CAN IDs for status updates, potential out-of-bounds memory access in the FSM and watchdog modules, and inverted logic in pedal deviation checks. Furthermore, improvements are needed for throttle setpoint normalization, type-safe CAN data decoding, and the implementation of actual precharge sensing logic instead of hardcoded bits.

gemini-code-assist · 2026-04-11T22:35:45Z

+    uint8_t moco_status_rx_data[CAN_DLC_MC_VELOCITYMEASUREMENT] = {0};
+
+    can_status_t result =
+        can_fd_recv(motorfdcan, CAN_ID_MC_VELOCITYMEASUREMENT, &header, moco_status_rx_data, delay);


The MotorCAN_Recv_Status function is incorrectly calling can_fd_recv with CAN_ID_MC_VELOCITYMEASUREMENT (0x402) instead of CAN_ID_MC_STATUS (0x401). This will cause the status decoding logic to operate on velocity data.

can_fd_recv(motorfdcan, CAN_ID_MC_STATUS, &header, moco_status_rx_data, delay);

gemini-code-assist · 2026-04-11T22:35:45Z

+        &wd_buffers[wd_count]
+    );
+    configASSERT(t != NULL);
+    wd_timers[wd_count] = t;


The watchdog timer handle is being stored in the wd_timers array using wd_count as the index, but watchdog_received_can_message (and other functions) access it using the idx parameter. If idx does not match the creation order, this will lead to accessing the wrong timer or a NULL pointer, causing a crash. Use idx as the array index for consistency.

wd_timers[idx] = t;

gemini-code-assist · 2026-04-11T22:35:45Z

+    fsm_inputs = xEventGroupGetBits(fsmInputGroup);
+    current_state = FSM[current_state.NextStates[fsm_inputs]];


The fsm_inputs value retrieved from the event group is used directly as an index into the NextStates array. Since the event group can contain more bits than the array size (512), any unexpected bits being set will cause an out-of-bounds memory access. The inputs should be masked with FSM_INPUTS_MASK_ALL before indexing.

fsm_inputs = xEventGroupGetBits(fsmInputGroup) & FSM_INPUTS_MASK_ALL; current_state = FSM[current_state.NextStates[fsm_inputs]];

gemini-code-assist · 2026-04-11T22:35:45Z

+        // if we're actually going backwards, let off the pedal until we slow down
+        MotorCAN_Send_Drive_Cmd(0.0f, 0.0f, 0);
+    } else {
+        MotorCAN_Send_Drive_Cmd(MAX_VELOCITY, apply_rollover_limit(g_data_read->accel_brake.AccelPedal_Main_Pos), 0);


The pedal position AccelPedal_Main_Pos (likely 0-100 or 0-255) is being passed directly to the motor controller drive command. The Wavesculptor 22 expects a normalized current setpoint between 0.0f and 1.0f. Sending raw values will result in incorrect torque requests. Use map_to_percent to normalize the value.

float throttle = map_to_percent(g_data_read->accel_brake.AccelPedal_Main_Pos, 0, 100, 0, 100); MotorCAN_Send_Drive_Cmd(MAX_VELOCITY, apply_rollover_limit(throttle), 0);

gemini-code-assist · 2026-04-11T22:35:45Z

+        g_data_read->accel_brake.BrakePedal_Redundant_Fault ||
+        fabs(g_data_read->accel_brake.AccelPedal_Main_Pos -
+            g_data_read->accel_brake.AccelPedal_Redundant_Pos) < ACCEPTABLE_PEDAL_DEVIATION) {
+        mask |= FAULT_BIT(FAULT_ID_PEDAL_BOARD_FAULT);


The pedal deviation fault logic is inverted. It currently triggers a fault if the difference between the main and redundant sensors is less than the acceptable deviation. It should trigger if the difference is greater than the threshold.

fabs((float)g_data_read->accel_brake.AccelPedal_Main_Pos - (float)g_data_read->accel_brake.AccelPedal_Redundant_Pos) > ACCEPTABLE_PEDAL_DEVIATION) {

gemini-code-assist · 2026-04-11T22:35:45Z

+        else if(faults_is_active(VCU_STATUS_VCU_FAULT_MOTOR_PCHG_CONTACTOR_SENSE))
+            vcu_fault = VCU_STATUS_VCU_FAULT_MOTOR_PCHG_CONTACTOR_SENSE;
+        else if(faults_is_active(VCU_STATUS_VCU_FAULT_MOTOR_CONTACTOR_SENSE))
+            vcu_fault = VCU_STATUS_VCU_FAULT_MOTOR_CONTACTOR_SENSE;


The faults_is_active function is being called with values from the vcu_status_vcu_fault_e enum instead of the FaultID_e enum. This results in checking the wrong bits in the fault event group. For example, passing VCU_STATUS_VCU_FAULT_MOTOR_PCHG_CONTACTOR_SENSE (value 2) will check if FAULT_ID_MOTOR_DC_BUS_OVERVOLTAGE is active.

Suggested change

else if(faults_is_active(VCU_STATUS_VCU_FAULT_MOTOR_PCHG_CONTACTOR_SENSE))

vcu_fault = VCU_STATUS_VCU_FAULT_MOTOR_PCHG_CONTACTOR_SENSE;

else if(faults_is_active(VCU_STATUS_VCU_FAULT_MOTOR_CONTACTOR_SENSE))

vcu_fault = VCU_STATUS_VCU_FAULT_MOTOR_CONTACTOR_SENSE;

else if(faults_is_active(FAULT_ID_PRECHARGE_SENSE_MISMATCH))

vcu_fault = VCU_STATUS_VCU_FAULT_MOTOR_PCHG_CONTACTOR_SENSE;

else if(faults_is_active(FAULT_ID_MOTOR_SENSE_MISMATCH))

vcu_fault = VCU_STATUS_VCU_FAULT_MOTOR_CONTACTOR_SENSE;

gemini-code-assist · 2026-04-11T22:35:45Z

+        out->MC_MotorVelocity = *((float *)&motor_vel_rx_data[0]);
+        out->MC_VehicleVelocity = *((float *)&motor_vel_rx_data[4]);


Directly dereferencing a casted pointer to float from a uint8_t array can lead to strict aliasing violations and potential alignment faults on some architectures. It is safer to use memcpy for decoding floating-point values from the CAN buffer.

memcpy(&out->MC_MotorVelocity, &motor_vel_rx_data[0], sizeof(float)); memcpy(&out->MC_VehicleVelocity, &motor_vel_rx_data[4], sizeof(float));

gemini-code-assist · 2026-04-11T22:35:45Z

+    if (g_data_read->driver_input.Cruise_Enable) s |= CRUISE_CONTROL_BUTTON_BIT;
+    if (g_data_read->bps_status.BPS_Regen_OK) s |= READY_TO_REGEN_BIT;
+    // if (contactor_get_sense(MOTOR_CONTACTOR) && contactor_get_sense(MOTOR_PRE_CONTACTOR))
+    s |= PRECHARGE_COMPLETE_BIT;


The PRECHARGE_COMPLETE_BIT is hardcoded to be always set, bypassing the actual contactor sense logic. This is dangerous as it allows the FSM to transition to drive states before precharge is actually finished. The commented-out logic should be implemented.

guytonde · 2026-04-12T04:50:00Z

/gemini review

gemini-code-assist

Code Review

This pull request implements a comprehensive refactor of the Vehicle Control Unit (VCU) firmware, introducing a new Finite State Machine (FSM) for motor control, a centralized fault handling system, and a watchdog monitoring system for CAN messages. The CAN driver was also updated to provide dedicated APIs for motor and car communication buses. Feedback highlights several critical bugs, including incorrect indexing in the watchdog module, inverted logic for pedal deviation faults, and the use of an incorrect CAN ID for motor status. Furthermore, improvements are needed regarding the scaling of pedal inputs, adherence to strict aliasing rules during CAN data parsing, and ensuring that the FSM is properly disabled when a fault occurs.

gemini-code-assist · 2026-04-12T04:52:53Z

+        &wd_buffers[wd_count]
+    );
+    configASSERT(t != NULL);
+    wd_timers[wd_count] = t;


The timer handle is being stored using wd_count as the index, but other functions in this module (such as watchdog_received_can_message and watchdog_received_can_message_ISR) use the idx parameter to access the wd_timers array. If watchdogs are created out of order or if idx does not match the creation sequence, this will lead to incorrect timer access or null pointer dereferences.

wd_timers[idx] = t;

gemini-code-assist · 2026-04-12T04:52:53Z

+        fabs(g_data_read->accel_brake.AccelPedal_Main_Pos -
+            g_data_read->accel_brake.AccelPedal_Redundant_Pos) < ACCEPTABLE_PEDAL_DEVIATION) {


The logic for detecting pedal deviation faults is inverted. Currently, a fault is triggered if the difference between the main and redundant pedal positions is less than the acceptable deviation. It should be triggered if the difference exceeds the threshold.

fabs(g_data_read->accel_brake.AccelPedal_Main_Pos - g_data_read->accel_brake.AccelPedal_Redundant_Pos) > ACCEPTABLE_PEDAL_DEVIATION) {

gemini-code-assist · 2026-04-12T04:52:53Z

+    uint8_t moco_status_rx_data[CAN_DLC_MC_VELOCITYMEASUREMENT] = {0};
+
+    can_status_t result =
+        can_fd_recv(motorfdcan, CAN_ID_MC_VELOCITYMEASUREMENT, &header, moco_status_rx_data, delay);


The MotorCAN_Recv_Status function is incorrectly attempting to receive data using CAN_ID_MC_VELOCITYMEASUREMENT (0x403) instead of CAN_ID_MC_STATUS (0x401). This will result in the status struct being populated with velocity data, leading to incorrect fault and limit flag readings.

can_fd_recv(motorfdcan, CAN_ID_MC_STATUS, &header, moco_status_rx_data, delay);

gemini-code-assist · 2026-04-12T04:52:53Z

+        // if we're actually going backwards, let off the pedal until we slow down
+        MotorCAN_Send_Drive_Cmd(0.0f, 0.0f, 0);
+    } else {
+        MotorCAN_Send_Drive_Cmd(MAX_VELOCITY, apply_rollover_limit(g_data_read->accel_brake.AccelPedal_Main_Pos), 0);


The pedal position (AccelPedal_Main_Pos) is being passed directly as the current setpoint to the motor controller. The motor controller expects a normalized float value (typically 0.0 to 1.0), but the pedal position is a uint8_t (likely 0-100). This will cause the motor to receive a setpoint that is orders of magnitude higher than intended. Use the map_to_percent function to scale the input correctly.

MotorCAN_Send_Drive_Cmd(MAX_VELOCITY, apply_rollover_limit(map_to_percent(g_data_read->accel_brake.AccelPedal_Main_Pos, 0, 100, 0, 100)), 0);

gemini-code-assist · 2026-04-12T04:52:53Z

+        out->MC_MotorVelocity = *((float *)&motor_vel_rx_data[0]);
+        out->MC_VehicleVelocity = *((float *)&motor_vel_rx_data[4]);


Directly casting a uint8_t pointer to a float pointer and dereferencing it violates strict aliasing rules and can lead to undefined behavior or alignment issues. Use memcpy to safely extract the floating-point values from the byte array.

memcpy(&out->MC_MotorVelocity, &motor_vel_rx_data[0], sizeof(float)); memcpy(&out->MC_VehicleVelocity, &motor_vel_rx_data[4], sizeof(float));

gemini-code-assist · 2026-04-12T04:52:53Z

-
-            Set_Fault_LED();
-            Fault_Loop();
+            // fsm_disable();


The call to fsm_disable() is commented out. In a fault condition, it is critical to transition the FSM to a safe state (DISABLED) to ensure that no further drive commands are sent to the motor controller.

fsm_disable();

guytonde · 2026-04-12T04:54:19Z

/gemini is there anything wrong with precharge

gemini-code-assist · 2026-04-12T04:54:37Z

The precharge sequence is handled within Firmware/Tasks/Src/PrechargeTask.c. The sequence transitions through WAITING, INITIAL, PRECHARGING, and COMPLETE states.

Key aspects of the precharge logic include:

Fault Monitoring: The Fault_Checker function (called in PRECHARGING and COMPLETE states) monitors for overvoltage, undervoltage, and contactor sense mismatches. If any fault is detected, it triggers the fault handler, which opens all contactors.
Timing: The PRECHARGE_TIMEOUT_MS (400ms) ensures the precharge sequence does not hang indefinitely. If the motor voltage does not reach 90% of the battery voltage within this time, a FAULT_ID_PRECHARGE_TIMEOUT is set.
Hysteresis: Once in the COMPLETE state, the system continuously monitors the voltage ratio. If the motor voltage drops below 80% of the battery voltage (PRECHARGE_THRESHOLD_80), it triggers a FAULT_ID_MOTOR_LT_BATTERY to ensure the system remains in a safe state.

If you are experiencing issues with the precharge sequence, I recommend checking the FaultHandlerTask logs to see if any of these specific fault IDs are being triggered.

guytonde and others added 10 commits March 20, 2026 15:38

fixed canbus test, rewrote faults

d0e0c7d

some shit

91309a6

shifting stuff around

483aedc

building

2960d7a

changed carcan stuff to motor can change back

f74c1c4

tony a bum

2f29cb4

less goonery

e13ce68

can stuff fixed + wrote tests, vcu status works

7ab0d8e

computer gonna die

43aed17

switched back car to motor

ac9986a

gemini-code-assist Bot reviewed Apr 11, 2026

View reviewed changes

guytonde added 2 commits April 11, 2026 19:09

Merge branch 'main' into fsm_integration (kinda)

49b5c7b

ts shit builds

00648f7

gemini-code-assist Bot reviewed Apr 12, 2026

View reviewed changes

GavinW-UT and others added 12 commits April 12, 2026 03:43

precharge working; tested

99dbcae

goon

03d001d

erm dawgs -> no dawgs

c3c40d2

stuff semi working

ddcbc8f

forwarding works

6985b47

somestuff

8182ab4

mirror send drive on carcan

9d9f039

fix oopsie

74d4bbc

add deadzone

b3d6507

convert m/s to mph for swoc thresh

0d9c562

add release artifact workflow

2f77ad9

some bs

06655b6

		fsm_inputs = xEventGroupGetBits(fsmInputGroup);
		current_state = FSM[current_state.NextStates[fsm_inputs]];

		out->MC_MotorVelocity = ((float )&motor_vel_rx_data[0]);
		out->MC_VehicleVelocity = ((float )&motor_vel_rx_data[4]);

		fabs(g_data_read->accel_brake.AccelPedal_Main_Pos -
		g_data_read->accel_brake.AccelPedal_Redundant_Pos) < ACCEPTABLE_PEDAL_DEVIATION) {

Conversation

guytonde commented Apr 11, 2026

Uh oh!

gemini-code-assist Bot left a comment

Choose a reason for hiding this comment

Code Review

Uh oh!

gemini-code-assist Bot Apr 11, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 11, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 11, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 11, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 11, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 11, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 11, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 11, 2026

Choose a reason for hiding this comment

Uh oh!

guytonde commented Apr 12, 2026

Uh oh!

gemini-code-assist Bot left a comment

Choose a reason for hiding this comment

Code Review

Uh oh!

gemini-code-assist Bot Apr 12, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 12, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 12, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 12, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 12, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist Bot Apr 12, 2026

Choose a reason for hiding this comment

Uh oh!

guytonde commented Apr 12, 2026

Uh oh!

gemini-code-assist Bot commented Apr 12, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants