diff --git a/src/controller/ebike_app.c b/src/controller/ebike_app.c index 6225fab2..b06f5561 100755 --- a/src/controller/ebike_app.c +++ b/src/controller/ebike_app.c @@ -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); } @@ -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; diff --git a/src/controller/main.h b/src/controller/main.h index 8c20d1c8..b9191335 100644 --- a/src/controller/main.h +++ b/src/controller/main.h @@ -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 diff --git a/src/controller/motor.c b/src/controller/motor.c index 0e18c35f..ec2692bd 100755 --- a/src/controller/motor.c +++ b/src/controller/motor.c @@ -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; @@ -907,20 +908,11 @@ 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 @@ -928,41 +920,28 @@ void TIM1_CAP_COM_IRQHandler(void) __interrupt(TIM1_CAP_COM_IRQHANDLER) // 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; + } } /****************************************************************************/