Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Instantiate just one copy of Button in Plane and Copter #12912

Merged
merged 4 commits into from
Dec 10, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ArduCopter/Copter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -199,7 +199,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(userhook_SuperSlowLoop, 1, 75),
#endif
#if BUTTON_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Button, &copter.g2.button, update, 5, 100),
SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100),
#endif
#if STATS_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Stats, &copter.g2.stats, update, 1, 100),
Expand Down
5 changes: 0 additions & 5 deletions ArduCopter/Copter.h
Original file line number Diff line number Diff line change
Expand Up @@ -531,11 +531,6 @@ class Copter : public AP_Vehicle {
AP_Parachute parachute{relay};
#endif

// Button
#if BUTTON_ENABLED == ENABLED
AP_Button button;
#endif

// Landing Gear Controller
AP_LandingGear landinggear;

Expand Down
3 changes: 2 additions & 1 deletion ArduCopter/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -735,7 +735,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
#if BUTTON_ENABLED == ENABLED
// @Group: BTN_
// @Path: ../libraries/AP_Button/AP_Button.cpp
AP_SUBGROUPINFO(button, "BTN_", 2, ParametersG2, AP_Button),
AP_SUBGROUPPTR(button_ptr, "BTN_", 2, ParametersG2, AP_Button),
#endif

#if MODE_THROW_ENABLED == ENABLED
Expand Down Expand Up @@ -1052,6 +1052,7 @@ ParametersG2::ParametersG2(void)
#if MODE_AUTOROTATE_ENABLED == ENABLED
,arot(copter.inertial_nav)
#endif
,button_ptr(&copter.button)
{
AP_Param::setup_object_defaults(this, var_info);
}
Expand Down
4 changes: 1 addition & 3 deletions ArduCopter/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -485,10 +485,8 @@ class ParametersG2 {
// altitude at which nav control can start in takeoff
AP_Float wp_navalt_min;

#if BUTTON_ENABLED == ENABLED
// button checking
AP_Button button;
#endif
AP_Button *button_ptr;

#if STATS_ENABLED == ENABLED
// vehicle statistics
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/ArduPlane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50),
SCHED_TASK(avoidance_adsb_update, 10, 100),
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200),
SCHED_TASK_CLASS(AP_Button, &plane.g2.button, update, 5, 100),
SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100),
#if STATS_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Stats, &plane.g2.stats, update, 1, 100),
#endif
Expand Down
3 changes: 2 additions & 1 deletion ArduPlane/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1104,7 +1104,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {

// @Group: BTN_
// @Path: ../libraries/AP_Button/AP_Button.cpp
AP_SUBGROUPINFO(button, "BTN_", 1, ParametersG2, AP_Button),
AP_SUBGROUPPTR(button_ptr, "BTN_", 1, ParametersG2, AP_Button),

// @Group: ICE_
// @Path: ../libraries/AP_ICEngine/AP_ICEngine.cpp
Expand Down Expand Up @@ -1253,6 +1253,7 @@ ParametersG2::ParametersG2(void) :
#if SOARING_ENABLED == ENABLED
,soaring_controller(plane.ahrs, plane.TECS_controller, plane.aparm)
#endif
,button_ptr(&plane.button)
{
AP_Param::setup_object_defaults(this, var_info);
}
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -505,7 +505,7 @@ class ParametersG2 {
static const struct AP_Param::GroupInfo var_info[];

// button reporting library
AP_Button button;
AP_Button *button_ptr;

#if STATS_ENABLED == ENABLED
// vehicle statistics
Expand Down
38 changes: 0 additions & 38 deletions Tools/autotest/apmrover2.py
Original file line number Diff line number Diff line change
Expand Up @@ -981,44 +981,6 @@ def test_sysid_enforce(self):
if ex is not None:
raise ex

def drain_mav_seconds(self, seconds):
tstart = self.get_sim_time_cached()
while self.get_sim_time_cached() - tstart < seconds:
self.drain_mav();
self.delay_sim_time(0.5)

def test_button(self):
self.set_parameter("SIM_PIN_MASK", 0)
self.set_parameter("BTN_ENABLE", 1)
btn = 2
pin = 3
self.drain_mav()
self.set_parameter("BTN_PIN%u" % btn, pin)
m = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
self.progress("m: %s" % str(m))
if m is None:
raise NotAchievedException("Did not get BUTTON_CHANGE event")
mask = 1<<btn
if m.state & mask:
raise NotAchievedException("Bit incorrectly set in mask (got=%u dontwant=%u)" % (m.state, mask))
# SITL instantly reverts the pin to its old value
m2 = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
self.progress("m2: %s" % str(m2))
if m2 is None:
raise NotAchievedException("Did not get repeat message")
# wait for messages to stop coming:
self.drain_mav_seconds(15)

self.set_parameter("SIM_PIN_MASK", 0)
m3 = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
self.progress("m3: %s" % str(m3))
if m3 is None:
raise NotAchievedException("Did not get new message")
if m.last_change_ms == m3.last_change_ms:
raise NotAchievedException("last_change_ms same as first message")
if m3.state != 0:
raise NotAchievedException("Didn't get expected mask back in message (mask=0 state=%u" % (m3.state))

def test_rally_points(self):
self.reboot_sitl() # to ensure starting point is as expected

Expand Down
4 changes: 4 additions & 0 deletions Tools/autotest/arducopter.py
Original file line number Diff line number Diff line change
Expand Up @@ -4282,6 +4282,10 @@ def tests(self):
"Test Camera/Antenna Mount",
self.test_mount),

("Button",
"Test Buttons",
self.test_button),

("RangeFinder",
"Test RangeFinder Basic Functionality",
self.test_rangefinder),
Expand Down
4 changes: 4 additions & 0 deletions Tools/autotest/arduplane.py
Original file line number Diff line number Diff line change
Expand Up @@ -1638,6 +1638,10 @@ def tests(self):
"Test ADSB",
self.test_adsb),

("Button",
"Test Buttons",
self.test_button),

("AdvancedFailsafe",
"Test Advanced Failsafe",
self.test_advanced_failsafe),
Expand Down
38 changes: 38 additions & 0 deletions Tools/autotest/common.py
Original file line number Diff line number Diff line change
Expand Up @@ -3590,6 +3590,44 @@ def test_advanced_failsafe(self):
if ex is not None:
raise ex

def drain_mav_seconds(self, seconds):
tstart = self.get_sim_time_cached()
while self.get_sim_time_cached() - tstart < seconds:
self.drain_mav();
self.delay_sim_time(0.5)

def test_button(self):
self.set_parameter("SIM_PIN_MASK", 0)
self.set_parameter("BTN_ENABLE", 1)
btn = 2
pin = 3
self.drain_mav()
self.set_parameter("BTN_PIN%u" % btn, pin)
m = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
self.progress("m: %s" % str(m))
if m is None:
raise NotAchievedException("Did not get BUTTON_CHANGE event")
mask = 1<<btn
if m.state & mask:
raise NotAchievedException("Bit incorrectly set in mask (got=%u dontwant=%u)" % (m.state, mask))
# SITL instantly reverts the pin to its old value
m2 = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
self.progress("m2: %s" % str(m2))
if m2 is None:
raise NotAchievedException("Did not get repeat message")
# wait for messages to stop coming:
self.drain_mav_seconds(15)

self.set_parameter("SIM_PIN_MASK", 0)
m3 = self.mav.recv_match(type='BUTTON_CHANGE', blocking=True, timeout=1)
self.progress("m3: %s" % str(m3))
if m3 is None:
raise NotAchievedException("Did not get new message")
if m.last_change_ms == m3.last_change_ms:
raise NotAchievedException("last_change_ms same as first message")
if m3.state != 0:
raise NotAchievedException("Didn't get expected mask back in message (mask=0 state=%u" % (m3.state))

def tests(self):
return [
("PIDTuning",
Expand Down
16 changes: 16 additions & 0 deletions libraries/AP_Button/AP_Button.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@

extern const AP_HAL::HAL& hal;

AP_Button *AP_Button::_singleton;

const AP_Param::GroupInfo AP_Button::var_info[] = {

// @Param: ENABLE
Expand Down Expand Up @@ -72,6 +74,11 @@ const AP_Param::GroupInfo AP_Button::var_info[] = {
AP_Button::AP_Button(void)
{
AP_Param::setup_object_defaults(this, var_info);

if (_singleton != nullptr) {
AP_HAL::panic("AP_Button must be singleton");
}
_singleton = this;
}

/*
Expand Down Expand Up @@ -166,3 +173,12 @@ void AP_Button::setup_pins(void)
hal.gpio->write(pin[i], 1);
}
}

namespace AP {

AP_Button &button()
{
return *AP_Button::get_singleton();
}

}
16 changes: 15 additions & 1 deletion libraries/AP_Button/AP_Button.h
Original file line number Diff line number Diff line change
Expand Up @@ -28,12 +28,23 @@ class AP_Button {
// constructor
AP_Button(void);

/* Do not allow copies */
AP_Button(const AP_Button &other) = delete;
AP_Button &operator=(const AP_Button&) = delete;

static const struct AP_Param::GroupInfo var_info[];

// update button state and send messages, called periodically by main loop
void update(void);


static AP_Button *get_singleton(void) {
return _singleton;
}

private:

static AP_Button *_singleton;

AP_Int8 enable;
AP_Int8 pin[AP_BUTTON_NUM_PINS];

Expand Down Expand Up @@ -65,3 +76,6 @@ class AP_Button {
void setup_pins();
};

namespace AP {
AP_Button &button();
};