From 4e15f0ea906891c3790e0f9075fd2736a42168ca Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Fri, 5 Apr 2024 10:08:21 +0300 Subject: [PATCH] mavlink: update streams to use orb polling --- .../streams/ACTUATOR_OUTPUT_STATUS.hpp | 12 ++++++++- src/modules/mavlink/streams/ADSB_VEHICLE.hpp | 12 ++++++++- src/modules/mavlink/streams/ALTITUDE.hpp | 16 +++++++++++- src/modules/mavlink/streams/ATTITUDE.hpp | 15 ++++++++++- .../mavlink/streams/ATTITUDE_QUATERNION.hpp | 17 +++++++++++- .../mavlink/streams/ATTITUDE_TARGET.hpp | 16 +++++++++++- .../AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp | 20 +++++++++++++- .../mavlink/streams/BATTERY_STATUS.hpp | 14 +++++++++- .../mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp | 14 +++++++++- .../mavlink/streams/CAMERA_TRIGGER.hpp | 15 ++++++++++- src/modules/mavlink/streams/COLLISION.hpp | 14 +++++++++- src/modules/mavlink/streams/COMMAND_LONG.hpp | 14 +++++++++- src/modules/mavlink/streams/DEBUG.hpp | 14 +++++++++- .../mavlink/streams/DEBUG_FLOAT_ARRAY.hpp | 14 +++++++++- src/modules/mavlink/streams/DEBUG_VECT.hpp | 14 +++++++++- .../mavlink/streams/DISTANCE_SENSOR.hpp | 14 +++++++++- src/modules/mavlink/streams/EFI_STATUS.hpp | 14 +++++++++- src/modules/mavlink/streams/ESC_INFO.hpp | 14 +++++++++- src/modules/mavlink/streams/ESC_STATUS.hpp | 14 +++++++++- .../mavlink/streams/ESTIMATOR_STATUS.hpp | 15 ++++++++++- .../mavlink/streams/EXTENDED_SYS_STATE.hpp | 13 ++++++++++ .../mavlink/streams/FLIGHT_INFORMATION.hpp | 10 +++++++ .../streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp | 14 +++++++++- .../streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp | 14 +++++++++- .../streams/GIMBAL_MANAGER_INFORMATION.hpp | 14 +++++++++- .../mavlink/streams/GIMBAL_MANAGER_STATUS.hpp | 14 +++++++++- .../mavlink/streams/GLOBAL_POSITION_INT.hpp | 17 +++++++++++- src/modules/mavlink/streams/GPS2_RAW.hpp | 14 +++++++++- .../mavlink/streams/GPS_GLOBAL_ORIGIN.hpp | 14 +++++++++- src/modules/mavlink/streams/GPS_RAW_INT.hpp | 14 +++++++++- src/modules/mavlink/streams/GPS_RTCM_DATA.hpp | 14 +++++++++- src/modules/mavlink/streams/GPS_STATUS.hpp | 14 +++++++++- src/modules/mavlink/streams/HEARTBEAT.hpp | 16 +++++++++++- src/modules/mavlink/streams/HIGHRES_IMU.hpp | 20 +++++++++++++- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 26 +++++++++++++++++++ .../mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp | 12 +++++++++ .../mavlink/streams/HIL_STATE_QUATERNION.hpp | 17 +++++++++++- src/modules/mavlink/streams/HOME_POSITION.hpp | 14 +++++++++- .../mavlink/streams/HYGROMETER_SENSOR.hpp | 14 +++++++++- .../mavlink/streams/LANDING_TARGET.hpp | 14 +++++++++- .../mavlink/streams/LOCAL_POSITION_NED.hpp | 14 +++++++++- .../mavlink/streams/MAG_CAL_REPORT.hpp | 15 ++++++++++- .../mavlink/streams/MANUAL_CONTROL.hpp | 15 ++++++++++- .../mavlink/streams/MOUNT_ORIENTATION.hpp | 15 ++++++++++- .../mavlink/streams/NAMED_VALUE_FLOAT.hpp | 14 +++++++++- .../mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp | 16 +++++++++++- .../mavlink/streams/OBSTACLE_DISTANCE.hpp | 14 +++++++++- src/modules/mavlink/streams/ODOMETRY.hpp | 14 +++++++++- .../streams/OPEN_DRONE_ID_BASIC_ID.hpp | 14 +++++++++- .../streams/OPEN_DRONE_ID_LOCATION.hpp | 19 +++++++++++++- .../mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp | 15 ++++++++++- .../mavlink/streams/OPTICAL_FLOW_RAD.hpp | 14 +++++++++- .../streams/ORBIT_EXECUTION_STATUS.hpp | 14 +++++++++- .../streams/POSITION_TARGET_GLOBAL_INT.hpp | 16 +++++++++++- .../streams/POSITION_TARGET_LOCAL_NED.hpp | 14 +++++++++- src/modules/mavlink/streams/RAW_RPM.hpp | 14 +++++++++- src/modules/mavlink/streams/RC_CHANNELS.hpp | 14 +++++++++- src/modules/mavlink/streams/SCALED_IMU.hpp | 16 +++++++++++- src/modules/mavlink/streams/SCALED_IMU2.hpp | 16 +++++++++++- src/modules/mavlink/streams/SCALED_IMU3.hpp | 16 +++++++++++- .../mavlink/streams/SCALED_PRESSURE.hpp | 15 ++++++++++- .../mavlink/streams/SCALED_PRESSURE2.hpp | 15 ++++++++++- .../mavlink/streams/SCALED_PRESSURE3.hpp | 15 ++++++++++- .../mavlink/streams/SERVO_OUTPUT_RAW.hpp | 14 +++++++++- .../mavlink/streams/SMART_BATTERY_INFO.hpp | 14 +++++++++- src/modules/mavlink/streams/STATUSTEXT.hpp | 10 ++++++- src/modules/mavlink/streams/SYS_STATUS.hpp | 17 +++++++++++- .../streams/TIME_ESTIMATE_TO_TARGET.hpp | 14 +++++++++- .../TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp | 14 +++++++++- .../streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp | 15 ++++++++++- .../mavlink/streams/UTM_GLOBAL_POSITION.hpp | 18 ++++++++++++- src/modules/mavlink/streams/VFR_HUD.hpp | 18 ++++++++++++- src/modules/mavlink/streams/VIBRATION.hpp | 15 ++++++++++- src/modules/mavlink/streams/WIND_COV.hpp | 15 ++++++++++- 74 files changed, 1029 insertions(+), 70 deletions(-) diff --git a/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp b/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp index 5c5cfbdefd7f..76ced56aa243 100644 --- a/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp +++ b/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp @@ -53,7 +53,17 @@ class MavlinkStreamActuatorOutputStatus : public MavlinkStream } private: - explicit MavlinkStreamActuatorOutputStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamActuatorOutputStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamActuatorOutputStatus() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { ORB_ID::actuator_outputs }; uORB::Subscription _act_output_sub{ORB_ID(actuator_outputs)}; diff --git a/src/modules/mavlink/streams/ADSB_VEHICLE.hpp b/src/modules/mavlink/streams/ADSB_VEHICLE.hpp index 56f2a94e42a6..16353299fa7c 100644 --- a/src/modules/mavlink/streams/ADSB_VEHICLE.hpp +++ b/src/modules/mavlink/streams/ADSB_VEHICLE.hpp @@ -55,7 +55,17 @@ class MavlinkStreamADSBVehicle : public MavlinkStream } private: - explicit MavlinkStreamADSBVehicle(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamADSBVehicle(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamADSBVehicle() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { ORB_ID::transponder_report }; uORB::Subscription _transponder_report_sub{ORB_ID(transponder_report)}; diff --git a/src/modules/mavlink/streams/ALTITUDE.hpp b/src/modules/mavlink/streams/ALTITUDE.hpp index f3c437d0d9ae..ea88da73ba26 100644 --- a/src/modules/mavlink/streams/ALTITUDE.hpp +++ b/src/modules/mavlink/streams/ALTITUDE.hpp @@ -56,7 +56,21 @@ class MavlinkStreamAltitude : public MavlinkStream } private: - explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamAltitude() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[3] { + ORB_ID::vehicle_air_data, + ORB_ID::home_position, + ORB_ID::vehicle_local_position + }; uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _home_sub{ORB_ID(home_position)}; diff --git a/src/modules/mavlink/streams/ATTITUDE.hpp b/src/modules/mavlink/streams/ATTITUDE.hpp index 94c1f4a42209..7b0380eaaa39 100644 --- a/src/modules/mavlink/streams/ATTITUDE.hpp +++ b/src/modules/mavlink/streams/ATTITUDE.hpp @@ -54,7 +54,20 @@ class MavlinkStreamAttitude : public MavlinkStream } private: - explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamAttitude() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::vehicle_attitude, + ORB_ID::vehicle_angular_velocity + }; uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; diff --git a/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp b/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp index 64189419ad1e..6d6e54d3158a 100644 --- a/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp +++ b/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp @@ -55,7 +55,22 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream } private: - explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamAttitudeQuaternion() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + + ORB_ID _orbs[3] { + ORB_ID::vehicle_attitude, + ORB_ID::vehicle_angular_velocity, + ORB_ID::vehicle_status + }; uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; diff --git a/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp b/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp index dd93fc8dfe00..deaaee7ef1f1 100644 --- a/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp +++ b/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp @@ -54,7 +54,21 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream } private: - explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamAttitudeTarget() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + + ORB_ID _orbs[2] { + ORB_ID::vehicle_attitude_setpoint, + ORB_ID::vehicle_rates_setpoint + }; uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _att_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; diff --git a/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp b/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp index 5e4dea2a03d1..eb015f89b3bf 100644 --- a/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp +++ b/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp @@ -62,7 +62,25 @@ class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream } private: - explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamAutopilotStateForGimbalDevice() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[6] { + ORB_ID::estimator_selector_status, + ORB_ID::estimator_status, + ORB_ID::vehicle_attitude, + ORB_ID::vehicle_local_position, + ORB_ID::vehicle_attitude_setpoint, + ORB_ID::vehicle_land_detected + }; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 73068f0de360..c59762fcf3ba 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -54,7 +54,19 @@ class MavlinkStreamBatteryStatus : public MavlinkStream } private: - explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamBatteryStatus() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::battery_status + }; uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; diff --git a/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp b/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp index fede0f3c27df..b1bb255d66a2 100644 --- a/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp +++ b/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp @@ -55,7 +55,19 @@ class MavlinkStreamCameraImageCaptured : public MavlinkStream } private: - explicit MavlinkStreamCameraImageCaptured(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamCameraImageCaptured(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamCameraImageCaptured() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::camera_capture + }; uORB::Subscription _capture_sub{ORB_ID(camera_capture)}; diff --git a/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp index e64a772d2d22..f5f45a1910e5 100644 --- a/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp +++ b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp @@ -61,7 +61,20 @@ class MavlinkStreamCameraTrigger : public MavlinkStream } private: - explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamCameraTrigger() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::camera_trigger, + ORB_ID::camera_status + }; uORB::Subscription _camera_trigger_sub{ORB_ID(camera_trigger)}; uORB::Subscription _camera_status_sub{ORB_ID(camera_status)}; diff --git a/src/modules/mavlink/streams/COLLISION.hpp b/src/modules/mavlink/streams/COLLISION.hpp index a3469c62d9a0..320638fe1a9a 100644 --- a/src/modules/mavlink/streams/COLLISION.hpp +++ b/src/modules/mavlink/streams/COLLISION.hpp @@ -53,7 +53,19 @@ class MavlinkStreamCollision : public MavlinkStream } private: - explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamCollision() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::collision_report + }; uORB::Subscription _collision_sub{ORB_ID(collision_report)}; diff --git a/src/modules/mavlink/streams/COMMAND_LONG.hpp b/src/modules/mavlink/streams/COMMAND_LONG.hpp index 89f4a77dbe4f..4f33575a1d9c 100644 --- a/src/modules/mavlink/streams/COMMAND_LONG.hpp +++ b/src/modules/mavlink/streams/COMMAND_LONG.hpp @@ -53,7 +53,19 @@ class MavlinkStreamCommandLong : public MavlinkStream } private: - explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamCommandLong() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::vehicle_command + }; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; diff --git a/src/modules/mavlink/streams/DEBUG.hpp b/src/modules/mavlink/streams/DEBUG.hpp index c0c2a52322d9..5e51da2bfa7e 100644 --- a/src/modules/mavlink/streams/DEBUG.hpp +++ b/src/modules/mavlink/streams/DEBUG.hpp @@ -53,7 +53,19 @@ class MavlinkStreamDebug : public MavlinkStream } private: - explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamDebug() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::debug_value + }; uORB::Subscription _debug_value_sub{ORB_ID(debug_value)}; diff --git a/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp b/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp index a5029ffe398c..75cd39e4d576 100644 --- a/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp +++ b/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp @@ -53,7 +53,19 @@ class MavlinkStreamDebugFloatArray : public MavlinkStream } private: - explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamDebugFloatArray() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::debug_array + }; uORB::Subscription _debug_array_sub{ORB_ID(debug_array)}; diff --git a/src/modules/mavlink/streams/DEBUG_VECT.hpp b/src/modules/mavlink/streams/DEBUG_VECT.hpp index 9fa293188d89..e67f6363abb1 100644 --- a/src/modules/mavlink/streams/DEBUG_VECT.hpp +++ b/src/modules/mavlink/streams/DEBUG_VECT.hpp @@ -53,7 +53,19 @@ class MavlinkStreamDebugVect : public MavlinkStream } private: - explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamDebugVect() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::debug_vect + }; uORB::Subscription _debug_sub{ORB_ID(debug_vect)}; diff --git a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp index 9042403a4868..7ed4335e41f6 100644 --- a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp +++ b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp @@ -54,7 +54,19 @@ class MavlinkStreamDistanceSensor : public MavlinkStream } private: - explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamDistanceSensor() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::distance_sensor + }; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; diff --git a/src/modules/mavlink/streams/EFI_STATUS.hpp b/src/modules/mavlink/streams/EFI_STATUS.hpp index 48ea82459cf6..69c0d68465d9 100644 --- a/src/modules/mavlink/streams/EFI_STATUS.hpp +++ b/src/modules/mavlink/streams/EFI_STATUS.hpp @@ -54,7 +54,19 @@ class MavlinkStreamEfiStatus : public MavlinkStream } private: - explicit MavlinkStreamEfiStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamEfiStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamEfiStatus() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::internal_combustion_engine_status + }; uORB::Subscription _internal_combustion_engine_status_sub{ORB_ID(internal_combustion_engine_status)}; diff --git a/src/modules/mavlink/streams/ESC_INFO.hpp b/src/modules/mavlink/streams/ESC_INFO.hpp index 609112ed0ad8..e063054992cb 100644 --- a/src/modules/mavlink/streams/ESC_INFO.hpp +++ b/src/modules/mavlink/streams/ESC_INFO.hpp @@ -54,7 +54,19 @@ class MavlinkStreamESCInfo : public MavlinkStream } private: - explicit MavlinkStreamESCInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamESCInfo(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamESCInfo() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::esc_status + }; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uint8_t _number_of_batches{0}; diff --git a/src/modules/mavlink/streams/ESC_STATUS.hpp b/src/modules/mavlink/streams/ESC_STATUS.hpp index 1e8911577823..4bfa6627ccb3 100644 --- a/src/modules/mavlink/streams/ESC_STATUS.hpp +++ b/src/modules/mavlink/streams/ESC_STATUS.hpp @@ -54,7 +54,19 @@ class MavlinkStreamESCStatus : public MavlinkStream } private: - explicit MavlinkStreamESCStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamESCStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamESCStatus() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::esc_status + }; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uint8_t _number_of_batches{0}; diff --git a/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp b/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp index 3764940c312e..b3e3021f331c 100644 --- a/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp +++ b/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp @@ -54,7 +54,20 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream } private: - explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamEstimatorStatus() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::estimator_selector_status, + ORB_ID::estimator_status + }; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; diff --git a/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp b/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp index b6cf44e63436..fc85da1498eb 100644 --- a/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp +++ b/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp @@ -60,8 +60,21 @@ class MavlinkStreamExtendedSysState : public MavlinkStream { _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); } + ~MavlinkStreamExtendedSysState() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[4] { + ORB_ID::vehicle_status, + ORB_ID::vehicle_land_detected, + ORB_ID::position_setpoint_triplet, + ORB_ID::vehicle_control_mode + }; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; diff --git a/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp b/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp index 5dd9f88c88d8..253d24b9a242 100644 --- a/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp +++ b/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp @@ -56,8 +56,18 @@ class MavlinkStreamFlightInformation : public MavlinkStream explicit MavlinkStreamFlightInformation(Mavlink *mavlink) : MavlinkStream(mavlink) { _param_com_flight_uuid = param_find("COM_FLIGHT_UUID"); + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); } + ~MavlinkStreamFlightInformation() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::vehicle_status + }; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; param_t _param_com_flight_uuid{PARAM_INVALID}; diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index 615a797488d4..876732ee4f58 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -57,7 +57,19 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream } private: - explicit MavlinkStreamGimbalDeviceAttitudeStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamGimbalDeviceAttitudeStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamGimbalDeviceAttitudeStatus() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::gimbal_device_attitude_status + }; uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp index 413e7361e6e1..b6b4655c9922 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp @@ -57,7 +57,19 @@ class MavlinkStreamGimbalDeviceSetAttitude : public MavlinkStream } private: - explicit MavlinkStreamGimbalDeviceSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamGimbalDeviceSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamGimbalDeviceSetAttitude() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::gimbal_device_set_attitude + }; uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)}; diff --git a/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp b/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp index a6570362e138..6f70e5958967 100644 --- a/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp +++ b/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp @@ -57,7 +57,19 @@ class MavlinkStreamGimbalManagerInformation : public MavlinkStream } private: - explicit MavlinkStreamGimbalManagerInformation(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamGimbalManagerInformation(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamGimbalManagerInformation() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::gimbal_manager_information + }; uORB::Subscription _gimbal_manager_information_sub{ORB_ID(gimbal_manager_information)}; diff --git a/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp index ecab79410cbc..7583e419c51e 100644 --- a/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp @@ -57,7 +57,19 @@ class MavlinkStreamGimbalManagerStatus : public MavlinkStream } private: - explicit MavlinkStreamGimbalManagerStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamGimbalManagerStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamGimbalManagerStatus() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::gimbal_manager_status + }; uORB::Subscription _gimbal_manager_status_sub{ORB_ID(gimbal_manager_status)}; diff --git a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp index 6d2997fe7188..bec081da7393 100644 --- a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp +++ b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp @@ -56,7 +56,22 @@ class MavlinkStreamGlobalPositionInt : public MavlinkStream } private: - explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamGlobalPositionInt() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[4] { + ORB_ID::vehicle_global_position, + ORB_ID::vehicle_local_position, + ORB_ID::home_position, + ORB_ID::vehicle_air_data + }; uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/mavlink/streams/GPS2_RAW.hpp b/src/modules/mavlink/streams/GPS2_RAW.hpp index eb77c8c3190d..a4cd036e9851 100644 --- a/src/modules/mavlink/streams/GPS2_RAW.hpp +++ b/src/modules/mavlink/streams/GPS2_RAW.hpp @@ -55,7 +55,19 @@ class MavlinkStreamGPS2Raw : public MavlinkStream } private: - explicit MavlinkStreamGPS2Raw(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamGPS2Raw(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamGPS2Raw() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::sensor_gps + }; uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 1}; hrt_abstime _last_send_ts {}; diff --git a/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp b/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp index fb707b3d14cc..c3fe5fbcf8e5 100644 --- a/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp +++ b/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp @@ -63,7 +63,19 @@ class MavlinkStreamGpsGlobalOrigin : public MavlinkStream } private: - explicit MavlinkStreamGpsGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamGpsGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamGpsGlobalOrigin() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::vehicle_local_position + }; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/mavlink/streams/GPS_RAW_INT.hpp b/src/modules/mavlink/streams/GPS_RAW_INT.hpp index f6887c73c992..254de12f1179 100644 --- a/src/modules/mavlink/streams/GPS_RAW_INT.hpp +++ b/src/modules/mavlink/streams/GPS_RAW_INT.hpp @@ -55,7 +55,19 @@ class MavlinkStreamGPSRawInt : public MavlinkStream } private: - explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamGPSRawInt() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::sensor_gps + }; uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 0}; hrt_abstime _last_send_ts {}; diff --git a/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp b/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp index 8730d2e88109..189f46991ade 100644 --- a/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp +++ b/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp @@ -53,7 +53,19 @@ class MavlinkStreamGPSRTCMData : public MavlinkStream } private: - explicit MavlinkStreamGPSRTCMData(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamGPSRTCMData(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamGPSRTCMData() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::gps_inject_data + }; uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data), 0}; diff --git a/src/modules/mavlink/streams/GPS_STATUS.hpp b/src/modules/mavlink/streams/GPS_STATUS.hpp index 82d36f2eae9d..bbe2c26751ae 100644 --- a/src/modules/mavlink/streams/GPS_STATUS.hpp +++ b/src/modules/mavlink/streams/GPS_STATUS.hpp @@ -53,7 +53,19 @@ class MavlinkStreamGPSStatus : public MavlinkStream } private: - explicit MavlinkStreamGPSStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamGPSStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamGPSStatus() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::satellite_info + }; uORB::Subscription _satellite_info_sub{ORB_ID(satellite_info)}; diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index 26d966a85ac5..2424648be101 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -57,7 +57,21 @@ class MavlinkStreamHeartbeat : public MavlinkStream } private: - explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamHeartbeat() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[3] { + ORB_ID::actuator_armed, + ORB_ID::vehicle_control_mode, + ORB_ID::vehicle_status + }; uORB::Subscription _acturator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; diff --git a/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/modules/mavlink/streams/HIGHRES_IMU.hpp index a6682213bf93..678e8596d1a1 100644 --- a/src/modules/mavlink/streams/HIGHRES_IMU.hpp +++ b/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -61,7 +61,25 @@ class MavlinkStreamHighresIMU : public MavlinkStream } private: - explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamHighresIMU() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[7] { + ORB_ID::vehicle_imu, + ORB_ID::estimator_sensor_bias, + ORB_ID::estimator_selector_status, + ORB_ID::sensor_selection, + ORB_ID::differential_pressure, + ORB_ID::vehicle_magnetometer, + ORB_ID::vehicle_air_data + }; uORB::SubscriptionMultiArray _vehicle_imu_subs{ORB_ID::vehicle_imu}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index c96d0b6cb9d1..e8582a680619 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -91,6 +91,12 @@ class MavlinkStreamHighLatency2 : public MavlinkStream _windspeed(SimpleAnalyzer::AVERAGE) { reset_last_sent(); + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamHighLatency2() + { + _mavlink->unregister_orb_poll(get_id_static()); } struct PerBatteryData { @@ -621,6 +627,26 @@ class MavlinkStreamHighLatency2 : public MavlinkStream msg.wp_num = UINT16_MAX; } + ORB_ID _orbs[17] { + ORB_ID::vehicle_thrust_setpoint, + ORB_ID::airspeed, + ORB_ID::vehicle_attitude_setpoint, + ORB_ID::estimator_selector_status, + ORB_ID::estimator_status, + ORB_ID::position_controller_status, + ORB_ID::geofence_result, + ORB_ID::vehicle_global_position, + ORB_ID::vehicle_local_position, + ORB_ID::vehicle_gps_position, + ORB_ID::mission_result, + ORB_ID::vehicle_status, + ORB_ID::failsafe_flags, + ORB_ID::tecs_status, + ORB_ID::wind, + ORB_ID::health_report, + ORB_ID::battery_status + }; + uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0}; uORB::Subscription _vehicle_thrust_setpoint_1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; diff --git a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp index 48d548092881..8994b757a91c 100644 --- a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp +++ b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp @@ -58,8 +58,20 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink) { _act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); } + ~MavlinkStreamHILActuatorControls() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[3] { + ORB_ID::actuator_outputs, + ORB_ID::vehicle_status, + ORB_ID::vehicle_control_mode + }; + uORB::Subscription _act_sub{ORB_ID(actuator_outputs)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; diff --git a/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp b/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp index b2dcb5c42615..d614b428469f 100644 --- a/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp +++ b/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp @@ -55,7 +55,22 @@ class MavlinkStreamHILStateQuaternion : public MavlinkStream } private: - explicit MavlinkStreamHILStateQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamHILStateQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamHILStateQuaternion() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[4] { + ORB_ID::vehicle_angular_velocity_groundtruth, + ORB_ID::vehicle_attitude_groundtruth, + ORB_ID::vehicle_global_position_groundtruth, + ORB_ID::vehicle_local_position_groundtruth + }; uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Subscription _att_sub{ORB_ID(vehicle_attitude_groundtruth)}; diff --git a/src/modules/mavlink/streams/HOME_POSITION.hpp b/src/modules/mavlink/streams/HOME_POSITION.hpp index bb67d14d2dbf..69831bf5b924 100644 --- a/src/modules/mavlink/streams/HOME_POSITION.hpp +++ b/src/modules/mavlink/streams/HOME_POSITION.hpp @@ -53,7 +53,19 @@ class MavlinkStreamHomePosition : public MavlinkStream } private: - explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamHomePosition() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::home_position + }; uORB::Subscription _home_sub{ORB_ID(home_position)}; diff --git a/src/modules/mavlink/streams/HYGROMETER_SENSOR.hpp b/src/modules/mavlink/streams/HYGROMETER_SENSOR.hpp index 9787f9dee279..c85b8f75c851 100644 --- a/src/modules/mavlink/streams/HYGROMETER_SENSOR.hpp +++ b/src/modules/mavlink/streams/HYGROMETER_SENSOR.hpp @@ -55,7 +55,19 @@ class MavlinkStreamHygrometerSensor : public MavlinkStream } private: - explicit MavlinkStreamHygrometerSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamHygrometerSensor(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamHygrometerSensor() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::sensor_hygrometer + }; uORB::SubscriptionMultiArray _sensor_hygrometer_subs{ORB_ID::sensor_hygrometer}; diff --git a/src/modules/mavlink/streams/LANDING_TARGET.hpp b/src/modules/mavlink/streams/LANDING_TARGET.hpp index c4c52d0f71d3..6db9dd681b9a 100644 --- a/src/modules/mavlink/streams/LANDING_TARGET.hpp +++ b/src/modules/mavlink/streams/LANDING_TARGET.hpp @@ -53,7 +53,19 @@ class MavlinkStreamLandingTarget : public MavlinkStream } private: - explicit MavlinkStreamLandingTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamLandingTarget(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamLandingTarget() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::landing_target_pose + }; uORB::Subscription _landing_target_sub{ORB_ID(landing_target_pose)}; diff --git a/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp b/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp index c06fef2120ec..25eb0ee1dd77 100644 --- a/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp +++ b/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp @@ -53,7 +53,19 @@ class MavlinkStreamLocalPositionNED : public MavlinkStream } private: - explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamLocalPositionNED() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::vehicle_local_position + }; uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp index d52e26e71867..c9b0553dd844 100644 --- a/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp +++ b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp @@ -56,7 +56,20 @@ class MavlinkStreamMagCalReport : public MavlinkStream } private: - explicit MavlinkStreamMagCalReport(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamMagCalReport(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamMagCalReport() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::sensor_mag, + ORB_ID::parameter_update + }; static constexpr int MAX_SENSOR_COUNT = 4; diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp index 8be68fdc3fad..0ff8d597497a 100644 --- a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp +++ b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -55,7 +55,20 @@ class MavlinkStreamManualControl : public MavlinkStream } private: - explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamManualControl() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::manual_control_setpoint, + ORB_ID::manual_control_switches + }; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; diff --git a/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp b/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp index a07159c90542..e145c3642698 100644 --- a/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp +++ b/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp @@ -54,7 +54,20 @@ class MavlinkStreamMountOrientation : public MavlinkStream } private: - explicit MavlinkStreamMountOrientation(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamMountOrientation(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamMountOrientation() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::mount_orientation, + ORB_ID::vehicle_local_position + }; uORB::Subscription _mount_orientation_sub{ORB_ID(mount_orientation)}; uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp b/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp index 96317960567f..9c8096398a57 100644 --- a/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp +++ b/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp @@ -53,7 +53,19 @@ class MavlinkStreamNamedValueFloat : public MavlinkStream } private: - explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamNamedValueFloat() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::debug_key_value + }; uORB::Subscription _debug_key_value_sub{ORB_ID(debug_key_value)}; diff --git a/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp b/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp index 5f0f50e9e049..5f9c352884fa 100644 --- a/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp +++ b/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp @@ -56,7 +56,21 @@ class MavlinkStreamNavControllerOutput : public MavlinkStream } private: - explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamNavControllerOutput() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[3] { + ORB_ID::position_controller_status, + ORB_ID::tecs_status, + ORB_ID::vehicle_global_position + }; uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)}; uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; diff --git a/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp b/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp index 93d936dea400..a11de4b8aaa5 100644 --- a/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp +++ b/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp @@ -54,7 +54,19 @@ class MavlinkStreamObstacleDistance : public MavlinkStream } private: - explicit MavlinkStreamObstacleDistance(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamObstacleDistance(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamObstacleDistance() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::obstacle_distance_fused + }; uORB::Subscription _obstacle_distance_fused_sub{ORB_ID(obstacle_distance_fused)}; diff --git a/src/modules/mavlink/streams/ODOMETRY.hpp b/src/modules/mavlink/streams/ODOMETRY.hpp index 60b8564bc2ad..9d39d197bb6a 100644 --- a/src/modules/mavlink/streams/ODOMETRY.hpp +++ b/src/modules/mavlink/streams/ODOMETRY.hpp @@ -53,7 +53,19 @@ class MavlinkStreamOdometry : public MavlinkStream } private: - explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamOdometry() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::vehicle_odometry + }; uORB::Subscription _vehicle_odometry_sub{ORB_ID(vehicle_odometry)}; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp index d7869dcbb6c9..61bd7a3319ae 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp @@ -53,7 +53,19 @@ class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream } private: - explicit MavlinkStreamOpenDroneIdBasicId(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamOpenDroneIdBasicId(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamOpenDroneIdBasicId() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::vehicle_status + }; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp index f3586ae12655..6f5f16ac4f82 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp @@ -58,7 +58,24 @@ class MavlinkStreamOpenDroneIdLocation : public MavlinkStream } private: - explicit MavlinkStreamOpenDroneIdLocation(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamOpenDroneIdLocation(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamOpenDroneIdLocation() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[6] { + ORB_ID::home_position, + ORB_ID::vehicle_air_data, + ORB_ID::vehicle_gps_position, + ORB_ID::vehicle_land_detected, + ORB_ID::vehicle_local_position, + ORB_ID::vehicle_status + }; uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp index bcc7e0f1452e..6d0072375684 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp @@ -58,7 +58,20 @@ class MavlinkStreamOpenDroneIdSystem : public MavlinkStream } private: - explicit MavlinkStreamOpenDroneIdSystem(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamOpenDroneIdSystem(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamOpenDroneIdSystem() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::home_position, + ORB_ID::vehicle_gps_position + }; uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; diff --git a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp index 02f368b4c319..28309fbf8477 100644 --- a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp +++ b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp @@ -54,7 +54,19 @@ class MavlinkStreamOpticalFlowRad : public MavlinkStream } private: - explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamOpticalFlowRad() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::vehicle_optical_flow + }; uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)}; diff --git a/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp b/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp index 5aa3da2475f3..6ee25d44fe25 100644 --- a/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp +++ b/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp @@ -53,7 +53,19 @@ class MavlinkStreamOrbitStatus : public MavlinkStream } private: - explicit MavlinkStreamOrbitStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamOrbitStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamOrbitStatus() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::orbit_status + }; uORB::SubscriptionMultiArray _orbit_status_subs{ORB_ID::orbit_status}; diff --git a/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp b/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp index f568e2aadc60..fcbb50e8c95c 100644 --- a/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp +++ b/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp @@ -56,7 +56,21 @@ class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream } private: - explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamPositionTargetGlobalInt() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[3] { + ORB_ID::vehicle_control_mode, + ORB_ID::vehicle_local_position_setpoint, + ORB_ID::position_setpoint_triplet + }; uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _lpos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; diff --git a/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp b/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp index 69f3fba111ac..70358a386a99 100644 --- a/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp +++ b/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp @@ -53,7 +53,19 @@ class MavlinkStreamPositionTargetLocalNed : public MavlinkStream } private: - explicit MavlinkStreamPositionTargetLocalNed(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamPositionTargetLocalNed(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamPositionTargetLocalNed() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::vehicle_local_position_setpoint + }; uORB::Subscription _pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; diff --git a/src/modules/mavlink/streams/RAW_RPM.hpp b/src/modules/mavlink/streams/RAW_RPM.hpp index 9d2088123ff5..d653122db876 100644 --- a/src/modules/mavlink/streams/RAW_RPM.hpp +++ b/src/modules/mavlink/streams/RAW_RPM.hpp @@ -53,7 +53,19 @@ class MavlinkStreamRawRpm : public MavlinkStream } private: - explicit MavlinkStreamRawRpm(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamRawRpm(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamRawRpm() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::rpm + }; uORB::SubscriptionMultiArray _rpm_subs{ORB_ID::rpm}; diff --git a/src/modules/mavlink/streams/RC_CHANNELS.hpp b/src/modules/mavlink/streams/RC_CHANNELS.hpp index 868908155cee..a5180da42ef4 100644 --- a/src/modules/mavlink/streams/RC_CHANNELS.hpp +++ b/src/modules/mavlink/streams/RC_CHANNELS.hpp @@ -53,7 +53,19 @@ class MavlinkStreamRCChannels : public MavlinkStream } private: - explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamRCChannels() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::input_rc + }; uORB::Subscription _input_rc_sub{ORB_ID(input_rc)}; diff --git a/src/modules/mavlink/streams/SCALED_IMU.hpp b/src/modules/mavlink/streams/SCALED_IMU.hpp index 95d0b48e4b36..d46d96415d6f 100644 --- a/src/modules/mavlink/streams/SCALED_IMU.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU.hpp @@ -62,7 +62,21 @@ class MavlinkStreamScaledIMU : public MavlinkStream } private: - explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamScaledIMU() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[3] { + ORB_ID::vehicle_imu, + ORB_ID::vehicle_imu_status, + ORB_ID::sensor_mag + }; uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 0}; uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), 0}; diff --git a/src/modules/mavlink/streams/SCALED_IMU2.hpp b/src/modules/mavlink/streams/SCALED_IMU2.hpp index 3f65189056d2..283f3e324f8b 100644 --- a/src/modules/mavlink/streams/SCALED_IMU2.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU2.hpp @@ -62,7 +62,21 @@ class MavlinkStreamScaledIMU2 : public MavlinkStream } private: - explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamScaledIMU2() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[3] { + ORB_ID::vehicle_imu, + ORB_ID::vehicle_imu_status, + ORB_ID::sensor_mag + }; uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 1}; uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), 1}; diff --git a/src/modules/mavlink/streams/SCALED_IMU3.hpp b/src/modules/mavlink/streams/SCALED_IMU3.hpp index cb180c7b8b2b..60d9df91aa42 100644 --- a/src/modules/mavlink/streams/SCALED_IMU3.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU3.hpp @@ -62,7 +62,21 @@ class MavlinkStreamScaledIMU3 : public MavlinkStream } private: - explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamScaledIMU3() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[3] { + ORB_ID::vehicle_imu, + ORB_ID::vehicle_imu_status, + ORB_ID::sensor_mag + }; uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 2}; uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), 2}; diff --git a/src/modules/mavlink/streams/SCALED_PRESSURE.hpp b/src/modules/mavlink/streams/SCALED_PRESSURE.hpp index 7ae99083168d..81386a60bfea 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE.hpp @@ -58,7 +58,20 @@ class MavlinkStreamScaledPressure : public MavlinkStream } private: - explicit MavlinkStreamScaledPressure(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamScaledPressure(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamScaledPressure() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::differential_pressure, + ORB_ID::sensor_baro + }; uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure), 0}; uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), 0}; diff --git a/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp b/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp index f453c4ff184d..941fbe69a39f 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp @@ -58,7 +58,20 @@ class MavlinkStreamScaledPressure2 : public MavlinkStream } private: - explicit MavlinkStreamScaledPressure2(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamScaledPressure2(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamScaledPressure2() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::differential_pressure, + ORB_ID::sensor_baro + }; uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure), 1}; uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), 1}; diff --git a/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp b/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp index 7b3011bb66e8..76400699f29b 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp @@ -58,7 +58,20 @@ class MavlinkStreamScaledPressure3 : public MavlinkStream } private: - explicit MavlinkStreamScaledPressure3(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamScaledPressure3(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamScaledPressure3() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::differential_pressure, + ORB_ID::sensor_baro + }; uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure), 2}; uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), 2}; diff --git a/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp b/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp index 77640368ff25..81759232089e 100644 --- a/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp +++ b/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp @@ -66,7 +66,19 @@ class MavlinkStreamServoOutputRaw : public MavlinkStream } private: - explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamServoOutputRaw() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::actuator_outputs + }; uORB::Subscription _act_sub{ORB_ID(actuator_outputs), N}; diff --git a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp b/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp index 3476db3a02b2..cc77aee598f9 100644 --- a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp +++ b/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp @@ -54,7 +54,19 @@ class MavlinkStreamSmartBatteryInfo : public MavlinkStream } private: - explicit MavlinkStreamSmartBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamSmartBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamSmartBatteryInfo() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::battery_status + }; uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; diff --git a/src/modules/mavlink/streams/STATUSTEXT.hpp b/src/modules/mavlink/streams/STATUSTEXT.hpp index 3409a27ab3fd..f1aa5c8d0885 100644 --- a/src/modules/mavlink/streams/STATUSTEXT.hpp +++ b/src/modules/mavlink/streams/STATUSTEXT.hpp @@ -55,13 +55,21 @@ class MavlinkStreamStatustext : public MavlinkStream } private: - explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } ~MavlinkStreamStatustext() { + _mavlink->unregister_orb_poll(get_id_static()); perf_free(_missed_msg_count_perf); } + ORB_ID _orbs[1] { + ORB_ID::mavlink_log + }; + uORB::Subscription _mavlink_log_sub{ORB_ID(mavlink_log)}; perf_counter_t _missed_msg_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": STATUSTEXT missed messages")}; uint16_t _id{0}; diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 6859ea7f4be1..0e75ead5d896 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -57,7 +57,22 @@ class MavlinkStreamSysStatus : public MavlinkStream } private: - explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamSysStatus() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[4] { + ORB_ID::vehicle_status, + ORB_ID::cpuload, + ORB_ID::health_report, + ORB_ID::battery_status + }; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; diff --git a/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp b/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp index b28a8bd4dd0f..2fce063ab1b7 100644 --- a/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp +++ b/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp @@ -53,7 +53,19 @@ class MavlinkStreamTimeEstimateToTarget : public MavlinkStream } private: - explicit MavlinkStreamTimeEstimateToTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamTimeEstimateToTarget(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamTimeEstimateToTarget() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::rtl_time_estimate + }; uORB::Subscription _rtl_estimate_sub{ORB_ID(rtl_time_estimate)}; diff --git a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp b/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp index 174c633fd2fc..f1b678a47d14 100644 --- a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp +++ b/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp @@ -57,7 +57,19 @@ class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream } private: - explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamTrajectoryRepresentationWaypoints() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[1] { + ORB_ID::vehicle_trajectory_waypoint_desired + }; uORB::Subscription _traj_wp_avoidance_sub{ORB_ID(vehicle_trajectory_waypoint_desired)}; diff --git a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp index fdc8ae1ecc74..f506ddfaca40 100644 --- a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp +++ b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp @@ -58,8 +58,21 @@ class MavlinkStreamUavionixADSBOutDynamic : public ModuleParams, public MavlinkS } private: - explicit MavlinkStreamUavionixADSBOutDynamic(Mavlink *mavlink) : ModuleParams(nullptr), MavlinkStream(mavlink) {} + explicit MavlinkStreamUavionixADSBOutDynamic(Mavlink *mavlink) : ModuleParams(nullptr), MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamUavionixADSBOutDynamic() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + ORB_ID _orbs[3] { + ORB_ID::vehicle_gps_position, + ORB_ID::vehicle_air_data, + ORB_ID::vehicle_status + }; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; diff --git a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp index 2cf17b69f2eb..25f0c4122816 100644 --- a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp +++ b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp @@ -59,7 +59,23 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream } private: - explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamUTMGlobalPosition() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[5] { + ORB_ID::vehicle_local_position, + ORB_ID::vehicle_global_position, + ORB_ID::position_setpoint_triplet, + ORB_ID::vehicle_status, + ORB_ID::vehicle_land_detected + }; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; diff --git a/src/modules/mavlink/streams/VFR_HUD.hpp b/src/modules/mavlink/streams/VFR_HUD.hpp index bbdfe0d3c862..68a50d1f2ad1 100644 --- a/src/modules/mavlink/streams/VFR_HUD.hpp +++ b/src/modules/mavlink/streams/VFR_HUD.hpp @@ -61,7 +61,23 @@ class MavlinkStreamVFRHUD : public MavlinkStream } private: - explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamVFRHUD() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[5] { + ORB_ID::vehicle_local_position, + ORB_ID::actuator_armed, + ORB_ID::vehicle_thrust_setpoint, + ORB_ID::airspeed_validated, + ORB_ID::vehicle_air_data + }; uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; diff --git a/src/modules/mavlink/streams/VIBRATION.hpp b/src/modules/mavlink/streams/VIBRATION.hpp index 661c9528ac46..75da44f93f4e 100644 --- a/src/modules/mavlink/streams/VIBRATION.hpp +++ b/src/modules/mavlink/streams/VIBRATION.hpp @@ -58,7 +58,20 @@ class MavlinkStreamVibration : public MavlinkStream } private: - explicit MavlinkStreamVibration(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamVibration(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamVibration() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::sensor_selection, + ORB_ID::vehicle_imu_status + }; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; diff --git a/src/modules/mavlink/streams/WIND_COV.hpp b/src/modules/mavlink/streams/WIND_COV.hpp index 8cd5550db523..3d24b373e25f 100644 --- a/src/modules/mavlink/streams/WIND_COV.hpp +++ b/src/modules/mavlink/streams/WIND_COV.hpp @@ -54,7 +54,20 @@ class MavlinkStreamWindCov : public MavlinkStream } private: - explicit MavlinkStreamWindCov(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamWindCov(Mavlink *mavlink) : MavlinkStream(mavlink) + { + mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); + } + + ~MavlinkStreamWindCov() + { + _mavlink->unregister_orb_poll(get_id_static()); + } + + ORB_ID _orbs[2] { + ORB_ID::wind, + ORB_ID::vehicle_local_position + }; uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};