diff --git a/libraries/AP_Button/AP_Button.cpp b/libraries/AP_Button/AP_Button.cpp new file mode 100644 index 0000000000..8c8bd4a822 --- /dev/null +++ b/libraries/AP_Button/AP_Button.cpp @@ -0,0 +1,175 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + + +#include "AP_Button.h" +#include +#include + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_Button::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Enable button reporting + // @Description: This enables the button checking module. When this is disabled the parameters for setting button inputs are not visible + // @Values: 0:Disabled, 1:Enabled + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 0, AP_Button, enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: PIN1 + // @DisplayName: First button Pin + // @Description: Digital pin number for first button input. + // @User: Standard + // @Values: -1:Disabled,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 + AP_GROUPINFO("PIN1", 1, AP_Button, pin[0], -1), + + // @Param: PIN2 + // @DisplayName: Second button Pin + // @Description: Digital pin number for second button input. + // @User: Standard + // @Values: -1:Disabled,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 + AP_GROUPINFO("PIN2", 2, AP_Button, pin[1], -1), + + // @Param: PIN3 + // @DisplayName: Third button Pin + // @Description: Digital pin number for third button input. + // @User: Standard + // @Values: -1:Disabled,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 + AP_GROUPINFO("PIN3", 3, AP_Button, pin[2], -1), + + // @Param: PIN4 + // @DisplayName: Fourth button Pin + // @Description: Digital pin number for fourth button input. + // @User: Standard + // @Values: -1:Disabled,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 + AP_GROUPINFO("PIN4", 4, AP_Button, pin[3], -1), + + // @Param: REPORT_SEND + // @DisplayName: Report send time + // @Description: The duration in seconds that a BUTTON_CHANGE report is repeatedly sent to the GCS regarding a button changing state. Note that the BUTTON_CHANGE message is MAVLink2 only. + // @User: Standard + // @Range: 0 3600 + AP_GROUPINFO("REPORT_SEND", 5, AP_Button, report_send_time, 10), + + AP_GROUPEND +}; + + +// constructor +AP_Button::AP_Button(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +/* + update and report, called from main loop + */ +void AP_Button::update(void) +{ + if (!enable) { + return; + } + + // call setup pins at update rate (5Hz) to allow for runtime parameter change of pins + setup_pins(); + + if (!initialised) { + initialised = true; + + // get initial mask + last_mask = get_mask(); + + // register 1kHz timer callback + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Button::timer_update, void)); + } + + if (last_change_time_ms != 0 && + (AP_HAL::millis() - last_report_ms) > AP_BUTTON_REPORT_PERIOD_MS && + (AP_HAL::millis64() - last_change_time_ms) < report_send_time*1000ULL) { + // send a change report + last_report_ms = AP_HAL::millis(); + + // send a report to GCS + send_report(); + } +} + +/* + get current mask + */ +uint8_t AP_Button::get_mask(void) +{ + uint8_t mask = 0; + for (uint8_t i=0; iread(pin[i]) << i; + } + return mask; +} + +/* + called at 1kHz to check for button state change + */ +void AP_Button::timer_update(void) +{ + if (!enable) { + return; + } + uint8_t mask = get_mask(); + if (mask != last_mask) { + last_mask = mask; + last_change_time_ms = AP_HAL::millis64(); + } +} + +/* + send a BUTTON_CHANGE report to the GCS + */ +void AP_Button::send_report(void) +{ + uint8_t chan_mask = GCS_MAVLINK::active_channel_mask(); + uint32_t now = AP_HAL::millis(); + for (uint8_t i=0; ipinMode(pin[i], HAL_GPIO_INPUT); + // setup pullup + hal.gpio->write(pin[i], 1); + } +} diff --git a/libraries/AP_Button/AP_Button.h b/libraries/AP_Button/AP_Button.h new file mode 100644 index 0000000000..f52175a2a9 --- /dev/null +++ b/libraries/AP_Button/AP_Button.h @@ -0,0 +1,67 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#include + +// allow buttons for up to 4 pins +#define AP_BUTTON_NUM_PINS 4 + +// how often we send reports +#define AP_BUTTON_REPORT_PERIOD_MS 1000 + +class AP_Button { +public: + // constructor + AP_Button(void); + + static const struct AP_Param::GroupInfo var_info[]; + + // update button state and send messages, called periodically by main loop + void update(void); + +private: + AP_Int8 enable; + AP_Int8 pin[AP_BUTTON_NUM_PINS]; + + // number of seconds to send change notifications + AP_Int16 report_send_time; + + // last button press mask + uint8_t last_mask; + + // when the mask last changed + uint64_t last_change_time_ms; + + // time of last report + uint32_t last_report_ms; + + // has the timer been installed? + bool initialised:1; + + // called by timer thread + void timer_update(void); + + // get current mask + uint8_t get_mask(void); + + // send a BUTTON_CHANGE report + void send_report(void); + + // setup pins as pullup input + void setup_pins(); +}; +