diff --git a/boards/cuav/can-gps-v1/init/rc.board_defaults b/boards/cuav/can-gps-v1/init/rc.board_defaults index 5f510e1f92..e5e2c570d2 100644 --- a/boards/cuav/can-gps-v1/init/rc.board_defaults +++ b/boards/cuav/can-gps-v1/init/rc.board_defaults @@ -2,14 +2,13 @@ # # board specific defaults #------------------------------------------------------------------------------ -neopixel start -tone_alarm start if [ $AUTOCNF = yes ] then - # Disable safety switch by default - param set CBRK_IO_SAFETY 22027 fi -#safety_button start +neopixel start +safety_button start +tone_alarm start + diff --git a/boards/cuav/can-gps-v1/src/board_config.h b/boards/cuav/can-gps-v1/src/board_config.h index f19d43cdf7..ce2c932a66 100644 --- a/boards/cuav/can-gps-v1/src/board_config.h +++ b/boards/cuav/can-gps-v1/src/board_config.h @@ -61,7 +61,7 @@ #define GPIO_CAN1_SILENT_S0 /* PB5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) #define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) -#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN3) +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN3) /* Tone alarm output. */ #define TONE_ALARM_TIMER 2 /* timer 2 */ diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index a15d32d303..9db9c176d0 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -94,7 +94,12 @@ target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC # generated DSDL -set(DSDLC_INPUTS "${CMAKE_CURRENT_SOURCE_DIR}/dsdl/com" "${CMAKE_CURRENT_SOURCE_DIR}/dsdl/ardupilot" "${LIBUAVCAN_DIR}/dsdl/uavcan") +set(DSDLC_DIR "${CMAKE_CURRENT_SOURCE_DIR}/dsdl") +set(DSDLC_INPUTS + "${DSDLC_DIR}/com" + "${DSDLC_DIR}/standard" + "${LIBUAVCAN_DIR}/dsdl/uavcan" +) set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated") set(DSDLC_INPUT_FILES) diff --git a/src/drivers/uavcan/dsdl/ardupilot/indication/20000.SafetyState.uavcan b/src/drivers/uavcan/dsdl/standard/indication/21000.SafetyState.uavcan similarity index 100% rename from src/drivers/uavcan/dsdl/ardupilot/indication/20000.SafetyState.uavcan rename to src/drivers/uavcan/dsdl/standard/indication/21000.SafetyState.uavcan diff --git a/src/drivers/uavcan/dsdl/standard/indication/21001.Button.uavcan b/src/drivers/uavcan/dsdl/standard/indication/21001.Button.uavcan new file mode 100644 index 0000000000..879da4803f --- /dev/null +++ b/src/drivers/uavcan/dsdl/standard/indication/21001.Button.uavcan @@ -0,0 +1,11 @@ +# +# support for buttons on UAVCAN +# while a button is being pressed this message should be sent at 10Hz + +uint8 BUTTON_SAFETY = 1 + +uint8 button + +# number of 0.1s units the button has been pressed for. If over 255 +# then send 255 +uint8 press_time diff --git a/src/drivers/uavcan/safety_state.cpp b/src/drivers/uavcan/safety_state.cpp index 3e293c22da..9ef6032071 100644 --- a/src/drivers/uavcan/safety_state.cpp +++ b/src/drivers/uavcan/safety_state.cpp @@ -63,7 +63,7 @@ void UavcanSafetyState::periodic_update(const uavcan::TimerEvent &) actuator_armed_s actuator_armed; if (_actuator_armed_sub.update(&actuator_armed)) { - ardupilot::indication::SafetyState cmd; + standard::indication::SafetyState cmd; if (actuator_armed.armed || actuator_armed.prearmed) { cmd.status = cmd.STATUS_SAFETY_OFF; diff --git a/src/drivers/uavcan/safety_state.hpp b/src/drivers/uavcan/safety_state.hpp index 215cd99b8e..20a0b60c24 100644 --- a/src/drivers/uavcan/safety_state.hpp +++ b/src/drivers/uavcan/safety_state.hpp @@ -42,7 +42,7 @@ #pragma once #include -#include +#include #include #include @@ -74,7 +74,7 @@ private: /* * Publish CAN Safety state led */ - uavcan::Publisher _safety_state_pub; + uavcan::Publisher _safety_state_pub; uavcan::TimerEventForwarder _timer; diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt index 0829132b37..8a8d7fc9b4 100644 --- a/src/drivers/uavcannode/CMakeLists.txt +++ b/src/drivers/uavcannode/CMakeLists.txt @@ -94,7 +94,12 @@ target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC # generated DSDL -set(DSDLC_INPUTS "${PX4_SOURCE_DIR}/src/drivers/uavcan/dsdl/com" "${LIBUAVCAN_DIR}/dsdl/uavcan") +set(DSDLC_DIR "${PX4_SOURCE_DIR}/src/drivers/uavcan/dsdl") +set(DSDLC_INPUTS + "${DSDLC_DIR}/com" + "${DSDLC_DIR}/standard" + "${LIBUAVCAN_DIR}/dsdl/uavcan" +) set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated") set(DSDLC_INPUT_FILES) diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 30a60d7169..2f444646ab 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -83,6 +83,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _raw_air_data_publisher(_node), _range_sensor_measurement(_node), _flow_measurement_publisher(_node), + _indication_button_publisher(_node), _param_server(_node), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), _interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")), @@ -521,6 +522,20 @@ void UavcanNode::Run() } } + // safety -> standard::indication::button + if (_safety_sub.updated()) { + safety_s safety; + + if (_safety_sub.copy(&safety)) { + if (safety.safety_switch_available) { + standard::indication::Button Button{}; + Button.button = standard::indication::Button::BUTTON_SAFETY; + Button.press_time = safety.safety_off ? UINT8_MAX : 0; + _indication_button_publisher.broadcast(Button); + } + } + } + perf_end(_cycle_perf); pthread_mutex_unlock(&_node_mutex); diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index 8b66ad5c04..3b4dddadb7 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -66,6 +66,7 @@ #include #include +#include #include #include @@ -78,6 +79,7 @@ #include #include #include +#include #include #include #include @@ -182,6 +184,7 @@ private: uavcan::Publisher _raw_air_data_publisher; uavcan::Publisher _range_sensor_measurement; uavcan::Publisher _flow_measurement_publisher; + uavcan::Publisher _indication_button_publisher; hrt_abstime _last_static_temperature_publish{0}; @@ -197,6 +200,7 @@ private: {this, ORB_ID(distance_sensor), 3}, }; uORB::SubscriptionCallbackWorkItem _optical_flow_sub{this, ORB_ID(optical_flow)}; + uORB::SubscriptionCallbackWorkItem _safety_sub{this, ORB_ID(safety)}; uORB::SubscriptionCallbackWorkItem _sensor_baro_sub{this, ORB_ID(sensor_baro)}; uORB::SubscriptionCallbackWorkItem _sensor_mag_sub{this, ORB_ID(sensor_mag)}; uORB::SubscriptionCallbackWorkItem _sensor_gps_sub{this, ORB_ID(sensor_gps)};