RC pwm signal read fixes
Re-work on the RC pwm input signal timeout
This commit is contained in:
parent
7d80e564f0
commit
c099bec824
Binary file not shown.
|
@ -287,9 +287,9 @@
|
|||
#define PWM_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0)
|
||||
// Min / Max values of each channel (use DEBUG to determine these values)
|
||||
#define PWM_CH1_MAX 1000 // (0 - 1000)
|
||||
#define PWM_CH1_MIN -1000 // (-1000 - 0)
|
||||
#define PWM_CH2_MAX 500 // (0 - 1000)
|
||||
#define PWM_CH2_MIN -800 // (-1000 - 0)
|
||||
#define PWM_CH1_MIN -1000 // (-1000 - 0)
|
||||
#define PWM_CH2_MAX 1000 // (0 - 1000)
|
||||
#define PWM_CH2_MIN -1000 // (-1000 - 0)
|
||||
// right sensor board cable. Only read once during startup
|
||||
#define BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
|
||||
#define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0].
|
||||
|
|
|
@ -170,8 +170,6 @@ void PPM_ISR_Callback(void);
|
|||
void PWM_Init(void);
|
||||
//void PWM_ISR_CH1_Callback(void);
|
||||
void PWM_ISR_CH2_Callback(void);
|
||||
void PWM_SysTick_Callback(void);
|
||||
int PWM_Signal_Correct(int16_t x, int16_t max, int16_t min);
|
||||
|
||||
// Sideboard definitions
|
||||
#define LED1_SET (0x01)
|
||||
|
|
|
@ -73,6 +73,7 @@ void poweroffPressCheck(void);
|
|||
|
||||
// Read Command Function
|
||||
void readCommand(void);
|
||||
int PWM_Signal_Correct(int16_t u, int16_t min, int16_t max);
|
||||
|
||||
// Sideboard functions
|
||||
void sideboardLeds(uint8_t *leds);
|
||||
|
|
|
@ -123,25 +123,14 @@ uint32_t pwm_timeout = 0;
|
|||
|
||||
#define IN_RANGE(x, low, up) (((x) >= (low)) && ((x) <= (up)))
|
||||
|
||||
int PWM_Signal_Correct(int16_t x, int16_t max, int16_t min) {
|
||||
int outVal = 0;
|
||||
if(x > -PWM_DEADBAND && x < PWM_DEADBAND) {
|
||||
outVal = 0;
|
||||
} else if(x > 0) {
|
||||
outVal = (float)CLAMP(x-PWM_DEADBAND, 0, max - PWM_DEADBAND) / (max - PWM_DEADBAND) * 1000;
|
||||
} else {
|
||||
outVal = 0 - ((float)CLAMP(x+PWM_DEADBAND, min + PWM_DEADBAND, 0) / (min + PWM_DEADBAND) * 1000);
|
||||
}
|
||||
return outVal;
|
||||
}
|
||||
|
||||
/*
|
||||
void PWM_ISR_CH1_Callback(void) {
|
||||
// Dummy loop with 16 bit count wrap around
|
||||
uint16_t rc_signal = TIM3->CNT;
|
||||
TIM3->CNT = 0;
|
||||
|
||||
if (IN_RANGE(rc_signal, 900, 2100)){
|
||||
// The interval check below should be larger than the feasible PWM interval of ~[500, 2500] ms
|
||||
if (IN_RANGE(rc_signal, 200, 4000)){
|
||||
timeout = 0;
|
||||
pwm_timeout = 0;
|
||||
pwm_captured_ch1_value = CLAMP(rc_signal, 1000, 2000) - 1000;
|
||||
|
@ -154,7 +143,8 @@ void PWM_ISR_CH2_Callback(void) {
|
|||
uint16_t rc_signal = TIM2->CNT;
|
||||
TIM2->CNT = 0;
|
||||
|
||||
if (IN_RANGE(rc_signal, 900, 2100)){
|
||||
// The interval check below should be larger than the feasible PWM interval of ~[900, 2100] ms
|
||||
if (IN_RANGE(rc_signal, 200, 3000)){
|
||||
timeout = 0;
|
||||
pwm_timeout = 0;
|
||||
pwm_captured_ch2_value = CLAMP(rc_signal, 1000, 2000) - 1000;
|
||||
|
@ -164,11 +154,11 @@ void PWM_ISR_CH2_Callback(void) {
|
|||
// SysTick executes once each ms
|
||||
void PWM_SysTick_Callback(void) {
|
||||
pwm_timeout++;
|
||||
// Stop after 500 ms without PPM signal
|
||||
// Stop after 500 ms without PWM signal
|
||||
if(pwm_timeout > 500) {
|
||||
//pwm_captured_ch1_value = 500;
|
||||
pwm_captured_ch2_value = 500;
|
||||
pwm_timeout = 0;
|
||||
pwm_timeout = 500; // limit the timeout to max timeout value of 500 ms
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -177,18 +167,18 @@ void PWM_Init(void) {
|
|||
/*
|
||||
GPIO_InitTypeDef GPIO_InitStruct2;
|
||||
// Configure GPIO pin : PA2
|
||||
GPIO_InitStruct2.Pin = GPIO_PIN_2;
|
||||
GPIO_InitStruct2.Mode = GPIO_MODE_IT_RISING_FALLING;
|
||||
GPIO_InitStruct2.Speed = GPIO_SPEED_FREQ_HIGH;
|
||||
GPIO_InitStruct2.Pull = GPIO_PULLDOWN;
|
||||
GPIO_InitStruct2.Pin = GPIO_PIN_2;
|
||||
GPIO_InitStruct2.Mode = GPIO_MODE_IT_RISING_FALLING;
|
||||
GPIO_InitStruct2.Speed = GPIO_SPEED_FREQ_HIGH;
|
||||
GPIO_InitStruct2.Pull = GPIO_PULLDOWN;
|
||||
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct2);
|
||||
|
||||
__HAL_RCC_TIM3_CLK_ENABLE();
|
||||
TimHandle2.Instance = TIM3;
|
||||
TimHandle2.Init.Period = UINT16_MAX;
|
||||
TimHandle2.Init.Prescaler = (SystemCoreClock/DELAY_TIM_FREQUENCY_US)-1;;
|
||||
TimHandle2.Instance = TIM3;
|
||||
TimHandle2.Init.Period = UINT16_MAX;
|
||||
TimHandle2.Init.Prescaler = (SystemCoreClock/DELAY_TIM_FREQUENCY_US)-1;;
|
||||
TimHandle2.Init.ClockDivision = 0;
|
||||
TimHandle2.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||
TimHandle2.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||
HAL_TIM_Base_Init(&TimHandle2);
|
||||
|
||||
// EXTI interrupt init
|
||||
|
@ -201,18 +191,18 @@ void PWM_Init(void) {
|
|||
|
||||
GPIO_InitTypeDef GPIO_InitStruct;
|
||||
/*Configure GPIO pin : PA3 */
|
||||
GPIO_InitStruct.Pin = GPIO_PIN_3;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
|
||||
GPIO_InitStruct.Pin = GPIO_PIN_3;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
|
||||
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||||
|
||||
__HAL_RCC_TIM2_CLK_ENABLE();
|
||||
TimHandle.Instance = TIM2;
|
||||
TimHandle.Init.Period = UINT16_MAX;
|
||||
TimHandle.Init.Prescaler = (SystemCoreClock/DELAY_TIM_FREQUENCY_US)-1;;
|
||||
TimHandle.Init.ClockDivision = 0;
|
||||
TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||
TimHandle.Instance = TIM2;
|
||||
TimHandle.Init.Period = UINT16_MAX;
|
||||
TimHandle.Init.Prescaler = (SystemCoreClock/DELAY_TIM_FREQUENCY_US)-1;;
|
||||
TimHandle.Init.ClockDivision = 0;
|
||||
TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||
HAL_TIM_Base_Init(&TimHandle);
|
||||
|
||||
/* EXTI interrupt init*/
|
||||
|
|
|
@ -251,8 +251,8 @@ int main(void) {
|
|||
// speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA);
|
||||
mixerFcn(speed << 4, steer << 4, &speedR, &speedL); // This function implements the equations above
|
||||
|
||||
// ####### SET OUTPUTS (if the target change is less than +/- 50) #######
|
||||
if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) {
|
||||
// ####### SET OUTPUTS (if the target change is less than +/- 100) #######
|
||||
if ((speedL > lastSpeedL-100 && speedL < lastSpeedL+100) && (speedR > lastSpeedR-100 && speedR < lastSpeedR+100) && timeout < TIMEOUT) {
|
||||
#ifdef INVERT_R_DIRECTION
|
||||
pwmr = speedR;
|
||||
#else
|
||||
|
|
21
Src/util.c
21
Src/util.c
|
@ -649,8 +649,8 @@ void readCommand(void) {
|
|||
#endif
|
||||
|
||||
#ifdef CONTROL_PWM
|
||||
cmd1 = 0; // CLAMP(PWM_Signal_Correct((pwm_captured_ch1_value - 500) * 2, PWM_CH1_MAX, PWM_CH1_MIN), INPUT_MIN, INPUT_MAX);
|
||||
cmd2 = CLAMP(PWM_Signal_Correct((pwm_captured_ch2_value - 500) * 2, PWM_CH2_MAX, PWM_CH2_MIN), INPUT_MIN, INPUT_MAX);
|
||||
cmd1 = 0; // CLAMP(PWM_Signal_Correct((pwm_captured_ch1_value - 500) * 2, PWM_CH1_MIN, PWM_CH1_MAX), INPUT_MIN, INPUT_MAX);
|
||||
cmd2 = CLAMP(PWM_Signal_Correct((pwm_captured_ch2_value - 500) * 2, PWM_CH2_MIN, PWM_CH2_MAX), INPUT_MIN, INPUT_MAX);
|
||||
#endif
|
||||
|
||||
#ifdef CONTROL_ADC
|
||||
|
@ -811,7 +811,6 @@ void readCommand(void) {
|
|||
if (main_loop_counter % 30 == 0) {
|
||||
HAL_UART_DMAStop(&huart3);
|
||||
HAL_UART_Receive_DMA(&huart3, (uint8_t *)&Sideboard_Rnew, sizeof(Sideboard_Rnew));
|
||||
Sideboard_Rnew.start = 0xFFFF; // Change the Start Frame to avoid entering again here if no data is received
|
||||
}
|
||||
}
|
||||
timeoutFlagSerial = timeoutFlagSerial_R;
|
||||
|
@ -840,6 +839,22 @@ void readCommand(void) {
|
|||
}
|
||||
|
||||
|
||||
/*
|
||||
* PWM Signal Correction
|
||||
* This function realizes a dead-band around 0 and scales the input within a min and a max
|
||||
*/
|
||||
int PWM_Signal_Correct(int16_t u, int16_t min, int16_t max) {
|
||||
int outVal = 0;
|
||||
if(u > -PWM_DEADBAND && u < PWM_DEADBAND) {
|
||||
outVal = 0;
|
||||
} else if(u > 0) {
|
||||
outVal = (INPUT_MAX * CLAMP(u - PWM_DEADBAND, 0, max - PWM_DEADBAND)) / (max - PWM_DEADBAND);
|
||||
} else {
|
||||
outVal = (INPUT_MIN * CLAMP(u + PWM_DEADBAND, min + PWM_DEADBAND, 0)) / (min + PWM_DEADBAND);
|
||||
}
|
||||
return outVal;
|
||||
}
|
||||
|
||||
|
||||
/* =========================== Sideboard Functions =========================== */
|
||||
|
||||
|
|
Loading…
Reference in New Issue