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

@ -288,8 +288,8 @@
// Min / Max values of each channel (use DEBUG to determine these values) // Min / Max values of each channel (use DEBUG to determine these values)
#define PWM_CH1_MAX 1000 // (0 - 1000) #define PWM_CH1_MAX 1000 // (0 - 1000)
#define PWM_CH1_MIN -1000 // (-1000 - 0) #define PWM_CH1_MIN -1000 // (-1000 - 0)
#define PWM_CH2_MAX 500 // (0 - 1000) #define PWM_CH2_MAX 1000 // (0 - 1000)
#define PWM_CH2_MIN -800 // (-1000 - 0) #define PWM_CH2_MIN -1000 // (-1000 - 0)
// right sensor board cable. Only read once during startup // 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 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]. #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_Init(void);
//void PWM_ISR_CH1_Callback(void); //void PWM_ISR_CH1_Callback(void);
void PWM_ISR_CH2_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 // Sideboard definitions
#define LED1_SET (0x01) #define LED1_SET (0x01)

View File

@ -73,6 +73,7 @@ void poweroffPressCheck(void);
// Read Command Function // Read Command Function
void readCommand(void); void readCommand(void);
int PWM_Signal_Correct(int16_t u, int16_t min, int16_t max);
// Sideboard functions // Sideboard functions
void sideboardLeds(uint8_t *leds); 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))) #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) { void PWM_ISR_CH1_Callback(void) {
// Dummy loop with 16 bit count wrap around // Dummy loop with 16 bit count wrap around
uint16_t rc_signal = TIM3->CNT; uint16_t rc_signal = TIM3->CNT;
TIM3->CNT = 0; 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; timeout = 0;
pwm_timeout = 0; pwm_timeout = 0;
pwm_captured_ch1_value = CLAMP(rc_signal, 1000, 2000) - 1000; 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; uint16_t rc_signal = TIM2->CNT;
TIM2->CNT = 0; 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; timeout = 0;
pwm_timeout = 0; pwm_timeout = 0;
pwm_captured_ch2_value = CLAMP(rc_signal, 1000, 2000) - 1000; 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 // SysTick executes once each ms
void PWM_SysTick_Callback(void) { void PWM_SysTick_Callback(void) {
pwm_timeout++; pwm_timeout++;
// Stop after 500 ms without PPM signal // Stop after 500 ms without PWM signal
if(pwm_timeout > 500) { if(pwm_timeout > 500) {
//pwm_captured_ch1_value = 500; //pwm_captured_ch1_value = 500;
pwm_captured_ch2_value = 500; pwm_captured_ch2_value = 500;
pwm_timeout = 0; pwm_timeout = 500; // limit the timeout to max timeout value of 500 ms
} }
} }

View File

@ -251,8 +251,8 @@ int main(void) {
// speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA); // 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 mixerFcn(speed << 4, steer << 4, &speedR, &speedL); // This function implements the equations above
// ####### SET OUTPUTS (if the target change is less than +/- 50) ####### // ####### SET OUTPUTS (if the target change is less than +/- 100) #######
if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) { if ((speedL > lastSpeedL-100 && speedL < lastSpeedL+100) && (speedR > lastSpeedR-100 && speedR < lastSpeedR+100) && timeout < TIMEOUT) {
#ifdef INVERT_R_DIRECTION #ifdef INVERT_R_DIRECTION
pwmr = speedR; pwmr = speedR;
#else #else

View File

@ -649,8 +649,8 @@ void readCommand(void) {
#endif #endif
#ifdef CONTROL_PWM #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); 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_MAX, PWM_CH2_MIN), 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 #endif
#ifdef CONTROL_ADC #ifdef CONTROL_ADC
@ -811,7 +811,6 @@ void readCommand(void) {
if (main_loop_counter % 30 == 0) { if (main_loop_counter % 30 == 0) {
HAL_UART_DMAStop(&huart3); HAL_UART_DMAStop(&huart3);
HAL_UART_Receive_DMA(&huart3, (uint8_t *)&Sideboard_Rnew, sizeof(Sideboard_Rnew)); 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; 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 =========================== */ /* =========================== Sideboard Functions =========================== */