From fb5f084467495a858ab516e82d8b622f53fb8a36 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Thu, 6 Jun 2024 13:45:59 +0300 Subject: [PATCH 01/10] Revert "mavlink: update streams to use orb polling" This reverts commit f3947f10f6ae0dac127d9b5d7fed17faf8c582f3. --- .../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, 70 insertions(+), 1029 deletions(-) diff --git a/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp b/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp index 76ced56aa243..5c5cfbdefd7f 100644 --- a/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp +++ b/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp @@ -53,17 +53,7 @@ class MavlinkStreamActuatorOutputStatus : public MavlinkStream } private: - 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 }; + explicit MavlinkStreamActuatorOutputStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 16353299fa7c..56f2a94e42a6 100644 --- a/src/modules/mavlink/streams/ADSB_VEHICLE.hpp +++ b/src/modules/mavlink/streams/ADSB_VEHICLE.hpp @@ -55,17 +55,7 @@ class MavlinkStreamADSBVehicle : public MavlinkStream } private: - 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 }; + explicit MavlinkStreamADSBVehicle(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 ea88da73ba26..f3c437d0d9ae 100644 --- a/src/modules/mavlink/streams/ALTITUDE.hpp +++ b/src/modules/mavlink/streams/ALTITUDE.hpp @@ -56,21 +56,7 @@ class MavlinkStreamAltitude : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 7b0380eaaa39..94c1f4a42209 100644 --- a/src/modules/mavlink/streams/ATTITUDE.hpp +++ b/src/modules/mavlink/streams/ATTITUDE.hpp @@ -54,20 +54,7 @@ class MavlinkStreamAttitude : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 6d6e54d3158a..64189419ad1e 100644 --- a/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp +++ b/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp @@ -55,22 +55,7 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 deaaee7ef1f1..dd93fc8dfe00 100644 --- a/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp +++ b/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp @@ -54,21 +54,7 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 eb015f89b3bf..5e4dea2a03d1 100644 --- a/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp +++ b/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp @@ -62,25 +62,7 @@ class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream } private: - 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 - }; - + explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 c59762fcf3ba..73068f0de360 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -54,19 +54,7 @@ class MavlinkStreamBatteryStatus : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 b1bb255d66a2..fede0f3c27df 100644 --- a/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp +++ b/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp @@ -55,19 +55,7 @@ class MavlinkStreamCameraImageCaptured : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamCameraImageCaptured(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 f5f45a1910e5..e64a772d2d22 100644 --- a/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp +++ b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp @@ -61,20 +61,7 @@ class MavlinkStreamCameraTrigger : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 320638fe1a9a..a3469c62d9a0 100644 --- a/src/modules/mavlink/streams/COLLISION.hpp +++ b/src/modules/mavlink/streams/COLLISION.hpp @@ -53,19 +53,7 @@ class MavlinkStreamCollision : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 4f33575a1d9c..89f4a77dbe4f 100644 --- a/src/modules/mavlink/streams/COMMAND_LONG.hpp +++ b/src/modules/mavlink/streams/COMMAND_LONG.hpp @@ -53,19 +53,7 @@ class MavlinkStreamCommandLong : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 5e51da2bfa7e..c0c2a52322d9 100644 --- a/src/modules/mavlink/streams/DEBUG.hpp +++ b/src/modules/mavlink/streams/DEBUG.hpp @@ -53,19 +53,7 @@ class MavlinkStreamDebug : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 75cd39e4d576..a5029ffe398c 100644 --- a/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp +++ b/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp @@ -53,19 +53,7 @@ class MavlinkStreamDebugFloatArray : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 e67f6363abb1..9fa293188d89 100644 --- a/src/modules/mavlink/streams/DEBUG_VECT.hpp +++ b/src/modules/mavlink/streams/DEBUG_VECT.hpp @@ -53,19 +53,7 @@ class MavlinkStreamDebugVect : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 7ed4335e41f6..9042403a4868 100644 --- a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp +++ b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp @@ -54,19 +54,7 @@ class MavlinkStreamDistanceSensor : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 69c0d68465d9..48ea82459cf6 100644 --- a/src/modules/mavlink/streams/EFI_STATUS.hpp +++ b/src/modules/mavlink/streams/EFI_STATUS.hpp @@ -54,19 +54,7 @@ class MavlinkStreamEfiStatus : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamEfiStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 e063054992cb..609112ed0ad8 100644 --- a/src/modules/mavlink/streams/ESC_INFO.hpp +++ b/src/modules/mavlink/streams/ESC_INFO.hpp @@ -54,19 +54,7 @@ class MavlinkStreamESCInfo : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamESCInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 4bfa6627ccb3..1e8911577823 100644 --- a/src/modules/mavlink/streams/ESC_STATUS.hpp +++ b/src/modules/mavlink/streams/ESC_STATUS.hpp @@ -54,19 +54,7 @@ class MavlinkStreamESCStatus : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamESCStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 b3e3021f331c..3764940c312e 100644 --- a/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp +++ b/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp @@ -54,20 +54,7 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 fc85da1498eb..b6cf44e63436 100644 --- a/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp +++ b/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp @@ -60,21 +60,8 @@ 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 253d24b9a242..5dd9f88c88d8 100644 --- a/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp +++ b/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp @@ -56,18 +56,8 @@ 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 876732ee4f58..615a797488d4 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -57,19 +57,7 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamGimbalDeviceAttitudeStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 b6b4655c9922..413e7361e6e1 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp @@ -57,19 +57,7 @@ class MavlinkStreamGimbalDeviceSetAttitude : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamGimbalDeviceSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 6f70e5958967..a6570362e138 100644 --- a/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp +++ b/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp @@ -57,19 +57,7 @@ class MavlinkStreamGimbalManagerInformation : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamGimbalManagerInformation(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 7583e419c51e..ecab79410cbc 100644 --- a/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp @@ -57,19 +57,7 @@ class MavlinkStreamGimbalManagerStatus : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamGimbalManagerStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 bec081da7393..6d2997fe7188 100644 --- a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp +++ b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp @@ -56,22 +56,7 @@ class MavlinkStreamGlobalPositionInt : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 a4cd036e9851..eb77c8c3190d 100644 --- a/src/modules/mavlink/streams/GPS2_RAW.hpp +++ b/src/modules/mavlink/streams/GPS2_RAW.hpp @@ -55,19 +55,7 @@ class MavlinkStreamGPS2Raw : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamGPS2Raw(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 c3fe5fbcf8e5..fb707b3d14cc 100644 --- a/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp +++ b/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp @@ -63,19 +63,7 @@ class MavlinkStreamGpsGlobalOrigin : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamGpsGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 254de12f1179..f6887c73c992 100644 --- a/src/modules/mavlink/streams/GPS_RAW_INT.hpp +++ b/src/modules/mavlink/streams/GPS_RAW_INT.hpp @@ -55,19 +55,7 @@ class MavlinkStreamGPSRawInt : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 189f46991ade..8730d2e88109 100644 --- a/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp +++ b/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp @@ -53,19 +53,7 @@ class MavlinkStreamGPSRTCMData : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamGPSRTCMData(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 bbe2c26751ae..82d36f2eae9d 100644 --- a/src/modules/mavlink/streams/GPS_STATUS.hpp +++ b/src/modules/mavlink/streams/GPS_STATUS.hpp @@ -53,19 +53,7 @@ class MavlinkStreamGPSStatus : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamGPSStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 2424648be101..26d966a85ac5 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -57,21 +57,7 @@ class MavlinkStreamHeartbeat : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 678e8596d1a1..a6682213bf93 100644 --- a/src/modules/mavlink/streams/HIGHRES_IMU.hpp +++ b/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -61,25 +61,7 @@ class MavlinkStreamHighresIMU : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 e8582a680619..c96d0b6cb9d1 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -91,12 +91,6 @@ 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 { @@ -627,26 +621,6 @@ 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 3a51aa279ef3..182c1d40fab4 100644 --- a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp +++ b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp @@ -62,7 +62,6 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams ModuleParams(nullptr) { _act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) { char param_name[17]; @@ -71,17 +70,6 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams } } - ~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 d614b428469f..b2dcb5c42615 100644 --- a/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp +++ b/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp @@ -55,22 +55,7 @@ class MavlinkStreamHILStateQuaternion : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamHILStateQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 69831bf5b924..bb67d14d2dbf 100644 --- a/src/modules/mavlink/streams/HOME_POSITION.hpp +++ b/src/modules/mavlink/streams/HOME_POSITION.hpp @@ -53,19 +53,7 @@ class MavlinkStreamHomePosition : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 c85b8f75c851..9787f9dee279 100644 --- a/src/modules/mavlink/streams/HYGROMETER_SENSOR.hpp +++ b/src/modules/mavlink/streams/HYGROMETER_SENSOR.hpp @@ -55,19 +55,7 @@ class MavlinkStreamHygrometerSensor : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamHygrometerSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 6db9dd681b9a..c4c52d0f71d3 100644 --- a/src/modules/mavlink/streams/LANDING_TARGET.hpp +++ b/src/modules/mavlink/streams/LANDING_TARGET.hpp @@ -53,19 +53,7 @@ class MavlinkStreamLandingTarget : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamLandingTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 25eb0ee1dd77..c06fef2120ec 100644 --- a/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp +++ b/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp @@ -53,19 +53,7 @@ class MavlinkStreamLocalPositionNED : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 c9b0553dd844..d52e26e71867 100644 --- a/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp +++ b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp @@ -56,20 +56,7 @@ class MavlinkStreamMagCalReport : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamMagCalReport(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 0ff8d597497a..8be68fdc3fad 100644 --- a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp +++ b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -55,20 +55,7 @@ class MavlinkStreamManualControl : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 e145c3642698..a07159c90542 100644 --- a/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp +++ b/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp @@ -54,20 +54,7 @@ class MavlinkStreamMountOrientation : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamMountOrientation(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 9c8096398a57..96317960567f 100644 --- a/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp +++ b/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp @@ -53,19 +53,7 @@ class MavlinkStreamNamedValueFloat : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 5f9c352884fa..5f0f50e9e049 100644 --- a/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp +++ b/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp @@ -56,21 +56,7 @@ class MavlinkStreamNavControllerOutput : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 a11de4b8aaa5..93d936dea400 100644 --- a/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp +++ b/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp @@ -54,19 +54,7 @@ class MavlinkStreamObstacleDistance : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamObstacleDistance(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 9d39d197bb6a..60b8564bc2ad 100644 --- a/src/modules/mavlink/streams/ODOMETRY.hpp +++ b/src/modules/mavlink/streams/ODOMETRY.hpp @@ -53,19 +53,7 @@ class MavlinkStreamOdometry : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 61bd7a3319ae..d7869dcbb6c9 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp @@ -53,19 +53,7 @@ class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamOpenDroneIdBasicId(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 6f5f16ac4f82..f3586ae12655 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp @@ -58,24 +58,7 @@ class MavlinkStreamOpenDroneIdLocation : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamOpenDroneIdLocation(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 6d0072375684..bcc7e0f1452e 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp @@ -58,20 +58,7 @@ class MavlinkStreamOpenDroneIdSystem : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamOpenDroneIdSystem(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 28309fbf8477..02f368b4c319 100644 --- a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp +++ b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp @@ -54,19 +54,7 @@ class MavlinkStreamOpticalFlowRad : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 6ee25d44fe25..5aa3da2475f3 100644 --- a/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp +++ b/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp @@ -53,19 +53,7 @@ class MavlinkStreamOrbitStatus : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamOrbitStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 fcbb50e8c95c..f568e2aadc60 100644 --- a/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp +++ b/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp @@ -56,21 +56,7 @@ class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 70358a386a99..69f3fba111ac 100644 --- a/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp +++ b/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp @@ -53,19 +53,7 @@ class MavlinkStreamPositionTargetLocalNed : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamPositionTargetLocalNed(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 d653122db876..9d2088123ff5 100644 --- a/src/modules/mavlink/streams/RAW_RPM.hpp +++ b/src/modules/mavlink/streams/RAW_RPM.hpp @@ -53,19 +53,7 @@ class MavlinkStreamRawRpm : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamRawRpm(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 a5180da42ef4..868908155cee 100644 --- a/src/modules/mavlink/streams/RC_CHANNELS.hpp +++ b/src/modules/mavlink/streams/RC_CHANNELS.hpp @@ -53,19 +53,7 @@ class MavlinkStreamRCChannels : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 d46d96415d6f..95d0b48e4b36 100644 --- a/src/modules/mavlink/streams/SCALED_IMU.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU.hpp @@ -62,21 +62,7 @@ class MavlinkStreamScaledIMU : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 283f3e324f8b..3f65189056d2 100644 --- a/src/modules/mavlink/streams/SCALED_IMU2.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU2.hpp @@ -62,21 +62,7 @@ class MavlinkStreamScaledIMU2 : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 60d9df91aa42..cb180c7b8b2b 100644 --- a/src/modules/mavlink/streams/SCALED_IMU3.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU3.hpp @@ -62,21 +62,7 @@ class MavlinkStreamScaledIMU3 : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 81386a60bfea..7ae99083168d 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE.hpp @@ -58,20 +58,7 @@ class MavlinkStreamScaledPressure : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamScaledPressure(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 941fbe69a39f..f453c4ff184d 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp @@ -58,20 +58,7 @@ class MavlinkStreamScaledPressure2 : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamScaledPressure2(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 76400699f29b..7b3011bb66e8 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp @@ -58,20 +58,7 @@ class MavlinkStreamScaledPressure3 : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamScaledPressure3(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 81759232089e..77640368ff25 100644 --- a/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp +++ b/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp @@ -66,19 +66,7 @@ class MavlinkStreamServoOutputRaw : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 cc77aee598f9..3476db3a02b2 100644 --- a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp +++ b/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp @@ -54,19 +54,7 @@ class MavlinkStreamSmartBatteryInfo : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamSmartBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 f1aa5c8d0885..3409a27ab3fd 100644 --- a/src/modules/mavlink/streams/STATUSTEXT.hpp +++ b/src/modules/mavlink/streams/STATUSTEXT.hpp @@ -55,21 +55,13 @@ class MavlinkStreamStatustext : public MavlinkStream } private: - explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } + explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) {} ~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 0e75ead5d896..6859ea7f4be1 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -57,22 +57,7 @@ class MavlinkStreamSysStatus : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 2fce063ab1b7..b28a8bd4dd0f 100644 --- a/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp +++ b/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp @@ -53,19 +53,7 @@ class MavlinkStreamTimeEstimateToTarget : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamTimeEstimateToTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 f1b678a47d14..174c633fd2fc 100644 --- a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp +++ b/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp @@ -57,19 +57,7 @@ class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 f506ddfaca40..fdc8ae1ecc74 100644 --- a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp +++ b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp @@ -58,21 +58,8 @@ class MavlinkStreamUavionixADSBOutDynamic : public ModuleParams, public MavlinkS } private: - 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()); - } + explicit MavlinkStreamUavionixADSBOutDynamic(Mavlink *mavlink) : ModuleParams(nullptr), MavlinkStream(mavlink) {} - 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 25f0c4122816..2cf17b69f2eb 100644 --- a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp +++ b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp @@ -59,23 +59,7 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 68a50d1f2ad1..bbdfe0d3c862 100644 --- a/src/modules/mavlink/streams/VFR_HUD.hpp +++ b/src/modules/mavlink/streams/VFR_HUD.hpp @@ -61,23 +61,7 @@ class MavlinkStreamVFRHUD : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 75da44f93f4e..661c9528ac46 100644 --- a/src/modules/mavlink/streams/VIBRATION.hpp +++ b/src/modules/mavlink/streams/VIBRATION.hpp @@ -58,20 +58,7 @@ class MavlinkStreamVibration : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamVibration(Mavlink *mavlink) : MavlinkStream(mavlink) {} 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 3d24b373e25f..8cd5550db523 100644 --- a/src/modules/mavlink/streams/WIND_COV.hpp +++ b/src/modules/mavlink/streams/WIND_COV.hpp @@ -54,20 +54,7 @@ class MavlinkStreamWindCov : public MavlinkStream } private: - 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 - }; + explicit MavlinkStreamWindCov(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; From 3eeefee8848980de631989a83cef3abd5583ef59 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 7 Jun 2024 09:29:59 +0300 Subject: [PATCH 02/10] Revert "mavlink_stream: fix set_interval comment" This reverts commit 545cdd41b6e29f16f681b2d3abeee077e3c65d41. --- src/modules/mavlink/mavlink_stream.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index b5e3e53d9da2..24019a248846 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -324,7 +324,7 @@ MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_ms) { pthread_mutex_lock(&_mtx); - // Renew all uorb subscriptions of given stream with new interval value + // Update interval for all orb subscriptions and find the maximum interval for (int i = 0; i < _count; i++) { if (_reqs[i].stream_id == stream_id) { From 6b03c33afad4917ea7de46a89bbba424ffb58316 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 7 Jun 2024 09:30:10 +0300 Subject: [PATCH 03/10] Revert "mavlink_stream: update fd list after changing interval" This reverts commit 27dcc66a1ee1503a102854ccb3704a64c54fcc46. --- src/modules/mavlink/mavlink_stream.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 24019a248846..1299e5de53c8 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -340,13 +340,6 @@ MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_ms) } } - // Update fds - for (int j = 0; j < _count; j++) { - _fds[j].fd = _orbs[j].fd; - _fds[j].events = POLLIN; - _fds[j].revents = 0; - } - pthread_mutex_unlock(&_mtx); return PX4_OK; } From a66d064c3f350c6b6ff5bf88441d8510633c64f9 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 7 Jun 2024 09:30:21 +0300 Subject: [PATCH 04/10] Revert "mavlink_stream: mutex lock for poll and ack" This reverts commit 3cfe1022b510c9586b13e5133d0215c97a84d49a. --- src/modules/mavlink/mavlink_stream.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 1299e5de53c8..8c23ed8909ae 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -351,19 +351,12 @@ int MavlinkStreamPoll::poll(const hrt_abstime timeout) { int timeout_ms = timeout / 1000; - int ret; if (timeout_ms <= 0) { timeout_ms = 1; } - pthread_mutex_lock(&_mtx); - - ret = px4_poll(_fds, _count, timeout_ms); - - pthread_mutex_unlock(&_mtx); - - return ret; + return px4_poll(_fds, _count, timeout_ms); } /** @@ -372,12 +365,8 @@ MavlinkStreamPoll::poll(const hrt_abstime timeout) void MavlinkStreamPoll::ack_all() { - pthread_mutex_lock(&_mtx); - for (int i = 0; i < _count; i++) { orb_ack(_orbs[i].fd); } - - pthread_mutex_unlock(&_mtx); } #endif /* CONFIG_MAVLINK_UORB_POLL */ From 05256f5c2ee5706426a26226999e2a9ad5d2d08d Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 7 Jun 2024 09:30:48 +0300 Subject: [PATCH 05/10] Revert "mavlink_stream: mutex lock for register/unregister poll" This reverts commit 036da03a738453a22060c8cbc9829720c7d47309. --- src/modules/mavlink/mavlink_stream.cpp | 13 ------------- src/modules/mavlink/mavlink_stream.h | 1 - 2 files changed, 14 deletions(-) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 8c23ed8909ae..98fafdf95ec7 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -145,7 +145,6 @@ MavlinkStreamPoll::MavlinkStreamPoll() : _orbs = (MavStreamPollItem *)malloc(16 * sizeof(MavStreamPollItem)); _fds = (orb_poll_struct_t *)malloc(16 * sizeof(orb_poll_struct_t)); _reqs = (MavStreamOrbPollReq *)malloc(16 * sizeof(MavStreamOrbPollReq)); - pthread_mutex_init(&_mtx, nullptr); } MavlinkStreamPoll::~MavlinkStreamPoll() @@ -161,8 +160,6 @@ MavlinkStreamPoll::~MavlinkStreamPoll() if (_reqs) { free(_reqs); } - - pthread_mutex_destroy(&_mtx); } int @@ -240,8 +237,6 @@ MavlinkStreamPoll::register_orbs(uint16_t stream_id, ORB_ID *orbs, int cnt) return PX4_OK; } - pthread_mutex_lock(&_mtx); - for (int i = 0; i < cnt; i++) { // Increase reqs capacity if necessary @@ -251,7 +246,6 @@ MavlinkStreamPoll::register_orbs(uint16_t stream_id, ORB_ID *orbs, int cnt) if (!_reqs) { PX4_ERR("failed to allocate memory for orb poll reqs"); - pthread_mutex_unlock(&_mtx); return PX4_ERROR; } } @@ -273,7 +267,6 @@ MavlinkStreamPoll::register_orbs(uint16_t stream_id, ORB_ID *orbs, int cnt) _fds[i].revents = 0; } - pthread_mutex_unlock(&_mtx); return PX4_OK; } @@ -285,8 +278,6 @@ MavlinkStreamPoll::unregister_orbs(uint16_t stream_id) { int i = 0; - pthread_mutex_lock(&_mtx); - while (i < _reqs_count) { if (_reqs[i].stream_id == stream_id) { @@ -312,7 +303,6 @@ MavlinkStreamPoll::unregister_orbs(uint16_t stream_id) _fds[j].revents = 0; } - pthread_mutex_unlock(&_mtx); return PX4_OK; } @@ -322,8 +312,6 @@ MavlinkStreamPoll::unregister_orbs(uint16_t stream_id) int MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_ms) { - pthread_mutex_lock(&_mtx); - // Update interval for all orb subscriptions and find the maximum interval for (int i = 0; i < _count; i++) { if (_reqs[i].stream_id == stream_id) { @@ -340,7 +328,6 @@ MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_ms) } } - pthread_mutex_unlock(&_mtx); return PX4_OK; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index cefe9bedb4f2..a19d16f0d3ad 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -234,7 +234,6 @@ class MavlinkStreamPoll int _capacity; int _count; - pthread_mutex_t _mtx {}; }; #endif /* CONFIG_MAVLINK_UORB_POLL */ From 71ada804d2e961a751121f64f961db9be34445dc Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 7 Jun 2024 09:31:04 +0300 Subject: [PATCH 06/10] Revert "mavlink: fix uorb poll build config" This reverts commit 8b0a54e706228d843ebd37128cb1cc66ad5d46a5. --- src/modules/mavlink/mavlink_stream.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index a19d16f0d3ad..18fa1c5c794f 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -42,7 +42,6 @@ #define MAVLINK_STREAM_H_ #include -#include #include #include #include From 2251ae4ef5f2d81a12be51112edc0056ffeb2824 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 7 Jun 2024 09:31:27 +0300 Subject: [PATCH 07/10] Revert "mavlink_main: use orb polling" This reverts commit 090bb19df991e994f46db66749215ef93df29fde. --- src/modules/mavlink/Kconfig | 8 - src/modules/mavlink/mavlink_main.cpp | 73 +------- src/modules/mavlink/mavlink_main.h | 12 -- src/modules/mavlink/mavlink_stream.cpp | 229 ------------------------- src/modules/mavlink/mavlink_stream.h | 97 +---------- 5 files changed, 3 insertions(+), 416 deletions(-) diff --git a/src/modules/mavlink/Kconfig b/src/modules/mavlink/Kconfig index e3e07c6f7ac6..8fb9219d9d61 100644 --- a/src/modules/mavlink/Kconfig +++ b/src/modules/mavlink/Kconfig @@ -24,11 +24,3 @@ menuconfig USER_MAVLINK depends on BOARD_PROTECTED && MODULES_MAVLINK ---help--- Put mavlink in userspace memory - -menuconfig MAVLINK_UORB_POLL - bool "mavlink uorb poll in main loop" - default n - depends on MODULES_MAVLINK - ---help--- - Use uorb topic polling in mavlink main loop instead of - using px4_usleep for stream rate handling. diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c88a154115ca..26e86c64df29 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -134,10 +134,6 @@ Mavlink::Mavlink() : _event_sub.subscribe(); _telemetry_status_pub.advertise(); - -#if defined(CONFIG_MAVLINK_UORB_POLL) - _stream_poller = new MavlinkStreamPoll(); -#endif } Mavlink::~Mavlink() @@ -179,14 +175,6 @@ Mavlink::~Mavlink() } } -#if defined(CONFIG_MAVLINK_UORB_POLL) - - if (_stream_poller) { - delete _stream_poller; - } - -#endif - perf_free(_loop_perf); perf_free(_loop_interval_perf); perf_free(_send_byte_error_perf); @@ -1168,7 +1156,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (strcmp(stream_name, stream->get_name()) == 0) { if (interval != 0) { /* set new interval */ - set_stream_interval(stream, interval); + stream->set_interval(interval); } else { /* delete stream */ @@ -1185,7 +1173,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) MavlinkStream *stream = create_mavlink_stream(stream_name, this); if (stream != nullptr) { - set_stream_interval(stream, interval); + stream->set_interval(interval); _streams.add(stream); return OK; @@ -2337,29 +2325,9 @@ Mavlink::task_main(int argc, char *argv[]) _task_running.store(true); -#if defined(CONFIG_MAVLINK_UORB_POLL) - int uorb_poll_error_counter = 0; -#endif - while (!should_exit()) { /* main loop */ -#if defined(CONFIG_MAVLINK_UORB_POLL) - int uorb_poll_ret = _stream_poller->poll(MAIN_LOOP_DELAY); - - if (uorb_poll_ret < 0) { - /* this is seriously bad - should be an emergency */ - if (uorb_poll_error_counter < 10 || uorb_poll_error_counter % 50 == 0) { - /* use a counter to prevent flooding (and slowing us down) */ - PX4_ERR("ERROR while polling uorbs: %d", uorb_poll_ret); - } - - uorb_poll_error_counter++; - } - - _stream_poller->ack_all(); -#else px4_usleep(_main_loop_delay); -#endif if (!should_transmit()) { check_requested_subscriptions(); @@ -2881,43 +2849,6 @@ void Mavlink::configure_sik_radio() } } - -int -Mavlink::register_orb_poll(uint16_t stream_id, ORB_ID *orbs, int count) -{ -#if defined(CONFIG_MAVLINK_UORB_POLL) - return _stream_poller->register_orbs(stream_id, orbs, count); -#else - (void)stream_id; - (void)orbs; - (void)count; - return PX4_OK; -#endif -} - -int -Mavlink::unregister_orb_poll(uint16_t stream_id) -{ -#if defined(CONFIG_MAVLINK_UORB_POLL) - return _stream_poller->unregister_orbs(stream_id); -#else - (void)stream_id; - return PX4_OK; -#endif -} - -int -Mavlink::set_stream_interval(MavlinkStream *stream, int interval) -{ - stream->set_interval(interval); -#if defined(CONFIG_MAVLINK_UORB_POLL) - return _stream_poller->set_interval(stream->get_id(), interval / 1000); -#else - return PX4_OK; -#endif -} - - int Mavlink::start_helper(int argc, char *argv[]) { /* create the instance in task context */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index cfbfbb129aa0..85c0361de3ad 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -524,19 +524,7 @@ class Mavlink final : public ModuleParams bool radio_status_critical() const { return _radio_status_critical; } - /** - * Register/Unregister a stream orbs for polling - */ - int register_orb_poll(uint16_t stream_id, ORB_ID *orbs, int count); - int unregister_orb_poll(uint16_t stream_id); - - int set_stream_interval(MavlinkStream *stream, int interval); - private: -#if defined(CONFIG_MAVLINK_UORB_POLL) - MavlinkStreamPoll *_stream_poller {nullptr}; -#endif - MavlinkReceiver _receiver; int _instance_id{-1}; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 98fafdf95ec7..6b00fd4e49f5 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -128,232 +128,3 @@ MavlinkStream::update(const hrt_abstime &t) return -1; } - -#if defined(CONFIG_MAVLINK_UORB_POLL) -// 20ms default update rate for polled topics, if not set otherwise -const unsigned int MavlinkStreamDefaultTopicInterval = 20; - -MavlinkStreamPoll::MavlinkStreamPoll() : - _fds{nullptr}, - _orbs{nullptr}, - _reqs{nullptr}, - _reqs_capacity{16}, - _reqs_count{0}, - _capacity{16}, - _count{0} -{ - _orbs = (MavStreamPollItem *)malloc(16 * sizeof(MavStreamPollItem)); - _fds = (orb_poll_struct_t *)malloc(16 * sizeof(orb_poll_struct_t)); - _reqs = (MavStreamOrbPollReq *)malloc(16 * sizeof(MavStreamOrbPollReq)); -} - -MavlinkStreamPoll::~MavlinkStreamPoll() -{ - if (_fds) { - free(_fds); - } - - if (_orbs) { - free(_orbs); - } - - if (_reqs) { - free(_reqs); - } -} - -int -MavlinkStreamPoll::_add_orb(ORB_ID orb_id, int interval_ms) -{ - // Check if the orb is already in the list - bool exists = false; - - for (int i = 0; i < _count; i++) { - if (_orbs[i].orb_id == orb_id && _orbs[i].interval == interval_ms) { - _orbs[i].usage_count++; - exists = true; - break; - } - } - - // Did not exist, add new one to the list - if (!exists) { - while (_count >= _capacity) { - _capacity = _capacity + 16; - _orbs = (MavStreamPollItem *)realloc(_orbs, _capacity * sizeof(MavStreamPollItem)); - _fds = (orb_poll_struct_t *)realloc(_fds, _capacity * sizeof(orb_poll_struct_t)); - - if (!_fds || !_orbs) { - PX4_ERR("failed to allocate memory for orb poll items"); - return PX4_ERROR; - } - } - - _orbs[_count].orb_id = orb_id; - _orbs[_count].interval = interval_ms; - _orbs[_count].usage_count = 1; - _orbs[_count].fd = orb_subscribe(get_orb_meta(orb_id)); - orb_set_interval(_orbs[_count].fd, interval_ms); - _count++; - } - - return PX4_OK; -} - -int -MavlinkStreamPoll::_remove_orb(ORB_ID orb_id, int interval_ms) -{ - // Decrement usage/remove the item from the orbs list - for (int i = 0; i < _count; i++) { - if (_orbs[i].orb_id == orb_id && - _orbs[i].interval == interval_ms) { - _orbs[i].usage_count--; - - // If this was the last request for the orb, - // unsubscribe and remove from orbs list - if (_orbs[i].usage_count == 0) { - orb_unsubscribe(_orbs[i].fd); - // Replace the current item with - // the last item in the _orbs list. - // Loop counting is not disturbed because - // we break out from loop after this. - _orbs[i] = _orbs[--_count]; - } - - break; - } - } - - return PX4_OK; -} - -/** - * Register stream object to poll list of orbs - */ -int -MavlinkStreamPoll::register_orbs(uint16_t stream_id, ORB_ID *orbs, int cnt) -{ - if (!orbs || cnt <= 0) { - return PX4_OK; - } - - for (int i = 0; i < cnt; i++) { - - // Increase reqs capacity if necessary - while (_reqs_count >= _reqs_capacity) { - _reqs_capacity = _reqs_capacity + 16; - _reqs = (MavStreamOrbPollReq *)realloc(_reqs, _reqs_capacity * sizeof(MavStreamOrbPollReq)); - - if (!_reqs) { - PX4_ERR("failed to allocate memory for orb poll reqs"); - return PX4_ERROR; - } - } - - MavStreamOrbPollReq *req = &_reqs[_reqs_count]; - req->stream_id = stream_id; - req->orb_id = orbs[i]; - req->interval = MavlinkStreamDefaultTopicInterval; - _reqs_count++; - - // Update orbs list - _add_orb(orbs[i], req->interval); - } - - // Update fds - for (int i = 0; i < _count; i++) { - _fds[i].fd = _orbs[i].fd; - _fds[i].events = POLLIN; - _fds[i].revents = 0; - } - - return PX4_OK; -} - -/** - * Unregister stream object from orbs polling - */ -int -MavlinkStreamPoll::unregister_orbs(uint16_t stream_id) -{ - int i = 0; - - while (i < _reqs_count) { - if (_reqs[i].stream_id == stream_id) { - - // Remove orb from the orbs list - _remove_orb(_reqs[i].orb_id, _reqs[i].interval); - - // Finally, replace the current item with - // the last item in the _reqs list - _reqs[i] = _reqs[--_reqs_count]; - - } else { - // Only increment if current _reqs item is not removed. - // Otherwise we have moved the last item to the current - // position, so we need to check the same index again. - i++; - } - } - - // Update fds - for (int j = 0; j < _count; j++) { - _fds[j].fd = _orbs[j].fd; - _fds[j].events = POLLIN; - _fds[j].revents = 0; - } - - return PX4_OK; -} - -/** - * Set stream update interval to adjust orb polling accordingly - */ -int -MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_ms) -{ - // Update interval for all orb subscriptions and find the maximum interval - for (int i = 0; i < _count; i++) { - if (_reqs[i].stream_id == stream_id) { - - // Remove orb_id subscription with current old interval - _remove_orb(_reqs[i].orb_id, _reqs[i].interval); - - // Update interval - _reqs[i].interval = interval_ms; - - // Add orb_id subscription with new interval - _add_orb(_reqs[i].orb_id, _reqs[i].interval); - - } - } - - return PX4_OK; -} - -/** - * Perform orb polling - */ -int -MavlinkStreamPoll::poll(const hrt_abstime timeout) -{ - int timeout_ms = timeout / 1000; - - if (timeout_ms <= 0) { - timeout_ms = 1; - } - - return px4_poll(_fds, _count, timeout_ms); -} - -/** - * Acknowledge all orb data for next poll - */ -void -MavlinkStreamPoll::ack_all() -{ - for (int i = 0; i < _count; i++) { - orb_ack(_orbs[i].fd); - } -} -#endif /* CONFIG_MAVLINK_UORB_POLL */ diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 18fa1c5c794f..67034e0c9304 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -44,7 +44,6 @@ #include #include #include -#include class Mavlink; @@ -67,10 +66,7 @@ class MavlinkStream : public ListNode * * @param interval the interval in microseconds (us) between messages */ - void set_interval(const int interval) - { - _interval = interval; - } + void set_interval(const int interval) { _interval = interval; } /** * Get the interval @@ -145,96 +141,5 @@ class MavlinkStream : public ListNode bool _first_message_sent{false}; }; -#if defined(CONFIG_MAVLINK_UORB_POLL) - -/** - * Structure of objects in _reqs list - */ -struct MavStreamOrbPollReq { - uint16_t stream_id; - ORB_ID orb_id; - int interval; -}; - -/** - * Structure of objects in _orbs list - */ -struct MavStreamPollItem { - ORB_ID orb_id; - int interval; - int usage_count; - orb_sub_t fd; -}; - -class MavlinkStreamPoll -{ -public: - MavlinkStreamPoll(); - ~MavlinkStreamPoll(); - - /** - * Add a stream to the poll list - */ - int register_orbs(uint16_t stream_id, ORB_ID *orbs, int cnt); - - /** - * Remove a stream from the poll list - */ - int unregister_orbs(uint16_t stream_id); - - /** - * Set stream update interval - */ - int set_interval(uint16_t stream_id, int interval_ms); - - /** - * Poll all streams for updates - */ - int poll(const hrt_abstime timeout); - - /** - * Acknowledge all orb data for next poll - */ - void ack_all(); - -private: - - /** - * Add a orb_id/interval pair to the orbs list - */ - int _add_orb(ORB_ID orb_id, int interval_ms); - - /** - * Remove a orb_id/interval pair from the orbs list - */ - int _remove_orb(ORB_ID orb_id, int interval_ms); - - /** - * Poll file descriptors for updates - */ - orb_poll_struct_t *_fds; - - /** - * List of different orbs to poll - */ - MavStreamPollItem *_orbs; - - /** - * Requests from stream objects, contains orb poll requests - * count and capacity of the requests list - */ - MavStreamOrbPollReq *_reqs; - int _reqs_capacity; - int _reqs_count; - - /** - * Count and capacity of the orbs/fds lists - */ - int _capacity; - int _count; - -}; - -#endif /* CONFIG_MAVLINK_UORB_POLL */ #endif /* MAVLINK_STREAM_H_ */ From 8cc837230281818b9be442d58bc5b4e80a79d93c Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Thu, 6 Jun 2024 13:50:44 +0300 Subject: [PATCH 08/10] Revert "uORB: ack uorb topic" This reverts commit 7fbba1dc087f79f5fc0e04bc7c3094f8f76e1a4c. --- platforms/common/uORB/Subscription.hpp | 11 ----------- platforms/common/uORB/SubscriptionInterval.hpp | 11 ----------- platforms/common/uORB/uORB.cpp | 5 ----- platforms/common/uORB/uORB.h | 2 -- platforms/common/uORB/uORBDeviceNode.hpp | 2 -- platforms/common/uORB/uORBManager.cpp | 6 ------ platforms/common/uORB/uORBManager.hpp | 8 -------- 7 files changed, 45 deletions(-) diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index eefa7e310413..6850e280d5e3 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -143,17 +143,6 @@ class Subscription return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, true) : false; } - void ack() - { - if (!valid()) { - subscribe(); - } - - if (valid()) { - Manager::orb_data_ack(_node, _last_generation); - } - } - /** * Copy the struct * @param dst The uORB message struct we are updating. diff --git a/platforms/common/uORB/SubscriptionInterval.hpp b/platforms/common/uORB/SubscriptionInterval.hpp index 045011a00634..83f27a520dea 100644 --- a/platforms/common/uORB/SubscriptionInterval.hpp +++ b/platforms/common/uORB/SubscriptionInterval.hpp @@ -100,17 +100,6 @@ class SubscriptionInterval return false; } - /** - * Acknowledge a new update. - */ - void ack() - { - _subscription.ack(); - const hrt_abstime now = hrt_absolute_time(); - // shift last update time forward, but don't let it get further behind than the interval - _last_update = math::constrain(_last_update + _interval_us, now - _interval_us, now); - } - /** * Copy the struct if updated. * @param dst The destination pointer where the struct will be copied. diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index 3dcf20155e4d..d09be27d6326 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -130,11 +130,6 @@ int orb_check(orb_sub_t handle, bool *updated) return uORB::Manager::get_instance()->orb_check(handle, updated); } -int orb_ack(orb_sub_t handle) -{ - return uORB::Manager::get_instance()->orb_ack(handle); -} - int orb_exists(const struct orb_metadata *meta, int instance) { return uORB::Manager::orb_exists(meta, instance); diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index 3b3b80ee7ee1..2006ef08ebcd 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -246,8 +246,6 @@ extern int orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buf */ extern int orb_check(orb_sub_t handle, bool *updated) __EXPORT; -extern int orb_ack(orb_sub_t handle) __EXPORT; - /** * @see uORB::Manager::orb_exists() */ diff --git a/platforms/common/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp index 83e1d5752e33..c0f94174b757 100644 --- a/platforms/common/uORB/uORBDeviceNode.hpp +++ b/platforms/common/uORB/uORBDeviceNode.hpp @@ -219,8 +219,6 @@ class DeviceNode */ bool copy(void *dst, orb_advert_t &handle, unsigned &generation); - void ack(unsigned &generation) { generation = _generation.load(); } - static bool register_callback(orb_advert_t &node_handle, SubscriptionCallback *callback_sub, int8_t poll_lock, hrt_abstime last_update, uint32_t interval_us, uorb_cb_handle_t &cb_handle) { diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index e2846c096bbc..73f559470db1 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -365,12 +365,6 @@ int uORB::Manager::orb_check(orb_sub_t handle, bool *updated) return PX4_OK; } -int uORB::Manager::orb_ack(orb_sub_t handle) -{ - ((uORB::SubscriptionInterval *)handle)->ack(); - return PX4_OK; -} - int uORB::Manager::orb_set_interval(orb_sub_t handle, unsigned interval) { ((uORB::SubscriptionInterval *)handle)->set_interval_us(interval * 1000); diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 96e95c7b04b5..cfc297cfedc1 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -292,9 +292,6 @@ class Manager */ static int orb_check(orb_sub_t handle, bool *updated); - - static int orb_ack(orb_sub_t handle); - /** * Check if a topic has already been created and published (advertised) * @@ -360,11 +357,6 @@ class Manager static uint8_t orb_get_queue_size(const orb_advert_t &node_handle) {return node(node_handle)->get_queue_size();} - static void orb_data_ack(orb_advert_t &node_handle, unsigned &generation) - { - node(node_handle)->ack(generation); - } - static bool orb_data_copy(orb_advert_t &node_handle, void *dst, unsigned &generation, bool only_if_updated) { From 42052475f30be3bf47560b56a68786d2030401c1 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Thu, 6 Jun 2024 15:13:52 +0300 Subject: [PATCH 09/10] Implement HRT-based polling into mavlink main loop instead of sleep Sleeping depends on the systick, which prevents using slower than 1ms systicks. Using HRT provides as performant way of handling the mavlink polling and is also more accurate. Co-authored-by: Jari Nippula Signed-off-by: Jukka Laitinen --- src/modules/mavlink/mavlink_main.cpp | 29 +++++- src/modules/mavlink/mavlink_main.h | 4 + src/modules/mavlink/mavlink_stream.cpp | 117 +++++++++++++++++++++++++ src/modules/mavlink/mavlink_stream.h | 67 ++++++++++++++ 4 files changed, 214 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 26e86c64df29..32389b719130 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -134,6 +134,7 @@ Mavlink::Mavlink() : _event_sub.subscribe(); _telemetry_status_pub.advertise(); + _stream_poller = new MavlinkStreamPoll(); } Mavlink::~Mavlink() @@ -175,6 +176,8 @@ Mavlink::~Mavlink() } } + delete _stream_poller; + perf_free(_loop_perf); perf_free(_loop_interval_perf); perf_free(_send_byte_error_perf); @@ -1156,7 +1159,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (strcmp(stream_name, stream->get_name()) == 0) { if (interval != 0) { /* set new interval */ - stream->set_interval(interval); + set_stream_interval(stream, interval); } else { /* delete stream */ @@ -1173,7 +1176,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) MavlinkStream *stream = create_mavlink_stream(stream_name, this); if (stream != nullptr) { - stream->set_interval(interval); + set_stream_interval(stream, interval); _streams.add(stream); return OK; @@ -2325,9 +2328,22 @@ Mavlink::task_main(int argc, char *argv[]) _task_running.store(true); + int mavlink_poll_error_counter = 0; + while (!should_exit()) { /* main loop */ - px4_usleep(_main_loop_delay); + + int mavlink_poll_ret = _stream_poller->poll(MAIN_LOOP_DELAY); + + if (mavlink_poll_ret < 0 && mavlink_poll_ret != -ETIMEDOUT) { + /* this is seriously bad - should be an emergency */ + if (mavlink_poll_error_counter < 10 || mavlink_poll_error_counter % 50 == 0) { + /* use a counter to prevent flooding (and slowing us down) */ + PX4_ERR("ERROR while polling streams: %d", mavlink_poll_ret); + } + + mavlink_poll_error_counter++; + } if (!should_transmit()) { check_requested_subscriptions(); @@ -2849,6 +2865,13 @@ void Mavlink::configure_sik_radio() } } +int +Mavlink::set_stream_interval(MavlinkStream *stream, int interval) +{ + stream->set_interval(interval); + return _stream_poller->set_interval(stream->get_id(), interval); +} + int Mavlink::start_helper(int argc, char *argv[]) { /* create the instance in task context */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 85c0361de3ad..f9ff0a45f827 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -524,7 +524,11 @@ class Mavlink final : public ModuleParams bool radio_status_critical() const { return _radio_status_critical; } + int set_stream_interval(MavlinkStream *stream, int interval); + private: + MavlinkStreamPoll *_stream_poller {nullptr}; + MavlinkReceiver _receiver; int _instance_id{-1}; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 6b00fd4e49f5..f414cefb733c 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -44,6 +44,13 @@ #include "mavlink_stream.h" #include "mavlink_main.h" +/** + * If stream rate is set to unlimited, set the rate to 50 Hz. To get higher + * rates, it needs to be set explicitly. + */ + +const uint32_t MavlinkStreamUnlimitedInterval = 20000; + MavlinkStream::MavlinkStream(Mavlink *mavlink) : _mavlink(mavlink) { @@ -128,3 +135,113 @@ MavlinkStream::update(const hrt_abstime &t) return -1; } + +MavlinkStreamPoll::MavlinkStreamPoll() +{ + px4_sem_init(&_poll_sem, 1, 0); +#if defined(__PX4_NUTTX) + sem_setprotocol(&_poll_sem, SEM_PRIO_NONE); +#endif + + pthread_mutex_init(&_mtx, nullptr); +} + +MavlinkStreamPoll::~MavlinkStreamPoll() +{ + // This removes and deletes every object in the list + _reqs.clear(); + + px4_sem_destroy(&_poll_sem); + pthread_mutex_destroy(&_mtx); +} + +int +MavlinkStreamPoll::register_poll(uint16_t stream_id, uint32_t interval_us) +{ + // Streans with interval 0 are disabled and don't need to be registered here + + if (interval_us == 0) { + return OK; + } + + MavStreamPollReq *req = new MavStreamPollReq(stream_id); + + if (req == nullptr) { + return -ENOMEM; + } + + pthread_mutex_lock(&_mtx); + _reqs.add(req); + hrt_call_every(&req->_hrt_req, (hrt_abstime)interval_us, + (hrt_abstime)interval_us, hrt_callback, &_poll_sem); + pthread_mutex_unlock(&_mtx); + + return OK; +} + +int +MavlinkStreamPoll::unregister_poll(uint16_t stream_id) +{ + + pthread_mutex_lock(&_mtx); + + for (auto req : _reqs) { + if (req->_stream_id == stream_id) { + _reqs.remove(req); + hrt_cancel(&req->_hrt_req); + delete (req); + break; + } + } + + pthread_mutex_unlock(&_mtx); + + return OK; +} + +int +MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_us) +{ + unregister_poll(stream_id); + + if (interval_us < 0) { + interval_us = MavlinkStreamUnlimitedInterval; + } + + return register_poll(stream_id, interval_us); +} + +/** + * Perform orb polling + */ +int +MavlinkStreamPoll::poll(const hrt_abstime timeout_us) +{ + int ret; + + // Wait event for a maximum timeout time + + struct timespec to; +#if defined(CONFIG_ARCH_BOARD_PX4_SITL) + px4_clock_gettime(CLOCK_MONOTONIC, &to); +#else + px4_clock_gettime(CLOCK_REALTIME, &to); +#endif + hrt_abstime now = ts_to_abstime(&to); + abstime_to_ts(&to, now + timeout_us); + + ret = px4_sem_timedwait(&_poll_sem, &to); + + if (ret < 0) { + ret = -errno; + } + + return ret; +} + +void +MavlinkStreamPoll::hrt_callback(void *arg) +{ + px4_sem_post((px4_sem_t *)arg); +} + diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 67034e0c9304..3d5c6e809be7 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -43,6 +43,7 @@ #include #include +#include #include class Mavlink; @@ -141,5 +142,71 @@ class MavlinkStream : public ListNode bool _first_message_sent{false}; }; +/** + * Class to manage polling of stream intervals + */ + +class MavlinkStreamPoll +{ +public: + MavlinkStreamPoll(); + ~MavlinkStreamPoll(); + + /** + * Add a stream to the poll list + */ + int register_poll(uint16_t stream_id, uint32_t interval_us); + + /** + * Remove a stream from the poll list + */ + int unregister_poll(uint16_t stream_id); + + /** + * Re-set interval + */ + int set_interval(uint16_t stream_id, int interval_us); + + /** + * Poll all streams for updates + */ + int poll(const hrt_abstime timeout_us); + +private: + + class MavStreamPollReq : public ListNode + { + public: + MavStreamPollReq(uint16_t stream_id) : _stream_id(stream_id) {} + ~MavStreamPollReq() + { + hrt_cancel(&_hrt_req); + } + + uint16_t _stream_id; + struct hrt_call _hrt_req; + }; + + + /** + * HRT interrupt callback posting the semaphore + */ + static void hrt_callback(void *arg); + + /** + * Requests from stream objects + */ + List _reqs; + + /** + * Signalling semaphore to release the poll + */ + px4_sem_t _poll_sem; + + /** + * Mutex to protect the list of poll request (hrt) items + */ + pthread_mutex_t _mtx {}; +}; #endif /* MAVLINK_STREAM_H_ */ From 5c7a56aa2c55f1934afd2bedb9d9b00c20f400ec Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 7 Jun 2024 11:11:26 +0300 Subject: [PATCH 10/10] mavlink: Optimize mavlink polling with HRT timer Only use the needed HRT intervals, i.e. the shortest one by which all the other ones are evenly divisible with. Signed-off-by: Jukka Laitinen --- src/modules/mavlink/mavlink_stream.cpp | 55 ++++++++++++++++++---- src/modules/mavlink/mavlink_stream.h | 65 ++++++++++++++++++++++++-- 2 files changed, 108 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index f414cefb733c..3a08186baf3a 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -155,6 +155,34 @@ MavlinkStreamPoll::~MavlinkStreamPoll() pthread_mutex_destroy(&_mtx); } +void MavlinkStreamPoll::recalculate_roots_and_start(MavStreamPollReq *req) +{ + // Now go through the ordered _reqs list, to see if this timer needs to + // be started, and if some others can be stopped + + bool is_root = true; + uint32_t interval_us = req->interval_us(); + + for (auto r : _reqs) { + if (r->interval_us() <= interval_us) { + if (r->is_root() && interval_us % r->interval_us() == 0) { + is_root = false; + } + + } else { + if (is_root && r->is_root() && r->interval_us() % interval_us == 0) { + r->stop_interval(); + } + } + } + + // If this was a new root interval, start the hrt + + if (is_root) { + req->start_interval(hrt_callback, &_poll_sem); + } +} + int MavlinkStreamPoll::register_poll(uint16_t stream_id, uint32_t interval_us) { @@ -164,31 +192,42 @@ MavlinkStreamPoll::register_poll(uint16_t stream_id, uint32_t interval_us) return OK; } - MavStreamPollReq *req = new MavStreamPollReq(stream_id); + MavStreamPollReq *req = new MavStreamPollReq(stream_id, interval_us); if (req == nullptr) { return -ENOMEM; } pthread_mutex_lock(&_mtx); - _reqs.add(req); - hrt_call_every(&req->_hrt_req, (hrt_abstime)interval_us, - (hrt_abstime)interval_us, hrt_callback, &_poll_sem); - pthread_mutex_unlock(&_mtx); + recalculate_roots_and_start(req); + _reqs.add_sorted(req); + + + pthread_mutex_unlock(&_mtx); return OK; } int MavlinkStreamPoll::unregister_poll(uint16_t stream_id) { - pthread_mutex_lock(&_mtx); for (auto req : _reqs) { - if (req->_stream_id == stream_id) { + if (req->stream_id() == stream_id) { _reqs.remove(req); - hrt_cancel(&req->_hrt_req); + + if (req->is_root()) { + // This interval may be driving other streams. Re-calculate root clocks for all the + // remaining requests + + for (auto r : _reqs) { + recalculate_roots_and_start(r); + } + + req->stop_interval(); + } + delete (req); break; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 3d5c6e809be7..999447ca5908 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -177,16 +177,73 @@ class MavlinkStreamPoll class MavStreamPollReq : public ListNode { public: - MavStreamPollReq(uint16_t stream_id) : _stream_id(stream_id) {} + MavStreamPollReq(uint16_t stream_id, uint32_t interval_us) : _stream_id(stream_id), _interval_us(interval_us), + _is_root(false) {} ~MavStreamPollReq() { - hrt_cancel(&_hrt_req); + if (_is_root) { + hrt_cancel(&_hrt_req); + } } - uint16_t _stream_id; + void start_interval(hrt_callout cb, px4_sem_t *sem) + { + _is_root = true; + hrt_call_every(&_hrt_req, _interval_us, + _interval_us, cb, sem); + } + + void stop_interval() { _is_root = false; hrt_cancel(&_hrt_req); } + + uint32_t interval_us() { return _interval_us; } + uint16_t stream_id() { return _stream_id; } + bool is_root() { return _is_root; } + private: + uint16_t _stream_id; + uint32_t _interval_us; + bool _is_root; struct hrt_call _hrt_req; }; + class MavlinkStreamPollReqList : public List + { + public: + void add_sorted(MavStreamPollReq *req) + { + if (_head == nullptr || _head->interval_us() > req->interval_us()) { + // add as head + req->setSibling(_head); + _head = req; + return; + + } else { + // find the correct place in the list, sorted by the interval + MavStreamPollReq *node = _head; + + while (node != nullptr) { + if (node->getSibling() == nullptr || node->getSibling()->interval_us() > req->interval_us()) { + // found the end or the correct place + req->setSibling(node->getSibling()); + node->setSibling(req); + return; + } + + node = node->getSibling(); + } + } + } + }; + + /** + * Check if some stream already runs hrt at an interval, by which + * this request is evenly divisible with. This means that there is + * no need to start another periodic timer, i.e. the interval is + * not root. + * + * If the stream is root, start the timer for it and stop all the + * other timers which are evenly divisible with this one. + */ + void recalculate_roots_and_start(MavStreamPollReq *req); /** * HRT interrupt callback posting the semaphore @@ -196,7 +253,7 @@ class MavlinkStreamPoll /** * Requests from stream objects */ - List _reqs; + MavlinkStreamPollReqList _reqs; /** * Signalling semaphore to release the poll