From 38574153f078f81644fb4d17c26286db333ae858 Mon Sep 17 00:00:00 2001 From: EmanuelFeru Date: Tue, 24 Mar 2020 20:36:19 +0100 Subject: [PATCH] Added deadband for PPM signal Issue #29 --- Inc/config.h | 26 ++++++++++++++++---------- Inc/util.h | 2 +- Src/main.c | 11 +++++++++-- Src/util.c | 20 ++++++++++---------- 4 files changed, 36 insertions(+), 23 deletions(-) diff --git a/Inc/config.h b/Inc/config.h index 64f8f4e..7631376 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -188,7 +188,7 @@ */ // #define DEBUG_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! -#if defined(VARIANT_ADC) +#if defined(VARIANT_ADC) || defined(VARIANT_PPM) #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! #endif @@ -273,6 +273,12 @@ */ #define CONTROL_PPM // use PPM-Sum as input. disable CONTROL_SERIAL_USART2! #define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used. + #define PPM_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 PPM_CH1_MAX 1000 // (0 - 1000) + #define PPM_CH1_MIN -1000 // (-1000 - 0) + #define PPM_CH2_MAX 1000 // (0 - 1000) + #define PPM_CH2_MIN -1000 // (-1000 - 0) #endif // ############################# END OF VARIANT_PPM SETTINGS ############################ @@ -284,16 +290,16 @@ * Channel 1: steering, Channel 2: speed. */ #define CONTROL_PWM // use RC PWM as input. disable DEBUG_SERIAL_USART2! - // #define SUPPORT_BUTTONS // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! - #define PWM_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) + #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 1000 // (0 - 1000) - #define PWM_CH2_MIN -1000 // (-1000 - 0) - #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. - #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 - #define STEER_COEFFICIENT 0 // 0.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 0 = 0.0 * 2^14. If you do not want any steering, set it to 0. + #define PWM_CH1_MAX 1000 // (0 - 1000) + #define PWM_CH1_MIN -1000 // (-1000 - 0) + #define PWM_CH2_MAX 1000 // (0 - 1000) + #define PWM_CH2_MIN -1000 // (-1000 - 0) + #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. + #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 + #define STEER_COEFFICIENT 0 // 0.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 0 = 0.0 * 2^14. If you do not want any steering, set it to 0. + // #define SUPPORT_BUTTONS // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! // #define INVERT_R_DIRECTION // #define INVERT_L_DIRECTION #endif diff --git a/Inc/util.h b/Inc/util.h index e39b618..07f8684 100644 --- a/Inc/util.h +++ b/Inc/util.h @@ -73,7 +73,7 @@ void poweroffPressCheck(void); // Read Command Function void readCommand(void); -int PWM_Signal_Correct(int16_t u, int16_t min, int16_t max); +int addDeadBand(int16_t u, int16_t deadBand, int16_t min, int16_t max); // Sideboard functions void sideboardLeds(uint8_t *leds); diff --git a/Src/main.c b/Src/main.c index bade613..de2b5c3 100644 --- a/Src/main.c +++ b/Src/main.c @@ -82,8 +82,11 @@ extern SerialSideboard Sideboard_L; #if defined(SIDEBOARD_SERIAL_USART3) extern SerialSideboard Sideboard_R; #endif +#if defined(CONTROL_PPM) && defined(DEBUG_SERIAL_USART3) +extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1]; +#endif #if defined(CONTROL_PWM) && defined(DEBUG_SERIAL_USART3) -//extern volatile uint16_t pwm_captured_ch1_value; +extern volatile uint16_t pwm_captured_ch1_value; extern volatile uint16_t pwm_captured_ch2_value; #endif @@ -385,8 +388,12 @@ int main(void) { setScopeChannel(0, (int16_t)adc_buffer.l_tx2); // 1: ADC1 setScopeChannel(1, (int16_t)adc_buffer.l_rx2); // 2: ADC2 #endif + #ifdef CONTROL_PPM + setScopeChannel(0, ppm_captured_value[0]); // 1: CH1 + setScopeChannel(1, ppm_captured_value[1]); // 2: CH2 + #endif #ifdef CONTROL_PWM - setScopeChannel(0, 0);//pwm_captured_ch1_value); // 1: CH1 + setScopeChannel(0, pwm_captured_ch1_value); // 1: CH1 setScopeChannel(1, pwm_captured_ch2_value); // 2: CH2 #endif setScopeChannel(2, (int16_t)speedR); // 3: output command: [-1000, 1000] diff --git a/Src/util.c b/Src/util.c index 69ed6df..81181d9 100644 --- a/Src/util.c +++ b/Src/util.c @@ -637,8 +637,8 @@ void readCommand(void) { #endif #ifdef CONTROL_PPM - cmd1 = CLAMP((ppm_captured_value[0] - 500) * 2, INPUT_MIN, INPUT_MAX); - cmd2 = CLAMP((ppm_captured_value[1] - 500) * 2, INPUT_MIN, INPUT_MAX); + cmd1 = CLAMP(addDeadBand((ppm_captured_value[0] - 500) * 2, PPM_DEADBAND, PPM_CH1_MIN, PPM_CH1_MAX), INPUT_MIN, INPUT_MAX); + cmd2 = CLAMP(addDeadBand((ppm_captured_value[1] - 500) * 2, PPM_DEADBAND, PPM_CH1_MIN, PPM_CH1_MAX), INPUT_MIN, INPUT_MAX); #ifdef SUPPORT_BUTTONS button1 = ppm_captured_value[5] > 500; button2 = 0; @@ -647,8 +647,8 @@ void readCommand(void) { #endif #ifdef CONTROL_PWM - cmd1 = 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); + cmd1 = CLAMP(addDeadBand((pwm_captured_ch1_value - 500) * 2, PWM_DEADBAND, PWM_CH1_MIN, PWM_CH1_MAX), INPUT_MIN, INPUT_MAX); + cmd2 = CLAMP(addDeadBand((pwm_captured_ch2_value - 500) * 2, PWM_DEADBAND, PWM_CH2_MIN, PWM_CH2_MAX), INPUT_MIN, INPUT_MAX); #ifdef SUPPORT_BUTTONS button1 = !HAL_GPIO_ReadPin(BUTTON1_RIGHT_PORT, BUTTON1_RIGHT_PIN); button2 = !HAL_GPIO_ReadPin(BUTTON2_RIGHT_PORT, BUTTON2_RIGHT_PIN); @@ -842,18 +842,18 @@ void readCommand(void) { /* - * PWM Signal Correction + * Add Dead-band to a signal * 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) { -#ifdef CONTROL_PWM +int addDeadBand(int16_t u, int16_t deadBand, int16_t min, int16_t max) { +#if defined(CONTROL_PPM) || defined(CONTROL_PWM) int outVal = 0; - if(u > -PWM_DEADBAND && u < PWM_DEADBAND) { + if(u > -deadBand && u < deadBand) { outVal = 0; } else if(u > 0) { - outVal = (INPUT_MAX * CLAMP(u - PWM_DEADBAND, 0, max - PWM_DEADBAND)) / (max - PWM_DEADBAND); + outVal = (INPUT_MAX * CLAMP(u - deadBand, 0, max - deadBand)) / (max - deadBand); } else { - outVal = (INPUT_MIN * CLAMP(u + PWM_DEADBAND, min + PWM_DEADBAND, 0)) / (min + PWM_DEADBAND); + outVal = (INPUT_MIN * CLAMP(u + deadBand, min + deadBand, 0)) / (min + deadBand); } return outVal; #else