mawei/panda-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/panda.git synced 2026-06-08 07:45:00 +08:00
Code Issues Packages Projects Releases Wiki Activity

Body v2: Hoverboard Motors (#2308)

Browse Source 
* body

* hoverboard motors

* pass test

* body ignition exception and remove used comms

Co-authored-by: rexblade21 <zkhuang4968@gmail.com>

* send extra empty message to distinguish fingerprint

* can.h body v2 put safety hook = 1

* change v2 extra id to 0x396

* fix mirsa style error

* move ignition exception to seperate bus 2 if statement

* silence heartbeat siren on body

* turn ignition off on switch repress

* 0x250 interception in panda fw

* send extra body data, same as v1 body

* cleanup data passing for more predictable inputs and lower rpm deadband

* remove extra downscaling

* change v2 fingerprint from 0x002 to 0x222

* clean up, bus 0 ignition exception and trq mode

* keep in SPD mode for now

* remove comment

* clean

* apply suggestions

---------

Co-authored-by: stefpi <19478336+stefpi@users.noreply.github.com>
This commit is contained in:
Jason Huang
2026-04-08 12:17:01 -04:00
committed by GitHub
parent 14b1906563
commit 7c0c1d956b
19 changed files with 5031 additions and 543 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
board/body/__init__.py
View File

@@ -8,35 +8,37 @@ class PandaBody(Panda):
MOTOR_LEFT: int = 1
MOTOR_RIGHT: int = 2
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self._rpm_left: int = 0
self._rpm_right: int = 0
@property
def rpm_left(self) -> int:
return self._rpm_left
@rpm_left.setter
def rpm_left(self, value: int) -> None:
self._rpm_left = int(value)
self._handle.controlWrite(Panda.REQUEST_OUT, 0xb3, self._rpm_left, self._rpm_right, b'')
@property
def rpm_right(self) -> int:
return self._rpm_right
@rpm_right.setter
def rpm_right(self, value: int) -> None:
self._rpm_right = int(value)
self._handle.controlWrite(Panda.REQUEST_OUT, 0xb3, self._rpm_left, self._rpm_right, b'')
# ****************** Motor Control *****************
@staticmethod
def _ensure_valid_motor(motor: int) -> None:
if motor not in (PandaBody.MOTOR_LEFT, PandaBody.MOTOR_RIGHT):
raise ValueError("motor must be MOTOR_LEFT or MOTOR_RIGHT")
def motor_set_speed(self, motor: int, speed: int) -> None:
self._ensure_valid_motor(motor)
assert -100 <= speed <= 100, "speed must be between -100 and 100"
speed_param = speed if speed >= 0 else (256 + speed)
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe0, motor, speed_param, b'')
def motor_set_target_rpm(self, motor: int, rpm: float) -> None:
self._ensure_valid_motor(motor)
target_deci_rpm = int(round(rpm * 10.0))
assert -32768 <= target_deci_rpm <= 32767, "target rpm out of range (-3276.8 to 3276.7)"
target_param = target_deci_rpm if target_deci_rpm >= 0 else (target_deci_rpm + (1 << 16))
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe4, motor, target_param, b'')
def motor_stop(self, motor: int) -> None:
self._ensure_valid_motor(motor)
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe1, motor, 0, b'')
def motor_get_encoder_state(self, motor: int) -> tuple[int, float]:
self._ensure_valid_motor(motor)
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xe2, motor, 0, 8)
position, rpm_milli = struct.unpack("<ii", dat)
return position, rpm_milli / 1000.0
def motor_reset_encoder(self, motor: int) -> None:
self._ensure_valid_motor(motor)
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe3, motor, 0, b'')

3306
board/body/bldc/BLDC_controller.c Normal file
View File

File diff suppressed because it is too large Load Diff

394
board/body/bldc/BLDC_controller.h Normal file
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

386
board/body/bldc/BLDC_controller_data.c Normal file
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]
*/

311
board/body/bldc/bldc.h Normal file
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

56
board/body/bldc/bldc_defs.h Normal file
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

104
board/body/bldc/rtwtypes.h Normal file
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]
*/

53
board/body/boards/board_body.h
View File

@@ -1,21 +1,46 @@
#include "board/body/motor_control.h"
#include "board/body/boards/board_declarations.h"
void board_body_init(void) {
motor_init();
motor_encoder_init();
motor_speed_controller_init();
motor_encoder_reset(1);
motor_encoder_reset(2);
// Initialize CAN pins
set_gpio_pullup(GPIOD, 0, PULL_NONE);
set_gpio_alternate(GPIOD, 0, GPIO_AF9_FDCAN1);
set_gpio_pullup(GPIOD, 1, PULL_NONE);
set_gpio_alternate(GPIOD, 1, GPIO_AF9_FDCAN1);
set_gpio_pullup(CAN_RX_PORT, CAN_RX_PIN, PULL_NONE);
set_gpio_alternate(CAN_RX_PORT, CAN_RX_PIN, GPIO_AF9_FDCAN1);
set_gpio_pullup(CAN_TX_PORT, CAN_TX_PIN, PULL_NONE);
set_gpio_alternate(CAN_TX_PORT, CAN_TX_PIN, GPIO_AF9_FDCAN1);
// Initialize button input (PC15)
set_gpio_mode(IGNITION_SW_PORT, IGNITION_SW_PIN, MODE_INPUT);
SYSCFG->EXTICR[3] &= ~(SYSCFG_EXTICR4_EXTI15);
SYSCFG->EXTICR[3] |= SYSCFG_EXTICR4_EXTI15_PC;
EXTI->PR1 = (1U << 15);
EXTI->RTSR1 |= (1U << 15);
EXTI->FTSR1 |= (1U << 15);
EXTI->IMR1 |= (1U << 15);
// Initialize barrel jack detection input (PC13)
set_gpio_pullup(CHARGING_DETECT_PORT, CHARGING_DETECT_PIN, PULL_UP);
set_gpio_mode(CHARGING_DETECT_PORT, CHARGING_DETECT_PIN, MODE_INPUT);
SYSCFG->EXTICR[3] &= ~(SYSCFG_EXTICR4_EXTI13);
SYSCFG->EXTICR[3] |= SYSCFG_EXTICR4_EXTI13_PC;
EXTI->PR1 = (1U << 13);
EXTI->RTSR1 |= (1U << 13);
EXTI->FTSR1 |= (1U << 13);
EXTI->IMR1 |= (1U << 13);
// Initialize and turn on mici power
set_gpio_mode(OBDC_POWER_ON_PORT, OBDC_POWER_ON_PIN, MODE_OUTPUT);
set_gpio_output(OBDC_POWER_ON_PORT, OBDC_POWER_ON_PIN, 1);
// Initialize and turn off gpu power
set_gpio_mode(GPU_POWER_ON_PORT, GPU_POWER_ON_PIN, MODE_OUTPUT);
set_gpio_output(GPU_POWER_ON_PORT, GPU_POWER_ON_PIN, 0);
// Initialize and turn off ignition output
set_gpio_mode(OBDC_IGNITION_ON_PORT, OBDC_IGNITION_ON_PIN, MODE_OUTPUT);
set_gpio_output(OBDC_IGNITION_ON_PORT, OBDC_IGNITION_ON_PIN, 0);
}
board board_body = {
.led_GPIO = {GPIOC, GPIOC, GPIOC},
.led_pin = {7, 7, 7},
.led_GPIO = {GPIOA, GPIOA, GPIOA},
.led_pin = {10, 10, 10},
.init = board_body_init,
};
};

34
board/body/boards/board_declarations.h
View File

@@ -2,6 +2,7 @@
#include <stdint.h>
#include <stdbool.h>
#include "board/body/bldc/bldc_defs.h"
// ******************** Prototypes ********************
typedef void (*board_init)(void);
@@ -19,3 +20,36 @@ struct board {
// ******************* Definitions ********************
#define HW_TYPE_BODY 0xB1U
// Pin definitions
// CAN
#define CAN_TX_PORT GPIOD
#define CAN_TX_PIN 1
#define CAN_RX_PORT GPIOD
#define CAN_RX_PIN 0
#define CAN_TRANSCEIVER_EN_PORT GPIOD
#define CAN_TRANSCEIVER_EN_PIN 12
// Ignition and charging detection
#define IGNITION_SW_PORT GPIOC
#define IGNITION_SW_PIN 15
#define CHARGING_DETECT_PORT GPIOC
#define CHARGING_DETECT_PIN 13
// Dotstar LED
#define DOTSTAR_CLK_PORT GPIOB
#define DOTSTAR_CLK_PIN 3
#define DOTSTAR_DATA_PORT GPIOB
#define DOTSTAR_DATA_PIN 5
// Mici Power On
#define OBDC_POWER_ON_PORT GPIOB
#define OBDC_POWER_ON_PIN 12
// GPU Power On
#define GPU_POWER_ON_PORT GPIOD
#define GPU_POWER_ON_PIN 8
// Ignition On
#define OBDC_IGNITION_ON_PORT GPIOB
#define OBDC_IGNITION_ON_PIN 11

102
board/body/can.h
View File

@@ -5,65 +5,95 @@
#include "board/can.h"
#include "board/health.h"
#include "board/body/motor_control.h"
#include "board/body/boards/board_declarations.h"
#include "board/drivers/drivers.h"
#include "opendbc/safety/declarations.h"
#include "board/body/bldc/bldc.h"
#define BODY_CAN_ADDR_MOTOR_SPEED 0x201U
#define BODY_CAN_MOTOR_SPEED_PERIOD_US 10000U
#define BODY_BUS_NUMBER 0U
#define BODY_CAN_ADDR_MOTOR_SPEED 0x201U
#define BODY_CAN_ADDR_VAR_VALUES 0x202U
#define BODY_CAN_ADDR_BODY_DATA 0x203U
#define BODY_CAN_ADDR_V2_ID 0x222U
#define BODY_CAN_MOTOR_SPEED_PERIOD_US 10000U
#define BODY_CAN_CMD_TIMEOUT_US 100000U
#define BODY_BUS_NUMBER 0U
static struct {
bool pending;
int32_t left_target_deci_rpm;
int32_t right_target_deci_rpm;
} body_can_command;
static uint32_t last_can_cmd_timestamp_us = 0U;
static uint16_t counter = 0U;
void body_can_send_motor_speeds(uint8_t bus, float left_speed_rpm, float right_speed_rpm) {
CANPacket_t pkt;
CANPacket_t pkt = {0};
pkt.bus = bus;
pkt.addr = BODY_CAN_ADDR_MOTOR_SPEED;
pkt.returned = 0;
pkt.rejected = 0;
pkt.extended = 0;
pkt.fd = 0;
pkt.data_len_code = 8;
int16_t left_speed_deci = left_speed_rpm * 10;
int16_t right_speed_deci = right_speed_rpm * 10;
int16_t left_speed_deci = left_speed_rpm;
int16_t right_speed_deci = -(right_speed_rpm);
pkt.data[0] = (uint8_t)((left_speed_deci >> 8) & 0xFFU);
pkt.data[1] = (uint8_t)(left_speed_deci & 0xFFU);
pkt.data[2] = (uint8_t)((right_speed_deci >> 8) & 0xFFU);
pkt.data[3] = (uint8_t)(right_speed_deci & 0xFFU);
pkt.data[4] = 0U;
pkt.data[5] = 0U;
pkt.data[6] = 0U;
pkt.data[7] = 0U;
pkt.data[6] = counter & 0xFFU;
can_set_checksum(&pkt);
can_send(&pkt, bus, true);
counter++;
}
void body_can_send_var_values(uint8_t bus, bool ignition, bool enable_motors, uint8_t fault, uint8_t left_z_errcode, uint8_t right_z_errcode) {
CANPacket_t pkt = {0};
pkt.bus = bus;
pkt.addr = BODY_CAN_ADDR_VAR_VALUES;
pkt.data_len_code = 3;
pkt.data[0] = (ignition ? 1U : 0U) | ((enable_motors ? 1U : 0U) << 1U) | ((fault & 0x3FU) << 2U);
pkt.data[1] = left_z_errcode;
pkt.data[2] = right_z_errcode;
can_set_checksum(&pkt);
can_send(&pkt, bus, true);
}
void body_can_send_body_data(uint8_t bus, uint8_t mcu_temp_raw, uint16_t batt_voltage_raw, uint8_t batt_percentage, bool charger_connected) {
CANPacket_t pkt = {0};
pkt.bus = bus;
pkt.addr = BODY_CAN_ADDR_BODY_DATA;
pkt.data_len_code = 4;
pkt.data[0] = mcu_temp_raw;
pkt.data[1] = (uint8_t)((batt_voltage_raw >> 8) & 0xFFU);
pkt.data[2] = (uint8_t)(batt_voltage_raw & 0xFFU);
pkt.data[3] = (charger_connected ? 1U : 0U) | ((batt_percentage & 0x7FU) << 1U);
can_set_checksum(&pkt);
can_send(&pkt, bus, true);
}
void body_can_process_target(int16_t left_target_deci_rpm, int16_t right_target_deci_rpm) {
body_can_command.left_target_deci_rpm = (int32_t)left_target_deci_rpm;
body_can_command.right_target_deci_rpm = (int32_t)right_target_deci_rpm;
body_can_command.pending = true;
rpm_left = (int)(((float)left_target_deci_rpm) * 0.1f);
rpm_right = (int)(((float)right_target_deci_rpm) * 0.1f);
last_can_cmd_timestamp_us = microsecond_timer_get();
}
void body_can_rx(CANPacket_t *msg) {
if ((msg->addr == 0x250U) && (GET_LEN(msg) >= 4U)) {
int16_t left_target_deci_rpm = (int16_t)((msg->data[0] << 8U) | msg->data[1]);
int16_t right_target_deci_rpm = (int16_t)((msg->data[2] << 8U) | msg->data[3]);
body_can_process_target(left_target_deci_rpm, right_target_deci_rpm);
}
}
void body_can_init(void) {
body_can_command.pending = false;
last_can_cmd_timestamp_us = 0U;
can_silent = false;
can_loopback = false;
(void)set_safety_hooks(SAFETY_BODY, 0U);
set_gpio_output(GPIOD, 2U, 0); // Enable CAN transceiver
set_gpio_output(CAN_TRANSCEIVER_EN_PORT, CAN_TRANSCEIVER_EN_PIN, 0); // Enable CAN transceiver
can_init_all();
}
void body_can_periodic(uint32_t now) {
if (body_can_command.pending) {
body_can_command.pending = false;
float left_target_rpm = ((float)body_can_command.left_target_deci_rpm) * 0.1f;
float right_target_rpm = ((float)body_can_command.right_target_deci_rpm) * 0.1f;
motor_speed_controller_set_target_rpm(BODY_MOTOR_LEFT, left_target_rpm);
motor_speed_controller_set_target_rpm(BODY_MOTOR_RIGHT, right_target_rpm);
void body_can_periodic(uint32_t now, bool ignition, bool plug_charging) {
if ((last_can_cmd_timestamp_us != 0U) &&
((now - last_can_cmd_timestamp_us) >= BODY_CAN_CMD_TIMEOUT_US)) {
rpm_left = 0;
rpm_right = 0;
last_can_cmd_timestamp_us = 0U;
}
static uint32_t last_motor_speed_tx_us = 0;
@@ -71,6 +101,18 @@ void body_can_periodic(uint32_t now) {
float left_speed_rpm = motor_encoder_get_speed_rpm(BODY_MOTOR_LEFT);
float right_speed_rpm = motor_encoder_get_speed_rpm(BODY_MOTOR_RIGHT);
body_can_send_motor_speeds(BODY_BUS_NUMBER, left_speed_rpm, right_speed_rpm);
body_can_send_var_values(BODY_BUS_NUMBER, ignition, enable_motors, 0U, rtY_Left.z_errCode, rtY_Right.z_errCode);
body_can_send_body_data(BODY_BUS_NUMBER, 0U, batt_voltage_raw, batt_percentage, plug_charging);
// Send message on 0x222 to identify as body v2
CANPacket_t id_pkt = {0};
id_pkt.bus = BODY_BUS_NUMBER;
id_pkt.addr = BODY_CAN_ADDR_V2_ID;
id_pkt.data_len_code = 1;
id_pkt.data[0] = 1U;
can_set_checksum(&id_pkt);
can_send(&id_pkt, BODY_BUS_NUMBER, true);
last_motor_speed_tx_us = now;
}
}

210
board/body/dotstar.h Normal file
View File

@@ -0,0 +1,210 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "board/config.h"
#include "board/body/boards/board_declarations.h"
#define DOTSTAR_LED_COUNT 10U
#define DOTSTAR_GLOBAL_BRIGHTNESS_MAX 31U
typedef struct {
uint8_t r;
uint8_t g;
uint8_t b;
} dotstar_rgb_t;
typedef struct {
bool initialized;
uint8_t global_brightness;
dotstar_rgb_t pixels[DOTSTAR_LED_COUNT];
} dotstar_state_t;
static dotstar_state_t dotstar_state = {
.initialized = false,
.global_brightness = DOTSTAR_GLOBAL_BRIGHTNESS_MAX,
};
static inline void dotstar_set_clk(bool high) {
if (high) {
DOTSTAR_CLK_PORT->BSRR = (uint32_t)(1U << DOTSTAR_CLK_PIN);
} else {
DOTSTAR_CLK_PORT->BSRR = (uint32_t)(1U << (DOTSTAR_CLK_PIN + 16U));
}
}
static inline void dotstar_set_data(bool high) {
if (high) {
DOTSTAR_DATA_PORT->BSRR = (uint32_t)(1U << DOTSTAR_DATA_PIN);
} else {
DOTSTAR_DATA_PORT->BSRR = (uint32_t)(1U << (DOTSTAR_DATA_PIN + 16U));
}
}
static inline void dotstar_write_byte(uint8_t value) {
for (int8_t bit = 7; bit >= 0; bit--) {
dotstar_set_data(((value >> bit) & 0x1U) != 0U);
delay(15);
dotstar_set_clk(true);
delay(15);
dotstar_set_clk(false);
delay(15);
}
}
static inline uint16_t dotstar_latch_len(uint16_t led_count) {
uint16_t len = (uint16_t)((led_count + 15U) / 16U);
if (len < led_count) {
len = led_count;
}
return (len < 4U) ? 4U : len;
}
static inline void dotstar_send_start_frame(void) {
for (uint8_t i = 0U; i < 4U; i++) {
dotstar_write_byte(0x00U);
}
}
static inline void dotstar_send_end_frame(uint16_t led_count) {
uint16_t latch_len = dotstar_latch_len(led_count);
for (uint16_t i = 0U; i < latch_len; i++) {
dotstar_write_byte(0xFFU);
}
}
static inline void dotstar_show(void) {
if (!dotstar_state.initialized) {
return;
}
dotstar_send_start_frame();
uint8_t prefix = (uint8_t)(0xE0U | dotstar_state.global_brightness);
for (uint16_t i = 0U; i < DOTSTAR_LED_COUNT; i++) {
dotstar_rgb_t *pixel = &dotstar_state.pixels[i];
dotstar_write_byte(prefix);
dotstar_write_byte(pixel->b);
dotstar_write_byte(pixel->g);
dotstar_write_byte(pixel->r);
}
dotstar_send_end_frame(DOTSTAR_LED_COUNT);
}
static inline void dotstar_init(void) {
set_gpio_pullup(DOTSTAR_CLK_PORT, DOTSTAR_CLK_PIN, PULL_NONE);
set_gpio_output_type(DOTSTAR_CLK_PORT, DOTSTAR_CLK_PIN, OUTPUT_TYPE_PUSH_PULL);
set_gpio_mode(DOTSTAR_CLK_PORT, DOTSTAR_CLK_PIN, MODE_OUTPUT);
register_set(&(DOTSTAR_CLK_PORT->OSPEEDR), GPIO_OSPEEDR_OSPEED5, GPIO_OSPEEDR_OSPEED5_Msk);
dotstar_set_clk(false);
set_gpio_pullup(DOTSTAR_DATA_PORT, DOTSTAR_DATA_PIN, PULL_NONE);
set_gpio_output_type(DOTSTAR_DATA_PORT, DOTSTAR_DATA_PIN, OUTPUT_TYPE_PUSH_PULL);
set_gpio_mode(DOTSTAR_DATA_PORT, DOTSTAR_DATA_PIN, MODE_OUTPUT);
register_set(&(DOTSTAR_DATA_PORT->OSPEEDR), GPIO_OSPEEDR_OSPEED5, GPIO_OSPEEDR_OSPEED5_Msk);
dotstar_set_data(false);
dotstar_state.initialized = true;
dotstar_state.global_brightness = DOTSTAR_GLOBAL_BRIGHTNESS_MAX;
for (uint16_t i = 0U; i < DOTSTAR_LED_COUNT; i++) {
dotstar_state.pixels[i].r = 0U;
dotstar_state.pixels[i].g = 0U;
dotstar_state.pixels[i].b = 0U;
}
dotstar_show();
}
static inline void dotstar_set_pixel(uint16_t index, uint8_t r, uint8_t g, uint8_t b) {
if (!dotstar_state.initialized || (index >= DOTSTAR_LED_COUNT)) {
return;
}
dotstar_state.pixels[index].r = r;
dotstar_state.pixels[index].g = g;
dotstar_state.pixels[index].b = b;
}
static inline void dotstar_fill(uint8_t r, uint8_t g, uint8_t b) {
if (!dotstar_state.initialized) {
return;
}
for (uint16_t i = 0U; i < DOTSTAR_LED_COUNT; i++) {
dotstar_state.pixels[i].r = r;
dotstar_state.pixels[i].g = g;
dotstar_state.pixels[i].b = b;
}
}
static inline void dotstar_set_global_brightness(uint8_t brightness) {
dotstar_state.global_brightness = (brightness > DOTSTAR_GLOBAL_BRIGHTNESS_MAX) ? DOTSTAR_GLOBAL_BRIGHTNESS_MAX : brightness;
}
static inline void dotstar_hue_to_rgb(uint16_t hue, uint8_t *r, uint8_t *g, uint8_t *b) {
hue %= 765U;
if (hue < 255U) {
*r = (uint8_t)(255U - hue);
*g = (uint8_t)hue;
*b = 0U;
} else if (hue < 510U) {
hue -= 255U;
*r = 0U;
*g = (uint8_t)(255U - hue);
*b = (uint8_t)hue;
} else {
hue -= 510U;
*r = (uint8_t)hue;
*g = 0U;
*b = (uint8_t)(255U - hue);
}
}
static inline void dotstar_run_rainbow(uint32_t now_us) {
uint32_t brightness_phase = (now_us / 40000U) % 62U;
uint8_t brightness = (brightness_phase <= 31U) ? (uint8_t)(brightness_phase + 1U) : (uint8_t)(62U - brightness_phase);
if (brightness == 0U) {
brightness = 1U;
}
dotstar_set_global_brightness(brightness);
uint32_t base_hue = (now_us / 10000U) % 765U;
for (uint16_t i = 0U; i < DOTSTAR_LED_COUNT; i++) {
uint16_t hue = (uint16_t)((base_hue + (i * 70U)) % 765U);
uint8_t r, g, b;
dotstar_hue_to_rgb(hue, &r, &g, &b);
dotstar_set_pixel(i, r, g, b);
}
}
static inline void dotstar_apply_breathe(dotstar_rgb_t color, uint32_t now_us, uint32_t cycle_us) {
if (!dotstar_state.initialized) {
return;
}
if (cycle_us == 0U) {
dotstar_set_global_brightness(DOTSTAR_GLOBAL_BRIGHTNESS_MAX);
dotstar_fill(color.r, color.g, color.b);
return;
}
uint32_t phase = now_us % cycle_us;
uint32_t half_cycle = cycle_us / 2U;
if (half_cycle == 0U) {
half_cycle = 1U;
}
uint32_t amplitude = (phase <= half_cycle) ? phase : (cycle_us - phase);
uint32_t scale = (amplitude * 255U) / half_cycle;
if (scale > 255U) {
scale = 255U;
}
uint8_t r = (uint8_t)((color.r * scale) / 255U);
uint8_t g = (uint8_t)((color.g * scale) / 255U);
uint8_t b = (uint8_t)((color.b * scale) / 255U);
dotstar_set_global_brightness(DOTSTAR_GLOBAL_BRIGHTNESS_MAX);
dotstar_fill(r, g, b);
}

77
board/body/main.c
View File

@@ -7,17 +7,23 @@
#include "board/drivers/usb.h"
#include "board/early_init.h"
#include "board/obj/gitversion.h"
#include "board/body/motor_control.h"
#include "board/body/can.h"
#include "opendbc/safety/safety.h"
#include "board/drivers/can_common.h"
#include "board/drivers/fdcan.h"
#include "board/can_comms.h"
#include "board/body/dotstar.h"
#include "bldc/bldc.h"
extern int _app_start[0xc000];
#include "board/body/main_comms.h"
static volatile uint32_t tick_count = 0U;
static volatile uint32_t ignition_press_timestamp_us = 0U;
static volatile bool ignition = false;
static volatile bool plug_charging = false;
void debug_ring_callback(uart_ring *ring) {
char rcv;
while (get_char(ring, &rcv)) {
@@ -34,7 +40,12 @@ void __initialize_hardware_early(void) {
early_initialization();
}
volatile uint32_t tick_count = 0;
void bldc_tim8_handler(void) {
if ((LEFT_TIM->SR & TIM_SR_UIF) != 0) {
LEFT_TIM->SR = ~TIM_SR_UIF;
bldc_step();
}
}
void tick_handler(void) {
if (TICK_TIMER->SR != 0) {
@@ -49,6 +60,32 @@ void tick_handler(void) {
TICK_TIMER->SR = 0;
}
static void exti15_10_handler(void) {
if ((EXTI->PR1 & (1U << CHARGING_DETECT_PIN)) != 0U) {
EXTI->PR1 = (1U << CHARGING_DETECT_PIN);
plug_charging = (get_gpio_input(CHARGING_DETECT_PORT, CHARGING_DETECT_PIN) != 0);
}
if ((EXTI->PR1 & (1U << IGNITION_SW_PIN)) != 0U) {
EXTI->PR1 = (1U << IGNITION_SW_PIN);
static uint32_t last_press_event_us = 0U;
const uint32_t debounce_us = 200000U; // 200 ms
uint32_t now = microsecond_timer_get();
bool pressed = (get_gpio_input(IGNITION_SW_PORT, IGNITION_SW_PIN) == 0);
if (pressed) {
if (get_ts_elapsed(now, last_press_event_us) > debounce_us) {
last_press_event_us = now;
ignition_press_timestamp_us = now;
ignition = !ignition;
set_gpio_output(OBDC_IGNITION_ON_PORT, OBDC_IGNITION_ON_PIN, ignition);
}
}
}
}
int main(void) {
disable_interrupts();
init_interrupts(true);
@@ -60,7 +97,15 @@ int main(void) {
hw_type = HW_TYPE_BODY;
current_board->init();
REGISTER_INTERRUPT(EXTI15_10_IRQn, exti15_10_handler, 10000U, FAULT_INTERRUPT_RATE_EXTI);
NVIC_ClearPendingIRQ(EXTI15_10_IRQn);
NVIC_EnableIRQ(EXTI15_10_IRQn);
REGISTER_INTERRUPT(TIM8_UP_TIM13_IRQn, bldc_tim8_handler, 100000U, FAULT_INTERRUPT_RATE_TICK);
NVIC_ClearPendingIRQ(TIM8_UP_TIM13_IRQn);
NVIC_EnableIRQ(TIM8_UP_TIM13_IRQn);
REGISTER_INTERRUPT(TICK_TIMER_IRQ, tick_handler, 10U, FAULT_INTERRUPT_RATE_TICK);
led_init();
@@ -68,14 +113,32 @@ int main(void) {
tick_timer_init();
usb_init();
body_can_init();
dotstar_init();
bldc_init();
enable_interrupts();
plug_charging = (get_gpio_input(CHARGING_DETECT_PORT, CHARGING_DETECT_PIN) != 0);
while (true) {
uint32_t now = microsecond_timer_get();
motor_speed_controller_update(now);
body_can_periodic(now);
if (plug_charging) {
motor_set_enable(false);
dotstar_apply_breathe((dotstar_rgb_t){255U, 40U, 0U}, now, 2000000U);
} else if (ignition) {
dotstar_run_rainbow(now);
} else {
dotstar_apply_breathe((dotstar_rgb_t){0U, 255U, 10U}, now, 1500000U);
}
if (ignition) {
motor_set_enable(true);
body_can_periodic(now, ignition, plug_charging);
} else {
motor_set_enable(false);
}
dotstar_show();
}
return 0;
}
}

50
board/body/main_comms.h
View File

@@ -54,41 +54,6 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
NVIC_SystemReset();
break;
// **** 0xe0: set motor speed
case 0xe0:
motor_set_speed((uint8_t)req->param1, (int8_t)req->param2);
break;
// **** 0xe1: stop motor
case 0xe1:
motor_set_speed((uint8_t)req->param1, 0);
break;
// **** 0xe2: get motor encoder state
case 0xe2: {
uint8_t motor = (uint8_t)req->param1;
int32_t position = motor_encoder_get_position(motor);
float rpm = motor_encoder_get_speed_rpm(motor);
int32_t rpm_milli = (int32_t)(rpm * 1000.0f);
(void)memcpy(resp, &position, sizeof(position));
(void)memcpy(resp + sizeof(position), &rpm_milli, sizeof(rpm_milli));
resp_len = (uint8_t)(sizeof(position) + sizeof(rpm_milli));
break;
}
// **** 0xe3: reset encoder position
case 0xe3:
motor_encoder_reset((uint8_t)req->param1);
break;
// **** 0xe4: set motor target speed (rpm * 0.1)
case 0xe4: {
uint8_t motor = (uint8_t)req->param1;
float target_rpm = ((int16_t)req->param2) * 0.1f;
motor_speed_controller_set_target_rpm(motor, target_rpm);
break;
}
// **** 0xdd: get healthpacket and CANPacket version hashes
case 0xdd: {
uint32_t versions[2] = {HEALTH_PACKET_VERSION, CAN_PACKET_VERSION_HASH};
@@ -97,6 +62,21 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
break;
}
// **** 0xb3: set motor speeds
case 0xb3:
rpm_left = (int16_t)req->param1;
rpm_right = (int16_t)req->param2;
break;
// **** 0xb4: enable/disable motors
case 0xb4:
enable_motors = (req->param1 == 1U);
if (enable_motors == 0) {
rpm_left = 0;
rpm_right = 0;
}
break;
default:
// Ignore unhandled requests
break;

15
board/body/motor_common.h
View File

@@ -1,15 +0,0 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#define BODY_MOTOR_COUNT 2U
typedef enum {
BODY_MOTOR_LEFT = 1U,
BODY_MOTOR_RIGHT = 2U,
} body_motor_id_e;
static inline bool body_motor_is_valid(uint8_t motor) {
return (motor > 0U) && (motor <= BODY_MOTOR_COUNT);
}

251
board/body/motor_control.h
View File

@@ -1,251 +0,0 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "board/body/motor_common.h"
#include "board/body/motor_encoder.h"
// Motor pin map:
// M1 drive: PB8 -> TIM16_CH1, PB9 -> TIM17_CH1, PE2/PE3 enables
// M2 drive: PA2 -> TIM15_CH1, PA3 -> TIM15_CH2, PE8/PE9 enables
#define PWM_TIMER_CLOCK_HZ 120000000U
#define PWM_FREQUENCY_HZ 5000U
#define PWM_PERCENT_MAX 100
#define PWM_RELOAD_TICKS ((PWM_TIMER_CLOCK_HZ + (PWM_FREQUENCY_HZ / 2U)) / PWM_FREQUENCY_HZ)
#define KP 0.25f
#define KI 0.5f
#define KD 0.008f
#define KFF 0.9f
#define MAX_RPM 100.0f
#define OUTPUT_MAX 100.0f
#define DEADBAND_RPM 1.0f
#define UPDATE_PERIOD_US 1000U
typedef struct {
TIM_TypeDef *forward_timer;
uint8_t forward_channel;
TIM_TypeDef *reverse_timer;
uint8_t reverse_channel;
GPIO_TypeDef *pwm_port[2];
uint8_t pwm_pin[2];
uint8_t pwm_af[2];
GPIO_TypeDef *enable_port[2];
uint8_t enable_pin[2];
} motor_pwm_config_t;
static const motor_pwm_config_t motor_pwm_config[BODY_MOTOR_COUNT] = {
[BODY_MOTOR_LEFT - 1U] = {
TIM16, 1U, TIM17, 1U,
{GPIOB, GPIOB}, {8U, 9U}, {GPIO_AF1_TIM16, GPIO_AF1_TIM17},
{GPIOE, GPIOE}, {2U, 3U},
},
[BODY_MOTOR_RIGHT - 1U] = {
TIM15, 2U, TIM15, 1U,
{GPIOA, GPIOA}, {2U, 3U}, {GPIO_AF4_TIM15, GPIO_AF4_TIM15},
{GPIOE, GPIOE}, {8U, 9U},
},
};
typedef struct {
bool active;
float target_rpm;
float integral;
float previous_error;
float last_output;
uint32_t last_update_us;
} motor_speed_state_t;
static inline float motor_absf(float value) {
return (value < 0.0f) ? -value : value;
}
static inline float motor_clampf(float value, float min_value, float max_value) {
if (value < min_value) {
return min_value;
}
if (value > max_value) {
return max_value;
}
return value;
}
static motor_speed_state_t motor_speed_states[BODY_MOTOR_COUNT];
static inline void motor_pwm_write(TIM_TypeDef *timer, uint8_t channel, uint8_t percentage) {
uint32_t period = (timer->ARR != 0U) ? timer->ARR : PWM_RELOAD_TICKS;
uint16_t comp = (uint16_t)((period * (uint32_t)percentage) / 100U);
if (channel == 1U) {
register_set(&(timer->CCR1), comp, 0xFFFFU);
} else if (channel == 2U) {
register_set(&(timer->CCR2), comp, 0xFFFFU);
}
}
static inline motor_speed_state_t *motor_speed_state_get(uint8_t motor) {
return body_motor_is_valid(motor) ? &motor_speed_states[motor - 1U] : NULL;
}
static inline void motor_speed_state_reset(motor_speed_state_t *state) {
state->active = false;
state->target_rpm = 0.0f;
state->integral = 0.0f;
state->previous_error = 0.0f;
state->last_output = 0.0f;
state->last_update_us = 0U;
}
static void motor_speed_controller_init(void) {
for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) {
motor_speed_state_reset(&motor_speed_states[i]);
}
}
static void motor_speed_controller_disable(uint8_t motor) {
motor_speed_state_t *state = motor_speed_state_get(motor);
if (state == NULL) {
return;
}
motor_speed_state_reset(state);
}
static inline void motor_write_enable(uint8_t motor_index, bool enable) {
const motor_pwm_config_t *cfg = &motor_pwm_config[motor_index];
uint8_t level = enable ? 1U : 0U;
set_gpio_output(cfg->enable_port[0], cfg->enable_pin[0], level);
set_gpio_output(cfg->enable_port[1], cfg->enable_pin[1], level);
}
static void motor_init(void) {
register_set_bits(&(RCC->AHB4ENR), RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN | RCC_AHB4ENR_GPIOEEN);
register_set_bits(&(RCC->APB2ENR), RCC_APB2ENR_TIM16EN | RCC_APB2ENR_TIM17EN | RCC_APB2ENR_TIM15EN);
for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) {
const motor_pwm_config_t *cfg = &motor_pwm_config[i];
motor_write_enable(i, false);
set_gpio_alternate(cfg->pwm_port[0], cfg->pwm_pin[0], cfg->pwm_af[0]);
set_gpio_alternate(cfg->pwm_port[1], cfg->pwm_pin[1], cfg->pwm_af[1]);
pwm_init(cfg->forward_timer, cfg->forward_channel);
pwm_init(cfg->reverse_timer, cfg->reverse_channel);
TIM_TypeDef *forward_timer = cfg->forward_timer;
register_set(&(forward_timer->PSC), 0U, 0xFFFFU);
register_set(&(forward_timer->ARR), PWM_RELOAD_TICKS, 0xFFFFU);
forward_timer->EGR |= TIM_EGR_UG;
register_set(&(forward_timer->BDTR), TIM_BDTR_MOE, 0xFFFFU);
if (cfg->reverse_timer != cfg->forward_timer) {
TIM_TypeDef *reverse_timer = cfg->reverse_timer;
register_set(&(reverse_timer->PSC), 0U, 0xFFFFU);
register_set(&(reverse_timer->ARR), PWM_RELOAD_TICKS, 0xFFFFU);
reverse_timer->EGR |= TIM_EGR_UG;
register_set(&(reverse_timer->BDTR), TIM_BDTR_MOE, 0xFFFFU);
}
}
}
static void motor_apply_pwm(uint8_t motor, int32_t speed_command) {
if (!body_motor_is_valid(motor)) {
return;
}
int8_t speed = (int8_t)motor_clampf((float)speed_command, -(float)PWM_PERCENT_MAX, (float)PWM_PERCENT_MAX);
uint8_t pwm_value = (uint8_t)((speed < 0) ? -speed : speed);
uint8_t motor_index = motor - 1U;
motor_write_enable(motor_index, speed != 0);
const motor_pwm_config_t *cfg = &motor_pwm_config[motor_index];
if (speed > 0) {
motor_pwm_write(cfg->forward_timer, cfg->forward_channel, pwm_value);
motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, 0U);
} else if (speed < 0) {
motor_pwm_write(cfg->forward_timer, cfg->forward_channel, 0U);
motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, pwm_value);
} else {
motor_pwm_write(cfg->forward_timer, cfg->forward_channel, 0U);
motor_pwm_write(cfg->reverse_timer, cfg->reverse_channel, 0U);
}
}
static inline void motor_set_speed(uint8_t motor, int8_t speed) {
motor_speed_controller_disable(motor);
motor_apply_pwm(motor, (int32_t)speed);
}
static inline void motor_speed_controller_set_target_rpm(uint8_t motor, float target_rpm) {
motor_speed_state_t *state = motor_speed_state_get(motor);
if (state == NULL) {
return;
}
target_rpm = motor_clampf(target_rpm, -MAX_RPM, MAX_RPM);
if (motor_absf(target_rpm) <= DEADBAND_RPM) {
motor_speed_controller_disable(motor);
motor_apply_pwm(motor, 0);
return;
}
state->active = true;
state->target_rpm = target_rpm;
state->integral = 0.0f;
state->previous_error = target_rpm - motor_encoder_get_speed_rpm(motor);
state->last_output = 0.0f;
state->last_update_us = 0U;
}
static inline void motor_speed_controller_update(uint32_t now_us) {
for (uint8_t motor = BODY_MOTOR_LEFT; motor <= BODY_MOTOR_RIGHT; motor++) {
motor_speed_state_t *state = motor_speed_state_get(motor);
if (!state->active) {
continue;
}
bool first_update = (state->last_update_us == 0U);
uint32_t dt_us = first_update ? UPDATE_PERIOD_US : (now_us - state->last_update_us);
if (!first_update && (dt_us < UPDATE_PERIOD_US)) {
continue;
}
float measured_rpm = motor_encoder_get_speed_rpm(motor);
float error = state->target_rpm - measured_rpm;
if ((motor_absf(state->target_rpm) <= DEADBAND_RPM) && (motor_absf(error) <= DEADBAND_RPM) && (motor_absf(measured_rpm) <= DEADBAND_RPM)) {
motor_speed_controller_disable(motor);
motor_apply_pwm(motor, 0);
continue;
}
float dt_s = (float)dt_us * 1.0e-6f;
float control = KFF * state->target_rpm;
if (dt_s > 0.0f) {
state->integral += error * dt_s;
float derivative = first_update ? 0.0f : (error - state->previous_error) / dt_s;
control += (KP * error) + (KI * state->integral) + (KD * derivative);
} else {
state->integral = 0.0f;
control += KP * error;
}
if ((state->target_rpm > 0.0f) && (control < 0.0f)) {
control = 0.0f;
state->integral = 0.0f;
} else if ((state->target_rpm < 0.0f) && (control > 0.0f)) {
control = 0.0f;
state->integral = 0.0f;
}
control = motor_clampf(control, -OUTPUT_MAX, OUTPUT_MAX);
int32_t command = (control >= 0.0f) ? (int32_t)(control + 0.5f) : (int32_t)(control - 0.5f);
motor_apply_pwm(motor, command);
state->previous_error = error;
state->last_output = control;
state->last_update_us = now_us;
}
}

170
board/body/motor_encoder.h
View File

@@ -1,170 +0,0 @@
#pragma once
#include <stdint.h>
#include "board/body/motor_common.h"
// Encoder pin map:
// Left motor: PB6 -> TIM4_CH1, PB7 -> TIM4_CH2
// Right motor: PA6 -> TIM3_CH1, PA7 -> TIM3_CH2
typedef struct {
TIM_TypeDef *timer;
GPIO_TypeDef *pin_a_port;
uint8_t pin_a;
uint8_t pin_a_af;
GPIO_TypeDef *pin_b_port;
uint8_t pin_b;
uint8_t pin_b_af;
int8_t direction;
uint32_t counts_per_output_rev;
uint32_t min_dt_us;
float speed_alpha;
uint32_t filter;
} motor_encoder_config_t;
typedef struct {
const motor_encoder_config_t *config;
uint16_t last_timer_count;
int32_t accumulated_count;
int32_t last_speed_count;
uint32_t last_speed_timestamp_us;
float cached_speed_rps;
} motor_encoder_state_t;
static const motor_encoder_config_t motor_encoder_config[BODY_MOTOR_COUNT] = {
[BODY_MOTOR_LEFT - 1U] = {
.timer = TIM4,
.pin_a_port = GPIOB, .pin_a = 6U, .pin_a_af = GPIO_AF2_TIM4,
.pin_b_port = GPIOB, .pin_b = 7U, .pin_b_af = GPIO_AF2_TIM4,
.direction = -1,
.counts_per_output_rev = 44U * 90U,
.min_dt_us = 250U,
.speed_alpha = 0.2f,
.filter = 3U,
},
[BODY_MOTOR_RIGHT - 1U] = {
.timer = TIM3,
.pin_a_port = GPIOA, .pin_a = 6U, .pin_a_af = GPIO_AF2_TIM3,
.pin_b_port = GPIOA, .pin_b = 7U, .pin_b_af = GPIO_AF2_TIM3,
.direction = 1,
.counts_per_output_rev = 44U * 90U,
.min_dt_us = 250U,
.speed_alpha = 0.2f,
.filter = 3U,
},
};
static motor_encoder_state_t motor_encoders[BODY_MOTOR_COUNT] = {
{ .config = &motor_encoder_config[0] },
{ .config = &motor_encoder_config[1] },
};
static void motor_encoder_configure_gpio(const motor_encoder_config_t *cfg) {
set_gpio_pullup(cfg->pin_a_port, cfg->pin_a, PULL_UP);
set_gpio_output_type(cfg->pin_a_port, cfg->pin_a, OUTPUT_TYPE_PUSH_PULL);
set_gpio_alternate(cfg->pin_a_port, cfg->pin_a, cfg->pin_a_af);
set_gpio_pullup(cfg->pin_b_port, cfg->pin_b, PULL_UP);
set_gpio_output_type(cfg->pin_b_port, cfg->pin_b, OUTPUT_TYPE_PUSH_PULL);
set_gpio_alternate(cfg->pin_b_port, cfg->pin_b, cfg->pin_b_af);
}
static void motor_encoder_configure_timer(motor_encoder_state_t *state) {
const motor_encoder_config_t *cfg = state->config;
TIM_TypeDef *timer = cfg->timer;
timer->CR1 = 0U;
timer->CR2 = 0U;
timer->SMCR = 0U;
timer->DIER = 0U;
timer->SR = 0U;
timer->CCMR1 = (TIM_CCMR1_CC1S_0) | (TIM_CCMR1_CC2S_0) | (cfg->filter << TIM_CCMR1_IC1F_Pos) | (cfg->filter << TIM_CCMR1_IC2F_Pos);
timer->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E;
timer->PSC = 0U;
timer->ARR = 0xFFFFU;
timer->CNT = 0U;
state->last_timer_count = 0U;
state->accumulated_count = 0;
state->last_speed_count = 0;
state->cached_speed_rps = 0.0f;
timer->SMCR = (TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1);
timer->CR1 = TIM_CR1_CEN;
}
static void motor_encoder_init(void) {
register_set_bits(&(RCC->APB1LENR), RCC_APB1LENR_TIM4EN | RCC_APB1LENR_TIM3EN);
register_set_bits(&(RCC->APB1LRSTR), RCC_APB1LRSTR_TIM4RST | RCC_APB1LRSTR_TIM3RST);
register_clear_bits(&(RCC->APB1LRSTR), RCC_APB1LRSTR_TIM4RST | RCC_APB1LRSTR_TIM3RST);
for (uint8_t i = 0U; i < BODY_MOTOR_COUNT; i++) {
motor_encoder_state_t *state = &motor_encoders[i];
const motor_encoder_config_t *cfg = state->config;
motor_encoder_configure_gpio(cfg);
motor_encoder_configure_timer(state);
state->last_speed_timestamp_us = 0U;
}
}
static inline int32_t motor_encoder_refresh(motor_encoder_state_t *state) {
const motor_encoder_config_t *cfg = state->config;
TIM_TypeDef *timer = cfg->timer;
uint16_t raw = (uint16_t)timer->CNT;
int32_t delta = (int16_t)(raw - state->last_timer_count);
delta *= cfg->direction;
state->last_timer_count = raw;
state->accumulated_count += delta;
return state->accumulated_count;
}
static inline int32_t motor_encoder_get_position(uint8_t motor) {
if (!body_motor_is_valid(motor)) {
return 0;
}
motor_encoder_state_t *state = &motor_encoders[motor - 1U];
return motor_encoder_refresh(state);
}
static void motor_encoder_reset(uint8_t motor) {
if (!body_motor_is_valid(motor)) {
return;
}
motor_encoder_state_t *state = &motor_encoders[motor - 1U];
state->config->timer->CNT = 0U;
state->last_timer_count = 0U;
state->accumulated_count = 0;
state->last_speed_count = 0;
state->last_speed_timestamp_us = 0U;
state->cached_speed_rps = 0.0f;
}
static float motor_encoder_get_speed_rpm(uint8_t motor) {
if (!body_motor_is_valid(motor)) {
return 0.0f;
}
motor_encoder_state_t *state = &motor_encoders[motor - 1U];
const motor_encoder_config_t *cfg = state->config;
motor_encoder_refresh(state);
uint32_t now = microsecond_timer_get();
if (state->last_speed_timestamp_us == 0U) {
state->last_speed_timestamp_us = now;
state->last_speed_count = state->accumulated_count;
state->cached_speed_rps = 0.0f;
return 0.0f;
}
uint32_t dt = now - state->last_speed_timestamp_us;
int32_t delta = state->accumulated_count - state->last_speed_count;
if ((dt < cfg->min_dt_us) || (delta == 0)) {
return state->cached_speed_rps * 60.0f;
}
state->last_speed_count = state->accumulated_count;
state->last_speed_timestamp_us = now;
float counts_per_second = ((float)delta * 1000000.0f) / (float)dt;
float new_speed_rps = (cfg->counts_per_output_rev != 0U) ? (counts_per_second / (float)cfg->counts_per_output_rev) : 0.0f;
state->cached_speed_rps += cfg->speed_alpha * (new_speed_rps - state->cached_speed_rps);
return state->cached_speed_rps * 60.0f;
}

2
board/comms_definitions.h
View File

@@ -1,3 +1,5 @@
#pragma once
typedef struct {
uint8_t request;
uint16_t param1;

5
board/drivers/can_common.h
View File

@@ -200,7 +200,12 @@ void ignition_can_hook(CANPacket_t *msg) {
ignition_can = (msg->data[0] >> 5) == 0x6U;
ignition_can_cnt = 0U;
}
}
// body exception
if (((msg->bus == 0U) || (msg->bus == 2U)) && (msg->addr == 0x201U)) {
ignition_can = true;
ignition_can_cnt = 0U;
}
}

4
board/drivers/fdcan.h
View File

@@ -215,6 +215,10 @@ void can_rx(uint8_t can_number) {
can_health[can_number].total_fwd_cnt += 1U;
}
#ifdef PANDA_BODY
body_can_rx(&to_push);
#endif
safety_rx_invalid += safety_rx_hook(&to_push) ? 0U : 1U;
ignition_can_hook(&to_push);