Motor Interfacing + FSM by adiv06 · Pull Request #4 · lhr-solar/PS-VehicleControlUnit

adiv06 · 2026-01-19T08:48:49Z

DO NOT MERGE YET.

Implementation for motor FSM. I made the architecture so more things can be added later, but just wanted to make sure it looked right.

TODO:

Figure out how to expand macros in CAN recv header with enums and not adding a header to embedded sharepoint can.c
Add more faults for motor status (SWOC, Speed, etc..), also add more faults for other things like BPS
Makefile
Clean up macros, move things to headers, etc
Add an equation for limiting speed from motor

gemini-code-assist · 2026-01-19T08:49:14Z

Summary of Changes

Hello @adiv06, I'm Gemini Code Assist¹! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed!

This pull request lays the groundwork for the vehicle's motor control system by introducing a comprehensive Finite State Machine (FSM) architecture. It establishes the necessary CAN communication interfaces for both receiving driver inputs and sending commands to the motor controller. A critical aspect of this work is the integration of a robust fault handling system and a CAN watchdog, designed to ensure system stability and provide mechanisms for recovery or safe shutdown. The changes also include the setup of FreeRTOS tasks to orchestrate these components, forming a foundational embedded software structure.

Highlights

Motor Control FSM Implementation: Introduced a Finite State Machine (FSM) in SendTritium.c to manage various motor control states (e.g., Forward, Reverse, Neutral, Regen, Cruise Control, Disabled).
CAN Communication Integration: Defined and integrated CAN IDs for both car control inputs (pedals, gears, buttons, BPS, ignition) and motor commands, with dedicated receive entries for CAN1 and CAN2.
Robust Fault Handling System: Implemented a FreeRTOS-based fault management framework (faults.h, faults.c) capable of detecting, handling, and potentially recovering from system faults, including a dedicated Task_FaultHandler.
CAN Watchdog for Reliability: Added a Task_CANWatchdog to monitor the timely reception of critical CAN messages, enhancing system reliability and fault detection.
FreeRTOS Task Orchestration: Configured main.c to initialize and manage the FSM, CAN Watchdog, and Fault Handler as FreeRTOS tasks, establishing the core embedded system architecture.
Submodule Integration & VSCode Setup: Included Firmware/Embedded-Sharepoint as a Git submodule and provided VSCode C/C++ configuration for improved development experience.

🧠 New Feature in Public Preview: You can now enable Memory to help Gemini Code Assist learn from your team's feedback. This makes future code reviews more consistent and personalized to your project's style. Click here to enable Memory in your admin console.

Using Gemini Code Assist

The full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips.

Invoking Gemini

You can request assistance from Gemini at any point by creating a comment using either /gemini <command> or @gemini-code-assist <command>. Below is a summary of the supported commands on the current page.

Feature	Command	Description
Code Review	`/gemini review`	Performs a code review for the current pull request in its current state.
Pull Request Summary	`/gemini summary`	Provides a summary of the current pull request in its current state.
Comment	@gemini-code-assist	Responds in comments when explicitly tagged, both in pull request comments and review comments.
Help	`/gemini help`	Displays a list of available commands.

Customization

To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a .gemini/ folder in the base of the repository. Detailed instructions can be found here.

Limitations & Feedback

Gemini Code Assist may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments. If you're interested in giving your feedback about your experience with Gemini Code Assist for Github and other Google products, sign up here.

You can also get AI-powered code generation, chat, as well as code reviews directly in the IDE at no cost with the Gemini Code Assist IDE Extension.

Review the Privacy Notices, Generative AI Prohibited Use Policy, Terms of Service, and learn how to configure Gemini Code Assist in GitHub here. Gemini can make mistakes, so double check it and use code with caution. ↩

gemini-code-assist

Code Review

This pull request introduces a significant architectural change by implementing a motor control finite state machine (FSM) using FreeRTOS. The new architecture includes dedicated tasks for the FSM, fault handling, and a CAN watchdog. While this is a good direction, the code in its current state is not ready for merging. It contains several critical issues that prevent compilation, such as multiple CAN signals sharing the same ID, incorrect definitions of global variables in header files leading to linker errors, and numerous undefined variables and functions. My review focuses on these fundamental blocking issues that need to be addressed before a proper logic review can be conducted.

gemini-code-assist · 2026-01-19T08:51:10Z

+}
+
+void init() {
+  assert(FSM_SIGNAL_COUNT != SEND_TRITIUM_IDS); //ensuring signal for every ID


This assertion assert(FSM_SIGNAL_COUNT != SEND_TRITIUM_IDS) will always fail and halt the program because both FSM_SIGNAL_COUNT and SEND_TRITIUM_IDS are defined as 8. Based on the comment "ensuring signal for every ID", it seems you intend to check for equality.

assert(FSM_SIGNAL_COUNT == SEND_TRITIUM_IDS); //ensuring signal for every ID

…Unit into NewFSM

…olUnit into NewFSM

guytonde · 2026-03-09T14:49:24Z

@gemini review

gemini-code-assist

Code Review

This pull request introduces a significant amount of new infrastructure for the motor controller's finite state machine, including CAN message definitions, fault handling, watchdog timers, and the core FSM logic. The overall architecture is well-structured. My review focuses on improving code quality, maintainability, and correctness. I've identified several instances of code duplication that can be refactored, some unprofessional naming conventions, and a few critical logic errors that need to be addressed. Additionally, there are some configurations set for testing (like CAN loopback mode) that will need to be changed for production, and some uses of printf in time-sensitive contexts that should be replaced with a more robust logging solution.

_{Note: Security Review did not run due to the size of the PR.}

gemini-code-assist · 2026-03-09T14:55:35Z

+    if (status->LWS_OK) {
+        faults_throw_fault(FAULT_ID_STEERING_SENSOR_BAD_DATA);
+    }


This logic appears to be inverted. A fault is thrown if status->LWS_OK is true. The name LWS_OK suggests the sensor is functioning correctly, so a fault should likely be thrown when !status->LWS_OK. Please verify this logic.

Suggested change

if (status->LWS_OK) {

faults_throw_fault(FAULT_ID_STEERING_SENSOR_BAD_DATA);

}

if (!status->LWS_OK) {

faults_throw_fault(FAULT_ID_STEERING_SENSOR_BAD_DATA);

}

gemini-code-assist · 2026-03-09T14:55:35Z

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


The timer handle is being stored at wd_timers[wd_count], but it is accessed elsewhere using wd_timers[idx]. This implementation is fragile and will fail if watchdog indices are not created in a strictly sequential order starting from 0. To make this more robust, you should use idx to index the wd_timers array here as well.

Suggested change

wd_timers[wd_count] = t;

wd_timers[idx] = t;

gemini-code-assist · 2026-03-09T14:55:36Z

+    if (bps_status.BPS_Regen_OK)     s |= READY_TO_REGEN_BIT;
+    if (precharge_complete)          s |= PRECHARGE_COMPLETE_BIT;
+
+    accel_pedal_pct = accel_brake.Accel_Pos_Main;


The raw uint8_t value from accel_brake.Accel_Pos_Main is being directly cast to a float and used as the motor current setpoint. This value should be scaled to a percentage (e.g., 0.0 to 1.0) before being used. The provided map_to_percent function appears to be designed for this purpose and should be used here.

accel_pedal_pct = map_to_percent(accel_brake.Accel_Pos_Main, 0, 255, 0, 100); // TODO: Use calibrated min/max pedal values

gemini-code-assist · 2026-03-09T14:55:36Z

+} vcu_status_vcu_fault_e;
+
+typedef enum {
+    BBPDU_SET_SWITCHES_HSS_CONTROL_13_DO_NOTHING_YOU_RE_STUPID = 3,


The enum member name BBPDU_SET_SWITCHES_HSS_CONTROL_13_DO_NOTHING_YOU_RE_STUPID is unprofessional. Please use professional and descriptive names for all enum members, variables, and functions. This applies to several other similar enum definitions in this file. A more appropriate name could be BBPDU_SET_SWITCHES_HSS_CONTROL_13_INVALID or BBPDU_SET_SWITCHES_HSS_CONTROL_13_RESERVED.

gemini-code-assist · 2026-03-09T14:55:36Z

+#define CAN_ID_MC_DRIVECOMMAND 0x401
+#define CAN_ID_MC_POWERCOMMAND 0x402
+#define CAN_ID_MC_RESETCOMMAND 0x403
+#define CAN_ID_MC_INFO 0x420
+#define CAN_ID_MC_STATUS 0x421
+#define CAN_ID_MC_BUSMEASUREMENT 0x422
+#define CAN_ID_MC_VELOCITYMEASUREMENT 0x423
+#define CAN_ID_MC_PHASECURRENTMEASUREMENT 0x424
+#define CAN_ID_MC_MOTORVOLTAGEVECTORMEASUREMENT 0x425
+#define CAN_ID_MC_MOTORCURRENTVECTORMEASUREMENT 0x426
+#define CAN_ID_MC_BACKEMFMEASUREMENTPREDICTION 0x427
+#define CAN_ID_MC_15VRAILMEASUREMENT 0x428
+#define CAN_ID_MC_3V319VRAILMEASUREMENT 0x429
+#define CAN_ID_MC_MOTOR_TEMPMEASUREMENT 0x42B
+#define CAN_ID_MC_DSPBOARDTEMPMEASUREMENT 0x42C
+#define CAN_ID_MC_ODOMETERBUSAHMEASUREMENT 0x42E
+#define CAN_ID_MC_SLIPSPEEDMEASUREMENT 0x437


These CAN ID macros are already defined in MotorCAN_can_msgs.h. Redefining them here creates code duplication and increases the risk of inconsistencies if one file is updated and the other is not. Please remove these definitions and include MotorCAN_can_msgs.h instead.

Suggested change

#define CAN_ID_MC_DRIVECOMMAND 0x401

#define CAN_ID_MC_POWERCOMMAND 0x402

#define CAN_ID_MC_RESETCOMMAND 0x403

#define CAN_ID_MC_INFO 0x420

#define CAN_ID_MC_STATUS 0x421

#define CAN_ID_MC_BUSMEASUREMENT 0x422

#define CAN_ID_MC_VELOCITYMEASUREMENT 0x423

#define CAN_ID_MC_PHASECURRENTMEASUREMENT 0x424

#define CAN_ID_MC_MOTORVOLTAGEVECTORMEASUREMENT 0x425

#define CAN_ID_MC_MOTORCURRENTVECTORMEASUREMENT 0x426

#define CAN_ID_MC_BACKEMFMEASUREMENTPREDICTION 0x427

#define CAN_ID_MC_15VRAILMEASUREMENT 0x428

#define CAN_ID_MC_3V319VRAILMEASUREMENT 0x429

#define CAN_ID_MC_MOTOR_TEMPMEASUREMENT 0x42B

#define CAN_ID_MC_DSPBOARDTEMPMEASUREMENT 0x42C

#define CAN_ID_MC_ODOMETERBUSAHMEASUREMENT 0x42E

#define CAN_ID_MC_SLIPSPEEDMEASUREMENT 0x437

#include "MotorCAN_can_msgs.h"

gemini-code-assist · 2026-03-09T14:55:36Z

+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",


The includePath entry "${workspaceFolder}/**" is very broad and can cause performance issues with IntelliSense by making it scan every file in the workspace. Since you have more specific include paths defined, this one is redundant and should be removed to improve editor performance.

gemini-code-assist · 2026-03-09T14:55:36Z

+static void wd_callback(TimerHandle_t xTimer) {
+    uint8_t idx = (uint8_t)(uintptr_t)pvTimerGetTimerID(xTimer);
+    wd_alive[idx] = false;
+    printf("WD timeout: signal %d\n", idx);


Using printf within a timer callback is not recommended in production embedded systems, as it can be slow, blocking, and is often not re-entrant. For the final implementation, this should be replaced with a more robust, non-blocking logging mechanism, such as a dedicated logging task.

gemini-code-assist · 2026-03-09T14:55:36Z

+
+        for (int i = 0; i < FAULT_ID_COUNT; i++) {
+            if (active & (1U << i)) {
+                printf("[FAULT] %s (bit %d) \n", fault_names[i], i);


Using printf in a high-priority fault handler task can introduce blocking behavior and affect the real-time performance of the system. For production code, consider using a non-blocking logging mechanism, such as sending log messages to a dedicated, lower-priority logging task.

gemini-code-assist · 2026-03-09T14:55:36Z

+typedef struct {
+    uint8_t BPS_Tap_ID;
+    uint8_t BPS_VoltTemp_BQ_Fault;
+    uint8_t BPS_Temperature_Tap_Fault;
+    uint16_t BPS_Voltage_Tap_Data;
+    int16_t BPS_Temperature_Tap_Data;
+    uint16_t BPS_Temperature_Tap_RawV;
+} bps_voltage_temperature_0_t;
+
+typedef struct {
+    uint8_t BPS_Tap_ID;
+    uint8_t BPS_VoltTemp_BQ_Fault;
+    uint8_t BPS_Temperature_Tap_Fault;
+    uint16_t BPS_Voltage_Tap_Data;
+    int16_t BPS_Temperature_Tap_Data;
+    uint16_t BPS_Temperature_Tap_RawV;
+} bps_voltage_temperature_1_t;
+
+typedef struct {
+    uint8_t BPS_Tap_ID;
+    uint8_t BPS_VoltTemp_BQ_Fault;
+    uint8_t BPS_Temperature_Tap_Fault;
+    uint16_t BPS_Voltage_Tap_Data;
+    int16_t BPS_Temperature_Tap_Data;
+    uint16_t BPS_Temperature_Tap_RawV;
+} bps_voltage_temperature_2_t;
+
+typedef struct {
+    uint8_t BPS_Tap_ID;
+    uint8_t BPS_VoltTemp_BQ_Fault;
+    uint8_t BPS_Temperature_Tap_Fault;
+    uint16_t BPS_Voltage_Tap_Data;
+    int16_t BPS_Temperature_Tap_Data;
+    uint16_t BPS_Temperature_Tap_RawV;
+} bps_voltage_temperature_3_t;
+
+typedef struct {
+    uint8_t BPS_Tap_ID;
+    uint8_t BPS_VoltTemp_BQ_Fault;
+    uint8_t BPS_Temperature_Tap_Fault;
+    uint16_t BPS_Voltage_Tap_Data;
+    int16_t BPS_Temperature_Tap_Data;
+    uint16_t BPS_Temperature_Tap_RawV;
+} bps_voltage_temperature_4_t;
+
+typedef struct {
+    uint8_t BPS_Tap_ID;
+    uint8_t BPS_VoltTemp_BQ_Fault;
+    uint8_t BPS_Temperature_Tap_Fault;
+    uint16_t BPS_Voltage_Tap_Data;
+    int16_t BPS_Temperature_Tap_Data;
+    uint16_t BPS_Temperature_Tap_RawV;
+} bps_voltage_temperature_5_t;
+
+typedef struct {
+    uint8_t BPS_Tap_ID;
+    uint8_t BPS_VoltTemp_BQ_Fault;
+    uint8_t BPS_Temperature_Tap_Fault;
+    uint16_t BPS_Voltage_Tap_Data;
+    int16_t BPS_Temperature_Tap_Data;
+    uint16_t BPS_Temperature_Tap_RawV;
+} bps_voltage_temperature_6_t;
+
+typedef struct {
+    uint8_t BPS_Tap_ID;
+    uint8_t BPS_VoltTemp_BQ_Fault;
+    uint8_t BPS_Temperature_Tap_Fault;
+    uint16_t BPS_Voltage_Tap_Data;
+    int16_t BPS_Temperature_Tap_Data;
+    uint16_t BPS_Temperature_Tap_RawV;
+} bps_voltage_temperature_7_t;


The structs bps_voltage_temperature_0_t through bps_voltage_temperature_7_t are all identical. This leads to significant code duplication. To improve maintainability, you should define a single generic struct, for example bps_voltage_temperature_t, and reuse it for all these messages.

gemini-code-assist · 2026-03-09T14:55:36Z

+            ((uint8_t)precharge_complete                       << 0) |  // Motor_Contactor_State
+            ((uint8_t)precharge_complete                       << 1) |  // Motor_Precharge_Contactor_State
+            ((uint8_t)precharge_complete                       << 2) |  // Motor_Ready_To_Drive


The motor contactor state and ready-to-drive status are all derived from the precharge_complete flag. This is likely a placeholder, but it's important to note that for the final implementation, these status bits should reflect the actual hardware state. For example, contactor state should be based on feedback from the BPS or other relevant sensors, not just the VCU's internal precharge status.

adiv06 added 4 commits December 6, 2025 11:23

first commit

166a49a

CAN attached, no tasks

1b6434d

Made RTOS Tasks, fault arch, small CAN changes,

925f253

Merge remote-tracking branch 'origin/main' into NewFSM

5c44c13

gemini-code-assist bot reviewed Jan 19, 2026

View reviewed changes

Rav4s requested review from Lakshay983, Rav4s and Tony5427 January 19, 2026 16:43

Rav4s assigned adiv06 Jan 26, 2026

adiv06 added 20 commits January 28, 2026 02:12

made timer CAN arch

c5d2778

Removed an infinite loop

75baa21

split motor and telem tasks

0224fd8

Got Motor Params and added to faults

6b04b03

closer to compiling

e41f8e6

FSM header gets generated now in FSM.h from python script

8814fa1

Merge branch 'main' of https://github.com/lhr-solar/PS-VehicleControl…

c84e47a

…Unit into NewFSM

might need to run on board to compile for CAN

366b49a

merge in ES

9bdeede

Merge branch 'NewFSM' of https://github.com/lhr-solar/PS-VehicleContr…

472a453

…olUnit into NewFSM

fixed fwd/rev logic and gonna delete runtime FSM

7a32800

added braking, this feels more and more cursed

1a957a8

FSM in init now

b6e13c5

chopped

e6ea799

generate CAN from DBC

7afb8cb

correct linking for can gen

e9ebe01

Added real can msgs and init task

5cf31d0

Added VCU broadcasting

c800404

Added some semaphores/mutexes and fixed some deps

55670a3

made the CAN recv literals

f050a93

changed to g4

eb8dba3

guytonde marked this pull request as ready for review March 4, 2026 15:50

guytonde added 8 commits March 5, 2026 17:42

not building but errors are stupid, will fix tn

7def261

it compilin 🍆😋

2bd478c

moved can logic out of fsm and made a drivers folder

71bac6b

fix some shit?

94ebe3a

fix github action?

004e5ca

bruh

e8ec94e

submod

35dfabc

fixed some fsm stuff

9b7a3c9

gemini-code-assist bot reviewed Mar 9, 2026

View reviewed changes

Conversation

adiv06 commented Jan 19, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

gemini-code-assist bot commented Jan 19, 2026

Summary of Changes

Highlights

Footnotes

Uh oh!

gemini-code-assist bot left a comment

Choose a reason for hiding this comment

Code Review

Uh oh!

Uh oh!

Uh oh!

gemini-code-assist bot Jan 19, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

guytonde commented Mar 9, 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 Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

adiv06 commented Jan 19, 2026 •

edited

Loading