diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 816b566c510..94c654369ae 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -64,9 +64,6 @@ RCInput::~RCInput() { orb_unadvertise(_to_input_rc); - orb_unsubscribe(_adc_sub); - orb_unsubscribe(_vehicle_cmd_sub); - #ifdef RC_SERIAL_PORT dsm_deinit(); #endif @@ -82,15 +79,13 @@ RCInput::~RCInput() int RCInput::init() { - _adc_sub = orb_subscribe(ORB_ID(adc_report)); - #ifdef RC_SERIAL_PORT # ifdef RF_RADIO_POWER_CONTROL // power radio on RF_RADIO_POWER_CONTROL(true); # endif - _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + // dsm_init sets some file static variables and returns a file descriptor _rcs_fd = dsm_init(RC_SERIAL_PORT); // assume SBUS input and immediately switch it to @@ -297,13 +292,9 @@ RCInput::cycle() #if defined(SPEKTRUM_POWER) /* vehicle command */ - bool vehicle_command_updated = false; - orb_check(_vehicle_cmd_sub, &vehicle_command_updated); - - if (vehicle_command_updated) { - vehicle_command_s vcmd = {}; - orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub, &vcmd); + vehicle_command_s vcmd; + if (_vehicle_cmd_sub.update(&vcmd)) { // Check for a pairing command if ((unsigned int)vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) { if (!_rc_scan_locked /* !_armed.armed */) { // TODO: add armed check? @@ -336,13 +327,9 @@ RCInput::cycle() /* update ADC sampling */ #ifdef ADC_RC_RSSI_CHANNEL - bool adc_updated = false; - orb_check(_adc_sub, &adc_updated); + adc_report_s adc; - if (adc_updated) { - - struct adc_report_s adc; - orb_copy(ORB_ID(adc_report), _adc_sub, &adc); + if (_adc_sub.update(&adc)) { const unsigned adc_chans = sizeof(adc.channel_id) / sizeof(adc.channel_id[0]); for (unsigned i = 0; i < adc_chans; i++) { diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index fed21b51ccb..12c7632bd92 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -121,8 +121,8 @@ private: static struct work_s _work; - int _vehicle_cmd_sub{-1}; - int _adc_sub{-1}; + uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _adc_sub{ORB_ID(adc_report)}; input_rc_s _rc_in{}; diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 46bf069c9f3..8aca7162ffa 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -34,23 +34,11 @@ #include "crsf_telemetry.h" #include -CRSFTelemetry::CRSFTelemetry(int uart_fd) - : _vehicle_gps_position_sub(orb_subscribe(ORB_ID(vehicle_gps_position))), - _battery_status_sub(orb_subscribe(ORB_ID(battery_status))), - _vehicle_attitude_sub(orb_subscribe(ORB_ID(vehicle_attitude))), - _vehicle_status_sub(orb_subscribe(ORB_ID(vehicle_status))), - _uart_fd(uart_fd) +CRSFTelemetry::CRSFTelemetry(int uart_fd) : + _uart_fd(uart_fd) { } -CRSFTelemetry::~CRSFTelemetry() -{ - orb_unsubscribe(_vehicle_gps_position_sub); - orb_unsubscribe(_battery_status_sub); - orb_unsubscribe(_vehicle_attitude_sub); - orb_unsubscribe(_vehicle_status_sub); -} - bool CRSFTelemetry::update(const hrt_abstime &now) { const int update_rate_hz = 10; @@ -89,7 +77,7 @@ bool CRSFTelemetry::send_battery() { battery_status_s battery_status; - if (orb_copy(ORB_ID(battery_status), _battery_status_sub, &battery_status) != 0) { + if (!_battery_status_sub.update(&battery_status)) { return false; } @@ -104,7 +92,7 @@ bool CRSFTelemetry::send_gps() { vehicle_gps_position_s vehicle_gps_position; - if (orb_copy(ORB_ID(vehicle_gps_position), _vehicle_gps_position_sub, &vehicle_gps_position) != 0) { + if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) { return false; } @@ -123,7 +111,7 @@ bool CRSFTelemetry::send_attitude() { vehicle_attitude_s vehicle_attitude; - if (orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude) != 0) { + if (!_vehicle_attitude_sub.update(&vehicle_attitude)) { return false; } @@ -138,7 +126,7 @@ bool CRSFTelemetry::send_flight_mode() { vehicle_status_s vehicle_status; - if (orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status) != 0) { + if (!_vehicle_status_sub.update(&vehicle_status)) { return false; } diff --git a/src/drivers/rc_input/crsf_telemetry.h b/src/drivers/rc_input/crsf_telemetry.h index 3a0e989e6f9..6a6e3d78074 100644 --- a/src/drivers/rc_input/crsf_telemetry.h +++ b/src/drivers/rc_input/crsf_telemetry.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include @@ -62,7 +63,7 @@ public: */ CRSFTelemetry(int uart_fd); - ~CRSFTelemetry(); + ~CRSFTelemetry() = default; /** * Send telemetry data. Call this regularly (i.e. at 100Hz), it will automatically @@ -77,10 +78,10 @@ private: bool send_attitude(); bool send_flight_mode(); - int _vehicle_gps_position_sub; - int _battery_status_sub; - int _vehicle_attitude_sub; - int _vehicle_status_sub; + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; hrt_abstime _last_update{0};