From 7e0b1c6c426963c0d12557da6521c4c28ffe67e6 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 5 Jan 2025 16:54:15 -0500 Subject: [PATCH 01/36] adding in linter w/ rules and step --- .clang-tidy | 3 +++ .github/workflows/platformio.yml | 5 ++++- .gitignore | 1 + platformio.ini | 8 +++++++- 4 files changed, 15 insertions(+), 2 deletions(-) create mode 100644 .clang-tidy diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..9c1501f --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,3 @@ +Checks: bugprone-*, cppcoreguidelines-* +WarningsAsErrors: 'cppcoreguidelines-*' +FormatStyle: 'file' \ No newline at end of file diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index 5d913b2..8fdaf95 100644 --- a/.github/workflows/platformio.yml +++ b/.github/workflows/platformio.yml @@ -24,4 +24,7 @@ jobs: run: pio run -e teensy41 - name: Run Tests - run: pio test -e test_systems_env \ No newline at end of file + run: pio test -e test_systems_env + + - name: run checks + run: pio check -e teensy41 --fail-on-defect high \ No newline at end of file diff --git a/.gitignore b/.gitignore index df66742..19e3307 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ .pio .vscode/* compile_commands.json +.cache/ \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index d23c416..7d90741 100644 --- a/platformio.ini +++ b/platformio.ini @@ -4,6 +4,8 @@ ; https://docs.platformio.org/page/projectconf.html [common] + + lib_deps_shared = https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 https://github.com/hytech-racing/shared_firmware_types.git#HT09-shared-datatypes @@ -15,12 +17,16 @@ lib_deps_shared = ; * UPLOAD to compile and upload on-car software. ; * TEST is not yet configured. [env:teensy41] +check_tool = clangtidy +check_flags = + clangtidy: --config-file=./.clang-tidy +check_severity = medium, high + test_framework=googletest build_unflags = -std=gnu++11 build_flags = -std=c++17 -D TEENSY_OPT_SMALLEST_CODE -check_tool = clangtidy check_src_filters = + + From 6a48d293e53cba7ea19c0d13944019df236bba5b Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 5 Jan 2025 16:54:27 -0500 Subject: [PATCH 02/36] added in check steps --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 50a95be..53acaab 100644 --- a/README.md +++ b/README.md @@ -16,5 +16,11 @@ If you are not already familiar with PlatformIO, please read [Module 4](https:// To TEST code, open a PlatformIO terminal (CTRL-SHIFT-P, then `PlatformIO: New Terminal`). Then, to test systems (non hardware-specific logic), call `pio test -e test_systems_env`. This will run all unit tests in the `test/test_systems` directory. +## Check code + +in the PlatformIO terminal: + +`pio check -e teensy41 --fail-on-defect high` + ## FAQ: Why red squiggles? See [this](https://wiki.hytechracing.org/books/hytech-racing-general-info/page/vscode-setup-for-auto-completion-and-code-navigation) page on our BookStack for help configuring VSCode for auto completion and code navigation within external library dependencies. From b728f9c19ddf69003d17e904e2464c8cd5364630 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 5 Jan 2025 17:42:16 -0500 Subject: [PATCH 03/36] updating clang / ide stuff --- .clang-tidy | 10 +++++++--- .clangd | 2 ++ 2 files changed, 9 insertions(+), 3 deletions(-) create mode 100644 .clangd diff --git a/.clang-tidy b/.clang-tidy index 9c1501f..0ded994 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,3 +1,7 @@ -Checks: bugprone-*, cppcoreguidelines-* -WarningsAsErrors: 'cppcoreguidelines-*' -FormatStyle: 'file' \ No newline at end of file +Checks: 'bugprone-*, cppcoreguidelines-*, readability-identifier-naming' +WarningsAsErrors: 'cppcoreguidelines-*, bugprone-*, readability-identifier-naming' +FormatStyle: 'file' +CheckOptions: + - { key: readability-identifier-naming.ClassMemberCase, value: 'lower_case' } + - { key: readability-identifier-naming.ClassMemberPrefix, value: '_' } + - { key: readability-identifier-naming.FunctionCase, value: 'lower_case' } \ No newline at end of file diff --git a/.clangd b/.clangd new file mode 100644 index 0000000..ac05184 --- /dev/null +++ b/.clangd @@ -0,0 +1,2 @@ +CompileFlags: + CompilationDatabase: . \ No newline at end of file From 29b73b12a559150ddc21cbcc46a832bc41a03719 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 5 Jan 2025 16:54:15 -0500 Subject: [PATCH 04/36] adding in linter w/ rules and step --- .clang-tidy | 3 +++ .github/workflows/platformio.yml | 5 ++++- platformio.ini | 8 +++++++- 3 files changed, 14 insertions(+), 2 deletions(-) create mode 100644 .clang-tidy diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..9c1501f --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,3 @@ +Checks: bugprone-*, cppcoreguidelines-* +WarningsAsErrors: 'cppcoreguidelines-*' +FormatStyle: 'file' \ No newline at end of file diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index 5d913b2..8fdaf95 100644 --- a/.github/workflows/platformio.yml +++ b/.github/workflows/platformio.yml @@ -24,4 +24,7 @@ jobs: run: pio run -e teensy41 - name: Run Tests - run: pio test -e test_systems_env \ No newline at end of file + run: pio test -e test_systems_env + + - name: run checks + run: pio check -e teensy41 --fail-on-defect high \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index d23c416..7d90741 100644 --- a/platformio.ini +++ b/platformio.ini @@ -4,6 +4,8 @@ ; https://docs.platformio.org/page/projectconf.html [common] + + lib_deps_shared = https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 https://github.com/hytech-racing/shared_firmware_types.git#HT09-shared-datatypes @@ -15,12 +17,16 @@ lib_deps_shared = ; * UPLOAD to compile and upload on-car software. ; * TEST is not yet configured. [env:teensy41] +check_tool = clangtidy +check_flags = + clangtidy: --config-file=./.clang-tidy +check_severity = medium, high + test_framework=googletest build_unflags = -std=gnu++11 build_flags = -std=c++17 -D TEENSY_OPT_SMALLEST_CODE -check_tool = clangtidy check_src_filters = + + From c8e4f288db4485b0d2ad948bb71baeea11d6a65f Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 5 Jan 2025 16:54:27 -0500 Subject: [PATCH 05/36] added in check steps --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 50a95be..53acaab 100644 --- a/README.md +++ b/README.md @@ -16,5 +16,11 @@ If you are not already familiar with PlatformIO, please read [Module 4](https:// To TEST code, open a PlatformIO terminal (CTRL-SHIFT-P, then `PlatformIO: New Terminal`). Then, to test systems (non hardware-specific logic), call `pio test -e test_systems_env`. This will run all unit tests in the `test/test_systems` directory. +## Check code + +in the PlatformIO terminal: + +`pio check -e teensy41 --fail-on-defect high` + ## FAQ: Why red squiggles? See [this](https://wiki.hytechracing.org/books/hytech-racing-general-info/page/vscode-setup-for-auto-completion-and-code-navigation) page on our BookStack for help configuring VSCode for auto completion and code navigation within external library dependencies. From ab999fe09497de3a4517a24d0f08ce54ce2386a0 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 5 Jan 2025 17:42:16 -0500 Subject: [PATCH 06/36] updating clang / ide stuff --- .clang-tidy | 10 +++++++--- .clangd | 2 ++ 2 files changed, 9 insertions(+), 3 deletions(-) create mode 100644 .clangd diff --git a/.clang-tidy b/.clang-tidy index 9c1501f..0ded994 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,3 +1,7 @@ -Checks: bugprone-*, cppcoreguidelines-* -WarningsAsErrors: 'cppcoreguidelines-*' -FormatStyle: 'file' \ No newline at end of file +Checks: 'bugprone-*, cppcoreguidelines-*, readability-identifier-naming' +WarningsAsErrors: 'cppcoreguidelines-*, bugprone-*, readability-identifier-naming' +FormatStyle: 'file' +CheckOptions: + - { key: readability-identifier-naming.ClassMemberCase, value: 'lower_case' } + - { key: readability-identifier-naming.ClassMemberPrefix, value: '_' } + - { key: readability-identifier-naming.FunctionCase, value: 'lower_case' } \ No newline at end of file diff --git a/.clangd b/.clangd new file mode 100644 index 0000000..ac05184 --- /dev/null +++ b/.clangd @@ -0,0 +1,2 @@ +CompileFlags: + CompilationDatabase: . \ No newline at end of file From 79206eed08a1a9a79fdb5126b275f879d7ac17e1 Mon Sep 17 00:00:00 2001 From: Justin Hwang Date: Sun, 5 Jan 2025 20:13:29 -0500 Subject: [PATCH 07/36] WIP: Fixing linter errors --- include/VCR_Globals.h | 4 ++-- include/VCR_Tasks.h | 10 +++++----- lib/state_machine/include/VehicleStateMachine.h | 4 ++-- lib/state_machine/src/VehicleStateMachine.cpp | 4 ++++ lib/systems/include/BaseController.h | 2 +- lib/systems/include/{Buzzer.h => BuzzerController.h} | 0 lib/systems/include/PhysicalParameters.h | 6 +++--- src/VCR_Tasks.cpp | 2 +- 8 files changed, 18 insertions(+), 14 deletions(-) rename lib/systems/include/{Buzzer.h => BuzzerController.h} (100%) diff --git a/include/VCR_Globals.h b/include/VCR_Globals.h index af12fc8..eb8cef6 100644 --- a/include/VCR_Globals.h +++ b/include/VCR_Globals.h @@ -11,8 +11,8 @@ #include "VCR_Constants.h" /* Interface and system data structs */ -extern VCRInterfaceData_s interface_data; -extern VCRSystemData_s system_data; +extern VCRInterfaceData_s interface_data; // NOLINT +extern VCRSystemData_s system_data; // NOLINT /* ADC setup */ extern MCP_ADC<8> adc_0; // MCP3208. ADC0 in VCR schematic. Used for valuable telem data. diff --git a/include/VCR_Tasks.h b/include/VCR_Tasks.h index 30c8521..9594603 100644 --- a/include/VCR_Tasks.h +++ b/include/VCR_Tasks.h @@ -38,7 +38,7 @@ */ bool init_test_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); bool run_test_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); -extern HT_TASK::Task test_task; +extern const HT_TASK::Task test_task; /** @@ -47,7 +47,7 @@ extern HT_TASK::Task test_task; * singleton classes to minimize the need for passing instances around. */ bool run_tick_state_machine_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); -extern HT_TASK::Task tick_state_machine_task; +extern const HT_TASK::Task tick_state_machine_task; @@ -58,7 +58,7 @@ extern HT_TASK::Task tick_state_machine_task; */ bool init_read_adc0_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); bool run_read_adc0_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); -extern HT_TASK::Task read_adc0_task; +extern const HT_TASK::Task read_adc0_task; @@ -72,7 +72,7 @@ extern HT_TASK::Task read_adc0_task; */ bool init_read_adc1_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); bool run_read_adc1_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); -extern HT_TASK::Task read_adc1_task; +extern const HT_TASK::Task read_adc1_task; @@ -81,6 +81,6 @@ extern HT_TASK::Task read_adc1_task; * update function of the buzzer controller. */ bool run_update_buzzer_controller_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); -extern HT_TASK::Task update_buzzer_controller_task; +extern const HT_TASK::Task update_buzzer_controller_task; #endif /* VCR_TASKS */ \ No newline at end of file diff --git a/lib/state_machine/include/VehicleStateMachine.h b/lib/state_machine/include/VehicleStateMachine.h index 636af0b..de48950 100644 --- a/lib/state_machine/include/VehicleStateMachine.h +++ b/lib/state_machine/include/VehicleStateMachine.h @@ -19,7 +19,7 @@ * enough time has passed, we enter the next state. * READY_TO_DRIVE - While in this state, pedal inputs command the drivetrain. */ -enum class CAR_STATE +const enum class CAR_STATE { STARTUP = 0, TRACTIVE_SYSTEM_NOT_ACTIVE = 1, @@ -36,7 +36,7 @@ enum class CAR_STATE * VehicleStateMachine uses instance data directly from other singleton classes (Buzzer, Pedals, etc) * and from the global structs (VCRInterfaceData_s, VCRSystemData_s, etc.). */ -class VehicleStateMachine +const class VehicleStateMachine { public: static VehicleStateMachine& getInstance() diff --git a/lib/state_machine/src/VehicleStateMachine.cpp b/lib/state_machine/src/VehicleStateMachine.cpp index 4d48c2e..55c1f36 100644 --- a/lib/state_machine/src/VehicleStateMachine.cpp +++ b/lib/state_machine/src/VehicleStateMachine.cpp @@ -140,6 +140,7 @@ void VehicleStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_mil void VehicleStateMachine::handle_exit_logic_(CAR_STATE prev_state, unsigned long curr_millis) { + // NOLINTBEGIN switch (get_state()) { case CAR_STATE::STARTUP: @@ -157,10 +158,12 @@ void VehicleStateMachine::handle_exit_logic_(CAR_STATE prev_state, unsigned long default: break; } + // NOLINTEND } void VehicleStateMachine::handle_entry_logic_(CAR_STATE new_state, unsigned long curr_millis) { + // NOLINTBEGIN switch (new_state) { case CAR_STATE::STARTUP: @@ -181,4 +184,5 @@ void VehicleStateMachine::handle_entry_logic_(CAR_STATE new_state, unsigned long default: break; } + // NOLINTEND } diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h index 0186cfa..a48fb59 100644 --- a/lib/systems/include/BaseController.h +++ b/lib/systems/include/BaseController.h @@ -4,7 +4,7 @@ /// @brief defines namespace for definition of a drivetrain command with no torque for clearer code in the Muxer /// This can be used to define other specific car states in the future -namespace BaseControllerParams +const namespace BaseControllerParams { const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = { .speeds_rpm = {0.0, 0.0, 0.0, 0.0}, diff --git a/lib/systems/include/Buzzer.h b/lib/systems/include/BuzzerController.h similarity index 100% rename from lib/systems/include/Buzzer.h rename to lib/systems/include/BuzzerController.h diff --git a/lib/systems/include/PhysicalParameters.h b/lib/systems/include/PhysicalParameters.h index ace4710..9de092f 100644 --- a/lib/systems/include/PhysicalParameters.h +++ b/lib/systems/include/PhysicalParameters.h @@ -1,5 +1,5 @@ -#ifndef __PHYSICALPARAMETERS_H__ -#define __PHYSICALPARAMETERS_H__ +#ifndef PHYSICALPARAMETERS +#define PHYSICALPARAMETERS namespace PhysicalParameters { @@ -17,4 +17,4 @@ constexpr const float METERS_PER_SECOND_TO_RPM = 1.0 / RPM_TO_METERS_PER_SECO const float RPM_TO_RAD_PER_SECOND = 2 * 3.1415 / 60.0; const float RAD_PER_SECOND_TO_RPM = 1 / RPM_TO_RAD_PER_SECOND; -#endif /* __PHYSICALPARAMETERS_H__ */ \ No newline at end of file +#endif /* PHYSICALPARAMETERS */ \ No newline at end of file diff --git a/src/VCR_Tasks.cpp b/src/VCR_Tasks.cpp index 9e95273..c605c7e 100644 --- a/src/VCR_Tasks.cpp +++ b/src/VCR_Tasks.cpp @@ -9,7 +9,7 @@ /* Local includes */ #include "VCR_Constants.h" -#include "Buzzer.h" +#include "BuzzerController.h" #include "VCR_Globals.h" #include "VehicleStateMachine.h" From 9da3d5b1287ce004a6097b904703235442be63ed Mon Sep 17 00:00:00 2001 From: Justin Hwang Date: Sun, 5 Jan 2025 20:43:21 -0500 Subject: [PATCH 08/36] WIP: Fixing Linter errors --- include/VCR_Tasks.h | 1 + lib/state_machine/include/VehicleStateMachine.h | 6 +++--- lib/systems/include/BaseController.h | 2 +- lib/systems/include/SafetySystem.h | 2 +- lib/systems/include/TorqueControllerMux.hpp | 4 ++-- src/VCR_Globals.cpp | 6 +++++- src/VCR_Tasks.cpp | 13 +++++++------ src/main.cpp | 12 ++---------- 8 files changed, 22 insertions(+), 24 deletions(-) diff --git a/include/VCR_Tasks.h b/include/VCR_Tasks.h index 9594603..4f9b3c9 100644 --- a/include/VCR_Tasks.h +++ b/include/VCR_Tasks.h @@ -25,6 +25,7 @@ #ifndef VCR_TASKS #define VCR_TASKS +// NOLINTBEGIN // Allow tasks to be non-constant to avoid updating the HT_SCHED library. /* From HT_SCHED library */ #include "ht_sched.hpp" diff --git a/lib/state_machine/include/VehicleStateMachine.h b/lib/state_machine/include/VehicleStateMachine.h index de48950..287c39e 100644 --- a/lib/state_machine/include/VehicleStateMachine.h +++ b/lib/state_machine/include/VehicleStateMachine.h @@ -4,7 +4,7 @@ /* From local systems library */ #include "DrivetrainSystem.h" #include "SafetySystem.h" -#include "Buzzer.h" +#include "BuzzerController.h" /** * Enum representing possible states for the vehicle's state machine. @@ -19,7 +19,7 @@ * enough time has passed, we enter the next state. * READY_TO_DRIVE - While in this state, pedal inputs command the drivetrain. */ -const enum class CAR_STATE +enum class CAR_STATE //NOLINT { STARTUP = 0, TRACTIVE_SYSTEM_NOT_ACTIVE = 1, @@ -36,7 +36,7 @@ const enum class CAR_STATE * VehicleStateMachine uses instance data directly from other singleton classes (Buzzer, Pedals, etc) * and from the global structs (VCRInterfaceData_s, VCRSystemData_s, etc.). */ -const class VehicleStateMachine +class VehicleStateMachine //NOLINT { public: static VehicleStateMachine& getInstance() diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h index a48fb59..0186cfa 100644 --- a/lib/systems/include/BaseController.h +++ b/lib/systems/include/BaseController.h @@ -4,7 +4,7 @@ /// @brief defines namespace for definition of a drivetrain command with no torque for clearer code in the Muxer /// This can be used to define other specific car states in the future -const namespace BaseControllerParams +namespace BaseControllerParams { const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = { .speeds_rpm = {0.0, 0.0, 0.0, 0.0}, diff --git a/lib/systems/include/SafetySystem.h b/lib/systems/include/SafetySystem.h index 196937b..d9feb73 100644 --- a/lib/systems/include/SafetySystem.h +++ b/lib/systems/include/SafetySystem.h @@ -1,7 +1,7 @@ #ifndef SAFETY_SYSTEM #define SAFETY_SYSTEM -class SafetySystem +class SafetySystem //NOLINT { public: static SafetySystem& getInstance() diff --git a/lib/systems/include/TorqueControllerMux.hpp b/lib/systems/include/TorqueControllerMux.hpp index 15467a6..8dfdb7e 100644 --- a/lib/systems/include/TorqueControllerMux.hpp +++ b/lib/systems/include/TorqueControllerMux.hpp @@ -115,8 +115,8 @@ class TorqueControllerMux {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0f}, {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0f}}; size_t _num_motors; - float max_change_speed_, max_torque_pos_change_delta_, max_power_limit_; - DrivetrainCommand_s prev_command_ = {}; + float _max_change_speed, _max_torque_pos_change_delta, _max_power_limit; + DrivetrainCommand_s _prev_command = {}; TorqueControllerMuxStatus_s active_status_ = {}; TorqueControllerMuxError_e can_switch_controller_(DrivetrainDynamicReport_s active_drivetrain_data, DrivetrainCommand_s previous_controller_command, diff --git a/src/VCR_Globals.cpp b/src/VCR_Globals.cpp index 2edd121..8b90da3 100644 --- a/src/VCR_Globals.cpp +++ b/src/VCR_Globals.cpp @@ -7,10 +7,14 @@ /* From shared-firmware-types */ #include "SharedFirmwareTypes.h" +//NOLINTBEGIN + /* Interface and system data structs */ VCRInterfaceData_s interface_data; VCRSystemData_s system_data; /* ADC setup */ MCP_ADC<8> adc_0 = MCP_ADC<8>(ADC0_CS); // MCP3208. ADC0 in VCR schematic. Used for valuable telem data. -MCP_ADC<8> adc_1 = MCP_ADC<8>(ADC1_CS); // MCP3208. ADC1 in VCR schematic. Used for extra thermistors or extra sensors while testing. \ No newline at end of file +MCP_ADC<8> adc_1 = MCP_ADC<8>(ADC1_CS); // MCP3208. ADC1 in VCR schematic. Used for extra thermistors or extra sensors while testing. + +//NOLINTEND \ No newline at end of file diff --git a/src/VCR_Tasks.cpp b/src/VCR_Tasks.cpp index c605c7e..fa16901 100644 --- a/src/VCR_Tasks.cpp +++ b/src/VCR_Tasks.cpp @@ -25,7 +25,7 @@ bool run_test_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& task return true; } -HT_TASK::Task test_task = HT_TASK::Task(init_test_task, run_test_task, 1, 100000UL); // 100,000us is 10hz +HT_TASK::Task test_task = HT_TASK::Task(init_test_task, run_test_task, 1, 100000UL); // 100,000us is 10hz //NOLINT @@ -62,7 +62,7 @@ bool run_read_adc0_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& return true; } -HT_TASK::Task read_adc0_task = HT_TASK::Task(init_read_adc0_task, run_read_adc0_task, 10, 1000UL); // 1000us is 1kHz +HT_TASK::Task read_adc0_task = HT_TASK::Task(init_read_adc0_task, run_read_adc0_task, 10, 1000UL); // 1000us is 1kHz //NOLINT @@ -72,13 +72,13 @@ bool run_tick_state_machine_task(const unsigned long& sysMicros, const HT_TASK:: return true; } -HT_TASK::Task tick_state_machine_task = HT_TASK::Task(HT_TASK::DUMMY_FUNCTION, run_tick_state_machine_task, 2); // Idle (constant-update) task +HT_TASK::Task tick_state_machine_task = HT_TASK::Task(HT_TASK::DUMMY_FUNCTION, run_tick_state_machine_task, 2); // Idle (constant-update) task //NOLINT bool init_read_adc1_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { - + /* NOLINTBEGIN */ // Thermistor channels are for testing purposes only, the pin numbers 0-7 are acceptable "magic numbers". // Initialize all eight channels to scale = 1, offset = 0 adc_1.setChannelScaleAndOffset(0, 1, 0); adc_1.setChannelScaleAndOffset(1, 1, 0); @@ -88,6 +88,7 @@ bool init_read_adc1_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo adc_1.setChannelScaleAndOffset(5, 1, 0); adc_1.setChannelScaleAndOffset(6, 1, 0); adc_1.setChannelScaleAndOffset(7, 1, 0); + /* NOLINTEND */ hal_printf("Initialized ADC0 at %d (micros)\n", sysMicros); @@ -103,7 +104,7 @@ bool run_read_adc1_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& return true; } -HT_TASK::Task read_adc1_task = HT_TASK::Task(init_read_adc1_task, run_read_adc1_task, 10, 40000UL); // 20000us is 25Hz +HT_TASK::Task read_adc1_task = HT_TASK::Task(init_read_adc1_task, run_read_adc1_task, 10, 40000UL); // 20000us is 25Hz //NOLINT @@ -115,4 +116,4 @@ bool run_update_buzzer_controller_task(const unsigned long& sysMicros, const HT_ return true; } -HT_TASK::Task update_buzzer_controller_task = HT_TASK::Task(HT_TASK::DUMMY_FUNCTION, run_update_buzzer_controller_task, 10, 20000UL); // 20000us is 50hz \ No newline at end of file +HT_TASK::Task update_buzzer_controller_task = HT_TASK::Task(HT_TASK::DUMMY_FUNCTION, run_update_buzzer_controller_task, 10, 20000UL); // 20000us is 50hz //NOLINT \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 5036ed2..1dec98b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -27,15 +27,7 @@ /* Scheduler setup */ -HT_SCHED::Scheduler& scheduler = HT_SCHED::Scheduler::getInstance(); - -auto start_time = std::chrono::high_resolution_clock::now(); -unsigned long stdMicros() -{ - auto now = std::chrono::high_resolution_clock::now(); - auto elapsed = std::chrono::duration_cast(now - start_time).count(); - return static_cast(elapsed); -} +const HT_SCHED::Scheduler& scheduler = HT_SCHED::Scheduler::getInstance(); @@ -46,7 +38,7 @@ qindesign::network::EthernetUDP protobuf_recv_socket; void setup() { - scheduler.setTimingFunction(stdMicros); + scheduler.setTimingFunction(micros); scheduler.schedule(tick_state_machine_task); scheduler.schedule(read_adc0_task); From 72fb475e1fbfd0a75deccd706ff55c8b2839b089 Mon Sep 17 00:00:00 2001 From: Justin Hwang Date: Sun, 5 Jan 2025 21:24:28 -0500 Subject: [PATCH 09/36] Updated code to abide by clangtidy rules --- .clang-tidy | 2 +- include/VCR_Globals.h | 5 +- include/VCR_Tasks.h | 10 ++-- .../include/VehicleStateMachine.h | 4 +- lib/state_machine/src/VehicleStateMachine.cpp | 4 -- lib/systems/include/BuzzerController.h | 3 ++ lib/systems/include/DrivetrainSystem.h | 42 ++++++++-------- lib/systems/include/DrivetrainSystem.tpp | 10 ++-- lib/systems/include/PhysicalParameters.h | 16 +++---- lib/systems/include/TorqueControllerMux.hpp | 22 ++++----- lib/systems/include/TorqueControllerMux.tpp | 48 +++++++++---------- src/VCR_Globals.cpp | 8 +--- src/VCR_Tasks.cpp | 4 +- 13 files changed, 87 insertions(+), 91 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 0ded994..a895436 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,4 +1,4 @@ -Checks: 'bugprone-*, cppcoreguidelines-*, readability-identifier-naming' +Checks: 'bugprone-*, cppcoreguidelines-*, readability-identifier-naming, -cppcoreguidelines-avoid-non-const-global-variables, -bugprone-branch-clone, -cppcoreguidelines-pro-type-vararg, -bugprone-easily-swappable-parameters' WarningsAsErrors: 'cppcoreguidelines-*, bugprone-*, readability-identifier-naming' FormatStyle: 'file' CheckOptions: diff --git a/include/VCR_Globals.h b/include/VCR_Globals.h index eb8cef6..541faef 100644 --- a/include/VCR_Globals.h +++ b/include/VCR_Globals.h @@ -15,7 +15,8 @@ extern VCRInterfaceData_s interface_data; // NOLINT extern VCRSystemData_s system_data; // NOLINT /* ADC setup */ -extern MCP_ADC<8> adc_0; // MCP3208. ADC0 in VCR schematic. Used for valuable telem data. -extern MCP_ADC<8> adc_1; // MCP3208. ADC1 in VCR schematic. Used for extra thermistors or extra sensors while testing. +constexpr unsigned int channels_within_mcp_adc = 8; +extern MCP_ADC adc_0; // MCP3208. ADC0 in VCR schematic. Used for valuable telem data. +extern MCP_ADC adc_1; // MCP3208. ADC1 in VCR schematic. Used for extra thermistors or extra sensors while testing. #endif /* VCR_GLOBALS */ \ No newline at end of file diff --git a/include/VCR_Tasks.h b/include/VCR_Tasks.h index 4f9b3c9..2c1cd89 100644 --- a/include/VCR_Tasks.h +++ b/include/VCR_Tasks.h @@ -39,7 +39,7 @@ */ bool init_test_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); bool run_test_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); -extern const HT_TASK::Task test_task; +extern HT_TASK::Task test_task; /** @@ -48,7 +48,7 @@ extern const HT_TASK::Task test_task; * singleton classes to minimize the need for passing instances around. */ bool run_tick_state_machine_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); -extern const HT_TASK::Task tick_state_machine_task; +extern HT_TASK::Task tick_state_machine_task; @@ -59,7 +59,7 @@ extern const HT_TASK::Task tick_state_machine_task; */ bool init_read_adc0_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); bool run_read_adc0_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); -extern const HT_TASK::Task read_adc0_task; +extern HT_TASK::Task read_adc0_task; @@ -73,7 +73,7 @@ extern const HT_TASK::Task read_adc0_task; */ bool init_read_adc1_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); bool run_read_adc1_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); -extern const HT_TASK::Task read_adc1_task; +extern HT_TASK::Task read_adc1_task; @@ -82,6 +82,6 @@ extern const HT_TASK::Task read_adc1_task; * update function of the buzzer controller. */ bool run_update_buzzer_controller_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); -extern const HT_TASK::Task update_buzzer_controller_task; +extern HT_TASK::Task update_buzzer_controller_task; #endif /* VCR_TASKS */ \ No newline at end of file diff --git a/lib/state_machine/include/VehicleStateMachine.h b/lib/state_machine/include/VehicleStateMachine.h index 287c39e..7157fb2 100644 --- a/lib/state_machine/include/VehicleStateMachine.h +++ b/lib/state_machine/include/VehicleStateMachine.h @@ -19,7 +19,7 @@ * enough time has passed, we enter the next state. * READY_TO_DRIVE - While in this state, pedal inputs command the drivetrain. */ -enum class CAR_STATE //NOLINT +enum class CAR_STATE { STARTUP = 0, TRACTIVE_SYSTEM_NOT_ACTIVE = 1, @@ -36,7 +36,7 @@ enum class CAR_STATE //NOLINT * VehicleStateMachine uses instance data directly from other singleton classes (Buzzer, Pedals, etc) * and from the global structs (VCRInterfaceData_s, VCRSystemData_s, etc.). */ -class VehicleStateMachine //NOLINT +class VehicleStateMachine { public: static VehicleStateMachine& getInstance() diff --git a/lib/state_machine/src/VehicleStateMachine.cpp b/lib/state_machine/src/VehicleStateMachine.cpp index 55c1f36..4d48c2e 100644 --- a/lib/state_machine/src/VehicleStateMachine.cpp +++ b/lib/state_machine/src/VehicleStateMachine.cpp @@ -140,7 +140,6 @@ void VehicleStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_mil void VehicleStateMachine::handle_exit_logic_(CAR_STATE prev_state, unsigned long curr_millis) { - // NOLINTBEGIN switch (get_state()) { case CAR_STATE::STARTUP: @@ -158,12 +157,10 @@ void VehicleStateMachine::handle_exit_logic_(CAR_STATE prev_state, unsigned long default: break; } - // NOLINTEND } void VehicleStateMachine::handle_entry_logic_(CAR_STATE new_state, unsigned long curr_millis) { - // NOLINTBEGIN switch (new_state) { case CAR_STATE::STARTUP: @@ -184,5 +181,4 @@ void VehicleStateMachine::handle_entry_logic_(CAR_STATE new_state, unsigned long default: break; } - // NOLINTEND } diff --git a/lib/systems/include/BuzzerController.h b/lib/systems/include/BuzzerController.h index 88f8a06..157ead4 100644 --- a/lib/systems/include/BuzzerController.h +++ b/lib/systems/include/BuzzerController.h @@ -16,6 +16,8 @@ class BuzzerController static BuzzerController instance; return instance; } + + BuzzerController(BuzzerController&&) = default; /** * Calling this command will activate the buzzer for BUZZER_PERIOD_MS milliseconds. @@ -32,6 +34,7 @@ class BuzzerController { _last_activation_time_ms = 0; } + ~BuzzerController() = default; /** * Returns whether or not the buzzer should currently be active, according to the system. diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 689e4c4..c93d536 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -26,8 +26,8 @@ class DrivetrainSystem void setup_retry() { reset_drivetrain(); - hv_en_requested_ = false; - enable_requested_ = false; + _hv_en_requested = false; + _enable_requested = false; } bool handle_inverter_startup(unsigned long curr_time); @@ -72,38 +72,38 @@ class DrivetrainSystem // dynamic_data_ = {}; // } - std::array inverters_; + std::array _inverters; // MCUInterface *mcu_interface_; - int init_time_limit_ms_; - uint16_t min_hv_voltage_; - int motor_pole_pairs_; - float lambda_magnetic_flux_wb_, L_d_inductance_H_; + int _init_time_limit_ms; + uint16_t _min_hv_voltage; + int _motor_pole_pairs; + float _lambda_magnetic_flux_wb, _L_d_inductance_H; // startup statuses: - bool hv_en_requested_, enable_requested_; + bool _hv_en_requested, _enable_requested; // reset inverters - bool reset_requested_; - unsigned long last_reset_pressed_time_; - unsigned long reset_interval_; + bool _reset_requested; + unsigned long _last_reset_pressed_time; + unsigned long _reset_interval; /// @param curr_time current system tick time (millis()) that sets the init phase start time - void enable_drivetrain_hv_(unsigned long curr_time); - void request_enable_(); + void enable_drivetrain_hv(unsigned long curr_time); + void request_enable(); // startup phase 1 // status check for start of enable - bool drivetrain_ready_(); + bool drivetrain_ready(); // startup phase 2 - bool check_drivetrain_quit_dc_on_(); + bool check_drivetrain_quit_dc_on(); // final check for drivetrain initialization to check if quit inverter on - bool drivetrain_enabled_(); + bool drivetrain_enabled(); - unsigned long curr_system_millis_; - unsigned int min_cmd_period_; - unsigned long last_no_torque_cmd_time_, last_reset_cmd_time_, last_disable_cmd_time_, last_general_cmd_time_; + unsigned long _curr_system_millis; + unsigned int _min_cmd_period; + unsigned long _last_no_torque_cmd_time, last_reset_cmd_time, last_disable_cmd_time, last_general_cmd_time; - unsigned long drivetrain_initialization_phase_start_time_; + unsigned long _drivetrain_initialization_phase_start_time; // DrivetrainCommand_s current_drivetrain_command_; // DrivetrainDynamicReport_s dynamic_data_; - float max_torque_setpoint_nm_; + float _max_torque_setpoint_nm; }; #include "DrivetrainSystem.tpp" diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index 882c626..d96c9c1 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -22,13 +22,13 @@ bool DrivetrainSystem::handle_inverter_startup(unsigned long curr_ } template -void DrivetrainSystem::enable_drivetrain_hv_(unsigned long curr_time) +void DrivetrainSystem::enable_drivetrain_hv(unsigned long curr_time) { } template -void DrivetrainSystem::request_enable_() +void DrivetrainSystem::request_enable() { } @@ -88,19 +88,19 @@ bool DrivetrainSystem::hv_over_threshold_on_drivetrain() return false; } template -bool DrivetrainSystem::drivetrain_ready_() +bool DrivetrainSystem::drivetrain_ready() { return false; } template -bool DrivetrainSystem::check_drivetrain_quit_dc_on_() +bool DrivetrainSystem::check_drivetrain_quit_dc_on() { return false; } template -bool DrivetrainSystem::drivetrain_enabled_() +bool DrivetrainSystem::drivetrain_enabled() { return false; } \ No newline at end of file diff --git a/lib/systems/include/PhysicalParameters.h b/lib/systems/include/PhysicalParameters.h index 9de092f..f0e5c2b 100644 --- a/lib/systems/include/PhysicalParameters.h +++ b/lib/systems/include/PhysicalParameters.h @@ -4,17 +4,17 @@ namespace PhysicalParameters { const float AMK_MAX_RPM = 20000; - const float AMK_MAX_TORQUE = 21.42; - const float MAX_REGEN_TORQUE = 10.0; + const float AMK_MAX_TORQUE = 21.42f; + const float MAX_REGEN_TORQUE = 10.0f; } -constexpr const float GEARBOX_RATIO = 11.83; -constexpr const float WHEEL_DIAMETER = 0.4064; // meters -constexpr const float RPM_TO_METERS_PER_SECOND = WHEEL_DIAMETER * 3.1415 / GEARBOX_RATIO / 60.0; -constexpr const float RPM_TO_KILOMETERS_PER_HOUR = RPM_TO_METERS_PER_SECOND * 3600.0 / 1000.0; -constexpr const float METERS_PER_SECOND_TO_RPM = 1.0 / RPM_TO_METERS_PER_SECOND; +constexpr const float GEARBOX_RATIO = 11.83f; +constexpr const float WHEEL_DIAMETER = 0.4064f; // meters +constexpr const float RPM_TO_METERS_PER_SECOND = WHEEL_DIAMETER * 3.1415f / GEARBOX_RATIO / 60.0f; +constexpr const float RPM_TO_KILOMETERS_PER_HOUR = RPM_TO_METERS_PER_SECOND * 3600.0f / 1000.0f; +constexpr const float METERS_PER_SECOND_TO_RPM = 1.0f / RPM_TO_METERS_PER_SECOND; -const float RPM_TO_RAD_PER_SECOND = 2 * 3.1415 / 60.0; +const float RPM_TO_RAD_PER_SECOND = 2 * 3.1415f / 60.0f; const float RAD_PER_SECOND_TO_RPM = 1 / RPM_TO_RAD_PER_SECOND; #endif /* PHYSICALPARAMETERS */ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMux.hpp b/lib/systems/include/TorqueControllerMux.hpp index 8dfdb7e..b94c9f5 100644 --- a/lib/systems/include/TorqueControllerMux.hpp +++ b/lib/systems/include/TorqueControllerMux.hpp @@ -86,14 +86,14 @@ class TorqueControllerMux float max_power_limit = TC_MUX_DEFAULT_PARAMS::MAX_POWER_LIMIT, size_t num_motors = 4) : _controller_evals(controller_evals), _mux_bypass_limits(mux_bypass_limits), - max_change_speed_(max_change_speed), - max_torque_pos_change_delta_(max_torque_pos_change_delta), - max_power_limit_(max_power_limit), + _max_change_speed(max_change_speed), + _max_torque_pos_change_delta(max_torque_pos_change_delta), + _max_power_limit(max_power_limit), _num_motors(num_motors) {} - const TorqueControllerMuxStatus_s &get_tc_mux_status() { return active_status_; } + const TorqueControllerMuxStatus_s &get_tc_mux_status() { return _active_status; } /// @brief function that evaluates the mux, controllers and gets the active command /// @param requested_controller_type the requested controller type from the dial state @@ -110,41 +110,41 @@ class TorqueControllerMux std::array _mux_bypass_limits; - std::unordered_map torque_limit_map_ = { + std::unordered_map _torque_limit_map = { {TorqueLimit_e::TCMUX_FULL_TORQUE, PhysicalParameters::AMK_MAX_TORQUE}, {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0f}, {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0f}}; size_t _num_motors; float _max_change_speed, _max_torque_pos_change_delta, _max_power_limit; DrivetrainCommand_s _prev_command = {}; - TorqueControllerMuxStatus_s active_status_ = {}; - TorqueControllerMuxError_e can_switch_controller_(DrivetrainDynamicReport_s active_drivetrain_data, + TorqueControllerMuxStatus_s _active_status = {}; + TorqueControllerMuxError_e can_switch_controller(DrivetrainDynamicReport_s active_drivetrain_data, DrivetrainCommand_s previous_controller_command, DrivetrainCommand_s desired_controller_out); /// @brief Clamps negative rpms to 0f /// @param const DrivetrainCommand_s &command provides the rpm info as a DrivetrainCommand_s /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method - DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); + DrivetrainCommand_s apply_positive_speed_limit(const DrivetrainCommand_s &command); /// @brief Ensure torque is at most at the specified limit. If exceeding, then limit it in the returned DrivetrainCommand_s /// @param const DrivetrainCommand_s &command is a DrivetrainCommand_s, which provides torque info /// @param float max_torque this is the maximum average torque the wheels are allowed to experience before it is limited. /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method - DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); + DrivetrainCommand_s apply_torque_limit(const DrivetrainCommand_s &command, float max_torque); /// @brief Apply power limit (watts) such that the mechanical power of all wheels never exceeds the preset mechanical power limit. Scales all wheels down to preserve functionality of torque controllers /// @param const DrivetrainCommand_s &command provides torque info, which is used to calculate mechanical power /// @param const DrivetrainDynamicReport_s &drivetrain provides RPMS, which are used to calculate radians / s /// @param float max_torque is used to indirectly specifiy the max power /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method - DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); + DrivetrainCommand_s apply_power_limit(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); /// @brief begin limiting regen at noRegenLimitKPH (hardcoded in func) and completely limit regen at fullRegenLimitKPH (hardcoded in func) /// @param const DrivetrainCommand_s &command /// @param const DrivetrainDynamicReport_s &drivetrain_data provides RPMs /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method - DrivetrainCommand_s apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); + DrivetrainCommand_s apply_regen_limit(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); }; // } diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index bb78a3a..dbdc4a1 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -16,57 +16,57 @@ DrivetrainCommand_s TorqueControllerMux::get_drivetrain_command .ready = true}; int req_controller_mode_index = static_cast(requested_controller_type); - int active_controller_mode_index = static_cast(active_status_.active_controller_mode); + int active_controller_mode_index = static_cast(_active_status.active_controller_mode); if ((std::size_t)req_controller_mode_index > ( _controller_evals.size() - 1 )) { - active_status_.active_error = TorqueControllerMuxError_e::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS; + _active_status.active_error = TorqueControllerMuxError_e::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS; return empty_command; } if( (!_controller_evals[active_controller_mode_index]) || (!_controller_evals[req_controller_mode_index])) { - active_status_.active_error = TorqueControllerMuxError_e::ERROR_CONTROLLER_NULL_POINTER; + _active_status.active_error = TorqueControllerMuxError_e::ERROR_CONTROLLER_NULL_POINTER; return empty_command; } current_output = _controller_evals[active_controller_mode_index](input_state); // std::cout << "output torques " << current_output.command.inverter_torque_limit[0] << " " << current_output.command.inverter_torque_limit[1] << " " << current_output.command.inverter_torque_limit[2] << " " << current_output.command.inverter_torque_limit[3] << std::endl; - bool requesting_controller_change = requested_controller_type != active_status_.active_controller_mode; + bool requesting_controller_change = requested_controller_type != _active_status.active_controller_mode; if (requesting_controller_change) { TorqueControllerOutput_s proposed_output = _controller_evals[req_controller_mode_index](input_state); - TorqueControllerMuxError_e error_state = can_switch_controller_(input_state.drivetrain_data, current_output.command, proposed_output.command); + TorqueControllerMuxError_e error_state = can_switch_controller(input_state.drivetrain_data, current_output.command, proposed_output.command); if (error_state == TorqueControllerMuxError_e::NO_ERROR) { - active_status_.active_controller_mode = requested_controller_type; + _active_status.active_controller_mode = requested_controller_type; active_controller_mode_index = req_controller_mode_index; current_output = proposed_output; } - active_status_.active_error = error_state; + _active_status.active_error = error_state; } if (!_mux_bypass_limits[active_controller_mode_index]) { - active_status_.active_torque_limit_enum = requested_torque_limit; - current_output.command = apply_regen_limit_(current_output.command, input_state.drivetrain_data); + _active_status.active_torque_limit_enum = requested_torque_limit; + current_output.command = apply_regen_limit(current_output.command, input_state.drivetrain_data); // std::cout << "output torques before " << current_output.command.inverter_torque_limit[0] << " " << current_output.command.inverter_torque_limit[1] << " " << current_output.command.inverter_torque_limit[2] << " " << current_output.command.inverter_torque_limit[3] << std::endl; - current_output.command = apply_torque_limit_(current_output.command, torque_limit_map_[requested_torque_limit]); + current_output.command = apply_torque_limit(current_output.command, _torque_limit_map[requested_torque_limit]); // std::cout << "output torques after " << current_output.command.inverter_torque_limit[0] << " " <::get_drivetrain_command } template -TorqueControllerMuxError_e TorqueControllerMux::can_switch_controller_(DrivetrainDynamicReport_s active_drivetrain_data, +TorqueControllerMuxError_e TorqueControllerMux::can_switch_controller(DrivetrainDynamicReport_s active_drivetrain_data, DrivetrainCommand_s previous_controller_command, DrivetrainCommand_s desired_controller_out) { @@ -83,9 +83,9 @@ TorqueControllerMuxError_e TorqueControllerMux::can_switch_cont bool torqueDeltaPreventsModeChange = false; for (int i = 0; i < _num_motors; i++) { - speedPreventsModeChange = (abs(active_drivetrain_data.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND) >= max_change_speed_); + speedPreventsModeChange = (abs(active_drivetrain_data.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND) >= _max_change_speed); // only if the torque delta is positive do we not want to switch to the new one - torqueDeltaPreventsModeChange = (desired_controller_out.inverter_torque_limit[i] - previous_controller_command.inverter_torque_limit[i]) > max_torque_pos_change_delta_; + torqueDeltaPreventsModeChange = (desired_controller_out.inverter_torque_limit[i] - previous_controller_command.inverter_torque_limit[i]) > _max_torque_pos_change_delta; if (speedPreventsModeChange) { return TorqueControllerMuxError_e::ERROR_SPEED_DIFF_TOO_HIGH; @@ -100,7 +100,7 @@ TorqueControllerMuxError_e TorqueControllerMux::can_switch_cont /* Apply limit such that wheelspeed never goes negative */ template -DrivetrainCommand_s TorqueControllerMux::apply_positive_speed_limit_(const DrivetrainCommand_s &command) +DrivetrainCommand_s TorqueControllerMux::apply_positive_speed_limit(const DrivetrainCommand_s &command) { DrivetrainCommand_s out; out = command; @@ -112,7 +112,7 @@ DrivetrainCommand_s TorqueControllerMux::apply_positive_speed_l } template -DrivetrainCommand_s TorqueControllerMux::apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque) +DrivetrainCommand_s TorqueControllerMux::apply_torque_limit(const DrivetrainCommand_s &command, float max_torque) { DrivetrainCommand_s out = command; float avg_torque = 0; @@ -144,7 +144,7 @@ DrivetrainCommand_s TorqueControllerMux::apply_torque_limit_(co preserve functionality of torque controllers */ template -DrivetrainCommand_s TorqueControllerMux::apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque) +DrivetrainCommand_s TorqueControllerMux::apply_power_limit(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque) { DrivetrainCommand_s out = command; float net_torque_mag = 0; @@ -185,7 +185,7 @@ DrivetrainCommand_s TorqueControllerMux::apply_power_limit_(con } template -DrivetrainCommand_s TorqueControllerMux::apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data) +DrivetrainCommand_s TorqueControllerMux::apply_regen_limit(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data) { DrivetrainCommand_s out = command; const float noRegenLimitKPH = 10.0; diff --git a/src/VCR_Globals.cpp b/src/VCR_Globals.cpp index 8b90da3..d975325 100644 --- a/src/VCR_Globals.cpp +++ b/src/VCR_Globals.cpp @@ -7,14 +7,10 @@ /* From shared-firmware-types */ #include "SharedFirmwareTypes.h" -//NOLINTBEGIN - /* Interface and system data structs */ VCRInterfaceData_s interface_data; VCRSystemData_s system_data; /* ADC setup */ -MCP_ADC<8> adc_0 = MCP_ADC<8>(ADC0_CS); // MCP3208. ADC0 in VCR schematic. Used for valuable telem data. -MCP_ADC<8> adc_1 = MCP_ADC<8>(ADC1_CS); // MCP3208. ADC1 in VCR schematic. Used for extra thermistors or extra sensors while testing. - -//NOLINTEND \ No newline at end of file +MCP_ADC<8> adc_0 = MCP_ADC<8>(ADC0_CS); // MCP3208. ADC0 in VCR schematic. Used for valuable telem data. //NOLINT +MCP_ADC<8> adc_1 = MCP_ADC<8>(ADC1_CS); // MCP3208. ADC1 in VCR schematic. Used for extra thermistors or extra sensors while testing. //NOLINT \ No newline at end of file diff --git a/src/VCR_Tasks.cpp b/src/VCR_Tasks.cpp index fa16901..0a37506 100644 --- a/src/VCR_Tasks.cpp +++ b/src/VCR_Tasks.cpp @@ -68,7 +68,7 @@ HT_TASK::Task read_adc0_task = HT_TASK::Task(init_read_adc0_task, run_read_adc0_ bool run_tick_state_machine_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { - VehicleStateMachine::getInstance().tick_state_machine(sysMicros / 1000, system_data); // tick function requires millis + VehicleStateMachine::getInstance().tick_state_machine(sysMicros / 1000, system_data); // tick function requires millis //NOLINT return true; } @@ -111,7 +111,7 @@ HT_TASK::Task read_adc1_task = HT_TASK::Task(init_read_adc1_task, run_read_adc1_ bool run_update_buzzer_controller_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { - system_data.buzzer_is_active = BuzzerController::getInstance().buzzer_is_active(sysMicros / 1000); + system_data.buzzer_is_active = BuzzerController::getInstance().buzzer_is_active(sysMicros / 1000); //NOLINT return true; } From e4045dd74fd9b858ba94db6a783f3428b91e4a3f Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 6 Jan 2025 14:29:14 -0500 Subject: [PATCH 10/36] (feat) changed some more linting rules --- .clang-tidy | 2 +- include/VCR_Tasks.h | 2 +- src/VCR_Tasks.cpp | 9 +++++---- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index a895436..551f468 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,4 +1,4 @@ -Checks: 'bugprone-*, cppcoreguidelines-*, readability-identifier-naming, -cppcoreguidelines-avoid-non-const-global-variables, -bugprone-branch-clone, -cppcoreguidelines-pro-type-vararg, -bugprone-easily-swappable-parameters' +Checks: 'bugprone-*, cppcoreguidelines-*, readability-identifier-naming, -cppcoreguidelines-avoid-non-const-global-variables, -bugprone-branch-clone, -bugprone-easily-swappable-parameters' WarningsAsErrors: 'cppcoreguidelines-*, bugprone-*, readability-identifier-naming' FormatStyle: 'file' CheckOptions: diff --git a/include/VCR_Tasks.h b/include/VCR_Tasks.h index 2c1cd89..ca28781 100644 --- a/include/VCR_Tasks.h +++ b/include/VCR_Tasks.h @@ -51,7 +51,7 @@ bool run_tick_state_machine_task(const unsigned long& sysMicros, const HT_TASK:: extern HT_TASK::Task tick_state_machine_task; - + /** * The read_adc0 task will command adc0 to sample all eight channels, convert the outputs, and * store them in structs defined in shared_firmware_types. This function relies on adc_0 being diff --git a/src/VCR_Tasks.cpp b/src/VCR_Tasks.cpp index 0a37506..79211a6 100644 --- a/src/VCR_Tasks.cpp +++ b/src/VCR_Tasks.cpp @@ -15,13 +15,14 @@ bool init_test_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { - hal_printf("Initialized function at %d (micros)\n", sysMicros); + hal_printf("Initialized function at %d (micros)\n", sysMicros); // NOLINT return true; } bool run_test_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { - hal_printf("Ran function at %d (micros) for the %dth time\n", sysMicros, taskInfo.executions); + + hal_printf("Ran function at %d (micros) for the %dth time\n", sysMicros, taskInfo.executions); // NOLINT return true; } @@ -40,7 +41,7 @@ bool init_read_adc0_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo adc_0.setChannelScaleAndOffset(RL_SUS_POT_CHANNEL, RL_SUS_POT_SCALE, RL_SUS_POT_OFFSET); adc_0.setChannelScaleAndOffset(RR_SUS_POT_CHANNEL, RR_SUS_POT_SCALE, RR_SUS_POT_OFFSET); - hal_printf("Initialized ADC0 at %d (micros)\n", sysMicros); + hal_printf("Initialized ADC0 at %d (micros)\n", sysMicros); // NOLINT return true; } @@ -90,7 +91,7 @@ bool init_read_adc1_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo adc_1.setChannelScaleAndOffset(7, 1, 0); /* NOLINTEND */ - hal_printf("Initialized ADC0 at %d (micros)\n", sysMicros); + hal_printf("Initialized ADC0 at %d (micros)\n", sysMicros); // NOLINT return true; } From 5e6453e046e4d26cb724c6ee5332332e9c29337e Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 8 Jan 2025 23:02:18 -0500 Subject: [PATCH 11/36] adding in v2 for now --- lib/systems/include/DrivetrainSystem.h | 16 ++- lib/systems/include/DrivetrainSystem.tpp | 2 +- lib/systems/include/DrivetrainSystemv2.h | 123 +++++++++++++++++++++++ lib/systems/src/DrivetrainSystemv2.cpp | 12 +++ platformio.ini | 1 + 5 files changed, 148 insertions(+), 6 deletions(-) create mode 100644 lib/systems/include/DrivetrainSystemv2.h create mode 100644 lib/systems/src/DrivetrainSystemv2.cpp diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index c93d536..d399e22 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -6,6 +6,16 @@ #include "stdint.h" #include "SysClock.h" +enum class DRIVETRAIN_STATE +{ + TRACTIVE_SYSTEM_NOT_ACTIVE = 0, + TRACTIVE_SYSTEM_ACTIVE = 1, + ENABLING_INVERTERS = 2, + ENABLED = 3, + ERROR = 4 +}; + +// #include "DrivetrainSystemStateMachine.h" /** * As of now, only the minimum functions for VehicleStateMachine to compile have been implemented. * TODO: Re-add the rest of the necessary functions @@ -15,11 +25,7 @@ template class DrivetrainSystem { public: - static DrivetrainSystem& getInstance() - { - static DrivetrainSystem instance; - return instance; - } + DrivetrainSystem(); void tick(const SysTick_s &tick); diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index d96c9c1..ffa26cb 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -3,7 +3,7 @@ template void DrivetrainSystem::tick(const SysTick_s &tick) { - + } template diff --git a/lib/systems/include/DrivetrainSystemv2.h b/lib/systems/include/DrivetrainSystemv2.h new file mode 100644 index 0000000..ef2fe50 --- /dev/null +++ b/lib/systems/include/DrivetrainSystemv2.h @@ -0,0 +1,123 @@ +#ifndef DRIVETRAINSYSTEM +#define DRIVETRAINSYSTEM + +#include +#include +#include "stdint.h" + +#include "SharedFirmwareTypes.h" +#include "SysClock.h" + +enum class drivetrain_state +{ + NOT_ENABLED_NO_HV_PRESENT = 0, + NOT_ENABLED_HV_PRESENT = 1, + ENABLING_INVERTERS_SPEED_MODE = 2, + ENABLING_INVERTERS_TORQUE_MODE = 3, + ENABLED_SPEED_MODE = 4, + ENABLED_TORQUE_MODE = 5, + ERROR = 6 +}; + +// #include "DrivetrainSystemStateMachine.h" +/** + * As of now, only the minimum functions for VehicleStateMachine to compile have been implemented. + * TODO: Re-add the rest of the necessary functions + * TODO: Add DrivetrainSystem.tpp to implement all functions + */ + +struct DrivetrainSpeedCommand_s +{ + veh_vec desired_speed_rpm; + veh_vec torque_limit_nm; +}; + +struct DrivetrainTorqueCommand_s +{ + veh_vec desired_torque_nm; +}; + +struct DrivetrainInit_s +{ + bool init_drivetrain; +}; + +struct InverterStatus_s +{ + float dc_bus_voltage; + float torque_nm; + float speed_rpm; + float mech_power_w; + float inverter_temp_c; + float motor_temp_c; + float igbt_temp_c; + uint16_t error_status_id; + bool inverter_ready : 1; + bool quit_dc : 1; + bool quit_inverter : 1; + bool error_present : 1; +}; + +class DrivetrainSystem +{ +public: + DrivetrainSystem() =delete; + DrivetrainSystem(veh_vec inverter_interfaces); + + void command_drivetrain(etl::variant cmd); + // DrivetrainDynamicReport_s get_dynamic_data(); + + struct InverterFuncts { + std::function _set_speed; + std::function _set_torque; + std::function _set_disable_inverter; + std::function _set_no_torque; + std::function _reset_inverter; + std::function _set_enable_inverter; + std::function _set_enable_hv; + std::function _get_status; + }; + + // struct DrivetrainInterfaceState_s + // { + + // }; +private: + veh_vec _inverter_interfaces; + // std::function<>; + + // MCUInterface *mcu_interface_; + int _init_time_limit_ms; + uint16_t _min_hv_voltage; + int _motor_pole_pairs; + float _lambda_magnetic_flux_wb, _L_d_inductance_H; + // startup statuses: + bool _hv_en_requested, _enable_requested; + // reset inverters + bool _reset_requested; + unsigned long _last_reset_pressed_time; + unsigned long _reset_interval; + /// @param curr_time current system tick time (millis()) that sets the init phase start time + void enable_drivetrain_hv(unsigned long curr_time); + void request_enable(); + // startup phase 1 + // status check for start of enable + bool drivetrain_ready(); + // startup phase 2 + bool check_drivetrain_quit_dc_on(); + + // final check for drivetrain initialization to check if quit inverter on + bool drivetrain_enabled(); + + unsigned long _curr_system_millis; + unsigned int _min_cmd_period; + unsigned long _last_no_torque_cmd_time, last_reset_cmd_time, last_disable_cmd_time, last_general_cmd_time; + + unsigned long _drivetrain_initialization_phase_start_time; + // DrivetrainCommand_s current_drivetrain_command_; + // DrivetrainDynamicReport_s dynamic_data_; + float _max_torque_setpoint_nm; +}; + +#include "DrivetrainSystem.tpp" +#endif /* DRIVETRAINSYSTEM */ \ No newline at end of file diff --git a/lib/systems/src/DrivetrainSystemv2.cpp b/lib/systems/src/DrivetrainSystemv2.cpp new file mode 100644 index 0000000..50b2df8 --- /dev/null +++ b/lib/systems/src/DrivetrainSystemv2.cpp @@ -0,0 +1,12 @@ +#include + +// DrivetrainSystem::DrivetrainSystem() +// { + +// }; +DrivetrainSystem::DrivetrainSystem( + veh_vec inverter_interfaces) + : _inverter_interfaces(inverter_interfaces) {}; + + +DrivetrainStatus_s DrivetrainSystem::evaluate_drivetrain(etl::variant cmd) {} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 7d90741..704ba8e 100644 --- a/platformio.ini +++ b/platformio.ini @@ -11,6 +11,7 @@ lib_deps_shared = https://github.com/hytech-racing/shared_firmware_types.git#HT09-shared-datatypes https://github.com/ssilverman/QNEthernet#v0.26.0 https://github.com/hytech-racing/HT_SCHED + Embedded Template Library@^20.39.4 ; Teensy41 Environment. This environment is the primary environment for uploading code to the car. ; * Build to verify the on-car software. From 82b7724a7c4bb4ac6a2aca743dde23edb787ffa7 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 10 Jan 2025 21:35:59 -0500 Subject: [PATCH 12/36] progress on drivetrain system --- .../include/VehicleStateMachine.h | 4 +- lib/systems/include/DrivetrainSystem.h | 146 +++++++++++------- lib/systems/include/DrivetrainSystem_old.h | 120 ++++++++++++++ ...ainSystem.tpp => DrivetrainSystem_old.tpp} | 2 +- lib/systems/include/DrivetrainSystemv2.h | 123 --------------- lib/systems/src/DrivetrainSystem.cpp | 125 +++++++++++++++ lib/systems/src/DrivetrainSystemv2.cpp | 12 -- platformio.ini | 3 +- 8 files changed, 341 insertions(+), 194 deletions(-) create mode 100644 lib/systems/include/DrivetrainSystem_old.h rename lib/systems/include/{DrivetrainSystem.tpp => DrivetrainSystem_old.tpp} (98%) delete mode 100644 lib/systems/include/DrivetrainSystemv2.h create mode 100644 lib/systems/src/DrivetrainSystem.cpp delete mode 100644 lib/systems/src/DrivetrainSystemv2.cpp diff --git a/lib/state_machine/include/VehicleStateMachine.h b/lib/state_machine/include/VehicleStateMachine.h index 7157fb2..b57d9f0 100644 --- a/lib/state_machine/include/VehicleStateMachine.h +++ b/lib/state_machine/include/VehicleStateMachine.h @@ -58,9 +58,9 @@ class VehicleStateMachine CAR_STATE get_state() { return _current_state; } private: - VehicleStateMachine() : + VehicleStateMachine(DrivetrainSystem & drivetrain_system) : _current_state(CAR_STATE::STARTUP), - _drivetrain(DrivetrainSystem::getInstance()), + _drivetrain(drivetrain_system), _buzzer(BuzzerController::getInstance()), _safetysystem(SafetySystem::getInstance()) {}; diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index d399e22..f96e221 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -1,18 +1,32 @@ #ifndef DRIVETRAINSYSTEM #define DRIVETRAINSYSTEM -#include "SharedFirmwareTypes.h" +#include "etl/variant.h" + #include +#include #include "stdint.h" + +#include "SharedFirmwareTypes.h" #include "SysClock.h" -enum class DRIVETRAIN_STATE +enum class DrivetrainState_e { - TRACTIVE_SYSTEM_NOT_ACTIVE = 0, - TRACTIVE_SYSTEM_ACTIVE = 1, - ENABLING_INVERTERS = 2, - ENABLED = 3, - ERROR = 4 + NOT_CONNECTED = 0, + NOT_ENABLED_NO_HV_PRESENT = 1, + NOT_ENABLED_HV_PRESENT = 2, + ENABLING_INVERTERS_SPEED_MODE = 3, + ENABLING_INVERTERS_TORQUE_MODE = 4, + ENABLED_SPEED_MODE = 5, + ENABLED_TORQUE_MODE = 6, + ERROR = 7 +}; + +enum class DrivetrainCmdResponse_e +{ + COMMAND_OK = 0, + CANNOT_INIT_NOT_CONNECTED = 1, + COMMAND_INVALID = 2 }; // #include "DrivetrainSystemStateMachine.h" @@ -21,64 +35,87 @@ enum class DRIVETRAIN_STATE * TODO: Re-add the rest of the necessary functions * TODO: Add DrivetrainSystem.tpp to implement all functions */ -template -class DrivetrainSystem + +struct DrivetrainSpeedCommand_s { -public: - DrivetrainSystem(); - - void tick(const SysTick_s &tick); + veh_vec desired_speed_rpm; + veh_vec torque_limit_nm; +}; - void setup_retry() - { - reset_drivetrain(); - _hv_en_requested = false; - _enable_requested = false; - } +struct DrivetrainTorqueCommand_s +{ + veh_vec desired_torque_nm; +}; - bool handle_inverter_startup(unsigned long curr_time); +struct DrivetrainInit_s +{ + bool init_drivetrain; +}; - // on entry logic - void command_drivetrain_no_torque(); - void command_drivetrain_debug(); - // check to see if init time limit has passed - bool inverter_init_timeout(unsigned long curr_time); - bool hv_over_threshold_on_drivetrain(); - void disable(); - void disable_no_pins(); - bool drivetrain_error_occured(); - void reset_drivetrain(); - // void command_drivetrain(const DrivetrainCommand_s &data); +struct InverterStatus_s +{ + float dc_bus_voltage; + float torque_nm; + float speed_rpm; + float mech_power_w; + float inverter_temp_c; + float motor_temp_c; + float igbt_temp_c; + uint16_t error_status_id; + bool inverter_ready : 1; + bool quit_dc : 1; + bool quit_inverter : 1; + bool error_present : 1; + bool connected : 1; + bool hv_present : 1; +}; - void enable_drivetrain_reset(); - void check_reset_condition(); +struct DrivetrainStatus_s +{ + bool all_inverters_connected; + veh_vec inverter_statuses; + DrivetrainCmdResponse_e cmd_resp; +}; +class DrivetrainSystem +{ +public: + using CmdVariant = etl::variant; + DrivetrainSystem() = delete; + + DrivetrainStatus_s evaluate_drivetrain(CmdVariant cmd); + DrivetrainState_e get_state(); // DrivetrainDynamicReport_s get_dynamic_data(); -private: - // DrivetrainSystem(const std::array &inverters, MCUInterface *mcu_interface, int init_time_limit_ms, uint16_t min_hv_voltage = 60, int min_cmd_period_ms = 1, float max_torque_setpoint_nm = 21.42) - // : inverters_(inverters), init_time_limit_ms_(init_time_limit_ms), min_hv_voltage_(min_hv_voltage), min_cmd_period_(min_cmd_period_ms), max_torque_setpoint_nm_(max_torque_setpoint_nm) + struct InverterFuncts { + std::function set_speed; + std::function set_torque; + std::function set_disable_inverter; + std::function set_no_torque; + std::function reset_inverter; + std::function set_enable_inverter; + std::function set_enable_hv; + std::function get_status; + }; + + // struct DrivetrainInterfaceState_s // { - // // values from: https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/fse/motor_data_sheet_a2370dd_dd5.pdf - // motor_pole_pairs_ = 5; - // lambda_magnetic_flux_wb_ = 1.0; - // hv_en_requested_ = false; - // enable_requested_ = false; - // reset_requested_ = false; - // last_reset_pressed_time_ = 0; - // reset_interval_ = 5000; // ms - // curr_system_millis_ = 0; - // last_no_torque_cmd_time_ = 0; - // last_reset_cmd_time_ = 0; - // last_disable_cmd_time_ = 0; - // last_general_cmd_time_ = 0; // ms - // mcu_interface_ = mcu_interface; - // dynamic_data_ = {}; - // } - - std::array _inverters; + + // }; +private: + bool _check_inverter_flags(std::function flag_check_func); +private: + DrivetrainState_e _state; + veh_vec _inverter_interfaces; + std::function _check_inverter_ready_flag; + std::function _check_inverter_connected_flag; + std::function _check_inverter_quit_dc_flag; + std::function _check_inverter_error_flag; + std::function _check_inverter_hv_present_flag; + std::function _check_inverter_hv_not_present_flag; + // MCUInterface *mcu_interface_; int _init_time_limit_ms; uint16_t _min_hv_voltage; @@ -112,5 +149,4 @@ class DrivetrainSystem float _max_torque_setpoint_nm; }; -#include "DrivetrainSystem.tpp" #endif /* DRIVETRAINSYSTEM */ \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem_old.h b/lib/systems/include/DrivetrainSystem_old.h new file mode 100644 index 0000000..93aad9f --- /dev/null +++ b/lib/systems/include/DrivetrainSystem_old.h @@ -0,0 +1,120 @@ +#ifndef DRIVETRAINSYSTEM +#define DRIVETRAINSYSTEM + +#include "etl/singleton.h" + +#include "SharedFirmwareTypes.h" +#include +#include "stdint.h" +#include "SysClock.h" + +enum class DRIVETRAIN_STATE +{ + TRACTIVE_SYSTEM_NOT_ACTIVE = 0, + TRACTIVE_SYSTEM_ACTIVE = 1, + ENABLING_INVERTERS = 2, + ENABLED = 3, + ERROR = 4 +}; + +// #include "DrivetrainSystemStateMachine.h" +/** + * As of now, only the minimum functions for VehicleStateMachine to compile have been implemented. + * TODO: Re-add the rest of the necessary functions + * TODO: Add DrivetrainSystem.tpp to implement all functions + */ +template +class DrivetrainSystem +{ +public: + DrivetrainSystem(); + + void tick(const SysTick_s &tick); + + void setup_retry() + { + reset_drivetrain(); + _hv_en_requested = false; + _enable_requested = false; + } + + bool handle_inverter_startup(unsigned long curr_time); + + // on entry logic + void command_drivetrain_no_torque(); + void command_drivetrain_debug(); + + // check to see if init time limit has passed + bool inverter_init_timeout(unsigned long curr_time); + + bool hv_over_threshold_on_drivetrain(); + void disable(); + void disable_no_pins(); + bool drivetrain_error_occured(); + void reset_drivetrain(); + // void command_drivetrain(const DrivetrainCommand_s &data); + + void enable_drivetrain_reset(); + void check_reset_condition(); + + // DrivetrainDynamicReport_s get_dynamic_data(); + +private: + // DrivetrainSystem(const std::array &inverters, MCUInterface *mcu_interface, int init_time_limit_ms, uint16_t min_hv_voltage = 60, int min_cmd_period_ms = 1, float max_torque_setpoint_nm = 21.42) + // : inverters_(inverters), init_time_limit_ms_(init_time_limit_ms), min_hv_voltage_(min_hv_voltage), min_cmd_period_(min_cmd_period_ms), max_torque_setpoint_nm_(max_torque_setpoint_nm) + // { + // // values from: https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/fse/motor_data_sheet_a2370dd_dd5.pdf + // motor_pole_pairs_ = 5; + // lambda_magnetic_flux_wb_ = 1.0; + // hv_en_requested_ = false; + // enable_requested_ = false; + // reset_requested_ = false; + // last_reset_pressed_time_ = 0; + // reset_interval_ = 5000; // ms + // curr_system_millis_ = 0; + // last_no_torque_cmd_time_ = 0; + // last_reset_cmd_time_ = 0; + // last_disable_cmd_time_ = 0; + // last_general_cmd_time_ = 0; // ms + // mcu_interface_ = mcu_interface; + // dynamic_data_ = {}; + // } + + std::array _inverters; + // MCUInterface *mcu_interface_; + int _init_time_limit_ms; + uint16_t _min_hv_voltage; + int _motor_pole_pairs; + float _lambda_magnetic_flux_wb, _L_d_inductance_H; + // startup statuses: + bool _hv_en_requested, _enable_requested; + // reset inverters + bool _reset_requested; + unsigned long _last_reset_pressed_time; + unsigned long _reset_interval; + /// @param curr_time current system tick time (millis()) that sets the init phase start time + void enable_drivetrain_hv(unsigned long curr_time); + void request_enable(); + // startup phase 1 + // status check for start of enable + bool drivetrain_ready(); + // startup phase 2 + bool check_drivetrain_quit_dc_on(); + + // final check for drivetrain initialization to check if quit inverter on + bool drivetrain_enabled(); + + unsigned long _curr_system_millis; + unsigned int _min_cmd_period; + unsigned long _last_no_torque_cmd_time, last_reset_cmd_time, last_disable_cmd_time, last_general_cmd_time; + + unsigned long _drivetrain_initialization_phase_start_time; + // DrivetrainCommand_s current_drivetrain_command_; + // DrivetrainDynamicReport_s dynamic_data_; + float _max_torque_setpoint_nm; +}; + +#include "DrivetrainSystem_old.tpp" + +// using DrivetrainSystemSingleton<> = etl::singleton>; +#endif /* DRIVETRAINSYSTEM */ \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem_old.tpp similarity index 98% rename from lib/systems/include/DrivetrainSystem.tpp rename to lib/systems/include/DrivetrainSystem_old.tpp index ffa26cb..fabf4ae 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem_old.tpp @@ -1,4 +1,4 @@ -#include "DrivetrainSystem.h" +#include "DrivetrainSystem_old.h" template void DrivetrainSystem::tick(const SysTick_s &tick) diff --git a/lib/systems/include/DrivetrainSystemv2.h b/lib/systems/include/DrivetrainSystemv2.h deleted file mode 100644 index ef2fe50..0000000 --- a/lib/systems/include/DrivetrainSystemv2.h +++ /dev/null @@ -1,123 +0,0 @@ -#ifndef DRIVETRAINSYSTEM -#define DRIVETRAINSYSTEM - -#include -#include -#include "stdint.h" - -#include "SharedFirmwareTypes.h" -#include "SysClock.h" - -enum class drivetrain_state -{ - NOT_ENABLED_NO_HV_PRESENT = 0, - NOT_ENABLED_HV_PRESENT = 1, - ENABLING_INVERTERS_SPEED_MODE = 2, - ENABLING_INVERTERS_TORQUE_MODE = 3, - ENABLED_SPEED_MODE = 4, - ENABLED_TORQUE_MODE = 5, - ERROR = 6 -}; - -// #include "DrivetrainSystemStateMachine.h" -/** - * As of now, only the minimum functions for VehicleStateMachine to compile have been implemented. - * TODO: Re-add the rest of the necessary functions - * TODO: Add DrivetrainSystem.tpp to implement all functions - */ - -struct DrivetrainSpeedCommand_s -{ - veh_vec desired_speed_rpm; - veh_vec torque_limit_nm; -}; - -struct DrivetrainTorqueCommand_s -{ - veh_vec desired_torque_nm; -}; - -struct DrivetrainInit_s -{ - bool init_drivetrain; -}; - -struct InverterStatus_s -{ - float dc_bus_voltage; - float torque_nm; - float speed_rpm; - float mech_power_w; - float inverter_temp_c; - float motor_temp_c; - float igbt_temp_c; - uint16_t error_status_id; - bool inverter_ready : 1; - bool quit_dc : 1; - bool quit_inverter : 1; - bool error_present : 1; -}; - -class DrivetrainSystem -{ -public: - DrivetrainSystem() =delete; - DrivetrainSystem(veh_vec inverter_interfaces); - - void command_drivetrain(etl::variant cmd); - // DrivetrainDynamicReport_s get_dynamic_data(); - - struct InverterFuncts { - std::function _set_speed; - std::function _set_torque; - std::function _set_disable_inverter; - std::function _set_no_torque; - std::function _reset_inverter; - std::function _set_enable_inverter; - std::function _set_enable_hv; - std::function _get_status; - }; - - // struct DrivetrainInterfaceState_s - // { - - // }; -private: - veh_vec _inverter_interfaces; - // std::function<>; - - // MCUInterface *mcu_interface_; - int _init_time_limit_ms; - uint16_t _min_hv_voltage; - int _motor_pole_pairs; - float _lambda_magnetic_flux_wb, _L_d_inductance_H; - // startup statuses: - bool _hv_en_requested, _enable_requested; - // reset inverters - bool _reset_requested; - unsigned long _last_reset_pressed_time; - unsigned long _reset_interval; - /// @param curr_time current system tick time (millis()) that sets the init phase start time - void enable_drivetrain_hv(unsigned long curr_time); - void request_enable(); - // startup phase 1 - // status check for start of enable - bool drivetrain_ready(); - // startup phase 2 - bool check_drivetrain_quit_dc_on(); - - // final check for drivetrain initialization to check if quit inverter on - bool drivetrain_enabled(); - - unsigned long _curr_system_millis; - unsigned int _min_cmd_period; - unsigned long _last_no_torque_cmd_time, last_reset_cmd_time, last_disable_cmd_time, last_general_cmd_time; - - unsigned long _drivetrain_initialization_phase_start_time; - // DrivetrainCommand_s current_drivetrain_command_; - // DrivetrainDynamicReport_s dynamic_data_; - float _max_torque_setpoint_nm; -}; - -#include "DrivetrainSystem.tpp" -#endif /* DRIVETRAINSYSTEM */ \ No newline at end of file diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp new file mode 100644 index 0000000..54b6336 --- /dev/null +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -0,0 +1,125 @@ +#include + +// DrivetrainSystem::DrivetrainSystem() +// { + +// }; +DrivetrainSystem::DrivetrainSystem( + veh_vec inverter_interfaces) + : _inverter_interfaces(inverter_interfaces), _state(DrivetrainState_e::NOT_CONNECTED), + _check_inverter_ready_flag([](const InverterStatus_s & status) -> bool {return status.inverter_ready;}), + _check_inverter_connected_flag([](const InverterStatus_s & status) -> bool {return status.connected;}), + _check_inverter_quit_dc_flag([](const InverterStatus_s & status) -> bool {return status.quit_dc;}), + _check_inverter_error_flag([](const InverterStatus_s & status) -> bool {return status.error_present;}), + _check_inverter_hv_present_flag([](const InverterStatus_s & status) -> bool {return status.hv_present;}), + _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return !status.hv_present;}){}; + + +DrivetrainState_e DrivetrainSystem::get_state() +{ + return _state; +} + +DrivetrainStatus_s DrivetrainSystem::evaluate_drivetrain(DrivetrainSystem::CmdVariant cmd) +{ + _evaluate_state_machine(cmd); + + auto state = get_state(); + + DrivetrainStatus_s status; + status.all_inverters_connected = false; + // while not all inverters are connected, some may still be here so we can return the entire status for partially connected drivetrain. + status.inverter_statuses = {_inverter_interfaces.FL.get_status(), + _inverter_interfaces.FR.get_status(), + _inverter_interfaces.RL.get_status(), + _inverter_interfaces.RR.get_status() }; + + bool attempting_init_while_not_connected = ((state == DrivetrainState_e::NOT_CONNECTED) && etl::holds_alternative(cmd) && cmd.get().init_drivetrain); + if (etl::holds_alternative(cmd) && cmd.get().init_drivetrain) + { + status.cmd_resp = DrivetrainCmdResponse_e::CANNOT_INIT_NOT_CONNECTED; + } else { + status.cmd_resp = DrivetrainCmdResponse_e::COMMAND_INVALID; + } + + return status; +} + +void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) +{ + switch(get_state()) + { + // TODO need to ensure that the inverter outputs CAN messages on idle even not when being sent msgs + case DrivetrainState_e::NOT_CONNECTED: + { + bool connected_no_hv_present = (_check_inverter_flags(_check_inverter_connected_flag) && _check_inverter_flags(_check_inverter_hv_not_present_flag) ) + bool connected_hv_present = (_check_inverter_flags(_check_inverter_connected_flag) && _check_inverter_flags(_check_inverter_hv_present_flag)); + + if(connected_no_hv_present) + { + _set_state(DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT); + } else if(connected_hv_present) + { + _set_state(DrivetrainState_e::NOT_ENABLED_HV_PRESENT); + } + + break; + } + + case DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT: + { + + break; + } + + case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: + { + break; + } + + case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: + { + break; + } + case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: + { + break; + } + case DrivetrainState_e::ENABLED_SPEED_MODE: + { + break; + } + case DrivetrainState_e::ENABLED_TORQUE_MODE: + { + break; + } + case DrivetrainState_e::ERROR: + { + break; + } + } +} + +// returns false if any of the inverters fail the flag check. +bool DrivetrainSystem::_check_inverter_flags(std::function flag_check_func) +{ + auto funcs_arr = _inverter_interfaces.as_array(); + for(const auto & func : funcs_arr) + { + auto inverter_status = func.get_status(); + if(!flag_check_func(inverter_status)) + { + return false; + } + } + return true; +} + +void DrivetrainSystem::keepalive_disabled() +{ + auto funcs_arr = _inverter_interfaces.as_array(); + for(const auto & func: funcs_arr) + { + func.set_disable_inverter(); + } +} diff --git a/lib/systems/src/DrivetrainSystemv2.cpp b/lib/systems/src/DrivetrainSystemv2.cpp deleted file mode 100644 index 50b2df8..0000000 --- a/lib/systems/src/DrivetrainSystemv2.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include - -// DrivetrainSystem::DrivetrainSystem() -// { - -// }; -DrivetrainSystem::DrivetrainSystem( - veh_vec inverter_interfaces) - : _inverter_interfaces(inverter_interfaces) {}; - - -DrivetrainStatus_s DrivetrainSystem::evaluate_drivetrain(etl::variant cmd) {} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 704ba8e..9b3b5c8 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,7 +8,8 @@ lib_deps_shared = https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 - https://github.com/hytech-racing/shared_firmware_types.git#HT09-shared-datatypes + https://github.com/hytech-racing/shared_firmware_types.git#feature/veh_vec_addition + ; ../shared_firmware_types https://github.com/ssilverman/QNEthernet#v0.26.0 https://github.com/hytech-racing/HT_SCHED Embedded Template Library@^20.39.4 From c14d410a321559e40a25c696ebb0b6c1a9292659 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 11 Jan 2025 22:51:51 -0500 Subject: [PATCH 13/36] added reqs / todos --- lib/systems/include/DrivetrainSystem.h | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index f96e221..b2dfd82 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -10,6 +10,21 @@ #include "SharedFirmwareTypes.h" #include "SysClock.h" + +// requirements: +// - [ ] must support ability to initialize the drivetrain +// - [ ] ability to command inverters individually and be able to return a failure status when attempting to send invalid command for a certain state +// - [ ] contain a state machine for managing the state of the drivetrain as a whole (aka: all inverters have the same state) +// - [ ] initialization states included +// - [ ] different control mode states +// - [ ] single point of interaction / control of the drivetrain that can receive "commands" +// (at least for now, need to see how this works out once we start using it) +// - [ ] be decoupled from the inverter class +// std::function / etl::delegate registered functions for the inverter interface. mostly for ease of testing. +// - [ ] be able to reset drivetrain + // - [ ] + + enum class DrivetrainState_e { NOT_CONNECTED = 0, @@ -29,13 +44,6 @@ enum class DrivetrainCmdResponse_e COMMAND_INVALID = 2 }; -// #include "DrivetrainSystemStateMachine.h" -/** - * As of now, only the minimum functions for VehicleStateMachine to compile have been implemented. - * TODO: Re-add the rest of the necessary functions - * TODO: Add DrivetrainSystem.tpp to implement all functions - */ - struct DrivetrainSpeedCommand_s { veh_vec desired_speed_rpm; From b83d447d89e97c796ce407a27df08f9238562c5c Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 12 Jan 2025 14:02:01 -0500 Subject: [PATCH 14/36] working through more of the state machine --- lib/systems/include/DrivetrainSystem.h | 59 +++++++++++++------- lib/systems/src/DrivetrainSystem.cpp | 76 ++++++++++++++++++++++---- 2 files changed, 103 insertions(+), 32 deletions(-) diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index b2dfd82..d47cc04 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -30,11 +30,14 @@ enum class DrivetrainState_e NOT_CONNECTED = 0, NOT_ENABLED_NO_HV_PRESENT = 1, NOT_ENABLED_HV_PRESENT = 2, - ENABLING_INVERTERS_SPEED_MODE = 3, - ENABLING_INVERTERS_TORQUE_MODE = 4, - ENABLED_SPEED_MODE = 5, - ENABLED_TORQUE_MODE = 6, - ERROR = 7 + INVERTERS_READY = 3, + INVERTERS_HV_ENABLED = 4, + INVERTERS_ENABLED = 5, + ENABLING_INVERTERS_SPEED_MODE = 6, + ENABLING_INVERTERS_TORQUE_MODE = 7, + ENABLED_SPEED_MODE = 8, + ENABLED_TORQUE_MODE = 9, + ERROR = 10 }; enum class DrivetrainCmdResponse_e @@ -55,29 +58,36 @@ struct DrivetrainTorqueCommand_s veh_vec desired_torque_nm; }; +enum class DrivetrainModeRequest_e +{ + UNINITIALIZED = 0, + INIT_SPEED_MODE = 1, + INIT_TORQUE_MODE =2 +}; + struct DrivetrainInit_s { - bool init_drivetrain; + DrivetrainModeRequest_e init_drivetrain; }; struct InverterStatus_s { - float dc_bus_voltage; - float torque_nm; - float speed_rpm; - float mech_power_w; - float inverter_temp_c; - float motor_temp_c; - float igbt_temp_c; - uint16_t error_status_id; - bool inverter_ready : 1; - bool quit_dc : 1; - bool quit_inverter : 1; - bool error_present : 1; - bool connected : 1; - bool hv_present : 1; + float dc_bus_voltage; + float torque_nm; + float speed_rpm; + float mech_power_w; + float inverter_temp_c; + float motor_temp_c; + float igbt_temp_c; + uint16_t error_status_id; + bool inverter_ready : 1; + bool quit_dc : 1; + bool quit_inverter : 1; + bool error_present : 1; + bool connected : 1; + bool hv_present : 1; }; struct DrivetrainStatus_s @@ -87,6 +97,12 @@ struct DrivetrainStatus_s DrivetrainCmdResponse_e cmd_resp; }; +struct DrivetrainGPIO_s +{ + bool torque_mode_pin_state; + bool speed_mode_pin_state; +}; + class DrivetrainSystem { public: @@ -123,7 +139,8 @@ class DrivetrainSystem std::function _check_inverter_error_flag; std::function _check_inverter_hv_present_flag; std::function _check_inverter_hv_not_present_flag; - + + std::function _set_gpio_states; // MCUInterface *mcu_interface_; int _init_time_limit_ms; uint16_t _min_hv_voltage; diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index 54b6336..f70bb9e 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -12,7 +12,7 @@ DrivetrainSystem::DrivetrainSystem( _check_inverter_quit_dc_flag([](const InverterStatus_s & status) -> bool {return status.quit_dc;}), _check_inverter_error_flag([](const InverterStatus_s & status) -> bool {return status.error_present;}), _check_inverter_hv_present_flag([](const InverterStatus_s & status) -> bool {return status.hv_present;}), - _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return !status.hv_present;}){}; + _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return !status.hv_present;}) { }; DrivetrainState_e DrivetrainSystem::get_state() @@ -22,24 +22,21 @@ DrivetrainState_e DrivetrainSystem::get_state() DrivetrainStatus_s DrivetrainSystem::evaluate_drivetrain(DrivetrainSystem::CmdVariant cmd) { - _evaluate_state_machine(cmd); - - auto state = get_state(); + auto state = _evaluate_state_machine(cmd); DrivetrainStatus_s status; - status.all_inverters_connected = false; + status.all_inverters_connected = (_inverter_interfaces.FL.get_status().connected && _inverter_interfaces.FR.get_status().connected && _inverter_interfaces.RL.get_status().connected && _inverter_interfaces.RR.get_status().connected); // while not all inverters are connected, some may still be here so we can return the entire status for partially connected drivetrain. status.inverter_statuses = {_inverter_interfaces.FL.get_status(), _inverter_interfaces.FR.get_status(), _inverter_interfaces.RL.get_status(), _inverter_interfaces.RR.get_status() }; - bool attempting_init_while_not_connected = ((state == DrivetrainState_e::NOT_CONNECTED) && etl::holds_alternative(cmd) && cmd.get().init_drivetrain); - if (etl::holds_alternative(cmd) && cmd.get().init_drivetrain) + bool attempting_init_while_not_connected = ((state == DrivetrainState_e::NOT_CONNECTED) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED)); + + if (attempting_init_while_not_connected) { status.cmd_resp = DrivetrainCmdResponse_e::CANNOT_INIT_NOT_CONNECTED; - } else { - status.cmd_resp = DrivetrainCmdResponse_e::COMMAND_INVALID; } return status; @@ -63,24 +60,70 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) _set_state(DrivetrainState_e::NOT_ENABLED_HV_PRESENT); } + _keepalive_disabled(); // TODO dont know if this should be sent here, but it shouldn't hurt break; } case DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT: { - + if(_check_inverter_flags(_check_inverter_hv_present_flag)) + { + _set_state(DrivetrainState_e::NOT_ENABLED_HV_PRESENT); + } + _keepalive_disabled(); break; } case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: { + // bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); + + if(_check_inverter_flags(_check_inverter_ready_flag)) + { + _set_state(DrivetrainState_e::INVERTERS_READY); + } + + _keepalive_disabled(); + break; + } + + case DrivetrainState_e::INVERTERS_READY: + { + bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); + bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); + bool inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); + + if(requesting_init && inverters_ready && !hv_enabled) + { + _set_enable_drivetrain_hv(); + } else if(requesting_init && inverters_ready && hv_enabled) + { + _set_enable_drivetrain_hv(); // TODO determine if this still needs to be here, i dont think it does + _set_state(DrivetrainState_e::INVERTERS_HV_ENABLED); + } else if(!inverters_ready) + { + _set_state(DrivetrainState_e::NOT_ENABLED_HV_PRESENT); + } + + break; + } + + case DrivetrainState_e::INVERTERS_HV_ENABLED: + { + bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); + if() break; } case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: { + DrivetrainGPIO_s states; + states.torque_mode_pin_state = false; + states.speed_mode_pin_state = true; + _set_gpio_states(states); break; } + case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: { break; @@ -98,6 +141,17 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) break; } } + + return get_state(); +} + +void DrivetrainSystem::_set_enable_drivetrain_hv() +{ + auto funcs_arr = _inverter_interfaces.as_array(); + for(const auto & func : funcs_arr) + { + func.set_enable_hv(); + } } // returns false if any of the inverters fail the flag check. @@ -115,7 +169,7 @@ bool DrivetrainSystem::_check_inverter_flags(std::function Date: Mon, 13 Jan 2025 15:09:38 -0500 Subject: [PATCH 15/36] added current docs for drivetrain state machine --- lib/systems/README.md | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 lib/systems/README.md diff --git a/lib/systems/README.md b/lib/systems/README.md new file mode 100644 index 0000000..e54ffe7 --- /dev/null +++ b/lib/systems/README.md @@ -0,0 +1,26 @@ +```mermaid +--- +title: drivetrain state machine +--- +stateDiagram-v2 + + nc : NOT_CONNECTED + not_en : NOT_ENABLED_NO_HV_PRESENT + not_en_hv : NOT_ENABLED_HV_PRESENT + ready : INVERTERS_READY + hv_en : INVERTERS_HV_ENABLED + en : INVERTERS_ENABLED + en_sp_mode : ENABLING_INVERTERS_SPEED_MODE + en_torq_mode : ENABLING_INVERTERS_TORQUE_MODE + speed_mode : ENABLED_SPEED_MODE + torq_mode : ENABLED_TORQUE_MODE + err: ERROR + + nc --> not_en: received data from all inverters + nc --> not_en_hv: received data from all inverters AND the inverters have a voltage above HV level + not_en --> not_en_hv: HV present to all inverters + not_en_hv --> ready: all inverters have their ready flag set + ready --> hv_en: requesting initialization of drivetrain AND all inverters have their quit dc flag on + ready --> not_en_hv: if any of the inverters have their ready flags not set + +``` \ No newline at end of file From 93d08a197335c48884dba0d8b8fc0686b5a5bfb0 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Mon, 13 Jan 2025 15:11:12 -0500 Subject: [PATCH 16/36] new adds to drivetrain system --- lib/systems/include/DrivetrainSystem.h | 8 +++-- lib/systems/src/DrivetrainSystem.cpp | 42 +++++++++++++++++++++++--- 2 files changed, 43 insertions(+), 7 deletions(-) diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index d47cc04..ab39f68 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -70,8 +70,6 @@ struct DrivetrainInit_s DrivetrainModeRequest_e init_drivetrain; }; - - struct InverterStatus_s { float dc_bus_voltage; @@ -139,9 +137,11 @@ class DrivetrainSystem std::function _check_inverter_error_flag; std::function _check_inverter_hv_present_flag; std::function _check_inverter_hv_not_present_flag; + std::function _check_inverter_enabled; std::function _set_gpio_states; - // MCUInterface *mcu_interface_; + + int _init_time_limit_ms; uint16_t _min_hv_voltage; int _motor_pole_pairs; @@ -152,6 +152,8 @@ class DrivetrainSystem bool _reset_requested; unsigned long _last_reset_pressed_time; unsigned long _reset_interval; + + /// @param curr_time current system tick time (millis()) that sets the init phase start time void enable_drivetrain_hv(unsigned long curr_time); void request_enable(); diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index f70bb9e..349b1a8 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -12,7 +12,8 @@ DrivetrainSystem::DrivetrainSystem( _check_inverter_quit_dc_flag([](const InverterStatus_s & status) -> bool {return status.quit_dc;}), _check_inverter_error_flag([](const InverterStatus_s & status) -> bool {return status.error_present;}), _check_inverter_hv_present_flag([](const InverterStatus_s & status) -> bool {return status.hv_present;}), - _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return !status.hv_present;}) { }; + _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return !status.hv_present;}), + _check_inverter_enabled([](const InverterStatus_s & status) -> bool {return !status.quit_inverter;}) { }; DrivetrainState_e DrivetrainSystem::get_state() @@ -90,9 +91,8 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) case DrivetrainState_e::INVERTERS_READY: { bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); - bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); bool inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); - + bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); if(requesting_init && inverters_ready && !hv_enabled) { _set_enable_drivetrain_hv(); @@ -111,16 +111,41 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) case DrivetrainState_e::INVERTERS_HV_ENABLED: { bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); - if() + bool requesting_speed_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); + bool requesting_torque_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); + + bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); + bool inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); + bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); + + if(requesting_init && hv_enabled && inverters_ready && hv_enabled && !inverters_enabled) + { + _set_enable_drivetrain(); + } else if(requesting_speed_mode && hv_enabled && inverters_ready && hv_enabled && inverters_enabled); + { + _set_enable_drivetrain(); + _set_state(DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE); + } else if(requesting_torque_mode && hv_enabled && inverters_ready && hv_enabled && inverters_enabled); + { + _set_enable_drivetrain(); + _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); + } + // TODO need to see if we need more checks to go back to previous states break; } case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: { + bool requesting_speed_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); + bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); DrivetrainGPIO_s states; states.torque_mode_pin_state = false; states.speed_mode_pin_state = true; _set_gpio_states(states); + + // TODO check some flag for torque mode to make sure we are in the speed mode + + _set_state(DrivetrainState_e::ENABLED_SPEED_MODE); break; } @@ -154,6 +179,15 @@ void DrivetrainSystem::_set_enable_drivetrain_hv() } } +void DrivetrainSystem::_set_enable_drivetrain() +{ + auto funcs_arr = _inverter_interfaces.as_array(); + for(const auto & func : funcs_arr) + { + func.set_enable_inverter(); + } +} + // returns false if any of the inverters fail the flag check. bool DrivetrainSystem::_check_inverter_flags(std::function flag_check_func) { From 452100cc21516789f2d9a1e1b8b786ae214a4cb5 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 18 Jan 2025 16:58:15 -0500 Subject: [PATCH 17/36] added more design docs to the drivetrain system --- lib/systems/README.md | 42 ++++++++- lib/systems/include/DrivetrainSystem.h | 35 ++++---- lib/systems/src/DrivetrainSystem.cpp | 117 +++++++++++++++++++++---- 3 files changed, 156 insertions(+), 38 deletions(-) diff --git a/lib/systems/README.md b/lib/systems/README.md index e54ffe7..68e970a 100644 --- a/lib/systems/README.md +++ b/lib/systems/README.md @@ -1,3 +1,26 @@ + +## Drivetrain System (tentative design docs) + +### interface / usage description + +the user of the drivetrain system will be expected to use the `evaluate_drivetrain` function to interact / command and receive the current status of the drivetrain. + + +#### initialization +the user shall call the `evaluate_drivetrain` function with the cmdvariant type set to the initialization struct and populated with desired mode to put the drivetrain into. the user should continuously call the `evaluate_drivetrain` with this struct until the drivetrain's state reaches the initialization state expected. + +#### drivetrain commanding + +once initialized, the user is able to command the drivetrain with either speed or torque commands. + +The user may switch between command modes so long as the drivetrain is not active / the car is not driving (all inverter RPMs < 100). this is purely for safety, however this needs to be tested for the exact requirements to switch between control modes during runtime. + +### state machine description + +Instead of the drivetrain system being a direct api interface on top of the inverter interface with calls to the drivetrain system that call the lower-level inverter interface immediately to queue CAN messages, the drivetrain system will instead update pieces of the inverter interface's internal state (such as bit flags) for it to send periodically. + +This is being done to simplify the interaction between the inverters and the rest of our firmware and to resolve the CAN saturation issues that were occuring with the last drivetrain system / inverter interface issues that were hackily solved with a metro timer dictating when the queue of CAN messages that were being sent out from the inverter could be appended to. + ```mermaid --- title: drivetrain state machine @@ -15,6 +38,11 @@ stateDiagram-v2 speed_mode : ENABLED_SPEED_MODE torq_mode : ENABLED_TORQUE_MODE err: ERROR + clear_err: CLEARING_ERRORS + + note right of err + during this state the inverters will be not be sent any control setpoints + end note nc --> not_en: received data from all inverters nc --> not_en_hv: received data from all inverters AND the inverters have a voltage above HV level @@ -22,5 +50,17 @@ stateDiagram-v2 not_en_hv --> ready: all inverters have their ready flag set ready --> hv_en: requesting initialization of drivetrain AND all inverters have their quit dc flag on ready --> not_en_hv: if any of the inverters have their ready flags not set - + hv_en --> en: on set of all inverters enabled flags + en --> en_sp_mode: on command request of speed mode + en_sp_mode --> speed_mode: on verification of inverters GPIO of speed mode set + en --> en_torq_mode: on command request of torque mode + en_torq_mode --> torq_mode: on verification of inverters GPIO of torque mode set + torq_mode --> err: on error during operation + speed_mode --> err: on error during operation + torq_mode --> en_sp_mode: on request of speed mode AND drivetrain is not active + speed_mode --> en_torq_mode: on request of torque mode AND drivetrain is not active + speed_mode --> err: incorrect user command type OR any inverter error + torq_mode --> err: incorrect user command type OR any inverter error + err --> clear_err: on user request of error reset + clear_err --> not_en_hv: on successful reset of errors ``` \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index ab39f68..e9c0626 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -25,6 +25,8 @@ // - [ ] + +// TODO move these into the shared types after finishing the system enum class DrivetrainState_e { NOT_CONNECTED = 0, @@ -95,10 +97,17 @@ struct DrivetrainStatus_s DrivetrainCmdResponse_e cmd_resp; }; -struct DrivetrainGPIO_s +// output pin of micro, read by inverters +struct DrivetrainOutputPins_s +{ + bool torque_mode_pin_state : 1; +}; + + +// the pin set by the inverters themselves ("input": pin being read by micro) +struct DrivetrainInputPins_s { - bool torque_mode_pin_state; - bool speed_mode_pin_state; + bool torque_mode_enabled_pin_state : 1; }; class DrivetrainSystem @@ -122,12 +131,10 @@ class DrivetrainSystem std::function get_status; }; - // struct DrivetrainInterfaceState_s - // { - - // }; private: bool _check_inverter_flags(std::function flag_check_func); + void _handle_exit_logic(DrivetrainState_e prev_state); + void _handle_entry_logic(DrivetrainState_e new_state); private: DrivetrainState_e _state; veh_vec _inverter_interfaces; @@ -139,8 +146,8 @@ class DrivetrainSystem std::function _check_inverter_hv_not_present_flag; std::function _check_inverter_enabled; - std::function _set_gpio_states; - + std::function _set_gpio_state; + std::function _get_gpio_state; int _init_time_limit_ms; uint16_t _min_hv_voltage; @@ -152,16 +159,6 @@ class DrivetrainSystem bool _reset_requested; unsigned long _last_reset_pressed_time; unsigned long _reset_interval; - - - /// @param curr_time current system tick time (millis()) that sets the init phase start time - void enable_drivetrain_hv(unsigned long curr_time); - void request_enable(); - // startup phase 1 - // status check for start of enable - bool drivetrain_ready(); - // startup phase 2 - bool check_drivetrain_quit_dc_on(); // final check for drivetrain initialization to check if quit inverter on bool drivetrain_enabled(); diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index 349b1a8..8dcd3c2 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -98,7 +98,7 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) _set_enable_drivetrain_hv(); } else if(requesting_init && inverters_ready && hv_enabled) { - _set_enable_drivetrain_hv(); // TODO determine if this still needs to be here, i dont think it does + _send_enable_drivetrain_hv(); _set_state(DrivetrainState_e::INVERTERS_HV_ENABLED); } else if(!inverters_ready) { @@ -111,8 +111,8 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) case DrivetrainState_e::INVERTERS_HV_ENABLED: { bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); - bool requesting_speed_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); - bool requesting_torque_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); + // bool requesting_speed_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); + // bool requesting_torque_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); bool inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); @@ -120,32 +120,39 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) if(requesting_init && hv_enabled && inverters_ready && hv_enabled && !inverters_enabled) { - _set_enable_drivetrain(); - } else if(requesting_speed_mode && hv_enabled && inverters_ready && hv_enabled && inverters_enabled); - { - _set_enable_drivetrain(); - _set_state(DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE); - } else if(requesting_torque_mode && hv_enabled && inverters_ready && hv_enabled && inverters_enabled); + _set_enable_drivetrain(); // should be done on entry of this state + } else if(hv_enabled && inverters_ready && hv_enabled && inverters_enabled); { _set_enable_drivetrain(); - _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); + _set_state(DrivetrainState_e::INVERTERS_ENABLED); } // TODO need to see if we need more checks to go back to previous states break; } - case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: + case DrivetrainState_e::INVERTERS_ENABLED: { bool requesting_speed_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); + bool requesting_torque_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); + bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); - DrivetrainGPIO_s states; - states.torque_mode_pin_state = false; - states.speed_mode_pin_state = true; - _set_gpio_states(states); - // TODO check some flag for torque mode to make sure we are in the speed mode - _set_state(DrivetrainState_e::ENABLED_SPEED_MODE); + + break; + } + + case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: + { + bool requesting_speed_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); + bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); + // TODO check some flag for torque mode to make sure we are in the speed mode + auto gpio_state = _get_gpio_state(); + if((!gpio_state.torque_mode_enabled_pin_state) && inverters_enabled && requesting_speed_mode) + { + _set_state(DrivetrainState_e::ENABLED_SPEED_MODE); + } + break; } @@ -165,12 +172,82 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) { break; } + default: + break; } return get_state(); } -void DrivetrainSystem::_set_enable_drivetrain_hv() +void DrivetrainSystem::_set_state(DrivetrainState_e new_state) +{ + _handle_exit_logic(_state); + _state = new_state; + _handle_entry_logic(new_state); +} + +void DrivetrainSystem::_handle_exit_logic(DrivetrainState_e prev_state) +{ + switch (prev_state) + { + case DrivetrainState_e::NOT_CONNECTED: + case DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT: + case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: + case DrivetrainState_e::INVERTERS_READY: + case DrivetrainState_e::INVERTERS_HV_ENABLED: + case DrivetrainState_e::INVERTERS_ENABLED: + case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: + case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: + case DrivetrainState_e::ENABLED_SPEED_MODE: + case DrivetrainState_e::ENABLED_TORQUE_MODE: + case DrivetrainState_e::ERROR: + break; + default: + break; + } +} + +void DrivetrainSystem::_handle_entry_logic(DrivetrainState_e new_state) +{ + switch (new_state) + { + case DrivetrainState_e::NOT_CONNECTED: + break; + case DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT: + break; + case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: + break; + case DrivetrainState_e::INVERTERS_READY: + break; + case DrivetrainState_e::INVERTERS_HV_ENABLED: + break; + case DrivetrainState_e::INVERTERS_ENABLED: + break; + case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: + { + // set the torque mode pin state to false (aka: keep the inverter in the main control mode which is speed mode) + _set_gpio_state({false}); + break; + } + + case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: + { + _set_gpio_state({true}); + break; + } + break; + case DrivetrainState_e::ENABLED_SPEED_MODE: + break; + case DrivetrainState_e::ENABLED_TORQUE_MODE: + break; + case DrivetrainState_e::ERROR: + break; + default: + break; + } +} + +void DrivetrainSystem::_send_enable_drivetrain_hv() { auto funcs_arr = _inverter_interfaces.as_array(); for(const auto & func : funcs_arr) @@ -210,4 +287,8 @@ void DrivetrainSystem::_keepalive_disabled() { func.set_disable_inverter(); } + + DrivetrainGPIO_s states; + states.torque_mode_pin_state = false; + _set_gpio_states(states); } From 9adec30d984f2fea4af4576e5ffd1ade166b9c1d Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 18 Jan 2025 20:02:29 -0500 Subject: [PATCH 18/36] added description for the inverter command messages --- lib/interfaces/README.md | 40 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 lib/interfaces/README.md diff --git a/lib/interfaces/README.md b/lib/interfaces/README.md new file mode 100644 index 0000000..58ba35b --- /dev/null +++ b/lib/interfaces/README.md @@ -0,0 +1,40 @@ +## Inverter Interface + + +### GPIO connections + + +### CAN parameter / message descriptions + +#### VCR output / control messages: + +below is the description of the control messages that will be configured within the free CAN configuration of the AMKs. these are subject to change, however it is primarily a modification of the "fixed CAN commnicaiton" protocol that the AMK kit ships with / is documented + +- Control Word (will be sent every 50ms, monitored by the inverter to be expected at least every 75ms) + - `bool inverter_enable` + - AKA (`AMK_bInverterOn`) + - `bool hv_enable` + - AKA (`AMK_bDcOn`) + - `bool driver_enable` + - AKA (`AMK_bEnable`) + - `bool remove_error` + - AKA (`AMK_bErrorReset`) + +- Control Input Message (will be attempted to be sent every 5ms, monitored by the inverter to be expected at least every 20ms) + - `int16_t speed_setpoint` + - __NOTE: active only when in speed control mode__ + - AKA (`AMK_TargetVelocity`) / Special Signal index 6 / AKA 16-bit version of SERCOS parameter ID36 (unknown why this is 16 bit vs the 32 bit SERCOS parameter in manual) + - the RPM setpoint for the inverter in units of RPM. + - `int16_t positive_torque_limit` + - AKA (`AMK_TorqueLimitPositiv`) / SERCOS parameter ID82 / Special Signal index 13 + - positive torque limit for reaching the desired rpm in units of 0.1% of the nominal torque (=9.8Nm)(known as Mn in docs). + + EXAMPLE: `positive_torque_limit=1000` means that the positive torque limit is set to 100% of the nominal torque, so the actual torque limit being set by this is equal to 9.8Nm, and 200% would be equal to 19.6Nm. + - `int16_t negative_torque_limit` + - AKA (`AMK_TorqueLimitNegativ`) / SERCOS parameter ID83 / Special Signal index 14 + - `positive_torque_limit` with a negative sign. + - `int16_t torque_setpoint` + - __NOTE: active only when in torque control mode.__ + - AKA SERCOS parameter ID80 / Special Signal index 17 + - the torque setpoint for the inverter in units of percentage as described in the `positive_torque_limit` message member above + From da5c80f989efb7de0db87c933c4541150fc211e7 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 19 Jan 2025 21:50:05 -0500 Subject: [PATCH 19/36] updated system and interface docs for inverter / drivetrain --- lib/interfaces/README.md | 72 ++++++++++++++++++++++++++++++++++++++++ lib/systems/README.md | 37 ++++++++++++++++++--- 2 files changed, 105 insertions(+), 4 deletions(-) diff --git a/lib/interfaces/README.md b/lib/interfaces/README.md index 58ba35b..28b225e 100644 --- a/lib/interfaces/README.md +++ b/lib/interfaces/README.md @@ -38,3 +38,75 @@ below is the description of the control messages that will be configured within - AKA SERCOS parameter ID80 / Special Signal index 17 - the torque setpoint for the inverter in units of percentage as described in the `positive_torque_limit` message member above +#### inverter messages + +- inverter status / ( `AMK_Status` (16 bit) + + DC bus voltage (16 bit) + `AMK_ErrorInfo` (16 bit kinda(?))) + - `bool system_ready` + - AKA (`AMK_bSystemReady`) / bit 9 within Special Signal Status word formula student (System Ready (SBM)) / + - displays when the system is error free / ready to be initialized + - `bool error` + - AKA (`AMK_bError`) / bit 10 within Special Signal Status word formula student + - displays that an error is present + - `bool warning` + - AKA (`AMK_bWarn`) / bit 11 + - warning present (such as if derating is on) + - `bool quit_dc_on` + - AKA (`AMK_bQuitDcOn`) / bit 12 + - HV activation acknowledgment / verification (says whether or not HV is ACTUALLY present to the inverter) + - `bool dc_on` + - AKA (`AMK_bDcOn`) / bit 13 + - mirrors / is feedback of what is set by the VCR for its `hv_enable` + - `bool quit_inverter_on` + - AKA (`AMK_bQuitInverterOn`) / bit 14 + - controller is enabled + - `bool inverter_on` + - AKA (`AMK_bInverterOn`) / bit 15 + - mirror / feedback of `inverter_enable` set by the control word + - `bool derating_on` + - AKA (`AMK_bDerating`) / bit 16 + - says whether or not derating is active (torque derating) + - `uint16_t dc_bus_voltage` + - AKA SERCOS Paramter ID32836 + - actual DC bus voltage + - `uint16_t diagnostic_number` + - AKA (`AMK_ErrorInfo`) + +- inverter temps + - `int16_t motor_temp` + - temperature of the motor in units of 0.1 degrees celcius + - AKA (`AMK_TempMotor`) / Special Signal index 7 / SERCOS parameter ID33117 + - `int16_t inverter_temp` + - temp of the inverter cold plate in units of 0.1 degrees celcius + - AKA (`AMK_TempInverter`) / Special Signal index 8 / SERCOS parameter ID33116 + + - details: "ID33116 shows the temperature of the cold plate (heat sink of the IGBT and at the same time of the rear wall of the device). +The triggering thresholds are device-specific, are set in the SEEP (Device-internal memory, serial EEPROM) at the factory and cannot be changed by the user. +If critical temperatures occur for the devices, the warning 2350 'Device temperature warning' is generated as well as the error +message 2346 'Converter temperature error' after the warning time1) (ID32943) has expired." + - `int16_t igbt_temp` + - temp of the IGBTs in units of 0.1 degrees celcius + - AKA (`AMK_TempIGBT`) / Special Signal index 27 / SERCOS parameter ID34215 + + +- inverter dynamics data + - `uint32_t actual_power_w` + - mechanical motor power in watts (from actual torque and actual speed) + - AKA SERCOS parameter ID33100 + - `int16_t actual_torque_nm` + - actual torque value in the same units as `positive_torque_limit` + - AKA Special Signal index 19 / SERCOS parameter ID84 + - `int16_t actual_speed_rpm` + - motor speed in units of rpm + - AKA Special Signal value index 6 + +- inverter electrical power data + - `int32_t active_power_w` + - electrical power (use / creation) in watts of the inverter + - can be negative when in regen / positive when driving motor + - AKA SERCOS parameter ID33171 + - `int32_t reactive_power_var` + - electrical reactive power (inductive or capacitive) + - positive value = inductive consumer, negative value = capacitive consumer + - AKA SERCOS parameter ID33172 + + \ No newline at end of file diff --git a/lib/systems/README.md b/lib/systems/README.md index 68e970a..ec89b9c 100644 --- a/lib/systems/README.md +++ b/lib/systems/README.md @@ -21,7 +21,8 @@ Instead of the drivetrain system being a direct api interface on top of the inve This is being done to simplify the interaction between the inverters and the rest of our firmware and to resolve the CAN saturation issues that were occuring with the last drivetrain system / inverter interface issues that were hackily solved with a metro timer dictating when the queue of CAN messages that were being sent out from the inverter could be appended to. -```mermaid + +``` --- title: drivetrain state machine --- @@ -39,18 +40,46 @@ stateDiagram-v2 torq_mode : ENABLED_TORQUE_MODE err: ERROR clear_err: CLEARING_ERRORS - + note2: note that that note shown is that any state other than NOT_CONNECTED can go to the ERROR state if in any of the states any inverter has an error flag present + note right of err - during this state the inverters will be not be sent any control setpoints + during this state all setpoints will be set to 0 and the inverter control word will have the inverter_enable flag set to false + end note + + note right of clear_err + during this state all setpoints will be set to 0, (inverter_enable: false, hv_enable: true, driver_enable: true, remove_error: true) + end note + + note right of speed_mode + during this mode the drivetrain is free to receive speed commands and car will move + end note + + note right of torq_mode + during this mode the drivetrain is free to receive torque commands and car will move end note + note right of en_sp_mode + during this mode the VCR sets the enable torque mode GPIO pin to be off and waits for the inverter's GPIO for signaling that the secondary control is not in control + end note + + note right of en_torq_mode + same as the ENABLING_SPEED_MODE, however the VCR is turning on its GPIO pin and waits for the inverter's GPIO to be signaled on + end note + + nc --> not_en: received data from all inverters nc --> not_en_hv: received data from all inverters AND the inverters have a voltage above HV level + not_en --> not_en_hv: HV present to all inverters not_en_hv --> ready: all inverters have their ready flag set + + hv_en --> ready: on quit dc flag off + ready --> hv_en: requesting initialization of drivetrain AND all inverters have their quit dc flag on ready --> not_en_hv: if any of the inverters have their ready flags not set hv_en --> en: on set of all inverters enabled flags + en --> hv_en: on inverter enabled flag off + en --> en_sp_mode: on command request of speed mode en_sp_mode --> speed_mode: on verification of inverters GPIO of speed mode set en --> en_torq_mode: on command request of torque mode @@ -62,5 +91,5 @@ stateDiagram-v2 speed_mode --> err: incorrect user command type OR any inverter error torq_mode --> err: incorrect user command type OR any inverter error err --> clear_err: on user request of error reset - clear_err --> not_en_hv: on successful reset of errors + clear_err --> not_en_hv: on successful reset of errors (either internally to the drivetrain system or of the inverters themselves) ``` \ No newline at end of file From 015cd9f49b5b650deb1d6da58dd71e5451cd9a61 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 19 Jan 2025 22:11:41 -0500 Subject: [PATCH 20/36] updated inverter messaging --- lib/interfaces/README.md | 32 ++++++++++++++++++++++++++------ 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/lib/interfaces/README.md b/lib/interfaces/README.md index 28b225e..374d6f6 100644 --- a/lib/interfaces/README.md +++ b/lib/interfaces/README.md @@ -37,10 +37,21 @@ below is the description of the control messages that will be configured within - __NOTE: active only when in torque control mode.__ - AKA SERCOS parameter ID80 / Special Signal index 17 - the torque setpoint for the inverter in units of percentage as described in the `positive_torque_limit` message member above - + +- Control parameter message (will be sent intermitently, not monitored by the inverter to be expected at any regular period) + - `uint16_t speed_control_kp` + - P for the speed control mode + - AKA SERCOS parameter ID100 + - `uint16_t speed_control_ki` + - I parameter to be set internally on the inverter + - AKA SERCOS parameter ID101 + - `uint16_t speed_control_kd` + - D parameter to be set internally on the inverter + - AKA SERCOS parameter ID102 + #### inverter messages -- inverter status / ( `AMK_Status` (16 bit) + + DC bus voltage (16 bit) + `AMK_ErrorInfo` (16 bit kinda(?))) +- inverter status / ( `AMK_Status` (16 bit) + + DC bus voltage (16 bit) + `AMK_ErrorInfo` (16 bit kinda(?))) (will be periodically sent from the inverter every 20ms) - `bool system_ready` - AKA (`AMK_bSystemReady`) / bit 9 within Special Signal Status word formula student (System Ready (SBM)) / - displays when the system is error free / ready to be initialized @@ -71,7 +82,7 @@ below is the description of the control messages that will be configured within - `uint16_t diagnostic_number` - AKA (`AMK_ErrorInfo`) -- inverter temps +- inverter temps (will be periodically sent from the inverter every 20ms) - `int16_t motor_temp` - temperature of the motor in units of 0.1 degrees celcius - AKA (`AMK_TempMotor`) / Special Signal index 7 / SERCOS parameter ID33117 @@ -88,7 +99,7 @@ message 2346 'Converter temperature error' after the warning time1) (ID32943) ha - AKA (`AMK_TempIGBT`) / Special Signal index 27 / SERCOS parameter ID34215 -- inverter dynamics data +- inverter dynamics data (will be periodically sent from the inverter every 5ms) - `uint32_t actual_power_w` - mechanical motor power in watts (from actual torque and actual speed) - AKA SERCOS parameter ID33100 @@ -99,7 +110,7 @@ message 2346 'Converter temperature error' after the warning time1) (ID32943) ha - motor speed in units of rpm - AKA Special Signal value index 6 -- inverter electrical power data +- inverter electrical power data (will be periodically sent from the inverter every 5ms) - `int32_t active_power_w` - electrical power (use / creation) in watts of the inverter - can be negative when in regen / positive when driving motor @@ -109,4 +120,13 @@ message 2346 'Converter temperature error' after the warning time1) (ID32943) ha - positive value = inductive consumer, negative value = capacitive consumer - AKA SERCOS parameter ID33172 - \ No newline at end of file +- inverter parameter feedback (inverter's speed PID vals) (will be sent periodically from the inverter every 20ms) + - `uint16_t speed_control_kp` + - inverter's internal value of kp + - AKA SERCOS parameter ID100 + - `uint16_t speed_control_ki` + - inverter's internal value of (TN) ki + - AKA SERCOS parameter ID101 + - `uint16_t speed_control_kd` + - inverter's internal value of (TD) kd + - AKA SERCOS parameter ID102 \ No newline at end of file From 0ff34eca6704946f193deb2389de6a064b8d04b5 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 19 Jan 2025 22:56:33 -0500 Subject: [PATCH 21/36] adding in feedback types for the inverter interface --- lib/interfaces/README.md | 1 - lib/interfaces/include/InverterInterface.h | 76 ++++++++++++++++++++++ 2 files changed, 76 insertions(+), 1 deletion(-) create mode 100644 lib/interfaces/include/InverterInterface.h diff --git a/lib/interfaces/README.md b/lib/interfaces/README.md index 374d6f6..2c9b606 100644 --- a/lib/interfaces/README.md +++ b/lib/interfaces/README.md @@ -97,7 +97,6 @@ message 2346 'Converter temperature error' after the warning time1) (ID32943) ha - `int16_t igbt_temp` - temp of the IGBTs in units of 0.1 degrees celcius - AKA (`AMK_TempIGBT`) / Special Signal index 27 / SERCOS parameter ID34215 - - inverter dynamics data (will be periodically sent from the inverter every 5ms) - `uint32_t actual_power_w` diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h new file mode 100644 index 0000000..8c8e86c --- /dev/null +++ b/lib/interfaces/include/InverterInterface.h @@ -0,0 +1,76 @@ +#ifndef __INVERTERINTERFACE_H__ +#define __INVERTERINTERFACE_H__ +#include + +namespace HTUnits +{ + using celcius = float; + using watts = float; + using var = float; + using torque_nm = float; + using speed_rpm = float; + using volts = float; +}; + +// for the most part these are mirrors of the lower-level CAN struct data, except with already fully float-ized data + +struct InverterStatus_s +{ + bool system_ready : 1; + bool error : 1; + bool warning : 1; + bool quit_dc_on : 1; + bool dc_on : 1; + bool quit_inverter_on : 1; + bool inverter_on : 1; + bool derating_on : 1; + HTUnits::volts dc_bus_voltage; + uint16_t diagnostic_number; +}; + +struct InverterTemps_s +{ + HTUnits::celcius motor_temp; + HTUnits::celcius inverter_temp; + HTUnits::celcius igbt_temp; +}; + +struct InverterPower_s +{ + HTUnits::watts active_power; + HTUnits::var reactive_power; +}; + +struct MotorMechanics_s +{ + HTUnits::watts actual_power; + HTUnits::torque_nm actual_torque; + HTUnits::speed_rpm actual_speed; +}; + +struct InverterControlParams_s +{ + uint16_t speed_control_kp; + uint16_t speed_control_ki; + uint16_t speed_control_kd; +}; + +struct InverterFeedbackData_s +{ + InverterStatus_s status; + InverterTemps_s temps; + InverterPower_s power; + MotorMechanics_s motor_mechanics; + InverterControlParams_s control_params; +}; + +// struct InverterMotorControl_s +// { + +// } + +class InverterInterface +{ + +}; +#endif // __INVERTERINTERFACE_H__ \ No newline at end of file From 0fa1c43231de8e918dcb1ea22e0e6ae552743f23 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Tue, 21 Jan 2025 17:12:14 -0500 Subject: [PATCH 22/36] working through state machine --- lib/systems/README.md | 11 +- lib/systems/include/DrivetrainSystem.h | 6 + lib/systems/src/DrivetrainSystem.cpp | 169 ++++++++++++++++++------- 3 files changed, 138 insertions(+), 48 deletions(-) diff --git a/lib/systems/README.md b/lib/systems/README.md index ec89b9c..e41d3e8 100644 --- a/lib/systems/README.md +++ b/lib/systems/README.md @@ -1,11 +1,20 @@ ## Drivetrain System (tentative design docs) +### list of features + +- [ ] ability to initialize inverters +- [ ] ability to command inverters +- [ ] ability to switch inverter control modes +- [ ] ability to detect timeout of initialization of inverters +- [ ] ability to change parameters of inverters +- [ ] detailed error status for invalid usage of drivetrain system +- [ ] ability to get all data available from inverter + ### interface / usage description the user of the drivetrain system will be expected to use the `evaluate_drivetrain` function to interact / command and receive the current status of the drivetrain. - #### initialization the user shall call the `evaluate_drivetrain` function with the cmdvariant type set to the initialization struct and populated with desired mode to put the drivetrain into. the user should continuously call the `evaluate_drivetrain` with this struct until the drivetrain's state reaches the initialization state expected. diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index e9c0626..14e7562 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -135,6 +135,12 @@ class DrivetrainSystem bool _check_inverter_flags(std::function flag_check_func); void _handle_exit_logic(DrivetrainState_e prev_state); void _handle_entry_logic(DrivetrainState_e new_state); + + void _set_drivetrain_disabled(); + void _set_enable_drivetrain_hv(); + void _set_enable_drivetrain(); + + void _set_drivetrain_speed_command(DrivetrainSpeedCommand_s cmd); private: DrivetrainState_e _state; veh_vec _inverter_interfaces; diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index 8dcd3c2..05a8443 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -1,9 +1,8 @@ #include -// DrivetrainSystem::DrivetrainSystem() -// { +//- [ ] TODO handle inverter keepalives with correct settings of inverter flags for their associated states +//- [ ] TODO handle -// }; DrivetrainSystem::DrivetrainSystem( veh_vec inverter_interfaces) : _inverter_interfaces(inverter_interfaces), _state(DrivetrainState_e::NOT_CONNECTED), @@ -76,92 +75,166 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) } case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: - { - // bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); - - if(_check_inverter_flags(_check_inverter_ready_flag)) + { + bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + if(inverter_error_present) { - _set_state(DrivetrainState_e::INVERTERS_READY); + _set_state(DrivetrainState_e::ERROR); } + else if(_check_inverter_flags(_check_inverter_ready_flag)) + { + _set_state(DrivetrainState_e::INVERTERS_READY); + } _keepalive_disabled(); break; } case DrivetrainState_e::INVERTERS_READY: { + bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + + // in this state, we are requesting inverters to enable HV bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); bool inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); - if(requesting_init && inverters_ready && !hv_enabled) + + if(inverter_error_present) + { + _set_state(DrivetrainState_e::ERROR); + } else if(requesting_init && inverters_ready && !hv_enabled) { _set_enable_drivetrain_hv(); } else if(requesting_init && inverters_ready && hv_enabled) { - _send_enable_drivetrain_hv(); + _set_enable_drivetrain_hv(); _set_state(DrivetrainState_e::INVERTERS_HV_ENABLED); } else if(!inverters_ready) { _set_state(DrivetrainState_e::NOT_ENABLED_HV_PRESENT); } - break; } case DrivetrainState_e::INVERTERS_HV_ENABLED: { + // in this state, we are requesting the inverter to actually be enabled + + // cases that need to be handled: + // - [x] if the user stops requesting init -> go back to inverters ready state + // - [x] error present on an inverter -> go to error state + + bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); - // bool requesting_speed_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); - // bool requesting_torque_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); bool inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); - if(requesting_init && hv_enabled && inverters_ready && hv_enabled && !inverters_enabled) + if(inverter_error_present) + { + _set_drivetrain_disabled(); + _set_state(DrivetrainState_e::ERROR); + } + else if(requesting_init && hv_enabled && inverters_ready && hv_enabled && !inverters_enabled) { _set_enable_drivetrain(); // should be done on entry of this state } else if(hv_enabled && inverters_ready && hv_enabled && inverters_enabled); { _set_enable_drivetrain(); _set_state(DrivetrainState_e::INVERTERS_ENABLED); + } else if((!requesting_init) && inverters_ready) + { + // NOTE if the user only calls the evaluate_drivetrain function once with the initialization request, + // this statement will kick in and lead to the inverter staying disabled. the user must keep calling with the initialization + // request flag / struct type in order to keep this state machine evaluating properly + _set_drivetrain_disabled(); + _set_state(DrivetrainState_e::INVERTERS_READY); } - // TODO need to see if we need more checks to go back to previous states + break; } case DrivetrainState_e::INVERTERS_ENABLED: { + bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool requesting_speed_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); bool requesting_torque_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); - bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); - - + // for now i wont worry about checking whether or not the inverters are still enabled. + // bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); + if(inverter_error_present) + { + _set_drivetrain_disabled(); + _set_state(DrivetrainState_e::ERROR); + } + else if(requesting_speed_mode) + { + // on transition the pin will be written to the correct state (for speed mode it should be off) + + _set_enable_drivetrain(); // this is just being verbose here, underlying on the inverter's interfaces all maintain + _set_state(DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE); + } else if(requesting_torque_mode) + { + // on transition the pin will be written to the correct state (for torq mode it should be on) + _set_enable_drivetrain(); + _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); + } + break; } case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: - { - bool requesting_speed_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); - bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); - // TODO check some flag for torque mode to make sure we are in the speed mode - auto gpio_state = _get_gpio_state(); - if((!gpio_state.torque_mode_enabled_pin_state) && inverters_enabled && requesting_speed_mode) + { + bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverters_in_speed_mode = (!_get_gpio_state().torque_mode_enabled_pin_state); + + if(inverter_error_present) { - _set_state(DrivetrainState_e::ENABLED_SPEED_MODE); + _set_drivetrain_disabled(); + _set_state(DrivetrainState_e::ERROR); + } else if(inverters_in_speed_mode) + { + _set_enable_drivetrain(); + _set_state(DrivetrainState_e::ENABLED_SPEED_MODE); } - break; } case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: { + + bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverters_in_torque_mode = _get_gpio_state().torque_mode_enabled_pin_state; + + if(inverter_error_present) + { + _set_drivetrain_disabled(); + _set_state(DrivetrainState_e::ERROR); + } else if(inverters_in_torque_mode) + { + _set_enable_drivetrain(); + _set_state(DrivetrainState_e::ENABLED_TORQUE_MODE); + } + break; } + case DrivetrainState_e::ENABLED_SPEED_MODE: { + // now finally in this mode and the ENABLED_TORQUE_MODE can we command the drivetrain + bool user_requesting_speed_command = etl::holds_alternative(cmd); + + if(user_requesting_speed_command) + { + _set_drivetrain_speed_command(cmd.get()); + } else if(user_requesting_mode_switch) + { + + } break; } case DrivetrainState_e::ENABLED_TORQUE_MODE: @@ -170,6 +243,7 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) } case DrivetrainState_e::ERROR: { + break; } default: @@ -223,6 +297,7 @@ void DrivetrainSystem::_handle_entry_logic(DrivetrainState_e new_state) break; case DrivetrainState_e::INVERTERS_ENABLED: break; + case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: { // set the torque mode pin state to false (aka: keep the inverter in the main control mode which is speed mode) @@ -235,7 +310,7 @@ void DrivetrainSystem::_handle_entry_logic(DrivetrainState_e new_state) _set_gpio_state({true}); break; } - break; + case DrivetrainState_e::ENABLED_SPEED_MODE: break; case DrivetrainState_e::ENABLED_TORQUE_MODE: @@ -246,25 +321,6 @@ void DrivetrainSystem::_handle_entry_logic(DrivetrainState_e new_state) break; } } - -void DrivetrainSystem::_send_enable_drivetrain_hv() -{ - auto funcs_arr = _inverter_interfaces.as_array(); - for(const auto & func : funcs_arr) - { - func.set_enable_hv(); - } -} - -void DrivetrainSystem::_set_enable_drivetrain() -{ - auto funcs_arr = _inverter_interfaces.as_array(); - for(const auto & func : funcs_arr) - { - func.set_enable_inverter(); - } -} - // returns false if any of the inverters fail the flag check. bool DrivetrainSystem::_check_inverter_flags(std::function flag_check_func) { @@ -280,7 +336,7 @@ bool DrivetrainSystem::_check_inverter_flags(std::function Date: Wed, 22 Jan 2025 15:19:39 -0500 Subject: [PATCH 23/36] adding in drivetrain system --- .../include/VehicleStateMachine.h | 19 +- lib/state_machine/src/VehicleStateMachine.cpp | 85 +++++---- lib/systems/include/DrivetrainSystem.h | 43 +++-- lib/systems/include/DrivetrainSystem_old.h | 120 ------------- lib/systems/include/DrivetrainSystem_old.tpp | 106 ----------- lib/systems/src/DrivetrainSystem.cpp | 170 +++++++++++++++--- src/VCR_Tasks.cpp | 2 +- 7 files changed, 231 insertions(+), 314 deletions(-) delete mode 100644 lib/systems/include/DrivetrainSystem_old.h delete mode 100644 lib/systems/include/DrivetrainSystem_old.tpp diff --git a/lib/state_machine/include/VehicleStateMachine.h b/lib/state_machine/include/VehicleStateMachine.h index b57d9f0..c76c7e6 100644 --- a/lib/state_machine/include/VehicleStateMachine.h +++ b/lib/state_machine/include/VehicleStateMachine.h @@ -39,11 +39,12 @@ enum class CAR_STATE class VehicleStateMachine { public: - static VehicleStateMachine& getInstance() - { - static VehicleStateMachine instance; - return instance; - } + VehicleStateMachine(DrivetrainSystem & drivetrain_system) : + _current_state(CAR_STATE::STARTUP), + _drivetrain(drivetrain_system), + _buzzer(BuzzerController::getInstance()), + _safetysystem(SafetySystem::getInstance()) {}; + /** * This tick() function handles all the update logic for traversing states, and calls the functions @@ -58,12 +59,6 @@ class VehicleStateMachine CAR_STATE get_state() { return _current_state; } private: - VehicleStateMachine(DrivetrainSystem & drivetrain_system) : - _current_state(CAR_STATE::STARTUP), - _drivetrain(drivetrain_system), - _buzzer(BuzzerController::getInstance()), - _safetysystem(SafetySystem::getInstance()) {}; - void set_state_(CAR_STATE new_state, unsigned long curr_time); /** @@ -81,7 +76,7 @@ class VehicleStateMachine CAR_STATE _current_state; /* System references to show dependence on systems library */ - DrivetrainSystem &_drivetrain; //TODO: Make this InverterInterface instead of uint32_t + DrivetrainSystem &_drivetrain; //TODO: Make this InverterInterface instead of uint32_t BuzzerController &_buzzer; SafetySystem &_safetysystem; diff --git a/lib/state_machine/src/VehicleStateMachine.cpp b/lib/state_machine/src/VehicleStateMachine.cpp index 4d48c2e..50a284c 100644 --- a/lib/state_machine/src/VehicleStateMachine.cpp +++ b/lib/state_machine/src/VehicleStateMachine.cpp @@ -20,11 +20,11 @@ void VehicleStateMachine::tick_state_machine(unsigned long current_millis, const case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: { // if TS is above HV threshold, move to Tractive System Active - _drivetrain.disable_no_pins(); - if (_drivetrain.hv_over_threshold_on_drivetrain()) - { - set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); - } + // _drivetrain.disable_no_pins(); + // if (_drivetrain.hv_over_threshold_on_drivetrain()) + // { + // set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); + // } break; } @@ -35,13 +35,13 @@ void VehicleStateMachine::tick_state_machine(unsigned long current_millis, const _buzzer.deactivate(); } - _drivetrain.disable_no_pins(); + // _drivetrain.disable_no_pins(); - if (!_drivetrain.hv_over_threshold_on_drivetrain()) - { - set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - break; - } + // if (!_drivetrain.hv_over_threshold_on_drivetrain()) + // { + // set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + // break; + // } if (system_data.dash_input_state.start_btn_is_pressed && system_data.pedals_system_data.brake_is_pressed) { @@ -54,24 +54,25 @@ void VehicleStateMachine::tick_state_machine(unsigned long current_millis, const case CAR_STATE::ENABLING_INVERTERS: { + // TODO: replace old drivetrain function handling with new interaction paradigm // If HV is not active, go to TRACTIVE_SYSTEM_NOT_ACTIVE - if (_drivetrain.hv_over_threshold_on_drivetrain()) - { - set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - break; - } + // if (_drivetrain.hv_over_threshold_on_drivetrain()) + // { + // set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + // break; + // } // If motor controllers have error, but HV still active - if (_drivetrain.drivetrain_error_occured()) - { - set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); - } + // if (_drivetrain.drivetrain_error_occured()) + // { + // set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); + // } - if (_drivetrain.handle_inverter_startup(current_millis)) - { - set_state_(CAR_STATE::WAITING_READY_TO_DRIVE_SOUND, current_millis); - break; - } + // if (_drivetrain.handle_inverter_startup(current_millis)) + // { + // set_state_(CAR_STATE::WAITING_READY_TO_DRIVE_SOUND, current_millis); + // break; + // } break; } @@ -79,13 +80,13 @@ void VehicleStateMachine::tick_state_machine(unsigned long current_millis, const { // If HV is no longer active, return to TRACTIVE_SYSTEM_NOT_ACTIVE - if (_drivetrain.hv_over_threshold_on_drivetrain()) - { - set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - break; - } + // if (_drivetrain.hv_over_threshold_on_drivetrain()) + // { + // set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + // break; + // } - _drivetrain.command_drivetrain_no_torque(); // While waiting for RTD sound to complete, always command 0 torque + // _drivetrain.command_drivetrain_no_torque(); // While waiting for RTD sound to complete, always command 0 torque // If the ready-to-drive sound is done playing, move to ready to drive mode if (!_buzzer.buzzer_is_active(current_millis)) @@ -98,16 +99,18 @@ void VehicleStateMachine::tick_state_machine(unsigned long current_millis, const case CAR_STATE::READY_TO_DRIVE: { - + // If HV is no longer active, return to TRACTIVE_SYSTEM_NOT_ACTIVE - if (!_drivetrain.hv_over_threshold_on_drivetrain()) + + // TODO make this real: check to see if the state + if(_drivetrain.get_state() == DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT) { - hal_println("Drivetrain not over threshold while in READY_TO_DRIVE"); set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); break; } - - if (_drivetrain.drivetrain_error_occured()) + + bool drivetrain_in_driveable_mode = ((_drivetrain.get_state() == DrivetrainState_e::ENABLED_SPEED_MODE) || (_drivetrain.get_state() == DrivetrainState_e::ENABLED_TORQUE_MODE)); + if (!drivetrain_in_driveable_mode) { hal_println("Drivetrain error occurred while in READY_TO_DRIVE"); set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); @@ -117,12 +120,20 @@ void VehicleStateMachine::tick_state_machine(unsigned long current_millis, const if (_safetysystem.get_software_is_ok() && !system_data.pedals_system_data.implausibility_has_exceeded_max_duration) { // TODO: Fix with all references to singleton classes + // TODO: need to also handle request to mode switch via drivetrain init (?) // _drivetrain.command_drivetrain(controller_mux_->getDrivetrainCommand(dashboard_->getDialMode(), dashboard_->getTorqueLimitMode(), current_car_state)); + DrivetrainTorqueCommand_s example_cmd = {}; // will need to do a variant check from the controller mux to see what type it is (torque / speed) + + auto status = _drivetrain.evaluate_drivetrain(example_cmd); + } else { // If software is not OK or some implausibility has exceeded max duration, command 0 torque (but stay in RTD mode) - _drivetrain.command_drivetrain_no_torque(); + + // TODO: make this check to see exactly what drive mode the drivetrain is in and send its associated empty command. + DrivetrainTorqueCommand_s example_cmd = {}; // will need to do a variant check from the controller mux to see what type it is (torque / speed) + auto status = _drivetrain.evaluate_drivetrain(example_cmd); } break; diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 14e7562..dea4ad2 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -97,53 +97,72 @@ struct DrivetrainStatus_s DrivetrainCmdResponse_e cmd_resp; }; +struct DrivetrainResetError_s +{ + bool reset_errors; // true: reset the errors present on inverters, false: dont +}; + // output pin of micro, read by inverters struct DrivetrainOutputPins_s { bool torque_mode_pin_state : 1; }; - // the pin set by the inverters themselves ("input": pin being read by micro) struct DrivetrainInputPins_s { bool torque_mode_enabled_pin_state : 1; }; +struct InverterControlWord_s +{ + bool inverter_enable; + bool hv_enable; + bool driver_enable; + bool remove_error; +}; + class DrivetrainSystem { public: - using CmdVariant = etl::variant; + using CmdVariant = etl::variant; DrivetrainSystem() = delete; - + DrivetrainStatus_s evaluate_drivetrain(CmdVariant cmd); DrivetrainState_e get_state(); // DrivetrainDynamicReport_s get_dynamic_data(); struct InverterFuncts { std::function set_speed; - std::function set_torque; - std::function set_disable_inverter; - std::function set_no_torque; - std::function reset_inverter; - std::function set_enable_inverter; - std::function set_enable_hv; + std::function set_torque; + std::function set_idle; + std::function set_inverter_control_word; std::function get_status; }; + DrivetrainSystem(veh_vec inverter_interfaces); private: bool _check_inverter_flags(std::function flag_check_func); + bool _drivetrain_active(float max_active_rpm); + + void _set_state(DrivetrainState_e state); void _handle_exit_logic(DrivetrainState_e prev_state); void _handle_entry_logic(DrivetrainState_e new_state); void _set_drivetrain_disabled(); + void _set_drivetrain_keepalive_idle(); void _set_enable_drivetrain_hv(); void _set_enable_drivetrain(); - + void _set_drivetrain_error_reset(); + void _set_drivetrain_speed_command(DrivetrainSpeedCommand_s cmd); + void _set_drivetrain_torque_command(DrivetrainTorqueCommand_s cmd); + + DrivetrainState_e _evaluate_state_machine(CmdVariant cmd); + private: - DrivetrainState_e _state; veh_vec _inverter_interfaces; + DrivetrainState_e _state; std::function _check_inverter_ready_flag; std::function _check_inverter_connected_flag; std::function _check_inverter_quit_dc_flag; @@ -166,8 +185,6 @@ class DrivetrainSystem unsigned long _last_reset_pressed_time; unsigned long _reset_interval; - // final check for drivetrain initialization to check if quit inverter on - bool drivetrain_enabled(); unsigned long _curr_system_millis; unsigned int _min_cmd_period; diff --git a/lib/systems/include/DrivetrainSystem_old.h b/lib/systems/include/DrivetrainSystem_old.h deleted file mode 100644 index 93aad9f..0000000 --- a/lib/systems/include/DrivetrainSystem_old.h +++ /dev/null @@ -1,120 +0,0 @@ -#ifndef DRIVETRAINSYSTEM -#define DRIVETRAINSYSTEM - -#include "etl/singleton.h" - -#include "SharedFirmwareTypes.h" -#include -#include "stdint.h" -#include "SysClock.h" - -enum class DRIVETRAIN_STATE -{ - TRACTIVE_SYSTEM_NOT_ACTIVE = 0, - TRACTIVE_SYSTEM_ACTIVE = 1, - ENABLING_INVERTERS = 2, - ENABLED = 3, - ERROR = 4 -}; - -// #include "DrivetrainSystemStateMachine.h" -/** - * As of now, only the minimum functions for VehicleStateMachine to compile have been implemented. - * TODO: Re-add the rest of the necessary functions - * TODO: Add DrivetrainSystem.tpp to implement all functions - */ -template -class DrivetrainSystem -{ -public: - DrivetrainSystem(); - - void tick(const SysTick_s &tick); - - void setup_retry() - { - reset_drivetrain(); - _hv_en_requested = false; - _enable_requested = false; - } - - bool handle_inverter_startup(unsigned long curr_time); - - // on entry logic - void command_drivetrain_no_torque(); - void command_drivetrain_debug(); - - // check to see if init time limit has passed - bool inverter_init_timeout(unsigned long curr_time); - - bool hv_over_threshold_on_drivetrain(); - void disable(); - void disable_no_pins(); - bool drivetrain_error_occured(); - void reset_drivetrain(); - // void command_drivetrain(const DrivetrainCommand_s &data); - - void enable_drivetrain_reset(); - void check_reset_condition(); - - // DrivetrainDynamicReport_s get_dynamic_data(); - -private: - // DrivetrainSystem(const std::array &inverters, MCUInterface *mcu_interface, int init_time_limit_ms, uint16_t min_hv_voltage = 60, int min_cmd_period_ms = 1, float max_torque_setpoint_nm = 21.42) - // : inverters_(inverters), init_time_limit_ms_(init_time_limit_ms), min_hv_voltage_(min_hv_voltage), min_cmd_period_(min_cmd_period_ms), max_torque_setpoint_nm_(max_torque_setpoint_nm) - // { - // // values from: https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/fse/motor_data_sheet_a2370dd_dd5.pdf - // motor_pole_pairs_ = 5; - // lambda_magnetic_flux_wb_ = 1.0; - // hv_en_requested_ = false; - // enable_requested_ = false; - // reset_requested_ = false; - // last_reset_pressed_time_ = 0; - // reset_interval_ = 5000; // ms - // curr_system_millis_ = 0; - // last_no_torque_cmd_time_ = 0; - // last_reset_cmd_time_ = 0; - // last_disable_cmd_time_ = 0; - // last_general_cmd_time_ = 0; // ms - // mcu_interface_ = mcu_interface; - // dynamic_data_ = {}; - // } - - std::array _inverters; - // MCUInterface *mcu_interface_; - int _init_time_limit_ms; - uint16_t _min_hv_voltage; - int _motor_pole_pairs; - float _lambda_magnetic_flux_wb, _L_d_inductance_H; - // startup statuses: - bool _hv_en_requested, _enable_requested; - // reset inverters - bool _reset_requested; - unsigned long _last_reset_pressed_time; - unsigned long _reset_interval; - /// @param curr_time current system tick time (millis()) that sets the init phase start time - void enable_drivetrain_hv(unsigned long curr_time); - void request_enable(); - // startup phase 1 - // status check for start of enable - bool drivetrain_ready(); - // startup phase 2 - bool check_drivetrain_quit_dc_on(); - - // final check for drivetrain initialization to check if quit inverter on - bool drivetrain_enabled(); - - unsigned long _curr_system_millis; - unsigned int _min_cmd_period; - unsigned long _last_no_torque_cmd_time, last_reset_cmd_time, last_disable_cmd_time, last_general_cmd_time; - - unsigned long _drivetrain_initialization_phase_start_time; - // DrivetrainCommand_s current_drivetrain_command_; - // DrivetrainDynamicReport_s dynamic_data_; - float _max_torque_setpoint_nm; -}; - -#include "DrivetrainSystem_old.tpp" - -// using DrivetrainSystemSingleton<> = etl::singleton>; -#endif /* DRIVETRAINSYSTEM */ \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem_old.tpp b/lib/systems/include/DrivetrainSystem_old.tpp deleted file mode 100644 index fabf4ae..0000000 --- a/lib/systems/include/DrivetrainSystem_old.tpp +++ /dev/null @@ -1,106 +0,0 @@ -#include "DrivetrainSystem_old.h" - -template -void DrivetrainSystem::tick(const SysTick_s &tick) -{ - -} - -template -bool DrivetrainSystem::inverter_init_timeout(unsigned long curr_time) -{ - return false; -} - -/*----------------------------------------------------------------------------------------*/ -// async command functions -/*----------------------------------------------------------------------------------------*/ -template -bool DrivetrainSystem::handle_inverter_startup(unsigned long curr_time) -{ - return false; -} - -template -void DrivetrainSystem::enable_drivetrain_hv(unsigned long curr_time) -{ - -} - -template -void DrivetrainSystem::request_enable() -{ - -} -/*----------------------------------------------------------------------------------------*/ -// rate limited commands. we will only be commanding one of these at a time. -/*----------------------------------------------------------------------------------------*/ -template -void DrivetrainSystem::command_drivetrain_debug() -{ - -} - -template -void DrivetrainSystem::command_drivetrain_no_torque() -{ - -} - - - -template -void DrivetrainSystem::check_reset_condition() -{ - -} - -template -void DrivetrainSystem::reset_drivetrain() -{ - -} - -template -void DrivetrainSystem::disable_no_pins() -{ - -} - -template -void DrivetrainSystem::disable() -{ - -} - -/*----------------------------------------------------------------------------------------*/ -// feedback functions -/*----------------------------------------------------------------------------------------*/ -template -bool DrivetrainSystem::drivetrain_error_occured() -{ - return false; -} - -template -bool DrivetrainSystem::hv_over_threshold_on_drivetrain() -{ - return false; -} -template -bool DrivetrainSystem::drivetrain_ready() -{ - return false; -} - -template -bool DrivetrainSystem::check_drivetrain_quit_dc_on() -{ - return false; -} - -template -bool DrivetrainSystem::drivetrain_enabled() -{ - return false; -} \ No newline at end of file diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index 05a8443..8474657 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -1,7 +1,6 @@ #include -//- [ ] TODO handle inverter keepalives with correct settings of inverter flags for their associated states -//- [ ] TODO handle +//- [x] TODO handle inverter keepalives with correct settings of inverter flags for their associated states DrivetrainSystem::DrivetrainSystem( veh_vec inverter_interfaces) @@ -32,24 +31,27 @@ DrivetrainStatus_s DrivetrainSystem::evaluate_drivetrain(DrivetrainSystem::CmdVa _inverter_interfaces.RL.get_status(), _inverter_interfaces.RR.get_status() }; - bool attempting_init_while_not_connected = ((state == DrivetrainState_e::NOT_CONNECTED) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED)); + bool attempting_init_while_not_connected = ((state == DrivetrainState_e::NOT_CONNECTED) && (etl::get(cmd).init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED)); if (attempting_init_while_not_connected) { status.cmd_resp = DrivetrainCmdResponse_e::CANNOT_INIT_NOT_CONNECTED; + } else + { + status.cmd_resp = DrivetrainCmdResponse_e::COMMAND_OK; } return status; } -void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) +DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) { switch(get_state()) { // TODO need to ensure that the inverter outputs CAN messages on idle even not when being sent msgs case DrivetrainState_e::NOT_CONNECTED: { - bool connected_no_hv_present = (_check_inverter_flags(_check_inverter_connected_flag) && _check_inverter_flags(_check_inverter_hv_not_present_flag) ) + bool connected_no_hv_present = (_check_inverter_flags(_check_inverter_connected_flag) && _check_inverter_flags(_check_inverter_hv_not_present_flag) ); bool connected_hv_present = (_check_inverter_flags(_check_inverter_connected_flag) && _check_inverter_flags(_check_inverter_hv_present_flag)); if(connected_no_hv_present) @@ -60,7 +62,7 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) _set_state(DrivetrainState_e::NOT_ENABLED_HV_PRESENT); } - _keepalive_disabled(); // TODO dont know if this should be sent here, but it shouldn't hurt + _set_drivetrain_disabled(); // TODO dont know if this should be sent here, but it shouldn't hurt break; } @@ -70,7 +72,7 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) { _set_state(DrivetrainState_e::NOT_ENABLED_HV_PRESENT); } - _keepalive_disabled(); + _set_drivetrain_disabled(); break; } @@ -86,7 +88,7 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) { _set_state(DrivetrainState_e::INVERTERS_READY); } - _keepalive_disabled(); + _set_drivetrain_disabled(); break; } @@ -95,7 +97,7 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); // in this state, we are requesting inverters to enable HV - bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); + bool requesting_init = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); bool inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); @@ -126,7 +128,7 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool requesting_init = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); + bool requesting_init = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); bool inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); @@ -140,7 +142,7 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) else if(requesting_init && hv_enabled && inverters_ready && hv_enabled && !inverters_enabled) { _set_enable_drivetrain(); // should be done on entry of this state - } else if(hv_enabled && inverters_ready && hv_enabled && inverters_enabled); + } else if(hv_enabled && inverters_ready && hv_enabled && inverters_enabled) { _set_enable_drivetrain(); _set_state(DrivetrainState_e::INVERTERS_ENABLED); @@ -160,8 +162,8 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) { bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool requesting_speed_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); - bool requesting_torque_mode = etl::holds_alternative(cmd) && (cmd.get().init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); + bool requesting_speed_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); + bool requesting_torque_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); // for now i wont worry about checking whether or not the inverters are still enabled. // bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); @@ -225,25 +227,71 @@ void DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::CmdVariant cmd) case DrivetrainState_e::ENABLED_SPEED_MODE: { + if(!(_check_inverter_flags(_check_inverter_hv_present_flag))) + { + _set_state(DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT); + break; + } + + // TODO may need to verify that the gpio state is correct while in this state (GPIO torque mode low) + bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); // now finally in this mode and the ENABLED_TORQUE_MODE can we command the drivetrain bool user_requesting_speed_command = etl::holds_alternative(cmd); + bool user_requesting_torque_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); + + - if(user_requesting_speed_command) + if(inverter_error_present) { - _set_drivetrain_speed_command(cmd.get()); - } else if(user_requesting_mode_switch) + _set_drivetrain_disabled(); + _set_state(DrivetrainState_e::ERROR); + break; + } + else if(user_requesting_speed_command) { - + _set_drivetrain_speed_command(etl::get(cmd)); + } else if(user_requesting_torque_mode && !_drivetrain_active(100)) + { + _set_drivetrain_keepalive_idle(); + _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); } break; } case DrivetrainState_e::ENABLED_TORQUE_MODE: { + // TODO may need to verify that the gpio state is correct while in this state (GPIO torque mode high) + bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + + bool user_requesting_torque_command = etl::holds_alternative(cmd); + bool user_requesting_speed_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); + + if(inverter_error_present) + { + _set_drivetrain_disabled(); + _set_state(DrivetrainState_e::ERROR); + } + else if(user_requesting_torque_command) + { + _set_drivetrain_torque_command(etl::get(cmd)); + } else if(user_requesting_speed_mode && !_drivetrain_active(100)) + { + _set_drivetrain_keepalive_idle(); + _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); + } break; } case DrivetrainState_e::ERROR: { - + bool user_requesting_error_reset = etl::holds_alternative(cmd) && (etl::get(cmd).reset_errors); + bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + + if(user_requesting_error_reset && inverter_error_present) + { + _set_drivetrain_error_reset(); + } else if(user_requesting_error_reset && (!inverter_error_present)) + { + _set_state(DrivetrainState_e::NOT_ENABLED_HV_PRESENT); + } break; } default: @@ -265,15 +313,25 @@ void DrivetrainSystem::_handle_exit_logic(DrivetrainState_e prev_state) switch (prev_state) { case DrivetrainState_e::NOT_CONNECTED: + break; case DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT: + break; case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: + break; case DrivetrainState_e::INVERTERS_READY: + break; case DrivetrainState_e::INVERTERS_HV_ENABLED: + break; case DrivetrainState_e::INVERTERS_ENABLED: + break; case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: + break; case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: + break; case DrivetrainState_e::ENABLED_SPEED_MODE: + break; case DrivetrainState_e::ENABLED_TORQUE_MODE: + break; case DrivetrainState_e::ERROR: break; default: @@ -338,32 +396,94 @@ bool DrivetrainSystem::_check_inverter_flags(std::function= max_active_rpm) + { + return true; + } + } + return false; +} \ No newline at end of file diff --git a/src/VCR_Tasks.cpp b/src/VCR_Tasks.cpp index 79211a6..57b7bc0 100644 --- a/src/VCR_Tasks.cpp +++ b/src/VCR_Tasks.cpp @@ -69,7 +69,7 @@ HT_TASK::Task read_adc0_task = HT_TASK::Task(init_read_adc0_task, run_read_adc0_ bool run_tick_state_machine_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) { - VehicleStateMachine::getInstance().tick_state_machine(sysMicros / 1000, system_data); // tick function requires millis //NOLINT + // VehicleStateMachine::getInstance().tick_state_machine(sysMicros / 1000, system_data); // tick function requires millis //NOLINT return true; } From e542af4eedc848128aa8ca3232afbbcf2bc55220 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 22 Jan 2025 15:52:57 -0500 Subject: [PATCH 24/36] addressing linter errors --- lib/interfaces/include/InverterInterface.h | 4 +- lib/systems/include/DrivetrainSystem.h | 21 +----- lib/systems/src/DrivetrainSystem.cpp | 79 ++++++++++++++-------- 3 files changed, 54 insertions(+), 50 deletions(-) diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index 8c8e86c..c51c1c2 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -1,5 +1,5 @@ -#ifndef __INVERTERINTERFACE_H__ -#define __INVERTERINTERFACE_H__ +#ifndef INVERTERINTERFACE_H +#define INVERTERINTERFACE_H #include namespace HTUnits diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index dea4ad2..6f901bc 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -161,6 +161,7 @@ class DrivetrainSystem DrivetrainState_e _evaluate_state_machine(CmdVariant cmd); private: + const float _active_rpm_level = 100; veh_vec _inverter_interfaces; DrivetrainState_e _state; std::function _check_inverter_ready_flag; @@ -174,26 +175,6 @@ class DrivetrainSystem std::function _set_gpio_state; std::function _get_gpio_state; - int _init_time_limit_ms; - uint16_t _min_hv_voltage; - int _motor_pole_pairs; - float _lambda_magnetic_flux_wb, _L_d_inductance_H; - // startup statuses: - bool _hv_en_requested, _enable_requested; - // reset inverters - bool _reset_requested; - unsigned long _last_reset_pressed_time; - unsigned long _reset_interval; - - - unsigned long _curr_system_millis; - unsigned int _min_cmd_period; - unsigned long _last_no_torque_cmd_time, last_reset_cmd_time, last_disable_cmd_time, last_general_cmd_time; - - unsigned long _drivetrain_initialization_phase_start_time; - // DrivetrainCommand_s current_drivetrain_command_; - // DrivetrainDynamicReport_s dynamic_data_; - float _max_torque_setpoint_nm; }; #endif /* DRIVETRAINSYSTEM */ \ No newline at end of file diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index 8474657..b3ce969 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -51,8 +51,10 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm // TODO need to ensure that the inverter outputs CAN messages on idle even not when being sent msgs case DrivetrainState_e::NOT_CONNECTED: { - bool connected_no_hv_present = (_check_inverter_flags(_check_inverter_connected_flag) && _check_inverter_flags(_check_inverter_hv_not_present_flag) ); - bool connected_hv_present = (_check_inverter_flags(_check_inverter_connected_flag) && _check_inverter_flags(_check_inverter_hv_present_flag)); + bool connected_no_hv_present = false; + connected_no_hv_present = (_check_inverter_flags(_check_inverter_connected_flag) && _check_inverter_flags(_check_inverter_hv_not_present_flag) ); + bool connected_hv_present = false; + connected_hv_present = (_check_inverter_flags(_check_inverter_connected_flag) && _check_inverter_flags(_check_inverter_hv_present_flag)); if(connected_no_hv_present) { @@ -78,7 +80,8 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: { - bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverter_error_present = false; + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); if(inverter_error_present) { _set_state(DrivetrainState_e::ERROR); @@ -94,12 +97,16 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm case DrivetrainState_e::INVERTERS_READY: { - bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverter_error_present = false; + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); // in this state, we are requesting inverters to enable HV - bool requesting_init = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); - bool inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); - bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); + bool requesting_init = false; + requesting_init = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); + bool inverters_ready = false; + inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); + bool hv_enabled = false; + hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); if(inverter_error_present) { @@ -126,13 +133,18 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm // - [x] if the user stops requesting init -> go back to inverters ready state // - [x] error present on an inverter -> go to error state - bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverter_error_present = false; + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool requesting_init = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); + bool requesting_init = false; + requesting_init = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain != DrivetrainModeRequest_e::UNINITIALIZED); - bool hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); - bool inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); - bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); + bool hv_enabled = false; + hv_enabled = _check_inverter_flags(_check_inverter_quit_dc_flag); + bool inverters_ready = false; + inverters_ready = _check_inverter_flags(_check_inverter_ready_flag); + bool inverters_enabled = false; + inverters_enabled = _check_inverter_flags(_check_inverter_enabled); if(inverter_error_present) { @@ -160,10 +172,13 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm case DrivetrainState_e::INVERTERS_ENABLED: { - bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverter_error_present = false; + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool requesting_speed_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); - bool requesting_torque_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); + bool requesting_speed_mode = false; + requesting_speed_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); + bool requesting_torque_mode = false; + requesting_torque_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); // for now i wont worry about checking whether or not the inverters are still enabled. // bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); @@ -191,8 +206,10 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: { - bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool inverters_in_speed_mode = (!_get_gpio_state().torque_mode_enabled_pin_state); + bool inverter_error_present = false; + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverters_in_speed_mode = false; + inverters_in_speed_mode = (!_get_gpio_state().torque_mode_enabled_pin_state); if(inverter_error_present) { @@ -209,8 +226,10 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: { - bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool inverters_in_torque_mode = _get_gpio_state().torque_mode_enabled_pin_state; + bool inverter_error_present = false; + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverters_in_torque_mode = false; + inverters_in_torque_mode = _get_gpio_state().torque_mode_enabled_pin_state; if(inverter_error_present) { @@ -234,13 +253,14 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm } // TODO may need to verify that the gpio state is correct while in this state (GPIO torque mode low) - bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverter_error_present = false; + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); // now finally in this mode and the ENABLED_TORQUE_MODE can we command the drivetrain - bool user_requesting_speed_command = etl::holds_alternative(cmd); - bool user_requesting_torque_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); - + bool user_requesting_speed_command = false; + user_requesting_speed_command = etl::holds_alternative(cmd); + bool user_requesting_torque_mode = false; + user_requesting_torque_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); - if(inverter_error_present) { _set_drivetrain_disabled(); @@ -250,7 +270,8 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm else if(user_requesting_speed_command) { _set_drivetrain_speed_command(etl::get(cmd)); - } else if(user_requesting_torque_mode && !_drivetrain_active(100)) + + } else if(user_requesting_torque_mode && !_drivetrain_active(_active_rpm_level)) { _set_drivetrain_keepalive_idle(); _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); @@ -260,7 +281,8 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm case DrivetrainState_e::ENABLED_TORQUE_MODE: { // TODO may need to verify that the gpio state is correct while in this state (GPIO torque mode high) - bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverter_error_present = false; + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); bool user_requesting_torque_command = etl::holds_alternative(cmd); bool user_requesting_speed_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); @@ -273,7 +295,7 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm else if(user_requesting_torque_command) { _set_drivetrain_torque_command(etl::get(cmd)); - } else if(user_requesting_speed_mode && !_drivetrain_active(100)) + } else if(user_requesting_speed_mode && !_drivetrain_active(_active_rpm_level)) { _set_drivetrain_keepalive_idle(); _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); @@ -283,7 +305,8 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm case DrivetrainState_e::ERROR: { bool user_requesting_error_reset = etl::holds_alternative(cmd) && (etl::get(cmd).reset_errors); - bool inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + bool inverter_error_present = false; + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); if(user_requesting_error_reset && inverter_error_present) { From e1d09321cf04e35618c90e0029b7fd17f0bcad35 Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Sat, 25 Jan 2025 22:42:56 -0500 Subject: [PATCH 25/36] Add Inverter Interface --- lib/interfaces/include/HytechCANInterface.h | 5 + lib/interfaces/include/InverterInterface.h | 112 ++++++++++++++++-- lib/interfaces/include/MessageQueueDefine.h | 8 ++ lib/interfaces/src/InverterInterface.cpp | 122 ++++++++++++++++++++ platformio.ini | 12 +- 5 files changed, 250 insertions(+), 9 deletions(-) create mode 100644 lib/interfaces/include/HytechCANInterface.h create mode 100644 lib/interfaces/include/MessageQueueDefine.h create mode 100644 lib/interfaces/src/InverterInterface.cpp diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h new file mode 100644 index 0000000..46a8b33 --- /dev/null +++ b/lib/interfaces/include/HytechCANInterface.h @@ -0,0 +1,5 @@ +#ifndef HYTECHCANINTERFACE +#define HYTECHCANINTERFACE + + +#endif /* HYTECHCANINTERFACE */ \ No newline at end of file diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index c51c1c2..52edeb3 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -2,6 +2,11 @@ #define INVERTERINTERFACE_H #include +#include "FlexCAN_T4.h" +#include "MessageQueueDefine.h" + +#include + namespace HTUnits { using celcius = float; @@ -12,8 +17,42 @@ namespace HTUnits using volts = float; }; -// for the most part these are mirrors of the lower-level CAN struct data, except with already fully float-ized data +/** + * Struct containing id info for this specific inverter interface + */ +struct InverterIds_s +{ + uint32_t mc_energy_id; + uint32_t mc_status_id; + uint32_t mc_temps_id; + uint32_t mc_setpoint_commands_id; + uint32_t mc_torque_command_id; +}; + +/** + * Drivetrain system accessible structs for + * requesting change of state + */ +struct InverterSetpoints_s +{ + bool inverter_enable : 1; + bool hv_enable : 1; + bool driver_enable : 1; + bool remove_error : 1; + int16_t speed_rpm_setpoint; + int16_t positive_torque_limit; + int16_t negative_torque_limit; +}; +struct InverterTorqueCommand_s +{ + uint16_t torque_command; +}; + +/** + * For the most part these are mirrors of the lower-level CAN struct data, + * except with already fully float-ized data +**/ struct InverterStatus_s { bool system_ready : 1; @@ -64,13 +103,72 @@ struct InverterFeedbackData_s InverterControlParams_s control_params; }; -// struct InverterMotorControl_s -// { - -// } - class InverterInterface { + public: + + InverterInterface( + CANBufferType msg_queue, + uint32_t mc_energy_id, + uint32_t mc_status_id, + uint32_t mc_temps_id, + uint32_t mc_setpoint_commands_id, + uint32_t mc_torque_command_id) : msg_queue_(msg_queue) + { + inverter_ids.mc_energy_id = mc_energy_id; + inverter_ids.mc_status_id = mc_status_id; + inverter_ids.mc_temps_id = mc_temps_id; + inverter_ids.mc_setpoint_commands_id = mc_setpoint_commands_id; + inverter_ids.mc_torque_command_id = mc_torque_command_id; + } + + /* Request change of state */ + void set_inverter_setpoints( + bool inverter_enable, + bool hv_enable, + bool driver_enable, + bool remove_error, + int16_t speed_rpm_setpoint, + int16_t positive_torque_limit, + int16_t negative_torque_limit + ); + + void set_torque_command( + uint16_t torque_command + ); + + + private: + + InverterIds_s inverter_ids; + + InverterSetpoints_s _inverter_setpoints; + InverterTorqueCommand_s _inverter_torque_command; + + InverterStatus_s _inverter_status; + InverterTemps_s _inverter_temps; + InverterPower_s _inverter_power; + InverterControlParams_s _control_params; + InverterFeedbackData_s _feedback_data; + + /* Recieving callbacks */ + void recieve_MC_ENERGY(CAN_message_t &can_msg); + + void recieve_MC_STATUS(CAN_message_t &can_msg); + + void recieve_MC_TEMPS(CAN_message_t &can_msg); + + /* Sending */ + template + void enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *), uint32_t id); + + void send_MC_SETPOINT_COMMAND(); + + void send_MC_TORQUE_COMMAND(); + + CANBufferType msg_queue_; + }; -#endif // __INVERTERINTERFACE_H__ \ No newline at end of file +#endif // __INVERTERINTERFACE_H__ + diff --git a/lib/interfaces/include/MessageQueueDefine.h b/lib/interfaces/include/MessageQueueDefine.h new file mode 100644 index 0000000..8c07056 --- /dev/null +++ b/lib/interfaces/include/MessageQueueDefine.h @@ -0,0 +1,8 @@ +#ifndef MESAGE_QUEUE_DEFINE_H +#define MESAGE_QUEUE_DEFINE_H + +#include "FlexCAN_T4.h" + +using CANBufferType = Circular_Buffer; + +#endif \ No newline at end of file diff --git a/lib/interfaces/src/InverterInterface.cpp b/lib/interfaces/src/InverterInterface.cpp new file mode 100644 index 0000000..41ccfb6 --- /dev/null +++ b/lib/interfaces/src/InverterInterface.cpp @@ -0,0 +1,122 @@ +#include + +// TODO Parameterize this so it works for all motor controllers + +/** + * Request change of state + */ + +void InverterInterface::set_inverter_setpoints( + bool inverter_enable, + bool hv_enable, + bool driver_enable, + bool remove_error, + int16_t speed_rpm_setpoint, + int16_t positive_torque_limit, + int16_t negative_torque_limit +) +{ + _inverter_setpoints.inverter_enable = inverter_enable; + _inverter_setpoints.hv_enable = hv_enable; + _inverter_setpoints.driver_enable = driver_enable; + _inverter_setpoints.remove_error = remove_error; + _inverter_setpoints.speed_rpm_setpoint = speed_rpm_setpoint; + _inverter_setpoints.positive_torque_limit = positive_torque_limit; + _inverter_setpoints.negative_torque_limit = negative_torque_limit; +} + +void InverterInterface::set_torque_command(uint16_t torque_command) +{ + _inverter_torque_command.torque_command = torque_command; +} + + +/** + * Recieving CAN messages + */ +void InverterInterface::recieve_MC_ENERGY(CAN_message_t &can_msg) +{ + // Unpack the message + MC1_ENERGY_t unpacked_msg; + Unpack_MC1_ENERGY_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + + // Update inverter interface with new data + _inverter_status.dc_bus_voltage = unpacked_msg.dc_bus_voltage; + _feedback_data.motor_mechanics.actual_torque = unpacked_msg.feedback_torque; + _feedback_data.motor_mechanics.actual_power = unpacked_msg.motor_power; + +} + +void InverterInterface::recieve_MC_STATUS(CAN_message_t &can_msg) +{ + + // Unpack the message + MC1_STATUS_t unpacked_msg; + Unpack_MC1_STATUS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + + // Update inverter interface with new data + _inverter_status.system_ready = unpacked_msg.system_ready; + _inverter_status.error = unpacked_msg.error; + _inverter_status.warning = unpacked_msg.warning; + _inverter_status.quit_dc_on = unpacked_msg.quit_dc_on; + _inverter_status.dc_on = unpacked_msg.dc_on; + _inverter_status.quit_inverter_on = unpacked_msg.quit_inverter_on; + _inverter_status.derating_on = unpacked_msg.derating_on; + +} + +void InverterInterface::recieve_MC_TEMPS(CAN_message_t &can_msg) +{ + + // Unpack the message + MC1_TEMPS_t unpacked_msg; + Unpack_MC1_TEMPS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + + // Update inverter interface with new data + _inverter_status.diagnostic_number = unpacked_msg.diagnostic_number; + _inverter_temps.igbt_temp = HYTECH_igbt_temp_ro_fromS(unpacked_msg.igbt_temp_ro); + _inverter_temps.inverter_temp = HYTECH_inverter_temp_ro_fromS(unpacked_msg.inverter_temp_ro); + _inverter_temps.motor_temp = HYTECH_motor_temp_ro_fromS(unpacked_msg.motor_temp_ro); + +} + + +/** + * Sending CAN messages + */ + +template +void InverterInterface::enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *), uint32_t id) +{ + CAN_message_t can_msg; + pack_function(structure, can_msg.buf, &can_msg.len, (uint8_t *)&can_msg.flags.extended); + can_msg.id = id; + uint8_t buf[sizeof(CAN_message_t)] = {}; + memmove(buf, &can_msg, sizeof(CAN_message_t)); + msg_queue_->push_back(buf, sizeof(CAN_message_t)); +} + +void InverterInterface::send_MC_SETPOINT_COMMAND() +{ + MC1_SETPOINTS_COMMAND_t msg_out; + + msg_out.inverter_enable = _inverter_setpoints.inverter_enable; + msg_out.hv_enable = _inverter_setpoints.hv_enable; + msg_out.driver_enable = _inverter_setpoints.driver_enable; + msg_out.remove_error = _inverter_setpoints.remove_error; + msg_out.speed_setpoint_rpm = _inverter_setpoints.speed_rpm_setpoint; + msg_out.positive_torque_limit_ro = _inverter_setpoints.positive_torque_limit; + msg_out.negative_torque_limit_ro = _inverter_setpoints.negative_torque_limit; + + enqueue_new_CAN(&msg_out, &Pack_MC1_SETPOINTS_COMMAND_hytech, inverter_ids.mc_setpoint_commands_id); +} + +void InverterInterface::send_MC_TORQUE_COMMAND() +{ + MC1_TORQUE_COMMAND_t msg_out; + + msg_out.torque_command_ro = HYTECH_torque_command_ro_toS(_inverter_torque_command.torque_command); + + enqueue_new_CAN(&msg_out, &Pack_MC1_TORQUE_COMMAND_hytech, inverter_ids.mc_torque_command_id); +} + diff --git a/platformio.ini b/platformio.ini index 9b3b5c8..e325ac3 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,7 +8,7 @@ lib_deps_shared = https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 - https://github.com/hytech-racing/shared_firmware_types.git#feature/veh_vec_addition + https://github.com/hytech-racing/shared_firmware_types.git ; ../shared_firmware_types https://github.com/ssilverman/QNEthernet#v0.26.0 https://github.com/hytech-racing/HT_SCHED @@ -18,6 +18,10 @@ lib_deps_shared = ; * Build to verify the on-car software. ; * UPLOAD to compile and upload on-car software. ; * TEST is not yet configured. + +[platformio] +default_envs = teensy41 + [env:teensy41] check_tool = clangtidy check_flags = @@ -45,6 +49,9 @@ test_ignore = lib_deps = ${common.lib_deps_shared} https://github.com/hytech-racing/shared_firmware_interfaces.git + https://github.com/RCMast3r/hytech_can#testing_new_inv_ids + https://github.com/hytech-racing/HT_CAN/releases/download/127/can_lib.tar.gz + ; Test Systems Environment. This is only for compiling and uploading the hardware-abstracted code. ; * BUILD to verify the SYSTEMS compile. @@ -65,4 +72,5 @@ lib_ignore = test_ignore= test_interfaces* lib_deps = - ${common.lib_deps_shared} \ No newline at end of file + ${common.lib_deps_shared} + https://github.com/hytech-racing/HT_CAN/releases/download/127/can_lib.tar.gz \ No newline at end of file From dfead07341acfd24cdf2ffef6b36957f07ef03b7 Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Sun, 26 Jan 2025 03:10:05 -0500 Subject: [PATCH 26/36] first stab at CAN interface --- lib/interfaces/include/HytechCANInterface.h | 102 ++++++++++++++++++++ platformio.ini | 2 + 2 files changed, 104 insertions(+) diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 46a8b33..6cdbc44 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -1,5 +1,107 @@ #ifndef HYTECHCANINTERFACE #define HYTECHCANINTERFACE +#include +#include + +template +class HytechCANInterface { + public: + + HytechCANInterface() { } + + /** + * Registers CAN id to the handlers delegate + */ + template + void register_handler(void (callback)(CAN_message_t &msg)) + { + if constexpr ((can_id == can_ids) || ...) { + handlers[CAN_ID] = etl::delegate::create(callback); + } else { + static_assert(((can_id ==can_ids) || ...), "CAN id not registered.") + } + } + + /** + * Dispatches CAN messages from the specified buffer + * (Should be called in a loop) + */ + template + void dispatch_buffer(bufferType &rx_buffer) + { + while (rx_buffer.available()) + { + CAN_message_t recvd_msg; + uint8_t buf[sizeof(CAN_message_t)]; + rx_buffer.pop_front(buf, sizeof(CAN_message_t)); + memmove(&recvd_msg, buf, sizeof(recvd_msg)); + dispatch_msg(recvd_msg.id); + } + } + + /** + * Sending CAN messages + * (needs to be called in loop) + * */ + template + void send_all_CAN_msgs(bufferType &buffer, FlexCAN_T4_Base *can_interface) + { + CAN_message_t msg; + while (buffer.available()) + { + CAN_message_t msg; + uint8_t buf[sizeof(CAN_message_t)]; + buffer.pop_front(buf, sizeof(CAN_message_t)); + memmove(&msg, buf, sizeof(msg)); + can_interface->write(msg); + } + } + + /** + * Recieve Buffers + */ + + /* Recieve buffer for CAN 1 */ + Circular_Buffer CAN1_rxBuffer; + + /* Recieve buffer for CAN 2 */ + Circular_Buffer CAN2_rxBuffer; + + /* Recieve buffer for CAN 3 */ + Circular_Buffer CAN3_rxBuffer; + + /** + * Transfer buffers + */ + + /* Transfer buffer for CAN 1 */ + Circular_Buffer CAN1_txBuffer; + + /* Transfer buffer for CAN 2 */ + Circular_Buffer CAN2_txBuffer; + + /* Transfer buffer for CAN 3 */ + Circular_Buffer CAN3_txBuffer; + + private: + + /** + * Tries to dispatch CAN message out to the correct handler + * if there are more than 0 CAN ids + */ + void dispatch_msg(CAN_message_t &msg) + { + if constexpr (sizeof...(can_ids) > 0) { + (void)((can_id == can_ids && handlers[msg.id](msg)) || ...); + } + } + + /** + * Stores recieve callbacks + */ + etl::delegate handlers[sizeof...(can_ids)] = {}; + +}; #endif /* HYTECHCANINTERFACE */ \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index e325ac3..48c94a5 100644 --- a/platformio.ini +++ b/platformio.ini @@ -48,6 +48,7 @@ test_ignore = test_systems lib_deps = ${common.lib_deps_shared} + https://github.com/tonton81/FlexCAN_T4 https://github.com/hytech-racing/shared_firmware_interfaces.git https://github.com/RCMast3r/hytech_can#testing_new_inv_ids https://github.com/hytech-racing/HT_CAN/releases/download/127/can_lib.tar.gz @@ -73,4 +74,5 @@ test_ignore= test_interfaces* lib_deps = ${common.lib_deps_shared} + https://github.com/tonton81/FlexCAN_T4 https://github.com/hytech-racing/HT_CAN/releases/download/127/can_lib.tar.gz \ No newline at end of file From 3f91ea05bf2b37b3446fe5a0ad09574599178ee2 Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Sun, 26 Jan 2025 03:49:30 -0500 Subject: [PATCH 27/36] Add library to ini and fix small build errors --- lib/interfaces/include/HytechCANInterface.h | 11 ++++++----- lib/interfaces/include/InverterInterface.h | 9 ++++----- lib/interfaces/library.json | 13 +++++++++++++ platformio.ini | 1 + src/main.cpp | 3 +++ 5 files changed, 27 insertions(+), 10 deletions(-) create mode 100644 lib/interfaces/library.json diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 6cdbc44..db76de8 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -3,6 +3,7 @@ #include #include +#include template class HytechCANInterface { @@ -13,13 +14,13 @@ class HytechCANInterface { /** * Registers CAN id to the handlers delegate */ - template + template void register_handler(void (callback)(CAN_message_t &msg)) { - if constexpr ((can_id == can_ids) || ...) { - handlers[CAN_ID] = etl::delegate::create(callback); + if constexpr (((can_id == can_ids) || ...)) { + handlers[can_id] = callback; } else { - static_assert(((can_id ==can_ids) || ...), "CAN id not registered.") + static_assert(((can_id == can_ids) || ...), "CAN id not registered."); } } @@ -93,7 +94,7 @@ class HytechCANInterface { void dispatch_msg(CAN_message_t &msg) { if constexpr (sizeof...(can_ids) > 0) { - (void)((can_id == can_ids && handlers[msg.id](msg)) || ...); + (void)((msg.id == can_ids && handlers[msg.id](msg)) || ...); } } diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index 52edeb3..ef9c320 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -109,13 +109,13 @@ class InverterInterface public: InverterInterface( - CANBufferType msg_queue, + CANBufferType *msg_output_queue, uint32_t mc_energy_id, uint32_t mc_status_id, uint32_t mc_temps_id, uint32_t mc_setpoint_commands_id, - uint32_t mc_torque_command_id) : msg_queue_(msg_queue) - { + uint32_t mc_torque_command_id) : msg_queue_(msg_output_queue) + { inverter_ids.mc_energy_id = mc_energy_id; inverter_ids.mc_status_id = mc_status_id; inverter_ids.mc_temps_id = mc_temps_id; @@ -167,8 +167,7 @@ class InverterInterface void send_MC_TORQUE_COMMAND(); - CANBufferType msg_queue_; - + CANBufferType *msg_queue_; }; #endif // __INVERTERINTERFACE_H__ diff --git a/lib/interfaces/library.json b/lib/interfaces/library.json new file mode 100644 index 0000000..52e9b53 --- /dev/null +++ b/lib/interfaces/library.json @@ -0,0 +1,13 @@ +{ + "name": "interfaces-lib", + "version": "1.0.0", + "license": "MIT", + "dependencies": { + "can_lib": "*", + "shared_data": "*", + "CASE_lib": "*", + "nanopb": "*" + }, + "frameworks": "*", + "platforms": "*" + } \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 48c94a5..10494e4 100644 --- a/platformio.ini +++ b/platformio.ini @@ -7,6 +7,7 @@ lib_deps_shared = + interfaces-lib https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 https://github.com/hytech-racing/shared_firmware_types.git ; ../shared_firmware_types diff --git a/src/main.cpp b/src/main.cpp index 191e020..67f9206 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -25,6 +25,9 @@ #include "VCR_Tasks.h" #include "TorqueControllerMux.hpp" +#include +#include + /* Scheduler setup */ HT_SCHED::Scheduler& scheduler = HT_SCHED::Scheduler::getInstance(); From 6bba3192b87b75fee9791f53e7f88820ef21c079 Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Mon, 27 Jan 2025 01:59:34 -0500 Subject: [PATCH 28/36] Changed CAN interface and updated inverter inverter interface with new signals --- lib/interfaces/include/HytechCANInterface.h | 241 ++++++++++++-------- lib/interfaces/include/InverterInterface.h | 50 ++-- lib/interfaces/src/HyTechCANInterface.cpp | 35 +++ lib/interfaces/src/InverterInterface.cpp | 109 ++++++--- platformio.ini | 4 +- src/main.cpp | 109 +++++++-- 6 files changed, 391 insertions(+), 157 deletions(-) create mode 100644 lib/interfaces/src/HyTechCANInterface.cpp diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index db76de8..f19460f 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -4,105 +4,154 @@ #include #include #include +#include +#include +#include + +/* Recieve Buffers */ +extern CANBufferType CAN1_rxBuffer; +extern CANBufferType CAN2_rxBuffer; +extern CANBufferType CAN3_rxBuffer; + +/* Transfer Buffers */ +extern CANBufferType CAN1_txBuffer; +extern CANBufferType CAN2_txBuffer; +extern CANBufferType CAN3_txBuffer; + +/** + * Struct holding the interfaces processed by the ring buffer + */ + +struct CANInterfaces +{ + InverterInterface front_left_inv; + InverterInterface front_right_inv; + InverterInterface back_left_inv; + InverterInterface back_right_inv; +}; -template -class HytechCANInterface { - public: - - HytechCANInterface() { } - - /** - * Registers CAN id to the handlers delegate - */ - template - void register_handler(void (callback)(CAN_message_t &msg)) - { - if constexpr (((can_id == can_ids) || ...)) { - handlers[can_id] = callback; - } else { - static_assert(((can_id == can_ids) || ...), "CAN id not registered."); - } - } - - /** - * Dispatches CAN messages from the specified buffer - * (Should be called in a loop) - */ - template - void dispatch_buffer(bufferType &rx_buffer) - { - while (rx_buffer.available()) - { - CAN_message_t recvd_msg; - uint8_t buf[sizeof(CAN_message_t)]; - rx_buffer.pop_front(buf, sizeof(CAN_message_t)); - memmove(&recvd_msg, buf, sizeof(recvd_msg)); - dispatch_msg(recvd_msg.id); - } - } - - /** - * Sending CAN messages - * (needs to be called in loop) - * */ - template - void send_all_CAN_msgs(bufferType &buffer, FlexCAN_T4_Base *can_interface) +/** Methods called on can recieve */ +void on_can1_recieve(const CAN_message_t &msg); + +void on_can2_recieve(const CAN_message_t &msg); + +void on_can3_recieve(const CAN_message_t &msg); + +/** + * Recieving CAN messages + * (needs to be called in a loop) + */ +template +void process_ring_buffer(BufferType &rx_buffer, CANInterfaces interfaces) { + while (rx_buffer.available()) + { + CAN_message_t recvd_msg; + uint8_t buf[sizeof(CAN_message_t)]; + rx_buffer.pop_front(buf, sizeof(CAN_message_t)); + memmove(&recvd_msg, buf, sizeof(recvd_msg)); + switch (recvd_msg.id) { - CAN_message_t msg; - while (buffer.available()) - { - CAN_message_t msg; - uint8_t buf[sizeof(CAN_message_t)]; - buffer.pop_front(buf, sizeof(CAN_message_t)); - memmove(&msg, buf, sizeof(msg)); - can_interface->write(msg); - } - } - - /** - * Recieve Buffers - */ + // FL Inverter + case (MCI1_STATUS_CANID): + interfaces.front_left_inv.recieve_MCI_STATUS(recvd_msg); + break; + + case (MCI1_TEMPS_CANID): + interfaces.front_left_inv.recieve_MCI_TEMPS(recvd_msg); + break; + + case (MCI1_DYNAMICS_CANID): + interfaces.front_left_inv.recieve_MCI_DYNAMICS(recvd_msg); + break; + + case (MCI1_POWER_CANID): + interfaces.front_left_inv.recieve_MCI_POWER(recvd_msg); + break; + + case (MCI1_FEEDBACK_CANID): + interfaces.front_left_inv.recieve_MCI_FEEDBACK(recvd_msg); + + // FR inverter + case (MCI2_STATUS_CANID): + interfaces.front_right_inv.recieve_MCI_STATUS(recvd_msg); + break; + + case (MCI2_TEMPS_CANID): + interfaces.front_right_inv.recieve_MCI_TEMPS(recvd_msg); + break; + + case (MCI2_DYNAMICS_CANID): + interfaces.front_right_inv.recieve_MCI_DYNAMICS(recvd_msg); + break; + + case (MCI2_POWER_CANID): + interfaces.front_right_inv.recieve_MCI_POWER(recvd_msg); + break; + + case (MCI2_FEEDBACK_CANID): + interfaces.front_right_inv.recieve_MCI_FEEDBACK(recvd_msg); + + // RL Inverter + case (MCI3_STATUS_CANID): + interfaces.back_left_inv.recieve_MCI_STATUS(recvd_msg); + break; + + case (MCI3_TEMPS_CANID): + interfaces.back_left_inv.recieve_MCI_TEMPS(recvd_msg); + break; + + case (MCI3_DYNAMICS_CANID): + interfaces.back_left_inv.recieve_MCI_DYNAMICS(recvd_msg); + break; + + case (MCI3_POWER_CANID): + interfaces.back_left_inv.recieve_MCI_POWER(recvd_msg); + break; + + case (MCI3_FEEDBACK_CANID): + interfaces.back_left_inv.recieve_MCI_FEEDBACK(recvd_msg); + + // RR Inverter + case (MCI4_STATUS_CANID): + interfaces.back_right_inv.recieve_MCI_STATUS(recvd_msg); + break; + + case (MCI4_TEMPS_CANID): + interfaces.back_right_inv.recieve_MCI_TEMPS(recvd_msg); + break; + + case (MCI4_DYNAMICS_CANID): + interfaces.back_right_inv.recieve_MCI_DYNAMICS(recvd_msg); + break; + + case (MCI4_POWER_CANID): + interfaces.back_right_inv.recieve_MCI_POWER(recvd_msg); + break; + + case (MCI4_FEEDBACK_CANID): + interfaces.back_right_inv.recieve_MCI_FEEDBACK(recvd_msg); - /* Recieve buffer for CAN 1 */ - Circular_Buffer CAN1_rxBuffer; - - /* Recieve buffer for CAN 2 */ - Circular_Buffer CAN2_rxBuffer; - - /* Recieve buffer for CAN 3 */ - Circular_Buffer CAN3_rxBuffer; - - /** - * Transfer buffers - */ - - /* Transfer buffer for CAN 1 */ - Circular_Buffer CAN1_txBuffer; - - /* Transfer buffer for CAN 2 */ - Circular_Buffer CAN2_txBuffer; - - /* Transfer buffer for CAN 3 */ - Circular_Buffer CAN3_txBuffer; - - private: - - /** - * Tries to dispatch CAN message out to the correct handler - * if there are more than 0 CAN ids - */ - void dispatch_msg(CAN_message_t &msg) - { - if constexpr (sizeof...(can_ids) > 0) { - (void)((msg.id == can_ids && handlers[msg.id](msg)) || ...); - } } - - /** - * Stores recieve callbacks - */ - etl::delegate handlers[sizeof...(can_ids)] = {}; - -}; + + } +} + +/** +* Sending CAN messages +* (needs to be called in loop) +* */ +template +void send_all_CAN_msgs(bufferType &tx_buffer, FlexCAN_T4_Base *can_interface) +{ + CAN_message_t msg; + while (tx_buffer.available()) + { + CAN_message_t msg; + uint8_t buf[sizeof(CAN_message_t)]; + tx_buffer.pop_front(buf, sizeof(CAN_message_t)); + memmove(&msg, buf, sizeof(msg)); + can_interface->write(msg); + } +} #endif /* HYTECHCANINTERFACE */ \ No newline at end of file diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index ef9c320..13ac844 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -22,9 +22,6 @@ namespace HTUnits */ struct InverterIds_s { - uint32_t mc_energy_id; - uint32_t mc_status_id; - uint32_t mc_temps_id; uint32_t mc_setpoint_commands_id; uint32_t mc_torque_command_id; }; @@ -55,6 +52,7 @@ struct InverterTorqueCommand_s **/ struct InverterStatus_s { + bool new_data : 1; bool system_ready : 1; bool error : 1; bool warning : 1; @@ -69,6 +67,7 @@ struct InverterStatus_s struct InverterTemps_s { + bool new_data : 1; HTUnits::celcius motor_temp; HTUnits::celcius inverter_temp; HTUnits::celcius igbt_temp; @@ -76,12 +75,14 @@ struct InverterTemps_s struct InverterPower_s { + bool new_data : 1; HTUnits::watts active_power; HTUnits::var reactive_power; }; struct MotorMechanics_s { + bool new_data : 1; HTUnits::watts actual_power; HTUnits::torque_nm actual_torque; HTUnits::speed_rpm actual_speed; @@ -89,6 +90,7 @@ struct MotorMechanics_s struct InverterControlParams_s { + bool new_data : 1; uint16_t speed_control_kp; uint16_t speed_control_ki; uint16_t speed_control_kd; @@ -103,6 +105,9 @@ struct InverterFeedbackData_s InverterControlParams_s control_params; }; +/** + * Inverter interface + */ class InverterInterface { @@ -110,15 +115,9 @@ class InverterInterface InverterInterface( CANBufferType *msg_output_queue, - uint32_t mc_energy_id, - uint32_t mc_status_id, - uint32_t mc_temps_id, uint32_t mc_setpoint_commands_id, uint32_t mc_torque_command_id) : msg_queue_(msg_output_queue) { - inverter_ids.mc_energy_id = mc_energy_id; - inverter_ids.mc_status_id = mc_status_id; - inverter_ids.mc_temps_id = mc_temps_id; inverter_ids.mc_setpoint_commands_id = mc_setpoint_commands_id; inverter_ids.mc_torque_command_id = mc_torque_command_id; } @@ -138,6 +137,19 @@ class InverterInterface uint16_t torque_command ); + // TODO un-public these (they are public for testing) + + /* Recieving callbacks */ + void recieve_MCI_STATUS(CAN_message_t &can_msg); + + void recieve_MCI_TEMPS(CAN_message_t &can_msg); + + void recieve_MCI_DYNAMICS(CAN_message_t &can_msg); + + void recieve_MCI_POWER(CAN_message_t &can_msg); + + void recieve_MCI_FEEDBACK(CAN_message_t &can_msg); + private: @@ -146,18 +158,18 @@ class InverterInterface InverterSetpoints_s _inverter_setpoints; InverterTorqueCommand_s _inverter_torque_command; - InverterStatus_s _inverter_status; - InverterTemps_s _inverter_temps; - InverterPower_s _inverter_power; - InverterControlParams_s _control_params; + // InverterStatus_s _inverter_status; + // InverterTemps_s _inverter_temps; + // InverterPower_s _inverter_power; + // InverterControlParams_s _control_params; InverterFeedbackData_s _feedback_data; - /* Recieving callbacks */ - void recieve_MC_ENERGY(CAN_message_t &can_msg); - - void recieve_MC_STATUS(CAN_message_t &can_msg); - - void recieve_MC_TEMPS(CAN_message_t &can_msg); + /* Getters */ + InverterStatus_s get_status(); + InverterTemps_s get_temps(); + InverterPower_s get_power(); + MotorMechanics_s get_motor_mechanics(); + InverterControlParams_s get_control_params(); /* Sending */ template diff --git a/lib/interfaces/src/HyTechCANInterface.cpp b/lib/interfaces/src/HyTechCANInterface.cpp new file mode 100644 index 0000000..3220132 --- /dev/null +++ b/lib/interfaces/src/HyTechCANInterface.cpp @@ -0,0 +1,35 @@ +#include + +/* Recieve Buffers */ +CANBufferType CAN1_rxBuffer; +CANBufferType CAN2_rxBuffer; +CANBufferType CAN3_rxBuffer; + +/* Transfer Buffers */ +CANBufferType CAN1_txBuffer; +CANBufferType CAN2_txBuffer; +CANBufferType CAN3_txBuffer; + +/* Methods called on can recieve */ +void on_can1_recieve(const CAN_message_t &msg) +{ + uint8_t buf[sizeof(CAN_message_t)]; + memmove(buf, &msg, sizeof(msg)); + CAN1_rxBuffer.push_back(buf, sizeof(CAN_message_t)); +} + +void on_can2_recieve(const CAN_message_t &msg) +{ + uint8_t buf[sizeof(CAN_message_t)]; + memmove(buf, &msg, sizeof(msg)); + CAN2_rxBuffer.push_back(buf, sizeof(CAN_message_t)); + +} + +void on_can3_recieve(const CAN_message_t &msg) +{ + uint8_t buf[sizeof(CAN_message_t)]; + memmove(buf, &msg, sizeof(msg)); + CAN3_rxBuffer.push_back(buf, sizeof(CAN_message_t)); + +} \ No newline at end of file diff --git a/lib/interfaces/src/InverterInterface.cpp b/lib/interfaces/src/InverterInterface.cpp index 41ccfb6..ed85292 100644 --- a/lib/interfaces/src/InverterInterface.cpp +++ b/lib/interfaces/src/InverterInterface.cpp @@ -1,6 +1,34 @@ #include -// TODO Parameterize this so it works for all motor controllers +/** + * Getters for the data + */ + +InverterStatus_s InverterInterface::get_status() { + _feedback_data.status.new_data = false; + return _feedback_data.status; +} + +InverterTemps_s InverterInterface::get_temps() { + _feedback_data.temps.new_data = false; + return _feedback_data.temps; +} + +InverterPower_s InverterInterface::get_power() { + _feedback_data.power.new_data = false; + return _feedback_data.power; +} + +MotorMechanics_s InverterInterface::get_motor_mechanics() { + _feedback_data.motor_mechanics.new_data = false; + return _feedback_data.motor_mechanics; +} + + +InverterControlParams_s InverterInterface::get_control_params() { + _feedback_data.control_params.new_data = false; + return _feedback_data.control_params; +} /** * Request change of state @@ -34,52 +62,81 @@ void InverterInterface::set_torque_command(uint16_t torque_command) /** * Recieving CAN messages */ -void InverterInterface::recieve_MC_ENERGY(CAN_message_t &can_msg) + +void InverterInterface::recieve_MCI_STATUS(CAN_message_t &can_msg) +{ + // Unpack the message + MCI1_STATUS_t unpacked_msg; + Unpack_MCI1_STATUS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + + // Update inverter interface with new data + _feedback_data.status.system_ready = unpacked_msg.system_ready; + _feedback_data.status.error = unpacked_msg.error; + _feedback_data.status.warning = unpacked_msg.warning; + _feedback_data. status.quit_dc_on = unpacked_msg.quit_dc_on; + _feedback_data.status.dc_on = unpacked_msg.dc_on; + _feedback_data.status.quit_inverter_on = unpacked_msg.quit_inverter_on; + _feedback_data.status.derating_on = unpacked_msg.derating_on; + _feedback_data.status.dc_bus_voltage = unpacked_msg.dc_bus_voltage; + _feedback_data.status.diagnostic_number = unpacked_msg.diagnostic_number; +} + +void InverterInterface::recieve_MCI_TEMPS(CAN_message_t &can_msg) { + // Unpack the message - MC1_ENERGY_t unpacked_msg; - Unpack_MC1_ENERGY_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + MCI1_TEMPS_t unpacked_msg; + Unpack_MCI1_TEMPS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); // Update inverter interface with new data - _inverter_status.dc_bus_voltage = unpacked_msg.dc_bus_voltage; - _feedback_data.motor_mechanics.actual_torque = unpacked_msg.feedback_torque; - _feedback_data.motor_mechanics.actual_power = unpacked_msg.motor_power; + _feedback_data.temps.igbt_temp = HYTECH_igbt_temp_ro_fromS(unpacked_msg.igbt_temp_ro); + _feedback_data.temps.inverter_temp = HYTECH_inverter_temp_ro_fromS(unpacked_msg.inverter_temp_ro); + _feedback_data.temps.motor_temp = HYTECH_motor_temp_ro_fromS(unpacked_msg.motor_temp_ro); + _feedback_data.temps.new_data = true; } -void InverterInterface::recieve_MC_STATUS(CAN_message_t &can_msg) +void InverterInterface::recieve_MCI_DYNAMICS(CAN_message_t &can_msg) { // Unpack the message - MC1_STATUS_t unpacked_msg; - Unpack_MC1_STATUS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); - + MCI1_DYNAMICS_t unpacked_msg; + Unpack_MCI1_DYNAMICS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + // Update inverter interface with new data - _inverter_status.system_ready = unpacked_msg.system_ready; - _inverter_status.error = unpacked_msg.error; - _inverter_status.warning = unpacked_msg.warning; - _inverter_status.quit_dc_on = unpacked_msg.quit_dc_on; - _inverter_status.dc_on = unpacked_msg.dc_on; - _inverter_status.quit_inverter_on = unpacked_msg.quit_inverter_on; - _inverter_status.derating_on = unpacked_msg.derating_on; + _feedback_data.motor_mechanics.actual_power = unpacked_msg.actual_power_w; + _feedback_data.motor_mechanics.actual_torque = unpacked_msg.actual_torque_nm; + _feedback_data.motor_mechanics.actual_speed = unpacked_msg.actual_speed_rpm; + _feedback_data.motor_mechanics.new_data = true; } -void InverterInterface::recieve_MC_TEMPS(CAN_message_t &can_msg) +void InverterInterface::recieve_MCI_POWER(CAN_message_t &can_msg) { - // Unpack the message - MC1_TEMPS_t unpacked_msg; - Unpack_MC1_TEMPS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + MCI1_POWER_t unpacked_msg; + Unpack_MCI1_POWER_hytech(&unpacked_msg, can_msg.buf, can_msg.len); // Update inverter interface with new data - _inverter_status.diagnostic_number = unpacked_msg.diagnostic_number; - _inverter_temps.igbt_temp = HYTECH_igbt_temp_ro_fromS(unpacked_msg.igbt_temp_ro); - _inverter_temps.inverter_temp = HYTECH_inverter_temp_ro_fromS(unpacked_msg.inverter_temp_ro); - _inverter_temps.motor_temp = HYTECH_motor_temp_ro_fromS(unpacked_msg.motor_temp_ro); + _feedback_data.power.active_power = unpacked_msg.active_power_w; + _feedback_data.power.reactive_power = unpacked_msg.reactive_power_var; + _feedback_data.power.new_data = true; } +void InverterInterface::recieve_MCI_FEEDBACK(CAN_message_t &can_msg) +{ + // Unpack the message + MCI1_FEEDBACK_t unpacked_msg; + Unpack_MCI1_FEEDBACK_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + + // Update inverter interface with new data + _feedback_data.control_params.speed_control_kp = unpacked_msg.speed_control_kp; + _feedback_data.control_params.speed_control_ki = unpacked_msg.speed_control_ki; + _feedback_data.control_params.speed_control_kd = unpacked_msg.speed_control_kd; + _feedback_data.control_params.new_data = true; + +} /** * Sending CAN messages diff --git a/platformio.ini b/platformio.ini index 10494e4..b8819a5 100644 --- a/platformio.ini +++ b/platformio.ini @@ -52,7 +52,7 @@ lib_deps = https://github.com/tonton81/FlexCAN_T4 https://github.com/hytech-racing/shared_firmware_interfaces.git https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/127/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/134/can_lib.tar.gz ; Test Systems Environment. This is only for compiling and uploading the hardware-abstracted code. @@ -76,4 +76,4 @@ test_ignore= lib_deps = ${common.lib_deps_shared} https://github.com/tonton81/FlexCAN_T4 - https://github.com/hytech-racing/HT_CAN/releases/download/127/can_lib.tar.gz \ No newline at end of file + https://github.com/hytech-racing/HT_CAN/releases/download/134/can_lib.tar.gz \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 67f9206..8cea651 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,6 +6,7 @@ /* From C++ standard library */ #include +#include /* From shared_firmware_types libdep */ #include "SharedFirmwareTypes.h" @@ -28,27 +29,107 @@ #include #include +#include -/* Scheduler setup */ -HT_SCHED::Scheduler& scheduler = HT_SCHED::Scheduler::getInstance(); +// /* Scheduler setup */ +// HT_SCHED::Scheduler& scheduler = HT_SCHED::Scheduler::getInstance(); +// /* Ethernet message sockets */ // TODO: Move this into its own interface +// qindesign::network::EthernetUDP protobuf_send_socket; +// qindesign::network::EthernetUDP protobuf_recv_socket; -/* Ethernet message sockets */ // TODO: Move this into its own interface -qindesign::network::EthernetUDP protobuf_send_socket; -qindesign::network::EthernetUDP protobuf_recv_socket; +// // InverterInterface mci1_interface = InverterInterface(&CAN1_rxBuffer, 1, 2); +// void setup() { +// scheduler.setTimingFunction(micros); +// scheduler.schedule(tick_state_machine_task); +// scheduler.schedule(read_adc0_task); +// scheduler.schedule(read_adc1_task); +// scheduler.schedule(update_buzzer_controller_task); +// } -void setup() { - scheduler.setTimingFunction(micros); +// void loop() { +// scheduler.run(); +// } - scheduler.schedule(tick_state_machine_task); - scheduler.schedule(read_adc0_task); - scheduler.schedule(read_adc1_task); - scheduler.schedule(update_buzzer_controller_task); +FlexCAN_T4 TEST_CAN1; +FlexCAN_T4 TEST_CAN2; // Inverter CAN (now both are on same line) + +CAN_message_t msg; + +using CircularBufferType = CANBufferType; +using InverterInterfaceType = InverterInterface; + +InverterInterfaceType fl_inv = InverterInterfaceType(&CAN2_txBuffer, MC1_SETPOINTS_COMMAND_CANID, MC1_TORQUE_COMMAND_CANID); +InverterInterfaceType fr_inv = InverterInterfaceType(&CAN2_txBuffer, MC2_SETPOINTS_COMMAND_CANID, MC2_TORQUE_COMMAND_CANID); +InverterInterfaceType rl_inv = InverterInterfaceType(&CAN2_txBuffer, MC3_SETPOINTS_COMMAND_CANID, MC3_TORQUE_COMMAND_CANID); +InverterInterfaceType rr_inv = InverterInterfaceType(&CAN2_txBuffer, MC4_SETPOINTS_COMMAND_CANID, MC4_TORQUE_COMMAND_CANID); + +CANInterfaces CAN_interfaces = {fl_inv, fr_inv, rl_inv, rr_inv}; + +void init_can_interface() +{ + TEST_CAN2.begin(); + TEST_CAN2.setBaudRate(500000); + TEST_CAN2.setMaxMB(16); + TEST_CAN2.enableFIFO(); + TEST_CAN2.enableFIFOInterrupt(); + TEST_CAN2.onReceive(on_can2_recieve); + TEST_CAN2.mailboxStatus(); + + TEST_CAN1.begin(); + TEST_CAN1.setBaudRate(500000); + TEST_CAN1.setMaxMB(16); + TEST_CAN1.enableFIFO(); + TEST_CAN1.enableFIFOInterrupt(); + TEST_CAN1.enableMBInterrupts(); + TEST_CAN1.onReceive(on_can1_recieve); + TEST_CAN1.mailboxStatus(); +} + +void setup(void) +{ + while (!Serial) + { + // wait for Arduino Serial Monitor to be ready + } + init_can_interface(); } +bool ran_test = false; +void loop() +{ + // test for seeing if when CAN2 is connected to CAN1 the can1 rx buffer will get stuff + // testing whether or not a CAN message can be sent + + if (!ran_test) + { + msg.id = random(0x1, 0x7FE); + for (uint8_t i = 0; i < 8; i++) + msg.buf[i] = i + 1; + TEST_CAN2.write(msg); + ran_test = true; + delay(10); + Serial.println("======= TEST 1 ======="); + Serial.println("can1 rx buffer size: (should be 1)"); + Serial.println(CAN1_rxBuffer.available()); + Serial.println("======= TEST 2 ======="); + + Serial.println("can1 received message equal to sent msg"); + delay(10); + if (CAN1_rxBuffer.available()) + { + CAN_message_t recvd_msg; + uint8_t buf[sizeof(CAN_message_t)]; + CAN1_rxBuffer.pop_front(buf, sizeof(CAN_message_t)); + memmove(&recvd_msg, buf, sizeof(recvd_msg)); + + Serial.println("\tid = prev: (should be 1)"); + Serial.print("\t"); + Serial.println(recvd_msg.id == msg.id); + } + } +} + -void loop() { - scheduler.run(); -} \ No newline at end of file From 42aca007a51bcc9ca37c45709f1edbdacfe69972 Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Wed, 12 Feb 2025 21:40:35 -0500 Subject: [PATCH 29/36] changed inverter interface --- lib/interfaces/README.md | 5 - lib/interfaces/include/InverterInterface.h | 61 +++++------- lib/interfaces/src/HyTechCANInterface.cpp | 1 + lib/interfaces/src/InverterInterface.cpp | 75 +++++++------- lib/systems/include/DrivetrainSystem.h | 27 +----- src/main.cpp | 108 ++++++++++++++------- 6 files changed, 138 insertions(+), 139 deletions(-) diff --git a/lib/interfaces/README.md b/lib/interfaces/README.md index 2c9b606..c1c2778 100644 --- a/lib/interfaces/README.md +++ b/lib/interfaces/README.md @@ -22,7 +22,6 @@ below is the description of the control messages that will be configured within - Control Input Message (will be attempted to be sent every 5ms, monitored by the inverter to be expected at least every 20ms) - `int16_t speed_setpoint` - - __NOTE: active only when in speed control mode__ - AKA (`AMK_TargetVelocity`) / Special Signal index 6 / AKA 16-bit version of SERCOS parameter ID36 (unknown why this is 16 bit vs the 32 bit SERCOS parameter in manual) - the RPM setpoint for the inverter in units of RPM. - `int16_t positive_torque_limit` @@ -33,10 +32,6 @@ below is the description of the control messages that will be configured within - `int16_t negative_torque_limit` - AKA (`AMK_TorqueLimitNegativ`) / SERCOS parameter ID83 / Special Signal index 14 - `positive_torque_limit` with a negative sign. - - `int16_t torque_setpoint` - - __NOTE: active only when in torque control mode.__ - - AKA SERCOS parameter ID80 / Special Signal index 17 - - the torque setpoint for the inverter in units of percentage as described in the `positive_torque_limit` message member above - Control parameter message (will be sent intermitently, not monitored by the inverter to be expected at any regular period) - `uint16_t speed_control_kp` diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index 13ac844..98cd4d3 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -6,6 +6,7 @@ #include "MessageQueueDefine.h" #include +#include "DrivetrainSystem.h" namespace HTUnits { @@ -32,18 +33,17 @@ struct InverterIds_s */ struct InverterSetpoints_s { - bool inverter_enable : 1; - bool hv_enable : 1; - bool driver_enable : 1; - bool remove_error : 1; int16_t speed_rpm_setpoint; int16_t positive_torque_limit; int16_t negative_torque_limit; }; -struct InverterTorqueCommand_s +struct InverterControlWord_s { - uint16_t torque_command; + bool inverter_enable : 1; + bool hv_enable : 1; + bool driver_enable : 1; + bool remove_error : 1; }; /** @@ -122,21 +122,6 @@ class InverterInterface inverter_ids.mc_torque_command_id = mc_torque_command_id; } - /* Request change of state */ - void set_inverter_setpoints( - bool inverter_enable, - bool hv_enable, - bool driver_enable, - bool remove_error, - int16_t speed_rpm_setpoint, - int16_t positive_torque_limit, - int16_t negative_torque_limit - ); - - void set_torque_command( - uint16_t torque_command - ); - // TODO un-public these (they are public for testing) /* Recieving callbacks */ @@ -150,18 +135,30 @@ class InverterInterface void recieve_MCI_FEEDBACK(CAN_message_t &can_msg); + /* Sending */ + template + void enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *), uint32_t id); + + void send_MC_SETPOINT_COMMAND(); + + void send_MC_CONTROL_WORD(); + + /* Inverter Functs */ + void set_speed(float desired_rpm, float torque_limit_nm); + + void set_torque(float torque_nm); + + void set_idle(); + + void set_inverter_control_word(InverterControlWord_s control_word); + + InverterStatus_s get_inverter_status(); private: InverterIds_s inverter_ids; - InverterSetpoints_s _inverter_setpoints; - InverterTorqueCommand_s _inverter_torque_command; - - // InverterStatus_s _inverter_status; - // InverterTemps_s _inverter_temps; - // InverterPower_s _inverter_power; - // InverterControlParams_s _control_params; + InverterControlWord_s _inverter_control_word; InverterFeedbackData_s _feedback_data; /* Getters */ @@ -171,14 +168,6 @@ class InverterInterface MotorMechanics_s get_motor_mechanics(); InverterControlParams_s get_control_params(); - /* Sending */ - template - void enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *), uint32_t id); - - void send_MC_SETPOINT_COMMAND(); - - void send_MC_TORQUE_COMMAND(); - CANBufferType *msg_queue_; }; #endif // __INVERTERINTERFACE_H__ diff --git a/lib/interfaces/src/HyTechCANInterface.cpp b/lib/interfaces/src/HyTechCANInterface.cpp index 3220132..863980c 100644 --- a/lib/interfaces/src/HyTechCANInterface.cpp +++ b/lib/interfaces/src/HyTechCANInterface.cpp @@ -1,4 +1,5 @@ #include +#include /* Recieve Buffers */ CANBufferType CAN1_rxBuffer; diff --git a/lib/interfaces/src/InverterInterface.cpp b/lib/interfaces/src/InverterInterface.cpp index ed85292..bddac8d 100644 --- a/lib/interfaces/src/InverterInterface.cpp +++ b/lib/interfaces/src/InverterInterface.cpp @@ -30,34 +30,6 @@ InverterControlParams_s InverterInterface::get_control_params() { return _feedback_data.control_params; } -/** - * Request change of state - */ - -void InverterInterface::set_inverter_setpoints( - bool inverter_enable, - bool hv_enable, - bool driver_enable, - bool remove_error, - int16_t speed_rpm_setpoint, - int16_t positive_torque_limit, - int16_t negative_torque_limit -) -{ - _inverter_setpoints.inverter_enable = inverter_enable; - _inverter_setpoints.hv_enable = hv_enable; - _inverter_setpoints.driver_enable = driver_enable; - _inverter_setpoints.remove_error = remove_error; - _inverter_setpoints.speed_rpm_setpoint = speed_rpm_setpoint; - _inverter_setpoints.positive_torque_limit = positive_torque_limit; - _inverter_setpoints.negative_torque_limit = negative_torque_limit; -} - -void InverterInterface::set_torque_command(uint16_t torque_command) -{ - _inverter_torque_command.torque_command = torque_command; -} - /** * Recieving CAN messages @@ -84,6 +56,8 @@ void InverterInterface::recieve_MCI_STATUS(CAN_message_t &can_msg) void InverterInterface::recieve_MCI_TEMPS(CAN_message_t &can_msg) { + Serial.println("Recieved temps data!"); + // Unpack the message MCI1_TEMPS_t unpacked_msg; Unpack_MCI1_TEMPS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); @@ -157,10 +131,6 @@ void InverterInterface::send_MC_SETPOINT_COMMAND() { MC1_SETPOINTS_COMMAND_t msg_out; - msg_out.inverter_enable = _inverter_setpoints.inverter_enable; - msg_out.hv_enable = _inverter_setpoints.hv_enable; - msg_out.driver_enable = _inverter_setpoints.driver_enable; - msg_out.remove_error = _inverter_setpoints.remove_error; msg_out.speed_setpoint_rpm = _inverter_setpoints.speed_rpm_setpoint; msg_out.positive_torque_limit_ro = _inverter_setpoints.positive_torque_limit; msg_out.negative_torque_limit_ro = _inverter_setpoints.negative_torque_limit; @@ -168,12 +138,45 @@ void InverterInterface::send_MC_SETPOINT_COMMAND() enqueue_new_CAN(&msg_out, &Pack_MC1_SETPOINTS_COMMAND_hytech, inverter_ids.mc_setpoint_commands_id); } -void InverterInterface::send_MC_TORQUE_COMMAND() +void InverterInterface::send_MC_CONTROL_WORD() +{ + // TODO edit PCAN project to finish this +} + +/** + * Methods for use as inverter functs + */ + +void InverterInterface::set_speed(float desired_rpm, float torque_limit_nm) +{ + _inverter_setpoints.speed_rpm_setpoint = desired_rpm; + _inverter_setpoints.positive_torque_limit = torque_limit_nm; + _inverter_setpoints.negative_torque_limit = -torque_limit_nm; +} + +void InverterInterface::set_torque(float torque_nm) { - MC1_TORQUE_COMMAND_t msg_out; + // TODO set desired rpm to max? + _inverter_setpoints.positive_torque_limit = torque_nm; + _inverter_setpoints.negative_torque_limit = -torque_nm; +} - msg_out.torque_command_ro = HYTECH_torque_command_ro_toS(_inverter_torque_command.torque_command); +void InverterInterface::set_idle() +{ + _inverter_setpoints.negative_torque_limit = 0; + _inverter_setpoints.positive_torque_limit = 0; + _inverter_setpoints.speed_rpm_setpoint = 0; +} - enqueue_new_CAN(&msg_out, &Pack_MC1_TORQUE_COMMAND_hytech, inverter_ids.mc_torque_command_id); +void InverterInterface::set_inverter_control_word(InverterControlWord_s control_word) +{ + _inverter_control_word.driver_enable = control_word.driver_enable; + _inverter_control_word.hv_enable = control_word.hv_enable; + _inverter_control_word.inverter_enable = control_word.inverter_enable; + _inverter_control_word.remove_error = control_word.remove_error; } +InverterStatus_s InverterInterface::get_inverter_status() +{ + return _feedback_data.status; +} \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 6f901bc..f38d6c9 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -9,6 +9,7 @@ #include "SharedFirmwareTypes.h" #include "SysClock.h" +#include "InverterInterface.h" // requirements: @@ -72,24 +73,6 @@ struct DrivetrainInit_s DrivetrainModeRequest_e init_drivetrain; }; -struct InverterStatus_s -{ - float dc_bus_voltage; - float torque_nm; - float speed_rpm; - float mech_power_w; - float inverter_temp_c; - float motor_temp_c; - float igbt_temp_c; - uint16_t error_status_id; - bool inverter_ready : 1; - bool quit_dc : 1; - bool quit_inverter : 1; - bool error_present : 1; - bool connected : 1; - bool hv_present : 1; -}; - struct DrivetrainStatus_s { bool all_inverters_connected; @@ -114,14 +97,6 @@ struct DrivetrainInputPins_s bool torque_mode_enabled_pin_state : 1; }; -struct InverterControlWord_s -{ - bool inverter_enable; - bool hv_enable; - bool driver_enable; - bool remove_error; -}; - class DrivetrainSystem { public: diff --git a/src/main.cpp b/src/main.cpp index 8cea651..d3486a7 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,8 +2,6 @@ #include #endif - - /* From C++ standard library */ #include #include @@ -31,6 +29,7 @@ #include +#include // /* Scheduler setup */ // HT_SCHED::Scheduler& scheduler = HT_SCHED::Scheduler::getInstance(); @@ -56,16 +55,17 @@ FlexCAN_T4 TEST_CAN1; FlexCAN_T4 TEST_CAN2; // Inverter CAN (now both are on same line) +FlexCAN_T4 TEST_CAN3; // Inverter CAN (now both are on same line) CAN_message_t msg; using CircularBufferType = CANBufferType; using InverterInterfaceType = InverterInterface; -InverterInterfaceType fl_inv = InverterInterfaceType(&CAN2_txBuffer, MC1_SETPOINTS_COMMAND_CANID, MC1_TORQUE_COMMAND_CANID); -InverterInterfaceType fr_inv = InverterInterfaceType(&CAN2_txBuffer, MC2_SETPOINTS_COMMAND_CANID, MC2_TORQUE_COMMAND_CANID); -InverterInterfaceType rl_inv = InverterInterfaceType(&CAN2_txBuffer, MC3_SETPOINTS_COMMAND_CANID, MC3_TORQUE_COMMAND_CANID); -InverterInterfaceType rr_inv = InverterInterfaceType(&CAN2_txBuffer, MC4_SETPOINTS_COMMAND_CANID, MC4_TORQUE_COMMAND_CANID); +InverterInterfaceType fl_inv = InverterInterfaceType(&CAN3_txBuffer, MC1_SETPOINTS_COMMAND_CANID, MC1_TORQUE_COMMAND_CANID); +InverterInterfaceType fr_inv = InverterInterfaceType(&CAN3_txBuffer, MC2_SETPOINTS_COMMAND_CANID, MC2_TORQUE_COMMAND_CANID); +InverterInterfaceType rl_inv = InverterInterfaceType(&CAN3_txBuffer, MC3_SETPOINTS_COMMAND_CANID, MC3_TORQUE_COMMAND_CANID); +InverterInterfaceType rr_inv = InverterInterfaceType(&CAN3_txBuffer, MC4_SETPOINTS_COMMAND_CANID, MC4_TORQUE_COMMAND_CANID); CANInterfaces CAN_interfaces = {fl_inv, fr_inv, rl_inv, rr_inv}; @@ -87,6 +87,15 @@ void init_can_interface() TEST_CAN1.enableMBInterrupts(); TEST_CAN1.onReceive(on_can1_recieve); TEST_CAN1.mailboxStatus(); + + TEST_CAN3.begin(); + TEST_CAN3.setBaudRate(500000); + TEST_CAN3.setMaxMB(16); + TEST_CAN3.enableFIFO(); + TEST_CAN3.enableFIFOInterrupt(); + TEST_CAN3.enableMBInterrupts(); + TEST_CAN3.onReceive(on_can3_recieve); + TEST_CAN3.mailboxStatus(); } void setup(void) @@ -100,36 +109,63 @@ void setup(void) bool ran_test = false; void loop() { - // test for seeing if when CAN2 is connected to CAN1 the can1 rx buffer will get stuff - // testing whether or not a CAN message can be sent - if (!ran_test) - { - msg.id = random(0x1, 0x7FE); - for (uint8_t i = 0; i < 8; i++) - msg.buf[i] = i + 1; - TEST_CAN2.write(msg); - ran_test = true; - delay(10); - Serial.println("======= TEST 1 ======="); - Serial.println("can1 rx buffer size: (should be 1)"); - Serial.println(CAN1_rxBuffer.available()); - Serial.println("======= TEST 2 ======="); - - Serial.println("can1 received message equal to sent msg"); - delay(10); - if (CAN1_rxBuffer.available()) - { - CAN_message_t recvd_msg; - uint8_t buf[sizeof(CAN_message_t)]; - CAN1_rxBuffer.pop_front(buf, sizeof(CAN_message_t)); - memmove(&recvd_msg, buf, sizeof(recvd_msg)); - - Serial.println("\tid = prev: (should be 1)"); - Serial.print("\t"); - Serial.println(recvd_msg.id == msg.id); - } - } -} + // TEST SEND + + Serial.println("=== TESTING SEND ==="); + + Serial.println("Adding torque command to tx buffer..."); + + fl_inv.send_MC_TORQUE_COMMAND(); + + Serial.print("Tx buffer size (should be 1) "); + Serial.println(CAN3_txBuffer.size()); + + Serial.println("Sending CAN messages via can interface..."); + send_all_CAN_msgs(CAN3_txBuffer, &TEST_CAN3); + Serial.print("Tx buffer size (should be 0): "); + Serial.println(CAN3_txBuffer.size()); + delay(100); + + Serial.println(""); + + // Serial.println("=== TESTING RECIEVE ==="); + + // Serial.print("Rx buffer size before processing: "); + // Serial.println(CAN3_rxBuffer.size()); + + // process_ring_buffer(CAN3_rxBuffer, CAN_interfaces); + + // Serial.print("Rx buffer size after processing (shoud be 0): "); + // Serial.println(CAN3_rxBuffer.size()); + + // Serial.println(""); + // Serial.println(""); + + + + // for (uint8_t i = 0; i < 8; i++) + // msg.buf[i] = i + 1; + // TEST_CAN3.write(msg); + // ran_test = true; + // delay(10); + // Serial.println("======= TEST 1 ======="); + // Serial.println("can3 rx buffer size: (should be 1)"); + // Serial.println(CAN3_rxBuffer.available()); + // Serial.println("======= TEST 2 ======="); + // Serial.println("can1 received message equal to sent msg"); + // delay(10); + // if (CAN1_rxBuffer.available()) + // { + // CAN_message_t recvd_msg; + // uint8_t buf[sizeof(CAN_message_t)]; + // CAN1_rxBuffer.pop_front(buf, sizeof(CAN_message_t)); + // memmove(&recvd_msg, buf, sizeof(recvd_msg)); + + // Serial.println("\tid = prev: (should be 1)"); + // Serial.print("\t"); + // Serial.println(recvd_msg.id == msg.id); + // } +} From 296628d78c334bd44f466c2ba7d72a4c2a3e2ccd Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Wed, 12 Feb 2025 21:45:48 -0500 Subject: [PATCH 30/36] builds --- src/main.cpp | 85 +--------------------------------------------------- 1 file changed, 1 insertion(+), 84 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 9256d05..75f0054 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -26,32 +26,6 @@ /* Scheduler setup */ HT_SCHED::Scheduler& scheduler = HT_SCHED::Scheduler::getInstance(); - -// /* Scheduler setup */ -// HT_SCHED::Scheduler& scheduler = HT_SCHED::Scheduler::getInstance(); - -// /* Ethernet message sockets */ // TODO: Move this into its own interface -// qindesign::network::EthernetUDP protobuf_send_socket; -// qindesign::network::EthernetUDP protobuf_recv_socket; - -// // InverterInterface mci1_interface = InverterInterface(&CAN1_rxBuffer, 1, 2); - -// void setup() { -// scheduler.setTimingFunction(micros); - -// scheduler.schedule(tick_state_machine_task); -// scheduler.schedule(read_adc0_task); -// scheduler.schedule(read_adc1_task); -// scheduler.schedule(update_buzzer_controller_task); -// } - - scheduler.schedule(tick_state_machine_task); - scheduler.schedule(read_adc0_task); - scheduler.schedule(read_adc1_task); - scheduler.schedule(update_buzzer_controller_task); - scheduler.schedule(kick_watchdog_task); -} - void setup(void) { while (!Serial) @@ -64,62 +38,5 @@ bool ran_test = false; void loop() { - // TEST SEND - - Serial.println("=== TESTING SEND ==="); - - Serial.println("Adding torque command to tx buffer..."); - - fl_inv.send_MC_TORQUE_COMMAND(); - - Serial.print("Tx buffer size (should be 1) "); - Serial.println(CAN3_txBuffer.size()); - - Serial.println("Sending CAN messages via can interface..."); - send_all_CAN_msgs(CAN3_txBuffer, &TEST_CAN3); - - Serial.print("Tx buffer size (should be 0): "); - Serial.println(CAN3_txBuffer.size()); - - delay(100); - - Serial.println(""); - - // Serial.println("=== TESTING RECIEVE ==="); - - // Serial.print("Rx buffer size before processing: "); - // Serial.println(CAN3_rxBuffer.size()); - - // process_ring_buffer(CAN3_rxBuffer, CAN_interfaces); - - // Serial.print("Rx buffer size after processing (shoud be 0): "); - // Serial.println(CAN3_rxBuffer.size()); - - // Serial.println(""); - // Serial.println(""); - - - - // for (uint8_t i = 0; i < 8; i++) - // msg.buf[i] = i + 1; - // TEST_CAN3.write(msg); - // ran_test = true; - // delay(10); - // Serial.println("======= TEST 1 ======="); - // Serial.println("can3 rx buffer size: (should be 1)"); - // Serial.println(CAN3_rxBuffer.available()); - // Serial.println("======= TEST 2 ======="); - // Serial.println("can1 received message equal to sent msg"); - // delay(10); - // if (CAN1_rxBuffer.available()) - // { - // CAN_message_t recvd_msg; - // uint8_t buf[sizeof(CAN_message_t)]; - // CAN1_rxBuffer.pop_front(buf, sizeof(CAN_message_t)); - // memmove(&recvd_msg, buf, sizeof(recvd_msg)); - - // Serial.println("\tid = prev: (should be 1)"); - // Serial.print("\t"); - // Serial.println(recvd_msg.id == msg.id); - // } + } From 45d3f5b5e51804bfecc2c081655560846edb12c2 Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Thu, 13 Feb 2025 18:46:51 -0500 Subject: [PATCH 31/36] pluh --- lib/systems/README.md | 34 +++---------- lib/systems/include/DrivetrainSystem.h | 19 +------- lib/systems/src/DrivetrainSystem.cpp | 67 +------------------------- 3 files changed, 9 insertions(+), 111 deletions(-) diff --git a/lib/systems/README.md b/lib/systems/README.md index e41d3e8..2029328 100644 --- a/lib/systems/README.md +++ b/lib/systems/README.md @@ -43,13 +43,10 @@ stateDiagram-v2 ready : INVERTERS_READY hv_en : INVERTERS_HV_ENABLED en : INVERTERS_ENABLED - en_sp_mode : ENABLING_INVERTERS_SPEED_MODE - en_torq_mode : ENABLING_INVERTERS_TORQUE_MODE - speed_mode : ENABLED_SPEED_MODE - torq_mode : ENABLED_TORQUE_MODE + drive_mode : ENABLED_DRIVE_MODE err: ERROR clear_err: CLEARING_ERRORS - note2: note that that note shown is that any state other than NOT_CONNECTED can go to the ERROR state if in any of the states any inverter has an error flag present + note2: note that any state other than NOT_CONNECTED can go to the ERROR state if in any of the states any inverter has an error flag present note right of err during this state all setpoints will be set to 0 and the inverter control word will have the inverter_enable flag set to false @@ -59,22 +56,10 @@ stateDiagram-v2 during this state all setpoints will be set to 0, (inverter_enable: false, hv_enable: true, driver_enable: true, remove_error: true) end note - note right of speed_mode + note right of drive_mode during this mode the drivetrain is free to receive speed commands and car will move end note - note right of torq_mode - during this mode the drivetrain is free to receive torque commands and car will move - end note - - note right of en_sp_mode - during this mode the VCR sets the enable torque mode GPIO pin to be off and waits for the inverter's GPIO for signaling that the secondary control is not in control - end note - - note right of en_torq_mode - same as the ENABLING_SPEED_MODE, however the VCR is turning on its GPIO pin and waits for the inverter's GPIO to be signaled on - end note - nc --> not_en: received data from all inverters nc --> not_en_hv: received data from all inverters AND the inverters have a voltage above HV level @@ -89,16 +74,9 @@ stateDiagram-v2 hv_en --> en: on set of all inverters enabled flags en --> hv_en: on inverter enabled flag off - en --> en_sp_mode: on command request of speed mode - en_sp_mode --> speed_mode: on verification of inverters GPIO of speed mode set - en --> en_torq_mode: on command request of torque mode - en_torq_mode --> torq_mode: on verification of inverters GPIO of torque mode set - torq_mode --> err: on error during operation - speed_mode --> err: on error during operation - torq_mode --> en_sp_mode: on request of speed mode AND drivetrain is not active - speed_mode --> en_torq_mode: on request of torque mode AND drivetrain is not active - speed_mode --> err: incorrect user command type OR any inverter error - torq_mode --> err: incorrect user command type OR any inverter error + en --> drive_mode: on command request of drive mode + drive_mode --> err: on error during operation + drive_mode --> err: incorrect user command type OR any inverter error err --> clear_err: on user request of error reset clear_err --> not_en_hv: on successful reset of errors (either internally to the drivetrain system or of the inverters themselves) ``` \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index f38d6c9..3eef1bf 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -36,11 +36,8 @@ enum class DrivetrainState_e INVERTERS_READY = 3, INVERTERS_HV_ENABLED = 4, INVERTERS_ENABLED = 5, - ENABLING_INVERTERS_SPEED_MODE = 6, - ENABLING_INVERTERS_TORQUE_MODE = 7, - ENABLED_SPEED_MODE = 8, - ENABLED_TORQUE_MODE = 9, - ERROR = 10 + ENABLED_DRIVE_MODE = 6, + ERROR = 7 }; enum class DrivetrainCmdResponse_e @@ -61,18 +58,6 @@ struct DrivetrainTorqueCommand_s veh_vec desired_torque_nm; }; -enum class DrivetrainModeRequest_e -{ - UNINITIALIZED = 0, - INIT_SPEED_MODE = 1, - INIT_TORQUE_MODE =2 -}; - -struct DrivetrainInit_s -{ - DrivetrainModeRequest_e init_drivetrain; -}; - struct DrivetrainStatus_s { bool all_inverters_connected; diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index b3ce969..40b6acf 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -204,47 +204,7 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm break; } - case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: - { - bool inverter_error_present = false; - inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool inverters_in_speed_mode = false; - inverters_in_speed_mode = (!_get_gpio_state().torque_mode_enabled_pin_state); - - if(inverter_error_present) - { - _set_drivetrain_disabled(); - _set_state(DrivetrainState_e::ERROR); - } else if(inverters_in_speed_mode) - { - _set_enable_drivetrain(); - _set_state(DrivetrainState_e::ENABLED_SPEED_MODE); - } - break; - } - - case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: - { - - bool inverter_error_present = false; - inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool inverters_in_torque_mode = false; - inverters_in_torque_mode = _get_gpio_state().torque_mode_enabled_pin_state; - - if(inverter_error_present) - { - _set_drivetrain_disabled(); - _set_state(DrivetrainState_e::ERROR); - } else if(inverters_in_torque_mode) - { - _set_enable_drivetrain(); - _set_state(DrivetrainState_e::ENABLED_TORQUE_MODE); - } - - break; - } - - case DrivetrainState_e::ENABLED_SPEED_MODE: + case DrivetrainState_e::ENABLED_DRIVE_MODE: { if(!(_check_inverter_flags(_check_inverter_hv_present_flag))) { @@ -255,7 +215,6 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm // TODO may need to verify that the gpio state is correct while in this state (GPIO torque mode low) bool inverter_error_present = false; inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - // now finally in this mode and the ENABLED_TORQUE_MODE can we command the drivetrain bool user_requesting_speed_command = false; user_requesting_speed_command = etl::holds_alternative(cmd); bool user_requesting_torque_mode = false; @@ -278,30 +237,6 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm } break; } - case DrivetrainState_e::ENABLED_TORQUE_MODE: - { - // TODO may need to verify that the gpio state is correct while in this state (GPIO torque mode high) - bool inverter_error_present = false; - inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - - bool user_requesting_torque_command = etl::holds_alternative(cmd); - bool user_requesting_speed_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); - - if(inverter_error_present) - { - _set_drivetrain_disabled(); - _set_state(DrivetrainState_e::ERROR); - } - else if(user_requesting_torque_command) - { - _set_drivetrain_torque_command(etl::get(cmd)); - } else if(user_requesting_speed_mode && !_drivetrain_active(_active_rpm_level)) - { - _set_drivetrain_keepalive_idle(); - _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); - } - break; - } case DrivetrainState_e::ERROR: { bool user_requesting_error_reset = etl::holds_alternative(cmd) && (etl::get(cmd).reset_errors); From d3d879caa641bfbb70e457219c06780a3da896e6 Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Thu, 13 Feb 2025 19:38:53 -0500 Subject: [PATCH 32/36] drivetrain system inital rev w/o torque and speed modes --- lib/interfaces/include/InverterInterface.h | 1 + lib/interfaces/src/InverterInterface.cpp | 1 + lib/systems/include/DrivetrainSystem.h | 32 ++--- lib/systems/src/DrivetrainSystem.cpp | 148 ++++----------------- platformio.ini | 3 +- 5 files changed, 46 insertions(+), 139 deletions(-) diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index 98cd4d3..b2f2d7d 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -52,6 +52,7 @@ struct InverterControlWord_s **/ struct InverterStatus_s { + bool connected : 1; bool new_data : 1; bool system_ready : 1; bool error : 1; diff --git a/lib/interfaces/src/InverterInterface.cpp b/lib/interfaces/src/InverterInterface.cpp index bddac8d..daa1a94 100644 --- a/lib/interfaces/src/InverterInterface.cpp +++ b/lib/interfaces/src/InverterInterface.cpp @@ -42,6 +42,7 @@ void InverterInterface::recieve_MCI_STATUS(CAN_message_t &can_msg) Unpack_MCI1_STATUS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); // Update inverter interface with new data + _feedback_data.status.connected = true; // Will set to true once first CAN message has been recieved _feedback_data.status.system_ready = unpacked_msg.system_ready; _feedback_data.status.error = unpacked_msg.error; _feedback_data.status.warning = unpacked_msg.warning; diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 3eef1bf..77c3077 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -26,6 +26,9 @@ // - [ ] +namespace constants { + float MINIMUM_HV_VOLTAGE = 400.0; +} // TODO move these into the shared types after finishing the system enum class DrivetrainState_e @@ -47,17 +50,6 @@ enum class DrivetrainCmdResponse_e COMMAND_INVALID = 2 }; -struct DrivetrainSpeedCommand_s -{ - veh_vec desired_speed_rpm; - veh_vec torque_limit_nm; -}; - -struct DrivetrainTorqueCommand_s -{ - veh_vec desired_torque_nm; -}; - struct DrivetrainStatus_s { bool all_inverters_connected; @@ -82,10 +74,21 @@ struct DrivetrainInputPins_s bool torque_mode_enabled_pin_state : 1; }; +enum DrivetrainModeRequest_e +{ + UNINITIALIZED = 0, + INIT_DRIVE_MODE = 1 +}; + +struct DrivetrainInit_s +{ + DrivetrainModeRequest_e init_drivetrain; +}; + class DrivetrainSystem { public: - using CmdVariant = etl::variant; + using CmdVariant = etl::variant; DrivetrainSystem() = delete; DrivetrainStatus_s evaluate_drivetrain(CmdVariant cmd); @@ -98,6 +101,7 @@ class DrivetrainSystem std::function set_idle; std::function set_inverter_control_word; std::function get_status; + std::function get_motor_mechanics; }; DrivetrainSystem(veh_vec inverter_interfaces); @@ -114,9 +118,7 @@ class DrivetrainSystem void _set_enable_drivetrain_hv(); void _set_enable_drivetrain(); void _set_drivetrain_error_reset(); - - void _set_drivetrain_speed_command(DrivetrainSpeedCommand_s cmd); - void _set_drivetrain_torque_command(DrivetrainTorqueCommand_s cmd); + void _set_drivetrain_command(DrivetrainCommand_s cmd); DrivetrainState_e _evaluate_state_machine(CmdVariant cmd); diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index 40b6acf..24b5b75 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -5,13 +5,13 @@ DrivetrainSystem::DrivetrainSystem( veh_vec inverter_interfaces) : _inverter_interfaces(inverter_interfaces), _state(DrivetrainState_e::NOT_CONNECTED), - _check_inverter_ready_flag([](const InverterStatus_s & status) -> bool {return status.inverter_ready;}), - _check_inverter_connected_flag([](const InverterStatus_s & status) -> bool {return status.connected;}), - _check_inverter_quit_dc_flag([](const InverterStatus_s & status) -> bool {return status.quit_dc;}), - _check_inverter_error_flag([](const InverterStatus_s & status) -> bool {return status.error_present;}), - _check_inverter_hv_present_flag([](const InverterStatus_s & status) -> bool {return status.hv_present;}), - _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return !status.hv_present;}), - _check_inverter_enabled([](const InverterStatus_s & status) -> bool {return !status.quit_inverter;}) { }; + _check_inverter_ready_flag([](const InverterStatus_s & status) -> bool {return status.system_ready;}), + _check_inverter_connected_flag([](const InverterStatus_s & status) -> bool {return status.connected;}), // TODO fix this + _check_inverter_quit_dc_flag([](const InverterStatus_s & status) -> bool {return status.quit_dc_on;}), + _check_inverter_error_flag([](const InverterStatus_s & status) -> bool {return status.error;}), + _check_inverter_hv_present_flag([](const InverterStatus_s & status) -> bool {return status.dc_on;}), + _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return status.dc_bus_voltage > constants::MINIMUM_HV_VOLTAGE;}), // TODO fix this + _check_inverter_enabled([](const InverterStatus_s & status) -> bool {return !status.quit_inverter_on;}) { }; DrivetrainState_e DrivetrainSystem::get_state() @@ -175,10 +175,8 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm bool inverter_error_present = false; inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool requesting_speed_mode = false; - requesting_speed_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_SPEED_MODE); - bool requesting_torque_mode = false; - requesting_torque_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); + bool requesting_drive = false; + requesting_drive = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_DRIVE_MODE); // for now i wont worry about checking whether or not the inverters are still enabled. // bool inverters_enabled = _check_inverter_flags(_check_inverter_enabled); @@ -188,19 +186,11 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm _set_drivetrain_disabled(); _set_state(DrivetrainState_e::ERROR); } - else if(requesting_speed_mode) + else if(requesting_drive) { - // on transition the pin will be written to the correct state (for speed mode it should be off) - _set_enable_drivetrain(); // this is just being verbose here, underlying on the inverter's interfaces all maintain - _set_state(DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE); - } else if(requesting_torque_mode) - { - // on transition the pin will be written to the correct state (for torq mode it should be on) - _set_enable_drivetrain(); - _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); - } - + _set_state(DrivetrainState_e::ENABLED_DRIVE_MODE); + } break; } @@ -212,13 +202,10 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm break; } - // TODO may need to verify that the gpio state is correct while in this state (GPIO torque mode low) bool inverter_error_present = false; - inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); - bool user_requesting_speed_command = false; - user_requesting_speed_command = etl::holds_alternative(cmd); - bool user_requesting_torque_mode = false; - user_requesting_torque_mode = etl::holds_alternative(cmd) && (etl::get(cmd).init_drivetrain == DrivetrainModeRequest_e::INIT_TORQUE_MODE); + inverter_error_present = _check_inverter_flags(_check_inverter_error_flag); + + bool valid_drivetrain_command = etl::holds_alternative(cmd); if(inverter_error_present) { @@ -226,17 +213,13 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm _set_state(DrivetrainState_e::ERROR); break; } - else if(user_requesting_speed_command) - { - _set_drivetrain_speed_command(etl::get(cmd)); - - } else if(user_requesting_torque_mode && !_drivetrain_active(_active_rpm_level)) - { - _set_drivetrain_keepalive_idle(); - _set_state(DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE); + else if (valid_drivetrain_command) { + DrivetrainCommand_s drivetrain_command = etl::get(cmd); + _set_drivetrain_command(drivetrain_command); } break; } + case DrivetrainState_e::ERROR: { bool user_requesting_error_reset = etl::holds_alternative(cmd) && (etl::get(cmd).reset_errors); @@ -266,77 +249,6 @@ void DrivetrainSystem::_set_state(DrivetrainState_e new_state) _handle_entry_logic(new_state); } -void DrivetrainSystem::_handle_exit_logic(DrivetrainState_e prev_state) -{ - switch (prev_state) - { - case DrivetrainState_e::NOT_CONNECTED: - break; - case DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT: - break; - case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: - break; - case DrivetrainState_e::INVERTERS_READY: - break; - case DrivetrainState_e::INVERTERS_HV_ENABLED: - break; - case DrivetrainState_e::INVERTERS_ENABLED: - break; - case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: - break; - case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: - break; - case DrivetrainState_e::ENABLED_SPEED_MODE: - break; - case DrivetrainState_e::ENABLED_TORQUE_MODE: - break; - case DrivetrainState_e::ERROR: - break; - default: - break; - } -} - -void DrivetrainSystem::_handle_entry_logic(DrivetrainState_e new_state) -{ - switch (new_state) - { - case DrivetrainState_e::NOT_CONNECTED: - break; - case DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT: - break; - case DrivetrainState_e::NOT_ENABLED_HV_PRESENT: - break; - case DrivetrainState_e::INVERTERS_READY: - break; - case DrivetrainState_e::INVERTERS_HV_ENABLED: - break; - case DrivetrainState_e::INVERTERS_ENABLED: - break; - - case DrivetrainState_e::ENABLING_INVERTERS_SPEED_MODE: - { - // set the torque mode pin state to false (aka: keep the inverter in the main control mode which is speed mode) - _set_gpio_state({false}); - break; - } - - case DrivetrainState_e::ENABLING_INVERTERS_TORQUE_MODE: - { - _set_gpio_state({true}); - break; - } - - case DrivetrainState_e::ENABLED_SPEED_MODE: - break; - case DrivetrainState_e::ENABLED_TORQUE_MODE: - break; - case DrivetrainState_e::ERROR: - break; - default: - break; - } -} // returns false if any of the inverters fail the flag check. bool DrivetrainSystem::_check_inverter_flags(std::function flag_check_func) { @@ -415,20 +327,12 @@ void DrivetrainSystem::_set_drivetrain_error_reset() func.set_idle(); } } -void DrivetrainSystem::_set_drivetrain_speed_command(DrivetrainSpeedCommand_s cmd) -{ - _inverter_interfaces.FL.set_speed(cmd.desired_speed_rpm.FL, cmd.torque_limit_nm.FL); - _inverter_interfaces.FR.set_speed(cmd.desired_speed_rpm.FR, cmd.torque_limit_nm.FR); - _inverter_interfaces.RL.set_speed(cmd.desired_speed_rpm.RL, cmd.torque_limit_nm.RL); - _inverter_interfaces.RR.set_speed(cmd.desired_speed_rpm.RR, cmd.torque_limit_nm.RR); -} - -void DrivetrainSystem::_set_drivetrain_torque_command(DrivetrainTorqueCommand_s cmd) +void DrivetrainSystem::_set_drivetrain_command(DrivetrainCommand_s cmd) { - _inverter_interfaces.FL.set_torque(cmd.desired_torque_nm.FL); - _inverter_interfaces.FR.set_torque(cmd.desired_torque_nm.FR); - _inverter_interfaces.RL.set_torque(cmd.desired_torque_nm.RL); - _inverter_interfaces.RR.set_torque(cmd.desired_torque_nm.RR); + _inverter_interfaces.FL.set_speed(cmd.speeds_rpm[0], cmd.inverter_torque_limit[0]); + _inverter_interfaces.FR.set_speed(cmd.speeds_rpm[1], cmd.inverter_torque_limit[1]); + _inverter_interfaces.RL.set_speed(cmd.speeds_rpm[2], cmd.inverter_torque_limit[2]); + _inverter_interfaces.RR.set_speed(cmd.speeds_rpm[3], cmd.inverter_torque_limit[3]); } bool DrivetrainSystem::_drivetrain_active(float max_active_rpm) @@ -437,8 +341,8 @@ bool DrivetrainSystem::_drivetrain_active(float max_active_rpm) auto funcs_arr = _inverter_interfaces.as_array(); for(const auto & func : funcs_arr) { - auto inv_status = func.get_status(); - if(inv_status.speed_rpm >= max_active_rpm) + auto motor_mechanics = func.get_motor_mechanics(); + if(motor_mechanics.actual_speed >= max_active_rpm) { return true; } diff --git a/platformio.ini b/platformio.ini index 0722dc3..05f3d88 100644 --- a/platformio.ini +++ b/platformio.ini @@ -9,8 +9,7 @@ lib_deps_shared = interfaces-lib https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 - https://github.com/hytech-racing/shared_firmware_types.git - ; ../shared_firmware_types + https://github.com/hytech-racing/shared_firmware_types.git#9f4f017e5fd2a359d1123df37efef017ed06222e https://github.com/ssilverman/QNEthernet#v0.26.0 https://github.com/hytech-racing/HT_SCHED Embedded Template Library@^20.39.4 From 64da78617d8adacc157858d86d2bc3f6aa46a59e Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Thu, 13 Feb 2025 19:51:56 -0500 Subject: [PATCH 33/36] chill --- lib/systems/include/DrivetrainSystem.h | 21 +-------------------- lib/systems/src/DrivetrainSystem.cpp | 17 +++++++---------- 2 files changed, 8 insertions(+), 30 deletions(-) diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 77c3077..8251d30 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -62,18 +62,6 @@ struct DrivetrainResetError_s bool reset_errors; // true: reset the errors present on inverters, false: dont }; -// output pin of micro, read by inverters -struct DrivetrainOutputPins_s -{ - bool torque_mode_pin_state : 1; -}; - -// the pin set by the inverters themselves ("input": pin being read by micro) -struct DrivetrainInputPins_s -{ - bool torque_mode_enabled_pin_state : 1; -}; - enum DrivetrainModeRequest_e { UNINITIALIZED = 0, @@ -97,7 +85,6 @@ class DrivetrainSystem struct InverterFuncts { std::function set_speed; - std::function set_torque; std::function set_idle; std::function set_inverter_control_word; std::function get_status; @@ -110,8 +97,6 @@ class DrivetrainSystem bool _drivetrain_active(float max_active_rpm); void _set_state(DrivetrainState_e state); - void _handle_exit_logic(DrivetrainState_e prev_state); - void _handle_entry_logic(DrivetrainState_e new_state); void _set_drivetrain_disabled(); void _set_drivetrain_keepalive_idle(); @@ -132,11 +117,7 @@ class DrivetrainSystem std::function _check_inverter_error_flag; std::function _check_inverter_hv_present_flag; std::function _check_inverter_hv_not_present_flag; - std::function _check_inverter_enabled; - - std::function _set_gpio_state; - std::function _get_gpio_state; - + std::function _check_inverter_enabled; }; #endif /* DRIVETRAINSYSTEM */ \ No newline at end of file diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index 24b5b75..e3354f9 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -6,7 +6,7 @@ DrivetrainSystem::DrivetrainSystem( veh_vec inverter_interfaces) : _inverter_interfaces(inverter_interfaces), _state(DrivetrainState_e::NOT_CONNECTED), _check_inverter_ready_flag([](const InverterStatus_s & status) -> bool {return status.system_ready;}), - _check_inverter_connected_flag([](const InverterStatus_s & status) -> bool {return status.connected;}), // TODO fix this + _check_inverter_connected_flag([](const InverterStatus_s & status) -> bool {return status.connected;}), _check_inverter_quit_dc_flag([](const InverterStatus_s & status) -> bool {return status.quit_dc_on;}), _check_inverter_error_flag([](const InverterStatus_s & status) -> bool {return status.error;}), _check_inverter_hv_present_flag([](const InverterStatus_s & status) -> bool {return status.dc_on;}), @@ -63,7 +63,6 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm { _set_state(DrivetrainState_e::NOT_ENABLED_HV_PRESENT); } - _set_drivetrain_disabled(); // TODO dont know if this should be sent here, but it shouldn't hurt break; } @@ -151,10 +150,10 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm _set_drivetrain_disabled(); _set_state(DrivetrainState_e::ERROR); } - else if(requesting_init && hv_enabled && inverters_ready && hv_enabled && !inverters_enabled) + else if(requesting_init && hv_enabled && inverters_ready && !inverters_enabled) { _set_enable_drivetrain(); // should be done on entry of this state - } else if(hv_enabled && inverters_ready && hv_enabled && inverters_enabled) + } else if(hv_enabled && inverters_ready && inverters_enabled) { _set_enable_drivetrain(); _set_state(DrivetrainState_e::INVERTERS_ENABLED); @@ -244,9 +243,7 @@ DrivetrainState_e DrivetrainSystem::_evaluate_state_machine(DrivetrainSystem::Cm void DrivetrainSystem::_set_state(DrivetrainState_e new_state) { - _handle_exit_logic(_state); _state = new_state; - _handle_entry_logic(new_state); } // returns false if any of the inverters fail the flag check. @@ -329,10 +326,10 @@ void DrivetrainSystem::_set_drivetrain_error_reset() } void DrivetrainSystem::_set_drivetrain_command(DrivetrainCommand_s cmd) { - _inverter_interfaces.FL.set_speed(cmd.speeds_rpm[0], cmd.inverter_torque_limit[0]); - _inverter_interfaces.FR.set_speed(cmd.speeds_rpm[1], cmd.inverter_torque_limit[1]); - _inverter_interfaces.RL.set_speed(cmd.speeds_rpm[2], cmd.inverter_torque_limit[2]); - _inverter_interfaces.RR.set_speed(cmd.speeds_rpm[3], cmd.inverter_torque_limit[3]); + _inverter_interfaces.FL.set_speed(cmd.desired_speeds.FL, cmd.torque_limits.FL); + _inverter_interfaces.FR.set_speed(cmd.desired_speeds.FR, cmd.torque_limits.FR); + _inverter_interfaces.RL.set_speed(cmd.desired_speeds.RL, cmd.torque_limits.RL); + _inverter_interfaces.RR.set_speed(cmd.desired_speeds.RR, cmd.torque_limits.RR); } bool DrivetrainSystem::_drivetrain_active(float max_active_rpm) From ebea17ea8acbd1ac3483f71f02d73160347d49f5 Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Thu, 13 Feb 2025 20:41:54 -0500 Subject: [PATCH 34/36] add inverter params --- lib/interfaces/include/InverterInterface.h | 10 +++++++++- lib/interfaces/src/InverterInterface.cpp | 1 + lib/systems/include/DrivetrainSystem.h | 5 ----- lib/systems/src/DrivetrainSystem.cpp | 4 ++-- 4 files changed, 12 insertions(+), 8 deletions(-) diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index b2f2d7d..a568e5a 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -18,6 +18,11 @@ namespace HTUnits using volts = float; }; +struct InverterParams_s +{ + float MINIMUM_HV_VOLTAGE; +}; + /** * Struct containing id info for this specific inverter interface */ @@ -52,6 +57,7 @@ struct InverterControlWord_s **/ struct InverterStatus_s { + bool hv_present : 1; bool connected : 1; bool new_data : 1; bool system_ready : 1; @@ -117,7 +123,8 @@ class InverterInterface InverterInterface( CANBufferType *msg_output_queue, uint32_t mc_setpoint_commands_id, - uint32_t mc_torque_command_id) : msg_queue_(msg_output_queue) + uint32_t mc_torque_command_id, + InverterParams_s inverter_params) : msg_queue_(msg_output_queue), _inverter_params(inverter_params) { inverter_ids.mc_setpoint_commands_id = mc_setpoint_commands_id; inverter_ids.mc_torque_command_id = mc_torque_command_id; @@ -161,6 +168,7 @@ class InverterInterface InverterSetpoints_s _inverter_setpoints; InverterControlWord_s _inverter_control_word; InverterFeedbackData_s _feedback_data; + InverterParams_s _inverter_params; /* Getters */ InverterStatus_s get_status(); diff --git a/lib/interfaces/src/InverterInterface.cpp b/lib/interfaces/src/InverterInterface.cpp index daa1a94..ca5beb1 100644 --- a/lib/interfaces/src/InverterInterface.cpp +++ b/lib/interfaces/src/InverterInterface.cpp @@ -52,6 +52,7 @@ void InverterInterface::recieve_MCI_STATUS(CAN_message_t &can_msg) _feedback_data.status.derating_on = unpacked_msg.derating_on; _feedback_data.status.dc_bus_voltage = unpacked_msg.dc_bus_voltage; _feedback_data.status.diagnostic_number = unpacked_msg.diagnostic_number; + _feedback_data.status.hv_present = _feedback_data.status.dc_bus_voltage > _inverter_params.MINIMUM_HV_VOLTAGE; } void InverterInterface::recieve_MCI_TEMPS(CAN_message_t &can_msg) diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 8251d30..f10823d 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -25,11 +25,6 @@ // - [ ] be able to reset drivetrain // - [ ] - -namespace constants { - float MINIMUM_HV_VOLTAGE = 400.0; -} - // TODO move these into the shared types after finishing the system enum class DrivetrainState_e { diff --git a/lib/systems/src/DrivetrainSystem.cpp b/lib/systems/src/DrivetrainSystem.cpp index e3354f9..3740749 100644 --- a/lib/systems/src/DrivetrainSystem.cpp +++ b/lib/systems/src/DrivetrainSystem.cpp @@ -4,13 +4,13 @@ DrivetrainSystem::DrivetrainSystem( veh_vec inverter_interfaces) - : _inverter_interfaces(inverter_interfaces), _state(DrivetrainState_e::NOT_CONNECTED), + : _inverter_interfaces(inverter_interfaces), _state(DrivetrainState_e::NOT_CONNECTED), _check_inverter_ready_flag([](const InverterStatus_s & status) -> bool {return status.system_ready;}), _check_inverter_connected_flag([](const InverterStatus_s & status) -> bool {return status.connected;}), _check_inverter_quit_dc_flag([](const InverterStatus_s & status) -> bool {return status.quit_dc_on;}), _check_inverter_error_flag([](const InverterStatus_s & status) -> bool {return status.error;}), _check_inverter_hv_present_flag([](const InverterStatus_s & status) -> bool {return status.dc_on;}), - _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return status.dc_bus_voltage > constants::MINIMUM_HV_VOLTAGE;}), // TODO fix this + _check_inverter_hv_not_present_flag([](const InverterStatus_s & status) -> bool {return status.hv_present;}), // TODO fix this _check_inverter_enabled([](const InverterStatus_s & status) -> bool {return !status.quit_inverter_on;}) { }; From f725a8d5519d3fc847c13adc76d336874bb4adb5 Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Thu, 13 Feb 2025 22:16:20 -0500 Subject: [PATCH 35/36] Resolve PR comments --- lib/interfaces/include/HytechCANInterface.h | 157 -------------------- lib/interfaces/include/InverterInterface.h | 18 +-- lib/interfaces/src/HyTechCANInterface.cpp | 36 ----- lib/interfaces/src/InverterInterface.cpp | 43 ++---- platformio.ini | 4 +- 5 files changed, 21 insertions(+), 237 deletions(-) delete mode 100644 lib/interfaces/include/HytechCANInterface.h delete mode 100644 lib/interfaces/src/HyTechCANInterface.cpp diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h deleted file mode 100644 index f19460f..0000000 --- a/lib/interfaces/include/HytechCANInterface.h +++ /dev/null @@ -1,157 +0,0 @@ -#ifndef HYTECHCANINTERFACE -#define HYTECHCANINTERFACE - -#include -#include -#include -#include -#include -#include - -/* Recieve Buffers */ -extern CANBufferType CAN1_rxBuffer; -extern CANBufferType CAN2_rxBuffer; -extern CANBufferType CAN3_rxBuffer; - -/* Transfer Buffers */ -extern CANBufferType CAN1_txBuffer; -extern CANBufferType CAN2_txBuffer; -extern CANBufferType CAN3_txBuffer; - -/** - * Struct holding the interfaces processed by the ring buffer - */ - -struct CANInterfaces -{ - InverterInterface front_left_inv; - InverterInterface front_right_inv; - InverterInterface back_left_inv; - InverterInterface back_right_inv; -}; - -/** Methods called on can recieve */ -void on_can1_recieve(const CAN_message_t &msg); - -void on_can2_recieve(const CAN_message_t &msg); - -void on_can3_recieve(const CAN_message_t &msg); - -/** - * Recieving CAN messages - * (needs to be called in a loop) - */ -template -void process_ring_buffer(BufferType &rx_buffer, CANInterfaces interfaces) { - while (rx_buffer.available()) - { - CAN_message_t recvd_msg; - uint8_t buf[sizeof(CAN_message_t)]; - rx_buffer.pop_front(buf, sizeof(CAN_message_t)); - memmove(&recvd_msg, buf, sizeof(recvd_msg)); - switch (recvd_msg.id) - { - // FL Inverter - case (MCI1_STATUS_CANID): - interfaces.front_left_inv.recieve_MCI_STATUS(recvd_msg); - break; - - case (MCI1_TEMPS_CANID): - interfaces.front_left_inv.recieve_MCI_TEMPS(recvd_msg); - break; - - case (MCI1_DYNAMICS_CANID): - interfaces.front_left_inv.recieve_MCI_DYNAMICS(recvd_msg); - break; - - case (MCI1_POWER_CANID): - interfaces.front_left_inv.recieve_MCI_POWER(recvd_msg); - break; - - case (MCI1_FEEDBACK_CANID): - interfaces.front_left_inv.recieve_MCI_FEEDBACK(recvd_msg); - - // FR inverter - case (MCI2_STATUS_CANID): - interfaces.front_right_inv.recieve_MCI_STATUS(recvd_msg); - break; - - case (MCI2_TEMPS_CANID): - interfaces.front_right_inv.recieve_MCI_TEMPS(recvd_msg); - break; - - case (MCI2_DYNAMICS_CANID): - interfaces.front_right_inv.recieve_MCI_DYNAMICS(recvd_msg); - break; - - case (MCI2_POWER_CANID): - interfaces.front_right_inv.recieve_MCI_POWER(recvd_msg); - break; - - case (MCI2_FEEDBACK_CANID): - interfaces.front_right_inv.recieve_MCI_FEEDBACK(recvd_msg); - - // RL Inverter - case (MCI3_STATUS_CANID): - interfaces.back_left_inv.recieve_MCI_STATUS(recvd_msg); - break; - - case (MCI3_TEMPS_CANID): - interfaces.back_left_inv.recieve_MCI_TEMPS(recvd_msg); - break; - - case (MCI3_DYNAMICS_CANID): - interfaces.back_left_inv.recieve_MCI_DYNAMICS(recvd_msg); - break; - - case (MCI3_POWER_CANID): - interfaces.back_left_inv.recieve_MCI_POWER(recvd_msg); - break; - - case (MCI3_FEEDBACK_CANID): - interfaces.back_left_inv.recieve_MCI_FEEDBACK(recvd_msg); - - // RR Inverter - case (MCI4_STATUS_CANID): - interfaces.back_right_inv.recieve_MCI_STATUS(recvd_msg); - break; - - case (MCI4_TEMPS_CANID): - interfaces.back_right_inv.recieve_MCI_TEMPS(recvd_msg); - break; - - case (MCI4_DYNAMICS_CANID): - interfaces.back_right_inv.recieve_MCI_DYNAMICS(recvd_msg); - break; - - case (MCI4_POWER_CANID): - interfaces.back_right_inv.recieve_MCI_POWER(recvd_msg); - break; - - case (MCI4_FEEDBACK_CANID): - interfaces.back_right_inv.recieve_MCI_FEEDBACK(recvd_msg); - - } - - } -} - -/** -* Sending CAN messages -* (needs to be called in loop) -* */ -template -void send_all_CAN_msgs(bufferType &tx_buffer, FlexCAN_T4_Base *can_interface) -{ - CAN_message_t msg; - while (tx_buffer.available()) - { - CAN_message_t msg; - uint8_t buf[sizeof(CAN_message_t)]; - tx_buffer.pop_front(buf, sizeof(CAN_message_t)); - memmove(&msg, buf, sizeof(msg)); - can_interface->write(msg); - } -} - -#endif /* HYTECHCANINTERFACE */ \ No newline at end of file diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index a568e5a..c8a4388 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -7,6 +7,7 @@ #include #include "DrivetrainSystem.h" +#include namespace HTUnits { @@ -132,21 +133,18 @@ class InverterInterface // TODO un-public these (they are public for testing) - /* Recieving callbacks */ - void recieve_MCI_STATUS(CAN_message_t &can_msg); + /* receiving callbacks */ + void receive_MCI_STATUS(CAN_message_t &can_msg); - void recieve_MCI_TEMPS(CAN_message_t &can_msg); + void receive_MCI_TEMPS(CAN_message_t &can_msg); - void recieve_MCI_DYNAMICS(CAN_message_t &can_msg); + void receive_MCI_DYNAMICS(CAN_message_t &can_msg); - void recieve_MCI_POWER(CAN_message_t &can_msg); + void receive_MCI_POWER(CAN_message_t &can_msg); - void recieve_MCI_FEEDBACK(CAN_message_t &can_msg); + void receive_MCI_FEEDBACK(CAN_message_t &can_msg); /* Sending */ - template - void enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *), uint32_t id); - void send_MC_SETPOINT_COMMAND(); void send_MC_CONTROL_WORD(); @@ -154,8 +152,6 @@ class InverterInterface /* Inverter Functs */ void set_speed(float desired_rpm, float torque_limit_nm); - void set_torque(float torque_nm); - void set_idle(); void set_inverter_control_word(InverterControlWord_s control_word); diff --git a/lib/interfaces/src/HyTechCANInterface.cpp b/lib/interfaces/src/HyTechCANInterface.cpp deleted file mode 100644 index 863980c..0000000 --- a/lib/interfaces/src/HyTechCANInterface.cpp +++ /dev/null @@ -1,36 +0,0 @@ -#include -#include - -/* Recieve Buffers */ -CANBufferType CAN1_rxBuffer; -CANBufferType CAN2_rxBuffer; -CANBufferType CAN3_rxBuffer; - -/* Transfer Buffers */ -CANBufferType CAN1_txBuffer; -CANBufferType CAN2_txBuffer; -CANBufferType CAN3_txBuffer; - -/* Methods called on can recieve */ -void on_can1_recieve(const CAN_message_t &msg) -{ - uint8_t buf[sizeof(CAN_message_t)]; - memmove(buf, &msg, sizeof(msg)); - CAN1_rxBuffer.push_back(buf, sizeof(CAN_message_t)); -} - -void on_can2_recieve(const CAN_message_t &msg) -{ - uint8_t buf[sizeof(CAN_message_t)]; - memmove(buf, &msg, sizeof(msg)); - CAN2_rxBuffer.push_back(buf, sizeof(CAN_message_t)); - -} - -void on_can3_recieve(const CAN_message_t &msg) -{ - uint8_t buf[sizeof(CAN_message_t)]; - memmove(buf, &msg, sizeof(msg)); - CAN3_rxBuffer.push_back(buf, sizeof(CAN_message_t)); - -} \ No newline at end of file diff --git a/lib/interfaces/src/InverterInterface.cpp b/lib/interfaces/src/InverterInterface.cpp index ca5beb1..94521fd 100644 --- a/lib/interfaces/src/InverterInterface.cpp +++ b/lib/interfaces/src/InverterInterface.cpp @@ -32,21 +32,21 @@ InverterControlParams_s InverterInterface::get_control_params() { /** - * Recieving CAN messages + * receiving CAN messages */ -void InverterInterface::recieve_MCI_STATUS(CAN_message_t &can_msg) +void InverterInterface::receive_MCI_STATUS(CAN_message_t &can_msg) { // Unpack the message MCI1_STATUS_t unpacked_msg; Unpack_MCI1_STATUS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); // Update inverter interface with new data - _feedback_data.status.connected = true; // Will set to true once first CAN message has been recieved + _feedback_data.status.connected = true; // Will set to true once first CAN message has been received _feedback_data.status.system_ready = unpacked_msg.system_ready; _feedback_data.status.error = unpacked_msg.error; _feedback_data.status.warning = unpacked_msg.warning; - _feedback_data. status.quit_dc_on = unpacked_msg.quit_dc_on; + _feedback_data.status.quit_dc_on = unpacked_msg.quit_dc_on; _feedback_data.status.dc_on = unpacked_msg.dc_on; _feedback_data.status.quit_inverter_on = unpacked_msg.quit_inverter_on; _feedback_data.status.derating_on = unpacked_msg.derating_on; @@ -55,11 +55,9 @@ void InverterInterface::recieve_MCI_STATUS(CAN_message_t &can_msg) _feedback_data.status.hv_present = _feedback_data.status.dc_bus_voltage > _inverter_params.MINIMUM_HV_VOLTAGE; } -void InverterInterface::recieve_MCI_TEMPS(CAN_message_t &can_msg) +void InverterInterface::receive_MCI_TEMPS(CAN_message_t &can_msg) { - Serial.println("Recieved temps data!"); - // Unpack the message MCI1_TEMPS_t unpacked_msg; Unpack_MCI1_TEMPS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); @@ -72,7 +70,7 @@ void InverterInterface::recieve_MCI_TEMPS(CAN_message_t &can_msg) } -void InverterInterface::recieve_MCI_DYNAMICS(CAN_message_t &can_msg) +void InverterInterface::receive_MCI_DYNAMICS(CAN_message_t &can_msg) { // Unpack the message @@ -87,7 +85,7 @@ void InverterInterface::recieve_MCI_DYNAMICS(CAN_message_t &can_msg) } -void InverterInterface::recieve_MCI_POWER(CAN_message_t &can_msg) +void InverterInterface::receive_MCI_POWER(CAN_message_t &can_msg) { // Unpack the message MCI1_POWER_t unpacked_msg; @@ -100,7 +98,7 @@ void InverterInterface::recieve_MCI_POWER(CAN_message_t &can_msg) } -void InverterInterface::recieve_MCI_FEEDBACK(CAN_message_t &can_msg) +void InverterInterface::receive_MCI_FEEDBACK(CAN_message_t &can_msg) { // Unpack the message MCI1_FEEDBACK_t unpacked_msg; @@ -118,17 +116,6 @@ void InverterInterface::recieve_MCI_FEEDBACK(CAN_message_t &can_msg) * Sending CAN messages */ -template -void InverterInterface::enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *), uint32_t id) -{ - CAN_message_t can_msg; - pack_function(structure, can_msg.buf, &can_msg.len, (uint8_t *)&can_msg.flags.extended); - can_msg.id = id; - uint8_t buf[sizeof(CAN_message_t)] = {}; - memmove(buf, &can_msg, sizeof(CAN_message_t)); - msg_queue_->push_back(buf, sizeof(CAN_message_t)); -} - void InverterInterface::send_MC_SETPOINT_COMMAND() { MC1_SETPOINTS_COMMAND_t msg_out; @@ -137,7 +124,7 @@ void InverterInterface::send_MC_SETPOINT_COMMAND() msg_out.positive_torque_limit_ro = _inverter_setpoints.positive_torque_limit; msg_out.negative_torque_limit_ro = _inverter_setpoints.negative_torque_limit; - enqueue_new_CAN(&msg_out, &Pack_MC1_SETPOINTS_COMMAND_hytech, inverter_ids.mc_setpoint_commands_id); + CAN_util::enqueue_msg(&msg_out, &Pack_MC1_SETPOINTS_COMMAND_hytech, inverter_ids.mc_setpoint_commands_id); } void InverterInterface::send_MC_CONTROL_WORD() @@ -152,15 +139,11 @@ void InverterInterface::send_MC_CONTROL_WORD() void InverterInterface::set_speed(float desired_rpm, float torque_limit_nm) { _inverter_setpoints.speed_rpm_setpoint = desired_rpm; - _inverter_setpoints.positive_torque_limit = torque_limit_nm; - _inverter_setpoints.negative_torque_limit = -torque_limit_nm; -} -void InverterInterface::set_torque(float torque_nm) -{ - // TODO set desired rpm to max? - _inverter_setpoints.positive_torque_limit = torque_nm; - _inverter_setpoints.negative_torque_limit = -torque_nm; + float converted_torque = std::abs(torque_limit_nm * (1000/9.8)); + + _inverter_setpoints.positive_torque_limit = converted_torque; + _inverter_setpoints.negative_torque_limit = -converted_torque; } void InverterInterface::set_idle() diff --git a/platformio.ini b/platformio.ini index 05f3d88..a69d9eb 100644 --- a/platformio.ini +++ b/platformio.ini @@ -7,7 +7,6 @@ lib_deps_shared = - interfaces-lib https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 https://github.com/hytech-racing/shared_firmware_types.git#9f4f017e5fd2a359d1123df37efef017ed06222e https://github.com/ssilverman/QNEthernet#v0.26.0 @@ -45,8 +44,7 @@ test_ignore = lib_deps = ${common.lib_deps_shared} https://github.com/tonton81/FlexCAN_T4 - https://github.com/hytech-racing/shared_firmware_interfaces.git - https://github.com/RCMast3r/hytech_can#testing_new_inv_ids + https://github.com/hytech-racing/shared_firmware_interfaces.git#4d7d617d63720f5fef586f1d95ecd979c1f8a075 https://github.com/hytech-racing/HT_CAN/releases/download/134/can_lib.tar.gz From 84806107998cff29f42e8d43441e6b7eeebd5cf3 Mon Sep 17 00:00:00 2001 From: Krish Kittur <73321517+KrishKittur@users.noreply.github.com> Date: Sat, 15 Feb 2025 02:05:09 -0500 Subject: [PATCH 36/36] Updated the inverter interface to include new CAN descriptions --- lib/interfaces/include/InverterInterface.h | 59 +++-- lib/interfaces/src/InverterInterface.cpp | 90 ++++--- .../include/VehicleStateMachine.h | 126 ++++------ lib/state_machine/src/VehicleStateMachine.cpp | 232 ++++++------------ lib/systems/README.md | 23 ++ platformio.ini | 2 +- 6 files changed, 245 insertions(+), 287 deletions(-) diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index c8a4388..b9a8f59 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -29,27 +29,35 @@ struct InverterParams_s */ struct InverterIds_s { - uint32_t mc_setpoint_commands_id; - uint32_t mc_torque_command_id; + uint32_t inv_control_word_id; + uint32_t inv_control_input_id; + uint32_t inv_control_parameter_id; }; /** * Drivetrain system accessible structs for * requesting change of state */ -struct InverterSetpoints_s +struct InverterControlWord_s +{ + bool inverter_enable : 1; + bool hv_enable : 1; + bool driver_enable : 1; + bool remove_error : 1; +}; + +struct InverterControlInput_s { int16_t speed_rpm_setpoint; int16_t positive_torque_limit; int16_t negative_torque_limit; }; -struct InverterControlWord_s +struct InverterControlParams_s { - bool inverter_enable : 1; - bool hv_enable : 1; - bool driver_enable : 1; - bool remove_error : 1; + uint16_t speed_control_kp; + uint16_t speed_control_ki; + uint16_t speed_control_kd; }; /** @@ -96,7 +104,7 @@ struct MotorMechanics_s HTUnits::speed_rpm actual_speed; }; -struct InverterControlParams_s +struct InverterControlFeedback_s { bool new_data : 1; uint16_t speed_control_kp; @@ -110,7 +118,7 @@ struct InverterFeedbackData_s InverterTemps_s temps; InverterPower_s power; MotorMechanics_s motor_mechanics; - InverterControlParams_s control_params; + InverterControlFeedback_s control_feedback; }; /** @@ -123,31 +131,35 @@ class InverterInterface InverterInterface( CANBufferType *msg_output_queue, - uint32_t mc_setpoint_commands_id, - uint32_t mc_torque_command_id, + uint32_t inv_control_word_id, + uint32_t inv_control_input_id, + uint32_t inv_control_params_id, InverterParams_s inverter_params) : msg_queue_(msg_output_queue), _inverter_params(inverter_params) { - inverter_ids.mc_setpoint_commands_id = mc_setpoint_commands_id; - inverter_ids.mc_torque_command_id = mc_torque_command_id; + inverter_ids.inv_control_word_id = inv_control_word_id; + inverter_ids.inv_control_parameter_id = inv_control_params_id; + inverter_ids.inv_control_input_id = inv_control_input_id; } // TODO un-public these (they are public for testing) /* receiving callbacks */ - void receive_MCI_STATUS(CAN_message_t &can_msg); + void receive_INV_STATUS(CAN_message_t &can_msg); - void receive_MCI_TEMPS(CAN_message_t &can_msg); + void receive_INV_TEMPS(CAN_message_t &can_msg); - void receive_MCI_DYNAMICS(CAN_message_t &can_msg); + void receive_INV_DYNAMICS(CAN_message_t &can_msg); - void receive_MCI_POWER(CAN_message_t &can_msg); + void receive_INV_POWER(CAN_message_t &can_msg); - void receive_MCI_FEEDBACK(CAN_message_t &can_msg); + void receive_INV_FEEDBACK(CAN_message_t &can_msg); /* Sending */ - void send_MC_SETPOINT_COMMAND(); + void send_INV_SETPOINT_COMMAND(); + + void send_INV_CONTROL_WORD(); - void send_MC_CONTROL_WORD(); + void send_INV_CONTROL_PARAMS(); /* Inverter Functs */ void set_speed(float desired_rpm, float torque_limit_nm); @@ -161,8 +173,9 @@ class InverterInterface private: InverterIds_s inverter_ids; - InverterSetpoints_s _inverter_setpoints; + InverterControlInput_s _inverter_control_inputs; InverterControlWord_s _inverter_control_word; + InverterControlParams_s _inverter_control_params; InverterFeedbackData_s _feedback_data; InverterParams_s _inverter_params; @@ -171,7 +184,7 @@ class InverterInterface InverterTemps_s get_temps(); InverterPower_s get_power(); MotorMechanics_s get_motor_mechanics(); - InverterControlParams_s get_control_params(); + InverterControlFeedback_s get_control_params(); CANBufferType *msg_queue_; }; diff --git a/lib/interfaces/src/InverterInterface.cpp b/lib/interfaces/src/InverterInterface.cpp index 94521fd..1dd8eb2 100644 --- a/lib/interfaces/src/InverterInterface.cpp +++ b/lib/interfaces/src/InverterInterface.cpp @@ -25,9 +25,9 @@ MotorMechanics_s InverterInterface::get_motor_mechanics() { } -InverterControlParams_s InverterInterface::get_control_params() { - _feedback_data.control_params.new_data = false; - return _feedback_data.control_params; +InverterControlFeedback_s InverterInterface::get_control_params() { + _feedback_data.control_feedback.new_data = false; + return _feedback_data.control_feedback; } @@ -35,11 +35,11 @@ InverterControlParams_s InverterInterface::get_control_params() { * receiving CAN messages */ -void InverterInterface::receive_MCI_STATUS(CAN_message_t &can_msg) +void InverterInterface::receive_INV_STATUS(CAN_message_t &can_msg) { // Unpack the message - MCI1_STATUS_t unpacked_msg; - Unpack_MCI1_STATUS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + INV1_STATUS_t unpacked_msg; + Unpack_INV1_STATUS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); // Update inverter interface with new data _feedback_data.status.connected = true; // Will set to true once first CAN message has been received @@ -55,12 +55,12 @@ void InverterInterface::receive_MCI_STATUS(CAN_message_t &can_msg) _feedback_data.status.hv_present = _feedback_data.status.dc_bus_voltage > _inverter_params.MINIMUM_HV_VOLTAGE; } -void InverterInterface::receive_MCI_TEMPS(CAN_message_t &can_msg) +void InverterInterface::receive_INV_TEMPS(CAN_message_t &can_msg) { // Unpack the message - MCI1_TEMPS_t unpacked_msg; - Unpack_MCI1_TEMPS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + INV1_TEMPS_t unpacked_msg; + Unpack_INV1_TEMPS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); // Update inverter interface with new data _feedback_data.temps.igbt_temp = HYTECH_igbt_temp_ro_fromS(unpacked_msg.igbt_temp_ro); @@ -70,12 +70,12 @@ void InverterInterface::receive_MCI_TEMPS(CAN_message_t &can_msg) } -void InverterInterface::receive_MCI_DYNAMICS(CAN_message_t &can_msg) +void InverterInterface::receive_INV_DYNAMICS(CAN_message_t &can_msg) { // Unpack the message - MCI1_DYNAMICS_t unpacked_msg; - Unpack_MCI1_DYNAMICS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + INV1_DYNAMICS_t unpacked_msg; + Unpack_INV1_DYNAMICS_hytech(&unpacked_msg, can_msg.buf, can_msg.len); // Update inverter interface with new data _feedback_data.motor_mechanics.actual_power = unpacked_msg.actual_power_w; @@ -85,11 +85,11 @@ void InverterInterface::receive_MCI_DYNAMICS(CAN_message_t &can_msg) } -void InverterInterface::receive_MCI_POWER(CAN_message_t &can_msg) +void InverterInterface::receive_INV_POWER(CAN_message_t &can_msg) { // Unpack the message - MCI1_POWER_t unpacked_msg; - Unpack_MCI1_POWER_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + INV1_POWER_t unpacked_msg; + Unpack_INV1_POWER_hytech(&unpacked_msg, can_msg.buf, can_msg.len); // Update inverter interface with new data _feedback_data.power.active_power = unpacked_msg.active_power_w; @@ -98,17 +98,17 @@ void InverterInterface::receive_MCI_POWER(CAN_message_t &can_msg) } -void InverterInterface::receive_MCI_FEEDBACK(CAN_message_t &can_msg) +void InverterInterface::receive_INV_FEEDBACK(CAN_message_t &can_msg) { // Unpack the message - MCI1_FEEDBACK_t unpacked_msg; - Unpack_MCI1_FEEDBACK_hytech(&unpacked_msg, can_msg.buf, can_msg.len); + INV1_FEEDBACK_t unpacked_msg; + Unpack_INV1_FEEDBACK_hytech(&unpacked_msg, can_msg.buf, can_msg.len); // Update inverter interface with new data - _feedback_data.control_params.speed_control_kp = unpacked_msg.speed_control_kp; - _feedback_data.control_params.speed_control_ki = unpacked_msg.speed_control_ki; - _feedback_data.control_params.speed_control_kd = unpacked_msg.speed_control_kd; - _feedback_data.control_params.new_data = true; + _feedback_data.control_feedback.speed_control_kp = unpacked_msg.speed_control_kp; + _feedback_data.control_feedback.speed_control_ki = unpacked_msg.speed_control_ki; + _feedback_data.control_feedback.speed_control_kd = unpacked_msg.speed_control_kd; + _feedback_data.control_feedback.new_data = true; } @@ -116,20 +116,38 @@ void InverterInterface::receive_MCI_FEEDBACK(CAN_message_t &can_msg) * Sending CAN messages */ -void InverterInterface::send_MC_SETPOINT_COMMAND() +void InverterInterface::send_INV_SETPOINT_COMMAND() { - MC1_SETPOINTS_COMMAND_t msg_out; + INV1_CONTROL_INPUT_t msg_out; - msg_out.speed_setpoint_rpm = _inverter_setpoints.speed_rpm_setpoint; - msg_out.positive_torque_limit_ro = _inverter_setpoints.positive_torque_limit; - msg_out.negative_torque_limit_ro = _inverter_setpoints.negative_torque_limit; + msg_out.speed_setpoint_rpm = _inverter_control_inputs.speed_rpm_setpoint; + msg_out.positive_torque_limit_ro = _inverter_control_inputs.positive_torque_limit; + msg_out.negative_torque_limit_ro = _inverter_control_inputs.negative_torque_limit; - CAN_util::enqueue_msg(&msg_out, &Pack_MC1_SETPOINTS_COMMAND_hytech, inverter_ids.mc_setpoint_commands_id); + CAN_util::enqueue_msg(&msg_out, &Pack_INV1_CONTROL_INPUT_hytech, inverter_ids.inv_control_input_id); } -void InverterInterface::send_MC_CONTROL_WORD() +void InverterInterface::send_INV_CONTROL_WORD() { - // TODO edit PCAN project to finish this + INV1_CONTROL_WORD_t msg_out; + + msg_out.driver_enable = _inverter_control_word.driver_enable; + msg_out.hv_enable = _inverter_control_word.hv_enable; + msg_out.inverter_enable = _inverter_control_word.inverter_enable; + msg_out.remove_error = _inverter_control_word.remove_error; + + CAN_util::enqueue_msg(&msg_out, &Pack_INV1_CONTROL_WORD_hytech, inverter_ids.inv_control_word_id); +} + +void InverterInterface::send_INV_CONTROL_PARAMS() +{ + INV1_CONTROL_PARAMETER_t msg_out; + + msg_out.speed_control_kd = _inverter_control_params.speed_control_kp; + msg_out.speed_control_ki = _inverter_control_params.speed_control_ki; + msg_out.speed_control_kd = _inverter_control_params.speed_control_kd; + + CAN_util::enqueue_msg(&msg_out, &Pack_INV1_CONTROL_PARAMETER_hytech, inverter_ids.inv_control_parameter_id); } /** @@ -138,19 +156,19 @@ void InverterInterface::send_MC_CONTROL_WORD() void InverterInterface::set_speed(float desired_rpm, float torque_limit_nm) { - _inverter_setpoints.speed_rpm_setpoint = desired_rpm; + _inverter_control_inputs.speed_rpm_setpoint = desired_rpm; float converted_torque = std::abs(torque_limit_nm * (1000/9.8)); - _inverter_setpoints.positive_torque_limit = converted_torque; - _inverter_setpoints.negative_torque_limit = -converted_torque; + _inverter_control_inputs.positive_torque_limit = converted_torque; + _inverter_control_inputs.negative_torque_limit = -converted_torque; } void InverterInterface::set_idle() { - _inverter_setpoints.negative_torque_limit = 0; - _inverter_setpoints.positive_torque_limit = 0; - _inverter_setpoints.speed_rpm_setpoint = 0; + _inverter_control_inputs.negative_torque_limit = 0; + _inverter_control_inputs.positive_torque_limit = 0; + _inverter_control_inputs.speed_rpm_setpoint = 0; } void InverterInterface::set_inverter_control_word(InverterControlWord_s control_word) diff --git a/lib/state_machine/include/VehicleStateMachine.h b/lib/state_machine/include/VehicleStateMachine.h index 55d6b48..6ad3ac5 100644 --- a/lib/state_machine/include/VehicleStateMachine.h +++ b/lib/state_machine/include/VehicleStateMachine.h @@ -1,82 +1,62 @@ -#ifndef VEHICLE_STATE_MACHINE -#define VEHICLE_STATE_MACHINE +#ifndef __VEHICLE_STATE_MACHINE__ +#define __VEHICLE_STATE_MACHINE__ -/* From local systems library */ -#include "DrivetrainSystem.h" -#include "BuzzerController.h" +#include -/** - * Enum representing possible states for the vehicle's state machine. - * - * STARTUP - Initial state. Vehicle never stays in STARTUP for more than 1 tick. - * TRACTIVE_SYSTEM_NOT_ACTIVE - Car stays here until the Drivetrain reports that the inverters' voltage - * is above the HV threshold. - * TRACTICE_SYSTEM_ACTIVE - HV is on, car is waiting for brake + start button (to start car) - * ENABLING_INVERTERS - While in this state, the car calls the drivetrain's enable command. If - * successful, (usually nearly-immediate), then car goes into WRTDS. - * WAITING_READY_TO_DRIVE_SOUND - When entering state, buzzer is activated. After BuzzerController says - * enough time has passed, we enter the next state. - * READY_TO_DRIVE - While in this state, pedal inputs command the drivetrain. - */ -enum class CAR_STATE -{ - STARTUP = 0, - TRACTIVE_SYSTEM_NOT_ACTIVE = 1, - TRACTIVE_SYSTEM_ACTIVE = 2, - ENABLING_INVERTERS = 3, - WAITING_READY_TO_DRIVE_SOUND = 4, - READY_TO_DRIVE = 5 +enum class VEHICLE_STATE { + TRACTIVE_SYSTEM_NOT_ACTIVE = 1, + TRACTIVE_SYSTEM_ACTIVE = 2, + WANTING_READY_TO_DRIVE = 3, + READY_TO_DRIVE = 4 }; -/** - * This singleton class represents the vehicle's state machine as we enable HV, enable the inverters, - * wait for the start button to be pressed, and enter ready-to-drive mode. Aside from getters, this - * class only has one public function, tick_state_machine(). In order to run its update logic, the - * VehicleStateMachine uses instance data directly from other singleton classes (Buzzer, Pedals, etc) - * and from the global structs (VCRInterfaceData_s, VCRSystemData_s, etc.). - */ class VehicleStateMachine { -public: - VehicleStateMachine(DrivetrainSystem & drivetrain_system) : - _current_state(CAR_STATE::STARTUP), - _drivetrain(drivetrain_system), - _buzzer(BuzzerController::getInstance()) {}; - - - /** - * This tick() function handles all the update logic for traversing states, and calls the functions - * of other classes as necessary. - * @pre Other systems are updated properly - * @pre All relevant data exists in the data structs (VCRInterfaceData, VCRSystemData, etc.) - * @param current_millis The system time, in millis. Passed in by the scheduler. - * @param system_data A reference to the global system data struct. - */ - void tick_state_machine(unsigned long current_millis, const VCRSystemData_s &system_data); - - CAR_STATE get_state() { return _current_state; } - -private: - void set_state_(CAR_STATE new_state, unsigned long curr_time); - - /** - * The function run upon the entry of the car into a new state. - * @param new_state The state in which we are entering. - */ - void handle_entry_logic_(CAR_STATE new_state, unsigned long curr_millis); - - /** - * The function run upon the exit of a state. - * @param prev_state the state in which we are leaving. - */ - void handle_exit_logic_(CAR_STATE prev_state, unsigned long curr_millis); - - CAR_STATE _current_state; + public: + VehicleStateMachine( + etl::delegate hv_over_threshold, + etl::delegate start_button_pressed, + etl::delegate brake_pressed, + etl::delegate drivetrain_error_ocurred, + etl::delegate drivetrain_ready, + etl::delegate handle_drivetrain_init, + etl::delegate command_drivetrain + ) : + _hv_over_threshold(hv_over_threshold), + _start_button_pressed(start_button_pressed), + _brake_pressed(brake_pressed), + _drivetrain_error_ocurred(drivetrain_error_ocurred), + _drivetrain_ready(drivetrain_ready), + _handle_drivetrain_init(handle_drivetrain_init), + _command_drivetrain(command_drivetrain) + { + _current_state = VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE; + + } + + void tick_state_machine(unsigned long curr_time_millis); + + VEHICLE_STATE get_state() { return _current_state; } + + private: + + void _set_state(VEHICLE_STATE new_state, unsigned long current_time_millis); + + void _handle_entry_logic(VEHICLE_STATE prev_state, unsigned long current_time_millis); + + void _handle_exit_logic(VEHICLE_STATE new_state, unsigned long current_time_millis); + + VEHICLE_STATE _current_state; + + // Lambdas neccesary for state machine to work + etl::delegate _hv_over_threshold; + etl::delegate _start_button_pressed; + etl::delegate _brake_pressed; + etl::delegate _drivetrain_error_ocurred; + etl::delegate _drivetrain_ready; + etl::delegate _handle_drivetrain_init; + etl::delegate _command_drivetrain; // Shouldn't need to pass anything; logic will be handled in the lambda - /* System references to show dependence on systems library */ - DrivetrainSystem &_drivetrain; //TODO: Make this InverterInterface instead of uint32_t - BuzzerController &_buzzer; - // AMSSystem &_ams_system; }; -#endif /* VEHICLE_STATE_MACHINE */ +#endif \ No newline at end of file diff --git a/lib/state_machine/src/VehicleStateMachine.cpp b/lib/state_machine/src/VehicleStateMachine.cpp index d182629..2764159 100644 --- a/lib/state_machine/src/VehicleStateMachine.cpp +++ b/lib/state_machine/src/VehicleStateMachine.cpp @@ -4,192 +4,116 @@ /* Local includes */ #include "VehicleStateMachine.h" -void VehicleStateMachine::tick_state_machine(unsigned long current_millis, const VCRSystemData_s &system_data) +void VehicleStateMachine::tick_state_machine(unsigned long current_millis) { switch (_current_state) { - - case CAR_STATE::STARTUP: - { - hal_println("In startup state"); - set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - break; - } - - case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: - { - // if TS is above HV threshold, move to Tractive System Active - // _drivetrain.disable_no_pins(); - // if (_drivetrain.hv_over_threshold_on_drivetrain()) - // { - // set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); - // } - break; - } - - case CAR_STATE::TRACTIVE_SYSTEM_ACTIVE: - { - if (_buzzer.buzzer_is_active(current_millis)) + case VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: { - _buzzer.deactivate(); + if (_hv_over_threshold()) + { + _set_state(VEHICLE_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); + break; + } + break; } - // _drivetrain.disable_no_pins(); - - // if (!_drivetrain.hv_over_threshold_on_drivetrain()) - // { - // set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - // break; - // } - - if (system_data.dash_input_state.start_btn_is_pressed && system_data.pedals_system_data.brake_is_pressed) + case VEHICLE_STATE::TRACTIVE_SYSTEM_ACTIVE: { - set_state_(CAR_STATE::ENABLING_INVERTERS, current_millis); + if (!_hv_over_threshold()) + { + _set_state(VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + break; + } + + if (_start_button_pressed() && _brake_pressed()) + { + _set_state(VEHICLE_STATE::WANTING_READY_TO_DRIVE, current_millis); + break; + } break; } - break; - } - - case CAR_STATE::ENABLING_INVERTERS: - { - // TODO: replace old drivetrain function handling with new interaction paradigm - // If HV is not active, go to TRACTIVE_SYSTEM_NOT_ACTIVE - // if (_drivetrain.hv_over_threshold_on_drivetrain()) - // { - // set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - // break; - // } - - // If motor controllers have error, but HV still active - // if (_drivetrain.drivetrain_error_occured()) - // { - // set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); - // } - - // if (_drivetrain.handle_inverter_startup(current_millis)) - // { - // set_state_(CAR_STATE::WAITING_READY_TO_DRIVE_SOUND, current_millis); - // break; - // } - break; - } - - case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: - { - - // If HV is no longer active, return to TRACTIVE_SYSTEM_NOT_ACTIVE - // if (_drivetrain.hv_over_threshold_on_drivetrain()) - // { - // set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); - // break; - // } - - // _drivetrain.command_drivetrain_no_torque(); // While waiting for RTD sound to complete, always command 0 torque - - // If the ready-to-drive sound is done playing, move to ready to drive mode - if (!_buzzer.buzzer_is_active(current_millis)) + case VEHICLE_STATE::WANTING_READY_TO_DRIVE: { - set_state_(CAR_STATE::READY_TO_DRIVE, current_millis); - } - break; - - } - - case CAR_STATE::READY_TO_DRIVE: - { - - // If HV is no longer active, return to TRACTIVE_SYSTEM_NOT_ACTIVE - - // TODO make this real: check to see if the state - if(_drivetrain.get_state() == DrivetrainState_e::NOT_ENABLED_NO_HV_PRESENT) - { - set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + if (!_hv_over_threshold) + { + _set_state(VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + break; + } + + if (_drivetrain_ready()) + { + _set_state(VEHICLE_STATE::READY_TO_DRIVE, current_millis); + break; + } break; } - - bool drivetrain_in_driveable_mode = ((_drivetrain.get_state() == DrivetrainState_e::ENABLED_SPEED_MODE) || (_drivetrain.get_state() == DrivetrainState_e::ENABLED_TORQUE_MODE)); - if (!drivetrain_in_driveable_mode) + + case VEHICLE_STATE::READY_TO_DRIVE: { - hal_println("Drivetrain error occurred while in READY_TO_DRIVE"); - set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); + if (!_hv_over_threshold) + { + _set_state(VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); + break; + } + + if (_drivetrain_error_ocurred) + { + _set_state(VEHICLE_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); + break; + } + + _command_drivetrain(); break; } - if (/* _ams_system.ams_ok() && */ !system_data.pedals_system_data.implausibility_has_exceeded_max_duration) + default: { - // TODO: Fix with all references to singleton classes - // TODO: need to also handle request to mode switch via drivetrain init (?) - // _drivetrain.command_drivetrain(controller_mux_->getDrivetrainCommand(dashboard_->getDialMode(), dashboard_->getTorqueLimitMode(), current_car_state)); - DrivetrainTorqueCommand_s example_cmd = {}; // will need to do a variant check from the controller mux to see what type it is (torque / speed) - - (void)_drivetrain.evaluate_drivetrain(example_cmd); - - } - else - { - // If software is not OK or some implausibility has exceeded max duration, command 0 torque (but stay in RTD mode) - - // TODO: make this check to see exactly what drive mode the drivetrain is in and send its associated empty command. - DrivetrainTorqueCommand_s example_cmd = {}; // will need to do a variant check from the controller mux to see what type it is (torque / speed) - (void)_drivetrain.evaluate_drivetrain(example_cmd); + break; } - break; - } - } } -void VehicleStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_millis) +void VehicleStateMachine::_set_state(VEHICLE_STATE new_state, unsigned long curr_millis) { - handle_exit_logic_(_current_state, curr_millis); + _handle_entry_logic(_current_state, curr_millis); _current_state = new_state; - handle_entry_logic_(new_state, curr_millis); + _handle_exit_logic(new_state, curr_millis); } -void VehicleStateMachine::handle_exit_logic_(CAR_STATE prev_state, unsigned long curr_millis) +void VehicleStateMachine::_handle_exit_logic(VEHICLE_STATE prev_state, unsigned long curr_millis) { - switch (get_state()) + switch (prev_state) { - case CAR_STATE::STARTUP: - break; - case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: - break; - case CAR_STATE::TRACTIVE_SYSTEM_ACTIVE: - break; - case CAR_STATE::ENABLING_INVERTERS: - break; - case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: - break; - case CAR_STATE::READY_TO_DRIVE: - break; - default: - break; - } + case VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: + break; + case VEHICLE_STATE::TRACTIVE_SYSTEM_ACTIVE: + break; + case VEHICLE_STATE::WANTING_READY_TO_DRIVE: + break; + case VEHICLE_STATE::READY_TO_DRIVE: + break; + default: + break; + } } -void VehicleStateMachine::handle_entry_logic_(CAR_STATE new_state, unsigned long curr_millis) +void VehicleStateMachine::_handle_entry_logic(VEHICLE_STATE new_state, unsigned long curr_millis) { switch (new_state) - { - case CAR_STATE::STARTUP: - break; - case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: - break; - case CAR_STATE::TRACTIVE_SYSTEM_ACTIVE: - break; - case CAR_STATE::ENABLING_INVERTERS: - break; - case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: - { - _buzzer.activate(curr_millis); - break; - } - case CAR_STATE::READY_TO_DRIVE: - break; - default: - break; - } + { + case VEHICLE_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: + break; + case VEHICLE_STATE::TRACTIVE_SYSTEM_ACTIVE: + break; + case VEHICLE_STATE::WANTING_READY_TO_DRIVE: + break; + case VEHICLE_STATE::READY_TO_DRIVE: + break; + default: + break; + } } diff --git a/lib/systems/README.md b/lib/systems/README.md index 2029328..38b99f4 100644 --- a/lib/systems/README.md +++ b/lib/systems/README.md @@ -79,4 +79,27 @@ stateDiagram-v2 drive_mode --> err: incorrect user command type OR any inverter error err --> clear_err: on user request of error reset clear_err --> not_en_hv: on successful reset of errors (either internally to the drivetrain system or of the inverters themselves) +``` + +``` +--- +title: Vehicle State Machine +--- +stateDiagram-v2 + + tsna : TRACTIVE_SYSTEM_NOT_ACTIVE + tsa : TRACTIVE_SYSTEM_ACTIVE + wrtd : WANTING_READY_TO_DRIVE + rtd : READY_TO_DRIVE + + tsna --> tsa : If hv is over threshold + + tsa --> tsna : If hv is under threshold + tsa --> wrtd : If the brake and start button are pressed + + wrtd --> tsna : If hv is under threshold + wrtd --> rtd : If the drivetrain system is in the ready state + + rtd --> tsna : If hv is under threshold + rtd --> tsa : If a drivetrain error occurs ``` \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index a69d9eb..a10a734 100644 --- a/platformio.ini +++ b/platformio.ini @@ -45,7 +45,7 @@ lib_deps = ${common.lib_deps_shared} https://github.com/tonton81/FlexCAN_T4 https://github.com/hytech-racing/shared_firmware_interfaces.git#4d7d617d63720f5fef586f1d95ecd979c1f8a075 - https://github.com/hytech-racing/HT_CAN/releases/download/134/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/141/can_lib.tar.gz ; Test Systems Environment. This is only for compiling and uploading the hardware-abstracted code.