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.

Sign up for GitHub

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

FMU: Implement baseline pairing command on safety button press #12794

Merged
merged 2 commits into from
Jan 22, 2020
Merged

FMU: Implement baseline pairing command on safety button press #12794

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
555d9ba
safety_button: add support for pairing command (3x pressing the button)
bkueng Jan 22, 2020
17580f4
safety_button: fix missing return when module exits
bkueng Jan 22, 2020
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
159 changes: 117 additions & 42 deletions src/drivers/safety_button/SafetyButton.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,16 @@
*
****************************************************************************/

#include <drivers/drv_tone_alarm.h>
#include "SafetyButton.hpp"

#ifndef GPIO_BTN_SAFETY
#error "board needs to define a safety button gpio pin to use this module"
#endif

using namespace time_literals;

static constexpr uint8_t CYCLE_COUNT{10}; /* safety switch must be held for 1 second to activate */
static constexpr uint8_t CYCLE_COUNT{30}; /* safety switch must be held for 1 second to activate */

// Define the various LED flash sequences for each system state.
enum class LED_PATTERN : uint16_t {
Expand All @@ -46,6 +51,16 @@ enum class LED_PATTERN : uint16_t {
IO_FMU_ARMED = 0xffff, /**< constantly on */
};

SafetyButton::SafetyButton() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
_safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);

if (_safety_disabled) {
_safety_btn_off = true;
}
}

SafetyButton::~SafetyButton()
{
ScheduleClear();
Expand All @@ -54,8 +69,7 @@ SafetyButton::~SafetyButton()
void
SafetyButton::CheckButton()
{
// Debounce the safety button, change state if it has been held for long enough.
bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);
const bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);

/*
* Keep pressed for a while to arm.
Expand Down Expand Up @@ -89,6 +103,63 @@ SafetyButton::CheckButton()
} else {
_button_counter = 0;
}

CheckPairingRequest(safety_button_pressed);

_safety_btn_prev_sate = safety_button_pressed;
}

void
SafetyButton::CheckPairingRequest(bool button_pressed)
{
// Need to press the button 3 times within 2 seconds
const hrt_abstime now = hrt_absolute_time();

if (now - _pairing_start > 2_s) {
// reset state
_pairing_start = 0;
_pairing_button_counter = 0;
}

if (!_safety_btn_prev_sate && button_pressed) {
if (_pairing_start == 0) {
_pairing_start = now;
}

++_pairing_button_counter;
}


if (_pairing_button_counter == 3) {


vehicle_command_s vcmd{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
vcmd.timestamp = now;
vcmd.param1 = 10.f; // GCS pairing request handled by a companion.
_to_command.publish(vcmd);
PX4_DEBUG("Sending GCS pairing request");

led_control_s led_control{};
led_control.led_mask = 0xff;
led_control.mode = led_control_s::MODE_BLINK_FAST;
led_control.color = led_control_s::COLOR_GREEN;
led_control.num_blinks = 1;
led_control.priority = 0;
led_control.timestamp = hrt_absolute_time();
_to_led_control.publish(led_control);

tune_control_s tune_control{};
tune_control.tune_id = TONE_NOTIFY_POSITIVE_TUNE;
tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
tune_control.tune_override = 0;
tune_control.timestamp = hrt_absolute_time();
_to_tune_control.publish(tune_control);

// reset state
_pairing_start = 0;
_pairing_button_counter = 0;
}
}

void
Expand All @@ -101,7 +172,7 @@ SafetyButton::FlashButton()
// Select the appropriate LED flash pattern depending on the current arm state
LED_PATTERN pattern = LED_PATTERN::FMU_REFUSE_TO_ARM;

// cycle the blink state machine at 10Hz
// cycle the blink state machine
if (_safety_btn_off) {
if (armed.armed) {
pattern = LED_PATTERN::IO_FMU_ARMED;
Expand All @@ -118,9 +189,9 @@ SafetyButton::FlashButton()
}

// Turn the LED on if we have a 1 at the current bit position
px4_arch_gpiowrite(GPIO_LED_SAFETY, !((uint16_t)pattern & (1 << _blink_counter++)));
px4_arch_gpiowrite(GPIO_LED_SAFETY, !((uint16_t)pattern & (1 << (_blink_counter++ / 3))));

if (_blink_counter > 15) {
if (_blink_counter > 45) {
_blink_counter = 0;
}
}
Expand All @@ -133,62 +204,52 @@ SafetyButton::Run()
{
if (should_exit()) {
exit_and_cleanup();
return;
}

// read safety switch input and control safety switch LED at 10Hz
CheckButton();

// Make the safety button flash anyway, no matter if it's used or not.
FlashButton();
// control safety switch LED & publish safety topic
if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) {
FlashButton();

safety_s safety{};
safety.timestamp = hrt_absolute_time();
safety.safety_switch_available = true;
safety.safety_off = _safety_btn_off;
safety_s safety{};
safety.timestamp = hrt_absolute_time();
safety.safety_switch_available = true;
safety.safety_off = _safety_btn_off || _safety_disabled;

// publish the safety status
_to_safety.publish(safety);
// publish the safety status
_to_safety.publish(safety);
}
}

int
SafetyButton::task_spawn(int argc, char *argv[])
{
if (PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) {
PX4_ERR("not starting (use px4io for safety button)");

return PX4_ERROR;

} else if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
PX4_WARN("disabled by CBRK_IO_SAFETY, exiting");
return PX4_ERROR;
SafetyButton *instance = new SafetyButton();

} else {
SafetyButton *instance = new SafetyButton();

if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;

if (instance->Start() == PX4_OK) {
return PX4_OK;
}
if (!instance) {
PX4_ERR("alloc failed");
return -1;
}

} else {
PX4_ERR("alloc failed");
}
int ret = instance->Start();

if (ret != PX4_OK) {
delete instance;
_object.store(nullptr);
_task_id = -1;
return ret;
}

return PX4_ERROR;
_object.store(instance);
_task_id = task_id_is_work_queue;

return ret;
}

int
SafetyButton::Start()
{
ScheduleOnInterval(100_ms); // run at 10 Hz
ScheduleOnInterval(33_ms); // run at 30 Hz

return PX4_OK;
}
Expand All @@ -199,6 +260,15 @@ SafetyButton::custom_command(int argc, char *argv[])
return print_usage("unknown command");
}

int
SafetyButton::print_status()
{
PX4_INFO("Safety Disabled: %s", _safety_disabled ? "yes" : "no");
PX4_INFO("Safety State (from button): %s", _safety_btn_off ? "off" : "on");

return 0;
}

int
SafetyButton::print_usage(const char *reason)
{
Expand All @@ -210,16 +280,21 @@ SafetyButton::print_usage(const char *reason)
R"DESCR_STR(
### Description
This module is responsible for the safety button.
Pressing the safety button 3 times quickly will trigger a GCS pairing request.

)DESCR_STR");

PRINT_MODULE_USAGE_NAME("safety_button", "driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the safety button driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

return 0;
}

extern "C" __EXPORT int safety_button_main(int argc, char *argv[])
extern "C" __EXPORT int safety_button_main(int argc, char *argv[]);

int
safety_button_main(int argc, char *argv[])
{
return SafetyButton::main(argc, argv);
}
21 changes: 19 additions & 2 deletions src/drivers/safety_button/SafetyButton.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,14 +43,18 @@
#include <px4_platform_common/module.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/led_control.h>
#include <uORB/topics/tune_control.h>

class SafetyButton : public ModuleBase<SafetyButton>, public px4::ScheduledWorkItem
{
public:
SafetyButton() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) {}
SafetyButton();
virtual ~SafetyButton();

/** @see ModuleBase */
Expand All @@ -62,6 +66,9 @@ class SafetyButton : public ModuleBase<SafetyButton>, public px4::ScheduledWorkI
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);

/** @see ModuleBase::print_status() */
int print_status() override;

void Run() override;

int Start();
Expand All @@ -70,14 +77,24 @@ class SafetyButton : public ModuleBase<SafetyButton>, public px4::ScheduledWorkI

void CheckButton();
void FlashButton();
void CheckPairingRequest(bool button_pressed);


uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
uORB::Publication<safety_s> _to_safety{ORB_ID(safety)};
uORB::PublicationQueued<vehicle_command_s> _to_command{ORB_ID(vehicle_command)};
uORB::PublicationQueued<led_control_s> _to_led_control{ORB_ID(led_control)};
uORB::Publication<tune_control_s> _to_tune_control{ORB_ID(tune_control)};


uint8_t _button_counter{0};
uint8_t _blink_counter{0};
bool _safety_off{false}; ///< State of the safety button from the subscribed safety topic
bool _safety_disabled{false}; ///< circuit breaker to disable the safety button
bool _safety_btn_off{false}; ///< State of the safety button read from the HW button
bool _safety_btn_prev_sate{false}; ///< Previous state of the HW button

// Pairing request
hrt_abstime _pairing_start{0};
int _pairing_button_counter{0};

};