admin/sunnypilot-byd
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

sunnypilot v2026.002.000

Browse Source 
version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
github-actions[bot] committed 2026-07-21 07:44:02 +00:00
commit de77c89cf6
3995 files changed
+1260688
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

panda/board/body/bldc/BLDC_controller.c
+3306
View File
File diff suppressed because it is too large. Load diff
panda/board/body/bldc/BLDC_controller.h
+394
View File
@@ -0,0 +1,394 @@
/*
* File: BLDC_controller.h
*
* Code generated for Simulink model 'BLDC_controller'.
*
* Model version : 1.1297
* Simulink Coder version : 8.13 (R2017b) 24-Jul-2017
* C/C++ source code generated on : Sun Mar 6 11:02:11 2022
*
* Target selection: ert.tlc
* Embedded hardware selection: ARM Compatible->ARM Cortex
* Emulation hardware selection:
* Differs from embedded hardware (MATLAB Host)
* Code generation objectives:
* 1. Execution efficiency
* 2. RAM efficiency
* Validation result: Not run
*/
#ifndef RTW_HEADER_BLDC_controller_h_
#define RTW_HEADER_BLDC_controller_h_
#include "rtwtypes.h"
#ifndef BLDC_controller_COMMON_INCLUDES_
# define BLDC_controller_COMMON_INCLUDES_
#include "rtwtypes.h"
#endif /* BLDC_controller_COMMON_INCLUDES_ */
/* Macros for accessing real-time model data structure */
/* Forward declaration for rtModel */
typedef struct tag_RTM RT_MODEL;
/* Block signals and states (auto storage) for system '<S13>/Counter' */
typedef struct {
int16_T UnitDelay_DSTATE; /* '<S18>/UnitDelay' */
} DW_Counter;
/* Block signals and states (auto storage) for system '<S50>/Low_Pass_Filter' */
typedef struct {
int32_T UnitDelay1_DSTATE[2]; /* '<S56>/UnitDelay1' */
} DW_Low_Pass_Filter;
/* Block signals and states (auto storage) for system '<S25>/Counter' */
typedef struct {
uint16_T UnitDelay_DSTATE; /* '<S30>/UnitDelay' */
} DW_Counter_b;
/* Block signals and states (auto storage) for system '<S21>/either_edge' */
typedef struct {
boolean_T UnitDelay_DSTATE; /* '<S26>/UnitDelay' */
} DW_either_edge;
/* Block signals and states (auto storage) for system '<S20>/Debounce_Filter' */
typedef struct {
DW_either_edge either_edge_p; /* '<S21>/either_edge' */
DW_Counter_b Counter_e; /* '<S24>/Counter' */
DW_Counter_b Counter_n1; /* '<S25>/Counter' */
boolean_T UnitDelay_DSTATE; /* '<S21>/UnitDelay' */
} DW_Debounce_Filter;
/* Block signals and states (auto storage) for system '<S83>/I_backCalc_fixdt' */
typedef struct {
int32_T UnitDelay_DSTATE; /* '<S88>/UnitDelay' */
int32_T UnitDelay_DSTATE_m; /* '<S90>/UnitDelay' */
} DW_I_backCalc_fixdt;
/* Block signals and states (auto storage) for system '<S63>/PI_clamp_fixdt' */
typedef struct {
int32_T ResettableDelay_DSTATE; /* '<S77>/Resettable Delay' */
uint8_T icLoad; /* '<S77>/Resettable Delay' */
boolean_T UnitDelay1_DSTATE; /* '<S74>/UnitDelay1' */
} DW_PI_clamp_fixdt;
/* Block signals and states (auto storage) for system '<S61>/PI_clamp_fixdt' */
typedef struct {
int32_T ResettableDelay_DSTATE; /* '<S67>/Resettable Delay' */
uint8_T icLoad; /* '<S67>/Resettable Delay' */
boolean_T UnitDelay1_DSTATE; /* '<S65>/UnitDelay1' */
} DW_PI_clamp_fixdt_m;
/* Block signals and states (auto storage) for system '<S62>/PI_clamp_fixdt' */
typedef struct {
int16_T ResettableDelay_DSTATE; /* '<S72>/Resettable Delay' */
uint8_T icLoad; /* '<S72>/Resettable Delay' */
boolean_T UnitDelay1_DSTATE; /* '<S69>/UnitDelay1' */
} DW_PI_clamp_fixdt_g;
/* Block signals and states (auto storage) for system '<Root>' */
typedef struct {
DW_PI_clamp_fixdt_g PI_clamp_fixdt_kh;/* '<S62>/PI_clamp_fixdt' */
DW_PI_clamp_fixdt_m PI_clamp_fixdt_l4;/* '<S61>/PI_clamp_fixdt' */
DW_PI_clamp_fixdt PI_clamp_fixdt_i; /* '<S63>/PI_clamp_fixdt' */
DW_I_backCalc_fixdt I_backCalc_fixdt_j;/* '<S82>/I_backCalc_fixdt' */
DW_I_backCalc_fixdt I_backCalc_fixdt1;/* '<S83>/I_backCalc_fixdt1' */
DW_I_backCalc_fixdt I_backCalc_fixdt_i;/* '<S83>/I_backCalc_fixdt' */
DW_either_edge either_edge_i; /* '<S20>/either_edge' */
DW_Debounce_Filter Debounce_Filter_k;/* '<S20>/Debounce_Filter' */
DW_Low_Pass_Filter Low_Pass_Filter_m;/* '<S50>/Low_Pass_Filter' */
DW_Counter Counter_e; /* '<S13>/Counter' */
int32_T Divide1; /* '<S81>/Divide1' */
int32_T UnitDelay_DSTATE; /* '<S40>/UnitDelay' */
int16_T Gain4_e[3]; /* '<S57>/Gain4' */
int16_T DataTypeConversion[2]; /* '<S56>/Data Type Conversion' */
int16_T z_counterRawPrev; /* '<S17>/z_counterRawPrev' */
int16_T Merge; /* '<S59>/Merge' */
int16_T Switch1; /* '<S78>/Switch1' */
int16_T Vd_max1; /* '<S80>/Vd_max1' */
int16_T Gain3; /* '<S80>/Gain3' */
int16_T Vq_max_M1; /* '<S80>/Vq_max_M1' */
int16_T Gain5; /* '<S80>/Gain5' */
int16_T i_max; /* '<S80>/i_max' */
int16_T Divide1_n; /* '<S80>/Divide1' */
int16_T Gain1; /* '<S80>/Gain1' */
int16_T Gain4; /* '<S80>/Gain4' */
int16_T Switch2_i; /* '<S87>/Switch2' */
int16_T Switch2_o; /* '<S93>/Switch2' */
int16_T Switch2_a; /* '<S91>/Switch2' */
int16_T Divide3; /* '<S42>/Divide3' */
int16_T Merge1; /* '<S33>/Merge1' */
int16_T Abs1; /* '<S5>/Abs1' */
int16_T Abs5_h; /* '<S50>/Abs5' */
int16_T Divide11; /* '<S17>/Divide11' */
int16_T r_sin_M1; /* '<S52>/r_sin_M1' */
int16_T r_cos_M1; /* '<S52>/r_cos_M1' */
int16_T UnitDelay3_DSTATE; /* '<S13>/UnitDelay3' */
int16_T UnitDelay4_DSTATE; /* '<S17>/UnitDelay4' */
int16_T UnitDelay2_DSTATE; /* '<S17>/UnitDelay2' */
int16_T UnitDelay3_DSTATE_o; /* '<S17>/UnitDelay3' */
int16_T UnitDelay5_DSTATE; /* '<S17>/UnitDelay5' */
int16_T UnitDelay4_DSTATE_e; /* '<S13>/UnitDelay4' */
int16_T UnitDelay4_DSTATE_eu; /* '<S8>/UnitDelay4' */
int8_T Switch2_e; /* '<S12>/Switch2' */
int8_T UnitDelay2_DSTATE_b; /* '<S12>/UnitDelay2' */
int8_T If1_ActiveSubsystem; /* '<S7>/If1' */
int8_T If2_ActiveSubsystem; /* '<S7>/If2' */
int8_T If1_ActiveSubsystem_j; /* '<S47>/If1' */
int8_T SwitchCase_ActiveSubsystem; /* '<S59>/Switch Case' */
int8_T If1_ActiveSubsystem_a; /* '<S59>/If1' */
int8_T If1_ActiveSubsystem_o; /* '<S48>/If1' */
int8_T SwitchCase_ActiveSubsystem_d; /* '<S80>/Switch Case' */
int8_T If2_ActiveSubsystem_f; /* '<S33>/If2' */
int8_T If2_ActiveSubsystem_a; /* '<S45>/If2' */
uint8_T z_ctrlMod; /* '<S5>/F03_02_Control_Mode_Manager' */
uint8_T UnitDelay3_DSTATE_fy; /* '<S10>/UnitDelay3' */
uint8_T UnitDelay1_DSTATE; /* '<S10>/UnitDelay1' */
uint8_T UnitDelay2_DSTATE_f; /* '<S10>/UnitDelay2' */
uint8_T UnitDelay_DSTATE_e; /* '<S20>/UnitDelay' */
uint8_T is_active_c1_BLDC_controller;/* '<S5>/F03_02_Control_Mode_Manager' */
uint8_T is_c1_BLDC_controller; /* '<S5>/F03_02_Control_Mode_Manager' */
uint8_T is_ACTIVE; /* '<S5>/F03_02_Control_Mode_Manager' */
boolean_T Merge_p; /* '<S21>/Merge' */
boolean_T dz_cntTrnsDet; /* '<S17>/dz_cntTrnsDet' */
boolean_T UnitDelay2_DSTATE_c; /* '<S2>/UnitDelay2' */
boolean_T UnitDelay5_DSTATE_m; /* '<S2>/UnitDelay5' */
boolean_T UnitDelay6_DSTATE; /* '<S2>/UnitDelay6' */
boolean_T UnitDelay_DSTATE_b; /* '<S39>/UnitDelay' */
boolean_T UnitDelay1_DSTATE_n; /* '<S17>/UnitDelay1' */
boolean_T n_commDeacv_Mode; /* '<S13>/n_commDeacv' */
boolean_T dz_cntTrnsDet_Mode; /* '<S17>/dz_cntTrnsDet' */
} DW;
/* Constant parameters (auto storage) */
typedef struct {
/* Computed Parameter: r_sin_M1_Table
* Referenced by: '<S52>/r_sin_M1'
*/
int16_T r_sin_M1_Table[181];
/* Computed Parameter: r_cos_M1_Table
* Referenced by: '<S52>/r_cos_M1'
*/
int16_T r_cos_M1_Table[181];
/* Computed Parameter: r_sin3PhaA_M1_Table
* Referenced by: '<S96>/r_sin3PhaA_M1'
*/
int16_T r_sin3PhaA_M1_Table[181];
/* Computed Parameter: r_sin3PhaB_M1_Table
* Referenced by: '<S96>/r_sin3PhaB_M1'
*/
int16_T r_sin3PhaB_M1_Table[181];
/* Computed Parameter: r_sin3PhaC_M1_Table
* Referenced by: '<S96>/r_sin3PhaC_M1'
*/
int16_T r_sin3PhaC_M1_Table[181];
/* Computed Parameter: iq_maxSca_M1_Table
* Referenced by: '<S80>/iq_maxSca_M1'
*/
uint16_T iq_maxSca_M1_Table[50];
/* Computed Parameter: z_commutMap_M1_table
* Referenced by: '<S94>/z_commutMap_M1'
*/
int8_T z_commutMap_M1_table[18];
/* Computed Parameter: vec_hallToPos_Value
* Referenced by: '<S11>/vec_hallToPos'
*/
int8_T vec_hallToPos_Value[8];
} ConstP;
/* External inputs (root inport signals with auto storage) */
typedef struct {
boolean_T b_motEna; /* '<Root>/b_motEna' */
uint8_T z_ctrlModReq; /* '<Root>/z_ctrlModReq' */
int16_T r_inpTgt; /* '<Root>/r_inpTgt' */
uint8_T b_hallA; /* '<Root>/b_hallA ' */
uint8_T b_hallB; /* '<Root>/b_hallB' */
uint8_T b_hallC; /* '<Root>/b_hallC' */
int16_T i_phaAB; /* '<Root>/i_phaAB' */
int16_T i_phaBC; /* '<Root>/i_phaBC' */
int16_T i_DCLink; /* '<Root>/i_DCLink' */
int16_T a_mechAngle; /* '<Root>/a_mechAngle' */
} ExtU;
/* External outputs (root outports fed by signals with auto storage) */
typedef struct {
int16_T DC_phaA; /* '<Root>/DC_phaA' */
int16_T DC_phaB; /* '<Root>/DC_phaB' */
int16_T DC_phaC; /* '<Root>/DC_phaC' */
uint8_T z_errCode; /* '<Root>/z_errCode' */
int16_T n_mot; /* '<Root>/n_mot' */
int16_T a_elecAngle; /* '<Root>/a_elecAngle' */
int16_T iq; /* '<Root>/iq' */
int16_T id; /* '<Root>/id' */
} ExtY;
/* Parameters (auto storage) */
struct P_ {
int32_T dV_openRate; /* Variable: dV_openRate
* Referenced by: '<S37>/dV_openRate'
*/
int16_T dz_cntTrnsDetHi; /* Variable: dz_cntTrnsDetHi
* Referenced by: '<S17>/dz_cntTrnsDet'
*/
int16_T dz_cntTrnsDetLo; /* Variable: dz_cntTrnsDetLo
* Referenced by: '<S17>/dz_cntTrnsDet'
*/
int16_T n_cruiseMotTgt; /* Variable: n_cruiseMotTgt
* Referenced by: '<S61>/n_cruiseMotTgt'
*/
int16_T z_maxCntRst; /* Variable: z_maxCntRst
* Referenced by:
* '<S13>/Counter'
* '<S13>/z_maxCntRst'
* '<S13>/z_maxCntRst2'
* '<S13>/UnitDelay3'
* '<S17>/z_counter'
*/
uint16_T cf_speedCoef; /* Variable: cf_speedCoef
* Referenced by: '<S17>/cf_speedCoef'
*/
uint16_T t_errDequal; /* Variable: t_errDequal
* Referenced by: '<S20>/t_errDequal'
*/
uint16_T t_errQual; /* Variable: t_errQual
* Referenced by: '<S20>/t_errQual'
*/
int16_T Vd_max; /* Variable: Vd_max
* Referenced by:
* '<S36>/Vd_max'
* '<S80>/Vd_max1'
*/
int16_T Vq_max_M1[46]; /* Variable: Vq_max_M1
* Referenced by: '<S80>/Vq_max_M1'
*/
int16_T Vq_max_XA[46]; /* Variable: Vq_max_XA
* Referenced by: '<S80>/Vq_max_XA'
*/
int16_T a_phaAdvMax; /* Variable: a_phaAdvMax
* Referenced by: '<S42>/a_phaAdvMax'
*/
int16_T i_max; /* Variable: i_max
* Referenced by:
* '<S36>/i_max'
* '<S80>/i_max'
*/
int16_T id_fieldWeakMax; /* Variable: id_fieldWeakMax
* Referenced by: '<S42>/id_fieldWeakMax'
*/
int16_T n_commAcvLo; /* Variable: n_commAcvLo
* Referenced by: '<S13>/n_commDeacv'
*/
int16_T n_commDeacvHi; /* Variable: n_commDeacvHi
* Referenced by: '<S13>/n_commDeacv'
*/
int16_T n_fieldWeakAuthHi; /* Variable: n_fieldWeakAuthHi
* Referenced by: '<S42>/n_fieldWeakAuthHi'
*/
int16_T n_fieldWeakAuthLo; /* Variable: n_fieldWeakAuthLo
* Referenced by: '<S42>/n_fieldWeakAuthLo'
*/
int16_T n_max; /* Variable: n_max
* Referenced by:
* '<S36>/n_max'
* '<S80>/n_max1'
*/
int16_T n_stdStillDet; /* Variable: n_stdStillDet
* Referenced by: '<S13>/n_stdStillDet'
*/
int16_T r_errInpTgtThres; /* Variable: r_errInpTgtThres
* Referenced by: '<S20>/r_errInpTgtThres'
*/
int16_T r_fieldWeakHi; /* Variable: r_fieldWeakHi
* Referenced by: '<S42>/r_fieldWeakHi'
*/
int16_T r_fieldWeakLo; /* Variable: r_fieldWeakLo
* Referenced by: '<S42>/r_fieldWeakLo'
*/
uint16_T cf_KbLimProt; /* Variable: cf_KbLimProt
* Referenced by:
* '<S82>/cf_KbLimProt'
* '<S83>/cf_KbLimProt'
*/
uint16_T cf_idKp; /* Variable: cf_idKp
* Referenced by: '<S63>/cf_idKp1'
*/
uint16_T cf_iqKp; /* Variable: cf_iqKp
* Referenced by: '<S62>/cf_iqKp'
*/
uint16_T cf_nKp; /* Variable: cf_nKp
* Referenced by: '<S61>/cf_nKp'
*/
uint16_T cf_currFilt; /* Variable: cf_currFilt
* Referenced by: '<S50>/cf_currFilt'
*/
uint16_T cf_idKi; /* Variable: cf_idKi
* Referenced by: '<S63>/cf_idKi1'
*/
uint16_T cf_iqKi; /* Variable: cf_iqKi
* Referenced by: '<S62>/cf_iqKi'
*/
uint16_T cf_iqKiLimProt; /* Variable: cf_iqKiLimProt
* Referenced by:
* '<S81>/cf_iqKiLimProt'
* '<S83>/cf_iqKiLimProt'
*/
uint16_T cf_nKi; /* Variable: cf_nKi
* Referenced by: '<S61>/cf_nKi'
*/
uint16_T cf_nKiLimProt; /* Variable: cf_nKiLimProt
* Referenced by:
* '<S82>/cf_nKiLimProt'
* '<S83>/cf_nKiLimProt'
*/
uint8_T n_polePairs; /* Variable: n_polePairs
* Referenced by: '<S15>/n_polePairs'
*/
uint8_T z_ctrlTypSel; /* Variable: z_ctrlTypSel
* Referenced by: '<S1>/z_ctrlTypSel'
*/
uint8_T z_selPhaCurMeasABC; /* Variable: z_selPhaCurMeasABC
* Referenced by: '<S49>/z_selPhaCurMeasABC'
*/
boolean_T b_angleMeasEna; /* Variable: b_angleMeasEna
* Referenced by:
* '<S3>/b_angleMeasEna'
* '<S13>/b_angleMeasEna'
*/
boolean_T b_cruiseCtrlEna; /* Variable: b_cruiseCtrlEna
* Referenced by: '<S1>/b_cruiseCtrlEna'
*/
boolean_T b_diagEna; /* Variable: b_diagEna
* Referenced by: '<S4>/b_diagEna'
*/
boolean_T b_fieldWeakEna; /* Variable: b_fieldWeakEna
* Referenced by:
* '<S6>/b_fieldWeakEna'
* '<S97>/b_fieldWeakEna'
*/
};
/* Parameters (auto storage) */
typedef struct P_ P;
/* Real-time Model Data Structure */
struct tag_RTM {
P *defaultParam;
ExtU *inputs;
ExtY *outputs;
DW *dwork;
};
/* Constant parameters (auto storage) */
extern const ConstP rtConstP;
/* Model entry point functions */
extern void BLDC_controller_initialize(RT_MODEL *const rtM);
extern void BLDC_controller_step(RT_MODEL *const rtM);
#endif
panda/board/body/bldc/BLDC_controller_data.c
+386
View File
@@ -0,0 +1,386 @@
/*
* File: BLDC_controller_data.c
*
* Code generated for Simulink model 'BLDC_controller'.
*
* Model version : 1.1297
* Simulink Coder version : 8.13 (R2017b) 24-Jul-2017
* C/C++ source code generated on : Sun Mar 6 11:02:11 2022
*
* Target selection: ert.tlc
* Embedded hardware selection: ARM Compatible->ARM Cortex
* Emulation hardware selection:
* Differs from embedded hardware (MATLAB Host)
* Code generation objectives:
* 1. Execution efficiency
* 2. RAM efficiency
* Validation result: Not run
*/
#include "BLDC_controller.h"
/* Constant parameters (auto storage) */
const ConstP rtConstP = {
/* Computed Parameter: r_sin_M1_Table
* Referenced by: '<S52>/r_sin_M1'
*/
{ 8192, 8682, 9162, 9630, 10087, 10531, 10963, 11381, 11786, 12176, 12551,
12911, 13255, 13583, 13894, 14189, 14466, 14726, 14968, 15191, 15396, 15582,
15749, 15897, 16026, 16135, 16225, 16294, 16344, 16374, 16384, 16374, 16344,
16294, 16225, 16135, 16026, 15897, 15749, 15582, 15396, 15191, 14968, 14726,
14466, 14189, 13894, 13583, 13255, 12911, 12551, 12176, 11786, 11381, 10963,
10531, 10087, 9630, 9162, 8682, 8192, 7692, 7182, 6664, 6138, 5604, 5063,
4516, 3964, 3406, 2845, 2280, 1713, 1143, 572, 0, -572, -1143, -1713, -2280,
-2845, -3406, -3964, -4516, -5063, -5604, -6138, -6664, -7182, -7692, -8192,
-8682, -9162, -9630, -10087, -10531, -10963, -11381, -11786, -12176, -12551,
-12911, -13255, -13583, -13894, -14189, -14466, -14726, -14968, -15191,
-15396, -15582, -15749, -15897, -16026, -16135, -16225, -16294, -16344,
-16374, -16384, -16374, -16344, -16294, -16225, -16135, -16026, -15897,
-15749, -15582, -15396, -15191, -14968, -14726, -14466, -14189, -13894,
-13583, -13255, -12911, -12551, -12176, -11786, -11381, -10963, -10531,
-10087, -9630, -9162, -8682, -8192, -7692, -7182, -6664, -6138, -5604, -5063,
-4516, -3964, -3406, -2845, -2280, -1713, -1143, -572, 0, 572, 1143, 1713,
2280, 2845, 3406, 3964, 4516, 5063, 5604, 6138, 6664, 7182, 7692, 8192 },
/* Computed Parameter: r_cos_M1_Table
* Referenced by: '<S52>/r_cos_M1'
*/
{ 14189, 13894, 13583, 13255, 12911, 12551, 12176, 11786, 11381, 10963, 10531,
10087, 9630, 9162, 8682, 8192, 7692, 7182, 6664, 6138, 5604, 5063, 4516,
3964, 3406, 2845, 2280, 1713, 1143, 572, 0, -572, -1143, -1713, -2280, -2845,
-3406, -3964, -4516, -5063, -5604, -6138, -6664, -7182, -7692, -8192, -8682,
-9162, -9630, -10087, -10531, -10963, -11381, -11786, -12176, -12551, -12911,
-13255, -13583, -13894, -14189, -14466, -14726, -14968, -15191, -15396,
-15582, -15749, -15897, -16026, -16135, -16225, -16294, -16344, -16374,
-16384, -16374, -16344, -16294, -16225, -16135, -16026, -15897, -15749,
-15582, -15396, -15191, -14968, -14726, -14466, -14189, -13894, -13583,
-13255, -12911, -12551, -12176, -11786, -11381, -10963, -10531, -10087,
-9630, -9162, -8682, -8192, -7692, -7182, -6664, -6138, -5604, -5063, -4516,
-3964, -3406, -2845, -2280, -1713, -1143, -572, 0, 572, 1143, 1713, 2280,
2845, 3406, 3964, 4516, 5063, 5604, 6138, 6664, 7182, 7692, 8192, 8682, 9162,
9630, 10087, 10531, 10963, 11381, 11786, 12176, 12551, 12911, 13255, 13583,
13894, 14189, 14466, 14726, 14968, 15191, 15396, 15582, 15749, 15897, 16026,
16135, 16225, 16294, 16344, 16374, 16384, 16374, 16344, 16294, 16225, 16135,
16026, 15897, 15749, 15582, 15396, 15191, 14968, 14726, 14466, 14189 },
/* Computed Parameter: r_sin3PhaA_M1_Table
* Referenced by: '<S96>/r_sin3PhaA_M1'
*/
{ -13091, -13634, -14126, -14565, -14953, -15289, -15577, -15816, -16009,
-16159, -16269, -16340, -16377, -16383, -16362, -16317, -16253, -16172,
-16079, -15977, -15870, -15762, -15656, -15555, -15461, -15377, -15306,
-15248, -15206, -15180, -15172, -15180, -15206, -15248, -15306, -15377,
-15461, -15555, -15656, -15762, -15870, -15977, -16079, -16172, -16253,
-16317, -16362, -16383, -16377, -16340, -16269, -16159, -16009, -15816,
-15577, -15289, -14953, -14565, -14126, -13634, -13091, -12496, -11849,
-11154, -10411, -9623, -8791, -7921, -7014, -6075, -5107, -4115, -3104,
-2077, -1041, 0, 1041, 2077, 3104, 4115, 5107, 6075, 7014, 7921, 8791, 9623,
10411, 11154, 11849, 12496, 13091, 13634, 14126, 14565, 14953, 15289, 15577,
15816, 16009, 16159, 16269, 16340, 16377, 16383, 16362, 16317, 16253, 16172,
16079, 15977, 15870, 15762, 15656, 15555, 15461, 15377, 15306, 15248, 15206,
15180, 15172, 15180, 15206, 15248, 15306, 15377, 15461, 15555, 15656, 15762,
15870, 15977, 16079, 16172, 16253, 16317, 16362, 16383, 16377, 16340, 16269,
16159, 16009, 15816, 15577, 15289, 14953, 14565, 14126, 13634, 13091, 12496,
11849, 11154, 10411, 9623, 8791, 7921, 7014, 6075, 5107, 4115, 3104, 2077,
1041, 0, -1041, -2077, -3104, -4115, -5107, -6075, -7014, -7921, -8791,
-9623, -10411, -11154, -11849, -12496, -13091 },
/* Computed Parameter: r_sin3PhaB_M1_Table
* Referenced by: '<S96>/r_sin3PhaB_M1'
*/
{ 15172, 15180, 15206, 15248, 15306, 15377, 15461, 15555, 15656, 15762, 15870,
15977, 16079, 16172, 16253, 16317, 16362, 16383, 16377, 16340, 16269, 16159,
16009, 15816, 15577, 15289, 14953, 14565, 14126, 13634, 13091, 12496, 11849,
11154, 10411, 9623, 8791, 7921, 7014, 6075, 5107, 4115, 3104, 2077, 1041, 0,
-1041, -2077, -3104, -4115, -5107, -6075, -7014, -7921, -8791, -9623, -10411,
-11154, -11849, -12496, -13091, -13634, -14126, -14565, -14953, -15289,
-15577, -15816, -16009, -16159, -16269, -16340, -16377, -16383, -16362,
-16317, -16253, -16172, -16079, -15977, -15870, -15762, -15656, -15555,
-15461, -15377, -15306, -15248, -15206, -15180, -15172, -15180, -15206,
-15248, -15306, -15377, -15461, -15555, -15656, -15762, -15870, -15977,
-16079, -16172, -16253, -16317, -16362, -16383, -16377, -16340, -16269,
-16159, -16009, -15816, -15577, -15289, -14953, -14565, -14126, -13634,
-13091, -12496, -11849, -11154, -10411, -9623, -8791, -7921, -7014, -6075,
-5107, -4115, -3104, -2077, -1041, 0, 1041, 2077, 3104, 4115, 5107, 6075,
7014, 7921, 8791, 9623, 10411, 11154, 11849, 12496, 13091, 13634, 14126,
14565, 14953, 15289, 15577, 15816, 16009, 16159, 16269, 16340, 16377, 16383,
16362, 16317, 16253, 16172, 16079, 15977, 15870, 15762, 15656, 15555, 15461,
15377, 15306, 15248, 15206, 15180, 15172 },
/* Computed Parameter: r_sin3PhaC_M1_Table
* Referenced by: '<S96>/r_sin3PhaC_M1'
*/
{ -13091, -12496, -11849, -11154, -10411, -9623, -8791, -7921, -7014, -6075,
-5107, -4115, -3104, -2077, -1041, 0, 1041, 2077, 3104, 4115, 5107, 6075,
7014, 7921, 8791, 9623, 10411, 11154, 11849, 12496, 13091, 13634, 14126,
14565, 14953, 15289, 15577, 15816, 16009, 16159, 16269, 16340, 16377, 16383,
16362, 16317, 16253, 16172, 16079, 15977, 15870, 15762, 15656, 15555, 15461,
15377, 15306, 15248, 15206, 15180, 15172, 15180, 15206, 15248, 15306, 15377,
15461, 15555, 15656, 15762, 15870, 15977, 16079, 16172, 16253, 16317, 16362,
16383, 16377, 16340, 16269, 16159, 16009, 15816, 15577, 15289, 14953, 14565,
14126, 13634, 13091, 12496, 11849, 11154, 10411, 9623, 8791, 7921, 7014,
6075, 5107, 4115, 3104, 2077, 1041, 0, -1041, -2077, -3104, -4115, -5107,
-6075, -7014, -7921, -8791, -9623, -10411, -11154, -11849, -12496, -13091,
-13634, -14126, -14565, -14953, -15289, -15577, -15816, -16009, -16159,
-16269, -16340, -16377, -16383, -16362, -16317, -16253, -16172, -16079,
-15977, -15870, -15762, -15656, -15555, -15461, -15377, -15306, -15248,
-15206, -15180, -15172, -15180, -15206, -15248, -15306, -15377, -15461,
-15555, -15656, -15762, -15870, -15977, -16079, -16172, -16253, -16317,
-16362, -16383, -16377, -16340, -16269, -16159, -16009, -15816, -15577,
-15289, -14953, -14565, -14126, -13634, -13091 },
/* Computed Parameter: iq_maxSca_M1_Table
* Referenced by: '<S80>/iq_maxSca_M1'
*/
{ 65535U, 65523U, 65484U, 65418U, 65326U, 65207U, 65062U, 64890U, 64691U,
64465U, 64211U, 63930U, 63620U, 63281U, 62913U, 62516U, 62088U, 61630U,
61140U, 60618U, 60062U, 59473U, 58848U, 58187U, 57489U, 56752U, 55974U,
55155U, 54291U, 53381U, 52422U, 51413U, 50349U, 49227U, 48043U, 46792U,
45470U, 44069U, 42581U, 40997U, 39307U, 37494U, 35541U, 33422U, 31105U,
28540U, 25655U, 22323U, 18304U, 12974U },
/* Computed Parameter: z_commutMap_M1_table
* Referenced by: '<S94>/z_commutMap_M1'
*/
{ -1, 1, 0, -1, 0, 1, 0, -1, 1, 1, -1, 0, 1, 0, -1, 0, 1, -1 },
/* Computed Parameter: vec_hallToPos_Value
* Referenced by: '<S11>/vec_hallToPos'
*/
{ 0, 2, 0, 1, 4, 3, 5, 0 }
};
P rtP_Left = {
/* Variable: dV_openRate
* Referenced by: '<S37>/dV_openRate'
*/
12288,
/* Variable: dz_cntTrnsDetHi
* Referenced by: '<S17>/dz_cntTrnsDet'
*/
40,
/* Variable: dz_cntTrnsDetLo
* Referenced by: '<S17>/dz_cntTrnsDet'
*/
20,
/* Variable: n_cruiseMotTgt
* Referenced by: '<S61>/n_cruiseMotTgt'
*/
0,
/* Variable: z_maxCntRst
* Referenced by:
* '<S13>/Counter'
* '<S13>/z_maxCntRst'
* '<S13>/z_maxCntRst2'
* '<S13>/UnitDelay3'
* '<S17>/z_counter'
*/
2000,
/* Variable: cf_speedCoef
* Referenced by: '<S17>/cf_speedCoef'
*/
5334U,
/* Variable: t_errDequal
* Referenced by: '<S20>/t_errDequal'
*/
9600U,
/* Variable: t_errQual
* Referenced by: '<S20>/t_errQual'
*/
1280U,
/* Variable: Vd_max
* Referenced by:
* '<S36>/Vd_max'
* '<S80>/Vd_max1'
*/
14400,
/* Variable: Vq_max_M1
* Referenced by: '<S80>/Vq_max_M1'
*/
{ 14400, 14396, 14386, 14368, 14343, 14311, 14271, 14225, 14171, 14109, 14040,
13963, 13879, 13786, 13685, 13576, 13459, 13333, 13198, 13053, 12900, 12736,
12562, 12377, 12181, 11973, 11753, 11520, 11273, 11011, 10733, 10438, 10124,
9790, 9433, 9051, 8640, 8196, 7713, 7184, 6597, 5935, 5170, 4245, 3019, 0 },
/* Variable: Vq_max_XA
* Referenced by: '<S80>/Vq_max_XA'
*/
{ 0, 320, 640, 960, 1280, 1600, 1920, 2240, 2560, 2880, 3200, 3520, 3840, 4160,
4480, 4800, 5120, 5440, 5760, 6080, 6400, 6720, 7040, 7360, 7680, 8000, 8320,
8640, 8960, 9280, 9600, 9920, 10240, 10560, 10880, 11200, 11520, 11840,
12160, 12480, 12800, 13120, 13440, 13760, 14080, 14400 },
/* Variable: a_phaAdvMax
* Referenced by: '<S42>/a_phaAdvMax'
*/
400,
/* Variable: i_max
* Referenced by:
* '<S36>/i_max'
* '<S80>/i_max'
*/
30000,
/* Variable: id_fieldWeakMax
* Referenced by: '<S42>/id_fieldWeakMax'
*/
4000,
/* Variable: n_commAcvLo
* Referenced by: '<S13>/n_commDeacv'
*/
240,
/* Variable: n_commDeacvHi
* Referenced by: '<S13>/n_commDeacv'
*/
480,
/* Variable: n_fieldWeakAuthHi
* Referenced by: '<S42>/n_fieldWeakAuthHi'
*/
6400,
/* Variable: n_fieldWeakAuthLo
* Referenced by: '<S42>/n_fieldWeakAuthLo'
*/
4800,
/* Variable: n_max
* Referenced by:
* '<S36>/n_max'
* '<S80>/n_max1'
*/
32000,
/* Variable: n_stdStillDet
* Referenced by: '<S13>/n_stdStillDet'
*/
48,
/* Variable: r_errInpTgtThres
* Referenced by: '<S20>/r_errInpTgtThres'
*/
9600,
/* Variable: r_fieldWeakHi
* Referenced by: '<S42>/r_fieldWeakHi'
*/
16000,
/* Variable: r_fieldWeakLo
* Referenced by: '<S42>/r_fieldWeakLo'
*/
12000,
/* Variable: cf_KbLimProt
* Referenced by:
* '<S82>/cf_KbLimProt'
* '<S83>/cf_KbLimProt'
*/
768U,
/* Variable: cf_idKp
* Referenced by: '<S63>/cf_idKp1'
*/
1638U,
/* Variable: cf_iqKp
* Referenced by: '<S62>/cf_iqKp'
*/
2458U,
/* Variable: cf_nKp
* Referenced by: '<S61>/cf_nKp'
*/
9666U,
/* Variable: cf_currFilt
* Referenced by: '<S50>/cf_currFilt'
*/
7864U,
/* Variable: cf_idKi
* Referenced by: '<S63>/cf_idKi1'
*/
1474U,
/* Variable: cf_iqKi
* Referenced by: '<S62>/cf_iqKi'
*/
2458U,
/* Variable: cf_iqKiLimProt
* Referenced by:
* '<S81>/cf_iqKiLimProt'
* '<S83>/cf_iqKiLimProt'
*/
1474U,
/* Variable: cf_nKi
* Referenced by: '<S61>/cf_nKi'
*/
502U,
/* Variable: cf_nKiLimProt
* Referenced by:
* '<S82>/cf_nKiLimProt'
* '<S83>/cf_nKiLimProt'
*/
492U,
/* Variable: n_polePairs
* Referenced by: '<S15>/n_polePairs'
*/
15U,
/* Variable: z_ctrlTypSel
* Referenced by: '<S1>/z_ctrlTypSel'
*/
2U,
/* Variable: z_selPhaCurMeasABC
* Referenced by: '<S49>/z_selPhaCurMeasABC'
*/
0U,
/* Variable: b_angleMeasEna
* Referenced by:
* '<S3>/b_angleMeasEna'
* '<S13>/b_angleMeasEna'
*/
0,
/* Variable: b_cruiseCtrlEna
* Referenced by: '<S1>/b_cruiseCtrlEna'
*/
0,
/* Variable: b_diagEna
* Referenced by: '<S4>/b_diagEna'
*/
1,
/* Variable: b_fieldWeakEna
* Referenced by:
* '<S6>/b_fieldWeakEna'
* '<S97>/b_fieldWeakEna'
*/
0
}; /* Modifiable parameters */
/*
* File trailer for generated code.
*
* [EOF]
*/
panda/board/body/bldc/bldc.h
+311
View File
@@ -0,0 +1,311 @@
#ifndef BLDC_H
#define BLDC_H
#include "board/body/bldc/bldc_defs.h"
#include "board/body/boards/board_declarations.h"
#include <stdint.h>
#include <stdbool.h>
#include "board/stm32h7/lladc.h"
// Matlab includes and defines - from auto-code generation
#include "BLDC_controller.h" /* Model's header file */
#include "BLDC_controller.c"
#include "BLDC_controller_data.c"
#include "rtwtypes.h"
static RT_MODEL rtM_Left_Obj;
static RT_MODEL rtM_Right_Obj;
RT_MODEL *const rtM_Left = &rtM_Left_Obj;
RT_MODEL *const rtM_Right = &rtM_Right_Obj;
static DW rtDW_Left; /* Observable states */
static ExtU rtU_Left; /* External inputs */
static ExtY rtY_Left; /* External outputs */
extern P rtP_Left; // This is defined in BLDC_controller_data.c
static DW rtDW_Right; /* Observable states */
static ExtU rtU_Right; /* External inputs */
static ExtY rtY_Right; /* External outputs */
static P rtP_Right; /* Parameters */
//static int16_t curDC_max = (I_DC_MAX * A2BIT_CONV);
static int16_t curL_phaA = 0, curL_phaC = 0, curL_DC = 0;
static int16_t curR_phaA = 0, curR_phaC = 0, curR_DC = 0;
volatile uint16_t batt_voltage_raw = 0;
volatile uint16_t batt_percentage = 0;
volatile int rpm_left = 0;
volatile int rpm_right = 0;
volatile bool enable_motors = 0; // initially motors are disabled for safety
static bool enableFin = 0;
static const uint16_t pwm_res = ( (uint32_t)CORE_FREQ * 1000000U / 2U ) / PWM_FREQ;
static uint16_t offsetcount = 0;
static uint32_t offsetrlA = 0;
static uint32_t offsetrlC = 0;
static uint32_t offsetrrA = 0;
static uint32_t offsetrrC = 0;
static uint32_t offsetdcl = 0;
static uint32_t offsetdcr = 0;
#define ADC_CHANNEL_BLDC(a, c) {.adc = (a), .channel = (c), .sample_time = SAMPLETIME_16_CYCLES, .oversampling = OVERSAMPLING_4}
const adc_signal_t adc_curL_phaA = ADC_CHANNEL_BLDC(ADC2, 10);
const adc_signal_t adc_curL_phaC = ADC_CHANNEL_BLDC(ADC2, 11);
const adc_signal_t adc_curL_DC = ADC_CHANNEL_BLDC(ADC2, 18);
const adc_signal_t adc_curR_phaA = ADC_CHANNEL_BLDC(ADC1, 7);
const adc_signal_t adc_curR_phaC = ADC_CHANNEL_BLDC(ADC1, 15);
const adc_signal_t adc_curR_DC = ADC_CHANNEL_BLDC(ADC1, 5);
const adc_signal_t adc_batVoltage = ADC_CHANNEL_BLDC(ADC1, 4);
void motor_set_enable(bool enable) {
enable_motors = enable;
}
float motor_encoder_get_speed_rpm(uint8_t motor) {
float speed_rpm = 0.0f;
if (motor == BODY_MOTOR_LEFT) {
speed_rpm = (float)rtY_Left.n_mot;
} else if (motor == BODY_MOTOR_RIGHT) {
speed_rpm = (float)rtY_Right.n_mot;
}
if (ABS(speed_rpm) < RPM_DEADBAND) {
speed_rpm = 0.0f;
}
return speed_rpm;
}
void bldc_init(void) {
adc_init(ADC1);
adc_init(ADC2);
// Initialize Hall Sensors for Left Motor (PB6, PB7, PB8)
set_gpio_mode(GPIOB, 6, MODE_INPUT); set_gpio_pullup(GPIOB, 6, PULL_UP);
set_gpio_mode(GPIOB, 7, MODE_INPUT); set_gpio_pullup(GPIOB, 7, PULL_UP);
set_gpio_mode(GPIOB, 8, MODE_INPUT); set_gpio_pullup(GPIOB, 8, PULL_UP);
// Initialize Hall Sensors for Right Motor (PA0, PA1, PA2)
set_gpio_mode(GPIOA, 0, MODE_INPUT); set_gpio_pullup(GPIOA, 0, PULL_UP);
set_gpio_mode(GPIOA, 1, MODE_INPUT); set_gpio_pullup(GPIOA, 1, PULL_UP);
set_gpio_mode(GPIOA, 2, MODE_INPUT); set_gpio_pullup(GPIOA, 2, PULL_UP);
// Setup the model pointers for Left motor
rtM_Left->defaultParam = &rtP_Left;
rtM_Left->inputs = &rtU_Left;
rtM_Left->outputs = &rtY_Left;
rtM_Left->dwork = &rtDW_Left;
BLDC_controller_initialize(rtM_Left);
/* Set BLDC controller parameters */
rtP_Left.b_angleMeasEna = 0; // Motor angle input: 0 = estimated angle, 1 = measured angle
rtP_Left.z_selPhaCurMeasABC = 2; // Left motor measured current phases {Green, Blue} = {iA, iB}
rtP_Left.z_ctrlTypSel = CTRL_TYP_SEL;
rtP_Left.b_diagEna = DIAG_ENA;
rtP_Left.i_max = (int16_t)(I_MOT_MAX * A2BIT_CONV) << 4;
rtP_Left.n_max = N_MOT_MAX << 4;
rtP_Left.b_fieldWeakEna = FIELD_WEAK_ENA;
rtP_Left.id_fieldWeakMax = (int16_t)(FIELD_WEAK_MAX * A2BIT_CONV) << 4;
rtP_Left.a_phaAdvMax = PHASE_ADV_MAX << 4;
rtP_Left.r_fieldWeakHi = FIELD_WEAK_HI << 4;
rtP_Left.r_fieldWeakLo = FIELD_WEAK_LO << 4;
rtP_Left.z_maxCntRst = 4000;
rtP_Left.cf_speedCoef = CF_SPEED_COEF;
rtP_Left.t_errQual = 1280U; // 80ms at 16kHz loop rate
rtP_Left.t_errDequal = T_ERR_DEQUAL_CYCLES;
// Setup the model pointers for Right motor
rtP_Right = rtP_Left; // copy parameters
// Right motor measured current phases A & C (based on adc_curR_phaA/C definitions)
rtP_Right.z_selPhaCurMeasABC = 2;
rtM_Right->defaultParam = &rtP_Right;
rtM_Right->inputs = &rtU_Right;
rtM_Right->outputs = &rtY_Right;
rtM_Right->dwork = &rtDW_Right;
BLDC_controller_initialize(rtM_Right);
// Initialize GPIOs for Motor Control
// Left Motor (TIM1): PE8(CH1N), PE9(CH1), PE10(CH2N), PE11(CH2), PE12(CH3N), PE13(CH3)
set_gpio_alternate(GPIOE, 8, GPIO_AF1_TIM1);
set_gpio_alternate(GPIOE, 9, GPIO_AF1_TIM1);
set_gpio_alternate(GPIOE, 10, GPIO_AF1_TIM1);
set_gpio_alternate(GPIOE, 11, GPIO_AF1_TIM1);
set_gpio_alternate(GPIOE, 12, GPIO_AF1_TIM1);
set_gpio_alternate(GPIOE, 13, GPIO_AF1_TIM1);
// Right Motor (TIM8): PC6(CH1), PC7(CH2), PC8(CH3), PA5(CH1N), PB14(CH2N), PB15(CH3N)
set_gpio_alternate(GPIOC, 6, GPIO_AF3_TIM8);
set_gpio_alternate(GPIOC, 7, GPIO_AF3_TIM8);
set_gpio_alternate(GPIOC, 8, GPIO_AF3_TIM8);
set_gpio_alternate(GPIOA, 5, GPIO_AF3_TIM8);
set_gpio_alternate(GPIOB, 14, GPIO_AF3_TIM8);
set_gpio_alternate(GPIOB, 15, GPIO_AF3_TIM8);
// --- LEFT MOTOR (TIM1) ---
// TIM8 is an advanced control timer. We configure it for 3-phase center-aligned PWM.
LEFT_TIM->PSC = 0;
LEFT_TIM->ARR = pwm_res; // Set auto-reload register for PWM_FREQ
LEFT_TIM->CR1 = TIM_CR1_CMS_0; // Center-aligned mode 1
LEFT_TIM->RCR = 1; // Update event once per 2 PWM periods (16kHz)
// Configure channel 1, 2, 3 as PWM mode 1, with preload enabled
LEFT_TIM->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE | \
TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2PE;
LEFT_TIM->CCMR2 = TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE;
// Enable complementary outputs for channels 1, 2, 3
LEFT_TIM->CCER = TIM_CCER_CC1E | TIM_CCER_CC1NE | \
TIM_CCER_CC2E | TIM_CCER_CC2NE | \
TIM_CCER_CC3E | TIM_CCER_CC3NE;
// --- RIGHT MOTOR (TIM8) ---
RIGHT_TIM->PSC = 0;
RIGHT_TIM->ARR = pwm_res;
RIGHT_TIM->CR1 = TIM_CR1_CMS_0;
RIGHT_TIM->RCR = 1;
RIGHT_TIM->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE | \
TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2PE;
RIGHT_TIM->CCMR2 = TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE;
RIGHT_TIM->CCER = TIM_CCER_CC1E | TIM_CCER_CC1NE | \
TIM_CCER_CC2E | TIM_CCER_CC2NE | \
TIM_CCER_CC3E | TIM_CCER_CC3NE;
// Set dead time (20 cycles -> ~166ns with 120MHz clock) and enable motor outputs
LEFT_TIM->BDTR = 20U | TIM_BDTR_MOE;
RIGHT_TIM->BDTR = 20U | TIM_BDTR_MOE;
// Generate an update event to load the registers
LEFT_TIM->EGR = TIM_EGR_UG;
RIGHT_TIM->EGR = TIM_EGR_UG;
// Enable TIM8 update interrupt for bldc_step
LEFT_TIM->DIER |= TIM_DIER_UIE;
// Start the timers
LEFT_TIM->CR1 |= TIM_CR1_CEN;
RIGHT_TIM->CR1 |= TIM_CR1_CEN;
}
void bldc_step(void) {
// Calibrate ADC offsets for the first few cycles
if(offsetcount < 2000) { // calibrate ADC offsets
offsetcount++;
uint32_t rawL_A = adc_get_raw(&adc_curL_phaA);
uint32_t rawL_C = adc_get_raw(&adc_curL_phaC);
uint32_t rawR_A = adc_get_raw(&adc_curR_phaA);
uint32_t rawR_C = adc_get_raw(&adc_curR_phaC);
uint32_t rawL_DC = adc_get_raw(&adc_curL_DC);
uint32_t rawR_DC = adc_get_raw(&adc_curR_DC);
if (offsetcount == 1) {
offsetrlA = rawL_A; offsetrlC = rawL_C;
offsetrrA = rawR_A; offsetrrC = rawR_C;
offsetdcl = rawL_DC; offsetdcr = rawR_DC;
} else {
offsetrlA = (rawL_A + offsetrlA) / 2;
offsetrlC = (rawL_C + offsetrlC) / 2;
offsetrrA = (rawR_A + offsetrrA) / 2;
offsetrrC = (rawR_C + offsetrrC) / 2;
offsetdcl = (rawL_DC + offsetdcl) / 2;
offsetdcr = (rawR_DC + offsetdcr) / 2;
}
return;
}
// Get Left motor currents
curL_phaA = (int16_t)(((int32_t)offsetrlA - (int32_t)adc_get_raw(&adc_curL_phaA)) >> 5);
curL_phaC = (int16_t)(((int32_t)offsetrlC - (int32_t)adc_get_raw(&adc_curL_phaC)) >> 5);
curL_DC = (int16_t)(((int32_t)offsetdcl - (int32_t)adc_get_raw(&adc_curL_DC)) >> 4);
// Get Right motor currents
curR_phaA = (int16_t)(((int32_t)offsetrrA - (int32_t)adc_get_raw(&adc_curR_phaA)) >> 5);
curR_phaC = (int16_t)(((int32_t)offsetrrC - (int32_t)adc_get_raw(&adc_curR_phaC)) >> 5);
curR_DC = (int16_t)(((int32_t)offsetdcr - (int32_t)adc_get_raw(&adc_curR_DC)) >> 4);
// Safety: Don't enable if offsets are bogus (e.g. ADC failed)
if (offsetrrA == 0 || offsetrrC == 0 || !enable_motors) {
enableFin = 0;
} else {
enableFin = 1;
}
// Read Hall Sensors
rtU_Left.b_hallA = !((GPIOB->IDR >> 6) & 1);
rtU_Left.b_hallB = !((GPIOB->IDR >> 7) & 1);
rtU_Left.b_hallC = !((GPIOB->IDR >> 8) & 1);
rtU_Right.b_hallA = !((GPIOA->IDR >> 0) & 1);
rtU_Right.b_hallB = !((GPIOA->IDR >> 1) & 1);
rtU_Right.b_hallC = !((GPIOA->IDR >> 2) & 1);
if (!enableFin) {
LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
} else {
LEFT_TIM->BDTR |= TIM_BDTR_MOE;
RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
}
// read battery voltage
batt_voltage_raw = adc_get_raw(&adc_batVoltage);
int16_t batVoltageCalib = batt_voltage_raw * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC;
batt_percentage = 100 - (((420 * BAT_CELLS) - batVoltageCalib) / BAT_CELLS / VOLTS_PER_PERCENT / 100);
// ========================= LEFT MOTOR ===========================
rtU_Left.b_motEna = enableFin;
rtU_Left.z_ctrlModReq = CTRL_MOD_REQ; // Speed Mode
int deadband_rpm_left = rpm_left;
if (ABS(deadband_rpm_left) < RPM_DEADBAND) {
deadband_rpm_left = 0;
}
rtU_Left.r_inpTgt = (CLAMP((int)deadband_rpm_left, -MAX_RPM, MAX_RPM) * RPM_TO_UNIT);
rtU_Left.i_phaAB = curL_phaA;
rtU_Left.i_phaBC = curL_phaC;
rtU_Left.i_DCLink = curL_DC;
BLDC_controller_step(rtM_Left);
int ul = rtY_Left.DC_phaA;
int vl = rtY_Left.DC_phaB;
int wl = rtY_Left.DC_phaC;
LEFT_TIM->CCR1 = (uint16_t)CLAMP((ul + pwm_res / 2), PWM_MARGIN, pwm_res - PWM_MARGIN);
LEFT_TIM->CCR2 = (uint16_t)CLAMP((vl + pwm_res / 2), PWM_MARGIN, pwm_res - PWM_MARGIN);
LEFT_TIM->CCR3 = (uint16_t)CLAMP((wl + pwm_res / 2), PWM_MARGIN, pwm_res - PWM_MARGIN);
// ========================= RIGHT MOTOR ===========================
rtU_Right.b_motEna = enableFin;
rtU_Right.z_ctrlModReq = CTRL_MOD_REQ; // Speed Mode
int deadband_rpm_right = rpm_right;
if (ABS(deadband_rpm_right) < RPM_DEADBAND) {
deadband_rpm_right = 0;
}
rtU_Right.r_inpTgt = -(CLAMP((int)deadband_rpm_right, -MAX_RPM, MAX_RPM) * RPM_TO_UNIT);
rtU_Right.i_phaAB = curR_phaA;
rtU_Right.i_phaBC = curR_phaC;
rtU_Right.i_DCLink = curR_DC;
BLDC_controller_step(rtM_Right);
int ur = rtY_Right.DC_phaA;
int vr = rtY_Right.DC_phaB;
int wr = rtY_Right.DC_phaC;
RIGHT_TIM->CCR1 = (uint16_t)CLAMP((ur + pwm_res / 2), PWM_MARGIN, pwm_res - PWM_MARGIN);
RIGHT_TIM->CCR2 = (uint16_t)CLAMP((vr + pwm_res / 2), PWM_MARGIN, pwm_res - PWM_MARGIN);
RIGHT_TIM->CCR3 = (uint16_t)CLAMP((wr + pwm_res / 2), PWM_MARGIN, pwm_res - PWM_MARGIN);
}
#endif
panda/board/body/bldc/bldc_defs.h
+56
View File
@@ -0,0 +1,56 @@
#ifndef BLDC_DEFS_H
#define BLDC_DEFS_H
#include "stm32h7xx.h" // For GPIO_TypeDef
#define COM_CTRL 0 // [-] Commutation Control Type
#define SIN_CTRL 1 // [-] Sinusoidal Control Type
#define FOC_CTRL 2 // [-] Field Oriented Control (FOC) Type
#define LEFT_TIM TIM8
#define RIGHT_TIM TIM1
#define PWM_FREQ 32000
#define PWM_MARGIN 100
#define CF_SPEED_COEF (PWM_FREQ / 3)
#define MAX_RPM 1000
#define RPM_TO_UNIT 16
#define RPM_DEADBAND 1
#define BODY_MOTOR_LEFT 1U
#define BODY_MOTOR_RIGHT 2U
// Stall detection recovery time. Time to wait after a stall before re-enabling controls.
#define STALL_DEQUAL_TIME_MS 3000
#define T_ERR_DEQUAL_CYCLES (uint16_t)(STALL_DEQUAL_TIME_MS * (PWM_FREQ / 2) / 1000)
// Motor
#define I_DC_MAX 6
#define I_MOT_MAX 6
#define A2BIT_CONV 310
#define N_MOT_MAX 1000
// Control selections
#define CTRL_TYP_SEL FOC_CTRL // [-] Control type selection: COM_CTRL, SIN_CTRL, FOC_CTRL (default)
#define CTRL_MOD_REQ SPD_MODE // [-] Control mode request: OPEN_MODE, VLT_MODE (default), SPD_MODE, TRQ_MODE. Note: SPD_MODE and TRQ_MODE are only available for CTRL_FOC!
#define DIAG_ENA 1 // [-] Motor Diagnostics enable flag: 0 = Disabled, 1 = Enabled (default)
// Field Weakening / Phase Advance
#define FIELD_WEAK_ENA 1 // [-] Field Weakening / Phase Advance enable flag: 0 = Disabled (default), 1 = Enabled
#define FIELD_WEAK_MAX 5 // [A] Maximum Field Weakening D axis current (only for FOC).
#define PHASE_ADV_MAX 25 // [deg] Maximum Phase Advance angle (only for SIN).
#define FIELD_WEAK_HI 1000 // Input target High threshold
#define FIELD_WEAK_LO 750 // Input target Low threshold
// Battery configuration
#define BAT_CELLS 3 // 3 sets in series
#define BAT_CELL_FULL_MV 4200U // mV per cell at 100%
#define BAT_CELL_EMPTY_MV 3386U // mV per cell at 0% (from V1: 4200 - 100 * 8.14)
#define VOLTS_PER_PERCENT 0.00814 // Volts per percent, for conversion of volts to percentage
#define BAT_CALIB_REAL_VOLTAGE 1260U // multimeter voltage
#define BAT_CALIB_ADC 1275U // adc reading voltage
void bldc_init(void);
void bldc_step(void);
#endif
panda/board/body/bldc/rtwtypes.h
+104
View File
@@ -0,0 +1,104 @@
/*
* File: rtwtypes.h
*
* Code generated for Simulink model 'BLDC_controller'.
*
* Model version : 1.1297
* Simulink Coder version : 8.13 (R2017b) 24-Jul-2017
* C/C++ source code generated on : Sun Mar 6 11:02:11 2022
*
* Target selection: ert.tlc
* Embedded hardware selection: ARM Compatible->ARM Cortex
* Emulation hardware selection:
* Differs from embedded hardware (MATLAB Host)
* Code generation objectives:
* 1. Execution efficiency
* 2. RAM efficiency
* 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: MATLAB Host
* Number of bits: char: 8 short: 16 int: 32
* long: 32 long long: 64
* 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 long long int64_T;
typedef unsigned long long uint64_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, ulonglong_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 unsigned long long ulonglong_T;
typedef char char_T;
typedef unsigned char uchar_T;
typedef char_T byte_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))
#define MAX_int64_T ((int64_T)(9223372036854775807LL))
#define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL))
#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
/* Block D-Work pointer type */
typedef void * pointer_T;
#endif /* RTWTYPES_H */
/*
* File trailer for generated code.
*
* [EOF]
*/