Skip to content

Add hall sensor tick counters for wheel odometry #599

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
EFeru merged 1 commit into from
Jul 4, 2026
Merged

Add hall sensor tick counters for wheel odometry #599

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions Inc/config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,10 @@
#define TEMP_POWEROFF 650 // overheat poweroff. (while not driving) [°C * 10]. Here 65.0 °C
// ######################## END OF TEMPERATURE ###############################

// ############################### ODOMETRY ######################################
// #define ENABLE_ODOMETRY // [-] Enable hall sensor tick counters for wheel odometry (adds wheelR_cnt / wheelL_cnt to serial feedback)
// ########################### END OF ODOMETRY ###################################



// ############################### MOTOR CONTROL #########################
Expand Down
33 changes: 33 additions & 0 deletions Src/bldc.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,23 @@ static int16_t offsetdcr = 2000;
int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE;
static int32_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 16; // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point

#ifdef ENABLE_ODOMETRY
int16_t odom_l = 0;
int16_t odom_r = 0;

static uint16_t wp_l_prev = 0;
static uint16_t wp_r_prev = 0;

static int16_t hallTickModulo(int16_t m, int16_t n) {
return (((m % n) + n) % n);
}

static int16_t hallTickDirection(int16_t prev, int16_t curr) {
static const uint16_t dir_lut[6] = {0, -1, -2, 0, 2, 1};
return dir_lut[hallTickModulo(prev - curr, 6)];
}
#endif

// =================================
// DMA interrupt frequency =~ 16 kHz
// =================================
Expand Down Expand Up @@ -202,6 +219,14 @@ void DMA1_Channel1_IRQHandler(void) {
// motSpeedLeft = rtY_Left.n_mot;
// motAngleLeft = rtY_Left.a_elecAngle;

#ifdef ENABLE_ODOMETRY
// Hall sensor tick counter (left)
uint8_t encoding = (uint8_t)((hall_ul << 2) + (hall_vl << 1) + hall_wl);
int wheel_pos = rtConstP.vec_hallToPos_Value[encoding];
odom_l = hallTickModulo(odom_l + hallTickDirection(wp_l_prev, wheel_pos), 9000);
wp_l_prev = wheel_pos;
#endif

/* Apply commands */
LEFT_TIM->LEFT_TIM_U = (uint16_t)CLAMP(ul + pwm_res / 2, pwm_margin, pwm_res-pwm_margin);
LEFT_TIM->LEFT_TIM_V = (uint16_t)CLAMP(vl + pwm_res / 2, pwm_margin, pwm_res-pwm_margin);
Expand Down Expand Up @@ -240,6 +265,14 @@ void DMA1_Channel1_IRQHandler(void) {
// motSpeedRight = rtY_Right.n_mot;
// motAngleRight = rtY_Right.a_elecAngle;

#ifdef ENABLE_ODOMETRY
// Hall sensor tick counter (right)
encoding = (uint8_t)((hall_ur << 2) + (hall_vr << 1) + hall_wr);
wheel_pos = rtConstP.vec_hallToPos_Value[encoding];
odom_r = hallTickModulo(odom_r - hallTickDirection(wp_r_prev, wheel_pos), 9000);
wp_r_prev = wheel_pos;
#endif

/* Apply commands */
RIGHT_TIM->RIGHT_TIM_U = (uint16_t)CLAMP(ur + pwm_res / 2, pwm_margin, pwm_res-pwm_margin);
RIGHT_TIM->RIGHT_TIM_V = (uint16_t)CLAMP(vr + pwm_res / 2, pwm_margin, pwm_res-pwm_margin);
Expand Down
23 changes: 21 additions & 2 deletions Src/main.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,11 @@ int16_t dc_curr; // global variable for Total DC Link current
int16_t cmdL; // global variable for Left Command
int16_t cmdR; // global variable for Right Command

#ifdef ENABLE_ODOMETRY
extern int16_t odom_l;
extern int16_t odom_r;
#endif

//------------------------------------------------------------------------
// Local variables
//------------------------------------------------------------------------
Expand All @@ -123,6 +128,10 @@ typedef struct{
int16_t cmd2;
int16_t speedR_meas;
int16_t speedL_meas;
#ifdef ENABLE_ODOMETRY
int16_t wheelR_cnt;
int16_t wheelL_cnt;
#endif
int16_t batVoltage;
int16_t boardTemp;
uint16_t cmdLed;
Expand Down Expand Up @@ -514,13 +523,20 @@ int main(void) {
Feedback.cmd2 = (int16_t)input2[inIdx].cmd;
Feedback.speedR_meas = (int16_t)rtY_Right.n_mot;
Feedback.speedL_meas = (int16_t)rtY_Left.n_mot;
#ifdef ENABLE_ODOMETRY
Feedback.wheelR_cnt = (int16_t)odom_r;
Feedback.wheelL_cnt = (int16_t)odom_l;
#endif
Feedback.batVoltage = (int16_t)batVoltageCalib;
Feedback.boardTemp = (int16_t)board_temp_deg_c;

#if defined(FEEDBACK_SERIAL_USART2)
if(__HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) {
Feedback.cmdLed = (uint16_t)sideboard_leds_L;
Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas
Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas
#ifdef ENABLE_ODOMETRY
^ Feedback.wheelR_cnt ^ Feedback.wheelL_cnt
#endif
^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed);

HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Feedback, sizeof(Feedback));
Expand All @@ -529,7 +545,10 @@ int main(void) {
#if defined(FEEDBACK_SERIAL_USART3)
if(__HAL_DMA_GET_COUNTER(huart3.hdmatx) == 0) {
Feedback.cmdLed = (uint16_t)sideboard_leds_R;
Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas
Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas
#ifdef ENABLE_ODOMETRY
^ Feedback.wheelR_cnt ^ Feedback.wheelL_cnt
#endif
^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed);

HAL_UART_Transmit_DMA(&huart3, (uint8_t *)&Feedback, sizeof(Feedback));
Expand Down
Loading