From e6721ba84967c975318e6e3234fedad9cc9bf7b7 Mon Sep 17 00:00:00 2001 From: latercomer Date: Thu, 29 Aug 2024 10:59:40 +0800 Subject: [PATCH] =?UTF-8?q?safety=5Fbutton=E9=A9=B1=E5=8A=A8=E5=8F=AF?= =?UTF-8?q?=E4=BB=A5=E7=BC=96=E8=AF=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- apps/peripheral/actuator/Kconfig | 3 + apps/peripheral/battery/Kconfig | 2 + apps/peripheral/notification/Kconfig | 3 + .../notification/safety_button/CMakeLists.txt | 42 - .../notification/safety_button/Kconfig | 17 +- .../notification/safety_button/SConscript | 13 + .../safety_button/SafetyButton.cpp | 303 +- .../safety_button/SafetyButton.hpp | 101 +- apps/peripheral/payload/Kconfig | 2 + apps/peripheral/radio_control/Kconfig | 3 + apps/peripheral/sensor/Kconfig | 4 +- bsps/px4/fmu-v5/config/safety.config | 1584 ++ pkgs/param/interface/param_autogen.hpp | 3728 ++-- pkgs/param/module_params.c | 15148 ++++++++-------- 14 files changed, 11373 insertions(+), 9580 deletions(-) create mode 100644 apps/peripheral/actuator/Kconfig create mode 100644 apps/peripheral/notification/Kconfig delete mode 100644 apps/peripheral/notification/safety_button/CMakeLists.txt create mode 100644 apps/peripheral/notification/safety_button/SConscript create mode 100644 apps/peripheral/radio_control/Kconfig create mode 100644 bsps/px4/fmu-v5/config/safety.config diff --git a/apps/peripheral/actuator/Kconfig b/apps/peripheral/actuator/Kconfig new file mode 100644 index 0000000000..7c00cef232 --- /dev/null +++ b/apps/peripheral/actuator/Kconfig @@ -0,0 +1,3 @@ +menu "actuator" +orsource "*/Kconfig" +endmenu diff --git a/apps/peripheral/battery/Kconfig b/apps/peripheral/battery/Kconfig index 3cc3964a94..a056aecd51 100644 --- a/apps/peripheral/battery/Kconfig +++ b/apps/peripheral/battery/Kconfig @@ -1 +1,3 @@ +menu "battery" orsource "*/Kconfig" +endmenu diff --git a/apps/peripheral/notification/Kconfig b/apps/peripheral/notification/Kconfig new file mode 100644 index 0000000000..3429adab02 --- /dev/null +++ b/apps/peripheral/notification/Kconfig @@ -0,0 +1,3 @@ +menu "notification" +orsource "*/Kconfig" +endmenu diff --git a/apps/peripheral/notification/safety_button/CMakeLists.txt b/apps/peripheral/notification/safety_button/CMakeLists.txt deleted file mode 100644 index 06083e96a0..0000000000 --- a/apps/peripheral/notification/safety_button/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_module( - MODULE drivers__safety_button - MAIN safety_button - SRCS - SafetyButton.cpp - DEPENDS - button_publisher - px4_work_queue - ) diff --git a/apps/peripheral/notification/safety_button/Kconfig b/apps/peripheral/notification/safety_button/Kconfig index e891c6f96c..a13e9b2125 100644 --- a/apps/peripheral/notification/safety_button/Kconfig +++ b/apps/peripheral/notification/safety_button/Kconfig @@ -1,5 +1,18 @@ -menuconfig DRIVERS_SAFETY_BUTTON +menuconfig DRV_USING_SAFETY_BUTTON bool "safety_button" + select PKG_USING_BUTTON_PUBLISHER default n ---help--- - Enable support for safety_button \ No newline at end of file + Enable support for safety_button + +if DRV_USING_SAFETY_BUTTON + comment "fast get pin number: (x - A) * 16 + n" + config GPIO_SAFETY_SWITCH_IN + int "safety button in pin (PE10=74)" + default 74 + + config GPIO_SAFETY_SWITCH_LED + int "safety led out pin (PE12=76)" + default 76 + +endif diff --git a/apps/peripheral/notification/safety_button/SConscript b/apps/peripheral/notification/safety_button/SConscript new file mode 100644 index 0000000000..2672da68ca --- /dev/null +++ b/apps/peripheral/notification/safety_button/SConscript @@ -0,0 +1,13 @@ +import os +import sys +import subprocess +from building import * + +cwd = GetCurrentDir() + +inc = [] +src = Glob("*.cpp") + +objs = DefineGroup("drv/safty_button", src, depend=["DRV_USING_SAFETY_BUTTON"]) + +Return("objs") diff --git a/apps/peripheral/notification/safety_button/SafetyButton.cpp b/apps/peripheral/notification/safety_button/SafetyButton.cpp index aa5c45e8d9..3326225aff 100644 --- a/apps/peripheral/notification/safety_button/SafetyButton.cpp +++ b/apps/peripheral/notification/safety_button/SafetyButton.cpp @@ -1,41 +1,21 @@ -/**************************************************************************** +/***************************************************************** + * _ __ __ ____ _ __ __ + * / | / /___ _ __ / /_ / __ \ (_)/ /____ / /_ + * / |/ // _ \ | |/_// __// /_/ // // // __ \ / __/ + * / /| // __/_> < / /_ / ____// // // /_/ // /_ + * /_/ |_/ \___//_/|_| \__//_/ /_//_/ \____/ \__/ * - * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ + * Copyright All Reserved © 2015-2024 NextPilot Development Team + ******************************************************************/ -#include +#define LOG_TAG "safety" +#define LOG_LVL LOG_LVL_INFO + +#include #include "SafetyButton.hpp" -#ifndef GPIO_BTN_SAFETY -#error "board needs to define a safety button gpio pin to use this module" +#ifndef GPIO_SAFETY_SWITCH_IN +# error "board needs to define a safety button gpio pin to use this module" #endif using namespace time_literals; @@ -44,191 +24,162 @@ static constexpr uint8_t CYCLE_COUNT{30}; /* safety switch must be held for 1 se // Define the various LED flash sequences for each system state. enum class LED_PATTERN : uint16_t { - FMU_OK_TO_ARM = 0x0003, /**< slow blinking */ - FMU_REFUSE_TO_ARM = 0x5555, /**< fast blinking */ - IO_ARMED = 0x5050, /**< long off, then double blink */ - FMU_ARMED = 0x5500, /**< long off, then quad blink */ - IO_FMU_ARMED = 0xffff, /**< constantly on */ + FMU_OK_TO_ARM = 0x0003, /**< slow blinking */ + FMU_REFUSE_TO_ARM = 0x5555, /**< fast blinking */ + IO_ARMED = 0x5050, /**< long off, then double blink */ + FMU_ARMED = 0x5500, /**< long off, then quad blink */ + IO_FMU_ARMED = 0xffff, /**< constantly on */ }; SafetyButton::SafetyButton() : - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) -{ - _has_px4io = PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO); + WorkItemScheduled(MODULE_NAME, nextpilot::wq_configurations::hp_default) { + _has_px4io = false; + //PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO); } -SafetyButton::~SafetyButton() -{ - ScheduleClear(); +SafetyButton::~SafetyButton() { + ScheduleClear(); } -void -SafetyButton::CheckSafetyRequest(bool button_pressed) -{ - /* Keep button pressed for one second to turn off safety */ - if (button_pressed) { - - if (_button_counter <= CYCLE_COUNT) { - _button_counter++; - } +void SafetyButton::CheckSafetyRequest(bool button_pressed) { + /* Keep button pressed for one second to turn off safety */ + if (button_pressed) { + if (_button_counter <= CYCLE_COUNT) { + _button_counter++; + } - if (_button_counter == CYCLE_COUNT) { - _button_publisher.safetyButtonTriggerEvent(); - } + if (_button_counter == CYCLE_COUNT) { + _button_publisher.safetyButtonTriggerEvent(); + } - } else { - _button_counter = 0; - } + } else { + _button_counter = 0; + } } -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 (!_button_prev_sate && button_pressed) { - if (_pairing_start == 0) { - _pairing_start = now; - } - - ++_pairing_button_counter; - } - - if (_pairing_button_counter == ButtonPublisher::PAIRING_BUTTON_EVENT_COUNT) { - _button_publisher.pairingButtonTriggerEvent(); - // reset state - _pairing_start = 0; - _pairing_button_counter = 0; - } +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 (!_button_prev_sate && button_pressed) { + if (_pairing_start == 0) { + _pairing_start = now; + } + + ++_pairing_button_counter; + } + + if (_pairing_button_counter == ButtonPublisher::PAIRING_BUTTON_EVENT_COUNT) { + _button_publisher.pairingButtonTriggerEvent(); + // reset state + _pairing_start = 0; + _pairing_button_counter = 0; + } } -void -SafetyButton::FlashButton() -{ -#if defined(GPIO_LED_SAFETY) - actuator_armed_s armed; +void SafetyButton::FlashButton() { +#if defined(GPIO_SAFETY_SWITCH_LED) + actuator_armed_s armed; - if (_armed_sub.copy(&armed)) { - // Select the appropriate LED flash pattern depending on the current arm state - LED_PATTERN pattern = LED_PATTERN::FMU_REFUSE_TO_ARM; + if (_armed_sub.copy(&armed)) { + // 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 - if (_button_prev_sate) { - if (armed.armed) { - pattern = LED_PATTERN::IO_FMU_ARMED; + // cycle the blink state machine + if (_button_prev_sate) { + if (armed.armed) { + pattern = LED_PATTERN::IO_FMU_ARMED; - } else { - pattern = LED_PATTERN::IO_ARMED; - } + } else { + pattern = LED_PATTERN::IO_ARMED; + } - } else if (armed.armed) { - pattern = LED_PATTERN::FMU_ARMED; + } else if (armed.armed) { + pattern = LED_PATTERN::FMU_ARMED; - } else { - pattern = LED_PATTERN::FMU_OK_TO_ARM; - } + } else { + pattern = LED_PATTERN::FMU_OK_TO_ARM; + } - // 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++ / 3)))); + // Turn the LED on if we have a 1 at the current bit position + rt_pin_write(GPIO_SAFETY_SWITCH_LED, !((uint16_t)pattern & (1 << (_blink_counter++ / 3)))); - if (_blink_counter > 45) { - _blink_counter = 0; - } - } + if (_blink_counter > 45) { + _blink_counter = 0; + } + } -#endif // GPIO_LED_SAFETY +#endif // GPIO_SAFETY_SWITCH_LED } -void -SafetyButton::Run() -{ - if (should_exit()) { - exit_and_cleanup(); - return; - } +void SafetyButton::Run() { + if (should_exit()) { + exit_and_cleanup(); + return; + } - const bool button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY); + const bool button_pressed = rt_pin_read(GPIO_SAFETY_SWITCH_IN); - // control safety switch LED & safety button - if (!_has_px4io) { - FlashButton(); - CheckSafetyRequest(button_pressed); - } + // control safety switch LED & safety button + if (!_has_px4io) { + FlashButton(); + CheckSafetyRequest(button_pressed); + } - CheckPairingRequest(button_pressed); - _button_prev_sate = button_pressed; + CheckPairingRequest(button_pressed); + _button_prev_sate = button_pressed; } -int -SafetyButton::task_spawn(int argc, char *argv[]) -{ - SafetyButton *instance = new SafetyButton(); - - if (!instance) { - PX4_ERR("alloc failed"); - return -1; - } - - int ret = instance->Start(); +int SafetyButton::init() { + rt_pin_mode(GPIO_SAFETY_SWITCH_IN, PIN_MODE_INPUT_PULLUP); + rt_pin_mode(GPIO_SAFETY_SWITCH_LED, PIN_MODE_OUTPUT); - if (ret != PX4_OK) { - delete instance; - return ret; - } + ScheduleOnInterval(33_ms); // run at 30 Hz - _object.store(instance); - _task_id = task_id_is_work_queue; - - return ret; -} - -int -SafetyButton::Start() -{ - ScheduleOnInterval(33_ms); // run at 30 Hz - - return PX4_OK; + return RT_EOK; } -int -SafetyButton::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); +int SafetyButton::custom_command(int argc, char *argv[]) { + return print_usage("unknown command"); } -int -SafetyButton::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } +int SafetyButton::print_usage(const char *reason) { + if (reason) { + LOG_RAW("%s\n", reason); + } - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( + PRINT_MODULE_DESCRIPTION( + 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("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + PRINT_MODULE_USAGE_NAME("safety_button", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - return 0; + return 0; } -extern "C" __EXPORT int safety_button_main(int argc, char *argv[]); +static int safety_button_main(int argc, char *argv[]) { + return SafetyButton::main(argc, argv); +} + +MSH_CMD_EXPORT_ALIAS(safety_button_main, safety, safety button); -int -safety_button_main(int argc, char *argv[]) -{ - return SafetyButton::main(argc, argv); +static int safety_button_start() { + const char *argv[] = {"safety_button", "start"}; + int argc = sizeof(argv) / sizeof(argv[0]); + + return SafetyButton::main(argc, (char **)argv); } + +INIT_APP_EXPORT(safety_button_start); diff --git a/apps/peripheral/notification/safety_button/SafetyButton.hpp b/apps/peripheral/notification/safety_button/SafetyButton.hpp index 0f38b6ba82..5c30da6f88 100644 --- a/apps/peripheral/notification/safety_button/SafetyButton.hpp +++ b/apps/peripheral/notification/safety_button/SafetyButton.hpp @@ -1,79 +1,58 @@ -/**************************************************************************** +/***************************************************************** + * _ __ __ ____ _ __ __ + * / | / /___ _ __ / /_ / __ \ (_)/ /____ / /_ + * / |/ // _ \ | |/_// __// /_/ // // // __ \ / __/ + * / /| // __/_> < / /_ / ____// // // /_/ // /_ + * /_/ |_/ \___//_/|_| \__//_/ /_//_/ \____/ \__/ * - * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ + * Copyright All Reserved © 2015-2024 NextPilot Development Team + ******************************************************************/ -#pragma once +#ifndef __SAFETYBUTTON_H__ +#define __SAFETYBUTTON_H__ #include - -#include -#include -#include +#include +#include +#include +#include +#include #include