RC pwm signal read fixes

Re-work on the RC pwm input signal timeout
This commit is contained in:
EmanuelFeru 2020-03-23 21:09:17 +01:00
parent 7d80e564f0
commit c099bec824
7 changed files with 47 additions and 43 deletions

View File

@ -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].

View File

@ -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)

View File

@ -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);

View File

@ -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*/

View File

@ -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

View File

@ -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 =========================== */