diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index 68b91cd43..e6d11572c 100644 --- a/.github/workflows/platformio.yml +++ b/.github/workflows/platformio.yml @@ -6,6 +6,9 @@ jobs: run_build_and_tests: runs-on: ubuntu-latest steps: + - uses: webfactory/ssh-agent@v0.9.0 + with: + ssh-private-key: ${{ secrets.CASE_SSH_PRIVATE_KEY }} - name: Check out the repository uses: actions/checkout@v3 diff --git a/README.md b/README.md index f4d5b0365..ed87478c5 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,21 @@ +## using and updating CASE + +#### DO THIS ONCE +you will need to add ssh key to your github account to allow platformio to pull in CASE as a library. + +[follow these instructions on how to do this.](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/adding-a-new-ssh-key-to-your-github-account) + +#### UPDATING CASE + +to update case, look for the latest tag at this repo: https://github.com/hytech-racing/CASE_lib/tags + +and then in the `platformio.ini` file in this repository, change the number at this line: + +`git+ssh://git@github.com/hytech-racing/CASE_lib.git#v(INSERT NUMBER HERE)` + +for example: + ```git+ssh://git@github.com/hytech-racing/CASE_lib.git#v34``` + ## building testing and running This project uses [Platformio](https://docs.platformio.org/en/latest/) for building, testing, checking and packaging. @@ -78,6 +96,16 @@ then you can select this configuration to be used with [this c/c++ extension](ht to select the configuration, use the hotkey `ctrl+shift+p` to open the command prompt in vscode, and then type \`C/C++: Select a Configuration\` and then select your configuration for which you named above. +## updating CASE (manual / old way) + +1. generate the .zip file in HyTech_sim by generating the code for CASE +2. while in the HyTech_sim directory, there exists `process_lib.py`. use this by running: +``` +python3 process_lib.py HT08_CONTROL_SYSTEM.zip /path/to/MCU/lib/CASE_lib CASE_lib +``` +while ensuring that you fill in your path to MCU. + +3. profit ## Design Lore of the Code ### outline diff --git a/flash.sh b/flash.sh new file mode 100755 index 000000000..d04411f02 --- /dev/null +++ b/flash.sh @@ -0,0 +1,3 @@ +#!/bin/bash +pio run --target upload --environment teensy41 +pio run --target upload --environment teensy41 diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index 63f6bc0de..1dedb0ce6 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -63,8 +63,6 @@ const int BRAKE2_PEDAL_OOR_MIN = 90; const int BRAKE1_PEDAL_OOR_MAX = 4000; const int BRAKE2_PEDAL_OOR_MAX = 4000; - - const float DEFAULT_PEDAL_DEADZONE = 0.05f; const float DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN = 0.10f; @@ -73,7 +71,7 @@ const float BRAKE_ACTIVATION_PERCENTAGE = 0.05f; const float BRAKE_MECH_THRESH = 0.40f; // Load Cell Defs to convert raw to lbs -// lbs = (scale)*raw + offset +// lbs = (raw + offset) * scale const float LOADCELL_FL_SCALE = 0.0553; const float LOADCELL_FL_OFFSET = 15.892 / LOADCELL_FL_SCALE; @@ -86,4 +84,12 @@ const float LOADCELL_RL_OFFSET = 21.842 / LOADCELL_RL_SCALE; const float LOADCELL_RR_SCALE = 0.0588; const float LOADCELL_RR_OFFSET = 19.576 / LOADCELL_RR_SCALE; + +// Steering parameters +const float PRIMARY_STEERING_SENSE_OFFSET = -21.18; // units are degrees +const int SECONDARY_STEERING_SENSE_LEFTMOST_BOUND = 812; +const int SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND = 3179; +const int SECONDARY_STEERING_SENSE_CENTER = 1970; +const float STEERING_RANGE_DEGREES = 256.05f; + #endif /* __MCU15_H__ */ \ No newline at end of file diff --git a/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath.cpp b/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath.cpp deleted file mode 100644 index 19ad3fa3c..000000000 --- a/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath.cpp +++ /dev/null @@ -1,519 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: BasicVehicleMath.cpp -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "BasicVehicleMath.h" -#include "rtwtypes.h" -#include -#include "BasicVehicleMath_private.h" - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#include "rt_defines.h" - -// -// Output and update for action system: -// '/Accel. Calc' -// '/Accel. Calc' -// '/Accel. Calc' -// '/Accel. Calc' -// -void BasicVehicleMath::BasicVehicleMath_AccelCalc(real_T rtu_CornerSpeed, real_T - rtu_WheelOmega1, real_T *rty_SL) -{ - real_T rtb_Switch3; - real_T tmp; - - // Product: '/Product' incorporates: - // Constant: '/Constant' - - rtb_Switch3 = rtu_WheelOmega1 * 0.2; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant1' - - if (rtb_Switch3 != 0.0) { - tmp = rtb_Switch3; - } else { - tmp = (rtInf); - } - - // Product: '/Divide' incorporates: - // Sum: '/Subtract' - // Switch: '/Switch' - - rtb_Switch3 = (rtb_Switch3 - rtu_CornerSpeed) / tmp; - - // Switch: '/Switch3' incorporates: - // Constant: '/Constant3' - - if (!(rtb_Switch3 > 0.0)) { - rtb_Switch3 = 0.0; - } - - // End of Switch: '/Switch3' - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant2' - - if (rtb_Switch3 > 1.0) { - *rty_SL = 1.0; - } else { - *rty_SL = rtb_Switch3; - } - - // End of Switch: '/Switch1' -} - -// -// Output and update for action system: -// '/Brake Calc' -// '/Brake Calc' -// '/Brake Calc' -// '/Brake Calc' -// -void BasicVehicleMath::BasicVehicleMath_BrakeCalc(real_T rtu_WheelOmega, real_T - rtu_CornerSpeed, real_T *rty_SL) -{ - real_T rtb_LongitudinalSlipSL; - - // Product: '/Divide' incorporates: - // Constant: '/Constant' - // Product: '/Product' - // Sum: '/Subtract' - - rtb_LongitudinalSlipSL = (rtu_WheelOmega * 0.2 - rtu_CornerSpeed) / - rtu_CornerSpeed; - - // Switch: '/Switch' incorporates: - // Abs: '/Abs' - // Constant: '/Constant1' - - if (std::abs(rtb_LongitudinalSlipSL) > 1.0) { - rtb_LongitudinalSlipSL = -1.0; - } - - // Gain: '/Gain' incorporates: - // Abs: '/Abs1' - // Switch: '/Switch' - - *rty_SL = -std::abs(rtb_LongitudinalSlipSL); -} - -real_T rt_atan2d_snf(real_T u0, real_T u1) -{ - real_T y; - if (std::isnan(u0) || std::isnan(u1)) { - y = (rtNaN); - } else if (std::isinf(u0) && std::isinf(u1)) { - int32_T tmp; - int32_T tmp_0; - if (u0 > 0.0) { - tmp = 1; - } else { - tmp = -1; - } - - if (u1 > 0.0) { - tmp_0 = 1; - } else { - tmp_0 = -1; - } - - y = std::atan2(static_cast(tmp), static_cast(tmp_0)); - } else if (u1 == 0.0) { - if (u0 > 0.0) { - y = RT_PI / 2.0; - } else if (u0 < 0.0) { - y = -(RT_PI / 2.0); - } else { - y = 0.0; - } - } else { - y = std::atan2(u0, u1); - } - - return y; -} - -// Model step function -void BasicVehicleMath::step() -{ - real_T rtb_LongCornerVel_FL_B; - real_T rtb_LongCornerVel_FR_B; - real_T rtb_Subtract1; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant2' - // Inport: '/Vx_B [m//s]' - - if (BasicVehicleMath_U.Vx_Bms > 0.0) { - rtb_Subtract1 = BasicVehicleMath_U.Vx_Bms; - } else { - rtb_Subtract1 = (rtInf); - } - - // Outport: '/Beta [rad]' incorporates: - // Inport: '/Vy_B [m//s]' - // Product: '/Divide2' - // Switch: '/Switch' - // Trigonometry: '/Atan' - - BasicVehicleMath_Y.Betarad = std::atan(BasicVehicleMath_U.Vy_Bms / - rtb_Subtract1); - - // MATLAB Function: '/MATLAB Function' incorporates: - // Constant: '/Constant' - // Constant: '/Constant2' - // Constant: '/Constant4' - // Constant: '/Constant6' - // Inport: '/Delta Left [rad]' - // Inport: '/Delta Right [rad]' - // Inport: '/Vx_B [m//s]' - // Inport: '/Vy_B [m//s]' - // Inport: '/Yaw Rate [rad//s]' - - rtb_Subtract1 = BasicVehicleMath_U.YawRaterads * 0.96437803790837118 * - 0.62216265449318331; - BasicVehicleMath_Y.LongitudinalCornerVelWRRms = rtb_Subtract1 + - BasicVehicleMath_U.Vx_Bms; - rtb_LongCornerVel_FR_B = BasicVehicleMath_U.YawRaterads * 0.96437803790837118 * - 0.78288800690392224 + BasicVehicleMath_U.Vy_Bms; - BasicVehicleMath_Y.AlphaFLrad = -(BasicVehicleMath_U.DeltaLeftrad - - rt_atan2d_snf(rtb_LongCornerVel_FR_B, - BasicVehicleMath_Y.LongitudinalCornerVelWRRms)); - if (std::isnan(BasicVehicleMath_Y.AlphaFLrad)) { - if (BasicVehicleMath_U.DeltaLeftrad != 0.0) { - BasicVehicleMath_Y.AlphaFLrad = -BasicVehicleMath_U.DeltaLeftrad; - } else { - BasicVehicleMath_Y.AlphaFLrad = 0.0; - } - } - - rtb_LongCornerVel_FL_B = BasicVehicleMath_Y.LongitudinalCornerVelWRRms; - BasicVehicleMath_Y.LongitudinalCornerVelWRRms = BasicVehicleMath_U.Vx_Bms - - rtb_Subtract1; - BasicVehicleMath_Y.AlphaFRrad = -(BasicVehicleMath_U.DeltaRightrad - - rt_atan2d_snf(rtb_LongCornerVel_FR_B, - BasicVehicleMath_Y.LongitudinalCornerVelWRRms)); - if (std::isnan(BasicVehicleMath_Y.AlphaFRrad)) { - if (BasicVehicleMath_U.DeltaRightrad != 0.0) { - BasicVehicleMath_Y.AlphaFRrad = -BasicVehicleMath_U.DeltaRightrad; - } else { - BasicVehicleMath_Y.AlphaFRrad = 0.0; - } - } - - rtb_LongCornerVel_FR_B = BasicVehicleMath_Y.LongitudinalCornerVelWRRms; - rtb_Subtract1 = BasicVehicleMath_U.YawRaterads * 0.98407316801140354 * - 0.60971076084969233; - BasicVehicleMath_Y.LongitudinalCornerVelWRRms = rtb_Subtract1 + - BasicVehicleMath_U.Vx_Bms; - BasicVehicleMath_Y.AlphaRRrad = rt_atan2d_snf(BasicVehicleMath_U.Vy_Bms - - BasicVehicleMath_U.YawRaterads * 0.98407316801140354 * 0.79262398910460008, - BasicVehicleMath_Y.LongitudinalCornerVelWRRms); - - // Outport: '/Alpha RL [rad]' incorporates: - // MATLAB Function: '/MATLAB Function' - - BasicVehicleMath_Y.AlphaRLrad = BasicVehicleMath_Y.AlphaRRrad; - - // MATLAB Function: '/MATLAB Function' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant7' - // Inport: '/Vx_B [m//s]' - // Inport: '/Vy_B [m//s]' - // Inport: '/Yaw Rate [rad//s]' - - if (std::isnan(BasicVehicleMath_Y.AlphaRRrad)) { - // Outport: '/Alpha RL [rad]' - BasicVehicleMath_Y.AlphaRLrad = 0.0; - } - - BasicVehicleMath_Y.LongitudinalCornerVelWRLms = - BasicVehicleMath_Y.LongitudinalCornerVelWRRms; - BasicVehicleMath_Y.LongitudinalCornerVelWRRms = BasicVehicleMath_U.Vx_Bms - - rtb_Subtract1; - BasicVehicleMath_Y.AlphaRRrad = rt_atan2d_snf(BasicVehicleMath_U.Vy_Bms - - BasicVehicleMath_U.YawRaterads * 0.98407316801140354 * 0.79262398910460019, - BasicVehicleMath_Y.LongitudinalCornerVelWRRms); - if (std::isnan(BasicVehicleMath_Y.AlphaRRrad)) { - // Outport: '/Alpha RR [rad]' - BasicVehicleMath_Y.AlphaRRrad = 0.0; - } - - // Product: '/Product' incorporates: - // Abs: '/Abs' - // Inport: '/Delta Left [rad]' - // Trigonometry: '/Cos' - - BasicVehicleMath_Y.LongitudinalCornerVelWFLms = rtb_LongCornerVel_FL_B * std:: - cos(std::abs(BasicVehicleMath_U.DeltaLeftrad)); - - // Product: '/Product1' incorporates: - // Abs: '/Abs1' - // Inport: '/Delta Right [rad]' - // Trigonometry: '/Cos1' - - BasicVehicleMath_Y.LongitudinalCornerVelWFRms = rtb_LongCornerVel_FR_B * std:: - cos(std::abs(BasicVehicleMath_U.DeltaRightrad)); - - // Merge: '/Merge' incorporates: - // Abs: '/Abs' - // Inport: '/Vx_B [m//s]' - - BasicVehicleMath_Y.SLRR = std::abs(BasicVehicleMath_U.Vx_Bms); - - // Outport: '/Kinematic Desired Yaw Rate [rad//s]' incorporates: - // Constant: '/wb' - // Inport: '/Delta Avg [rad]' - // Product: '/Divide' - // Trigonometry: '/Tan' - - BasicVehicleMath_Y.KinematicDesiredYawRaterads = BasicVehicleMath_Y.SLRR * std:: - tan(BasicVehicleMath_U.DeltaAvgrad) / 1.535; - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant2' - // Inport: '/Wheel Omega FL [rad//s]' - // Product: '/Product1' - - rtb_Subtract1 = BasicVehicleMath_U.WheelOmegaFLrads * 0.2 - - BasicVehicleMath_Y.LongitudinalCornerVelWFLms; - - // If: '/If2' incorporates: - // Inport: '/Wheel Omega FL [rad//s]' - - if (rtb_Subtract1 > 0.0) { - // Outputs for IfAction SubSystem: '/Accel. Calc' incorporates: - // ActionPort: '/Action Port' - - BasicVehicleMath_AccelCalc(BasicVehicleMath_Y.LongitudinalCornerVelWFLms, - BasicVehicleMath_U.WheelOmegaFLrads, &BasicVehicleMath_Y.SLRR); - - // End of Outputs for SubSystem: '/Accel. Calc' - } else if (rtb_Subtract1 < 0.0) { - // Outputs for IfAction SubSystem: '/Brake Calc' incorporates: - // ActionPort: '/Action Port' - - BasicVehicleMath_BrakeCalc(BasicVehicleMath_U.WheelOmegaFLrads, - BasicVehicleMath_Y.LongitudinalCornerVelWFLms, &BasicVehicleMath_Y.SLRR); - - // End of Outputs for SubSystem: '/Brake Calc' - } else { - // Outputs for IfAction SubSystem: '/If Action Subsystem' incorporates: - // ActionPort: '/Action Port' - - // Merge: '/Merge' incorporates: - // Constant: '/Constant1' - // SignalConversion generated from: '/in' - - BasicVehicleMath_Y.SLRR = 0.0; - - // End of Outputs for SubSystem: '/If Action Subsystem' - } - - // End of If: '/If2' - - // Outport: '/SL FL' incorporates: - // Switch: '/Switch' - - BasicVehicleMath_Y.SLFL = BasicVehicleMath_Y.SLRR; - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant2' - // Inport: '/Wheel Omega FR [rad//s]' - // Product: '/Product1' - - rtb_Subtract1 = BasicVehicleMath_U.WheelOmegaFRrads * 0.2 - - BasicVehicleMath_Y.LongitudinalCornerVelWFRms; - - // If: '/If2' incorporates: - // Inport: '/Wheel Omega FR [rad//s]' - - if (rtb_Subtract1 > 0.0) { - // Outputs for IfAction SubSystem: '/Accel. Calc' incorporates: - // ActionPort: '/Action Port' - - BasicVehicleMath_AccelCalc(BasicVehicleMath_Y.LongitudinalCornerVelWFRms, - BasicVehicleMath_U.WheelOmegaFRrads, &BasicVehicleMath_Y.SLRR); - - // End of Outputs for SubSystem: '/Accel. Calc' - } else if (rtb_Subtract1 < 0.0) { - // Outputs for IfAction SubSystem: '/Brake Calc' incorporates: - // ActionPort: '/Action Port' - - BasicVehicleMath_BrakeCalc(BasicVehicleMath_U.WheelOmegaFRrads, - BasicVehicleMath_Y.LongitudinalCornerVelWFRms, &BasicVehicleMath_Y.SLRR); - - // End of Outputs for SubSystem: '/Brake Calc' - } else { - // Outputs for IfAction SubSystem: '/If Action Subsystem' incorporates: - // ActionPort: '/Action Port' - - // Merge: '/Merge' incorporates: - // Constant: '/Constant1' - // SignalConversion generated from: '/in' - - BasicVehicleMath_Y.SLRR = 0.0; - - // End of Outputs for SubSystem: '/If Action Subsystem' - } - - // End of If: '/If2' - - // Outport: '/SL FR' incorporates: - // Switch: '/Switch1' - - BasicVehicleMath_Y.SLFR = BasicVehicleMath_Y.SLRR; - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant2' - // Inport: '/Wheel Omega RL [rad//s]' - // Product: '/Product1' - - rtb_Subtract1 = BasicVehicleMath_U.WheelOmegaRLrads * 0.2 - - BasicVehicleMath_Y.LongitudinalCornerVelWRLms; - - // If: '/If2' incorporates: - // Inport: '/Wheel Omega RL [rad//s]' - - if (rtb_Subtract1 > 0.0) { - // Outputs for IfAction SubSystem: '/Accel. Calc' incorporates: - // ActionPort: '/Action Port' - - BasicVehicleMath_AccelCalc(BasicVehicleMath_Y.LongitudinalCornerVelWRLms, - BasicVehicleMath_U.WheelOmegaRLrads, &BasicVehicleMath_Y.SLRR); - - // End of Outputs for SubSystem: '/Accel. Calc' - } else if (rtb_Subtract1 < 0.0) { - // Outputs for IfAction SubSystem: '/Brake Calc' incorporates: - // ActionPort: '/Action Port' - - BasicVehicleMath_BrakeCalc(BasicVehicleMath_U.WheelOmegaRLrads, - BasicVehicleMath_Y.LongitudinalCornerVelWRLms, &BasicVehicleMath_Y.SLRR); - - // End of Outputs for SubSystem: '/Brake Calc' - } else { - // Outputs for IfAction SubSystem: '/If Action Subsystem' incorporates: - // ActionPort: '/Action Port' - - // Merge: '/Merge' incorporates: - // Constant: '/Constant1' - // SignalConversion generated from: '/in' - - BasicVehicleMath_Y.SLRR = 0.0; - - // End of Outputs for SubSystem: '/If Action Subsystem' - } - - // End of If: '/If2' - - // Outport: '/SL RL' incorporates: - // Switch: '/Switch2' - - BasicVehicleMath_Y.SLRL = BasicVehicleMath_Y.SLRR; - - // Sum: '/Subtract1' incorporates: - // Constant: '/Constant2' - // Inport: '/Wheel Omega RR [rad//s]' - // MATLAB Function: '/MATLAB Function' - // Product: '/Product1' - - rtb_Subtract1 = BasicVehicleMath_U.WheelOmegaRRrads * 0.2 - - BasicVehicleMath_Y.LongitudinalCornerVelWRRms; - - // If: '/If2' incorporates: - // Inport: '/Wheel Omega RR [rad//s]' - // MATLAB Function: '/MATLAB Function' - - if (rtb_Subtract1 > 0.0) { - // Outputs for IfAction SubSystem: '/Accel. Calc' incorporates: - // ActionPort: '/Action Port' - - BasicVehicleMath_AccelCalc(BasicVehicleMath_Y.LongitudinalCornerVelWRRms, - BasicVehicleMath_U.WheelOmegaRRrads, &BasicVehicleMath_Y.SLRR); - - // End of Outputs for SubSystem: '/Accel. Calc' - } else if (rtb_Subtract1 < 0.0) { - // Outputs for IfAction SubSystem: '/Brake Calc' incorporates: - // ActionPort: '/Action Port' - - BasicVehicleMath_BrakeCalc(BasicVehicleMath_U.WheelOmegaRRrads, - BasicVehicleMath_Y.LongitudinalCornerVelWRRms, &BasicVehicleMath_Y.SLRR); - - // End of Outputs for SubSystem: '/Brake Calc' - } else { - // Outputs for IfAction SubSystem: '/If Action Subsystem' incorporates: - // ActionPort: '/Action Port' - - // Merge: '/Merge' incorporates: - // Constant: '/Constant1' - // SignalConversion generated from: '/in' - - BasicVehicleMath_Y.SLRR = 0.0; - - // End of Outputs for SubSystem: '/If Action Subsystem' - } - - // End of If: '/If2' -} - -// Model initialize function -void BasicVehicleMath::initialize() -{ - // Registration code - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); -} - -// Model terminate function -void BasicVehicleMath::terminate() -{ - // (no terminate code required) -} - -// Constructor -BasicVehicleMath::BasicVehicleMath() : - BasicVehicleMath_U(), - BasicVehicleMath_Y(), - BasicVehicleMath_M() -{ - // Currently there is no constructor body generated. -} - -// Destructor -// Currently there is no destructor body generated. -BasicVehicleMath::~BasicVehicleMath() = default; - -// Real-Time Model get method -BasicVehicleMath::RT_MODEL_BasicVehicleMath_T * BasicVehicleMath::getRTM() -{ - return (&BasicVehicleMath_M); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath.h b/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath.h deleted file mode 100644 index 22255d2f0..000000000 --- a/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath.h +++ /dev/null @@ -1,224 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: BasicVehicleMath.h -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_BasicVehicleMath_h_ -#define RTW_HEADER_BasicVehicleMath_h_ -#include "rtwtypes.h" -#include "BasicVehicleMath_types.h" - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -extern "C" -{ - -#include "rtGetInf.h" - -} - -extern "C" -{ - -#include "rtGetNaN.h" - -} - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -// Class declaration for model BasicVehicleMath -class BasicVehicleMath final -{ - // public data and function members - public: - // External inputs (root inport signals with default storage) - struct ExtU_BasicVehicleMath_T { - real_T Vx_Bms; // '/Vx_B [m//s]' - real_T Vy_Bms; // '/Vy_B [m//s]' - real_T YawRaterads; // '/Yaw Rate [rad//s]' - real_T DeltaLeftrad; // '/Delta Left [rad]' - real_T DeltaRightrad; // '/Delta Right [rad]' - real_T DeltaAvgrad; // '/Delta Avg [rad]' - real_T WheelOmegaFLrads; // '/Wheel Omega FL [rad//s]' - real_T WheelOmegaFRrads; // '/Wheel Omega FR [rad//s]' - real_T WheelOmegaRLrads; // '/Wheel Omega RL [rad//s]' - real_T WheelOmegaRRrads; // '/Wheel Omega RR [rad//s]' - }; - - // External outputs (root outports fed by signals with default storage) - struct ExtY_BasicVehicleMath_T { - real_T Betarad; // '/Beta [rad]' - real_T AlphaFLrad; // '/Alpha FL [rad]' - real_T AlphaFRrad; // '/Alpha FR [rad]' - real_T AlphaRLrad; // '/Alpha RL [rad]' - real_T AlphaRRrad; // '/Alpha RR [rad]' - real_T LongitudinalCornerVelWFLms; - // '/Longitudinal Corner Vel W FL [m//s]' - real_T LongitudinalCornerVelWFRms; - // '/Longitudinal Corner Vel W FR [m//s]' - real_T LongitudinalCornerVelWRLms; - // '/Longitudinal Corner Vel W RL [m//s]' - real_T LongitudinalCornerVelWRRms; - // '/Longitudinal Corner Vel W RR [m//s]' - real_T KinematicDesiredYawRaterads; - // '/Kinematic Desired Yaw Rate [rad//s]' - real_T SLFL; // '/SL FL' - real_T SLFR; // '/SL FR' - real_T SLRL; // '/SL RL' - real_T SLRR; // '/SL RR' - }; - - // Real-time Model Data Structure - struct RT_MODEL_BasicVehicleMath_T { - const char_T * volatile errorStatus; - }; - - // Copy Constructor - BasicVehicleMath(BasicVehicleMath const&) = delete; - - // Assignment Operator - BasicVehicleMath& operator= (BasicVehicleMath const&) & = delete; - - // Move Constructor - BasicVehicleMath(BasicVehicleMath &&) = delete; - - // Move Assignment Operator - BasicVehicleMath& operator= (BasicVehicleMath &&) = delete; - - // Real-Time Model get method - BasicVehicleMath::RT_MODEL_BasicVehicleMath_T * getRTM(); - - // Root inports set method - void setExternalInputs(const ExtU_BasicVehicleMath_T *pExtU_BasicVehicleMath_T) - { - BasicVehicleMath_U = *pExtU_BasicVehicleMath_T; - } - - // Root outports get method - const ExtY_BasicVehicleMath_T &getExternalOutputs() const - { - return BasicVehicleMath_Y; - } - - // model initialize function - void initialize(); - - // model step function - void step(); - - // model terminate function - static void terminate(); - - // Constructor - BasicVehicleMath(); - - // Destructor - ~BasicVehicleMath(); - - // private data and function members - private: - // External inputs - ExtU_BasicVehicleMath_T BasicVehicleMath_U; - - // External outputs - ExtY_BasicVehicleMath_T BasicVehicleMath_Y; - - // private member function(s) for subsystem '/Accel. Calc' - static void BasicVehicleMath_AccelCalc(real_T rtu_CornerSpeed, real_T - rtu_WheelOmega1, real_T *rty_SL); - - // private member function(s) for subsystem '/Brake Calc' - static void BasicVehicleMath_BrakeCalc(real_T rtu_WheelOmega, real_T - rtu_CornerSpeed, real_T *rty_SL); - - // Real-Time Model - RT_MODEL_BasicVehicleMath_T BasicVehicleMath_M; -}; - -//- -// These blocks were eliminated from the model due to optimizations: -// -// Block '/Scope' : Unused code path elimination -// Block '/Scope' : Unused code path elimination -// Block '/Scope' : Unused code path elimination -// Block '/Scope' : Unused code path elimination -// Block '/Constant' : Unused code path elimination -// Block '/Constant1' : Unused code path elimination -// Block '/Constant2' : Unused code path elimination -// Block '/Constant3' : Unused code path elimination -// Block '/Constant4' : Unused code path elimination -// Block '/Constant5' : Unused code path elimination -// Block '/Constant6' : Unused code path elimination -// Block '/Constant7' : Unused code path elimination - - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'BasicVehicleMath' -// '' : 'BasicVehicleMath/Body Slip' -// '' : 'BasicVehicleMath/Kinematic Yaw Rate' -// '' : 'BasicVehicleMath/Longitudinal Corner Vel B --> W Transformation' -// '' : 'BasicVehicleMath/Slip Angle and Corner Velocity' -// '' : 'BasicVehicleMath/Subsystem' -// '' : 'BasicVehicleMath/Slip Angle and Corner Velocity/MATLAB Function' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc FL' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc FR' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc RL' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc RR' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc FL/Accel. Calc' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc FL/Brake Calc' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc FL/If Action Subsystem' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc FR/Accel. Calc' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc FR/Brake Calc' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc FR/If Action Subsystem' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc RL/Accel. Calc' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc RL/Brake Calc' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc RL/If Action Subsystem' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc RR/Accel. Calc' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc RR/Brake Calc' -// '' : 'BasicVehicleMath/Subsystem/Longitudinal Slip Calc RR/If Action Subsystem' - -#endif // RTW_HEADER_BasicVehicleMath_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath_private.h b/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath_private.h deleted file mode 100644 index fd7767079..000000000 --- a/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath_private.h +++ /dev/null @@ -1,32 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: BasicVehicleMath_private.h -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_BasicVehicleMath_private_h_ -#define RTW_HEADER_BasicVehicleMath_private_h_ -#include "rtwtypes.h" -#include "BasicVehicleMath_types.h" - -extern real_T rt_atan2d_snf(real_T u0, real_T u1); - -#endif // RTW_HEADER_BasicVehicleMath_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath_types.h b/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath_types.h deleted file mode 100644 index 8aa51d0ef..000000000 --- a/lib/BasicVehicleMath_ert_rtw/BasicVehicleMath_types.h +++ /dev/null @@ -1,27 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: BasicVehicleMath_types.h -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_BasicVehicleMath_types_h_ -#define RTW_HEADER_BasicVehicleMath_types_h_ -#endif // RTW_HEADER_BasicVehicleMath_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/CMakeLists.txt b/lib/BasicVehicleMath_ert_rtw/CMakeLists.txt deleted file mode 100644 index 950b3a712..000000000 --- a/lib/BasicVehicleMath_ert_rtw/CMakeLists.txt +++ /dev/null @@ -1,125 +0,0 @@ -########################################################################### -# CMakeLists.txt generated for component BasicVehicleMath -# Product type: executable -########################################################################### -cmake_minimum_required(VERSION 3.12) -project(BasicVehicleMath) - -# Propagate the CMAKE_EXPORT_COMPILE_COMMANDS variable from the -# environment if it is defined as an environment variable, but not as a -# CMake variable. This is to work around a bug in CMake 3.19 when the -# "NMake Makefiles" generator is selected. -if(DEFINED ENV{CMAKE_EXPORT_COMPILE_COMMANDS} AND NOT DEFINED CMAKE_EXPORT_COMPILE_COMMANDS) - set(CMAKE_EXPORT_COMPILE_COMMANDS $ENV{CMAKE_EXPORT_COMPILE_COMMANDS}) -endif() - -# Define common variables that are used within the whole project. -set(SYSLIB_PREFIX $,$>>,lib,>) - -########################################################################### -## Path variables -########################################################################### -# Derive an absolute path to the code generation anchor folder. -get_filename_component(START_DIR .. ABSOLUTE) - -# Special directories defined by using CACHE variables can be overridden -# by setting the variable from the command line, e.g., -# -# cmake . -DMATLAB_ROOT=/path/to/another/matlab/root -set(MATLAB_ROOT C:/Program\ Files/MATLAB/R2023b CACHE PATH "") - -# Additional variables that are defined conditionally. -if("${CMAKE_CURRENT_BINARY_DIR}" STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}") - set(BINARY_START_DIR "${START_DIR}") -else() - set(BINARY_START_DIR "${CMAKE_BINARY_DIR}") -endif() - -########################################################################### -## System Libraries -########################################################################### -find_library(FOUND_LIBM m NO_SYSTEM_ENVIRONMENT_PATH PATHS ${CMAKE_C_IMPLICIT_LINK_DIRECTORIES} ${CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES}) - -########################################################################### -## Target definition and commands -########################################################################### - -# Definition of target "BasicVehicleMath". -add_executable(BasicVehicleMath ${START_DIR}/BasicVehicleMath_ert_rtw/BasicVehicleMath.cpp - ${START_DIR}/BasicVehicleMath_ert_rtw/ert_main.cpp - ${START_DIR}/BasicVehicleMath_ert_rtw/rtGetInf.cpp - ${START_DIR}/BasicVehicleMath_ert_rtw/rtGetNaN.cpp - ${START_DIR}/BasicVehicleMath_ert_rtw/rt_nonfinite.cpp) - -# Set properties for target "BasicVehicleMath". -set_target_properties(BasicVehicleMath PROPERTIES PREFIX "" - POSITION_INDEPENDENT_CODE ON - RUNTIME_OUTPUT_DIRECTORY "${BINARY_START_DIR}/./$<0:>" - LIBRARY_OUTPUT_DIRECTORY "${BINARY_START_DIR}/./$<0:>" - ARCHIVE_OUTPUT_DIRECTORY "${BINARY_START_DIR}/./$<0:>") - -# Specify language features required for target "BasicVehicleMath". -target_compile_features(BasicVehicleMath PUBLIC cxx_std_11) - -# Specify compiler preprocessor definitions for target "BasicVehicleMath". -target_compile_definitions(BasicVehicleMath PRIVATE -DMODEL=BasicVehicleMath - -DNUMST=1 - -DNCSTATES=0 - -DHAVESTDIO - -DMODEL_HAS_DYNAMICALLY_LOADED_SFCNS=0 - -DCLASSIC_INTERFACE=0 - -DALLOCATIONFCN=0 - -DTID01EQ=0 - -DTERMFCN=1 - -DONESTEPFCN=1 - -DMAT_FILE=0 - -DMULTI_INSTANCE_CODE=1 - -DINTEGER_CODE=0 - -DMT=0) - -# Specify include directories for target "BasicVehicleMath". -target_include_directories(BasicVehicleMath PRIVATE ${START_DIR} - ${START_DIR}/BasicVehicleMath_ert_rtw - ${MATLAB_ROOT}/extern/include - ${MATLAB_ROOT}/simulink/include - ${MATLAB_ROOT}/rtw/c/src - ${MATLAB_ROOT}/rtw/c/src/ext_mode/common - ${MATLAB_ROOT}/rtw/c/ert) - -# Specify library link dependencies for target "BasicVehicleMath". CMake -# generator expressions are used to create a CMakeLists.txt file that -# supports multiple platforms with differently named system library -# dependencies. -target_link_libraries(BasicVehicleMath PRIVATE $<$:m> - $<$:kernel32> - $<$:ws2_32> - $<$:mswsock> - $<$:advapi32>) - -# Extract DWARF debug symbols into a separate file when building -# executable programs for Apple platforms. -if(APPLE AND (CMAKE_C_COMPILER_ID STREQUAL "AppleClang")) - string(APPEND _dsymutilcmd "$," - "xcrun;dsymutil;$;--flat," - "true" - >) - add_custom_command(TARGET BasicVehicleMath - POST_BUILD - COMMAND "${_dsymutilcmd}" - COMMAND_EXPAND_LISTS - BYPRODUCTS BasicVehicleMath.dwarf) -endif() - - -########################################################################### -## Build success message -########################################################################### -add_custom_command(TARGET BasicVehicleMath POST_BUILD - COMMAND ${CMAKE_COMMAND} -E cmake_echo_color --cyan "\\#\\#\\# Created executable: $") - -########################################################################### -## Call toolchain hook function if defined -########################################################################### -if(COMMAND toolchain_target_hook) - toolchain_target_hook(BasicVehicleMath) -endif() diff --git a/lib/BasicVehicleMath_ert_rtw/buildInfo.mat b/lib/BasicVehicleMath_ert_rtw/buildInfo.mat deleted file mode 100644 index 61c90ec0b..000000000 Binary files a/lib/BasicVehicleMath_ert_rtw/buildInfo.mat and /dev/null differ diff --git a/lib/BasicVehicleMath_ert_rtw/ert_main.cpp b/lib/BasicVehicleMath_ert_rtw/ert_main.cpp deleted file mode 100644 index c61d78767..000000000 --- a/lib/BasicVehicleMath_ert_rtw/ert_main.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: ert_main.cpp -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include // This example main program uses printf/fflush -#include "BasicVehicleMath.h" // Model header file - -static BasicVehicleMath BasicVehicleMath_Obj;// Instance of model class - -// -// Associating rt_OneStep with a real-time clock or interrupt service routine -// is what makes the generated code "real-time". The function rt_OneStep is -// always associated with the base rate of the model. Subrates are managed -// by the base rate from inside the generated code. Enabling/disabling -// interrupts and floating point context switches are target specific. This -// example code indicates where these should take place relative to executing -// the generated code step function. Overrun behavior should be tailored to -// your application needs. This example simply sets an error status in the -// real-time model and returns from rt_OneStep. -// -void rt_OneStep(void); -void rt_OneStep(void) -{ - static boolean_T OverrunFlag{ false }; - - // Disable interrupts here - - // Check for overrun - if (OverrunFlag) { - rtmSetErrorStatus(BasicVehicleMath_Obj.getRTM(), "Overrun"); - return; - } - - OverrunFlag = true; - - // Save FPU context here (if necessary) - // Re-enable timer or interrupt here - // Set model inputs here - - // Step the model - BasicVehicleMath_Obj.step(); - - // Get model outputs here - - // Indicate task complete - OverrunFlag = false; - - // Disable interrupts here - // Restore FPU context here (if necessary) - // Enable interrupts here -} - -// -// The example main function illustrates what is required by your -// application code to initialize, execute, and terminate the generated code. -// Attaching rt_OneStep to a real-time clock is target specific. This example -// illustrates how you do this relative to initializing the model. -// -int_T main(int_T argc, const char *argv[]) -{ - // Unused arguments - (void)(argc); - (void)(argv); - - // Initialize model - BasicVehicleMath_Obj.initialize(); - - // Attach rt_OneStep to a timer or interrupt service routine with - // period 0.001 seconds (base rate of the model) here. - // The call syntax for rt_OneStep is - // - // rt_OneStep(); - - printf("Warning: The simulation will run forever. " - "Generated ERT main won't simulate model step behavior. " - "To change this behavior select the 'MAT-file logging' option.\n"); - fflush((nullptr)); - while (rtmGetErrorStatus(BasicVehicleMath_Obj.getRTM()) == (nullptr)) { - // Perform application tasks here - } - - // Terminate model - BasicVehicleMath_Obj.terminate(); - return 0; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/library.json b/lib/BasicVehicleMath_ert_rtw/library.json deleted file mode 100644 index 1a302efed..000000000 --- a/lib/BasicVehicleMath_ert_rtw/library.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "name": "BasicVehicleMath-lib", - "version": "1.0.0", - "license": "MIT", - "frameworks": "*", - "platforms": "*", - "build": { - "flags": [ - "-IR2023b/rtw/c/src", - "-IR2023b/simulink/include" - ] - } -} \ No newline at end of file diff --git a/lib/BasicVehicleMath_ert_rtw/rtGetInf.cpp b/lib/BasicVehicleMath_ert_rtw/rtGetInf.cpp deleted file mode 100644 index ca8be6bee..000000000 --- a/lib/BasicVehicleMath_ert_rtw/rtGetInf.cpp +++ /dev/null @@ -1,121 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: rtGetInf.cpp -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// - -#include "rtwtypes.h" - -extern "C" -{ - -#include "rtGetInf.h" - -} - -#include - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#define NumBitsPerChar 8U - -extern "C" -{ - // - // Initialize rtInf needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real_T rtGetInf(void) - { - size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; - - real_T inf{ 0.0 }; - - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - } - - return inf; - } - - // - // Initialize rtInfF needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real32_T rtGetInfF(void) - { - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; - } - - // - // Initialize rtMinusInf needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real_T rtGetMinusInf(void) - { - size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; - - real_T minf{ 0.0 }; - - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - } - - return minf; - } - - // - // Initialize rtMinusInfF needed by the generated code. - // Inf is initialized as non-signaling. Assumes IEEE. - // - real32_T rtGetMinusInfF(void) - { - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/rtGetInf.h b/lib/BasicVehicleMath_ert_rtw/rtGetInf.h deleted file mode 100644 index 65d4ce441..000000000 --- a/lib/BasicVehicleMath_ert_rtw/rtGetInf.h +++ /dev/null @@ -1,53 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: rtGetInf.h -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_rtGetInf_h_ -#define RTW_HEADER_rtGetInf_h_ - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#include "rtwtypes.h" -#ifdef __cplusplus - -extern "C" -{ - -#endif - - extern real_T rtGetInf(void); - extern real32_T rtGetInfF(void); - extern real_T rtGetMinusInf(void); - extern real32_T rtGetMinusInfF(void); - -#ifdef __cplusplus - -} // extern "C" - -#endif -#endif // RTW_HEADER_rtGetInf_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/rtGetNaN.cpp b/lib/BasicVehicleMath_ert_rtw/rtGetNaN.cpp deleted file mode 100644 index 8be617169..000000000 --- a/lib/BasicVehicleMath_ert_rtw/rtGetNaN.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: rtGetNaN.cpp -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// - -#include "rtwtypes.h" - -extern "C" -{ - -#include "rtGetNaN.h" - -} - -#include - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#define NumBitsPerChar 8U - -extern "C" -{ - // - // Initialize rtNaN needed by the generated code. - // NaN is initialized as non-signaling. Assumes IEEE. - // - real_T rtGetNaN(void) - { - size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; - - real_T nan{ 0.0 }; - - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - } - - return nan; - } - - // - // Initialize rtNaNF needed by the generated code. - // NaN is initialized as non-signaling. Assumes IEEE. - // - real32_T rtGetNaNF(void) - { - IEEESingle nanF{ { 0.0F } }; - - nanF.wordL.wordLuint = 0xFFC00000U; - return nanF.wordL.wordLreal; - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/rtGetNaN.h b/lib/BasicVehicleMath_ert_rtw/rtGetNaN.h deleted file mode 100644 index 016af3021..000000000 --- a/lib/BasicVehicleMath_ert_rtw/rtGetNaN.h +++ /dev/null @@ -1,51 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: rtGetNaN.h -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_rtGetNaN_h_ -#define RTW_HEADER_rtGetNaN_h_ - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#include "rtwtypes.h" -#ifdef __cplusplus - -extern "C" -{ - -#endif - - extern real_T rtGetNaN(void); - extern real32_T rtGetNaNF(void); - -#ifdef __cplusplus - -} // extern "C" - -#endif -#endif // RTW_HEADER_rtGetNaN_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/rt_defines.h b/lib/BasicVehicleMath_ert_rtw/rt_defines.h deleted file mode 100644 index ae03f17ba..000000000 --- a/lib/BasicVehicleMath_ert_rtw/rt_defines.h +++ /dev/null @@ -1,57 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: rt_defines.h -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_rt_defines_h_ -#define RTW_HEADER_rt_defines_h_ - -//===========* -// Constants * -// =========== -#define RT_PI 3.14159265358979323846 -#define RT_PIF 3.1415927F -#define RT_LN_10 2.30258509299404568402 -#define RT_LN_10F 2.3025851F -#define RT_LOG10E 0.43429448190325182765 -#define RT_LOG10EF 0.43429449F -#define RT_E 2.7182818284590452354 -#define RT_EF 2.7182817F - -// -// UNUSED_PARAMETER(x) -// Used to specify that a function parameter (argument) is required but not -// accessed by the function body. - -#ifndef UNUSED_PARAMETER -#if defined(__LCC__) -#define UNUSED_PARAMETER(x) // do nothing -#else - -// -// This is the semi-ANSI standard way of indicating that an -// unused function parameter is required. - -#define UNUSED_PARAMETER(x) (void) (x) -#endif -#endif -#endif // RTW_HEADER_rt_defines_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/rt_nonfinite.cpp b/lib/BasicVehicleMath_ert_rtw/rt_nonfinite.cpp deleted file mode 100644 index 935410420..000000000 --- a/lib/BasicVehicleMath_ert_rtw/rt_nonfinite.cpp +++ /dev/null @@ -1,124 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: rt_nonfinite.cpp -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// - -extern "C" -{ - -#include "rtGetNaN.h" - -} - -extern "C" -{ - -#include "rtGetInf.h" - -} - -#include -#include "rtwtypes.h" - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#define NumBitsPerChar 8U - -extern "C" -{ - real_T rtInf; - real_T rtMinusInf; - real_T rtNaN; - real32_T rtInfF; - real32_T rtMinusInfF; - real32_T rtNaNF; -} - -extern "C" -{ - // - // Initialize the rtInf, rtMinusInf, and rtNaN needed by the - // generated code. NaN is initialized as non-signaling. Assumes IEEE. - // - void rt_InitInfAndNaN(size_t realSize) - { - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); - } - - // Test if value is infinite - boolean_T rtIsInf(real_T value) - { - return (boolean_T)((value==rtInf || value==rtMinusInf) ? 1U : 0U); - } - - // Test if single-precision value is infinite - boolean_T rtIsInfF(real32_T value) - { - return (boolean_T)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); - } - - // Test if value is not a number - boolean_T rtIsNaN(real_T value) - { - boolean_T result{ (boolean_T) 0 }; - - size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; - - if (bitsPerReal == 32U) { - result = rtIsNaNF((real32_T)value); - } else { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.fltVal = value; - result = (boolean_T)((tmpVal.bitVal.words.wordH & 0x7FF00000) == - 0x7FF00000 && - ( (tmpVal.bitVal.words.wordH & 0x000FFFFF) != 0 || - (tmpVal.bitVal.words.wordL != 0) )); - } - - return result; - } - - // Test if single-precision value is not a number - boolean_T rtIsNaNF(real32_T value) - { - IEEESingle tmp; - tmp.wordL.wordLreal = value; - return (boolean_T)( (tmp.wordL.wordLuint & 0x7F800000) == 0x7F800000 && - (tmp.wordL.wordLuint & 0x007FFFFF) != 0 ); - } -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/rt_nonfinite.h b/lib/BasicVehicleMath_ert_rtw/rt_nonfinite.h deleted file mode 100644 index dfac233b3..000000000 --- a/lib/BasicVehicleMath_ert_rtw/rt_nonfinite.h +++ /dev/null @@ -1,74 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: rt_nonfinite.h -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_rt_nonfinite_h_ -#define RTW_HEADER_rt_nonfinite_h_ -#include -#include "rtwtypes.h" -#define NOT_USING_NONFINITE_LITERALS 1 -#ifdef __cplusplus - -extern "C" -{ - -#endif - - extern real_T rtInf; - extern real_T rtMinusInf; - extern real_T rtNaN; - extern real32_T rtInfF; - extern real32_T rtMinusInfF; - extern real32_T rtNaNF; - extern void rt_InitInfAndNaN(size_t realSize); - extern boolean_T rtIsInf(real_T value); - extern boolean_T rtIsInfF(real32_T value); - extern boolean_T rtIsNaN(real_T value); - extern boolean_T rtIsNaNF(real32_T value); - struct BigEndianIEEEDouble { - struct { - uint32_T wordH; - uint32_T wordL; - } words; - }; - - struct LittleEndianIEEEDouble { - struct { - uint32_T wordL; - uint32_T wordH; - } words; - }; - - struct IEEESingle { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; - }; - -#ifdef __cplusplus - -} // extern "C" - -#endif -#endif // RTW_HEADER_rt_nonfinite_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/BasicVehicleMath_ert_rtw/rtwtypes.h b/lib/BasicVehicleMath_ert_rtw/rtwtypes.h deleted file mode 100644 index 2c4d93d08..000000000 --- a/lib/BasicVehicleMath_ert_rtw/rtwtypes.h +++ /dev/null @@ -1,160 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: rtwtypes.h -// -// Code generated for Simulink model 'BasicVehicleMath'. -// -// Model version : 1.2 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Fri Apr 5 20:57:25 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// - -#ifndef RTWTYPES_H -#define RTWTYPES_H - -// Logical type definitions -#if (!defined(__cplusplus)) -#ifndef false -#define false (0U) -#endif - -#ifndef true -#define true (1U) -#endif -#endif - -//=======================================================================* -// Target hardware information -// Device type: Intel->x86-64 (Windows64) -// Number of bits: char: 8 short: 16 int: 32 -// long: 32 -// native word size: 64 -// Byte ordering: LittleEndian -// Signed integer division rounds to: Zero -// Shift right on a signed integer as arithmetic shift: on -// ======================================================================= - -//=======================================================================* -// Fixed width word size data types: * -// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * -// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * -// real32_T, real64_T - 32 and 64 bit floating point numbers * -// ======================================================================= -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -//===========================================================================* -// Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * -// real_T, time_T, ulong_T. * -// =========================================================================== -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef unsigned char uchar_T; -typedef char_T byte_T; - -//===========================================================================* -// Complex number type definitions * -// =========================================================================== -#define CREAL_T - -typedef struct { - real32_T re; - real32_T im; -} creal32_T; - -typedef struct { - real64_T re; - real64_T im; -} creal64_T; - -typedef struct { - real_T re; - real_T im; -} creal_T; - -#define CINT8_T - -typedef struct { - int8_T re; - int8_T im; -} cint8_T; - -#define CUINT8_T - -typedef struct { - uint8_T re; - uint8_T im; -} cuint8_T; - -#define CINT16_T - -typedef struct { - int16_T re; - int16_T im; -} cint16_T; - -#define CUINT16_T - -typedef struct { - uint16_T re; - uint16_T im; -} cuint16_T; - -#define CINT32_T - -typedef struct { - int32_T re; - int32_T im; -} cint32_T; - -#define CUINT32_T - -typedef struct { - uint32_T re; - uint32_T im; -} cuint32_T; - -//=======================================================================* -// Min and Max: * -// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * -// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * -// ======================================================================= -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255U)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535U)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) - -// Block D-Work pointer type -typedef void * pointer_T; - -#endif // RTWTYPES_H - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/PID_TV_ert_rtw/CMakeLists.txt b/lib/PID_TV_ert_rtw/CMakeLists.txt deleted file mode 100644 index e8d4390c6..000000000 --- a/lib/PID_TV_ert_rtw/CMakeLists.txt +++ /dev/null @@ -1,122 +0,0 @@ -########################################################################### -# CMakeLists.txt generated for component PID_TV -# Product type: executable -########################################################################### -cmake_minimum_required(VERSION 3.12) -project(PID_TV) - -# Propagate the CMAKE_EXPORT_COMPILE_COMMANDS variable from the -# environment if it is defined as an environment variable, but not as a -# CMake variable. This is to work around a bug in CMake 3.19 when the -# "NMake Makefiles" generator is selected. -if(DEFINED ENV{CMAKE_EXPORT_COMPILE_COMMANDS} AND NOT DEFINED CMAKE_EXPORT_COMPILE_COMMANDS) - set(CMAKE_EXPORT_COMPILE_COMMANDS $ENV{CMAKE_EXPORT_COMPILE_COMMANDS}) -endif() - -# Define common variables that are used within the whole project. -set(SYSLIB_PREFIX $,$>>,lib,>) - -########################################################################### -## Path variables -########################################################################### -# Derive an absolute path to the code generation anchor folder. -get_filename_component(START_DIR .. ABSOLUTE) - -# Special directories defined by using CACHE variables can be overridden -# by setting the variable from the command line, e.g., -# -# cmake . -DMATLAB_ROOT=/path/to/another/matlab/root -set(MATLAB_ROOT /usr/local/MATLAB/R2023b CACHE PATH "") - -# Additional variables that are defined conditionally. -if("${CMAKE_CURRENT_BINARY_DIR}" STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}") - set(BINARY_START_DIR "${START_DIR}") -else() - set(BINARY_START_DIR "${CMAKE_BINARY_DIR}") -endif() - -########################################################################### -## System Libraries -########################################################################### -find_library(FOUND_LIBM m NO_SYSTEM_ENVIRONMENT_PATH PATHS ${CMAKE_C_IMPLICIT_LINK_DIRECTORIES} ${CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES}) - -########################################################################### -## Target definition and commands -########################################################################### - -# Definition of target "PID_TV". -add_executable(PID_TV ${START_DIR}/PID_TV_ert_rtw/PID_TV.cpp - ${START_DIR}/PID_TV_ert_rtw/PID_TV_capi.cpp - ${START_DIR}/PID_TV_ert_rtw/ert_main.cpp) - -# Set properties for target "PID_TV". -set_target_properties(PID_TV PROPERTIES PREFIX "" - POSITION_INDEPENDENT_CODE ON - RUNTIME_OUTPUT_DIRECTORY "${BINARY_START_DIR}/./$<0:>" - LIBRARY_OUTPUT_DIRECTORY "${BINARY_START_DIR}/./$<0:>" - ARCHIVE_OUTPUT_DIRECTORY "${BINARY_START_DIR}/./$<0:>") - -# Specify language features required for target "PID_TV". -target_compile_features(PID_TV PUBLIC cxx_std_11) - -# Specify compiler preprocessor definitions for target "PID_TV". -target_compile_definitions(PID_TV PRIVATE -DMODEL=PID_TV - -DNUMST=1 - -DNCSTATES=0 - -DHAVESTDIO - -DMODEL_HAS_DYNAMICALLY_LOADED_SFCNS=0 - -DCLASSIC_INTERFACE=0 - -DALLOCATIONFCN=0 - -DTID01EQ=0 - -DTERMFCN=1 - -DONESTEPFCN=1 - -DMAT_FILE=0 - -DMULTI_INSTANCE_CODE=1 - -DINTEGER_CODE=0 - -DMT=0) - -# Specify include directories for target "PID_TV". -target_include_directories(PID_TV PRIVATE ${START_DIR} - ${START_DIR}/PID_TV_ert_rtw - ${MATLAB_ROOT}/extern/include - ${MATLAB_ROOT}/simulink/include - ${MATLAB_ROOT}/rtw/c/src - ${MATLAB_ROOT}/rtw/c/src/ext_mode/common - ${MATLAB_ROOT}/rtw/c/ert) - -# Specify library link dependencies for target "PID_TV". CMake generator -# expressions are used to create a CMakeLists.txt file that supports -# multiple platforms with differently named system library dependencies. -target_link_libraries(PID_TV PRIVATE $<$:m> - $<$:kernel32> - $<$:ws2_32> - $<$:mswsock> - $<$:advapi32>) - -# Extract DWARF debug symbols into a separate file when building -# executable programs for Apple platforms. -if(APPLE AND (CMAKE_C_COMPILER_ID STREQUAL "AppleClang")) - string(APPEND _dsymutilcmd "$," - "xcrun;dsymutil;$;--flat," - "true" - >) - add_custom_command(TARGET PID_TV - POST_BUILD - COMMAND "${_dsymutilcmd}" - COMMAND_EXPAND_LISTS - BYPRODUCTS PID_TV.dwarf) -endif() - - -########################################################################### -## Build success message -########################################################################### -add_custom_command(TARGET PID_TV POST_BUILD - COMMAND ${CMAKE_COMMAND} -E cmake_echo_color --cyan "\\#\\#\\# Created executable: $") - -########################################################################### -## Call toolchain hook function if defined -########################################################################### -if(COMMAND toolchain_target_hook) - toolchain_target_hook(PID_TV) -endif() diff --git a/lib/PID_TV_ert_rtw/PID_TV.cpp b/lib/PID_TV_ert_rtw/PID_TV.cpp deleted file mode 100644 index 74b49435e..000000000 --- a/lib/PID_TV_ert_rtw/PID_TV.cpp +++ /dev/null @@ -1,138 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: PID_TV.cpp -// -// Code generated for Simulink model 'PID_TV'. -// -// Model version : 1.14 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Mar 26 00:06:56 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "PID_TV.h" -#include -#include "rtwtypes.h" -#include "PID_TV_capi.h" - -// Model step function -void PID_TV::step() -{ - real_T rtb_Gain; - real_T rtb_NProdOut; - real_T rtb_YawErrorrads; - - // Outputs for Atomic SubSystem: '/PID_TV' - // Sum: '/Subtract' incorporates: - // Abs: '/Abs' - // Constant: '/wb' - // Inport: '/Vx_B' - // Inport: '/Wheel Delta [rad]' - // Inport: '/Yaw Rate [rad//s]' - // Product: '/Divide' - // Trigonometry: '/Tan' - - rtb_YawErrorrads = std::abs(PID_TV_U.Vx_B) * std::tan(PID_TV_U.WheelDeltarad) / - 1.535 - PID_TV_U.YawRaterads; - shit_in = rtb_YawErrorrads; - // Product: '/NProd Out' incorporates: - // DiscreteIntegrator: '/Filter' - // Inport: '/PID_D' - // Inport: '/PID_N' - // Product: '/DProd Out' - // Sum: '/SumD' - - rtb_NProdOut = (rtb_YawErrorrads * PID_TV_U.PID_D - PID_TV_DW.Filter_DSTATE) * - PID_TV_U.PID_N; - - // Gain: '/Gain' incorporates: - // DiscreteIntegrator: '/Integrator' - // Inport: '/PID_P' - // Product: '/PProd Out' - // Sum: '/Sum' - - rtb_Gain = -((rtb_YawErrorrads * PID_TV_U.PID_P + PID_TV_DW.Integrator_DSTATE) - + rtb_NProdOut); - shit_out = rtb_Gain; - // Update for DiscreteIntegrator: '/Integrator' incorporates: - // Inport: '/PID_I' - // Product: '/IProd Out' - - PID_TV_DW.Integrator_DSTATE += rtb_YawErrorrads * PID_TV_U.PID_I * 0.2; - - // Update for DiscreteIntegrator: '/Filter' - PID_TV_DW.Filter_DSTATE += 0.2 * rtb_NProdOut; - - // Outport: '/FR_out' incorporates: - // Inport: '/FR_in' - // Sum: '/Sum' - - PID_TV_Y.FR_out = rtb_Gain + PID_TV_U.FR_in; - - // Outport: '/RR_out' incorporates: - // Inport: '/RR_in' - // Sum: '/Sum1' - - PID_TV_Y.RR_out = rtb_Gain + PID_TV_U.RR_in; - - // Outport: '/FL_out' incorporates: - // Inport: '/FL_in' - // Sum: '/Sum2' - - PID_TV_Y.FL_out = PID_TV_U.FL_in - rtb_Gain; - - // Outport: '/RL_out' incorporates: - // Inport: '/RL_in' - // Sum: '/Sum3' - - PID_TV_Y.RL_out = PID_TV_U.RL_in - rtb_Gain; - - // End of Outputs for SubSystem: '/PID_TV' -} - -// Model initialize function -void PID_TV::initialize() -{ - // Registration code - - // Initialize DataMapInfo substructure containing ModelMap for C API - PID_TV_InitializeDataMapInfo((&PID_TV_M)); -} - -// Model terminate function -void PID_TV::terminate() -{ - // (no terminate code required) -} - -// Constructor -PID_TV::PID_TV() : - PID_TV_U(), - PID_TV_Y(), - PID_TV_DW(), - PID_TV_M() -{ - // Currently there is no constructor body generated. -} - -// Destructor -// Currently there is no destructor body generated. -PID_TV::~PID_TV() = default; - -// Real-Time Model get method -PID_TV::RT_MODEL_PID_TV_T * PID_TV::getRTM() -{ - return (&PID_TV_M); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/PID_TV_ert_rtw/PID_TV.h b/lib/PID_TV_ert_rtw/PID_TV.h deleted file mode 100644 index acc502a43..000000000 --- a/lib/PID_TV_ert_rtw/PID_TV.h +++ /dev/null @@ -1,233 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: PID_TV.h -// -// Code generated for Simulink model 'PID_TV'. -// -// Model version : 1.14 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Mar 26 00:06:56 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_PID_TV_h_ -#define RTW_HEADER_PID_TV_h_ -#include "rtwtypes.h" -#include "PID_TV_types.h" -#include "rtw_modelmap.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetDataMapInfo -#define rtmGetDataMapInfo(rtm) ((rtm)->DataMapInfo) -#endif - -#ifndef rtmSetDataMapInfo -#define rtmSetDataMapInfo(rtm, val) ((rtm)->DataMapInfo = (val)) -#endif - -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -// Function to get C API Model Mapping Static Info -extern const rtwCAPI_ModelMappingStaticInfo* - PID_TV_GetCAPIStaticMap(void); - -// Class declaration for model PID_TV -class PID_TV final -{ - // public data and function members - public: - - float shit_in; - float shit_out; - // Block states (default storage) for system '' - struct DW_PID_TV_T { - real_T Integrator_DSTATE; // '/Integrator' - real_T Filter_DSTATE; // '/Filter' - }; - - // External inputs (root inport signals with default storage) - struct ExtU_PID_TV_T { - real_T Vx_B; // '/Vx_B' - real_T WheelDeltarad; // '/Wheel Delta [rad]' - real_T YawRaterads; // '/Yaw Rate [rad//s]' - real_T PID_I; // '/PID_I' - real_T PID_D; // '/PID_D' - real_T PID_N; // '/PID_N' - real_T PID_P; // '/PID_P' - real_T FR_in; // '/FR_in' - real_T RR_in; // '/RR_in' - real_T FL_in; // '/FL_in' - real_T RL_in; // '/RL_in' - }; - - // External outputs (root outports fed by signals with default storage) - struct ExtY_PID_TV_T { - real_T FR_out; // '/FR_out' - real_T RR_out; // '/RR_out' - real_T FL_out; // '/FL_out' - real_T RL_out; // '/RL_out' - }; - - // Real-time Model Data Structure - struct RT_MODEL_PID_TV_T { - const char_T * volatile errorStatus; - - // - // DataMapInfo: - // The following substructure contains information regarding - // structures generated in the model's C API. - - struct { - rtwCAPI_ModelMappingInfo mmi; - } DataMapInfo; - }; - - // Copy Constructor - PID_TV(PID_TV const&) = delete; - - // Assignment Operator - PID_TV& operator= (PID_TV const&) & = delete; - - // Move Constructor - PID_TV(PID_TV &&) = delete; - - // Move Assignment Operator - PID_TV& operator= (PID_TV &&) = delete; - - // Real-Time Model get method - PID_TV::RT_MODEL_PID_TV_T * getRTM(); - - // Root inports set method - void setExternalInputs(const ExtU_PID_TV_T *pExtU_PID_TV_T) - { - PID_TV_U = *pExtU_PID_TV_T; - } - - // Root outports get method - const ExtY_PID_TV_T &getExternalOutputs() const - { - return PID_TV_Y; - } - - // model initialize function - void initialize(); - - // model step function - void step(); - - // model terminate function - static void terminate(); - - // Constructor - PID_TV(); - - // Destructor - ~PID_TV(); - - // private data and function members - private: - // External inputs - ExtU_PID_TV_T PID_TV_U; - - // External outputs - ExtY_PID_TV_T PID_TV_Y; - - // Block states - DW_PID_TV_T PID_TV_DW; - - // Real-Time Model - RT_MODEL_PID_TV_T PID_TV_M; -}; - -//- -// These blocks were eliminated from the model due to optimizations: -// -// Block '/Constant' : Unused code path elimination - - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'PID_TV' -// '' : 'PID_TV/PID_TV' -// '' : 'PID_TV/PID_TV/PID Controller' -// '' : 'PID_TV/PID_TV/Subsystem3' -// '' : 'PID_TV/PID_TV/PID Controller/Anti-windup' -// '' : 'PID_TV/PID_TV/PID Controller/D Gain' -// '' : 'PID_TV/PID_TV/PID Controller/Filter' -// '' : 'PID_TV/PID_TV/PID Controller/Filter ICs' -// '' : 'PID_TV/PID_TV/PID Controller/I Gain' -// '' : 'PID_TV/PID_TV/PID Controller/Ideal P Gain' -// '' : 'PID_TV/PID_TV/PID Controller/Ideal P Gain Fdbk' -// '' : 'PID_TV/PID_TV/PID Controller/Integrator' -// '' : 'PID_TV/PID_TV/PID Controller/Integrator ICs' -// '' : 'PID_TV/PID_TV/PID Controller/N Copy' -// '' : 'PID_TV/PID_TV/PID Controller/N Gain' -// '' : 'PID_TV/PID_TV/PID Controller/P Copy' -// '' : 'PID_TV/PID_TV/PID Controller/Parallel P Gain' -// '' : 'PID_TV/PID_TV/PID Controller/Reset Signal' -// '' : 'PID_TV/PID_TV/PID Controller/Saturation' -// '' : 'PID_TV/PID_TV/PID Controller/Saturation Fdbk' -// '' : 'PID_TV/PID_TV/PID Controller/Sum' -// '' : 'PID_TV/PID_TV/PID Controller/Sum Fdbk' -// '' : 'PID_TV/PID_TV/PID Controller/Tracking Mode' -// '' : 'PID_TV/PID_TV/PID Controller/Tracking Mode Sum' -// '' : 'PID_TV/PID_TV/PID Controller/Tsamp - Integral' -// '' : 'PID_TV/PID_TV/PID Controller/Tsamp - Ngain' -// '' : 'PID_TV/PID_TV/PID Controller/postSat Signal' -// '' : 'PID_TV/PID_TV/PID Controller/preSat Signal' -// '' : 'PID_TV/PID_TV/PID Controller/Anti-windup/Passthrough' -// '' : 'PID_TV/PID_TV/PID Controller/D Gain/External Parameters' -// '' : 'PID_TV/PID_TV/PID Controller/Filter/Disc. Forward Euler Filter' -// '' : 'PID_TV/PID_TV/PID Controller/Filter ICs/Internal IC - Filter' -// '' : 'PID_TV/PID_TV/PID Controller/I Gain/External Parameters' -// '' : 'PID_TV/PID_TV/PID Controller/Ideal P Gain/Passthrough' -// '' : 'PID_TV/PID_TV/PID Controller/Ideal P Gain Fdbk/Disabled' -// '' : 'PID_TV/PID_TV/PID Controller/Integrator/Discrete' -// '' : 'PID_TV/PID_TV/PID Controller/Integrator ICs/Internal IC' -// '' : 'PID_TV/PID_TV/PID Controller/N Copy/Disabled' -// '' : 'PID_TV/PID_TV/PID Controller/N Gain/External Parameters' -// '' : 'PID_TV/PID_TV/PID Controller/P Copy/Disabled' -// '' : 'PID_TV/PID_TV/PID Controller/Parallel P Gain/External Parameters' -// '' : 'PID_TV/PID_TV/PID Controller/Reset Signal/Disabled' -// '' : 'PID_TV/PID_TV/PID Controller/Saturation/Passthrough' -// '' : 'PID_TV/PID_TV/PID Controller/Saturation Fdbk/Disabled' -// '' : 'PID_TV/PID_TV/PID Controller/Sum/Sum_PID' -// '' : 'PID_TV/PID_TV/PID Controller/Sum Fdbk/Disabled' -// '' : 'PID_TV/PID_TV/PID Controller/Tracking Mode/Disabled' -// '' : 'PID_TV/PID_TV/PID Controller/Tracking Mode Sum/Passthrough' -// '' : 'PID_TV/PID_TV/PID Controller/Tsamp - Integral/TsSignalSpecification' -// '' : 'PID_TV/PID_TV/PID Controller/Tsamp - Ngain/Passthrough' -// '' : 'PID_TV/PID_TV/PID Controller/postSat Signal/Forward_Path' -// '' : 'PID_TV/PID_TV/PID Controller/preSat Signal/Forward_Path' - -#endif // RTW_HEADER_PID_TV_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/PID_TV_ert_rtw/PID_TV_capi.cpp b/lib/PID_TV_ert_rtw/PID_TV_capi.cpp deleted file mode 100644 index 2388b679c..000000000 --- a/lib/PID_TV_ert_rtw/PID_TV_capi.cpp +++ /dev/null @@ -1,222 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: PID_TV_capi.cpp -// -// Code generated for Simulink model 'PID_TV'. -// -// Model version : 1.14 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Mar 26 00:06:56 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "rtw_capi.h" -#ifdef HOST_CAPI_BUILD -#include "PID_TV_capi_host.h" -#define sizeof(s) ((size_t)(0xFFFF)) -#undef rt_offsetof -#define rt_offsetof(s,el) ((uint16_T)(0xFFFF)) -#define TARGET_CONST -#define TARGET_STRING(s) (s) -#else // HOST_CAPI_BUILD -#include "builtin_typeid_types.h" -#include "PID_TV.h" -#include "PID_TV_capi.h" -#include "PID_TV_private.h" -#ifdef LIGHT_WEIGHT_CAPI -#define TARGET_CONST -#define TARGET_STRING(s) ((nullptr)) -#else -#define TARGET_CONST const -#define TARGET_STRING(s) (s) -#endif -#endif // HOST_CAPI_BUILD - -// Block output signal information -static rtwCAPI_Signals rtBlockSignals[]{ - // addrMapIndex, sysNum, blockPath, - // signalName, portNumber, dataTypeIndex, dimIndex, fxpIndex, sTimeIndex - - { - 0, 0, (nullptr), (nullptr), 0, 0, 0, 0, 0 - } -}; - -// Individual block tuning is not valid when inline parameters is * -// selected. An empty map is produced to provide a consistent * -// interface independent of inlining parameters. * - -static rtwCAPI_BlockParameters rtBlockParameters[]{ - // addrMapIndex, blockPath, - // paramName, dataTypeIndex, dimIndex, fixPtIdx - - { - 0, (nullptr), (nullptr), 0, 0, 0 - } -}; - -// Tunable variable parameters -static rtwCAPI_ModelParameters rtModelParameters[]{ - // addrMapIndex, varName, dataTypeIndex, dimIndex, fixPtIndex - { 0, (nullptr), 0, 0, 0 } -}; - -// Data Type Map - use dataTypeMapIndex to access this structure -static TARGET_CONST rtwCAPI_DataTypeMap rtDataTypeMap[]{ - // cName, mwName, numElements, elemMapIndex, dataSize, slDataId, * - // isComplex, isPointer, enumStorageType - { - "", "", 0, 0, 0, 0, 0, 0, 0 - } -}; - -#ifdef HOST_CAPI_BUILD -#undef sizeof -#endif - -// Structure Element Map - use elemMapIndex to access this structure -static TARGET_CONST rtwCAPI_ElementMap rtElementMap[]{ - // elementName, elementOffset, dataTypeIndex, dimIndex, fxpIndex - { (nullptr), 0, 0, 0, 0 }, -}; - -// Dimension Map - use dimensionMapIndex to access elements of ths structure -static rtwCAPI_DimensionMap rtDimensionMap[]{ - // dataOrientation, dimArrayIndex, numDims, vardimsIndex - { - rtwCAPI_SCALAR, 0, 0, 0 - } -}; - -// Dimension Array- use dimArrayIndex to access elements of this array -static uint_T rtDimensionArray[]{ 0 }; - -// Fixed Point Map -static rtwCAPI_FixPtMap rtFixPtMap[]{ - // fracSlopePtr, biasPtr, scaleType, wordLength, exponent, isSigned - { (nullptr), (nullptr), rtwCAPI_FIX_RESERVED, 0, 0, (boolean_T)0 }, -}; - -// Sample Time Map - use sTimeIndex to access elements of ths structure -static rtwCAPI_SampleTimeMap rtSampleTimeMap[]{ - // samplePeriodPtr, sampleOffsetPtr, tid, samplingMode - { - (nullptr), (nullptr), 0, 0 - } -}; - -static rtwCAPI_ModelMappingStaticInfo mmiStatic{ - // Signals:{signals, numSignals, - // rootInputs, numRootInputs, - // rootOutputs, numRootOutputs}, - // Params: {blockParameters, numBlockParameters, - // modelParameters, numModelParameters}, - // States: {states, numStates}, - // Maps: {dataTypeMap, dimensionMap, fixPtMap, - // elementMap, sampleTimeMap, dimensionArray}, - // TargetType: targetType - - { rtBlockSignals, 0, - (nullptr), 0, - (nullptr), 0 }, - - { rtBlockParameters, 0, - rtModelParameters, 0 }, - - { (nullptr), 0 }, - - { rtDataTypeMap, rtDimensionMap, rtFixPtMap, - rtElementMap, rtSampleTimeMap, rtDimensionArray }, - "float", - - { 3444278580U, - 663374221U, - 3400913615U, - 3634599806U }, - (nullptr), 0, - (boolean_T)0 -}; - -// Function to get C API Model Mapping Static Info -const rtwCAPI_ModelMappingStaticInfo* - PID_TV_GetCAPIStaticMap(void) -{ - return &mmiStatic; -} - -// Cache pointers into DataMapInfo substructure of RTModel -#ifndef HOST_CAPI_BUILD - -void PID_TV_InitializeDataMapInfo(PID_TV::RT_MODEL_PID_TV_T *const PID_TV_M) -{ - // Set C-API version - rtwCAPI_SetVersion(PID_TV_M->DataMapInfo.mmi, 1); - - // Cache static C-API data into the Real-time Model Data structure - rtwCAPI_SetStaticMap(PID_TV_M->DataMapInfo.mmi, &mmiStatic); - - // Cache static C-API logging data into the Real-time Model Data structure - rtwCAPI_SetLoggingStaticMap(PID_TV_M->DataMapInfo.mmi, (nullptr)); - - // Set Instance specific path - rtwCAPI_SetPath(PID_TV_M->DataMapInfo.mmi, (nullptr)); - rtwCAPI_SetFullPath(PID_TV_M->DataMapInfo.mmi, (nullptr)); - - // Cache the instance C-API logging pointer - rtwCAPI_SetInstanceLoggingInfo(PID_TV_M->DataMapInfo.mmi, (nullptr)); - - // Set reference to submodels - rtwCAPI_SetChildMMIArray(PID_TV_M->DataMapInfo.mmi, (nullptr)); - rtwCAPI_SetChildMMIArrayLen(PID_TV_M->DataMapInfo.mmi, 0); -} - -#else // HOST_CAPI_BUILD -#ifdef __cplusplus - -extern "C" -{ - -#endif - - void PID_TV_host_InitializeDataMapInfo(PID_TV_host_DataMapInfo_T *dataMap, - const char *path) - { - // Set C-API version - rtwCAPI_SetVersion(dataMap->mmi, 1); - - // Cache static C-API data into the Real-time Model Data structure - rtwCAPI_SetStaticMap(dataMap->mmi, &mmiStatic); - - // host data address map is NULL - rtwCAPI_SetDataAddressMap(dataMap->mmi, (nullptr)); - - // host vardims address map is NULL - rtwCAPI_SetVarDimsAddressMap(dataMap->mmi, (nullptr)); - - // Set Instance specific path - rtwCAPI_SetPath(dataMap->mmi, path); - rtwCAPI_SetFullPath(dataMap->mmi, (nullptr)); - - // Set reference to submodels - rtwCAPI_SetChildMMIArray(dataMap->mmi, (nullptr)); - rtwCAPI_SetChildMMIArrayLen(dataMap->mmi, 0); - } - -#ifdef __cplusplus - -} - -#endif -#endif // HOST_CAPI_BUILD - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/PID_TV_ert_rtw/PID_TV_capi.h b/lib/PID_TV_ert_rtw/PID_TV_capi.h deleted file mode 100644 index c9c42f698..000000000 --- a/lib/PID_TV_ert_rtw/PID_TV_capi.h +++ /dev/null @@ -1,32 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: PID_TV_capi.h -// -// Code generated for Simulink model 'PID_TV'. -// -// Model version : 1.14 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Mar 26 00:06:56 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_PID_TV_capi_h_ -#define RTW_HEADER_PID_TV_capi_h_ -#include "PID_TV.h" - -extern void PID_TV_InitializeDataMapInfo(PID_TV::RT_MODEL_PID_TV_T *const - PID_TV_M); - -#endif // RTW_HEADER_PID_TV_capi_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/PID_TV_ert_rtw/PID_TV_capi_host.h b/lib/PID_TV_ert_rtw/PID_TV_capi_host.h deleted file mode 100644 index 4f7f6ba28..000000000 --- a/lib/PID_TV_ert_rtw/PID_TV_capi_host.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef RTW_HEADER_PID_TV_cap_host_h__ -#define RTW_HEADER_PID_TV_cap_host_h__ -#ifdef HOST_CAPI_BUILD -#include "rtw_capi.h" -#include "rtw_modelmap.h" - -struct PID_TV_host_DataMapInfo_T { - rtwCAPI_ModelMappingInfo mmi; -}; - -#ifdef __cplusplus - -extern "C" -{ - -#endif - - void PID_TV_host_InitializeDataMapInfo(PID_TV_host_DataMapInfo_T *dataMap, - const char *path); - -#ifdef __cplusplus - -} - -#endif -#endif // HOST_CAPI_BUILD -#endif // RTW_HEADER_PID_TV_cap_host_h__ - -// EOF: PID_TV_capi_host.h diff --git a/lib/PID_TV_ert_rtw/PID_TV_private.h b/lib/PID_TV_ert_rtw/PID_TV_private.h deleted file mode 100644 index 2f83c28ff..000000000 --- a/lib/PID_TV_ert_rtw/PID_TV_private.h +++ /dev/null @@ -1,30 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: PID_TV_private.h -// -// Code generated for Simulink model 'PID_TV'. -// -// Model version : 1.14 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Mar 26 00:06:56 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_PID_TV_private_h_ -#define RTW_HEADER_PID_TV_private_h_ -#include "rtwtypes.h" -#include "builtin_typeid_types.h" -#include "PID_TV_types.h" -#endif // RTW_HEADER_PID_TV_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/PID_TV_ert_rtw/PID_TV_types.h b/lib/PID_TV_ert_rtw/PID_TV_types.h deleted file mode 100644 index 937f0f01e..000000000 --- a/lib/PID_TV_ert_rtw/PID_TV_types.h +++ /dev/null @@ -1,27 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: PID_TV_types.h -// -// Code generated for Simulink model 'PID_TV'. -// -// Model version : 1.14 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Mar 26 00:06:56 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_PID_TV_types_h_ -#define RTW_HEADER_PID_TV_types_h_ -#endif // RTW_HEADER_PID_TV_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/PID_TV_ert_rtw/R2023b/rtw/c/src/rtw_capi.h b/lib/PID_TV_ert_rtw/R2023b/rtw/c/src/rtw_capi.h deleted file mode 100644 index 48982dc71..000000000 --- a/lib/PID_TV_ert_rtw/R2023b/rtw/c/src/rtw_capi.h +++ /dev/null @@ -1,319 +0,0 @@ -/* Copyright 1994-2023 The MathWorks, Inc. */ - -/* - * File: rtw_capi.h - * - * Abstract: - * Provides structure types and constants generated by CAPI. - * Macros for accessing the fields of the structures are also provided. - * This header file is included in MODEL_capi.c - * - */ - -#ifndef _RTW_CAPI_H_ -# define _RTW_CAPI_H_ - -#ifdef SL_INTERNAL -# include "simstruct/simstruc_types.h" -#else -#ifdef HOST_CAPI_BUILD -#include "simstruc_types.h" -#else -/* Macro SIMPLIFIED_RTWTYPES_COMPATIBILITY declares compatibility with simplified version of - * rtwtypes.h */ -#define SIMPLIFIED_RTWTYPES_COMPATIBILITY -#include "rtwtypes.h" -#undef SIMPLIFIED_RTWTYPES_COMPATIBILITY -#endif -#endif -#include - -#define SS_ENUM_TYPE (((uint8_T)(255U))-1) -#define SS_STRUCT (((uint8_T)(255U))) -#define rt_offsetof(s,el) offsetof(s,el) - -/* Enumeration for fixed point scaling type*/ -typedef enum { - rtwCAPI_FIX_UNIFORM_SCALING, - rtwCAPI_FIX_NONUNIFORM_SCALING, - rtwCAPI_FIX_RESERVED -} rtwCAPI_FixPtScalingType; - -/* Enumeration for data orientation */ -typedef enum { - rtwCAPI_SCALAR, - rtwCAPI_VECTOR, - rtwCAPI_MATRIX_ROW_MAJOR, - rtwCAPI_MATRIX_COL_MAJOR, - rtwCAPI_MATRIX_COL_MAJOR_ND, - rtwCAPI_MATRIX_ROW_MAJOR_ND -} rtwCAPI_Orientation; - -/* function pointers for matfile logging */ -typedef void(*RTWLoggingFcnPtr)(void*, const void*); - -/* Signals Structure */ -/* Fields of this structure can be used to monitor block output signals*/ -typedef struct rtwCAPI_Signals_tag { - uint_T addrMapIndex; /* index into the data address map */ - uint_T sysNum; /* system identification number, where 0 = root */ - const char_T *blockPath; /* block's full path name (RTW mangled version) */ - const char_T *signalName;/* signal label (unmangled, NULL if not present) */ - uint16_T portNumber; /* port number (starts at 0) */ - uint16_T dataTypeIndex; /* index into DataTypeMap, gives signal data type */ - uint16_T dimIndex; /* index into DimensionMap, gives signal dimension*/ - uint16_T fxpIndex; /* index into FixPtMap, gives Fixed point info */ - uint8_T sTimeIndex; /* index into SampleTimeMap, gives Task info */ -} rtwCAPI_Signals; - -/* Macros for accessing Signals structure fields */ -/* bio is a pointer to array of Signals structures */ -#define rtwCAPI_GetSignalAddrIdx(bio, i) ((bio)[(i)].addrMapIndex) -#define rtwCAPI_GetSignalBlockPath(bio, i) ((bio)[(i)].blockPath) -#define rtwCAPI_GetSignalName(bio, i) ((bio)[(i)].signalName) -#define rtwCAPI_GetSignalPortNumber(bio, i) ((bio)[(i)].portNumber) -#define rtwCAPI_GetSignalDataTypeIdx(bio, i) ((bio)[(i)].dataTypeIndex) -#define rtwCAPI_GetSignalDimensionIdx(bio, i) ((bio)[(i)].dimIndex) -#define rtwCAPI_GetSignalFixPtIdx(bio, i) ((bio)[(i)].fxpIndex) -#define rtwCAPI_GetSignalSampleTimeIdx(bio, i) ((bio)[(i)].sTimeIndex) -#define rtwCAPI_GetSignalSysNum(bio, i) ((bio)[(i)].sysNum) - -/* BlockParameters Structure */ -/* Fields of this structure can be used to monitor/modify block parameters */ -typedef struct rtwCAPI_BlockParameters_tag { - uint_T addrMapIndex; /* index into the data address map */ - const char_T *blockPath; /* block's full path name (RTW mangled version) */ - const char_T *paramName; /* parameter name */ - uint16_T dataTypeIndex; /* index into DataTypeMap, gives param data type */ - uint16_T dimIndex; /* index into DimensionMap, gives param dimensions*/ - uint16_T fxpIndex; /* index into FixPtMap, gives Fixed point info */ -} rtwCAPI_BlockParameters; - -/* Macros for accessing BlockParameter structure fields */ -/* prm is a pointer to array of BlockParameter structures */ -#define rtwCAPI_GetBlockParameterAddrIdx(prm, i) ((prm)[(i)].addrMapIndex) -#define rtwCAPI_GetBlockParameterBlockPath(prm, i) ((prm)[(i)].blockPath) -#define rtwCAPI_GetBlockParameterName(prm, i) ((prm)[(i)].paramName) -#define rtwCAPI_GetBlockParameterDataTypeIdx(prm, i) ((prm)[(i)].dataTypeIndex) -#define rtwCAPI_GetBlockParameterDimensionIdx(prm, i) ((prm)[(i)].dimIndex) -#define rtwCAPI_GetBlockParameterFixPtIdx(prm, i) ((prm)[(i)].fxpIndex) - -/* ModelParameter Structure */ -/* Fields of this structure can be used to monitor/modify model parameters * - * When Inline Parameters is on, you can configure tunable block parameters * - * as model parameters. */ -typedef struct rtwCAPI_ModelParameters_tag { - uint_T addrMapIndex; /* index into the data address map */ - const char_T *varName; /* variable name */ - uint16_T dataTypeIndex; /* index into DataTypeMap, gives param data type */ - uint16_T dimIndex; /* index into DimensionMap, gives param dimensions*/ - uint16_T fxpIndex; /* index into FixPtMap, gives Fixed point info */ -} rtwCAPI_ModelParameters; - -/* Macros for accessing ModelParameter structure fields */ -/* prm is a pointer to array of ModelParameter structures */ -#define rtwCAPI_GetModelParameterAddrIdx(prm, i) ((prm)[(i)].addrMapIndex) -#define rtwCAPI_GetModelParameterName(prm, i) ((prm)[(i)].varName) -#define rtwCAPI_GetModelParameterDataTypeIdx(prm, i) ((prm)[(i)].dataTypeIndex) -#define rtwCAPI_GetModelParameterDimensionIdx(prm, i) ((prm)[(i)].dimIndex) -#define rtwCAPI_GetModelParameterFixPtIdx(prm, i) ((prm)[(i)].fxpIndex) - -/* rtwCAPI_States Structure */ -/* Fields of this structure can be used to monitor block States */ -typedef struct rtwCAPI_States_tag { - uint_T addrMapIndex; /* index into the data address map */ - int_T contStateStartIndex; /* starting index in the model's continuous * - * state and state derivative vectors. This is * - * set to -1 for discrete states */ - const char_T *blockPath; /* block's full path name (RTW mangled version) */ - const char_T *stateName; /* state name, Default:DWork/Cont State Identifier */ - const char_T *pathAlias; /* Alias block path */ - uint16_T dWorkIndex; /* Index into the Block DWork, 0 for cont. states */ - uint16_T dataTypeIndex; /* index into DataTypeMap, gives state data type */ - uint16_T dimIndex; /* index into DimensionMap, gives state dimensions */ - uint16_T fxpIndex; /* index into FixPtMap, gives Fixed point info */ - uint8_T sTimeIndex; /* index into SampleTimeMap, gives Task info */ - uint8_T isContinuous; /* if the state is a Continuous state*/ - int_T hierInfoIdx; /* the hierInfo associated with this state, -1 if no hierInfo */ - uint_T flatElemIdx; /* flat element index in hierarchy */ -} rtwCAPI_States; - -/* Macros for accessing States structure fields */ -/* bState is a pointer to array of States structures */ -#define rtwCAPI_GetStateAddrIdx(bState, i) ((bState)[(i)].addrMapIndex) -#define rtwCAPI_GetContStateStartIndex(bState, i) \ - ((bState)[(i)].contStateStartIndex) -#define rtwCAPI_GetStateBlockPath(bState, i) ((bState)[(i)].blockPath) -#define rtwCAPI_GetStateName(bState, i) ((bState)[(i)].stateName) -#define rtwCAPI_GetStatePathAlias(bState, i) ((bState)[(i)].pathAlias) -#define rtwCAPI_GetStateDWorkIdx(bState, i) ((bState)[(i)].dWorkIndex) -#define rtwCAPI_GetStateDataTypeIdx(bState, i) ((bState)[(i)].dataTypeIndex) -#define rtwCAPI_GetStateDimensionIdx(bState, i) ((bState)[(i)].dimIndex) -#define rtwCAPI_GetStateFixPtIndex(bState, i) ((bState)[(i)].fxpIndex) -#define rtwCAPI_GetStateSampleTimeIdx(bState, i) ((bState)[(i)].sTimeIndex) -#define rtwCAPI_IsAContinuousState(bState, i) ((bState)[(i)].isContinuous == 1) -#define rtwCAPI_GetStateHierInfoIdx(bState, i) ((bState)[(i)].hierInfoIdx) -#define rtwCAPI_GetStateFlatElemIdx(bState, i) ((bState)[(i)].flatElemIdx) - -/* DataTypeMap structure */ -/* Members provide data type information of a signal or parameter */ -typedef struct rtwCAPI_DataTypeMap_tag { - const char_T *cDataName; /* C language data type name */ - const char_T *mwDataName; /* MathWorks data type, typedef in rtwtypes.h */ - uint16_T numElements; /* number of elements, 0 for non-structure data */ - uint16_T elemMapIndex;/* index into the ElementMap, gives Bus Info */ - uint16_T dataSize; /* data size in Bytes */ - uint8_T slDataId; /* enumerated data type from simstruc_types.h */ - unsigned int isComplex:1; /* is the data type complex (1=Complex, 0=Real) */ - unsigned int isPointer:1; /* is data accessed Via Pointer (1=yes, 0= no) */ - uint8_T enumStorageType; /* storage type for enum data types */ -} rtwCAPI_DataTypeMap; - -/* Notes on rtwCAPI_DataTypeMap: - * cDataName - The ANSI C equivalent data type. - * For fixed point data - * cDataName = the native integer equivalent - * For Complex data or non-virtual structures, - * cDataName = "struct" - * mwDataName - RTW defined data type. Typedef can be found in rtwtypes.h - * For fixed point data, - * mwDataName = the integer type in which the data is stored. - * Use rtwCAPI_FixPtMap to get slope & bias of the data. - * numElements - Number of elements/members in the data type. If the data type - * represents a non-virtual structure (for e.g, bus structure), - * this field gives the number of elements in the structure. - * For non-structure data type, the default value is 0 - * Complex data types are treated as non-structures - * elemMapIndex - Index into the rtwCAPI_ElementMap which maps each of the - * element/member of a structure data type. - * slDataId - Enumerated Simulink Data type defined in simstruc_types.h. - * The enumerations are - * o SS_DOUBLE - * o SS_SINGLE - * o SS_INT8 - * o SS_UINT8 - * o SS_INT16 - * o SS_UINT16 - * o SS_INT32 - * o SS_UINT32 - * o SS_BOOLEAN - * o SS_ENUM_TYPE (for enumerated data types) - * o SS_STRUCT (for non-virtual structures) - * For fixed point data: - * slDataId = enumeration corresponding to the integer type - * in which the data is stored - * For Complex data: - * slDataId = enumeration corresponding to the data type of - * the real part. - * isComplex - is the data type complex (1=Complex, 0=Real) - * isPointer - is data accessed Via Pointer (1=yes, 0= no) - */ - -/* Macros for accessing DataTypeMap fields/members */ -#define rtwCAPI_GetDataTypeCName(dTypeMap, i) ((dTypeMap)[(i)].cDataName) -#define rtwCAPI_GetDataTypeMWName(dTypeMap, i) ((dTypeMap)[(i)].mwDataName) -#define rtwCAPI_GetDataTypeNumElements(dTypeMap, i) ((dTypeMap)[(i)].numElements) -#define rtwCAPI_GetDataTypeElemMapIndex(dTypeMap,i) ((dTypeMap)[(i)].elemMapIndex) -#define rtwCAPI_GetDataTypeSLId(dTypeMap, i) ((dTypeMap)[(i)].slDataId) -#define rtwCAPI_GetDataTypeSize(dTypeMap, i) ((dTypeMap)[(i)].dataSize) -#define rtwCAPI_GetDataIsComplex(dTypeMap, i) ((dTypeMap)[(i)].isComplex) -#define rtwCAPI_GetDataIsPointer(dTypeMap, i) ((dTypeMap)[(i)].isPointer) -#define rtwCAPI_GetDataIsEnum(dTypeMap, i) ((dTypeMap)[(i)].slDataId == SS_ENUM_TYPE) -#define rtwCAPI_GetDataEnumStorageType(dTypeMap, i) ((dTypeMap)[(i)].enumStorageType) - -/* Macros for determining whether signals/states support MAT-File logging */ -#define rtwCAPI_CanLogSignalToMATFile(dTypeMap, bio, i) \ - (rtwCAPI_GetDataTypeSLId(dTypeMap, rtwCAPI_GetSignalDataTypeIdx(bio, i)) < SS_NUM_BUILT_IN_DTYPE) -#define rtwCAPI_CanLogStateToMATFile(dTypeMap, bState, i) \ - (rtwCAPI_GetDataTypeSLId(dTypeMap, rtwCAPI_GetStateDataTypeIdx(bState, i)) < SS_NUM_BUILT_IN_DTYPE) - -/* ElementMap structure */ -/* Fields provide information on elements (fields) in Simulink Bus structure */ -typedef struct rtwCAPI_ElementMap_tag { - const char_T *elementName; /* name of the element */ - uint32_T elementOffset; /* offset of the structure element in bytes */ - uint16_T dataTypeIndex; /* index into DataTypeMap, datatype of the element*/ - uint16_T dimIndex; /* index into DimensionMap, element's dimensions */ - uint16_T fxpIndex; /* index into fixPtMap, fixed point information */ -} rtwCAPI_ElementMap; - -/* Macros for accessing ElementMap fields */ -#define rtwCAPI_GetElementName(elemMap, i) ((elemMap)[(i)].elementName) -#define rtwCAPI_GetElementOffset(elemMap, i) ((elemMap)[(i)].elementOffset) -#define rtwCAPI_GetElementDataTypeIdx(elemMap, i) ((elemMap)[(i)].dataTypeIndex) -#define rtwCAPI_GetElementDimensionIdx(elemMap, i) ((elemMap)[(i)].dimIndex) -#define rtwCAPI_GetElementFixPtIdx(elemMap, i) ((elemMap)[(i)].fxpIndex) - -/* DimensionMap structure */ -/* Fields provide dimensions of a signal or a parameter */ -typedef struct rtwCAPI_DimensionMap_tag { - rtwCAPI_Orientation orientation; - /* orientation of data -scalar/vector/matrix/ND */ - uint_T dimArrayIndex; /* index into dimension array */ - uint8_T numDims; /* number of dimensions */ - uint_T vardimsIndex; /* index into vardims address array */ -} rtwCAPI_DimensionMap; - -/* Macros for accessing DimensionMap fields */ -#define rtwCAPI_GetOrientation(dimMap, i) ((dimMap)[(i)].orientation) -#define rtwCAPI_GetDimArrayIndex(dimMap, i) ((dimMap)[(i)].dimArrayIndex) -#define rtwCAPI_GetNumDims(dimMap, i) ((dimMap)[(i)].numDims) -#define rtwCAPI_GetDimsIsVariable(vardimsAddrMap, dimMap, i) (vardimsAddrMap[(dimMap)[(i)].vardimsIndex] != NULL) - -/* FixPtMap Structure */ -/* Fields provide fixed point information of a signal or parameter */ -typedef struct rtwCAPI_FixPtMap_tag { - const void* fracSlopePtr; /* pointer to fractional slope value */ - const void* biasPtr; /* pointer to bias value */ - rtwCAPI_FixPtScalingType scaleType; - /* scaling type - uniform/non-uniform */ - uint32_T wordLength; /* number of bits required to store value * - * In MATLAB, word length of the fi object */ - int32_T exponent; /* exponent */ - boolean_T isSigned; /* 1 = signed data, 0 = unsigned data */ -} rtwCAPI_FixPtMap; - -/* Macros for accessing FixPtMap fields */ -#define rtwCAPI_GetFxpFracSlopePtr(fxpMap, i) ((fxpMap)[(i)].fracSlopePtr) -#define rtwCAPI_GetFxpBiasPtr(fxpMap, i) ((fxpMap)[(i)].biasPtr) -#define rtwCAPI_GetFxpScaling(fxpMap, i) ((fxpMap)[(i)].scaleType) -#define rtwCAPI_GetFxpWordLength(fxpMap, i) ((fxpMap)[(i)].wordLength) -#define rtwCAPI_GetFxpExponent(fxpMap, i) ((fxpMap)[(i)].exponent) -#define rtwCAPI_GetFxpIsSigned(fxpMap, i) ((fxpMap)[(i)].isSigned) - -/* Macros to get the slope and bias values casted to DOUBLE */ -#define rtwCAPI_GetFxpFracSlope(fxpMap, i) (*((real_T*) rtwCAPI_GetFxpFracSlopePtr(fxpMap,i))) -#define rtwCAPI_GetFxpBias(fxpMap, i) (*((real_T*) rtwCAPI_GetFxpBiasPtr(fxpMap, i))) - -/* SampleTimeMap Structure */ -typedef struct rtwCAPI_SampleTimeMap_tag { - const void* samplePeriodPtr; /* pointer to sample time period value */ - const void* sampleOffsetPtr; /* pointer to sample time Offset value */ - int8_T tid; /* task identifier */ - uint8_T samplingMode; /* 1 = FrameBased, 0 = SampleBased */ -} rtwCAPI_SampleTimeMap; - -/* Macros for accessing SampleTimeMap fields */ - -#define rtwCAPI_GetSamplePeriodPtr(sampTimeMap, i) ((sampTimeMap)[(i)].samplePeriodPtr) -#define rtwCAPI_GetSampleOffsetPtr(sampTimeMap, i) ((sampTimeMap)[(i)].sampleOffsetPtr) -#define rtwCAPI_GetSampleTimeTID(sampTimeMap, i) ((sampTimeMap)[(i)].tid) -#define rtwCAPI_GetSamplingMode(sampTimeMap, i) ((sampTimeMap)[(i)].samplingMode) - -/* Macros to get sample period and offset casted to DOUBLE */ -/* if sample period is -3.0 and offset is 1.0, then it represents an initialize function, - if sample period is -3.0 and offset is 2.0, then it represents a terminate function, - if sample period is -3.0 and offset > 2.0, then it represents a reset function. -*/ -#define rtwCAPI_GetSamplePeriod(sampTimeMap, i) (*((real_T *) rtwCAPI_GetSamplePeriodPtr(sampTimeMap, i))) -#define rtwCAPI_GetSampleOffset(sampTimeMap, i) (*((real_T *) rtwCAPI_GetSampleOffsetPtr(sampTimeMap, i))) - -/* Macro to get the actual data address */ -#define rtwCAPI_GetDataAddress(dataAddrMap,addrIdx) ((dataAddrMap)[(addrIdx)]) - -/* Macro to get the actual var dims address */ -#define rtwCAPI_GetCurrentDimsAddr(vardimsAddrMap,dimMap,addrIdx) ((vardimsAddrMap)[(dimMap)[(addrIdx)].vardimsIndex]) - -#endif /* _RTW_CAPI_H_ */ - -/* EOF - rtw_capi.h */ diff --git a/lib/PID_TV_ert_rtw/R2023b/rtw/c/src/rtw_modelmap.h b/lib/PID_TV_ert_rtw/R2023b/rtw/c/src/rtw_modelmap.h deleted file mode 100644 index a35d06d67..000000000 --- a/lib/PID_TV_ert_rtw/R2023b/rtw/c/src/rtw_modelmap.h +++ /dev/null @@ -1,282 +0,0 @@ -/* Copyright 1994-2022 The MathWorks, Inc. */ - -/* - * File: rtw_modelmap.h - * - * Abstract: - * Model tuning information. Use the provided structure access methods - * whenever possible. - * - * For details about these structures see Simulink Coder User's guide. - */ - -#ifndef __RTW_MODELMAP__ -#define __RTW_MODELMAP__ - -#ifdef SL_INTERNAL - -# include "version.h" -# include "util/memmgr/memalloc.hpp" -# include "simstruct/simstruc_types.h" -# include "simulinkcoder_capi_export.hpp" - -#else - -# include - -#ifdef HOST_CAPI_BUILD -# include "simstruc_types.h" -#else -/* Macro SIMPLIFIED_RTWTYPES_COMPATIBILITY declares compatibility with simplified version of - * rtwtypes.h */ -#define SIMPLIFIED_RTWTYPES_COMPATIBILITY -#include "rtwtypes.h" -#undef SIMPLIFIED_RTWTYPES_COMPATIBILITY -#endif - -#ifndef __RTW_UTFREE__ -# define __RTW_UTFREE__ -# define utFree(arg) if (arg) free(arg) -# define utMalloc(arg) malloc(arg) -#endif - -# define SIMULINKCODER_CAPI_API extern -#endif - -#include "rtw_capi.h" -#include "rtw_modelmap_logging.h" - -typedef struct rtwCAPI_ModelMappingInfo_tag rtwCAPI_ModelMappingInfo; -typedef struct rtwCAPI_ModelMappingStaticInfo_tag rtwCAPI_ModelMappingStaticInfo; - -/* ModelMappingStaticInfo */ -struct rtwCAPI_ModelMappingStaticInfo_tag { - /* signals */ - struct { - rtwCAPI_Signals const *signals; /* Signals Array */ - uint_T numSignals; /* Num Signals */ - rtwCAPI_Signals const *rootInputs; /* Root Inputs array */ - uint_T numRootInputs; /* Num Root Inputs */ - rtwCAPI_Signals const *rootOutputs; /* Root Outputs array */ - uint_T numRootOutputs;/* Num Root Outputs */ - } Signals; - - /* parameters */ - struct { - rtwCAPI_BlockParameters const *blockParameters; /* Block parameters Array */ - uint_T numBlockParameters; /* Num block parameters */ - rtwCAPI_ModelParameters const *modelParameters; /* Model parameters Array*/ - uint_T numModelParameters; /* Num Model parameters */ - } Params; - - /* states */ - struct { - rtwCAPI_States const *states; /* States array */ - uint_T numStates; /* Num States */ - } States; - - /* Static maps */ - /* datatypes, dimensions, fixed point, structure elements, sample times */ - struct { - rtwCAPI_DataTypeMap const *dataTypeMap; /* Data Type Map */ - rtwCAPI_DimensionMap const *dimensionMap; /* Data Dimension Map */ - rtwCAPI_FixPtMap const *fixPtMap; /* Fixed Point Map */ - rtwCAPI_ElementMap const *elementMap; /* Structure Element map */ - rtwCAPI_SampleTimeMap const *sampleTimeMap; /* Sample Times Map */ - uint_T const *dimensionArray; /* Dimension Array */ - } Maps; - - /* TargetType - string specifying the intended target of the generated * - * C-API. * - * targetType = "float" - target supports floats and integer code * - * = "integer" - target supports integer only code */ - char_T const *targetType; - - /* for internal use */ - uint32_T modelChecksum[4]; - rtwCAPI_ModelMapLoggingStaticInfo const *staticLogInfo; - size_t rtpSize; - /* If this instance in a protected model*/ - boolean_T isProtectedModel; -}; - -/* ModelMappingInfo */ -struct rtwCAPI_ModelMappingInfo_tag { - /* ModelMappingInfo version */ - uint8_T versionNum; - - /* Reference to static model data, all model instances share this map */ - rtwCAPI_ModelMappingStaticInfo *staticMap; - - /* Instance specific Maps, each model instance has a unique InstanceMap */ - struct { - const char* path; /* Path to this instance */ - char* fullPath; - void** dataAddrMap; /* Data Address map */ - rtwCAPI_ModelMappingInfo** childMMIArray; /* array of child MMI */ - uint_T childMMIArrayLen; /* Number of child MMIs */ - int_T contStateStartIndex; - - /* for internal use */ - rtwCAPI_ModelMapLoggingInstanceInfo *instanceLogInfo; - int32_T** vardimsAddrMap; /* Vardims Address map */ - void* rtpAddress; - RTWLoggingFcnPtr* RTWLoggingPtrs; /* MatFile logging information */ - } InstanceMap; -}; - -/* Macros for accessing ModelMappingStaticInfo fields */ -#define rtwCAPI_GetSignalsFromStaticMap(SM) ((SM)->Signals.signals) -#define rtwCAPI_GetNumSignalsFromStaticMap(SM) ((SM)->Signals.numSignals) -#define rtwCAPI_GetLogSignalsArrayFromStaticMap(SM) ((SM)->Signals.logSignalsArray) -#define rtwCAPI_GetNumLogSignalsFromStaticMap(SM) ((SM)->Signals.numLogSignals) - -#define rtwCAPI_GetBlockParametersFromStaticMap(SM) ((SM)->Params.blockParameters) -#define rtwCAPI_GetNumBlockParametersFromStaticMap(SM) ((SM)->Params.numBlockParameters) -#define rtwCAPI_GetModelParametersFromStaticMap(SM) ((SM)->Params.modelParameters) -#define rtwCAPI_GetNumModelParametersFromStaticMap(SM) ((SM)->Params.numModelParameters) - -#define rtwCAPI_GetStatesFromStaticMap(SM) ((SM)->States.states) -#define rtwCAPI_GetNumStatesFromStaticMap(SM) ((SM)->States.numStates) - -#define rtwCAPI_GetRootInputsFromStaticMap(SM) ((SM)->Signals.rootInputs) -#define rtwCAPI_GetNumRootInputsFromStaticMap(SM) ((SM)->Signals.numRootInputs) -#define rtwCAPI_GetRootOutputsFromStaticMap(SM) ((SM)->Signals.rootOutputs) -#define rtwCAPI_GetNumRootOutputsFromStaticMap(SM) ((SM)->Signals.numRootOutputs) - -#define rtwCAPI_GetDataTypeMapFromStaticMap(SM) ((SM)->Maps.dataTypeMap) -#define rtwCAPI_GetDimensionMapFromStaticMap(SM) ((SM)->Maps.dimensionMap) -#define rtwCAPI_GetFixPtMapFromStaticMap(SM) ((SM)->Maps.fixPtMap) -#define rtwCAPI_GetElementMapFromStaticMap(SM) ((SM)->Maps.elementMap) -#define rtwCAPI_GetSampleTimeMapFromStaticMap(SM) ((SM)->Maps.sampleTimeMap) -#define rtwCAPI_GetDimensionArrayFromStaticMap(SM) ((SM)->Maps.dimensionArray) - -/* Macros for accessing ModelMappingInfo fields */ -#define rtwCAPI_GetSignals(MMI) rtwCAPI_GetSignalsFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetNumSignals(MMI) rtwCAPI_GetNumSignalsFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetLogSignalsArray(MMI) rtwCAPI_GetLogSignalsArrayFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetNumLogSignals(MMI) rtwCAPI_GetNumLogSignalsFromStaticMap((MMI)->staticMap) - -#define rtwCAPI_GetBlockParameters(MMI) rtwCAPI_GetBlockParametersFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetNumBlockParameters(MMI) rtwCAPI_GetNumBlockParametersFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetModelParameters(MMI) rtwCAPI_GetModelParametersFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetNumModelParameters(MMI) rtwCAPI_GetNumModelParametersFromStaticMap((MMI)->staticMap) - -#define rtwCAPI_GetStates(MMI) rtwCAPI_GetStatesFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetNumStates(MMI) rtwCAPI_GetNumStatesFromStaticMap((MMI)->staticMap) - -#define rtwCAPI_GetRootInputs(MMI) rtwCAPI_GetRootInputsFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetNumRootInputs(MMI) rtwCAPI_GetNumRootInputsFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetRootOutputs(MMI) rtwCAPI_GetRootOutputsFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetNumRootOutputs(MMI) rtwCAPI_GetNumRootOutputsFromStaticMap((MMI)->staticMap) - -#define rtwCAPI_GetDataTypeMap(MMI) rtwCAPI_GetDataTypeMapFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetDimensionMap(MMI) rtwCAPI_GetDimensionMapFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetFixPtMap(MMI) rtwCAPI_GetFixPtMapFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetElementMap(MMI) rtwCAPI_GetElementMapFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetSampleTimeMap(MMI) rtwCAPI_GetSampleTimeMapFromStaticMap((MMI)->staticMap) -#define rtwCAPI_GetDimensionArray(MMI) rtwCAPI_GetDimensionArrayFromStaticMap((MMI)->staticMap) -#define rtwCAPI_IsProtectedModel(MMI) ((MMI)->staticMap->isProtectedModel) - -#define rtwCAPI_GetStaticLoggingInfo(MMI) ((MMI)->staticMap->staticLogInfo) - -#define rtwCAPI_GetVersion(MMI) ((MMI)->versionNum) -#define rtwCAPI_GetDataAddressMap(MMI) ((MMI)->InstanceMap.dataAddrMap) -#define rtwCAPI_GetVarDimsAddressMap(MMI) ((MMI)->InstanceMap.vardimsAddrMap) -#define rtwCAPI_GetRTWLoggingPtrsMap(MMI) ((MMI)->InstanceMap.RTWLoggingPtrs) -#define rtwCAPI_GetPath(MMI) ((MMI)->InstanceMap.path) -#define rtwCAPI_GetFullPath(MMI) ((MMI)->InstanceMap.fullPath) -#define rtwCAPI_GetChildMMI(MMI,i) ((MMI)->InstanceMap.childMMIArray[i]) -#define rtwCAPI_GetChildMMIArray(MMI) ((MMI)->InstanceMap.childMMIArray) -#define rtwCAPI_GetChildMMIArrayLen(MMI) ((MMI)->InstanceMap.childMMIArrayLen) -#define rtwCAPI_MMIGetContStateStartIndex(MMI) ((MMI)->InstanceMap.contStateStartIndex) -#define rtwCAPI_GetInstanceLoggingInfo(MMI) ((MMI)->InstanceMap.instanceLogInfo) - -/* Macros for setting ModelMappingInfo fields */ -#define rtwCAPI_SetVersion(MMI, n) ((MMI).versionNum = (n)) -#define rtwCAPI_SetStaticMap(MMI, statMap) (MMI).staticMap = (statMap) -#define rtwCAPI_SetLoggingStaticMap(MMI,lStatMap) (MMI).staticMap->staticLogInfo = (lStatMap) -#define rtwCAPI_SetDataAddressMap(MMI, dAddr) (MMI).InstanceMap.dataAddrMap = (dAddr) -#define rtwCAPI_SetVarDimsAddressMap(MMI, vAddr) (MMI).InstanceMap.vardimsAddrMap = (vAddr) -#define rtwCAPI_SetLoggingPtrs(MMI, lAddr) (MMI).InstanceMap.RTWLoggingPtrs = (lAddr) -#define rtwCAPI_SetPath(MMI,p) (MMI).InstanceMap.path = (p) -#define rtwCAPI_SetFullPath(MMI,p) (MMI).InstanceMap.fullPath = (p) -#define rtwCAPI_SetChildMMI(MMI,i,cMMI) (MMI).InstanceMap.childMMIArray[i] = (cMMI) -#define rtwCAPI_SetChildMMIArray(MMI,cMMIs) (MMI).InstanceMap.childMMIArray = (cMMIs) -#define rtwCAPI_SetChildMMIArrayLen(MMI,n) (MMI).InstanceMap.childMMIArrayLen = (n) -#define rtwCAPI_MMISetContStateStartIndex(MMI,i) (MMI).InstanceMap.contStateStartIndex = (i) -#define rtwCAPI_SetInstanceLoggingInfo(MMI,l) (MMI).InstanceMap.instanceLogInfo = (l) - -/* Functions in rtw_modelmap_utils.c */ -#ifdef __cplusplus -extern "C" { -#endif -SIMULINKCODER_CAPI_API char* rtwCAPI_EncodePath(const char* path); -SIMULINKCODER_CAPI_API boolean_T rtwCAPI_HasStates(const rtwCAPI_ModelMappingInfo* mmi); -SIMULINKCODER_CAPI_API int_T rtwCAPI_GetNumStateRecords(const rtwCAPI_ModelMappingInfo* mmi); -SIMULINKCODER_CAPI_API int_T rtwCAPI_GetNumStateRecordsForRTWLogging(const rtwCAPI_ModelMappingInfo* mmi); -SIMULINKCODER_CAPI_API int_T rtwCAPI_GetNumContStateRecords(const rtwCAPI_ModelMappingInfo* mmi); -SIMULINKCODER_CAPI_API void rtwCAPI_FreeFullPaths(rtwCAPI_ModelMappingInfo* mmi); -SIMULINKCODER_CAPI_API const char_T* rtwCAPI_UpdateFullPaths(rtwCAPI_ModelMappingInfo* mmi, - const char_T* path, - boolean_T isCalledFromTopModel); -SIMULINKCODER_CAPI_API char* rtwCAPI_GetFullStateBlockPath(const char* stateBlockPath, - const char* mmiPath, - size_t mmiPathLen, - boolean_T crossingModel); -SIMULINKCODER_CAPI_API uint_T rtwCAPI_GetStateWidth(const rtwCAPI_DimensionMap* dimMap, - const uint_T* dimArray, - const rtwCAPI_States* states, - uint_T iState); -SIMULINKCODER_CAPI_API const char_T* rtwCAPI_GetStateRecordInfo(const rtwCAPI_ModelMappingInfo* mmi, - const char_T** sigBlockName, - const char_T** sigLabel, - const char_T** sigName, - int_T* sigWidth, - int_T* sigDataType, - int_T* logDataType, - int_T* sigComplexity, - void** sigDataAddr, - RTWLoggingFcnPtr* RTWLoggingPtrs, - boolean_T* sigCrossMdlRef, - boolean_T* sigInProtectedMdl, - const char_T** sigPathAlias, - double* sigSampleTime, - int_T* sigHierInfoIdx, - uint_T* sigFlatElemIdx, - const rtwCAPI_ModelMappingInfo** sigMMI, - int_T* sigIdx, - boolean_T crossingModel, - boolean_T isInProtectedMdl, - real_T* stateDerivVector, - boolean_T rtwLogging); -SIMULINKCODER_CAPI_API int_T rtwCAPI_GetNumSigLogRecords(const rtwCAPI_ModelMappingInfo* mmi); -SIMULINKCODER_CAPI_API int_T rtwCAPI_GetNumSigLogRecordsForRTWLogging(const rtwCAPI_ModelMappingInfo* mmi); -SIMULINKCODER_CAPI_API const char_T* rtwCAPI_GetSigLogRecordInfo(const rtwCAPI_ModelMappingInfo* mmi, - const char_T** sigBlockName, - const char_T** sigLabel, - int_T* sigWidth, - int_T* sigDataType, - int_T* logDataType, - int_T* sigComplexity, - void** sigDataAddr, - boolean_T* sigCrossMdlRef, - int_T* sigIdx, - boolean_T crossingModel, - boolean_T rtwLogging); -SIMULINKCODER_CAPI_API void rtwCAPI_CountSysRan(const rtwCAPI_ModelMappingInfo *mmi, - int *count); -SIMULINKCODER_CAPI_API void rtwCAPI_FillSysRan(const rtwCAPI_ModelMappingInfo *mmi, - sysRanDType **sysRan, - int *sysTid, - int *fillIdx); -#ifdef __cplusplus -} -#endif - -#endif /* __RTW_MODELMAP__ */ - -/* EOF - rtw_modelmap.h */ - -/* LocalWords: MMI Vardims utils - */ diff --git a/lib/PID_TV_ert_rtw/R2023b/rtw/c/src/rtw_modelmap_logging.h b/lib/PID_TV_ert_rtw/R2023b/rtw/c/src/rtw_modelmap_logging.h deleted file mode 100644 index 4ca6f118c..000000000 --- a/lib/PID_TV_ert_rtw/R2023b/rtw/c/src/rtw_modelmap_logging.h +++ /dev/null @@ -1,179 +0,0 @@ -/* Copyright 1994-2013 The MathWorks, Inc. */ - -/* - * File: rtw_modelmap_logging.h - * - * Abstract: - * Meta information used in conjunction with the ModelMappingInfo to - * facilitate logging. - * - */ - -#ifndef __RTW_MODELMAP_LOGGING__ -#define __RTW_MODELMAP_LOGGING__ - -#ifdef SL_INTERNAL -# include "simstruct/sysran_types.h" -#else -# include "sysran_types.h" -#endif - -typedef struct rtwCAPI_ModelMapLoggingStaticInfo_tag rtwCAPI_ModelMapLoggingStaticInfo; -typedef struct rtwCAPI_ModelMapLoggingInstanceInfo_tag rtwCAPI_ModelMapLoggingInstanceInfo; - -typedef struct rtwCAPI_LoggingMetaInfo_tag { - uint_T sigIdx; /* same index as the rtwCAPI_Signals */ - uint_T startIdx; /* starting index in the block I/O */ - const char *blockPath; - uint_T portIdx; - int_T parentSysNum; /* system number of the parent signal that requested - the logging */ -} rtwCAPI_LoggingMetaInfo; - -typedef enum { - rtwCAPI_bus, - rtwCAPI_signal -} rtwCAPI_LoggingBusElementType; - -typedef struct rtwCAPI_LoggingBusElement_tag { - uint_T index; - rtwCAPI_LoggingBusElementType type; -} rtwCAPI_LoggingBusElement; - -typedef struct rtwCAPI_LoggingBusSignals_tag { - const char *signalName; /* logname should be in the sigProp */ - const char *blockPath; - uint_T portIdx; - uint_T numElements; - - const rtwCAPI_LoggingBusElement *elements; -} rtwCAPI_LoggingBusSignals; - -typedef struct rtwCAPI_StateflowSignalLoggingInfo_tag { - const char *signalName; - const char *blockPath; - const char *loggingName; - uint_T maxPoints; - uint_T decimation; - boolean_T useCustomName; -} rtwCAPI_StateflowSignalLoggingInfo; - -/* Structure used for representing bus hierarchy information for dataset logging */ -typedef struct rtwCAPI_SignalHierLoggingInfo_tag { - const char_T* signalName; - uint_T numChildren; - int_T childStartIdx; -} rtwCAPI_SignalHierLoggingInfo; - -typedef struct rtwCAPI_SignalHierLoggingInfoTable_tag { - uint_T numSignals; - rtwCAPI_SignalHierLoggingInfo const* infoTable; - uint_T const* childIndexTable; -} rtwCAPI_SignalHierLoggingInfoTable; - -/* ModelMapLoggingStaticInfo */ -struct rtwCAPI_ModelMapLoggingStaticInfo_tag { - /* Total number of non-virtual systems */ - uint_T numSystems; - - /* Effective context systems array */ - int_T const * contextSystems; - - rtwCAPI_LoggingMetaInfo const *logInfo; - - /* Bus signals info */ - uint_T numBusSignals; - rtwCAPI_LoggingBusSignals const *busSignals; - - /* Dataset format hierarchy info */ - rtwCAPI_SignalHierLoggingInfoTable hierInfoTable; - - /* Dataset format Stateflow logging info */ - uint_T numChartSignals; - rtwCAPI_StateflowSignalLoggingInfo const *chartSignals; -}; - -/* Macros for accessing static fields */ -/* Note: These are all relative to the base MMI */ -#define rtwCAPI_GetHierInfoMap(MMI) \ - ((MMI)->staticMap->staticLogInfo->hierInfoTable) - -#define rtwCAPI_GetNumSystems(MMI) \ - ((MMI)->staticMap->staticLogInfo->numSystems) - -#define rtwCAPI_GetContextSystems(MMI) \ - ((MMI)->staticMap->staticLogInfo->contextSystems) - -#define rtwCAPI_GetLoggingInfoSigIdx(MMI, i) \ - ((MMI)->staticMap->staticLogInfo->logInfo[i].sigIdx) - -#define rtwCAPI_GetLoggingInfoStartIdx(MMI, i) \ - ((MMI)->staticMap->staticLogInfo->logInfo[i].startIdx) - -#define rtwCAPI_GetLoggingInfoBlockPath(MMI, i) \ - ((MMI)->staticMap->staticLogInfo->logInfo[i].blockPath) - -#define rtwCAPI_GetLoggingInfoPortNumber(MMI, i) \ - ((MMI)->staticMap->staticLogInfo->logInfo[i].portIdx) - -#define rtwCAPI_GetLoggingInfoParentSysNum(MMI, i) \ - ((MMI)->staticMap->staticLogInfo->logInfo[i].parentSysNum) - -#define rtwCAPI_GetLoggingInfoNumBusSignals(MMI) \ - ((MMI)->staticMap->staticLogInfo->numBusSignals) - -#define rtwCAPI_GetLoggingInfoBusSignalName(MMI, i) \ - ((MMI)->staticMap->staticLogInfo->busSignals[i].signalName) - -#define rtwCAPI_GetLoggingInfoBusBlockPath(MMI, i) \ - ((MMI)->staticMap->staticLogInfo->busSignals[i].blockPath) - -#define rtwCAPI_GetLoggingInfoBusPortIdx(MMI, i) \ - ((MMI)->staticMap->staticLogInfo->busSignals[i].portIdx) - -#define rtwCAPI_GetLoggingInfoBusNumElements(MMI, i) \ - ((MMI)->staticMap->staticLogInfo->busSignals[i].numElements) - -#define rtwCAPI_GetLoggingInfoBusElementIndex(MMI, i, j) \ - ((MMI)->staticMap->staticLogInfo->busSignals[i].elements[j].index) - -#define rtwCAPI_GetLoggingInfoBusElementType(MMI, i, j) \ - ((MMI)->staticMap->staticLogInfo->busSignals[i].elements[j].type) - -/* ModelMapLoggingInstanceInfo */ -struct rtwCAPI_ModelMapLoggingInstanceInfo_tag { - /* Pointer to the sysRan dwork pointers */ - sysRanDType **systemRan; - - /* Effective non-triggered tid of each system */ - int_T *systemTid; - - /* Points to the global tid map */ - int_T *globalTIDMap; -}; - -/* Note: These are all relative to the base MMI */ - -/* Get methods */ -#define rtwCAPI_GetSystemRan(MMI) \ - ((MMI)->InstanceMap.instanceLogInfo->systemRan) - -#define rtwCAPI_GetSystemTid(MMI) \ - ((MMI)->InstanceMap.instanceLogInfo->systemTid) - -#define rtwCAPI_GetGlobalTIDMap(MMI) \ - ((MMI)->InstanceMap.instanceLogInfo->globalTIDMap) - -/* Set methods */ -#define rtwCAPI_SetSystemRan(MMI,s) \ - ((MMI).InstanceMap.instanceLogInfo->systemRan) = (s) - -#define rtwCAPI_SetSystemTid(MMI,s) \ - ((MMI).InstanceMap.instanceLogInfo->systemTid) = (s) - -#define rtwCAPI_SetGlobalTIDMap(MMI,s) \ - ((MMI).InstanceMap.instanceLogInfo->globalTIDMap) = (s) - -#endif /* __RTW_MODELMAP_LOGGING__ */ - -/* EOF - rtw_modelmap_logging.h */ diff --git a/lib/PID_TV_ert_rtw/R2023b/simulink/include/sysran_types.h b/lib/PID_TV_ert_rtw/R2023b/simulink/include/sysran_types.h deleted file mode 100644 index cde1f0698..000000000 --- a/lib/PID_TV_ert_rtw/R2023b/simulink/include/sysran_types.h +++ /dev/null @@ -1,114 +0,0 @@ -/* Copyright 2004-2021 The MathWorks, Inc. */ - -/* - * File: sysran_types.h - * - * Abstract: - * This files defines the enum and the macros to do with the system ran - * breadcrumbs - * - * This file is used both by Simulink and some of the generated code - */ - -#ifndef SIMSTRUCT_SYSRAN_TYPES_H -#define SIMSTRUCT_SYSRAN_TYPES_H - -/* Subsystem run state -- used by Model Reference simulation target */ -typedef enum { - SUBSYS_RAN_BC_DISABLE, - SUBSYS_RAN_BC_ENABLE, - SUBSYS_RAN_BC_DISABLE_TO_ENABLE, - SUBSYS_RAN_BC_ENABLE_TO_DISABLE, - SUBSYS_RAN_BC_ONE_SHOT -} SubSystemRanBCTransition; - -/* This is the data type of the dwork entry for the system ran breadcrumb */ -typedef int8_T sysRanDType; - -/* - * System ran breadcrumb macros - * - * Clearing macro. 2 callers - * i) top of the sim-loop - * ii) top of each Outputs function for External mode - * - * - * Here is the basic idea: - * If a system is DISABLED, it stays DISABLED. - * For any other state, assume ENABLE_TO_DISABLE, - * If it runs then the OutputFcn will move it to ENABLE - * otherwise it will come back as ENABLE_TO_DISABLE(hence it never ran) - * - * DISABLE -> DISABLE (latched) - * ENABLE -> ENABLE_TO_DISABLE (assume) - * ENABLE_TO_DISABLE -> DISABLE (new latch) - * DISABLE_TO_ENABLE -> ENABLE (new enable) - * ONE_SHOT -> DISABLE (by definition) - * - * Note: These macros are called both by Simulink and the generated code - */ - -/* sr stands for SystemRan, BC stands for BreadCrumb */ -#define srClearBC(state) \ - { \ - SubSystemRanBCTransition prevState = (SubSystemRanBCTransition)state; \ - SubSystemRanBCTransition newState = SUBSYS_RAN_BC_DISABLE; \ - \ - switch (prevState) { \ - case SUBSYS_RAN_BC_DISABLE: \ - newState = SUBSYS_RAN_BC_DISABLE; \ - break; \ - case SUBSYS_RAN_BC_ENABLE: \ - newState = SUBSYS_RAN_BC_ENABLE_TO_DISABLE; \ - break; \ - case SUBSYS_RAN_BC_DISABLE_TO_ENABLE: \ - newState = SUBSYS_RAN_BC_ENABLE_TO_DISABLE; \ - break; \ - case SUBSYS_RAN_BC_ENABLE_TO_DISABLE: \ - newState = SUBSYS_RAN_BC_DISABLE; \ - break; \ - case SUBSYS_RAN_BC_ONE_SHOT: \ - newState = SUBSYS_RAN_BC_DISABLE; \ - break; \ - } \ - state = newState; \ - } - -/* - * Update macro - * - * Called by the OutputFcn of all conditionally exec'd subsystems - * from subsystm.cpp and commonbodlib.tlc - * - * DISABLE -> DISABLE_ENABLE (new enable) - * ENABLE_TO_DISABLE -> ENABLE (must have run last time step) - * All other state -> should really be an assert - * - */ -/* sr stands for SystemRan, BC stands for BreadCrumb */ -#define srUpdateBC(state) \ - { \ - SubSystemRanBCTransition prevState = (SubSystemRanBCTransition)state; \ - SubSystemRanBCTransition newState = prevState; \ - \ - switch (prevState) { \ - case SUBSYS_RAN_BC_DISABLE: \ - newState = SUBSYS_RAN_BC_DISABLE_TO_ENABLE; \ - break; \ - case SUBSYS_RAN_BC_ENABLE_TO_DISABLE: \ - newState = SUBSYS_RAN_BC_ENABLE; \ - break; \ - case SUBSYS_RAN_BC_ENABLE: \ - case SUBSYS_RAN_BC_DISABLE_TO_ENABLE: \ - case SUBSYS_RAN_BC_ONE_SHOT: \ - break; \ - } \ - state = newState; \ - } - -#endif /* SIMSTRUCT_SYSRAN_TYPES_H */ - -/* EOF sysran_types.h */ - -/* LocalWords: sr exec'd subsystm commonbodlib - */ diff --git a/lib/PID_TV_ert_rtw/buildInfo.mat b/lib/PID_TV_ert_rtw/buildInfo.mat deleted file mode 100644 index c92e8982a..000000000 Binary files a/lib/PID_TV_ert_rtw/buildInfo.mat and /dev/null differ diff --git a/lib/PID_TV_ert_rtw/builtin_typeid_types.h b/lib/PID_TV_ert_rtw/builtin_typeid_types.h deleted file mode 100644 index f3a9f2ade..000000000 --- a/lib/PID_TV_ert_rtw/builtin_typeid_types.h +++ /dev/null @@ -1,60 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: builtin_typeid_types.h -// -// Code generated for Simulink model 'PID_TV'. -// -// Model version : 1.14 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Mar 26 00:06:56 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// - -#ifndef BUILTIN_TYPEID_TYPES_H -#define BUILTIN_TYPEID_TYPES_H -#ifndef BUILTIN_TYPEID_TYPES -#define BUILTIN_TYPEID_TYPES - -// Enumeration of built-in data types -typedef enum { - SS_DOUBLE = 0, - SS_SINGLE = 1, - SS_INT8 = 2, - SS_UINT8 = 3, - SS_INT16 = 4, - SS_UINT16 = 5, - SS_INT32 = 6, - SS_UINT32 = 7, - SS_BOOLEAN = 8 -} BuiltInDTypeId; - -#define SS_NUM_BUILT_IN_DTYPE ((int)SS_BOOLEAN+1) - -// Enumeration for MAT-file logging code -typedef int DTypeId; - -// Enumeration of pre-defined data types -typedef enum { - SS_FCN_CALL = 9, - SS_INTEGER = 10, - SS_POINTER = 11, - SS_INTERNAL_DTYPE2 = 12, - SS_TIMER_UINT32_PAIR = 13, - SS_CONNECTION_TYPE = 14 -} PreDefinedDTypeId; - -#endif // BUILTIN_TYPEID_TYPES -#endif // BUILTIN_TYPEID_TYPES_H - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/PID_TV_ert_rtw/library.json b/lib/PID_TV_ert_rtw/library.json deleted file mode 100644 index bb9f718fa..000000000 --- a/lib/PID_TV_ert_rtw/library.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "name": "PID_TV-lib", - "version": "1.0.0", - "license": "MIT", - "frameworks": "*", - "platforms": "*", - "build": { - "flags": [ - "-IR2023b/rtw/c/src", - "-IR2023b/simulink/include" - ] - } -} \ No newline at end of file diff --git a/lib/PID_TV_ert_rtw/rtwtypes.h b/lib/PID_TV_ert_rtw/rtwtypes.h deleted file mode 100644 index a72220157..000000000 --- a/lib/PID_TV_ert_rtw/rtwtypes.h +++ /dev/null @@ -1,160 +0,0 @@ -// -// Academic License - for use in teaching, academic research, and meeting -// course requirements at degree granting institutions only. Not for -// government, commercial, or other organizational use. -// -// File: rtwtypes.h -// -// Code generated for Simulink model 'PID_TV'. -// -// Model version : 1.14 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Mar 26 00:06:56 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: Intel->x86-64 (Windows64) -// Code generation objectives: Unspecified -// Validation result: Not run -// - -#ifndef RTWTYPES_H -#define RTWTYPES_H - -// Logical type definitions -#if (!defined(__cplusplus)) -#ifndef false -#define false (0U) -#endif - -#ifndef true -#define true (1U) -#endif -#endif - -//=======================================================================* -// Target hardware information -// Device type: Intel->x86-64 (Windows64) -// Number of bits: char: 8 short: 16 int: 32 -// long: 32 -// native word size: 64 -// Byte ordering: LittleEndian -// Signed integer division rounds to: Zero -// Shift right on a signed integer as arithmetic shift: on -// ======================================================================= - -//=======================================================================* -// Fixed width word size data types: * -// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * -// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * -// real32_T, real64_T - 32 and 64 bit floating point numbers * -// ======================================================================= -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -//===========================================================================* -// Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * -// real_T, time_T, ulong_T. * -// =========================================================================== -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef unsigned char uchar_T; -typedef char_T byte_T; - -//===========================================================================* -// Complex number type definitions * -// =========================================================================== -#define CREAL_T - -typedef struct { - real32_T re; - real32_T im; -} creal32_T; - -typedef struct { - real64_T re; - real64_T im; -} creal64_T; - -typedef struct { - real_T re; - real_T im; -} creal_T; - -#define CINT8_T - -typedef struct { - int8_T re; - int8_T im; -} cint8_T; - -#define CUINT8_T - -typedef struct { - uint8_T re; - uint8_T im; -} cuint8_T; - -#define CINT16_T - -typedef struct { - int16_T re; - int16_T im; -} cint16_T; - -#define CUINT16_T - -typedef struct { - uint16_T re; - uint16_T im; -} cuint16_T; - -#define CINT32_T - -typedef struct { - int32_T re; - int32_T im; -} cint32_T; - -#define CUINT32_T - -typedef struct { - uint32_T re; - uint32_T im; -} cuint32_T; - -//=======================================================================* -// Min and Max: * -// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * -// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * -// ======================================================================= -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255U)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535U)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) - -// Block D-Work pointer type -typedef void * pointer_T; - -#endif // RTWTYPES_H - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index f52937ff5..92bdd33f5 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -12,6 +12,8 @@ #include "AMSInterface.h" #include "SABInterface.h" #include "VectornavInterface.h" +#include "HT08_CASE_types.h" +#include "MessageQueueDefine.h" /* struct holding interfaces processed by process_ring_buffer() FL = MC1 @@ -47,14 +49,11 @@ extern Circular_Buffer CAN3_rxBuffer; /* TX buffer for CAN1 */ -extern Circular_Buffer - CAN1_txBuffer; +extern CANBufferType CAN1_txBuffer; /* TX buffer for CAN2 */ -extern Circular_Buffer - CAN2_txBuffer; +extern CANBufferType CAN2_txBuffer; /* TX buffer for CAN3 */ -extern Circular_Buffer - CAN3_txBuffer; +extern CANBufferType CAN3_txBuffer; /* Receive callback function for CAN1 that pushes to circ. buffer */ void on_can1_receive(const CAN_message_t &msg); @@ -172,7 +171,7 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, interfaces.vn_interface->retrieve_vn_status_CAN(recvd_msg); // double check this break; case VN_ANGULAR_RATE_CANID: - interfaces.vn_interface->retrieve_lat_lon_CAN(recvd_msg); + interfaces.vn_interface->receive_ang_rates_CAN(recvd_msg); break; } } @@ -196,4 +195,28 @@ void send_all_CAN_msgs(bufferType &buffer, FlexCAN_T4_Base *can_interface) } } +// TODO should this be here? my reasoning right now for this being here is that right now this interface is +// only being used on MCU and is also tied integrally into other interfaces however the existence of this +// function necessitates the CAN_MESSAGE_BUS type from generated simulink code which in theory we want to keep +// segregated to the CASESystem. what we probably want our own CAN message type that we can use in the shared +// data lib that is seperate from both. idk, not worth it for now just leaving this brain dump here. + +/// @brief message enque function for matlab generated CAN_MESSAGE_BUS type +/// @tparam bufferType the message buffer template type onto which you are enquing a msg +/// @param msg_queue the pointer to the msg queue that will contain the generic message +/// @param structure the matlab generated CAN message type that will be sent +template +void enqueue_matlab_msg(bufferType *msg_queue, const CAN_MESSAGE_BUS & structure) +{ + CAN_message_t can_msg = {}; + can_msg.id = structure.ID; + can_msg.len = structure.Length; + // TODO ensure memory safety of this, but this should be fine + memmove( can_msg.buf, structure.Data, structure.Length ); + uint8_t buf[sizeof(CAN_message_t)] = {}; + memmove(buf, &can_msg, sizeof(CAN_message_t)); + msg_queue->push_back(buf, sizeof(CAN_message_t)); +} + + #endif /* HYTECHCANINTERFACE */ diff --git a/lib/interfaces/include/LoadCellInterface.h b/lib/interfaces/include/LoadCellInterface.h new file mode 100644 index 000000000..adf85befb --- /dev/null +++ b/lib/interfaces/include/LoadCellInterface.h @@ -0,0 +1,64 @@ +#ifndef __LOADCELLINTERFACE_H__ +#define __LOADCELLINTERFACE_H__ + +#include "Utility.h" +#include "SysClock.h" +#include "AnalogSensorsInterface.h" + +/* Structs */ + +struct LoadCellInterfaceTick_s +{ + const AnalogConversion_s &FLConversion; + const AnalogConversion_s &FRConversion; + const AnalogConversion_s &RLConversion; + const AnalogConversion_s &RRConversion; +}; + +struct LoadCellInterfaceOutput_s +{ + veh_vec loadCellForcesFiltered; + veh_vec loadCellConversions; + bool FIRSaturated; +}; + +class LoadCellInterface +{ +private: + /* + FIR filter designed with + http://t-filter.appspot.com + + sampling frequency: 100 Hz + + * 0 Hz - 10 Hz + gain = 1 + desired ripple = 5 dB + actual ripple = 1.7659949026015025 dB + + * 40 Hz - 50 Hz + gain = 0 + desired attenuation = -40 dB + actual attenuation = -47.34009380570117 dB + */ + const static int numFIRTaps_ = 5; + float FIRTaps_[numFIRTaps_] = + { + 0.07022690881526232, + 0.27638313122745306, + 0.408090001549378, + 0.27638313122745306, + 0.07022690881526232 + }; + int FIRCircBufferHead = 0; // index of the latest sample in the raw buffer + veh_vec loadCellConversions_; + veh_vec loadCellForcesUnfiltered_; + veh_vec loadCellForcesFiltered_; + bool FIRSaturated_ = false; +public: + LoadCellInterface() {} + void tick(const LoadCellInterfaceTick_s &intake); + LoadCellInterfaceOutput_s getLoadCellForces(); +}; + +#endif \ No newline at end of file diff --git a/lib/interfaces/include/MessageQueueDefine.h b/lib/interfaces/include/MessageQueueDefine.h index d6de69e82..79cb7f097 100644 --- a/lib/interfaces/include/MessageQueueDefine.h +++ b/lib/interfaces/include/MessageQueueDefine.h @@ -3,8 +3,7 @@ #include "FlexCAN_T4.h" -constexpr std::size_t CANBufferSize = 32; // does this need to be 3 params like our original definitions of the buffers? -using CANBufferType = Circular_Buffer; +using CANBufferType = Circular_Buffer; #endif \ No newline at end of file diff --git a/lib/interfaces/include/VectornavInterface.h b/lib/interfaces/include/VectornavInterface.h index 68b0011c0..edf525cd9 100644 --- a/lib/interfaces/include/VectornavInterface.h +++ b/lib/interfaces/include/VectornavInterface.h @@ -19,6 +19,7 @@ struct vector_nav { double ecef_coords[3]; // x,y,z uint64_t gps_time; // gps time uint8_t vn_status; // status + xyz_vec angular_rates; }; template @@ -39,6 +40,7 @@ class VNInterface // retrieve methods void retrieve_velocity_CAN(CAN_message_t &recvd_msg); + void retrieve_linear_accel_CAN(CAN_message_t &recvd_msg); void retrieve_uncompLinear_accel_CAN(CAN_message_t &recvd_msg); void retrieve_ypr_CAN(CAN_message_t &recvd_msg); @@ -47,6 +49,7 @@ class VNInterface void retrieve_vn_status_CAN(CAN_message_t &recvd_msg); void retrieve_vn_ecef_pos_xy_CAN(CAN_message_t &recvd_msg); void retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg); + void receive_ang_rates_CAN(CAN_message_t &recvd_msg); // getters vector_nav get_vn_struct(); diff --git a/lib/interfaces/include/VectornavInterface.tpp b/lib/interfaces/include/VectornavInterface.tpp index ca14b02d3..be07a4e08 100644 --- a/lib/interfaces/include/VectornavInterface.tpp +++ b/lib/interfaces/include/VectornavInterface.tpp @@ -24,6 +24,8 @@ void VNInterface::retrieve_linear_accel_CAN(CAN_message_t &recvd_ template void VNInterface::retrieve_uncompLinear_accel_CAN(CAN_message_t &recvd_msg) { VN_LINEAR_ACCEL_UNCOMP_t linear_accel_uncomp_data; + Unpack_VN_LINEAR_ACCEL_UNCOMP_hytech(&linear_accel_uncomp_data, recvd_msg.buf, recvd_msg.len); + vn_data.uncompLinear_accel[0] = HYTECH_vn_lin_uncomp_accel_x_ro_fromS(linear_accel_uncomp_data.vn_lin_uncomp_accel_x_ro); vn_data.uncompLinear_accel[1] = HYTECH_vn_lin_uncomp_accel_y_ro_fromS(linear_accel_uncomp_data.vn_lin_uncomp_accel_y_ro); vn_data.uncompLinear_accel[2] = HYTECH_vn_lin_uncomp_accel_z_ro_fromS(linear_accel_uncomp_data.vn_lin_uncomp_accel_z_ro); @@ -75,6 +77,15 @@ void VNInterface::retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd vn_data.ecef_coords[2] = HYTECH_vn_ecef_pos_z_ro_fromS(ecef_z.vn_ecef_pos_z_ro); } +template +void VNInterface::receive_ang_rates_CAN(CAN_message_t & recvd_msg) +{ + VN_ANGULAR_RATE_t ang_rates; + Unpack_VN_ANGULAR_RATE_hytech(&ang_rates, recvd_msg.buf, recvd_msg.len); + vn_data.angular_rates.x = HYTECH_angular_rate_x_ro_fromS(ang_rates.angular_rate_x_ro); + vn_data.angular_rates.y = HYTECH_angular_rate_y_ro_fromS(ang_rates.angular_rate_y_ro); + vn_data.angular_rates.z = HYTECH_angular_rate_z_ro_fromS(ang_rates.angular_rate_z_ro); +} /** * @brief * getter method for returning vn_data structure diff --git a/lib/interfaces/library.json b/lib/interfaces/library.json index 45121f2b2..202cee41a 100644 --- a/lib/interfaces/library.json +++ b/lib/interfaces/library.json @@ -4,7 +4,8 @@ "license": "MIT", "dependencies": { "can_lib": "*", - "shared_data": "*" + "shared_data": "*", + "CASE_lib": "*" }, "frameworks": "*", "platforms": "*" diff --git a/lib/interfaces/src/HytechCANInterface.cpp b/lib/interfaces/src/HytechCANInterface.cpp index ad732b049..a0c8dea46 100644 --- a/lib/interfaces/src/HytechCANInterface.cpp +++ b/lib/interfaces/src/HytechCANInterface.cpp @@ -4,9 +4,9 @@ Circular_Buffer CAN1_rxBuffer; Circular_Buffer CAN2_rxBuffer; Circular_Buffer CAN3_rxBuffer; -Circular_Buffer CAN1_txBuffer; -Circular_Buffer CAN2_txBuffer; -Circular_Buffer CAN3_txBuffer; +CANBufferType CAN1_txBuffer; +CANBufferType CAN2_txBuffer; +CANBufferType CAN3_txBuffer; void on_can1_receive(const CAN_message_t &msg) diff --git a/lib/interfaces/src/LoadCellInterface.cpp b/lib/interfaces/src/LoadCellInterface.cpp new file mode 100644 index 000000000..b6aa570f3 --- /dev/null +++ b/lib/interfaces/src/LoadCellInterface.cpp @@ -0,0 +1,42 @@ +#include "LoadCellInterface.h" + +void LoadCellInterface::tick(const LoadCellInterfaceTick_s &intake) +{ + // filter runs at 100hz + loadCellConversions_.FL = intake.FLConversion; + loadCellConversions_.FR = intake.FRConversion; + loadCellConversions_.RL = intake.RLConversion; + loadCellConversions_.RR = intake.RRConversion; + + loadCellForcesUnfiltered_.FL[FIRCircBufferHead] = intake.FLConversion.conversion; + loadCellForcesUnfiltered_.FR[FIRCircBufferHead] = intake.FRConversion.conversion; + loadCellForcesUnfiltered_.RL[FIRCircBufferHead] = intake.RLConversion.conversion; + loadCellForcesUnfiltered_.RR[FIRCircBufferHead] = intake.RRConversion.conversion; + + loadCellForcesFiltered_.FL = 0.0f; + loadCellForcesFiltered_.FR = 0.0f; + loadCellForcesFiltered_.RL = 0.0f; + loadCellForcesFiltered_.RR = 0.0f; + + for (int FIROffset = 0; FIROffset < numFIRTaps_; FIROffset++) + { + int index = (FIRCircBufferHead + FIROffset) % numFIRTaps_; + loadCellForcesFiltered_.FL += loadCellForcesUnfiltered_.FL[index] * FIRTaps_[FIROffset]; + loadCellForcesFiltered_.FR += loadCellForcesUnfiltered_.FR[index] * FIRTaps_[FIROffset]; + loadCellForcesFiltered_.RL += loadCellForcesUnfiltered_.RL[index] * FIRTaps_[FIROffset]; + loadCellForcesFiltered_.RR += loadCellForcesUnfiltered_.RR[index] * FIRTaps_[FIROffset]; + } + + FIRCircBufferHead = (FIRCircBufferHead + 1) % numFIRTaps_; + if (FIRCircBufferHead == numFIRTaps_ - 1) + FIRSaturated_ = true; +} + +LoadCellInterfaceOutput_s LoadCellInterface::getLoadCellForces() +{ + return (LoadCellInterfaceOutput_s) { + .loadCellForcesFiltered = loadCellForcesFiltered_, + .loadCellConversions = loadCellConversions_, + .FIRSaturated = FIRSaturated_ + }; +} \ No newline at end of file diff --git a/lib/library.json b/lib/library.json new file mode 100644 index 000000000..61921be1f --- /dev/null +++ b/lib/library.json @@ -0,0 +1,52 @@ +{ + "name": "CASE_lib", + "version": "1.0.0", + "build": { + "flags": [ + "-Istate_machine", + "-Istate_machine/include", + "-Iinterfaces", + "-Iinterfaces/include", + "-Iinterfaces/src", + "-Imock_interfaces", + "-Isystems", + "-Isystems/src", + "-Isystems/include", + "-Ishared_data", + "-Ishared_data/include", + "-ICASE_lib", + "-ICASE_lib/R2023b", + "-ICASE_lib/R2023b/simulink", + "-ICASE_lib/R2023b/simulink/include", + "-ICASE_lib/R2023b/rtw", + "-ICASE_lib/R2023b/rtw/c", + "-ICASE_lib/R2023b/rtw/c/src", + "-ICASE_lib/CASE", + "-ICASE_lib/CASE/HT08_CONTROL_SYSTEM_ert_rtw", + "-ICASE_lib/CASE/slprj", + "-ICASE_lib/CASE/slprj/ert", + "-ICASE_lib/CASE/slprj/ert/BasicVehicleMath", + "-ICASE_lib/CASE/slprj/ert/LAUNCH_CONTROL", + "-ICASE_lib/CASE/slprj/ert/NORMAL_FORCE_TV", + "-ICASE_lib/CASE/slprj/ert/PID_TV", + "-ICASE_lib/CASE/slprj/ert/POWER_LIMIT", + "-ICASE_lib/CASE/slprj/ert/_sharedutils", + "-ILateral Models", + "-ILateral Models/HT08_CONTROL_SYSTEM_ert_rtw", + "-ILateral Models/slprj", + "-ILateral Models/slprj/ert", + "-ILateral Models/slprj/ert/BasicVehicleMath", + "-ILateral Models/slprj/ert/LAUNCH_CONTROL", + "-ILateral Models/slprj/ert/NORMAL_FORCE_TV", + "-ILateral Models/slprj/ert/PID_TV", + "-ILateral Models/slprj/ert/POWER_LIMIT", + "-ILateral Models/slprj/ert/_sharedutils", + "-IR2023b", + "-IR2023b/simulink", + "-IR2023b/simulink/include", + "-IR2023b/rtw", + "-IR2023b/rtw/c", + "-IR2023b/rtw/c/src" + ] + } +} \ No newline at end of file diff --git a/lib/mock_interfaces/HytechCANInterface.h b/lib/mock_interfaces/HytechCANInterface.h new file mode 100644 index 000000000..e69de29bb diff --git a/lib/mock_interfaces/LoadCellInterface.h b/lib/mock_interfaces/LoadCellInterface.h new file mode 100644 index 000000000..d3588aa11 --- /dev/null +++ b/lib/mock_interfaces/LoadCellInterface.h @@ -0,0 +1,16 @@ +#ifndef __LOADCELLINTERFACE_H__ +#define __LOADCELLINTERFACE_H__ + +#include "Utility.h" +#include "AnalogSensorsInterface.h" + +/* Structs */ + +struct LoadCellInterfaceOutput_s +{ + veh_vec loadCellForcesFiltered; + veh_vec loadCellConversions; + bool FIRSaturated; +}; + +#endif \ No newline at end of file diff --git a/lib/mock_interfaces/VectornavInterface.h b/lib/mock_interfaces/VectornavInterface.h index b0d018c06..05c486d6e 100644 --- a/lib/mock_interfaces/VectornavInterface.h +++ b/lib/mock_interfaces/VectornavInterface.h @@ -17,6 +17,7 @@ struct vector_nav { double ecef_coords[3]; // x,y,z uint64_t gps_time; // gps time uint8_t vn_status; // status + xyz_vec angular_rates; }; template diff --git a/lib/shared_data/library.json b/lib/shared_data/library.json new file mode 100644 index 000000000..d13ad88bc --- /dev/null +++ b/lib/shared_data/library.json @@ -0,0 +1,7 @@ +{ + "name": "shared-data-lib", + "version": "1.0.0", + "license": "MIT", + "frameworks": "*", + "platforms": "*" + } \ No newline at end of file diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h new file mode 100644 index 000000000..8cf556e1a --- /dev/null +++ b/lib/systems/include/CASESystem.h @@ -0,0 +1,150 @@ +#ifndef CASESYSTEM +#define CASESYSTEM + +#include "HT08_CASE.h" +#include "HytechCANInterface.h" +#include "PedalsSystem.h" +#include "DrivetrainSystem.h" +#include "SteeringSystem.h" +#include "MCUStateMachine.h" + +struct CASEConfiguration +{ + float AbsoluteTorqueLimit; + float yaw_pid_p; + float yaw_pid_i; + float yaw_pid_d; + float tcs_pid_p; + float tcs_pid_i; + float tcs_pid_d; + bool useLaunch; + bool usePIDTV; + bool useTCSLimitedYawPID; + bool useNormalForce; + bool useTractionControl; + bool usePowerLimit; + bool usePIDPowerLimit; + bool useDecoupledYawBrakes; + bool useDiscontinuousYawPIDBrakes; + float tcsSLThreshold; + float launchSL; + float launchDeadZone; + float launchVelThreshold; + float tcsVelThreshold; + float yawPIDMaxDifferential; + float yawPIDErrorThreshold; + float yawPIDVelThreshold; + float yawPIDCoastThreshold; + float yaw_pid_brakes_p; + float yaw_pid_brakes_i; + float yaw_pid_brakes_d; + float decoupledYawPIDBrakesMaxDIfference; + float discontinuousBrakesPercentThreshold; + float TorqueMode; + float RegenLimit; + bool useNoRegen5kph; + bool useTorqueBias; + float DriveTorquePercentFront; + float BrakeTorquePercentFront; + float MechPowerMaxkW; + + float max_rpm; + float max_regen_torque; + float max_torque; +}; + +/// @brief this class with both take in sensor inputs as well as handle calculations for various derived states of the car. +// this class will also handle output onto the CAN bus of data +/// @tparam message_queue the msg queue that is being used with the underlying msging interfaces +template +class CASESystem +{ +public: + /// @brief constructor for state estimator system. + /// @param can_queue the pointer to the message queue that will have the CAN messages put onto it + /// @param send_period_ms the period in which messages will be put into the queue to be sent in milliseconds. + /// @param vehicle_math_offset_ms the offset in ms from controller message sending that the vehicle math messages will be sent + CASESystem( + message_queue *can_queue, + unsigned long controller_send_period_ms, + unsigned long vehicle_math_offset_ms, + CASEConfiguration config) + { + msg_queue_ = can_queue; + case_.initialize(); + vn_active_start_time_ = 0; + config_ = config; + last_controller_pt1_send_time_ = 0; + last_controller_pt2_send_time_ = 0; + + controller_send_period_ms_ = controller_send_period_ms; + last_vehm_send_time_ = 0; + vehicle_math_offset_ms_ = vehicle_math_offset_ms; + } + + /// @brief function that evaluates the CASE (controller and state estimation) system. updates the internal pstate_ and returns controller result + /// @param tick current system tick + /// @param body_velocity_ms body velocity vector + /// @param yaw_rate_rads yaw rate in rad / s + /// @param steering_norm steering value between -1 and 1 ish + /// @param wheel_rpms wheel rpms + /// @param pedals_data current pedals data + /// @param load_cell_vals load cell forces in N + /// @param power_kw current electrical power in kilo-watts + /// @param reset_integral bool of whether or not to reset integral term + /// @return controller output + DrivetrainCommand_s evaluate( + const SysTick_s &tick, + const vector_nav &vn_data, + const SteeringSystemData_s &steering_data, + const DrivetrainDynamicReport_s &drivetrain_data, + const veh_vec &load_cell_vals, + const PedalsSystemData_s &pedals_data, + float power_kw, + CAR_STATE fsm_state, + bool start_button_pressed, + uint8_t vn_status); + + void update_pid(float yaw_p, float yaw_i, float yaw_d, float tcs_p, float tcs_i, float tcs_d, float brake_p, float brake_i, float brake_d) + { + config_.yaw_pid_p = yaw_p; + config_.yaw_pid_p = yaw_i; + config_.yaw_pid_p = yaw_d; + + config_.tcs_pid_p = tcs_p; + config_.tcs_pid_i = tcs_i; + config_.tcs_pid_d = tcs_d; + + config_.yaw_pid_brakes_p = brake_p; + config_.yaw_pid_brakes_i = brake_i; + config_.yaw_pid_brakes_d = brake_d; + } + float calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_regen_torque, float max_rpm); + /// @brief configuration function to determine what CASE is using / turn on and off different features within CASE + /// @param config the configuration struct we will be setting + void configure(const CASEConfiguration &config) + { + config_ = config; + } + float get_rpm_setpoint(float final_torque) + { + if (final_torque > 0) + { + return config_.max_rpm; + } + else + { + return 0; + } + } + +private: + CASEConfiguration config_; + message_queue *msg_queue_; + HT08_CASE case_; + + unsigned long vn_active_start_time_, last_eval_time_, vehicle_math_offset_ms_, last_controller_pt1_send_time_, last_controller_pt2_send_time_, last_controller_pt3_send_time_, last_vehm_send_time_, controller_send_period_ms_; +}; + +#include "CASESystem.tpp" +#endif \ No newline at end of file diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp new file mode 100644 index 000000000..314cf1304 --- /dev/null +++ b/lib/systems/include/CASESystem.tpp @@ -0,0 +1,226 @@ +#include "CASESystem.h" + +template +DrivetrainCommand_s CASESystem::evaluate( + const SysTick_s &tick, + const vector_nav &vn_data, + const SteeringSystemData_s &steering_data, + const DrivetrainDynamicReport_s &drivetrain_data, + const veh_vec &load_cell_vals, + const PedalsSystemData_s &pedals_data, + float power_kw, + CAR_STATE fsm_state, + bool start_button_pressed, + uint8_t vn_status) +{ + HT08_CASE::ExtU_HT08_CASE_T in; + + // in. as defined in HT08_CASE.h + in.SteeringWheelAngleDeg = steering_data.angle; + + in.TorqueAverageNm = calculate_torque_request(pedals_data, config_.max_regen_torque, config_.max_rpm); + + in.YawRaterads = vn_data.angular_rates.z; + + // REAL + in.Vx_B = vn_data.velocity_x; + + // FAKE + // in.Vx_B = 5; + + in.FZFL = load_cell_vals.FL.conversion; + in.FZFR = load_cell_vals.FR.conversion; + in.FZRL = load_cell_vals.RL.conversion; + in.FZRR = load_cell_vals.RR.conversion; + + in.CurrentElectricalPowerkW = power_kw; + + // REAL + // in.MotorOmegaFLrpm = drivetrain_data.measuredSpeeds[0]; + // in.MotorOmegaFRrpm = drivetrain_data.measuredSpeeds[1]; + // in.MotorOmegaRLrpm = drivetrain_data.measuredSpeeds[2]; + // in.MotorOmegaRRrpm = drivetrain_data.measuredSpeeds[3]; + + // FAKE, 566.273 rpm = 1 m/s + // in.MotorOmegaFLrpm = 566.27330024 * 1.36; + // in.MotorOmegaFRrpm = 566.27330024 * 1.36; + // in.MotorOmegaRRrpm = 566.27330024 * 1.36; + // in.MotorOmegaRLrpm = 566.27330024 * 1.36; + + // FAKE max rpm + in.MotorOmegaFLrpm = 20000; + in.MotorOmegaFRrpm = 20000; + in.MotorOmegaRRrpm = 20000; + in.MotorOmegaRLrpm = 20000; + + in.usePIDTV = config_.usePIDTV; + in.useNormalForce = config_.useNormalForce; + in.usePowerLimit = config_.usePowerLimit; + in.usePIDPowerLimit = config_.usePIDPowerLimit; + in.useLaunch = config_.useLaunch; + + in.Vy_B = vn_data.velocity_y; + + in.YawPIDConfig[0] = config_.yaw_pid_p; + in.YawPIDConfig[1] = config_.yaw_pid_i; + in.YawPIDConfig[2] = config_.yaw_pid_d; + + in.AbsoluteTorqueLimit = config_.AbsoluteTorqueLimit; + + in.useTractionControl = config_.useTractionControl; + + in.TCS_SLThreshold = config_.tcsSLThreshold; + in.LaunchSL = config_.launchSL; + in.LaunchDeadZone = config_.launchDeadZone; + + in.TCSPIDConfig[0] = config_.tcs_pid_p; + in.TCSPIDConfig[1] = config_.tcs_pid_i; + in.TCSPIDConfig[2] = config_.tcs_pid_d; + + in.LaunchVelThreshold = config_.launchVelThreshold; + in.TCSVelThreshold = config_.tcsVelThreshold; + + in.YawPIDErrorThreshold = config_.yawPIDErrorThreshold; + in.YawPIDVelThreshold = config_.yawPIDVelThreshold; + in.YawPIDCoastThreshold = config_.yawPIDCoastThreshold; + + in.useTCSLimitedYawPID = config_.useTCSLimitedYawPID; + + in.YawPIDMaxDifferential = config_.yawPIDMaxDifferential; + + in.useDecoupledYawBrakes = config_.useDecoupledYawBrakes; + + in.YawPIDBrakesConfig[0] = config_.yaw_pid_brakes_p; + in.YawPIDBrakesConfig[1] = config_.yaw_pid_brakes_i; + in.YawPIDBrakesConfig[2] = config_.yaw_pid_brakes_d; + + in.decoupledYawPIDBrakesMaxDIffere = config_.decoupledYawPIDBrakesMaxDIfference; + + in.useDiscontinuousYawPIDBrakes = config_.useDiscontinuousYawPIDBrakes; + + in.discontinuousBrakesPercentThres = config_.discontinuousBrakesPercentThreshold; + + in.TorqueMode = config_.TorqueMode; + + in.RegenLimit = config_.RegenLimit; + + in.useNoRegen5kph = config_.useNoRegen5kph; + + in.useTorqueBias = config_.useTorqueBias; + + in.DriveTorquePercentFront = config_.DriveTorquePercentFront; + + in.BrakeTorquePercentFront = config_.BrakeTorquePercentFront; + + in.MechPowerMaxkW = config_.MechPowerMaxkW; + + if ((vn_active_start_time_ == 0) && (vn_status >= 2)) + { + vn_active_start_time_ = tick.millis; + } + else if (vn_status < 2) + { + vn_active_start_time_ = 0; + } + + case_.setExternalInputs(&in); + if ((tick.millis - last_eval_time_) >= 1) + { + if ((fsm_state == CAR_STATE::READY_TO_DRIVE) && (start_button_pressed)) + { + in.YawPIDConfig[1] = 0.0f; + } + case_.step(); + last_eval_time_ = tick.millis; + } + + auto res = case_.getExternalOutputs(); + + // send these out at the send period + + if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_)) + { + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p); + + last_controller_pt1_send_time_ = tick.millis; + } + + if (((tick.millis - last_controller_pt1_send_time_) >= (vehicle_math_offset_ms_ / 3)) && + ((tick.millis - last_controller_pt2_send_time_) > controller_send_period_ms_)) + { + + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pi); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st); + + last_controller_pt2_send_time_ = tick.millis; + } + + if (((tick.millis - last_controller_pt2_send_time_) >= (vehicle_math_offset_ms_ / 3)) && + ((tick.millis - last_controller_pt3_send_time_) > controller_send_period_ms_)) + { + + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_rege_p); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_torque); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_powe_p); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn); + + last_controller_pt3_send_time_ = tick.millis; + } + + if (((tick.millis - last_controller_pt1_send_time_) >= vehicle_math_offset_ms_) && + ((tick.millis - last_vehm_send_time_) > controller_send_period_ms_)) + { + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_alpha_deg); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_sl); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_long_corner_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_steer_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_kin_desired_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_beta_deg); + enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_lin_ve); + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co); + last_vehm_send_time_ = tick.millis; + } + + DrivetrainCommand_s command; + + command.torqueSetpoints[0] = res.FinalTorqueFL; + command.torqueSetpoints[1] = res.FinalTorqueFR; + command.torqueSetpoints[2] = res.FinalTorqueRL; + command.torqueSetpoints[3] = res.FinalTorqueRR; + + command.speeds_rpm[0] = get_rpm_setpoint(res.FinalTorqueFL); + command.speeds_rpm[1] = get_rpm_setpoint(res.FinalTorqueFR); + command.speeds_rpm[2] = get_rpm_setpoint(res.FinalTorqueRL); + command.speeds_rpm[3] = get_rpm_setpoint(res.FinalTorqueRR); + + return command; +} + +template +float CASESystem::calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_regen_torque, float max_rpm) +{ + // accelRequest goes between 1.0 and -1.0 + float accelRequest = pedals_data.accelPercent - pedals_data.regenPercent; + float torqueRequest; + + if (accelRequest >= 0.0) + { + // Positive torque request + torqueRequest = accelRequest * AMK_MAX_TORQUE; + } + else + { + // Negative torque request + torqueRequest = max_regen_torque * accelRequest; + } + return torqueRequest; +} \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index fbd6adfec..7a2c05640 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -45,6 +45,7 @@ class DrivetrainSystem last_disable_cmd_time_ = 0; last_general_cmd_time_ = 0; // ms mcu_interface_ = mcu_interface; + dynamic_data_ = {}; } void tick(const SysTick_s &tick); @@ -72,7 +73,7 @@ class DrivetrainSystem void enable_drivetrain_reset(); void check_reset_condition(); - DrivetrainDynamicReport_s get_current_data(); + DrivetrainDynamicReport_s get_dynamic_data(); private: std::array inverters_; @@ -105,6 +106,7 @@ class DrivetrainSystem unsigned long drivetrain_initialization_phase_start_time_; DrivetrainCommand_s current_drivetrain_command_; + DrivetrainDynamicReport_s dynamic_data_; }; #include "DrivetrainSystem.tpp" diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index 3395dfaa8..de01cc702 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -225,23 +225,23 @@ bool DrivetrainSystem::drivetrain_enabled_() } template -DrivetrainDynamicReport_s DrivetrainSystem::get_current_data() +DrivetrainDynamicReport_s DrivetrainSystem::get_dynamic_data() { - DrivetrainDynamicReport_s data; + // TODO idk - data.measuredInverterFLPackVoltage = inverters_[0]->dc_bus_voltage(); + dynamic_data_.measuredInverterFLPackVoltage = inverters_[0]->dc_bus_voltage(); int inverter_ind = 0; for (auto inv_pointer : inverters_) { auto iq = inv_pointer->get_torque_current(); // iq in A auto id = inv_pointer->get_mag_current(); // id in A - data.measuredSpeeds[inverter_ind] = inv_pointer->get_speed(); - data.measuredTorqueCurrents[inverter_ind] = iq; - data.measuredMagnetizingCurrents[inverter_ind] = id; + dynamic_data_.measuredSpeeds[inverter_ind] = inv_pointer->get_speed(); + dynamic_data_.measuredTorqueCurrents[inverter_ind] = iq; + dynamic_data_.measuredMagnetizingCurrents[inverter_ind] = id; // TODO // data.measuredTorques[inverter_ind] = inv_pointer->get_actual_torque(); inverter_ind++; } - return data; + return dynamic_data_; } \ No newline at end of file diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 579b2e159..afd664819 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -9,8 +9,8 @@ // Definitions // TODO: evalaute reasonable thresholds for agreement -#define STEERING_DIVERGENCE_ERROR_THRESHOLD (5.0) // Steering sensors can disagree by 5 degrees before output is considered erroneous -#define STEERING_DIVERGENCE_WARN_THRESHOLD (2.5) // Warning condition will be raised when steering sensors diverge 2.5 degrees +#define STEERING_DIVERGENCE_ERROR_THRESHOLD (14.0) // Steering sensors can disagree by 5 degrees before output is considered erroneous +#define STEERING_DIVERGENCE_WARN_THRESHOLD (8.0) // Warning condition will be raised when steering sensors diverge 2.5 degrees // Enums enum class SteeringSystemStatus_e @@ -22,6 +22,12 @@ enum class SteeringSystemStatus_e }; // Structs +struct SteeringSystemTick_s +{ + const SysTick_s &tick; + const AnalogConversion_s &secondaryConversion; +}; + struct SteeringSystemData_s { float angle; @@ -33,23 +39,22 @@ class SteeringSystem private: SteeringEncoderInterface *primarySensor_; SteeringEncoderConversion_s primaryConversion_; - SteeringSystemData_s data_; + SteeringSystemData_s steeringData_; public: - SteeringSystem(SteeringEncoderInterface *primarySensor) : primarySensor_(primarySensor) {} + SteeringSystem(SteeringEncoderInterface *primarySensor) + : primarySensor_(primarySensor) + {} /// @brief Computes steering angle and status of the steering system. /// @param secondaryAngle The computed steering angle as reported by the secondary steering sensor. /// @return SteeringSystemOutput_s contains steering angle and SteeringSystemStatus_e - void tick( - const SysTick_s &tick, - const AnalogConversion_s &secondaryConversion - ); + void tick(const SteeringSystemTick_s &intake); /// @brief Get a reference to the steering system's data /// @return const SteeringSystemData_s& const SteeringSystemData_s& getSteeringSystemData() { - return data_; + return steeringData_; } }; diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 1ae3a0542..cd8dc0501 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -10,7 +10,7 @@ #include "SteeringSystem.h" #include "DashboardInterface.h" #include "VectornavInterface.h" -// #include "AnalogSensorsInterface.h" +#include "LoadCellInterface.h" const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm @@ -19,11 +19,18 @@ const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm class TorqueControllerMux { private: + TorqueControllerNone torqueControllerNone_; + TorqueControllerSimple torqueControllerSimple_; + TorqueControllerLoadCellVectoring torqueControllerLoadCellVectoring_; + TorqueControllerSimpleLaunch torqueControllerSimpleLaunch_; + TorqueControllerSlipLaunch torqueControllerSlipLaunch_; + TorqueControllerCASEWrapper tcCASEWrapper_; + // Use this to map the dial to TCMUX modes std::unordered_map dialModeMap_ = { {DialMode_e::MODE_0, TorqueController_e::TC_SAFE_MODE}, {DialMode_e::MODE_1, TorqueController_e::TC_LOAD_CELL_VECTORING}, - {DialMode_e::MODE_2, TorqueController_e::TC_NO_CONTROLLER}, + {DialMode_e::MODE_2, TorqueController_e::TC_CASE_SYSTEM}, {DialMode_e::MODE_3, TorqueController_e::TC_SIMPLE_LAUNCH}, {DialMode_e::MODE_4, TorqueController_e::TC_SLIP_LAUNCH}, {DialMode_e::MODE_5, TorqueController_e::TC_NO_CONTROLLER}, @@ -39,19 +46,14 @@ class TorqueControllerMux TorqueControllerOutput_s controllerOutputs_[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)]; - TorqueControllerNone torqueControllerNone_; - TorqueControllerSimple torqueControllerSimple_; - TorqueControllerLoadCellVectoring torqueControllerLoadCellVectoring_; - TorqueControllerSimpleLaunch torqueControllerSimpleLaunch_; - TorqueControllerSlipLaunch torqueControllerSlipLaunch_; - TorqueControllerPIDTV torqueControllerPIDTV_; + TorqueControllerBase* controllers[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)] = { static_cast(&torqueControllerNone_), static_cast(&torqueControllerSimple_), static_cast(&torqueControllerLoadCellVectoring_), static_cast(&torqueControllerSimpleLaunch_), static_cast(&torqueControllerSlipLaunch_), - // static_cast(&torqueControllerPIDTV_), + static_cast(&tcCASEWrapper_) }; DrivetrainCommand_s drivetrainCommand_; @@ -67,7 +69,7 @@ class TorqueControllerMux , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)]) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) - , torqueControllerPIDTV_(controllerOutputs_[static_cast(TorqueController_e::TC_PID_VECTORING)]) {} + , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) {} /// @brief torque controller mux constructor that leaves all other TCs with defaults accept for simple TC @@ -79,22 +81,19 @@ class TorqueControllerMux , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) - , torqueControllerPIDTV_(controllerOutputs_[static_cast(TorqueController_e::TC_PID_VECTORING)]) {} + , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) {} // Functions void tick( const SysTick_s &tick, const DrivetrainDynamicReport_s &drivetrainData, const PedalsSystemData_s &pedalsData, const SteeringSystemData_s &steeringData, - const AnalogConversion_s &loadFLData, - const AnalogConversion_s &loadFRData, - const AnalogConversion_s &loadRLData, - const AnalogConversion_s &loadRRData, + const LoadCellInterfaceOutput_s &loadCellData, DialMode_e dashboardDialMode, bool dashboardTorqueModeButtonPressed, const vector_nav &vn_data, - float wheel_angle_rad); - + const DrivetrainCommand_s &CASECommand + ); const DrivetrainCommand_s &getDrivetrainCommand() { return drivetrainCommand_; @@ -140,11 +139,6 @@ class TorqueControllerMux return static_cast(&torqueControllerNone_); } } - - PIDTVTorqueControllerData get_pidtv_data() - { - return torqueControllerPIDTV_.get_data(); - } }; #endif /* __TORQUECTRLMUX_H__ */ diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 151c95705..e5dcc4fba 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -9,6 +9,8 @@ #include "DashboardInterface.h" #include "PhysicalParameters.h" #include "VectornavInterface.h" +#include "SteeringSystem.h" +#include "LoadCellInterface.h" #include "accel_lookup.h" @@ -28,7 +30,7 @@ const float AMK_MAX_RPM = 20000; // const float AMK_MAX_RPM = (2.235 * METERS_PER_SECOND_TO_RPM); // 5mph // const float AMK_MAX_RPM = (.89 * METERS_PER_SECOND_TO_RPM); // 1mph // const float -const float AMK_MAX_TORQUE = 21.42; // TODO: update this with the true value +const float AMK_MAX_TORQUE = 21.42; const float MAX_REGEN_TORQUE = 10.0; /* LAUNCH CONSTANTS */ @@ -45,11 +47,6 @@ const float launch_ready_speed_threshold = 5.0 * METERS_PER_SECOND_TO_RPM; // rp const float launch_go_accel_threshold = .9; const float launch_stop_accel_threshold = .5; -constexpr double EARTH_RADIUS_KM = 6371.0; -constexpr double toRadians(double degrees) { - return degrees * M_PI / 180.0; -} - /* DRIVETRAIN STRUCTS */ const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = { @@ -62,6 +59,13 @@ struct TorqueControllerOutput_s bool ready; }; +/* TC STRUCTS */ +struct TCCaseWrapperTick_s +{ + const DrivetrainCommand_s &command; + const SteeringSystemData_s &steeringData; +}; + /* ENUMS */ enum TorqueController_e @@ -72,7 +76,7 @@ enum TorqueController_e TC_SIMPLE_LAUNCH = 3, TC_SLIP_LAUNCH = 4, TC_LOOKUP_LAUNCH = 5, - TC_PID_VECTORING = 6, + TC_CASE_SYSTEM = 6, TC_NUM_CONTROLLERS = 7, }; @@ -84,28 +88,6 @@ enum class LaunchStates_e LAUNCHING }; -/* CONTROLLER FUNCTIONS */ - -/// @brief If a command fed through this function exceeds the specified power limit, all torques will be scaled down equally -/// @param command -/// @param drivetrainData -/// @param powerLimit In watts, not kilowatts -/// @param -/// @return A scaled down DrivetrainCommand_s -static DrivetrainCommand_s TCPowerLimitScaleDown( - DrivetrainCommand_s command, - DrivetrainDynamicReport_s *drivetrainData, - float powerLimit); - -/// @brief Apply a per-wheel torque limit -/// @param command -/// @param torqueLimits -/// @param -/// @return A torque-limited DrivetrainCommand_s -static DrivetrainCommand_s TCTorqueLimit( - DrivetrainCommand_s command, - float torqueLimits[NUM_MOTORS]); - /* TORQUE CONTROLLERS */ /* @@ -113,10 +95,9 @@ static DrivetrainCommand_s TCTorqueLimit( */ class TorqueControllerBase { - public: +public: /* returns the launch state for the purpose of lighting the dahsboard LED and unit testing. To be overridden in launch torque modes */ virtual LaunchStates_e get_launch_state() { return LaunchStates_e::NO_LAUNCH_MODE; } - }; template @@ -229,32 +210,26 @@ class TorqueControllerLoadCellVectoring : public TorqueController, public BaseLaunchController { private: float launch_rate_target_; -public: +public: /*! SIMPLE LAUNCH CONTROLLER This launch controller is based off of a specified launch rate and an initial speed target @@ -283,21 +258,21 @@ class TorqueControllerSimpleLaunch : public TorqueController, */ TorqueControllerSimpleLaunch(TorqueControllerOutput_s &writeout, float launch_rate, int16_t initial_speed_target) : BaseLaunchController(writeout, initial_speed_target), - launch_rate_target_(launch_rate) {} + launch_rate_target_(launch_rate) {} TorqueControllerSimpleLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSimpleLaunch(writeout, DEFAULT_LAUNCH_RATE, DEFAULT_LAUNCH_SPEED_TARGET) {} - LaunchStates_e get_launch_state() override { return launch_state; } + LaunchStates_e get_launch_state() override { return launch_state_; } - void calc_launch_algo(const vector_nav* vn_data) override; + void calc_launch_algo(const vector_nav *vn_data) override; }; class TorqueControllerSlipLaunch : public TorqueController, public BaseLaunchController { private: float slip_ratio_; -public: +public: /*! SLIP LAUNCH CONTROLLER This launch controller is based off of a specified slip constant. It will at all times attempt @@ -312,17 +287,17 @@ class TorqueControllerSlipLaunch : public TorqueController, publ TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSlipLaunch(writeout, DEFAULT_SLIP_RATIO, DEFAULT_LAUNCH_SPEED_TARGET) {} - LaunchStates_e get_launch_state() override { return launch_state; } + LaunchStates_e get_launch_state() override { return launch_state_; } - void calc_launch_algo(const vector_nav* vn_data) override; + void calc_launch_algo(const vector_nav *vn_data) override; }; class TorqueControllerLookupLaunch : public TorqueController, BaseLaunchController { private: bool init_position = false; -public: +public: /*! Lookup Launch Controller This launch controller is based off of a matlab and symlink generated lookup table. @@ -335,42 +310,23 @@ class TorqueControllerLookupLaunch : public TorqueController, : BaseLaunchController(writeout, initial_speed_target) {} TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerLookupLaunch(writeout, DEFAULT_LAUNCH_SPEED_TARGET) {} - - LaunchStates_e get_launch_state() override { return launch_state; } - - void calc_launch_algo(const vector_nav* vn_data) override; + + LaunchStates_e get_launch_state() override { return launch_state_; } + + void calc_launch_algo(const vector_nav *vn_data) override; }; -class TorqueControllerPIDTV: public TorqueController +class TorqueControllerCASEWrapper : public TorqueController { -public: - void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, float vx_b, float wheel_angle_rad, float yaw_rate); - TorqueControllerPIDTV(TorqueControllerOutput_s &writeout): writeout_(writeout) +public: + void tick(const TCCaseWrapperTick_s &intake); + TorqueControllerCASEWrapper(TorqueControllerOutput_s &writeout) : writeout_(writeout) { - tv_pid_.initialize(); - tv_pid_.setExternalInputs(&pid_input_); - pid_input_.PID_P = 2.0; - pid_input_.PID_I = 1.0; - pid_input_.PID_D = 0.0; - pid_input_.PID_N = 100; - - } - - PIDTVTorqueControllerData get_data(){ - PIDTVTorqueControllerData data; - data.controller_input = tv_pid_.shit_in; - data.controller_output = tv_pid_.shit_out; - data.fl_torque_delta = pid_input_.FL_in - tv_pid_.getExternalOutputs().FL_out; - data.fr_torque_delta = pid_input_.FR_in - tv_pid_.getExternalOutputs().FR_out; - data.rl_torque_delta = pid_input_.RL_in - tv_pid_.getExternalOutputs().RL_out; - data.rr_torque_delta = pid_input_.RR_in - tv_pid_.getExternalOutputs().RR_out; - return data; + writeout_ = writeout; } + private: TorqueControllerOutput_s &writeout_; - - PID_TV::ExtU_PID_TV_T pid_input_; - PID_TV tv_pid_; }; #endif /* __TORQUECONTROLLERS_H__ */ diff --git a/lib/systems/include/Utility.h b/lib/systems/include/Utility.h index 0240ecde7..dc2a9d508 100644 --- a/lib/systems/include/Utility.h +++ b/lib/systems/include/Utility.h @@ -10,4 +10,29 @@ const int NUM_MOTORS = 4; const int DEBOUNCE_MILLIS = 10; // milliseconds before registering another button press +/// @brief generic data vector type that can be used with tire and / or anything to do with 4 corners of the car. +template +struct veh_vec +{ + T FL; + T FR; + T RL; + T RR; +}; + +template +struct xyz_vec +{ + T x; + T y; + T z; +}; + +template +struct xy_vec +{ + T x; + T y; +}; + #endif /* __UTILITY_H__ */ \ No newline at end of file diff --git a/lib/systems/library.json b/lib/systems/library.json index 25f3598d7..50a076984 100644 --- a/lib/systems/library.json +++ b/lib/systems/library.json @@ -6,7 +6,7 @@ "interfaces-lib": "*", "shared-interfaces-lib": "*", "shared-systems-lib": "*", - "PID_TV-lib": "*", + "CASE_lib": "*", "shared_data": "*" }, "frameworks": "*", diff --git a/lib/systems/src/PedalsSystem.cpp b/lib/systems/src/PedalsSystem.cpp index c3c3a7d78..1a4107abf 100644 --- a/lib/systems/src/PedalsSystem.cpp +++ b/lib/systems/src/PedalsSystem.cpp @@ -134,7 +134,7 @@ bool PedalsSystem::evaluate_pedal_implausibilities_(const AnalogConversion_s &pe bool pedal1_min_max_implaus = evaluate_min_max_pedal_implausibilities_(pedalData1, params.min_pedal_1, params.max_pedal_1, params.implausibility_margin); bool pedal2_min_max_implaus = evaluate_min_max_pedal_implausibilities_(pedalData2, params.min_pedal_2, params.max_pedal_2, params.implausibility_margin); bool sens_not_within_req_percent = (fabs(pedalData1.conversion - pedalData2.conversion) > max_percent_diff); - if (pedal2_min_max_implaus || pedal2_min_max_implaus) + if (pedal1_min_max_implaus || pedal2_min_max_implaus) { return true; } diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 17a2c5999..72727354a 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -1,44 +1,50 @@ #include "SteeringSystem.h" #include - -void SteeringSystem::tick(const SysTick_s &tick, const AnalogConversion_s &secondaryConversion) +void SteeringSystem::tick(const SteeringSystemTick_s &intake) { // System works at 100hz // 1. Polls upper steering sensor // 2. Computes internal state - if (tick.triggers.trigger100) + if (intake.tick.triggers.trigger100) { // Poll upper steering sensor primarySensor_->sample(); primaryConversion_ = primarySensor_->convert(); - // Compute internal state - - // Both sensors are nominal - if ((primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL) && (secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD)) + // Both sensors are nominal and agree + if ( + (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL) + && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD) + && (std::abs(primaryConversion_.angle - intake.secondaryConversion.conversion) < STEERING_DIVERGENCE_WARN_THRESHOLD) + ) { - data_ = { + steeringData_ = { .angle = primaryConversion_.angle, .status = SteeringSystemStatus_e::STEERING_SYSTEM_NOMINAL }; } // One or both sensors are marginal // Sensors disagree by STEERING_DIVERGENCE_WARN_THRESHOLD degrees and less than STEERING_DIVERGENCE_ERROR_THRESHOLD degrees - else if ((primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL) - || (secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED) - || ((std::abs(primaryConversion_.angle - secondaryConversion.conversion) > STEERING_DIVERGENCE_WARN_THRESHOLD) && (std::abs(primaryConversion_.angle - secondaryConversion.conversion) < STEERING_DIVERGENCE_ERROR_THRESHOLD))) + else if ( + (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL) + || (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED) + || ((std::abs(primaryConversion_.angle - intake.secondaryConversion.conversion) >= STEERING_DIVERGENCE_WARN_THRESHOLD) && (std::abs(primaryConversion_.angle - intake.secondaryConversion.conversion) < STEERING_DIVERGENCE_ERROR_THRESHOLD)) + ) { - data_ = { + steeringData_ = { .angle = primaryConversion_.angle, .status = SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL }; } - // Upper steering sensor reports error, lower sensor is nominal - else if ((primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_ERROR) && (secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD)) + // Upper steering sensor reports error or is not detected, lower sensor is nominal + else if ( + (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_ERROR) + && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD) + ) { - data_ = { - .angle = secondaryConversion.conversion, + steeringData_ = { + .angle = intake.secondaryConversion.conversion, .status = SteeringSystemStatus_e::STEERING_SYSTEM_DEGRADED }; } @@ -46,7 +52,7 @@ void SteeringSystem::tick(const SysTick_s &tick, const AnalogConversion_s &secon // Complete failure of steering sensing else { - data_ = { + steeringData_ = { .angle = 0.0, .status = SteeringSystemStatus_e::STEERING_SYSTEM_ERROR }; diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 79bddcc1b..f246e0e62 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -7,22 +7,22 @@ void TorqueControllerMux::tick( const DrivetrainDynamicReport_s &drivetrainData, const PedalsSystemData_s &pedalsData, const SteeringSystemData_s &steeringData, - const AnalogConversion_s &loadFLData, - const AnalogConversion_s &loadFRData, - const AnalogConversion_s &loadRLData, - const AnalogConversion_s &loadRRData, + const LoadCellInterfaceOutput_s &loadCellData, DialMode_e dashboardDialMode, bool dashboardTorqueModeButtonPressed, const vector_nav &vn_data, - float wheel_angle_rad) + const DrivetrainCommand_s &CASECommand) { // Tick all torque controllers torqueControllerSimple_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_]); - torqueControllerLoadCellVectoring_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_], loadFLData, loadFRData, loadRLData, loadRRData); + torqueControllerLoadCellVectoring_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_], loadCellData); torqueControllerSimpleLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds, &vn_data); torqueControllerSlipLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds, &vn_data); - // torqueControllerPIDTV_.tick(tick, pedalsData, vn_data.velocity_x, wheel_angle_rad, vn_data.yaw); - + tcCASEWrapper_.tick( + (TCCaseWrapperTick_s){ + .command = CASECommand, + .steeringData = steeringData}); + // Tick torque button logic at 50hz if (tick.triggers.trigger50) { @@ -49,7 +49,8 @@ void TorqueControllerMux::tick( { // Check if speed permits mode change bool speedPreventsModeChange = false; - for (int i = 0; i < NUM_MOTORS; i++) { + for (int i = 0; i < NUM_MOTORS; i++) + { speedPreventsModeChange |= drivetrainData.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND >= MAX_SPEED_FOR_MODE_CHANGE; } @@ -92,7 +93,6 @@ void TorqueControllerMux::tick( applyTorqueLimit(&drivetrainCommand_); applyPowerLimit(&drivetrainCommand_, &drivetrainData); applyPosSpeedLimit(&drivetrainCommand_); - } } @@ -100,7 +100,7 @@ void TorqueControllerMux::tick( Apply limit to make sure that regenerative braking is not applied when wheelspeed is below 5kph on all wheels. */ -void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain) +void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s *command, const DrivetrainDynamicReport_s *drivetrain) { const float noRegenLimitKPH = 10.0; const float fullRegenLimitKPH = 5.0; @@ -108,9 +108,15 @@ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s* command, const Dr float torqueScaleDown = 0.0; bool allWheelsRegen = true; // true when all wheels are targeting speeds below the current wheel speed - for (int i = 0; i < NUM_MOTORS; i++) { - maxWheelSpeed = std::max(maxWheelSpeed, drivetrain->measuredSpeeds[i] * RPM_TO_KILOMETERS_PER_HOUR); - allWheelsRegen &= (command->speeds_rpm[i] < drivetrain->measuredSpeeds[i]); + for (int i = 0; i < NUM_MOTORS; i++) + { + #ifdef ARDUINO_TEENSY41 + maxWheelSpeed = std::max(maxWheelSpeed, abs(drivetrain->measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); + allWheelsRegen &= (command->speeds_rpm[i] < abs(drivetrain->measuredSpeeds[i]) || command->speeds_rpm[i] == 0); + #else + maxWheelSpeed = std::max(maxWheelSpeed, std::abs(drivetrain->measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); + allWheelsRegen &= (command->speeds_rpm[i] < std::abs(drivetrain->measuredSpeeds[i]) || command->speeds_rpm[i] == 0); + #endif } // begin limiting regen at noRegenLimitKPH and completely limit regen at fullRegenLimitKPH @@ -119,7 +125,8 @@ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s* command, const Dr if (allWheelsRegen) { - for (int i = 0; i < NUM_MOTORS; i++) { + for (int i = 0; i < NUM_MOTORS; i++) + { command->torqueSetpoints[i] *= torqueScaleDown; } } @@ -130,13 +137,14 @@ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s* command, const Dr exceeds the preset mechanical power limit. Scales all wheels down to preserve functionality of torque controllers */ -void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain) +void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s *command, const DrivetrainDynamicReport_s *drivetrain) { float net_torque_mag = 0; float net_power = 0; // calculate current mechanical power - for (int i = 0; i < NUM_MOTORS; i++) { + for (int i = 0; i < NUM_MOTORS; i++) + { // get the total magnitude of torque from all 4 wheels #ifdef ARDUINO_TEENSY41 // screw arduino.h macros net_torque_mag += abs(command->torqueSetpoints[i]); @@ -146,21 +154,33 @@ void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command, const Dr net_torque_mag += std::abs(command->torqueSetpoints[i]); // calculate P = T*w for each wheel and sum together net_power += std::abs(command->torqueSetpoints[i] * (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); - #endif +#endif } - + // only evaluate power limit if current power exceeds it - if (net_power > (MAX_POWER_LIMIT)) { - for (int i = 0; i < NUM_MOTORS; i++) { + if (net_power > (MAX_POWER_LIMIT)) + { + for (int i = 0; i < NUM_MOTORS; i++) + { + + // TODO: SOMEBODY PLZ MAKE THIS WORK + // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); + // calculate the percent of total torque requested per wheel - float torque_percent = command->torqueSetpoints[i] / net_torque_mag; + float torque_percent = abs(command->torqueSetpoints[i] / net_torque_mag); // based on the torque percent and max power limit, get the max power each wheel can use float power_per_corner = (torque_percent * MAX_POWER_LIMIT); + // power / omega (motor rad/s) to get torque per wheel - command->torqueSetpoints[i] = power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND); - } + #ifdef ARDUINO_TEENSY41 + command->torqueSetpoints[i] = abs(power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); + #else + command->torqueSetpoints[i] = std::abs(power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); + #endif + command->torqueSetpoints[i] = std::max(0.0f, std::min(command->torqueSetpoints[i], getMaxTorque())); + + } } - } /* @@ -169,35 +189,38 @@ void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command, const Dr This will uniformally scale down all torques as to not affect the functionality of non-symmetrical torque controllers. */ -void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s* command) -{ +void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s *command) +{ float max_torque = getMaxTorque(); float avg_torque = 0; // get the average torque accross all 4 wheels - for (int i = 0; i < NUM_MOTORS; i++) { - #ifdef ARDUINO_TEENSY41 // screw arduino.h macros + for (int i = 0; i < NUM_MOTORS; i++) + { +#ifdef ARDUINO_TEENSY41 // screw arduino.h macros avg_torque += abs(command->torqueSetpoints[i]); - #else +#else avg_torque += std::abs(command->torqueSetpoints[i]); - #endif +#endif } avg_torque /= NUM_MOTORS; // if this is greather than the torque limit, scale down - if (avg_torque > max_torque) { + if (avg_torque > max_torque) + { // get the scale of avg torque above max torque float scale = avg_torque / max_torque; // divide by scale to lower avg below max torque - for (int i = 0; i < NUM_MOTORS; i++) { + for (int i = 0; i < NUM_MOTORS; i++) + { command->torqueSetpoints[i] /= scale; } } - } /* Apply limit such that wheelspeed never goes negative */ -void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s* command) { +void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s *command) +{ for (int i = 0; i < NUM_MOTORS; i++) { command->speeds_rpm[i] = std::max(0.0f, command->speeds_rpm[i]); diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp index 229398b1e..4351c4dc8 100644 --- a/lib/systems/src/TorqueControllers.cpp +++ b/lib/systems/src/TorqueControllers.cpp @@ -59,19 +59,17 @@ void TorqueControllerLoadCellVectoring::tick( const SysTick_s &tick, const PedalsSystemData_s &pedalsData, float torqueLimit, - const AnalogConversion_s &flLoadCellData, - const AnalogConversion_s &frLoadCellData, - const AnalogConversion_s &rlLoadCellData, - const AnalogConversion_s &rrLoadCellData) + const LoadCellInterfaceOutput_s &loadCellData +) { // Calculate torque commands at 100hz if (tick.triggers.trigger100) { // Apply FIR filter to load cell data - loadCellForcesRaw_[0][FIRCircBufferHead] = flLoadCellData.conversion; - loadCellForcesRaw_[1][FIRCircBufferHead] = frLoadCellData.conversion; - loadCellForcesRaw_[2][FIRCircBufferHead] = rlLoadCellData.conversion; - loadCellForcesRaw_[3][FIRCircBufferHead] = rrLoadCellData.conversion; + loadCellForcesRaw_[0][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.FL; + loadCellForcesRaw_[1][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.FR; + loadCellForcesRaw_[2][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.RL; + loadCellForcesRaw_[3][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.RR; for (int i = 0; i < 4; i++) { @@ -87,10 +85,10 @@ void TorqueControllerLoadCellVectoring::tick( FIRSaturated_ = true; // Do sanity checks on raw data - loadCellsErrorCounter_[0] = flLoadCellData.raw != 4095 && flLoadCellData.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[0] + 1; - loadCellsErrorCounter_[1] = frLoadCellData.raw != 4095 && frLoadCellData.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[1] + 1; - loadCellsErrorCounter_[2] = rlLoadCellData.raw != 4095 && rlLoadCellData.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[2] + 1; - loadCellsErrorCounter_[3] = rrLoadCellData.raw != 4095 && rrLoadCellData.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[3] + 1; + loadCellsErrorCounter_[0] = loadCellData.loadCellConversions.FL.raw != 4095 && loadCellData.loadCellConversions.FL.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[0] + 1; + loadCellsErrorCounter_[1] = loadCellData.loadCellConversions.FR.raw != 4095 && loadCellData.loadCellConversions.FR.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[1] + 1; + loadCellsErrorCounter_[2] = loadCellData.loadCellConversions.RL.raw != 4095 && loadCellData.loadCellConversions.RL.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[2] + 1; + loadCellsErrorCounter_[3] = loadCellData.loadCellConversions.RR.raw != 4095 && loadCellData.loadCellConversions.RR.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[3] + 1; ready_ = FIRSaturated_ && loadCellsErrorCounter_[0] < errorCountThreshold_ && loadCellsErrorCounter_[1] < errorCountThreshold_ && loadCellsErrorCounter_[2] < errorCountThreshold_ && loadCellsErrorCounter_[3] < errorCountThreshold_; writeout_.ready = ready_; @@ -159,7 +157,7 @@ void BaseLaunchController::tick( if (tick.triggers.trigger100){ - current_millis = tick.millis; + current_millis_ = tick.millis; int16_t brake_torque_req = pedalsData.regenPercent * MAX_REGEN_TORQUE; @@ -170,7 +168,7 @@ void BaseLaunchController::tick( writeout_.ready = true; - switch(launch_state){ + switch(launch_state_){ case LaunchStates_e::LAUNCH_NOT_READY: // set torques and speed to 0 writeout_.command.speeds_rpm[FL] = 0.0; @@ -184,14 +182,14 @@ void BaseLaunchController::tick( writeout_.command.torqueSetpoints[RR] = brake_torque_req; //init launch vars - launch_speed_target = 0; - time_of_launch = tick.millis; + launch_speed_target_ = 0; + time_of_launch_ = tick.millis; // check speed is 0 and pedals not pressed if((pedalsData.accelPercent < launch_ready_accel_threshold) && (pedalsData.brakePercent < launch_ready_brake_threshold) && (max_speed < launch_ready_speed_threshold)) { - launch_state = LaunchStates_e::LAUNCH_READY; + launch_state_ = LaunchStates_e::LAUNCH_READY; } break; @@ -208,21 +206,21 @@ void BaseLaunchController::tick( writeout_.command.torqueSetpoints[RR] = brake_torque_req; //init launch vars - launch_speed_target = 0; - time_of_launch = current_millis; + launch_speed_target_ = 0; + time_of_launch_ = current_millis_; //check speed is 0 and brake not pressed if ((pedalsData.brakePercent >= launch_ready_brake_threshold) || (max_speed >= launch_ready_speed_threshold)) { - launch_state = LaunchStates_e::LAUNCH_NOT_READY; + launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; } else if(pedalsData.accelPercent >= launch_go_accel_threshold){ - initial_ecef_x = vn_data->ecef_coords[0]; - initial_ecef_y = vn_data->ecef_coords[1]; - initial_ecef_z = vn_data->ecef_coords[2]; + initial_ecef_x_ = vn_data->ecef_coords[0]; + initial_ecef_y_ = vn_data->ecef_coords[1]; + initial_ecef_z_ = vn_data->ecef_coords[2]; - launch_state = LaunchStates_e::LAUNCHING; + launch_state_ = LaunchStates_e::LAUNCHING; } //check accel above launch threshold and launch @@ -233,15 +231,15 @@ void BaseLaunchController::tick( if((pedalsData.accelPercent <= launch_stop_accel_threshold) || (pedalsData.brakePercent >= launch_ready_brake_threshold)) { - launch_state = LaunchStates_e::LAUNCH_NOT_READY; + launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; } calc_launch_algo(vn_data); - writeout_.command.speeds_rpm[FL] = launch_speed_target; - writeout_.command.speeds_rpm[FR] = launch_speed_target; - writeout_.command.speeds_rpm[RL] = launch_speed_target; - writeout_.command.speeds_rpm[RR] = launch_speed_target; + writeout_.command.speeds_rpm[FL] = launch_speed_target_; + writeout_.command.speeds_rpm[FR] = launch_speed_target_; + writeout_.command.speeds_rpm[RL] = launch_speed_target_; + writeout_.command.speeds_rpm[RR] = launch_speed_target_; writeout_.command.torqueSetpoints[FL] = AMK_MAX_TORQUE; writeout_.command.torqueSetpoints[FR] = AMK_MAX_TORQUE; @@ -266,58 +264,40 @@ void TorqueControllerSimpleLaunch::calc_launch_algo(const vector_nav* vn_data) { This is then converted to RPM for a speed target There is an initial speed target that is your iitial instant acceleration on the wheels */ - float secs_since_launch = (float)(current_millis - time_of_launch) / 1000.0; - launch_speed_target = (int16_t)((float) secs_since_launch * launch_rate_target_ * METERS_PER_SECOND_TO_RPM); - launch_speed_target += init_speed_target_; - launch_speed_target = std::min((int)AMK_MAX_RPM, std::max(0, (int)launch_speed_target)); + float secs_since_launch = (float)(current_millis_ - time_of_launch_) / 1000.0; + launch_speed_target_ = (int16_t)((float) secs_since_launch * launch_rate_target_ * METERS_PER_SECOND_TO_RPM); + launch_speed_target_ += init_speed_target_; + launch_speed_target_ = std::min((int)AMK_MAX_RPM, std::max(0, (int)launch_speed_target_)); } void TorqueControllerSlipLaunch::calc_launch_algo(const vector_nav* vn_data) { + // accelerate at constant speed for a period of time to get body velocity up + // may want to make this the ht07 launch algo + + // makes sure that the car launches at the target launch speed + launch_speed_target_ = std::max(launch_speed_target_, (float)DEFAULT_LAUNCH_SPEED_TARGET); - uint32_t ms_since_launch = (current_millis - time_of_launch); - // accelerate at constant speed for a period of time to get body velocity up - // may want to make this the ht07 launch algo - - // makes sure that the car launches at the target launch speed - launch_speed_target = std::max(launch_speed_target, (float)DEFAULT_LAUNCH_SPEED_TARGET); - - /* - New slip-ratio based launch algorithm by Luke Chen. The basic idea - is to always be pushing the car a certain 'slip_ratio_' faster than - the car is currently going, theoretically always keeping the car in slip - */ - // m/s - float new_speed_target = (1 + slip_ratio_) * (vn_data->velocity_x); - // rpm - new_speed_target *= METERS_PER_SECOND_TO_RPM; - // makes sure the car target speed never goes lower than prev. target - // allows for the vn to 'spool' up and us to get reliable vx data - launch_speed_target = std::max(launch_speed_target, new_speed_target); - + /* + New slip-ratio based launch algorithm by Luke Chen. The basic idea + is to always be pushing the car a certain 'slip_ratio_' faster than + the car is currently going, theoretically always keeping the car in slip + */ + // m/s + float new_speed_target = (1 + slip_ratio_) * (vn_data->velocity_x); + // rpm + new_speed_target *= METERS_PER_SECOND_TO_RPM; + // makes sure the car target speed never goes lower than prev. target + // allows for the vn to 'spool' up and us to get reliable vx data + launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); } void TorqueControllerLookupLaunch::calc_launch_algo(const vector_nav* vn_data) { - // initial_lat = toRadians(initial_lat); - // initial_lon = toRadians(initial_lon); - // double lat = toRadians(vn_data->latitude); - // double lon = toRadians(vn_data->longitude); + launch_speed_target_ = std::max((float)DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_); - - // /* BEWARE PARTIALLY WRITTEN BY CHAT */ - // double dLat = lat - initial_lat; - // double dLon = lon - initial_lon; - // double a = sin(dLat / 2) * sin(dLat / 2) + - // cos(lat) * cos(initial_lat) * - // sin(dLon / 2) * sin(dLon / 2); - // double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - // double distance = EARTH_RADIUS_KM * c; - - launch_speed_target = std::max((float)DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target); - - double dx = vn_data->ecef_coords[0] - initial_ecef_x; - double dy = vn_data->ecef_coords[1] - initial_ecef_y; - double dz = vn_data->ecef_coords[2] - initial_ecef_z; + double dx = vn_data->ecef_coords[0] - initial_ecef_x_; + double dy = vn_data->ecef_coords[1] - initial_ecef_y_; + double dz = vn_data->ecef_coords[2] - initial_ecef_z_; double distance = sqrt((dx*dx) + (dy*dy) + (dz*dz)); @@ -334,38 +314,19 @@ void TorqueControllerLookupLaunch::calc_launch_algo(const vector_nav* vn_data) { float mps_target = vel_dist_lookup[idx]; float new_speed_target = mps_target * METERS_PER_SECOND_TO_RPM; - launch_speed_target = std::max(launch_speed_target, new_speed_target); + launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); } -void TorqueControllerPIDTV::tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, float vx_b, float wheel_angle_rad, float yaw_rate) +void TorqueControllerCASEWrapper::tick(const TCCaseWrapperTick_s &intake) { - float accelRequest = pedalsData.accelPercent; - auto torqueRequest = accelRequest * AMK_MAX_TORQUE; - - if (tick.triggers.trigger100) + for (int i = 0; i < NUM_MOTORS; i++) { - pid_input_.Vx_B = vx_b; - pid_input_.WheelDeltarad = wheel_angle_rad; - pid_input_.YawRaterads = yaw_rate; - pid_input_.FR_in = torqueRequest; - - pid_input_.RR_in = torqueRequest; - pid_input_.FL_in = torqueRequest; - pid_input_.RL_in = torqueRequest; - - tv_pid_.step(); - const PID_TV::ExtY_PID_TV_T &out = tv_pid_.getExternalOutputs(); - - writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RR] = AMK_MAX_RPM; - - writeout_.command.torqueSetpoints[FL] = out.FL_out; - writeout_.command.torqueSetpoints[FR] = out.FR_out; - writeout_.command.torqueSetpoints[RL] = out.RL_out; - writeout_.command.torqueSetpoints[RR] = out.RR_out; + writeout_.command.speeds_rpm[i] = intake.command.speeds_rpm[i]; + writeout_.command.torqueSetpoints[i] = intake.command.torqueSetpoints[i]; } -} + + writeout_.ready = intake.steeringData.status != SteeringSystemStatus_e::STEERING_SYSTEM_ERROR; + +} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index d26f65235..d69ab7739 100644 --- a/platformio.ini +++ b/platformio.ini @@ -11,6 +11,7 @@ lib_ignore = test_ignore= test_interfaces* lib_deps= + git+ssh://git@github.com/hytech-racing/CASE_lib.git#v39 https://github.com/hytech-racing/shared_firmware_systems.git [env:teensy41] @@ -48,7 +49,8 @@ lib_deps = https://github.com/RCMast3r/spi_libs https://github.com/tonton81/FlexCAN_T4 https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/72/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/80/can_lib.tar.gz + git+ssh://git@github.com/hytech-racing/CASE_lib.git#v39 [env:test_can_on_teensy] lib_ignore = diff --git a/src/main.cpp b/src/main.cpp index d448b45c6..ec9079834 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -19,6 +19,7 @@ #include "TelemetryInterface.h" #include "SABInterface.h" #include "VectornavInterface.h" +#include "LoadCellInterface.h" /* Systems */ #include "SysClock.h" @@ -28,8 +29,10 @@ #include "PedalsSystem.h" #include "TorqueControllerMux.h" +#include "CASESystem.h" // /* State machine */ #include "MCUStateMachine.h" +#include "HT08_CASE.h" /* PARAMETER STRUCTS @@ -47,8 +50,7 @@ const TelemetryInterfaceReadChannels telem_read_channels = { .analog_steering_channel = MCU15_STEERING_CHANNEL, .current_channel = MCU15_CUR_POS_SENSE_CHANNEL, .current_ref_channel = MCU15_CUR_NEG_SENSE_CHANNEL, - .glv_sense_channel = MCU15_GLV_SENSE_CHANNEL -}; + .glv_sense_channel = MCU15_GLV_SENSE_CHANNEL}; const PedalsParams accel_params = { .min_pedal_1 = ACCEL1_PEDAL_MIN, @@ -62,8 +64,7 @@ const PedalsParams accel_params = { .activation_percentage = APPS_ACTIVATION_PERCENTAGE, .deadzone_margin = DEFAULT_PEDAL_DEADZONE, .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, - .mechanical_activation_percentage = APPS_ACTIVATION_PERCENTAGE -}; + .mechanical_activation_percentage = APPS_ACTIVATION_PERCENTAGE}; const PedalsParams brake_params = { .min_pedal_1 = BRAKE1_PEDAL_MIN, @@ -89,7 +90,8 @@ FlexCAN_T4 INV_CAN; // Inverter CAN (now both a FlexCAN_T4 TELEM_CAN; // telemetry CAN (basically everything except inverters) /* Set up CAN circular buffer */ -using CircularBufferType = Circular_Buffer; +// using CircularBufferType = Circular_Buffer; +using CircularBufferType = CANBufferType; /* Sensors */ MCP_ADC<8> a1 = MCP_ADC<8>(ADC1_CS); @@ -113,6 +115,7 @@ SABInterface sab_interface( LOADCELL_RR_SCALE, // RR Scale LOADCELL_RR_OFFSET // RR Offset ); +LoadCellInterface load_cell_interface; // /* Inverter Interface Type */ using InvInt_t = InverterInterface; @@ -138,6 +141,73 @@ PedalsSystem pedals_system(accel_params, brake_params); using DriveSys_t = DrivetrainSystem; DriveSys_t drivetrain = DriveSys_t({&inv.fl, &inv.fr, &inv.rl, &inv.rr}, &main_ecu, INVERTER_ENABLING_TIMEOUT_INTERVAL); TorqueControllerMux torque_controller_mux(1.0, 0.4); +// TODO ensure that case uses max regen torque, right now its not +CASEConfiguration case_config = { + // Following used for generated code + .AbsoluteTorqueLimit = AMK_MAX_TORQUE, // N-m + .yaw_pid_p = 1.33369, + .yaw_pid_i = 0.25, + .yaw_pid_d = 0.0, + .tcs_pid_p = 40.0, + .tcs_pid_i = 0.0, + .tcs_pid_d = 0.0, + .useLaunch = false, + .usePIDTV = true, + .useTCSLimitedYawPID = true, + .useNormalForce = false, + .useTractionControl = true, + .usePowerLimit = true, + .usePIDPowerLimit = false, + .useDecoupledYawBrakes = true, + .useDiscontinuousYawPIDBrakes = false, + .tcsSLThreshold = 0.2, + .launchSL = 0.2, + .launchDeadZone = 20, // N-m + .launchVelThreshold = 0.75, // m/s + .tcsVelThreshold = 2.5, // m/s + .yawPIDMaxDifferential = 10, // N-m + .yawPIDErrorThreshold = 0.1, // rad/s + .yawPIDVelThreshold = 1, // m/s + .yawPIDCoastThreshold = 2.5, // m/s + .yaw_pid_brakes_p = 0.25, + .yaw_pid_brakes_i = 0, + .yaw_pid_brakes_d = 0, + .decoupledYawPIDBrakesMaxDIfference = 2, // N-m + .discontinuousBrakesPercentThreshold = 0.4, + .TorqueMode = AMK_MAX_TORQUE, // N-m + // .TorqueMode = 2, // N-m + .RegenLimit = -10.0, // N-m + .useNoRegen5kph = true, + .useTorqueBias = true, + .DriveTorquePercentFront = 0.5, + .BrakeTorquePercentFront = 0.6, + .MechPowerMaxkW = 63, // kW + + // Following used for calculate_torque_request in CASESystem.tpp + .max_rpm = AMK_MAX_RPM, + .max_regen_torque = AMK_MAX_TORQUE, + .max_torque = AMK_MAX_TORQUE, +}; +// Torque limit used for yaw pid torque split overflow +// Yaw PID P +// Yaw PID I +// Yaw PID D +// TCS PID P +// TCS PID I +// TCS PID D +// Use launch +// Use PID TV +// Use normal force TV +// Use Traction Control (TCS) +// Use power limit +// Use PID power limit +// TCS activation threshold +// TCS launch SL target +// TCS launch torque deadzone (N-m) +// Max motor rpm +// Max regen torque +// Max torque +CASESystem case_system(&CAN3_txBuffer, 100, 70, case_config); /* Declare state machine */ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); @@ -184,6 +254,9 @@ void setup() a1.setChannelOffset(MCU15_ACCEL2_CHANNEL, -ACCEL2_PEDAL_MIN); a1.setChannelOffset(MCU15_BRAKE1_CHANNEL, -BRAKE1_PEDAL_MIN); a1.setChannelOffset(MCU15_BRAKE2_CHANNEL, -BRAKE2_PEDAL_MIN); + a1.setChannelOffset(MCU15_STEERING_CHANNEL, -1 * SECONDARY_STEERING_SENSE_CENTER); + a1.setChannelScale(MCU15_STEERING_CHANNEL, STEERING_RANGE_DEGREES / ((float)SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND - (float)SECONDARY_STEERING_SENSE_LEFTMOST_BOUND)); + a1.setChannelClamp(MCU15_STEERING_CHANNEL, -STEERING_RANGE_DEGREES / 0.5 * 1.15, STEERING_RANGE_DEGREES / 0.5 * 1.15); // 15% tolerance on each end of the steering sensor a2.setChannelScale(MCU15_FL_LOADCELL_CHANNEL, LOADCELL_FL_SCALE /*Todo*/); a3.setChannelScale(MCU15_FR_LOADCELL_CHANNEL, LOADCELL_FR_SCALE /*Todo*/); @@ -191,7 +264,6 @@ void setup() a2.setChannelOffset(MCU15_FL_LOADCELL_CHANNEL, LOADCELL_FL_OFFSET /*Todo*/); a3.setChannelOffset(MCU15_FR_LOADCELL_CHANNEL, LOADCELL_FR_OFFSET /*Todo*/); - // get latest tick from sys clock SysTick_s curr_tick = sys_clock.tick(micros()); @@ -203,9 +275,10 @@ void setup() wd_interface.init(curr_tick.millis); // initialize wd kick time ams_interface.init(curr_tick.millis); // initialize last heartbeat time steering1.init(); + steering1.setOffset(PRIMARY_STEERING_SENSE_OFFSET); Serial.begin(115200); - + /* Init Systems */ @@ -289,48 +362,55 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) TriggerBits_s t = current_system_tick.triggers; if (t.trigger10) // 10Hz { - // Serial.println("before buzzer"); - dashboard.tick10(&main_ecu, int(fsm.get_state()), - buzzer.buzzer_is_on(), - drivetrain.drivetrain_error_occured(), - torque_controller_mux.getTorqueLimit(), - ams_interface.get_filtered_min_cell_voltage(), - telem_interface.get_glv_voltage(a1.get()), - static_cast(torque_controller_mux.activeController()->get_launch_state()), - torque_controller_mux.getDialMode()); - - main_ecu.tick(static_cast(fsm.get_state()), - drivetrain.drivetrain_error_occured(), - safety_system.get_software_is_ok(), - static_cast(torque_controller_mux.getDriveMode()), - static_cast(torque_controller_mux.getTorqueLimit()), - torque_controller_mux.getMaxTorque(), - buzzer.buzzer_is_on(), - pedals_system.getPedalsSystemData(), - ams_interface.pack_charge_is_critical(), - dashboard.launchControlButtonPressed()); + dashboard.tick10( + &main_ecu, + int(fsm.get_state()), + buzzer.buzzer_is_on(), + drivetrain.drivetrain_error_occured(), + torque_controller_mux.getTorqueLimit(), + ams_interface.get_filtered_min_cell_voltage(), + telem_interface.get_glv_voltage(a1.get()), + static_cast(torque_controller_mux.activeController()->get_launch_state()), + dashboard.getDialMode()); + + main_ecu.tick( + static_cast(fsm.get_state()), + drivetrain.drivetrain_error_occured(), + safety_system.get_software_is_ok(), + static_cast(torque_controller_mux.getDriveMode()), + static_cast(torque_controller_mux.getTorqueLimit()), + torque_controller_mux.getMaxTorque(), + buzzer.buzzer_is_on(), + pedals_system.getPedalsSystemData(), + ams_interface.pack_charge_is_critical(), + dashboard.launchControlButtonPressed()); + + PedalsSystemData_s data2 = pedals_system.getPedalsSystemDataCopy(); + + telem_interface.tick( + a1.get(), + a2.get(), + a3.get(), + steering1.convert(), + &inv.fl, + &inv.fr, + &inv.rl, + &inv.rr, + data2.accelImplausible, + data2.brakeImplausible, + data2.accelPercent, + data2.brakePercent, + a1.get().conversions[MCU15_ACCEL1_CHANNEL], + a1.get().conversions[MCU15_ACCEL2_CHANNEL], + a1.get().conversions[MCU15_BRAKE1_CHANNEL], + a1.get().conversions[MCU15_BRAKE2_CHANNEL], + pedals_system.getMechBrakeActiveThreshold(), + {}); } + if (t.trigger50) // 50Hz { steering1.sample(); - PedalsSystemData_s data2 = pedals_system.getPedalsSystemDataCopy(); - telem_interface.tick(a1.get(), - a2.get(), - a3.get(), - steering1.convert(), - &inv.fl, - &inv.fr, - &inv.rl, - &inv.rr, - data2.accelImplausible, - data2.brakeImplausible, - data2.accelPercent, - data2.brakePercent, - a1.get().conversions[MCU15_ACCEL1_CHANNEL], - a1.get().conversions[MCU15_ACCEL2_CHANNEL], - a1.get().conversions[MCU15_BRAKE1_CHANNEL], - a1.get().conversions[MCU15_BRAKE2_CHANNEL], - pedals_system.getMechBrakeActiveThreshold(), torque_controller_mux.get_pidtv_data()); } if (t.trigger100) // 100Hz @@ -339,6 +419,12 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) a1.tick(); a2.tick(); a3.tick(); + load_cell_interface.tick( + (LoadCellInterfaceTick_s){ + .FLConversion = a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL], + .FRConversion = a3.get().conversions[MCU15_FR_LOADCELL_CHANNEL], + .RLConversion = sab_interface.rlLoadCell.convert(), + .RRConversion = sab_interface.rrLoadCell.convert()}); } // // Untriggered main_ecu.read_mcu_status(); // should be executed at the same rate as state machine @@ -370,26 +456,39 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) // tick steering system steering_system.tick( - current_system_tick, - a1.get().conversions[MCU15_STEERING_CHANNEL]); + (SteeringSystemTick_s){ + .tick = current_system_tick, + .secondaryConversion = a1.get().conversions[MCU15_STEERING_CHANNEL]}); + + // Serial.println("Steering angle"); + // Serial.println(steering_system.getSteeringSystemData().angle); + // Serial.println("Steering status"); + // Serial.println(static_cast(steering_system.getSteeringSystemData().status)); // tick drivetrain system drivetrain.tick(current_system_tick); // // tick torque controller mux - // TODO is this correct? - auto wheel_angle_rad = DEG_TO_RAD * steering1.convert().angle; + DrivetrainCommand_s controller_output = case_system.evaluate( + current_system_tick, + vn_interface.get_vn_struct(), + steering_system.getSteeringSystemData(), + drivetrain.get_dynamic_data(), + load_cell_interface.getLoadCellForces().loadCellConversions, // should CASE use filtered load cells? + pedals_system.getPedalsSystemData(), + 0, + fsm.get_state(), + dashboard.startButtonPressed(), + 3); + torque_controller_mux.tick( current_system_tick, - drivetrain.get_current_data(), + drivetrain.get_dynamic_data(), pedals_system.getPedalsSystemData(), steering_system.getSteeringSystemData(), - a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL], // FL load cell reading. TODO: fix index - a3.get().conversions[MCU15_FR_LOADCELL_CHANNEL], // FR load cell reading. TODO: fix index - sab_interface.rlLoadCell.convert(), // RL load cell reading. TODO: get data from rear load cells - sab_interface.rrLoadCell.convert(), // RR load cell reading. TODO: get data from rear load cells + load_cell_interface.getLoadCellForces(), dashboard.getDialMode(), dashboard.torqueModeButtonPressed(), vn_interface.get_vn_struct(), - wheel_angle_rad); + controller_output); } diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index 74d9c81cc..4ac9fd06a 100644 --- a/test/test_systems/main.cpp +++ b/test/test_systems/main.cpp @@ -5,7 +5,7 @@ #include "torque_controller_mux_test.h" #include "drivetrain_system_test.h" #include "safety_system_test.h" - +#include "test_CASE.h" int main(int argc, char **argv) { diff --git a/test/test_systems/pedals_system_test.h b/test/test_systems/pedals_system_test.h index 0571496a6..afbb8a121 100644 --- a/test/test_systems/pedals_system_test.h +++ b/test/test_systems/pedals_system_test.h @@ -225,11 +225,39 @@ TEST(PedalsSystemTesting, test_accel_and_brake_pressed_at_same_time_and_activati EXPECT_TRUE(reset_pedals_system_implaus_time(pedals)); // test with real data (accel = ~37%) accel pressed - float accel1 = 2583; - float accel2 = 1068; - float brake = 1510; - pedals.setParams({ACCEL1_PEDAL_MIN, ACCEL2_PEDAL_MIN, ACCEL1_PEDAL_MAX, ACCEL2_PEDAL_MAX, APPS_ACTIVATION_PERCENTAGE, DEFAULT_PEDAL_DEADZONE, DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, APPS_ACTIVATION_PERCENTAGE}, - {BRAKE1_PEDAL_MIN, BRAKE2_PEDAL_MIN, BRAKE1_PEDAL_MAX, BRAKE2_PEDAL_MAX, BRAKE_ACTIVATION_PERCENTAGE, DEFAULT_PEDAL_DEADZONE, DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, BRAKE_MECH_THRESH}); + int accel1 = 2583; + int accel2 = 1068; + int brake = 1510; + pedals.setParams( + { + .min_pedal_1 = ACCEL1_PEDAL_MIN, + .min_pedal_2 = ACCEL2_PEDAL_MIN, + .max_pedal_1 = ACCEL1_PEDAL_MAX, + .max_pedal_2 = ACCEL2_PEDAL_MAX, + .min_sensor_pedal_1 = ACCEL1_PEDAL_OOR_MIN, + .min_sensor_pedal_2 = ACCEL2_PEDAL_OOR_MIN, + .max_sensor_pedal_1 = ACCEL1_PEDAL_OOR_MAX, + .max_sensor_pedal_2 = ACCEL2_PEDAL_OOR_MAX, + .activation_percentage = APPS_ACTIVATION_PERCENTAGE, + .deadzone_margin = DEFAULT_PEDAL_DEADZONE, + .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, + .mechanical_activation_percentage = APPS_ACTIVATION_PERCENTAGE, + }, + { + .min_pedal_1 = BRAKE1_PEDAL_MIN, + .min_pedal_2 = BRAKE2_PEDAL_MIN, + .max_pedal_1 = BRAKE1_PEDAL_MAX, + .max_pedal_2 = BRAKE2_PEDAL_MAX, + .min_sensor_pedal_1 = BRAKE1_PEDAL_OOR_MIN, + .min_sensor_pedal_2 = BRAKE2_PEDAL_OOR_MIN, + .max_sensor_pedal_1 = BRAKE1_PEDAL_OOR_MAX, + .max_sensor_pedal_2 = BRAKE2_PEDAL_OOR_MAX, + .activation_percentage = BRAKE_ACTIVATION_PERCENTAGE, + .deadzone_margin = DEFAULT_PEDAL_DEADZONE, + .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, + .mechanical_activation_percentage = BRAKE_MECH_THRESH, + } + ); float conv1 = get_pedal_conversion_val(ACCEL1_PEDAL_MIN, ACCEL1_PEDAL_MAX, accel1); float conv2 = get_pedal_conversion_val(ACCEL2_PEDAL_MIN, ACCEL2_PEDAL_MAX, accel2); float convb = get_pedal_conversion_val(BRAKE1_PEDAL_MIN, BRAKE1_PEDAL_MAX, brake); diff --git a/test/test_systems/test_CASE.h b/test/test_systems/test_CASE.h new file mode 100644 index 000000000..aa2c86d65 --- /dev/null +++ b/test/test_systems/test_CASE.h @@ -0,0 +1,60 @@ +#pragma once + +#include "TorqueControllers.h" +#include "CASESystem.h" +#include + +class FakeMessageQueue +{ +public: + FakeMessageQueue() {} +}; + +void enqueue_matlab_msg(FakeMessageQueue *, const CAN_MESSAGE_BUS &msg) +{ +} + +TEST(CASESystemTesting, test_vn_start_time) +{ + FakeMessageQueue faked_msg_q; + CASEConfiguration case_config = { + + .AbsoluteTorqueLimit = AMK_MAX_TORQUE, // N-m + .yaw_pid_p = 1.33369, + .yaw_pid_i = 0.25, + .yaw_pid_d = 0.0, + .tcs_pid_p = 40.0, + .tcs_pid_i = 0.0, + .tcs_pid_d = 0.0, + .useLaunch = false, + .usePIDTV = true, + .useTCSLimitedYawPID = true, + .useNormalForce = false, + .useTractionControl = true, + .usePowerLimit = false, + .usePIDPowerLimit = false, + .useDecoupledYawBrakes = false, + .useDiscontinuousYawPIDBrakes = true, + .tcsSLThreshold = 0.2, + .launchSL = 0.2, + .launchDeadZone = 20, // N-m + .launchVelThreshold = 0.75, // m/s + .tcsVelThreshold = 2.5, // m/s + .yawPIDMaxDifferential = 10, // N-m + .yawPIDErrorThreshold = 0.1, // rad/s + .yawPIDVelThreshold = 1, // m/s + .yawPIDCoastThreshold = 2.5, // m/s + .yaw_pid_brakes_p = 0.25, + .yaw_pid_brakes_i = 0, + .yaw_pid_brakes_d = 0, + .decoupledYawPIDBrakesMaxDIfference = 2, // N-m + .discontinuousBrakesPercentThreshold = 0.4, + .TorqueMode = AMK_MAX_TORQUE, // N-m + .RegenLimit = -10.0, // N-m + + .max_rpm = AMK_MAX_RPM, + .max_regen_torque = AMK_MAX_TORQUE, + .max_torque = AMK_MAX_TORQUE, + }; + CASESystem case_system(&faked_msg_q, 100, 70, case_config); +} diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h index 5283e3eec..7d6a0e4c2 100644 --- a/test/test_systems/torque_controller_mux_test.h +++ b/test/test_systems/torque_controller_mux_test.h @@ -107,14 +107,11 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) simulated_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -131,14 +128,11 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) simulated_drivetrain_dynamics, simulated_no_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); // Now press the pedal again. Torque should be requested @@ -147,14 +141,11 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) simulated_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -228,14 +219,11 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) simulated_fast_drivetrain_dynamics, simulated_no_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -252,14 +240,11 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) simulated_slow_drivetrain_dynamics, simulated_no_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); // Now press the pedal. Torque should be requested @@ -268,14 +253,11 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) simulated_slow_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -444,14 +426,11 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_slow_drivetrain_dynamics, simulated_no_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); // change mode to mode 3 @@ -460,14 +439,11 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_slow_drivetrain_dynamics, simulated_no_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); // tick again to calculate state switch @@ -476,14 +452,11 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_slow_drivetrain_dynamics, simulated_no_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); LaunchStates_e launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -496,14 +469,11 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_no_launch_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -516,14 +486,11 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_barely_launch_drivetrain_dynamics, simulated_no_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -536,14 +503,11 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_barely_launch_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -555,14 +519,11 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_barely_launch_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); DrivetrainCommand_s commanded = torque_controller_mux.getDrivetrainCommand(); @@ -575,14 +536,11 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_barely_launch_drivetrain_dynamics, simulated_accel_and_brake_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -676,14 +634,11 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_slow_drivetrain_dynamics, simulated_no_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); // change mode to mode 3 @@ -692,14 +647,11 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_slow_drivetrain_dynamics, simulated_no_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); // tick again to calculate state switch @@ -708,14 +660,11 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_slow_drivetrain_dynamics, simulated_no_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); LaunchStates_e launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -728,14 +677,11 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_barely_launch_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); // tick TCMUX (10k us will hit 100hz) @@ -744,14 +690,11 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_barely_launch_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -766,14 +709,11 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_barely_launch_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -791,14 +731,11 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_barely_launch_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); commanded = torque_controller_mux.getDrivetrainCommand(); @@ -814,14 +751,11 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_barely_launch_drivetrain_dynamics, simulated_full_accel_press, (const SteeringSystemData_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, - (const AnalogConversion_s) {}, + (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, false, vn_data, - 0.0 + (const DrivetrainCommand_s) {} ); commanded = torque_controller_mux.getDrivetrainCommand();