Skip to content

Commit

Permalink
Wheel speed simplified math with a decaying counter
Browse files Browse the repository at this point in the history
  • Loading branch information
@dzid26
dzid26 committed Apr 8, 2024
1 parent 49ab6c3 commit f559933
Show file tree
Hide file tree
Showing 3 changed files with 39 additions and 77 deletions.
51 changes: 18 additions & 33 deletions src/controller/ebike_app.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1533,15 +1533,10 @@ static void apply_speed_limit(void)
static void calc_wheel_speed(void)
{
// calc wheel speed (km/h x10)
if (ui16_wheel_speed_sensor_ticks) {
uint16_t ui16_tmp = ui16_wheel_speed_sensor_ticks;
// rps = PWM_CYCLES_SECOND / ui16_wheel_speed_sensor_ticks (rev/sec)
// km/h*10 = rps * ui16_wheel_perimeter * ((3600 / (1000 * 1000)) * 10)
// !!!warning if PWM_CYCLES_SECOND is not a multiple of 1000
ui16_wheel_speed_x10 = (uint16_t)(((uint32_t) m_configuration_variables.ui16_wheel_perimeter * ((PWM_CYCLES_SECOND/1000)*36U)) / ui16_tmp);
} else {
ui16_wheel_speed_x10 = 0;
}
// rps = PWM_CYCLES_SECOND / ui16_wheel_speed_sensor_ticks (rev/sec)
// km/h*10 = rps * ui16_wheel_perimeter * ((3600 / (1000 * 1000)) * 10)
// !!!warning if PWM_CYCLES_SECOND is not a multiple of 1000
ui16_wheel_speed_x10 = (uint16_t)(((uint32_t) m_configuration_variables.ui16_wheel_perimeter * ((PWM_CYCLES_SECOND/1000)*36U)) / ui16_wheel_speed_sensor_ticks);
}


Expand Down Expand Up @@ -3310,30 +3305,20 @@ static void uart_send_package(void)


static void calc_oem_wheel_speed(void)
{
if(ui8_display_ready_flag)
{
uint32_t ui32_oem_wheel_speed;
uint32_t ui32_oem_wheel_perimeter;

// calc oem wheel speed (wheel turning time)
if(ui16_wheel_speed_sensor_ticks)
{
ui32_oem_wheel_speed = ((uint32_t) ui16_wheel_speed_sensor_ticks * 10) / OEM_WHEEL_SPEED_DIVISOR;

// speed conversion for different perimeter
ui32_oem_wheel_perimeter = ((uint32_t) ui8_oem_wheel_diameter * 7975) / 100; // 25.4 * 3.14 * 100 = 7975
ui32_oem_wheel_speed *= ui32_oem_wheel_perimeter;
ui32_oem_wheel_speed /= (uint32_t) m_configuration_variables.ui16_wheel_perimeter;

// oem wheel speed (wheel turning time)
ui16_oem_wheel_speed = (uint16_t) ui32_oem_wheel_speed;
}
else
{
ui16_oem_wheel_speed = 0;
}
}
{
uint32_t ui32_oem_wheel_speed;
uint32_t ui32_oem_wheel_perimeter;

// calc oem wheel speed (wheel turning time)
ui32_oem_wheel_speed = ((uint32_t) ui16_wheel_speed_sensor_ticks * 10) / OEM_WHEEL_SPEED_DIVISOR;

// speed conversion for different perimeter
ui32_oem_wheel_perimeter = ((uint32_t) ui8_oem_wheel_diameter * 7975) / 100; // 25.4 * 3.14 * 100 = 7975
ui32_oem_wheel_speed *= ui32_oem_wheel_perimeter;
ui32_oem_wheel_speed /= (uint32_t) m_configuration_variables.ui16_wheel_perimeter;

// oem wheel speed (wheel turning time)
ui16_oem_wheel_speed = (uint16_t) ui32_oem_wheel_speed;

#if ENABLE_ODOMETER_COMPENSATION
uint16_t ui16_wheel_speed;
Expand Down
4 changes: 1 addition & 3 deletions src/controller/main.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,12 +62,10 @@
#define CADENCE_SENSOR_STANDARD_MODE_SCHMITT_TRIGGER_THRESHOLD 0U // software based Schmitt trigger to stop motor jitter when at resolution limits (350 at 15.625KHz)
// Wheel speed sensor
#define MAX_PLAUSIBLE_WHEEL_SPEED_X10 800U
#define MIN_PLAUSIBLE_WHEEL_SPEED_X10 35U
#define WHEEL_SPEED_COUNTER_RESET 0U
#define WHEEL_SPEED_COUNTER_MAX UINT16_MAX
#define WHEEL_SPEED_TICKS_STOP UINT16_MAX
#define WHEEL_SPEED_SENSOR_TICKS_COUNTER_MAX ((uint16_t)((uint32_t)WHEEL_PERIMETER * PWM_CYCLES_SECOND / (MAX_PLAUSIBLE_WHEEL_SPEED_X10 / 10U) * 60U / 1000U * 60U / 1000U)) //1774
#define WHEEL_SPEED_SENSOR_TICKS_COUNTER_MIN ((uint16_t)((uint32_t)WHEEL_PERIMETER * PWM_CYCLES_SECOND / (MIN_PLAUSIBLE_WHEEL_SPEED_X10 / 10U) * 60U / 1000U * 60 / 1000U)) // 40555
#define WHEEL_SPEED_SENSOR_TICKS_COUNTER_MAX_SPEED ((uint16_t)((uint32_t)WHEEL_PERIMETER * PWM_CYCLES_SECOND / (MAX_PLAUSIBLE_WHEEL_SPEED_X10 / 10U) * 60U / 1000U * 60U / 1000U))// small value - fast rotation

#define PWM_DUTY_CYCLE_MAX 255U
#define PWM_DUTY_CYCLE_BITS 8
Expand Down
61 changes: 20 additions & 41 deletions src/controller/motor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -335,9 +335,10 @@ static uint8_t ui8_cadence_calc_ref_state = NO_PAS_REF;
const static uint8_t ui8_pas_old_valid_state[4] = { 0x01, 0x03, 0x00, 0x02 };

// wheel speed sensor
volatile uint16_t ui16_wheel_speed_sensor_ticks = 0;
volatile uint16_t ui16_wheel_speed_sensor_ticks_counter_min = 0;
volatile uint32_t ui32_wheel_speed_sensor_ticks_total = 0;
volatile uint16_t ui16_wheel_speed_sensor_ticks = WHEEL_SPEED_TICKS_STOP;
volatile uint32_t ui32_wheel_speed_sensor_ticks_total = 0U;
static uint16_t ui16_wheel_speed_sensor_ticks_counter = WHEEL_SPEED_COUNTER_MAX;
static uint8_t ui8_wheel_speed_sensor_pin_state_old;

// battery soc
static uint8_t ui8_battery_SOC_saved_flag = 0;
Expand Down Expand Up @@ -907,62 +908,40 @@ void TIM1_CAP_COM_IRQHandler(void) __interrupt(TIM1_CAP_COM_IRQHANDLER)
}

/****************************************************************************/

static uint16_t ui16_wheel_speed_sensor_ticks_counter;
static uint8_t ui8_wheel_speed_sensor_ticks_counter_started;
static uint8_t ui8_wheel_speed_sensor_pin_state_old;

// check wheel speed sensor pin state
uint8_t ui8_wheel_speed_sensor_pin_state = WHEEL_SPEED_SENSOR__PORT->IDR & WHEEL_SPEED_SENSOR__PIN;

// check wheel speed sensor ticks counter min value
if(ui16_wheel_speed_sensor_ticks) { ui16_wheel_speed_sensor_ticks_counter_min = ui16_wheel_speed_sensor_ticks >> 3; }
else { ui16_wheel_speed_sensor_ticks_counter_min = WHEEL_SPEED_SENSOR_TICKS_COUNTER_MIN >> 3; }

if(!ui8_wheel_speed_sensor_ticks_counter_started ||
(ui16_wheel_speed_sensor_ticks_counter > ui16_wheel_speed_sensor_ticks_counter_min)) {
//ignores pulses that would result in 8x previous speed
if(ui16_wheel_speed_sensor_ticks_counter > (ui16_wheel_speed_sensor_ticks / 8U)) { //starts with 65535 /
// check if wheel speed sensor pin state has changed
if (ui8_wheel_speed_sensor_pin_state != ui8_wheel_speed_sensor_pin_state_old) {
// update old wheel speed sensor pin state
ui8_wheel_speed_sensor_pin_state_old = ui8_wheel_speed_sensor_pin_state;

// only consider the 0 -> 1 transition
if (ui8_wheel_speed_sensor_pin_state) {
// check if first transition
if (!ui8_wheel_speed_sensor_ticks_counter_started) {
// start wheel speed sensor ticks counter as this is the first transition
ui8_wheel_speed_sensor_ticks_counter_started = 1;
} else {
// check if wheel speed sensor ticks counter is out of bounds
if (ui16_wheel_speed_sensor_ticks_counter < WHEEL_SPEED_SENSOR_TICKS_COUNTER_MAX) {
ui16_wheel_speed_sensor_ticks = 0;
ui16_wheel_speed_sensor_ticks_counter = 0;
ui8_wheel_speed_sensor_ticks_counter_started = 0;
} else {
//update latest tick and reset the counter
ui16_wheel_speed_sensor_ticks = ui16_wheel_speed_sensor_ticks_counter;
ui16_wheel_speed_sensor_ticks_counter = 0;
++ui32_wheel_speed_sensor_ticks_total;
}
}
// check if wheel speed sensor ticks counter is out of bounds
if (ui16_wheel_speed_sensor_ticks_counter < WHEEL_SPEED_SENSOR_TICKS_COUNTER_MAX_SPEED) {//if overspeed
ui16_wheel_speed_sensor_ticks = WHEEL_SPEED_SENSOR_TICKS_COUNTER_MAX_SPEED;
ui16_wheel_speed_sensor_ticks_counter = WHEEL_SPEED_SENSOR_TICKS_COUNTER_MAX_SPEED;
} else {
//update latest tick and reset the counter
ui16_wheel_speed_sensor_ticks = ui16_wheel_speed_sensor_ticks_counter;
ui16_wheel_speed_sensor_ticks_counter = WHEEL_SPEED_COUNTER_RESET;
++ui32_wheel_speed_sensor_ticks_total;
}
}
}
if (ui16_wheel_speed_sensor_ticks_counter > ui16_wheel_speed_sensor_ticks) {
//strat decaying the speed if thte pulse is coming later than last time
//start decaying the speed if the pulse is taking longer than from last pulse
ui16_wheel_speed_sensor_ticks = ui16_wheel_speed_sensor_ticks_counter;
}
}

// increment and also limit the ticks counter
if (ui8_wheel_speed_sensor_ticks_counter_started)
if (ui16_wheel_speed_sensor_ticks_counter < WHEEL_SPEED_SENSOR_TICKS_COUNTER_MIN) {
++ui16_wheel_speed_sensor_ticks_counter;
} else {
// reset variables
ui16_wheel_speed_sensor_ticks = 0;
ui16_wheel_speed_sensor_ticks_counter = 0;
ui8_wheel_speed_sensor_ticks_counter_started = 0;
}
if (ui16_wheel_speed_sensor_ticks_counter < WHEEL_SPEED_COUNTER_MAX) {
++ui16_wheel_speed_sensor_ticks_counter;
}
}

/****************************************************************************/
Expand Down

0 comments on commit f559933

Please sign in to comment.