diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index 8d9559f0dc9b..15ea9385a515 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -51,17 +52,15 @@ #include #include #include +#include #include #include #include -#include #include #include "tap_esc_common.h" -#define NAN_VALUE (0.0f/0.0f) - #ifndef B250000 #define B250000 250000 #endif @@ -81,7 +80,7 @@ */ static int _uart_fd = -1; //todo:refactor in to class -class TAP_ESC : public device::CDev +class TAP_ESC : public device::CDev, public ModuleParams { public: enum Mode { @@ -115,6 +114,7 @@ class TAP_ESC : public device::CDev // subscriptions int _armed_sub; int _test_motor_sub; + int _params_sub; orb_advert_t _outputs_pub; actuator_outputs_s _outputs; actuator_armed_s _armed; @@ -142,6 +142,12 @@ class TAP_ESC : public device::CDev unsigned _current_update_rate; ESC_UART_BUF uartbuf; EscPacket _packet; + + + DEFINE_PARAMETERS( + (ParamBool) _airmode ///< multicopter air-mode + ) + void subscribe(); void work_start(); @@ -169,11 +175,13 @@ TAP_ESC *tap_esc = nullptr; TAP_ESC::TAP_ESC(int channels_count): CDev("tap_esc", TAP_ESC_DEVICE_PATH), + ModuleParams(nullptr), _is_armed(false), _poll_fds_num(0), _mode(MODE_4PWM), //FIXME: what is this mode used for??? _armed_sub(-1), _test_motor_sub(-1), + _params_sub(-1), _outputs_pub(nullptr), _armed{}, _esc_feedback_pub(nullptr), @@ -576,6 +584,7 @@ TAP_ESC::cycle() _to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); _initialized = true; } @@ -613,6 +622,9 @@ TAP_ESC::cycle() _current_update_rate = max_rate; } + if (_mixers) { + _mixers->set_airmode(_airmode.get()); + } /* check if anything updated */ @@ -787,6 +799,16 @@ TAP_ESC::cycle() } + /* check for parameter updates */ + bool param_updated = false; + orb_check(_params_sub, ¶m_updated); + + if (param_updated) { + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + updateParams(); + } + } @@ -803,6 +825,7 @@ void TAP_ESC::work_stop() _armed_sub = -1; orb_unsubscribe(_test_motor_sub); _test_motor_sub = -1; + orb_unsubscribe(_params_sub); DEVICE_LOG("stopping"); _initialized = false; @@ -844,7 +867,7 @@ int TAP_ESC::control_callback(uint8_t control_group, uint8_t control_index, floa control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && control_index == actuator_controls_s::INDEX_THROTTLE) { /* set the throttle to an invalid value */ - input = NAN_VALUE; + input = NAN; } }