From b042e67677949e8baaf4db2f1ede6dd7dd403fe1 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 9 Feb 2022 00:32:46 +0000 Subject: [PATCH 1/4] AP_Button: trigger low on invalid PWM input --- libraries/AP_Button/AP_Button.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Button/AP_Button.cpp b/libraries/AP_Button/AP_Button.cpp index 1349341abb870..2b993f40b5a68 100644 --- a/libraries/AP_Button/AP_Button.cpp +++ b/libraries/AP_Button/AP_Button.cpp @@ -74,27 +74,27 @@ const AP_Param::GroupInfo AP_Button::var_info[] = { // @Param: OPTIONS1 // @DisplayName: Button Pin 1 Options - // @Description: Options for Pin 1. PWM input detects PWM above or below 1800/1200us instead of logic level. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. + // @Description: Options for Pin 1. PWM input detects PWM above or below 1800/1200us instead of logic level. If PWM is not detected or is less than 800us or above 2200us the button will interpreted as low. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. // @User: Standard // @Bitmask: 0:PWM Input,1:InvertInput AP_GROUPINFO("OPTIONS1", 6, AP_Button, options[0], 0), // @Param: OPTIONS2 // @DisplayName: Button Pin 2 Options - // @Description: Options for Pin 2. PWM input detects PWM above or below 1800/1200us instead of logic level. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. + // @Description: Options for Pin 2. PWM input detects PWM above or below 1800/1200us instead of logic level. If PWM is not detected or is less than 800us or above 2200us the button will interpreted as low. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. // @User: Standard // @Bitmask: 0:PWM Input,1:InvertInput AP_GROUPINFO("OPTIONS2", 7, AP_Button, options[1], 0), // @Param: OPTIONS3 // @DisplayName: Button Pin 3 Options - // @Description: Options for Pin 3. PWM input detects PWM above or below 1800/1200us instead of logic level. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. + // @Description: Options for Pin 3. PWM input detects PWM above or below 1800/1200us instead of logic level. If PWM is not detected or is less than 800us or above 2200us the button will interpreted as low. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. // @Bitmask: 0:PWM Input,1:InvertInput AP_GROUPINFO("OPTIONS3", 8, AP_Button, options[2], 0), // @Param: OPTIONS4 // @DisplayName: Button Pin 4 Options - // @Description: Options for Pin 4. PWM input detects PWM above or below 1800/1200us instead of logic level. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. + // @Description: Options for Pin 4. PWM input detects PWM above or below 1800/1200us instead of logic level. If PWM is not detected or is less than 800us or above 2200us the button will interpreted as low. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. // @User: Standard // @Bitmask: 0:PWM Input,1:InvertInput AP_GROUPINFO("OPTIONS4", 9, AP_Button, options[3], 0), @@ -183,6 +183,13 @@ void AP_Button::update(void) continue; } const uint16_t pwm_us = pwm_pin_source[i].get_pwm_us(); + if (pwm_us < RC_Channel::RC_MIN_LIMIT_PWM || pwm_us > RC_Channel::RC_MAX_LIMIT_PWM) { + // invalid pulse width, trigger low + if (pwm_state & mask) { + new_pwm_state &= ~mask; + } + continue; + } // these values are the same as used in RC_Channel: if (pwm_state & mask) { // currently asserted; check to see if we should de-assert From 1d92e8f9c5bab179c2013c002dbd04b74a2bfae4 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 15 Feb 2022 00:35:23 +0000 Subject: [PATCH 2/4] Copter: motor_test: use PWM min and max from RC_Channel --- ArduCopter/motor_test.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 2248aa3e16969..b222e24f4932c 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -6,8 +6,6 @@ */ // motor test definitions -#define MOTOR_TEST_PWM_MIN 800 // min pwm value accepted by the test -#define MOTOR_TEST_PWM_MAX 2200 // max pwm value accepted by the test #define MOTOR_TEST_TIMEOUT_SEC 600 // max timeout is 10 minutes (600 seconds) static uint32_t motor_test_start_ms; // system time the motor test began @@ -84,7 +82,7 @@ void Copter::motor_test_output() } // sanity check throttle values - if (pwm >= MOTOR_TEST_PWM_MIN && pwm <= MOTOR_TEST_PWM_MAX ) { + if (pwm >= RC_Channel::RC_MIN_LIMIT_PWM && pwm <= RC_Channel::RC_MAX_LIMIT_PWM) { // turn on motor to specified pwm value motors->output_test_seq(motor_test_seq, pwm); } else { From 661ff77bae79d918037bf97ec54d6bb57e352dab Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 15 Feb 2022 00:35:33 +0000 Subject: [PATCH 3/4] Plane: motor_test: use PWM min and max from RC_Channel --- ArduPlane/motor_test.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp index 8774aa18336e1..cfc640cab9859 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -7,8 +7,6 @@ */ // motor test definitions -#define MOTOR_TEST_PWM_MIN 800 // min pwm value accepted by the test -#define MOTOR_TEST_PWM_MAX 2200 // max pwm value accepted by the test #define MOTOR_TEST_TIMEOUT_MS_MAX 30000 // max timeout is 30 seconds // motor_test_output - checks for timeout and sends updates to motors objects @@ -68,7 +66,7 @@ void QuadPlane::motor_test_output() } // sanity check throttle values - if (pwm >= MOTOR_TEST_PWM_MIN && pwm <= MOTOR_TEST_PWM_MAX ) { + if (pwm >= RC_Channel::RC_MIN_LIMIT_PWM && pwm <= RC_Channel::RC_MAX_LIMIT_PWM) { // turn on motor to specified pwm vlaue motors->output_test_seq(motor_test.seq, pwm); } else { From 6ca1829fb763dc186c3ab3b5fe7dab99a497318b Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 15 Feb 2022 00:35:51 +0000 Subject: [PATCH 4/4] RC_Channel: update RC_MIN_LIMIT_PWM from 900 to 800 --- libraries/RC_Channel/RC_Channel.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 0c2df648aafab..bec21d8c18fbc 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -274,7 +274,7 @@ class RC_Channel { const char *string_for_aux_function(AUX_FUNC function) const; #endif // pwm value under which we consider that Radio value is invalid - static const uint16_t RC_MIN_LIMIT_PWM = 900; + static const uint16_t RC_MIN_LIMIT_PWM = 800; // pwm value above which we consider that Radio value is invalid static const uint16_t RC_MAX_LIMIT_PWM = 2200;