diff --git a/libraries/AP_Button/AP_Button.cpp b/libraries/AP_Button/AP_Button.cpp index 5478329eb2..7d89ab2159 100644 --- a/libraries/AP_Button/AP_Button.cpp +++ b/libraries/AP_Button/AP_Button.cpp @@ -97,6 +97,30 @@ const AP_Param::GroupInfo AP_Button::var_info[] = { // @Bitmask: 0:PWM Input,1:InvertInput AP_GROUPINFO("OPTIONS4", 9, AP_Button, options[3], 0), + // @Param: FUNC1 + // @DisplayName: Button Pin 1 RC Channel function + // @Description: Function executed on pin change + // @User: Standard + AP_GROUPINFO("FUNC1", 10, AP_Button, pin_func[0], (uint16_t)RC_Channel::AUX_FUNC::DO_NOTHING), + + // @Param: FUNC2 + // @DisplayName: Button Pin 2 RC Channel function + // @Description: Function executed on pin change + // @User: Standard + AP_GROUPINFO("FUNC2", 11, AP_Button, pin_func[1], (uint16_t)RC_Channel::AUX_FUNC::DO_NOTHING), + + // @Param: FUNC3 + // @DisplayName: Button Pin 3 RC Channel function + // @Description: Function executed on pin change + // @User: Standard + AP_GROUPINFO("FUNC3", 12, AP_Button, pin_func[2], (uint16_t)RC_Channel::AUX_FUNC::DO_NOTHING), + + // @Param: FUNC4 + // @DisplayName: Button Pin 4 RC Channel function + // @Description: Function executed on pin change + // @User: Standard + AP_GROUPINFO("FUNC4", 13, AP_Button, pin_func[3], (uint16_t)RC_Channel::AUX_FUNC::DO_NOTHING), + AP_GROUPEND }; @@ -144,31 +168,99 @@ void AP_Button::update(void) } // update the PWM state: - uint8_t new_pwm_state = 0; + uint8_t new_pwm_state = pwm_state; for (uint8_t i=0; i 1800 && pwm_us < 2200; + if (pwm_state & mask) { + // currently asserted; check to see if we should de-assert + if (pwm_us < RC_Channel::AUX_PWM_TRIGGER_LOW) { + new_pwm_state &= ~mask; + } + } else { + // currently not asserted; check to see if we should assert + if (pwm_us > RC_Channel::AUX_PWM_TRIGGER_HIGH) { + new_pwm_state |= mask; + } + } } if (new_pwm_state != pwm_state) { pwm_state = new_pwm_state; - last_debounced_change_ms = AP_HAL::millis64(); + last_debounce_ms = AP_HAL::millis64(); } - // act on any changes in state - if (last_change_time_ms != 0 && + if (last_debounce_ms != 0 && (AP_HAL::millis() - last_report_ms) > AP_BUTTON_REPORT_PERIOD_MS && - (AP_HAL::millis64() - last_change_time_ms) < report_send_time*1000ULL) { + (AP_HAL::millis64() - last_debounce_ms) < report_send_time*1000ULL) { // send a change report last_report_ms = AP_HAL::millis(); // send a report to GCS send_report(); } + + if (!aux_functions_initialised) { + run_aux_functions(true); + aux_functions_initialised = true; + } + + if (last_debounce_ms != 0 && + last_debounce_ms != last_action_time_ms) { + last_action_time_ms = last_debounce_ms; + run_aux_functions(false); + } +} + +void AP_Button::run_aux_functions(bool force) +{ + RC_Channel *rc_channel = rc().channel(1); + if (rc_channel == nullptr) { + return; + } + + for (uint8_t i=0; istring_for_aux_function(func); + if (str != nullptr) { + gcs().send_text(MAV_SEVERITY_INFO, "Button: executing (%s)", str); + } +#endif + rc_channel->do_aux_function(func, pos); + } } // get state of a button @@ -224,6 +316,7 @@ void AP_Button::timer_update(void) (now - last_change_time_ms) > DEBOUNCE_MS) { // crude de-bouncing, debounces all buttons as one, not individually debounce_mask = last_mask; + WITH_SEMAPHORE(last_debounced_change_ms_sem); last_debounced_change_ms = now; } } diff --git a/libraries/AP_Button/AP_Button.h b/libraries/AP_Button/AP_Button.h index c88966fded..525d58ce18 100644 --- a/libraries/AP_Button/AP_Button.h +++ b/libraries/AP_Button/AP_Button.h @@ -56,6 +56,11 @@ private: bool is_pwm_input(uint8_t n) const { return ((uint8_t)options[n].get() & (1U<<0)) != 0; } + bool is_input_inverted(uint8_t n) const { + return ((uint8_t)options[n].get() & (1U<<1)) != 0; + } + + AP_Int16 pin_func[AP_BUTTON_NUM_PINS]; // from the RC_Channel functions // number of seconds to send change notifications AP_Int16 report_send_time; @@ -75,11 +80,26 @@ private: // pwm sources are used when using PWM on the input AP_HAL::PWMSource pwm_pin_source[AP_BUTTON_NUM_PINS]; + // state from the timer interrupt: + HAL_Semaphore last_debounced_change_ms_sem; + // last time GPIO pins were debounced: uint64_t last_debounced_change_ms; + // time at least last debounce changes across both PWM and GPIO + // pins was detected: + uint64_t last_debounce_ms; + // when the mask last changed uint64_t last_change_time_ms; + // when button change events were last actioned + uint64_t last_action_time_ms; + + // true if allocated aux functions have been initialised + bool aux_functions_initialised; + // call init_aux_function for all used functions + void run_aux_functions(bool force); + // time of last report uint32_t last_report_ms; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index c647838d21..43cfe52b98 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -238,6 +238,11 @@ public: const char *string_for_aux_function(AUX_FUNC function) const; #endif + // pwm value above which the option will be invoked: + static const uint16_t AUX_PWM_TRIGGER_HIGH = 1800; + // pwm value below which the option will be disabled: + static const uint16_t AUX_PWM_TRIGGER_LOW = 1200; + protected: virtual void init_aux_function(aux_func_t ch_option, AuxSwitchPos); @@ -292,11 +297,6 @@ private: int16_t pwm_to_angle() const; int16_t pwm_to_angle_dz(uint16_t dead_zone) const; - // pwm value above which the option will be invoked: - static const uint16_t AUX_PWM_TRIGGER_HIGH = 1800; - // pwm value below which the option will be disabled: - static const uint16_t AUX_PWM_TRIGGER_LOW = 1200; - // Structure used to detect and debounce switch changes struct { int8_t debounce_position = -1;