diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 37d3c0531f70..a231e3f45752 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -221,7 +221,6 @@ def __init__(self, module_whitelist=[], topic_blacklist=[]): # to this, so that we can ignore it) special_cases_sub = [ ('sensors', r'voted_sensors_update\.cpp$', r'\binitSensorClass\b\(([^,)]+)', r'^meta$'), - ('mavlink', r'.*', r'\badd_orb_subscription\b\(([^,)]+)', r'^_topic$'), ('listener', r'.*', None, r'^(id)$'), ('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'), diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index 4388a46314ee..042dbe2097e1 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -52,9 +52,6 @@ * IOCTLs for individual topics. */ -/** Fetch the time at which the topic was last updated into *(uint64_t *)arg */ -#define ORBIOCLASTUPDATE _ORBIOC(10) - /** Check whether the topic has been updated since it was last read, sets *(bool *)arg */ #define ORBIOCUPDATED _ORBIOC(11) diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index e9f6a6e3f6ef..9ba0882c6de5 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -50,7 +50,6 @@ px4_add_module( mavlink_main.cpp mavlink_messages.cpp mavlink_mission.cpp - mavlink_orb_subscription.cpp mavlink_parameters.cpp mavlink_rate_limiter.cpp mavlink_receiver.cpp diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp index abe12b132193..3a6cd358a17b 100644 --- a/src/modules/mavlink/mavlink_high_latency2.cpp +++ b/src/modules/mavlink/mavlink_high_latency2.cpp @@ -43,56 +43,11 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include using matrix::wrap_2pi; -MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : MavlinkStream(mavlink), - _actuator_sub_0(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), - _actuator_time_0(0), - _actuator_sub_1(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))), - _actuator_time_1(0), - _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), - _airspeed_time(0), - _attitude_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), - _attitude_sp_time(0), - _estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))), - _estimator_status_time(0), - _pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))), - _pos_ctrl_status_time(0), - _geofence_sub(_mavlink->add_orb_subscription(ORB_ID(geofence_result))), - _geofence_time(0), - _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), - _local_pos_time(0), - _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), - _global_pos_time(0), - _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), - _gps_time(0), - _mission_result_sub(_mavlink->add_orb_subscription(ORB_ID(mission_result))), - _mission_result_time(0), - _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), - _status_time(0), - _status_flags_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status_flags))), - _status_flags_time(0), - _tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))), - _tecs_time(0), - _wind_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))), - _wind_time(0), +MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : + MavlinkStream(mavlink), _airspeed(SimpleAnalyzer::AVERAGE), _airspeed_sp(SimpleAnalyzer::AVERAGE), _climb_rate(SimpleAnalyzer::MAX), @@ -103,11 +58,6 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink _throttle(SimpleAnalyzer::AVERAGE), _windspeed(SimpleAnalyzer::AVERAGE) { - - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - _batteries[i].subscription = _mavlink->add_orb_subscription(ORB_ID(battery_status), i); - } - reset_last_sent(); } @@ -116,7 +66,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) // only send the struct if transmitting is allowed // this assures that the stream timer is only reset when actually a message is transmitted if (_mavlink->should_transmit()) { - mavlink_high_latency2_t msg = {}; + mavlink_high_latency2_t msg{}; set_default_values(msg); bool updated = _airspeed.valid(); @@ -218,7 +168,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) } else { // reset the analysers every 60 seconds if nothing should be transmitted - if ((t - _last_reset_time) > 60000000) { + if ((t - _last_reset_time) > 60_s) { reset_analysers(t); PX4_DEBUG("Resetting HIGH_LATENCY2 analysers"); } @@ -250,30 +200,29 @@ void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t) bool MavlinkStreamHighLatency2::write_airspeed(mavlink_high_latency2_t *msg) { - struct airspeed_s airspeed; - - const bool updated = _airspeed_sub->update(&_airspeed_time, &airspeed); + airspeed_s airspeed; - if (_airspeed_time > 0) { + if (_airspeed_sub.update(&airspeed)) { if (airspeed.confidence < 0.95f) { // the same threshold as for the commander msg->failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; } + + return true; } - return updated; + return false; } bool MavlinkStreamHighLatency2::write_attitude_sp(mavlink_high_latency2_t *msg) { - struct vehicle_attitude_setpoint_s attitude_sp; + vehicle_attitude_setpoint_s attitude_sp; - const bool updated = _attitude_sp_sub->update(&_attitude_sp_time, &attitude_sp); - - if (_attitude_sp_time > 0) { + if (_attitude_sp_sub.update(&attitude_sp)) { msg->target_heading = static_cast(math::degrees(wrap_2pi(attitude_sp.yaw_body)) * 0.5f); + return true; } - return updated; + return false; } bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *msg) @@ -282,7 +231,7 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms bool updated = false; for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_batteries[i].subscription->update(&_batteries[i].timestamp, &battery)) { + if (_batteries[i].subscription.update(&battery)) { updated = true; _batteries[i].connected = battery.connected; @@ -297,58 +246,56 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms bool MavlinkStreamHighLatency2::write_estimator_status(mavlink_high_latency2_t *msg) { - struct estimator_status_s estimator_status; + estimator_status_s estimator_status; - const bool updated = _estimator_status_sub->update(&_estimator_status_time, &estimator_status); - - if (_estimator_status_time > 0) { + if (_estimator_status_sub.update(&estimator_status)) { if (estimator_status.gps_check_fail_flags > 0 || estimator_status.filter_fault_flags > 0 || estimator_status.innovation_check_flags > 0) { + msg->failure_flags |= HL_FAILURE_FLAG_ESTIMATOR; } + + return true; } - return updated; + return false; } bool MavlinkStreamHighLatency2::write_fw_ctrl_status(mavlink_high_latency2_t *msg) { - position_controller_status_s pos_ctrl_status = {}; - - const bool updated = _pos_ctrl_status_sub->update(&_pos_ctrl_status_time, &pos_ctrl_status); + position_controller_status_s pos_ctrl_status; - if (_pos_ctrl_status_time > 0) { + if (_pos_ctrl_status_sub.update(&pos_ctrl_status)) { uint16_t target_distance; convert_limit_safe(pos_ctrl_status.wp_dist * 0.1f, target_distance); msg->target_distance = target_distance; + return true; } - return updated; + return false; } bool MavlinkStreamHighLatency2::write_geofence_result(mavlink_high_latency2_t *msg) { - struct geofence_result_s geofence; - - const bool updated = _geofence_sub->update(&_geofence_time, &geofence); + geofence_result_s geofence; - if (_geofence_time > 0) { + if (_geofence_sub.update(&geofence)) { if (geofence.geofence_violated) { msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE; } + + return true; } - return updated; + return false; } bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *msg) { - struct vehicle_global_position_s global_pos; + vehicle_global_position_s global_pos; - const bool updated = _global_pos_sub->update(&_global_pos_time, &global_pos); - - if (_global_pos_time > 0) { + if (_global_pos_sub.update(&global_pos)) { msg->latitude = global_pos.lat * 1e7; msg->longitude = global_pos.lon * 1e7; @@ -364,46 +311,45 @@ bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *m msg->altitude = altitude; msg->heading = static_cast(math::degrees(wrap_2pi(global_pos.yaw)) * 0.5f); + + return true; } - return updated; + return false; } bool MavlinkStreamHighLatency2::write_mission_result(mavlink_high_latency2_t *msg) { - struct mission_result_s mission_result; + mission_result_s mission_result; - const bool updated = _mission_result_sub->update(&_mission_result_time, &mission_result); - - if (_mission_result_time > 0) { + if (_mission_result_sub.update(&mission_result)) { msg->wp_num = mission_result.seq_current; + return true; } - return updated; + return false; } bool MavlinkStreamHighLatency2::write_tecs_status(mavlink_high_latency2_t *msg) { - struct tecs_status_s tecs_status; - - const bool updated = _tecs_status_sub->update(&_tecs_time, &tecs_status); + tecs_status_s tecs_status; - if (_tecs_time > 0) { + if (_tecs_status_sub.update(&tecs_status)) { int16_t target_altitude; convert_limit_safe(tecs_status.altitude_sp, target_altitude); msg->target_altitude = target_altitude; + + return true; } - return updated; + return false; } bool MavlinkStreamHighLatency2::write_vehicle_status(mavlink_high_latency2_t *msg) { - struct vehicle_status_s status; - - const bool updated = _status_sub->update(&_status_time, &status); + vehicle_status_s status; - if (_status_time > 0) { + if (_status_sub.update(&status)) { if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)) { msg->failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; @@ -452,18 +398,18 @@ bool MavlinkStreamHighLatency2::write_vehicle_status(mavlink_high_latency2_t *ms uint8_t mavlink_base_mode; get_mavlink_navigation_mode(&status, &mavlink_base_mode, &custom_mode); msg->custom_mode = custom_mode.custom_mode_hl; + + return true; } - return updated; + return false; } bool MavlinkStreamHighLatency2::write_vehicle_status_flags(mavlink_high_latency2_t *msg) { - struct vehicle_status_flags_s status_flags; + vehicle_status_flags_s status_flags; - const bool updated = _status_flags_sub->update(&_status_flags_time, &status_flags); - - if (_status_flags_time > 0) { + if (_status_flags_sub.update(&status_flags)) { if (!status_flags.condition_global_position_valid) { //TODO check if there is a better way to get only GPS failure msg->failure_flags |= HL_FAILURE_FLAG_GPS; } @@ -471,23 +417,24 @@ bool MavlinkStreamHighLatency2::write_vehicle_status_flags(mavlink_high_latency2 if (status_flags.offboard_control_signal_lost && status_flags.offboard_control_signal_found_once) { msg->failure_flags |= HL_FAILURE_FLAG_OFFBOARD_LINK; } + + return true; } - return updated; + return false; } bool MavlinkStreamHighLatency2::write_wind_estimate(mavlink_high_latency2_t *msg) { - struct wind_estimate_s wind; - - const bool updated = _wind_sub->update(&_wind_time, &wind); + wind_estimate_s wind; - if (_wind_time > 0) { - msg->wind_heading = static_cast( - math::degrees(wrap_2pi(atan2f(wind.windspeed_east, wind.windspeed_north))) * 0.5f); + if (_wind_sub.update(&wind)) { + msg->wind_heading = static_cast(math::degrees(wrap_2pi(atan2f(wind.windspeed_east, + wind.windspeed_north))) * 0.5f); + return true; } - return updated; + return false; } void MavlinkStreamHighLatency2::update_data() @@ -519,7 +466,7 @@ void MavlinkStreamHighLatency2::update_airspeed() { airspeed_s airspeed; - if (_airspeed_sub->update(&airspeed)) { + if (_airspeed_sub.update(&airspeed)) { _airspeed.add_value(airspeed.indicated_airspeed_m_s, _update_rate_filtered); _temperature.add_value(airspeed.air_temperature_celsius, _update_rate_filtered); } @@ -529,7 +476,7 @@ void MavlinkStreamHighLatency2::update_tecs_status() { tecs_status_s tecs_status; - if (_tecs_status_sub->update(&tecs_status)) { + if (_tecs_status_sub.update(&tecs_status)) { _airspeed_sp.add_value(tecs_status.airspeed_sp, _update_rate_filtered); } } @@ -539,7 +486,7 @@ void MavlinkStreamHighLatency2::update_battery_status() battery_status_s battery; for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_batteries[i].subscription->update(&battery)) { + if (_batteries[i].subscription.update(&battery)) { _batteries[i].connected = battery.connected; _batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered); } @@ -550,7 +497,7 @@ void MavlinkStreamHighLatency2::update_local_position() { vehicle_local_position_s local_pos; - if (_local_pos_sub->update(&local_pos)) { + if (_local_pos_sub.update(&local_pos)) { _climb_rate.add_value(fabsf(local_pos.vz), _update_rate_filtered); _groundspeed.add_value(sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy), _update_rate_filtered); } @@ -560,7 +507,7 @@ void MavlinkStreamHighLatency2::update_gps() { vehicle_gps_position_s gps; - if (_gps_sub->update(&gps)) { + if (_gps_sub.update(&gps)) { _eph.add_value(gps.eph, _update_rate_filtered); _epv.add_value(gps.epv, _update_rate_filtered); } @@ -570,17 +517,17 @@ void MavlinkStreamHighLatency2::update_vehicle_status() { vehicle_status_s status; - if (_status_sub->update(&status)) { + if (_status_sub.update(&status)) { if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - struct actuator_controls_s actuator = {}; + actuator_controls_s actuator{}; if (status.is_vtol && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (_actuator_sub_1->update(&actuator)) { + if (_actuator_1_sub.copy(&actuator)) { _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered); } } else { - if (_actuator_sub_0->update(&actuator)) { + if (_actuator_0_sub.copy(&actuator)) { _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered); } } @@ -595,7 +542,7 @@ void MavlinkStreamHighLatency2::update_wind_estimate() { wind_estimate_s wind; - if (_wind_sub->update(&wind)) { + if (_wind_sub.update(&wind)) { _windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east), _update_rate_filtered); } diff --git a/src/modules/mavlink/mavlink_high_latency2.h b/src/modules/mavlink/mavlink_high_latency2.h index da3ecb4b3bc9..24b36f1a9e01 100644 --- a/src/modules/mavlink/mavlink_high_latency2.h +++ b/src/modules/mavlink/mavlink_high_latency2.h @@ -44,6 +44,22 @@ #include "mavlink_simple_analyzer.h" #include "mavlink_stream.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + class MavlinkStreamHighLatency2 : public MavlinkStream { public: @@ -85,57 +101,28 @@ class MavlinkStreamHighLatency2 : public MavlinkStream private: struct PerBatteryData { - PerBatteryData() {} - MavlinkOrbSubscription *subscription{nullptr}; + PerBatteryData(uint8_t instance) : subscription(ORB_ID(battery_status), instance) {} + uORB::Subscription subscription; SimpleAnalyzer analyzer{SimpleAnalyzer::AVERAGE}; uint64_t timestamp{0}; bool connected{false}; }; - MavlinkOrbSubscription *_actuator_sub_0; - uint64_t _actuator_time_0; - - MavlinkOrbSubscription *_actuator_sub_1; - uint64_t _actuator_time_1; - - MavlinkOrbSubscription *_airspeed_sub; - uint64_t _airspeed_time; - - MavlinkOrbSubscription *_attitude_sp_sub; - uint64_t _attitude_sp_time; - - MavlinkOrbSubscription *_estimator_status_sub; - uint64_t _estimator_status_time; - - MavlinkOrbSubscription *_pos_ctrl_status_sub; - uint64_t _pos_ctrl_status_time; - - MavlinkOrbSubscription *_geofence_sub; - uint64_t _geofence_time; - - MavlinkOrbSubscription *_local_pos_sub; - uint64_t _local_pos_time; - - MavlinkOrbSubscription *_global_pos_sub; - uint64_t _global_pos_time; - - MavlinkOrbSubscription *_gps_sub; - uint64_t _gps_time; - - MavlinkOrbSubscription *_mission_result_sub; - uint64_t _mission_result_time; - - MavlinkOrbSubscription *_status_sub; - uint64_t _status_time; - - MavlinkOrbSubscription *_status_flags_sub; - uint64_t _status_flags_time; - - MavlinkOrbSubscription *_tecs_status_sub; - uint64_t _tecs_time; - - MavlinkOrbSubscription *_wind_sub; - uint64_t _wind_time; + uORB::Subscription _actuator_0_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _actuator_1_sub{ORB_ID(actuator_controls_1)}; + uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; + uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)}; + uORB::Subscription _geofence_sub{ORB_ID(geofence_result)}; + uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _status_flags_sub{ORB_ID(vehicle_status_flags)}; + uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; + uORB::Subscription _wind_sub{ORB_ID(wind_estimate)}; SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; @@ -151,7 +138,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream hrt_abstime _last_update_time = 0; float _update_rate_filtered = 0.0f; - PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES]; + PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES] {0, 1, 2, 3}; /* do not allow top copying this class */ MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 06468855a890..e97b7cb2fa61 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1155,12 +1155,11 @@ Mavlink::send_statustext_emergency(const char *string) void Mavlink::send_autopilot_capabilites() { - struct vehicle_status_s status; + uORB::Subscription status_sub{ORB_ID(vehicle_status)}; + vehicle_status_s status; - MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status)); - - if (status_sub->update(&status)) { - mavlink_autopilot_version_t msg = {}; + if (status_sub.copy(&status)) { + mavlink_autopilot_version_t msg{}; msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT; @@ -1242,27 +1241,6 @@ Mavlink::send_protocol_version() set_proto_version(curr_proto_ver); } -MavlinkOrbSubscription * -Mavlink::add_orb_subscription(const orb_id_t topic, int instance, bool disable_sharing) -{ - if (!disable_sharing) { - /* check if already subscribed to this topic */ - for (MavlinkOrbSubscription *sub : _subscriptions) { - if (sub->get_topic() == topic && sub->get_instance() == instance) { - /* already subscribed */ - return sub; - } - } - } - - /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance); - - _subscriptions.add(sub_new); - - return sub_new; -} - int Mavlink::configure_stream(const char *stream_name, const float rate) { @@ -2138,22 +2116,17 @@ Mavlink::task_main(int argc, char *argv[]) uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; - MavlinkOrbSubscription *cmd_sub = add_orb_subscription(ORB_ID(vehicle_command), 0, true); - MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); - uint64_t status_time = 0; - MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack), 0, true); - /* We don't want to miss the first advertise of an ACK, so we subscribe from the - * beginning and not just when the topic exists. */ - ack_sub->subscribe_from_beginning(true); - cmd_sub->subscribe_from_beginning(true); + uORB::Subscription cmd_sub{ORB_ID(vehicle_command)}; + uORB::Subscription status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription ack_sub{ORB_ID(vehicle_command_ack)}; /* command ack */ uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; - MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log)); + uORB::Subscription mavlink_log_sub{ORB_ID(mavlink_log)}; vehicle_status_s status{}; - status_sub->update(&status_time, &status); + status_sub.copy(&status); /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ _transmitting_enabled = true; @@ -2268,7 +2241,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_sik_radio(); - if (status_sub->update(&status_time, &status)) { + if (status_sub.update(&status)) { /* switch HIL mode if required */ set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); @@ -2291,9 +2264,9 @@ Mavlink::task_main(int argc, char *argv[]) } } - vehicle_command_s vehicle_cmd{}; + vehicle_command_s vehicle_cmd; - if (cmd_sub->update_if_changed(&vehicle_cmd)) { + if (cmd_sub.update(&vehicle_cmd)) { if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && (_mode == MAVLINK_MODE_IRIDIUM)) { if (vehicle_cmd.param1 > 0.5f) { @@ -2330,9 +2303,9 @@ Mavlink::task_main(int argc, char *argv[]) /* send command ACK */ uint16_t current_command_ack = 0; - vehicle_command_ack_s command_ack{}; + vehicle_command_ack_s command_ack; - if (ack_sub->update_if_changed(&command_ack)) { + if (ack_sub.update(&command_ack)) { if (!command_ack.from_external) { mavlink_command_ack_t msg; msg.result = command_ack.result; @@ -2351,9 +2324,9 @@ Mavlink::task_main(int argc, char *argv[]) } } - mavlink_log_s mavlink_log{}; + mavlink_log_s mavlink_log; - if (mavlink_log_sub->update_if_changed(&mavlink_log)) { + if (mavlink_log_sub.update(&mavlink_log)) { _logbuffer.put(&mavlink_log); } @@ -2497,9 +2470,6 @@ Mavlink::task_main(int argc, char *argv[]) /* delete streams */ _streams.clear(); - /* delete subscriptions */ - _subscriptions.clear(); - if (_uart_fd >= 0 && !_is_usb_uart) { /* close UART */ ::close(_uart_fd); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 7c3ab0f7fa06..9e5d36474eb7 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -79,7 +79,6 @@ #include "mavlink_command_sender.h" #include "mavlink_messages.h" -#include "mavlink_orb_subscription.h" #include "mavlink_shell.h" #include "mavlink_ulog.h" @@ -342,15 +341,6 @@ class Mavlink : public ModuleParams void handle_message(const mavlink_message_t *msg); - /** - * Add a mavlink orb topic subscription while ensuring that only a single object exists - * for a given topic id and instance. - * @param topic orb topic id - * @param instance topic instance - * @param disable_sharing if true, force creating a new instance - */ - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance = 0, bool disable_sharing = false); - int get_instance_id() const { return _instance_id; } /** @@ -565,7 +555,6 @@ class Mavlink : public ModuleParams unsigned _main_loop_delay{1000}; /**< mainloop delay, depends on data rate */ - List _subscriptions; List _streams; MavlinkShell *_mavlink_shell{nullptr}; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6665e2b19c55..29b0a33282d0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -55,6 +55,7 @@ #include #include +#include #include #include #include @@ -115,13 +116,7 @@ using matrix::Vector3f; using matrix::wrap_2pi; -static uint16_t cm_uint16_from_m_float(float m); - -static void get_mavlink_mode_state(const struct vehicle_status_s *const status, uint8_t *mavlink_state, - uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); - -uint16_t -cm_uint16_from_m_float(float m) +static uint16_t cm_uint16_from_m_float(float m) { if (m < 0.0f) { return 0; @@ -264,8 +259,8 @@ void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, ui } } -void get_mavlink_mode_state(const struct vehicle_status_s *const status, uint8_t *mavlink_state, - uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) +static void get_mavlink_mode_state(const struct vehicle_status_s *const status, uint8_t *mavlink_state, + uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -335,26 +330,21 @@ class MavlinkStreamHeartbeat : public MavlinkStream } private: - MavlinkOrbSubscription *_status_sub; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &) = delete; MavlinkStreamHeartbeat &operator = (const MavlinkStreamHeartbeat &) = delete; protected: - explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), - _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct vehicle_status_s status = {}; - - /* always send the heartbeat, independent of the update status of the topics */ - if (!_status_sub->update(&status)) { - /* if topic update failed fill it with defaults */ - memset(&status, 0, sizeof(status)); - } + // always send the heartbeat, independent of the update status of the topics + vehicle_status_s status{}; + _status_sub.copy(&status); uint8_t base_mode = 0; uint32_t custom_mode = 0; @@ -414,11 +404,11 @@ class MavlinkStreamStatustext : public MavlinkStream { if (!_mavlink->get_logbuffer()->empty() && _mavlink->is_connected()) { - struct mavlink_log_s mavlink_log = {}; + mavlink_log_s mavlink_log{}; if (_mavlink->get_logbuffer()->get(&mavlink_log)) { - mavlink_statustext_t msg; + mavlink_statustext_t msg{}; msg.severity = mavlink_log.severity; strncpy(msg.text, (const char *)mavlink_log.text, sizeof(msg.text)); msg.text[sizeof(msg.text) - 1] = '\0'; @@ -467,15 +457,14 @@ class MavlinkStreamCommandLong : public MavlinkStream } private: - MavlinkOrbSubscription *_cmd_sub; + uORB::Subscription _cmd_sub{ORB_ID(vehicle_command)}; /* do not allow top copying this class */ MavlinkStreamCommandLong(MavlinkStreamCommandLong &) = delete; MavlinkStreamCommandLong &operator = (const MavlinkStreamCommandLong &) = delete; protected: - explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), - _cmd_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_command), 0, true)) + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override @@ -483,7 +472,7 @@ class MavlinkStreamCommandLong : public MavlinkStream struct vehicle_command_s cmd; bool sent = false; - if (_cmd_sub->update_if_changed(&cmd)) { + if (_cmd_sub.update(&cmd)) { if (!cmd.from_external) { PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); @@ -536,47 +525,44 @@ class MavlinkStreamSysStatus : public MavlinkStream } private: - MavlinkOrbSubscription *_status_sub; - MavlinkOrbSubscription *_cpuload_sub; - MavlinkOrbSubscription *_battery_status_sub[ORB_MULTI_MAX_INSTANCES]; - - uint64_t _status_timestamp{0}; - uint64_t _cpuload_timestamp{0}; - uint64_t _battery_status_timestamp[ORB_MULTI_MAX_INSTANCES] {}; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; + uORB::Subscription _battery_status_sub[ORB_MULTI_MAX_INSTANCES] { + {ORB_ID(battery_status), 0}, {ORB_ID(battery_status), 1}, {ORB_ID(battery_status), 2}, {ORB_ID(battery_status), 3} + }; /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &) = delete; MavlinkStreamSysStatus &operator = (const MavlinkStreamSysStatus &) = delete; protected: - explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), - _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), - _cpuload_sub(_mavlink->add_orb_subscription(ORB_ID(cpuload))) + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink) { - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - _battery_status_sub[i] = _mavlink->add_orb_subscription(ORB_ID(battery_status), i); - _battery_status_timestamp[i] = 0; - } } bool send(const hrt_abstime t) override { - vehicle_status_s status{}; - cpuload_s cpuload{}; - battery_status_s battery_status[ORB_MULTI_MAX_INSTANCES] {}; - - const bool updated_status = _status_sub->update(&_status_timestamp, &status); - const bool updated_cpuload = _cpuload_sub->update(&_cpuload_timestamp, &cpuload); - bool updated_battery = false; for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status[i])) { + if (_battery_status_sub[i].updated()) { updated_battery = true; } } - if (updated_status || updated_cpuload || updated_battery) { + if (_status_sub.updated() || _cpuload_sub.updated() || updated_battery) { + vehicle_status_s status{}; + _status_sub.copy(&status); + + cpuload_s cpuload{}; + _cpuload_sub.copy(&cpuload); + + battery_status_s battery_status[ORB_MULTI_MAX_INSTANCES] {}; + + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + _battery_status_sub[i].copy(&battery_status[i]); + } + int lowest_battery_index = 0; for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { @@ -650,13 +636,21 @@ class MavlinkStreamBatteryStatus : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + unsigned total_size = 0; + + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (_battery_status_sub[i].advertised()) { + total_size += MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + } + + return total_size; } private: - MavlinkOrbSubscription *_battery_status_sub[ORB_MULTI_MAX_INSTANCES] {}; - - uint64_t _battery_status_timestamp[ORB_MULTI_MAX_INSTANCES] {}; + uORB::Subscription _battery_status_sub[ORB_MULTI_MAX_INSTANCES] { + {ORB_ID(battery_status), 0}, {ORB_ID(battery_status), 1}, {ORB_ID(battery_status), 2}, {ORB_ID(battery_status), 3} + }; /* do not allow top copying this class */ MavlinkStreamBatteryStatus(MavlinkStreamSysStatus &) = delete; @@ -665,9 +659,6 @@ class MavlinkStreamBatteryStatus : public MavlinkStream protected: explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink) { - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - _battery_status_sub[i] = _mavlink->add_orb_subscription(ORB_ID(battery_status), i); - } } bool send(const hrt_abstime t) override @@ -675,14 +666,9 @@ class MavlinkStreamBatteryStatus : public MavlinkStream bool updated = false; for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + battery_status_s battery_status; - if (!_battery_status_sub[i]) { - continue; - } - - battery_status_s battery_status{}; - - if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status)) { + if (_battery_status_sub[i].update(&battery_status)) { /* battery status message with higher resolution */ mavlink_battery_status_t bat_msg{}; // TODO: Determine how to better map between battery ID within the firmware and in MAVLink @@ -786,89 +772,64 @@ class MavlinkStreamHighresIMU : public MavlinkStream } private: - MavlinkOrbSubscription *_sensor_sub; - uint64_t _sensor_time; - - MavlinkOrbSubscription *_bias_sub; - MavlinkOrbSubscription *_differential_pressure_sub; - MavlinkOrbSubscription *_magnetometer_sub; - MavlinkOrbSubscription *_air_data_sub; - - uint64_t _accel_timestamp; - uint64_t _gyro_timestamp; - uint64_t _mag_timestamp; - uint64_t _baro_timestamp; - uint64_t _dpres_timestamp; + uORB::Subscription _sensor_sub{ORB_ID(sensor_combined)}; + uORB::Subscription _bias_sub{ORB_ID(estimator_sensor_bias)}; + uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; + uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; /* do not allow top copying this class */ MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &) = delete; MavlinkStreamHighresIMU &operator = (const MavlinkStreamHighresIMU &) = delete; protected: - explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), - _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), - _sensor_time(0), - _bias_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_sensor_bias))), - _differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))), - _magnetometer_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_magnetometer))), - _air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))), - _accel_timestamp(0), - _gyro_timestamp(0), - _mag_timestamp(0), - _baro_timestamp(0), - _dpres_timestamp(0) + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { sensor_combined_s sensor; - if (_sensor_sub->update(&_sensor_time, &sensor)) { + if (_sensor_sub.update(&sensor)) { uint16_t fields_updated = 0; - if (_accel_timestamp != sensor.timestamp + sensor.accelerometer_timestamp_relative) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - _accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative; - } + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); // accel + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); // gyro - if (_gyro_timestamp != sensor.timestamp) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - _gyro_timestamp = sensor.timestamp; - } - - vehicle_magnetometer_s magnetometer = {}; - _magnetometer_sub->update(&magnetometer); + vehicle_magnetometer_s magnetometer{}; - if (_mag_timestamp != magnetometer.timestamp) { + if (_magnetometer_sub.update(&magnetometer)) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - _mag_timestamp = magnetometer.timestamp; + + } else { + _magnetometer_sub.copy(&magnetometer); } - vehicle_air_data_s air_data = {}; - _air_data_sub->update(&air_data); + vehicle_air_data_s air_data{}; - if (_baro_timestamp != air_data.timestamp) { + if (_air_data_sub.update(&air_data)) { /* mark fourth group (baro fields) dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - _baro_timestamp = air_data.timestamp; - } - estimator_sensor_bias_s bias{}; - _bias_sub->update(&bias); + } else { + _air_data_sub.copy(&air_data); + } - differential_pressure_s differential_pressure = {}; - _differential_pressure_sub->update(&differential_pressure); + differential_pressure_s differential_pressure{}; - if (_dpres_timestamp != differential_pressure.timestamp) { + if (_differential_pressure_sub.update(&differential_pressure)) { /* mark fourth group (dpres field) dimensions as changed */ fields_updated |= (1 << 10); - _dpres_timestamp = differential_pressure.timestamp; + + } else { + _differential_pressure_sub.copy(&differential_pressure); } - mavlink_highres_imu_t msg = {}; + estimator_sensor_bias_s bias{}; + _bias_sub.copy(&bias); + + mavlink_highres_imu_t msg{}; msg.time_usec = sensor.timestamp; msg.xacc = sensor.accelerometer_m_s2[0] - bias.accel_bias[0]; @@ -915,41 +876,31 @@ class MavlinkStreamScaledPressureBase : public MavlinkStream } private: - MavlinkOrbSubscription *_differential_pressure_sub; - MavlinkOrbSubscription *_sensor_baro_sub; - - uint64_t _baro_timestamp; - uint64_t _dpres_timestamp; + uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), N}; /* do not allow top copying this class */ MavlinkStreamScaledPressureBase(MavlinkStreamScaledPressureBase &) = delete; MavlinkStreamScaledPressureBase &operator = (const MavlinkStreamScaledPressureBase &) = delete; protected: - explicit MavlinkStreamScaledPressureBase(Mavlink *mavlink) : MavlinkStream(mavlink), - _differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))), - _sensor_baro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_baro), N)), - _baro_timestamp(0), - _dpres_timestamp(0) + explicit MavlinkStreamScaledPressureBase(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - sensor_baro_s sensor_baro{}; - differential_pressure_s differential_pressure{}; + if (_sensor_baro_sub.updated() || _differential_pressure_sub.updated()) { + sensor_baro_s sensor_baro{}; + differential_pressure_s differential_pressure{}; + _sensor_baro_sub.copy(&sensor_baro); + _differential_pressure_sub.copy(&differential_pressure); - bool updated = false; - updated |= _sensor_baro_sub->update(&_baro_timestamp, &sensor_baro); - updated |= _differential_pressure_sub->update(&_dpres_timestamp, &differential_pressure); - - if (updated) { typename Derived::mav_msg_type msg{}; msg.time_boot_ms = sensor_baro.timestamp / 1000; msg.press_abs = sensor_baro.pressure; msg.press_diff = differential_pressure.differential_pressure_raw_pa; msg.temperature = sensor_baro.temperature; - Derived::send(_mavlink->get_channel(), &msg); return true; @@ -1081,44 +1032,35 @@ class MavlinkStreamScaledIMU : public MavlinkStream unsigned get_size() override { - return _raw_accel_sub->is_published() ? (MAVLINK_MSG_ID_SCALED_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_raw_accel_sub; - MavlinkOrbSubscription *_raw_gyro_sub; - MavlinkOrbSubscription *_raw_mag_sub; - - uint64_t _raw_accel_time; - uint64_t _raw_gyro_time; - uint64_t _raw_mag_time; + uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 0}; + uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 0}; + uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 0}; // do not allow top copy this class MavlinkStreamScaledIMU(MavlinkStreamScaledIMU &) = delete; MavlinkStreamScaledIMU &operator = (const MavlinkStreamScaledIMU &) = delete; protected: - explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink), - _raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 0)), - _raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 0)), - _raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 0)), - _raw_accel_time(0), - _raw_gyro_time(0), - _raw_mag_time(0) + explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - sensor_accel_integrated_s sensor_accel{}; - sensor_gyro_integrated_s sensor_gyro{}; - sensor_mag_s sensor_mag{}; + if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) { - bool updated = false; - updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel); - updated |= _raw_gyro_sub->update(&_raw_gyro_time, &sensor_gyro); - updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag); + sensor_accel_integrated_s sensor_accel{}; + _raw_accel_sub.copy(&sensor_accel); + + sensor_gyro_integrated_s sensor_gyro{}; + _raw_gyro_sub.copy(&sensor_gyro); + + sensor_mag_s sensor_mag{}; + _raw_mag_sub.copy(&sensor_mag); - if (updated) { mavlink_scaled_imu_t msg{}; msg.time_boot_ms = sensor_accel.timestamp / 1000; @@ -1150,7 +1092,6 @@ class MavlinkStreamScaledIMU : public MavlinkStream } }; - class MavlinkStreamScaledIMU2 : public MavlinkStream { public: @@ -1181,44 +1122,35 @@ class MavlinkStreamScaledIMU2 : public MavlinkStream unsigned get_size() override { - return _raw_accel_sub->is_published() ? (MAVLINK_MSG_ID_SCALED_IMU2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_raw_accel_sub; - MavlinkOrbSubscription *_raw_gyro_sub; - MavlinkOrbSubscription *_raw_mag_sub; - - uint64_t _raw_accel_time; - uint64_t _raw_gyro_time; - uint64_t _raw_mag_time; + uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 1}; + uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 1}; + uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 1}; // do not allow top copy this class MavlinkStreamScaledIMU2(MavlinkStreamScaledIMU2 &) = delete; MavlinkStreamScaledIMU2 &operator = (const MavlinkStreamScaledIMU2 &) = delete; protected: - explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink), - _raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 1)), - _raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 1)), - _raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 1)), - _raw_accel_time(0), - _raw_gyro_time(0), - _raw_mag_time(0) + explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - sensor_accel_integrated_s sensor_accel{}; - sensor_gyro_integrated_s sensor_gyro{}; - sensor_mag_s sensor_mag{}; + if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) { - bool updated = false; - updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel); - updated |= _raw_gyro_sub->update(&_raw_gyro_time, &sensor_gyro); - updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag); + sensor_accel_integrated_s sensor_accel{}; + _raw_accel_sub.copy(&sensor_accel); + + sensor_gyro_integrated_s sensor_gyro{}; + _raw_gyro_sub.copy(&sensor_gyro); + + sensor_mag_s sensor_mag{}; + _raw_mag_sub.copy(&sensor_mag); - if (updated) { mavlink_scaled_imu2_t msg{}; msg.time_boot_ms = sensor_accel.timestamp / 1000; @@ -1279,44 +1211,35 @@ class MavlinkStreamScaledIMU3 : public MavlinkStream unsigned get_size() override { - return _raw_accel_sub->is_published() ? (MAVLINK_MSG_ID_SCALED_IMU3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_raw_accel_sub; - MavlinkOrbSubscription *_raw_gyro_sub; - MavlinkOrbSubscription *_raw_mag_sub; - - uint64_t _raw_accel_time; - uint64_t _raw_gyro_time; - uint64_t _raw_mag_time; + uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 2}; + uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 2}; + uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 2}; // do not allow top copy this class MavlinkStreamScaledIMU3(MavlinkStreamScaledIMU3 &) = delete; MavlinkStreamScaledIMU3 &operator = (const MavlinkStreamScaledIMU3 &) = delete; protected: - explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink), - _raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 2)), - _raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 2)), - _raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 2)), - _raw_accel_time(0), - _raw_gyro_time(0), - _raw_mag_time(0) + explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - sensor_accel_integrated_s sensor_accel{}; - sensor_gyro_integrated_s sensor_gyro{}; - sensor_mag_s sensor_mag{}; + if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) { - bool updated = false; - updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel); - updated |= _raw_gyro_sub->update(&_raw_gyro_time, &sensor_gyro); - updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag); + sensor_accel_integrated_s sensor_accel{}; + _raw_accel_sub.copy(&sensor_accel); + + sensor_gyro_integrated_s sensor_gyro{}; + _raw_gyro_sub.copy(&sensor_gyro); + + sensor_mag_s sensor_mag{}; + _raw_mag_sub.copy(&sensor_mag); - if (updated) { mavlink_scaled_imu3_t msg{}; msg.time_boot_ms = sensor_accel.timestamp / 1000; @@ -1379,13 +1302,12 @@ class MavlinkStreamAttitude : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _att_sub.advertised() ? MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_att_sub; - MavlinkOrbSubscription *_angular_velocity_sub; - uint64_t _att_time{0}; + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &) = delete; @@ -1393,18 +1315,16 @@ class MavlinkStreamAttitude : public MavlinkStream protected: - explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), - _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), - _angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity))) + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { vehicle_attitude_s att; - if (_att_sub->update(&_att_time, &att)) { + if (_att_sub.update(&att)) { vehicle_angular_velocity_s angular_velocity{}; - _angular_velocity_sub->update(&angular_velocity); + _angular_velocity_sub.copy(&angular_velocity); mavlink_attitude_t msg{}; @@ -1458,36 +1378,32 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _att_sub.advertised() ? MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_att_sub; - MavlinkOrbSubscription *_angular_velocity_sub; - MavlinkOrbSubscription *_status_sub; - uint64_t _att_time{0}; + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &) = delete; MavlinkStreamAttitudeQuaternion &operator = (const MavlinkStreamAttitudeQuaternion &) = delete; protected: - explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), - _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), - _angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity))), - _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { vehicle_attitude_s att; - if (_att_sub->update(&_att_time, &att)) { + if (_att_sub.update(&att)) { vehicle_angular_velocity_s angular_velocity{}; - _angular_velocity_sub->update(&angular_velocity); + _angular_velocity_sub.copy(&angular_velocity); vehicle_status_s status{}; - _status_sub->update(&status); + _status_sub.copy(&status); mavlink_attitude_quaternion_t msg{}; @@ -1557,63 +1473,52 @@ class MavlinkStreamVFRHUD : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + if (_lpos_sub.advertised() || _airspeed_validated_sub.advertised()) { + return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; } private: - MavlinkOrbSubscription *_pos_sub; - uint64_t _pos_time; - - MavlinkOrbSubscription *_armed_sub; - uint64_t _armed_time; - - MavlinkOrbSubscription *_act0_sub; - MavlinkOrbSubscription *_act1_sub; - - MavlinkOrbSubscription *_airspeed_validated_sub; - uint64_t _airspeed_time; - - MavlinkOrbSubscription *_air_data_sub; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _act0_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _act1_sub{ORB_ID(actuator_controls_1)}; + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &) = delete; MavlinkStreamVFRHUD &operator = (const MavlinkStreamVFRHUD &) = delete; protected: - explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), - _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), - _pos_time(0), - _armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))), - _armed_time(0), - _act0_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), - _act1_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))), - _airspeed_validated_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed_validated))), - _airspeed_time(0), - _air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))) + explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - vehicle_local_position_s pos = {}; - actuator_armed_s armed = {}; - airspeed_validated_s airspeed_validated = {}; + if (_lpos_sub.updated() || _airspeed_validated_sub.updated()) { - bool updated = false; - updated |= _pos_sub->update(&_pos_time, &pos); - updated |= _armed_sub->update(&_armed_time, &armed); - updated |= _airspeed_validated_sub->update(&_airspeed_time, &airspeed_validated); + vehicle_local_position_s lpos{}; + _lpos_sub.copy(&lpos); + + actuator_armed_s armed{}; + _armed_sub.copy(&armed); + + airspeed_validated_s airspeed_validated{}; + _airspeed_validated_sub.copy(&airspeed_validated); - if (updated) { mavlink_vfr_hud_t msg{}; msg.airspeed = airspeed_validated.indicated_airspeed_m_s; - msg.groundspeed = sqrtf(pos.vx * pos.vx + pos.vy * pos.vy); - msg.heading = math::degrees(wrap_2pi(pos.yaw)); + msg.groundspeed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy); + msg.heading = math::degrees(wrap_2pi(lpos.yaw)); if (armed.armed) { - actuator_controls_s act0 = {}; - actuator_controls_s act1 = {}; - _act0_sub->update(&act0); - _act1_sub->update(&act1); + actuator_controls_s act0{}; + actuator_controls_s act1{}; + _act0_sub.copy(&act0); + _act1_sub.copy(&act1); // VFR_HUD throttle should only be used for operator feedback. // VTOLs switch between actuator_controls_0 and actuator_controls_1. During transition there isn't a @@ -1628,13 +1533,13 @@ class MavlinkStreamVFRHUD : public MavlinkStream msg.throttle = 0.0f; } - if (pos.z_valid && pos.z_global) { + if (lpos.z_valid && lpos.z_global) { /* use local position estimate */ - msg.alt = -pos.z + pos.ref_alt; + msg.alt = -lpos.z + lpos.ref_alt; } else { - vehicle_air_data_s air_data = {}; - _air_data_sub->update(&air_data); + vehicle_air_data_s air_data{}; + _air_data_sub.copy(&air_data); /* fall back to baro altitude */ if (air_data.timestamp > 0) { @@ -1642,8 +1547,8 @@ class MavlinkStreamVFRHUD : public MavlinkStream } } - if (pos.v_z_valid) { - msg.climb = -pos.vz; + if (lpos.v_z_valid) { + msg.climb = -lpos.vz; } mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg); @@ -1686,29 +1591,26 @@ class MavlinkStreamGPSRawInt : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _gps_sub.advertised() ? MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_gps_sub; - uint64_t _gps_time; + uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)}; /* do not allow top copying this class */ MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &) = delete; MavlinkStreamGPSRawInt &operator = (const MavlinkStreamGPSRawInt &) = delete; protected: - explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), - _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), - _gps_time(0) + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { vehicle_gps_position_s gps; - if (_gps_sub->update(&_gps_time, &gps)) { - mavlink_gps_raw_int_t msg = {}; + if (_gps_sub.update(&gps)) { + mavlink_gps_raw_int_t msg{}; msg.time_usec = gps.timestamp; msg.fix_type = gps.fix_type; @@ -1765,28 +1667,25 @@ class MavlinkStreamGPS2Raw : public MavlinkStream unsigned get_size() override { - return (_gps_time > 0) ? (MAVLINK_MSG_ID_GPS2_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _gps2_sub.advertised() ? (MAVLINK_MSG_ID_GPS2_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_gps_sub; - uint64_t _gps_time; + uORB::Subscription _gps2_sub{ORB_ID(vehicle_gps_position), 1}; /* do not allow top copying this class */ MavlinkStreamGPS2Raw(MavlinkStreamGPS2Raw &) = delete; MavlinkStreamGPS2Raw &operator = (const MavlinkStreamGPS2Raw &) = delete; protected: - explicit MavlinkStreamGPS2Raw(Mavlink *mavlink) : MavlinkStream(mavlink), - _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), 1)), - _gps_time(0) + explicit MavlinkStreamGPS2Raw(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { vehicle_gps_position_s gps; - if (_gps_sub->update(&_gps_time, &gps)) { + if (_gps2_sub.update(&gps)) { mavlink_gps2_raw_t msg = {}; msg.time_usec = gps.timestamp; @@ -1855,11 +1754,10 @@ class MavlinkStreamSystemTime : public MavlinkStream bool send(const hrt_abstime t) override { - mavlink_system_time_t msg = {}; timespec tv; - px4_clock_gettime(CLOCK_REALTIME, &tv); + mavlink_system_time_t msg{}; msg.time_boot_ms = hrt_absolute_time() / 1000; msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; @@ -1918,7 +1816,7 @@ class MavlinkStreamTimesync : public MavlinkStream bool send(const hrt_abstime t) override { - mavlink_timesync_t msg = {}; + mavlink_timesync_t msg{}; msg.tc1 = 0; msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds @@ -1964,33 +1862,32 @@ class MavlinkStreamADSBVehicle : public MavlinkStream unsigned get_size() override { - return (_pos_time > 0) ? MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _pos_sub.advertised() ? MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_pos_sub; - uint64_t _pos_time; + uORB::Subscription _pos_sub{ORB_ID(transponder_report)}; /* do not allow top copying this class */ MavlinkStreamADSBVehicle(MavlinkStreamADSBVehicle &) = delete; MavlinkStreamADSBVehicle &operator = (const MavlinkStreamADSBVehicle &) = delete; protected: - explicit MavlinkStreamADSBVehicle(Mavlink *mavlink) : MavlinkStream(mavlink), - _pos_sub(_mavlink->add_orb_subscription(ORB_ID(transponder_report))), - _pos_time(0) + explicit MavlinkStreamADSBVehicle(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct transponder_report_s pos; + transponder_report_s pos; bool sent = false; - while (_pos_sub->update(&_pos_time, &pos)) { - mavlink_adsb_vehicle_t msg = {}; + while (_pos_sub.update(&pos)) { - if (!(pos.flags & transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE)) { continue; } + if (!(pos.flags & transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE)) { + continue; + } + mavlink_adsb_vehicle_t msg{}; msg.ICAO_address = pos.icao_address; msg.lat = pos.lat * 1e7; msg.lon = pos.lon * 1e7; @@ -2061,155 +1958,144 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream unsigned get_size() override { - return _local_pos_time > 0 ? MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _global_pos_sub.advertised() ? MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_local_pos_sub; - uint64_t _local_pos_time = 0; - vehicle_local_position_s _local_position = {}; - - MavlinkOrbSubscription *_global_pos_sub; - uint64_t _global_pos_time = 0; - vehicle_global_position_s _global_position = {}; - - MavlinkOrbSubscription *_position_setpoint_triplet_sub; - uint64_t _setpoint_triplet_time = 0; - position_setpoint_triplet_s _setpoint_triplet = {}; - - MavlinkOrbSubscription *_vehicle_status_sub; - uint64_t _vehicle_status_time = 0; - vehicle_status_s _vehicle_status = {}; - - MavlinkOrbSubscription *_land_detected_sub; - uint64_t _land_detected_time = 0; - vehicle_land_detected_s _land_detected = {}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; /* do not allow top copying this class */ MavlinkStreamUTMGlobalPosition(MavlinkStreamUTMGlobalPosition &) = delete; MavlinkStreamUTMGlobalPosition &operator = (const MavlinkStreamUTMGlobalPosition &) = delete; protected: - explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink), - _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), - _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), - _position_setpoint_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), - _vehicle_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), - _land_detected_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))) + explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { + vehicle_global_position_s global_pos; - // Check if new uORB messages are available otherwise use the last received - _local_pos_sub->update(&_local_pos_time, &_local_position); - _global_pos_sub->update(&_global_pos_time, &_global_position); - _position_setpoint_triplet_sub->update(&_setpoint_triplet_time, &_setpoint_triplet); - _vehicle_status_sub->update(&_vehicle_status_time, &_vehicle_status); - _land_detected_sub->update(&_land_detected_time, &_land_detected); + if (_global_pos_sub.update(&global_pos)) { + mavlink_utm_global_position_t msg{}; - mavlink_utm_global_position_t msg = {}; + // Compute Unix epoch and set time field + timespec tv; + px4_clock_gettime(CLOCK_REALTIME, &tv); + uint64_t unix_epoch = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; - // Compute Unix epoch and set time field - timespec tv; - px4_clock_gettime(CLOCK_REALTIME, &tv); - uint64_t unix_epoch = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; - - // If the time is before 2001-01-01, it's probably the default 2000 - if (unix_epoch > 978307200000000) { - msg.time = unix_epoch; - msg.flags |= UTM_DATA_AVAIL_FLAGS_TIME_VALID; - } + // If the time is before 2001-01-01, it's probably the default 2000 + if (unix_epoch > 978307200000000) { + msg.time = unix_epoch; + msg.flags |= UTM_DATA_AVAIL_FLAGS_TIME_VALID; + } #ifndef BOARD_HAS_NO_UUID - px4_guid_t px4_guid; - board_get_px4_guid(px4_guid); - static_assert(sizeof(px4_guid_t) == sizeof(msg.uas_id), "GUID byte length mismatch"); - memcpy(&msg.uas_id, &px4_guid, sizeof(msg.uas_id)); - msg.flags |= UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE; + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + static_assert(sizeof(px4_guid_t) == sizeof(msg.uas_id), "GUID byte length mismatch"); + memcpy(&msg.uas_id, &px4_guid, sizeof(msg.uas_id)); + msg.flags |= UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE; #else - // TODO Fill ID with something reasonable - memset(&msg.uas_id[0], 0, sizeof(msg.uas_id)); + // TODO Fill ID with something reasonable + memset(&msg.uas_id[0], 0, sizeof(msg.uas_id)); #endif /* BOARD_HAS_NO_UUID */ - // Handle global position - if (_global_pos_time > 0) { - msg.lat = _global_position.lat * 1e7; - msg.lon = _global_position.lon * 1e7; - msg.alt = _global_position.alt_ellipsoid * 1000.0f; + // Handle global position + msg.lat = global_pos.lat * 1e7; + msg.lon = global_pos.lon * 1e7; + msg.alt = global_pos.alt_ellipsoid * 1000.0f; - msg.h_acc = _global_position.eph * 1000.0f; - msg.v_acc = _global_position.epv * 1000.0f; + msg.h_acc = global_pos.eph * 1000.0f; + msg.v_acc = global_pos.epv * 1000.0f; msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; - } - // Handle local position - if (_local_pos_time > 0) { - float evh = 0.0f; - float evv = 0.0f; + // Handle local position + vehicle_local_position_s local_pos; - if (_local_position.v_xy_valid) { - msg.vx = _local_position.vx * 100.0f; - msg.vy = _local_position.vy * 100.0f; - evh = _local_position.evh; - msg.flags |= UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE; - } + if (_local_pos_sub.copy(&local_pos)) { + float evh = 0.0f; + float evv = 0.0f; - if (_local_position.v_z_valid) { - msg.vz = _local_position.vz * 100.0f; - evv = _local_position.evv; - msg.flags |= UTM_DATA_AVAIL_FLAGS_VERTICAL_VELO_AVAILABLE; - } + if (local_pos.v_xy_valid) { + msg.vx = local_pos.vx * 100.0f; + msg.vy = local_pos.vy * 100.0f; + evh = local_pos.evh; + msg.flags |= UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE; + } + + if (local_pos.v_z_valid) { + msg.vz = local_pos.vz * 100.0f; + evv = local_pos.evv; + msg.flags |= UTM_DATA_AVAIL_FLAGS_VERTICAL_VELO_AVAILABLE; + } - msg.vel_acc = sqrtf(evh * evh + evv * evv) * 100.0f; + msg.vel_acc = sqrtf(evh * evh + evv * evv) * 100.0f; + + if (local_pos.dist_bottom_valid) { + msg.relative_alt = local_pos.dist_bottom * 1000.0f; + msg.flags |= UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE; + } + } - if (_local_position.dist_bottom_valid) { - msg.relative_alt = _local_position.dist_bottom * 1000.0f; - msg.flags |= UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE; + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + bool vehicle_in_auto_mode = vehicle_status.timestamp > 0 + && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + + // Handle next waypoint if it is valid + position_setpoint_triplet_s position_setpoint_triplet; + + if (vehicle_in_auto_mode && _position_setpoint_triplet_sub.copy(&position_setpoint_triplet)) { + if (position_setpoint_triplet.current.valid) { + msg.next_lat = position_setpoint_triplet.current.lat * 1e7; + msg.next_lon = position_setpoint_triplet.current.lon * 1e7; + // HACK We assume that the offset between AMSL and WGS84 is constant between the current + // vehicle position and the the target waypoint. + msg.next_alt = (position_setpoint_triplet.current.alt + (global_pos.alt_ellipsoid - global_pos.alt)) * 1000.0f; + msg.flags |= UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE; + } } - } - bool vehicle_in_auto_mode = _vehicle_status_time > 0 - && (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET - || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND - || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL - || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND - || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION - || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER - || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF - || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); - - // Handle next waypoint if it is valid - if (vehicle_in_auto_mode && _setpoint_triplet_time > 0 && _setpoint_triplet.current.valid) { - msg.next_lat = _setpoint_triplet.current.lat * 1e7; - msg.next_lon = _setpoint_triplet.current.lon * 1e7; - // HACK We assume that the offset between AMSL and WGS84 is constant between the current - // vehicle position and the the target waypoint. - msg.next_alt = (_setpoint_triplet.current.alt + (_global_position.alt_ellipsoid - _global_position.alt)) * 1000.0f; - msg.flags |= UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE; - } + // Handle flight state + vehicle_land_detected_s land_detected{}; + _land_detected_sub.copy(&land_detected); - // Handle flight state - if (_vehicle_status_time > 0 && _land_detected_time > 0 - && _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - if (_land_detected.landed) { - msg.flight_state |= UTM_FLIGHT_STATE_GROUND; + if (vehicle_status.timestamp > 0 && land_detected.timestamp > 0 + && vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (land_detected.landed) { + msg.flight_state |= UTM_FLIGHT_STATE_GROUND; + + } else { + msg.flight_state |= UTM_FLIGHT_STATE_AIRBORNE; + } } else { - msg.flight_state |= UTM_FLIGHT_STATE_AIRBORNE; + msg.flight_state |= UTM_FLIGHT_STATE_UNKNOWN; } - } else { - msg.flight_state |= UTM_FLIGHT_STATE_UNKNOWN; - } + msg.update_rate = 0; // Data driven mode - msg.update_rate = 0; // Data driven mode + mavlink_msg_utm_global_position_send_struct(_mavlink->get_channel(), &msg); - mavlink_msg_utm_global_position_send_struct(_mavlink->get_channel(), &msg); + return true; + } - return true; + return false; } }; @@ -2243,29 +2129,26 @@ class MavlinkStreamCollision : public MavlinkStream unsigned get_size() override { - return (_collision_time > 0) ? MAVLINK_MSG_ID_COLLISION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _collision_sub.advertised() ? MAVLINK_MSG_ID_COLLISION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_collision_sub; - uint64_t _collision_time; + uORB::Subscription _collision_sub{ORB_ID(collision_report)}; /* do not allow top copying this class */ MavlinkStreamCollision(MavlinkStreamCollision &) = delete; MavlinkStreamCollision &operator = (const MavlinkStreamCollision &) = delete; protected: - explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink), - _collision_sub(_mavlink->add_orb_subscription(ORB_ID(collision_report))), - _collision_time(0) + explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct collision_report_s report; + collision_report_s report; bool sent = false; - while (_collision_sub->update(&_collision_time, &report)) { + while (_collision_sub.update(&report)) { mavlink_collision_t msg = {}; msg.src = report.src; @@ -2319,29 +2202,26 @@ class MavlinkStreamCameraTrigger : public MavlinkStream unsigned get_size() override { - return (_trigger_time > 0) ? MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _trigger_sub.advertised() ? MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_trigger_sub; - uint64_t _trigger_time; + uORB::Subscription _trigger_sub{ORB_ID(camera_trigger)}; /* do not allow top copying this class */ MavlinkStreamCameraTrigger(MavlinkStreamCameraTrigger &) = delete; MavlinkStreamCameraTrigger &operator = (const MavlinkStreamCameraTrigger &) = delete; protected: - explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink), - _trigger_sub(_mavlink->add_orb_subscription(ORB_ID(camera_trigger))), - _trigger_time(0) + explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct camera_trigger_s trigger; + camera_trigger_s trigger; - if (_trigger_sub->update(&_trigger_time, &trigger)) { - mavlink_camera_trigger_t msg = {}; + if (_trigger_sub.update(&trigger)) { + mavlink_camera_trigger_t msg{}; msg.time_usec = trigger.timestamp; msg.seq = trigger.seq; @@ -2351,7 +2231,7 @@ class MavlinkStreamCameraTrigger : public MavlinkStream mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg); - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; vcmd.timestamp = hrt_absolute_time(); vcmd.param1 = 0.0f; // all cameras vcmd.param2 = 0.0f; // duration 0 because only taking one picture @@ -2368,7 +2248,7 @@ class MavlinkStreamCameraTrigger : public MavlinkStream // TODO: move this camera_trigger and publish as a vehicle_command /* send MAV_CMD_DO_DIGICAM_CONTROL*/ - mavlink_command_long_t digicam_ctrl_cmd = {}; + mavlink_command_long_t digicam_ctrl_cmd{}; digicam_ctrl_cmd.target_system = 0; // 0 for broadcast digicam_ctrl_cmd.target_component = MAV_COMP_ID_CAMERA; @@ -2427,30 +2307,27 @@ class MavlinkStreamCameraImageCaptured : public MavlinkStream unsigned get_size() override { - return (_capture_time > 0) ? MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _capture_sub.advertised() ? MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_capture_sub; - uint64_t _capture_time; + uORB::Subscription _capture_sub{ORB_ID(camera_capture)}; /* do not allow top copying this class */ MavlinkStreamCameraImageCaptured(MavlinkStreamCameraImageCaptured &) = delete; MavlinkStreamCameraImageCaptured &operator = (const MavlinkStreamCameraImageCaptured &) = delete; protected: - explicit MavlinkStreamCameraImageCaptured(Mavlink *mavlink) : MavlinkStream(mavlink), - _capture_sub(_mavlink->add_orb_subscription(ORB_ID(camera_capture))), - _capture_time(0) + explicit MavlinkStreamCameraImageCaptured(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct camera_capture_s capture; + camera_capture_s capture; - if (_capture_sub->update(&_capture_time, &capture)) { + if (_capture_sub.update(&capture)) { - mavlink_camera_image_captured_t msg; + mavlink_camera_image_captured_t msg{}; msg.time_boot_ms = capture.timestamp / 1000; msg.time_utc = capture.timestamp_utc; @@ -2506,59 +2383,47 @@ class MavlinkStreamGlobalPositionInt : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _gpos_sub.advertised() ? MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_gpos_sub; - uint64_t _gpos_time; - - MavlinkOrbSubscription *_lpos_sub; - uint64_t _lpos_time; - - MavlinkOrbSubscription *_home_sub; - MavlinkOrbSubscription *_air_data_sub; + uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _home_sub{ORB_ID(home_position)}; + uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; /* do not allow top copying this class */ MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &) = delete; MavlinkStreamGlobalPositionInt &operator = (const MavlinkStreamGlobalPositionInt &) = delete; protected: - explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), - _gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), - _gpos_time(0), - _lpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), - _lpos_time(0), - _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), - _air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))) + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - vehicle_global_position_s gpos = {}; - vehicle_local_position_s lpos = {}; + vehicle_global_position_s gpos; + vehicle_local_position_s lpos; - bool gpos_updated = _gpos_sub->update(&_gpos_time, &gpos); - bool lpos_updated = _lpos_sub->update(&_lpos_time, &lpos); + if (_gpos_sub.update(&gpos) && _lpos_sub.update(&lpos)) { - if (gpos_updated && lpos_updated) { - mavlink_global_position_int_t msg = {}; + mavlink_global_position_int_t msg{}; if (lpos.z_valid && lpos.z_global) { msg.alt = (-lpos.z + lpos.ref_alt) * 1000.0f; } else { // fall back to baro altitude - vehicle_air_data_s air_data = {}; - _air_data_sub->update(&air_data); + vehicle_air_data_s air_data{}; + _air_data_sub.copy(&air_data); if (air_data.timestamp > 0) { msg.alt = air_data.baro_alt_meter * 1000.0f; } } - home_position_s home = {}; - _home_sub->update(&home); + home_position_s home{}; + _home_sub.copy(&home); if ((home.timestamp > 0) && home.valid_alt) { if (lpos.z_valid) { @@ -2623,26 +2488,24 @@ class MavlinkStreamOdometry : public MavlinkStream unsigned get_size() override { - return (_odom_time > 0) ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + if (_mavlink->odometry_loopback_enabled()) { + return _vodom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + + } else { + return _odom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } } private: - MavlinkOrbSubscription *_odom_sub; - uint64_t _odom_time; - - MavlinkOrbSubscription *_vodom_sub; - uint64_t _vodom_time; + uORB::Subscription _odom_sub{ORB_ID(vehicle_odometry)}; + uORB::Subscription _vodom_sub{ORB_ID(vehicle_visual_odometry)}; /* do not allow top copying this class */ MavlinkStreamOdometry(MavlinkStreamOdometry &) = delete; MavlinkStreamOdometry &operator = (const MavlinkStreamOdometry &) = delete; protected: - explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink), - _odom_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_odometry))), - _odom_time(0), - _vodom_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_visual_odometry))), - _vodom_time(0) + explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override @@ -2651,15 +2514,15 @@ class MavlinkStreamOdometry : public MavlinkStream // check if it is to send visual odometry loopback or not bool odom_updated = false; - mavlink_odometry_t msg = {}; + mavlink_odometry_t msg{}; if (_mavlink->odometry_loopback_enabled()) { - odom_updated = _vodom_sub->update(&_vodom_time, &odom); + odom_updated = _vodom_sub.update(&odom); // frame matches the external vision system msg.frame_id = MAV_FRAME_VISION_NED; } else { - odom_updated = _odom_sub->update(&_odom_time, &odom); + odom_updated = _odom_sub.update(&odom); // frame matches the PX4 local NED frame msg.frame_id = MAV_FRAME_ESTIM_NED; } @@ -2758,37 +2621,34 @@ class MavlinkStreamLocalPositionNED : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _lpos_sub.advertised() ? MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_pos_sub; - uint64_t _pos_time; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; /* do not allow top copying this class */ MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &) = delete; MavlinkStreamLocalPositionNED &operator = (const MavlinkStreamLocalPositionNED &) = delete; protected: - explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), - _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), - _pos_time(0) + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - vehicle_local_position_s pos; + vehicle_local_position_s lpos; - if (_pos_sub->update(&_pos_time, &pos)) { - mavlink_local_position_ned_t msg = {}; + if (_lpos_sub.update(&lpos)) { + mavlink_local_position_ned_t msg{}; - msg.time_boot_ms = pos.timestamp / 1000; - msg.x = pos.x; - msg.y = pos.y; - msg.z = pos.z; - msg.vx = pos.vx; - msg.vy = pos.vy; - msg.vz = pos.vz; + msg.time_boot_ms = lpos.timestamp / 1000; + msg.x = lpos.x; + msg.y = lpos.y; + msg.z = lpos.z; + msg.vx = lpos.vx; + msg.vy = lpos.vy; + msg.vz = lpos.vz; mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg); @@ -2829,37 +2689,30 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_VIBRATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _est_sub.advertised() ? MAVLINK_MSG_ID_VIBRATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_est_sub; - uint64_t _est_time; - - MavlinkOrbSubscription *_sensor_accel_status_0_sub; - MavlinkOrbSubscription *_sensor_accel_status_1_sub; - MavlinkOrbSubscription *_sensor_accel_status_2_sub; + uORB::Subscription _est_sub{ORB_ID(estimator_status)}; + uORB::Subscription _sensor_accel_status_0_sub{ORB_ID(sensor_accel_status), 0}; + uORB::Subscription _sensor_accel_status_1_sub{ORB_ID(sensor_accel_status), 1}; + uORB::Subscription _sensor_accel_status_2_sub{ORB_ID(sensor_accel_status), 2}; /* do not allow top copying this class */ MavlinkStreamEstimatorStatus(MavlinkStreamEstimatorStatus &) = delete; MavlinkStreamEstimatorStatus &operator = (const MavlinkStreamEstimatorStatus &) = delete; protected: - explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink), - _est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))), - _est_time(0), - _sensor_accel_status_0_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_status), 0)), - _sensor_accel_status_1_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_status), 1)), - _sensor_accel_status_2_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_status), 2)) + explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { estimator_status_s est; - if (_est_sub->update(&_est_time, &est)) { + if (_est_sub.update(&est)) { // ESTIMATOR_STATUS - mavlink_estimator_status_t est_msg = {}; + mavlink_estimator_status_t est_msg{}; est_msg.time_usec = est.timestamp; est_msg.vel_ratio = est.vel_test_ratio; est_msg.pos_horiz_ratio = est.pos_test_ratio; @@ -2881,19 +2734,19 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream sensor_accel_status_s acc_status_0; - if (_sensor_accel_status_0_sub->update(&acc_status_0)) { + if (_sensor_accel_status_0_sub.copy(&acc_status_0)) { msg.clipping_0 = acc_status_0.clipping[0] + acc_status_0.clipping[1] + acc_status_0.clipping[2]; } sensor_accel_status_s acc_status_1; - if (_sensor_accel_status_1_sub->update(&acc_status_1)) { + if (_sensor_accel_status_1_sub.copy(&acc_status_1)) { msg.clipping_1 = acc_status_1.clipping[0] + acc_status_1.clipping[1] + acc_status_1.clipping[2]; } sensor_accel_status_s acc_status_2; - if (_sensor_accel_status_2_sub->update(&acc_status_2)) { + if (_sensor_accel_status_2_sub.copy(&acc_status_2)) { msg.clipping_2 = acc_status_2.clipping[0] + acc_status_2.clipping[1] + acc_status_2.clipping[2]; } @@ -2936,29 +2789,26 @@ class MavlinkStreamAttPosMocap : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _mocap_sub.advertised() ? MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_mocap_sub; - uint64_t _mocap_time; + uORB::Subscription _mocap_sub{ORB_ID(vehicle_mocap_odometry)}; /* do not allow top copying this class */ MavlinkStreamAttPosMocap(MavlinkStreamAttPosMocap &) = delete; MavlinkStreamAttPosMocap &operator = (const MavlinkStreamAttPosMocap &) = delete; protected: - explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink), - _mocap_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_mocap_odometry))), - _mocap_time(0) + explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { vehicle_odometry_s mocap; - if (_mocap_sub->update(&_mocap_time, &mocap)) { - mavlink_att_pos_mocap_t msg = {}; + if (_mocap_sub.update(&mocap)) { + mavlink_att_pos_mocap_t msg{}; msg.time_usec = mocap.timestamp; msg.q[0] = mocap.q[0]; @@ -3009,56 +2859,53 @@ class MavlinkStreamHomePosition : public MavlinkStream unsigned get_size() override { - return _home_sub->is_published() ? (MAVLINK_MSG_ID_HOME_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _home_sub.advertised() ? (MAVLINK_MSG_ID_HOME_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_home_sub; + uORB::Subscription _home_sub{ORB_ID(home_position)}; /* do not allow top copying this class */ MavlinkStreamHomePosition(MavlinkStreamHomePosition &) = delete; MavlinkStreamHomePosition &operator = (const MavlinkStreamHomePosition &) = delete; protected: - explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink), - _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) + explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { /* we're sending the GPS home periodically to ensure the * the GCS does pick it up at one point */ - if (_home_sub->is_published()) { - home_position_s home; + home_position_s home; - if (_home_sub->update(&home)) { - if (home.valid_hpos) { - mavlink_home_position_t msg; + if (_home_sub.advertised() && _home_sub.copy(&home)) { + if (home.valid_hpos) { + mavlink_home_position_t msg{}; - msg.latitude = home.lat * 1e7; - msg.longitude = home.lon * 1e7; - msg.altitude = home.alt * 1e3f; + msg.latitude = home.lat * 1e7; + msg.longitude = home.lon * 1e7; + msg.altitude = home.alt * 1e3f; - msg.x = home.x; - msg.y = home.y; - msg.z = home.z; + msg.x = home.x; + msg.y = home.y; + msg.z = home.z; - matrix::Quatf q(matrix::Eulerf(0.0f, 0.0f, home.yaw)); - msg.q[0] = q(0); - msg.q[1] = q(1); - msg.q[2] = q(2); - msg.q[3] = q(3); + matrix::Quatf q(matrix::Eulerf(0.0f, 0.0f, home.yaw)); + msg.q[0] = q(0); + msg.q[1] = q(1); + msg.q[2] = q(2); + msg.q[3] = q(3); - msg.approach_x = 0.0f; - msg.approach_y = 0.0f; - msg.approach_z = 0.0f; + msg.approach_x = 0.0f; + msg.approach_y = 0.0f; + msg.approach_z = 0.0f; - msg.time_usec = home.timestamp; + msg.time_usec = home.timestamp; - mavlink_msg_home_position_send_struct(_mavlink->get_channel(), &msg); + mavlink_msg_home_position_send_struct(_mavlink->get_channel(), &msg); - return true; - } + return true; } } @@ -3104,29 +2951,26 @@ class MavlinkStreamServoOutputRaw : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _act_sub.advertised() ? MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_act_sub; - uint64_t _act_time; + uORB::Subscription _act_sub{ORB_ID(actuator_outputs), N}; /* do not allow top copying this class */ MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &) = delete; MavlinkStreamServoOutputRaw &operator = (const MavlinkStreamServoOutputRaw &) = delete; protected: - explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), - _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N)), - _act_time(0) + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { actuator_outputs_s act; - if (_act_sub->update(&_act_time, &act)) { - mavlink_servo_output_raw_t msg = {}; + if (_act_sub.update(&act)) { + mavlink_servo_output_raw_t msg{}; static_assert(sizeof(act.output) / sizeof(act.output[0]) >= 16, "mavlink message requires at least 16 outputs"); @@ -3201,48 +3045,51 @@ class MavlinkStreamActuatorControlTarget : public MavlinkStream unsigned get_size() override { - return _act_ctrl_sub->is_published() ? (MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return (_act_ctrl_sub + && _act_ctrl_sub->advertised()) ? (MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_act_ctrl_sub; - uint64_t _act_ctrl_time; + uORB::Subscription *_act_ctrl_sub{nullptr}; /* do not allow top copying this class */ MavlinkStreamActuatorControlTarget(MavlinkStreamActuatorControlTarget &) = delete; MavlinkStreamActuatorControlTarget &operator = (const MavlinkStreamActuatorControlTarget &) = delete; protected: - explicit MavlinkStreamActuatorControlTarget(Mavlink *mavlink) : MavlinkStream(mavlink), - _act_ctrl_sub(nullptr), - _act_ctrl_time(0) + explicit MavlinkStreamActuatorControlTarget(Mavlink *mavlink) : MavlinkStream(mavlink) { // XXX this can be removed once the multiplatform system remaps topics switch (N) { case 0: - _act_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_0)}; break; case 1: - _act_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_1)); + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_1)}; break; case 2: - _act_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_2)); + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_2)}; break; case 3: - _act_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_3)); + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_3)}; break; } } + ~MavlinkStreamActuatorControlTarget() override + { + delete _act_ctrl_sub; + } + bool send(const hrt_abstime t) override { actuator_controls_s act_ctrl; - if (_act_ctrl_sub->update(&_act_ctrl_time, &act_ctrl)) { - mavlink_actuator_control_target_t msg = {}; + if (_act_ctrl_sub && _act_ctrl_sub->update(&act_ctrl)) { + mavlink_actuator_control_target_t msg{}; msg.time_usec = act_ctrl.timestamp; msg.group_mlx = N; @@ -3290,33 +3137,28 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _act_sub.advertised() ? MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_status_sub; - - MavlinkOrbSubscription *_act_sub; - uint64_t _act_time; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _act_sub{ORB_ID(actuator_outputs)}; /* do not allow top copying this class */ MavlinkStreamHILActuatorControls(MavlinkStreamHILActuatorControls &) = delete; MavlinkStreamHILActuatorControls &operator = (const MavlinkStreamHILActuatorControls &) = delete; protected: - explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink), - _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), - _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))), - _act_time(0) + explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { actuator_outputs_s act; - if (_act_sub->update(&_act_time, &act)) { - vehicle_status_s status = {}; - _status_sub->update(&status); + if (_act_sub.update(&act)) { + vehicle_status_s status{}; + _status_sub.copy(&status); if ((status.timestamp > 0) && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { /* translate the current system state to mavlink state and mode */ @@ -3451,39 +3293,37 @@ class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _pos_sp_triplet_sub.advertised() ? MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_control_mode_sub; - MavlinkOrbSubscription *_lpos_sp_sub; - MavlinkOrbSubscription *_pos_sp_triplet_sub; + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _lpos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; /* do not allow top copying this class */ MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &) = delete; MavlinkStreamPositionTargetGlobalInt &operator = (const MavlinkStreamPositionTargetGlobalInt &) = delete; protected: - explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), - _control_mode_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_control_mode))), - _lpos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))), - _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - vehicle_control_mode_s control_mode = {}; - _control_mode_sub->update(&control_mode); + vehicle_control_mode_s control_mode{}; + _control_mode_sub.copy(&control_mode); if (control_mode.flag_control_position_enabled) { - position_setpoint_triplet_s pos_sp_triplet = {}; - _pos_sp_triplet_sub->update(&pos_sp_triplet); + position_setpoint_triplet_s pos_sp_triplet{}; + _pos_sp_triplet_sub.copy(&pos_sp_triplet); - if (pos_sp_triplet.timestamp > 0 && pos_sp_triplet.current.valid - && PX4_ISFINITE(pos_sp_triplet.current.lat) && PX4_ISFINITE(pos_sp_triplet.current.lon)) { + if (pos_sp_triplet.timestamp > 0 && pos_sp_triplet.current.valid && PX4_ISFINITE(pos_sp_triplet.current.lat) + && PX4_ISFINITE(pos_sp_triplet.current.lon)) { - mavlink_position_target_global_int_t msg = {}; + mavlink_position_target_global_int_t msg{}; msg.time_boot_ms = hrt_absolute_time() / 1000; msg.coordinate_frame = MAV_FRAME_GLOBAL_INT; @@ -3493,7 +3333,7 @@ class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream vehicle_local_position_setpoint_s lpos_sp; - if (_lpos_sp_sub->update(&lpos_sp)) { + if (_lpos_sp_sub.copy(&lpos_sp)) { // velocity msg.vx = lpos_sp.vx; msg.vy = lpos_sp.vy; @@ -3550,29 +3390,26 @@ class MavlinkStreamLocalPositionSetpoint : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _pos_sp_sub.advertised() ? MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_pos_sp_sub; - uint64_t _pos_sp_time; + uORB::Subscription _pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; /* do not allow top copying this class */ MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &) = delete; MavlinkStreamLocalPositionSetpoint &operator = (const MavlinkStreamLocalPositionSetpoint &) = delete; protected: - explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), - _pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))), - _pos_sp_time(0) + explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { vehicle_local_position_setpoint_s pos_sp; - if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_position_target_local_ned_t msg = {}; + if (_pos_sp_sub.update(&pos_sp)) { + mavlink_position_target_local_ned_t msg{}; msg.time_boot_ms = pos_sp.timestamp / 1000; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; @@ -3628,40 +3465,35 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _att_sp_sub.advertised() ? MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_att_sp_sub; - MavlinkOrbSubscription *_att_rates_sp_sub; - - uint64_t _att_sp_time; + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _att_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /* do not allow top copying this class */ MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &) = delete; MavlinkStreamAttitudeTarget &operator = (const MavlinkStreamAttitudeTarget &) = delete; protected: - explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), - _att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), - _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), - _att_sp_time(0) + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { vehicle_attitude_setpoint_s att_sp; - if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { + if (_att_sp_sub.update(&att_sp)) { - vehicle_rates_setpoint_s att_rates_sp = {}; - _att_rates_sp_sub->update(&att_rates_sp); - - mavlink_attitude_target_t msg = {}; + mavlink_attitude_target_t msg{}; msg.time_boot_ms = att_sp.timestamp / 1000; matrix::Quatf(att_sp.q_d).copyTo(msg.q); + vehicle_rates_setpoint_s att_rates_sp{}; + _att_rates_sp_sub.copy(&att_rates_sp); + msg.body_roll_rate = att_rates_sp.roll; msg.body_pitch_rate = att_rates_sp.pitch; msg.body_yaw_rate = att_rates_sp.yaw; @@ -3708,31 +3540,28 @@ class MavlinkStreamRCChannels : public MavlinkStream unsigned get_size() override { - return _rc_sub->is_published() ? (MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _rc_sub.advertised() ? (MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_rc_sub; - uint64_t _rc_time; + uORB::Subscription _rc_sub{ORB_ID(input_rc)}; /* do not allow top copying this class */ MavlinkStreamRCChannels(MavlinkStreamRCChannels &) = delete; MavlinkStreamRCChannels &operator = (const MavlinkStreamRCChannels &) = delete; protected: - explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink), - _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))), - _rc_time(0) + explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { input_rc_s rc; - if (_rc_sub->update(&_rc_time, &rc)) { + if (_rc_sub.update(&rc)) { /* send RC channel data and RSSI */ - mavlink_rc_channels_t msg = {}; + mavlink_rc_channels_t msg{}; msg.time_boot_ms = rc.timestamp / 1000; msg.chancount = rc.channel_count; @@ -3796,29 +3625,26 @@ class MavlinkStreamManualControl : public MavlinkStream unsigned get_size() override { - return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _manual_sub.advertised() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_manual_sub; - uint64_t _manual_time; + uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)}; /* do not allow top copying this class */ MavlinkStreamManualControl(MavlinkStreamManualControl &) = delete; MavlinkStreamManualControl &operator = (const MavlinkStreamManualControl &) = delete; protected: - explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), - _manual_sub(_mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint))), - _manual_time(0) + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { manual_control_setpoint_s manual; - if (_manual_sub->update(&_manual_time, &manual)) { - mavlink_manual_control_t msg = {}; + if (_manual_sub.update(&manual)) { + mavlink_manual_control_t msg{}; msg.target = mavlink_system.sysid; msg.x = manual.x * 1000; @@ -3873,31 +3699,30 @@ class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream unsigned get_size() override { - return _traj_wp_avoidance_sub->is_published() ? (MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN + - MAVLINK_NUM_NON_PAYLOAD_BYTES) - : 0; + if (_traj_wp_avoidance_sub.advertised()) { + return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; } private: - MavlinkOrbSubscription *_traj_wp_avoidance_sub; - uint64_t _traj_wp_avoidance_time; + uORB::Subscription _traj_wp_avoidance_sub{ORB_ID(vehicle_trajectory_waypoint_desired)}; /* do not allow top copying this class */ MavlinkStreamTrajectoryRepresentationWaypoints(MavlinkStreamTrajectoryRepresentationWaypoints &); MavlinkStreamTrajectoryRepresentationWaypoints &operator = (const MavlinkStreamTrajectoryRepresentationWaypoints &); protected: - explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink), - _traj_wp_avoidance_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_trajectory_waypoint_desired))), - _traj_wp_avoidance_time(0) + explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct vehicle_trajectory_waypoint_s traj_wp_avoidance_desired; + vehicle_trajectory_waypoint_s traj_wp_avoidance_desired; - if (_traj_wp_avoidance_sub->update(&_traj_wp_avoidance_time, &traj_wp_avoidance_desired)) { - mavlink_trajectory_representation_waypoints_t msg = {}; + if (_traj_wp_avoidance_sub.update(&traj_wp_avoidance_desired)) { + mavlink_trajectory_representation_waypoints_t msg{}; msg.time_usec = traj_wp_avoidance_desired.timestamp; int number_valid_points = 0; @@ -3982,29 +3807,26 @@ class MavlinkStreamOpticalFlowRad : public MavlinkStream unsigned get_size() override { - return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_flow_sub; - uint64_t _flow_time; + uORB::Subscription _flow_sub{ORB_ID(optical_flow)}; /* do not allow top copying this class */ MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &) = delete; MavlinkStreamOpticalFlowRad &operator = (const MavlinkStreamOpticalFlowRad &) = delete; protected: - explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink), - _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), - _flow_time(0) + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { optical_flow_s flow; - if (_flow_sub->update(&_flow_time, &flow)) { - mavlink_optical_flow_rad_t msg = {}; + if (_flow_sub.update(&flow)) { + mavlink_optical_flow_rad_t msg{}; msg.time_usec = flow.timestamp; msg.sensor_id = flow.sensor_id; @@ -4059,29 +3881,26 @@ class MavlinkStreamNamedValueFloat : public MavlinkStream unsigned get_size() override { - return (_debug_time > 0) ? MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _debug_sub.advertised() ? MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_debug_sub; - uint64_t _debug_time; + uORB::Subscription _debug_sub{ORB_ID(debug_key_value)}; /* do not allow top copying this class */ MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &) = delete; MavlinkStreamNamedValueFloat &operator = (const MavlinkStreamNamedValueFloat &) = delete; protected: - explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), - _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_key_value))), - _debug_time(0) + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct debug_key_value_s debug; + debug_key_value_s debug; - if (_debug_sub->update(&_debug_time, &debug)) { - mavlink_named_value_float_t msg = {}; + if (_debug_sub.update(&debug)) { + mavlink_named_value_float_t msg{}; msg.time_boot_ms = debug.timestamp / 1000ULL; memcpy(msg.name, debug.key, sizeof(msg.name)); @@ -4128,30 +3947,26 @@ class MavlinkStreamDebug : public MavlinkStream unsigned get_size() override { - return (_debug_time > 0) ? MAVLINK_MSG_ID_DEBUG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _debug_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_debug_sub; - uint64_t _debug_time; + uORB::Subscription _debug_sub{ORB_ID(debug_value)}; /* do not allow top copying this class */ MavlinkStreamDebug(MavlinkStreamDebug &) = delete; MavlinkStreamDebug &operator = (const MavlinkStreamDebug &) = delete; protected: - explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink), - _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_value))), - _debug_time(0) + explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct debug_value_s debug = {}; - - if (_debug_sub->update(&_debug_time, &debug)) { - mavlink_debug_t msg = {}; + debug_value_s debug; + if (_debug_sub.update(&debug)) { + mavlink_debug_t msg{}; msg.time_boot_ms = debug.timestamp / 1000ULL; msg.ind = debug.ind; msg.value = debug.value; @@ -4195,29 +4010,26 @@ class MavlinkStreamDebugVect : public MavlinkStream unsigned get_size() override { - return (_debug_time > 0) ? MAVLINK_MSG_ID_DEBUG_VECT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _debug_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_VECT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_debug_sub; - uint64_t _debug_time; + uORB::Subscription _debug_sub{ORB_ID(debug_vect)}; /* do not allow top copying this class */ MavlinkStreamDebugVect(MavlinkStreamDebugVect &) = delete; MavlinkStreamDebugVect &operator = (const MavlinkStreamDebugVect &) = delete; protected: - explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink), - _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_vect))), - _debug_time(0) + explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct debug_vect_s debug = {}; + debug_vect_s debug; - if (_debug_sub->update(&_debug_time, &debug)) { - mavlink_debug_vect_t msg = {}; + if (_debug_sub.update(&debug)) { + mavlink_debug_vect_t msg{}; msg.time_usec = debug.timestamp; memcpy(msg.name, debug.name, sizeof(msg.name)); @@ -4266,29 +4078,26 @@ class MavlinkStreamDebugFloatArray : public MavlinkStream unsigned get_size() override { - return (_debug_time > 0) ? MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _debug_array_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_debug_array_sub; - uint64_t _debug_time; + uORB::Subscription _debug_array_sub{ORB_ID(debug_array)}; /* do not allow top copying this class */ MavlinkStreamDebugFloatArray(MavlinkStreamDebugFloatArray &); MavlinkStreamDebugFloatArray &operator = (const MavlinkStreamDebugFloatArray &); protected: - explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink), - _debug_array_sub(_mavlink->add_orb_subscription(ORB_ID(debug_array))), - _debug_time(0) + explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct debug_array_s debug = {}; + debug_array_s debug; - if (_debug_array_sub->update(&_debug_time, &debug)) { - mavlink_debug_float_array_t msg = {}; + if (_debug_array_sub.update(&debug)) { + mavlink_debug_float_array_t msg{}; msg.time_usec = debug.timestamp; msg.array_id = debug.id; @@ -4339,38 +4148,33 @@ class MavlinkStreamNavControllerOutput : public MavlinkStream unsigned get_size() override { - return (_pos_ctrl_status_sub->is_published()) ? + return (_pos_ctrl_status_sub.advertised()) ? MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_pos_ctrl_status_sub; - MavlinkOrbSubscription *_tecs_status_sub; - - uint64_t _pos_ctrl_status_timestamp{0}; - uint64_t _tecs_status_timestamp{0}; + uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)}; + uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; /* do not allow top copying this class */ MavlinkStreamNavControllerOutput(MavlinkStreamNavControllerOutput &) = delete; MavlinkStreamNavControllerOutput &operator = (const MavlinkStreamNavControllerOutput &) = delete; protected: - explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink), - _pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))), - _tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))) + explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - position_controller_status_s pos_ctrl_status = {}; - tecs_status_s tecs_status = {}; + if (_pos_ctrl_status_sub.updated()) { - bool updated = false; - updated |= _pos_ctrl_status_sub->update(&_pos_ctrl_status_timestamp, &pos_ctrl_status); - updated |= _tecs_status_sub->update(&_tecs_status_timestamp, &tecs_status); + position_controller_status_s pos_ctrl_status{}; + _pos_ctrl_status_sub.copy(&pos_ctrl_status); - if (updated) { - mavlink_nav_controller_output_t msg = {}; + tecs_status_s tecs_status{}; + _tecs_status_sub.copy(&tecs_status); + + mavlink_nav_controller_output_t msg{}; msg.nav_roll = math::degrees(pos_ctrl_status.nav_roll); msg.nav_pitch = math::degrees(pos_ctrl_status.nav_pitch); @@ -4424,23 +4228,22 @@ class MavlinkStreamCameraCapture : public MavlinkStream } private: - MavlinkOrbSubscription *_status_sub; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; /* do not allow top copying this class */ MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &) = delete; MavlinkStreamCameraCapture &operator = (const MavlinkStreamCameraCapture &) = delete; protected: - explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), - _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) + explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { vehicle_status_s status; - if (_status_sub->update(&status)) { - mavlink_command_long_t msg = {}; + if (_status_sub.update(&status)) { + mavlink_command_long_t msg{}; msg.target_system = 0; msg.target_component = MAV_COMP_ID_ALL; @@ -4492,33 +4295,29 @@ class MavlinkStreamDistanceSensor : public MavlinkStream unsigned get_size() override { - return _distance_sensor_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _distance_sensor_sub.advertised() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_distance_sensor_sub; - uint64_t _dist_sensor_time; + uORB::Subscription _distance_sensor_sub{ORB_ID(distance_sensor)}; /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &) = delete; MavlinkStreamDistanceSensor &operator = (const MavlinkStreamDistanceSensor &) = delete; protected: - explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), - _distance_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(distance_sensor))), - _dist_sensor_time(0) + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { distance_sensor_s dist_sensor; - if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) { - mavlink_distance_sensor_t msg = {}; + if (_distance_sensor_sub.update(&dist_sensor)) { + mavlink_distance_sensor_t msg{}; msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */ - switch (dist_sensor.type) { case MAV_DISTANCE_SENSOR_ULTRASOUND: msg.type = MAV_DISTANCE_SENSOR_ULTRASOUND; @@ -4587,10 +4386,10 @@ class MavlinkStreamExtendedSysState : public MavlinkStream } private: - MavlinkOrbSubscription *_status_sub; - MavlinkOrbSubscription *_landed_sub; - MavlinkOrbSubscription *_pos_sp_triplet_sub; - MavlinkOrbSubscription *_control_mode_sub; + 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)}; + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; mavlink_extended_sys_state_t _msg; /* do not allow top copying this class */ @@ -4599,10 +4398,6 @@ class MavlinkStreamExtendedSysState : public MavlinkStream protected: explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink), - _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), - _landed_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))), - _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), - _control_mode_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_control_mode))), _msg() { _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; @@ -4615,7 +4410,7 @@ class MavlinkStreamExtendedSysState : public MavlinkStream vehicle_status_s status; - if (_status_sub->update(&status)) { + if (_status_sub.copy(&status)) { updated = true; if (status.is_vtol) { @@ -4636,7 +4431,7 @@ class MavlinkStreamExtendedSysState : public MavlinkStream vehicle_land_detected_s land_detected; - if (_landed_sub->update(&land_detected)) { + if (_landed_sub.copy(&land_detected)) { updated = true; if (land_detected.landed) { @@ -4648,7 +4443,7 @@ class MavlinkStreamExtendedSysState : public MavlinkStream vehicle_control_mode_s control_mode; position_setpoint_triplet_s pos_sp_triplet; - if (_control_mode_sub->update(&control_mode) && _pos_sp_triplet_sub->update(&pos_sp_triplet)) { + if (_control_mode_sub.copy(&control_mode) && _pos_sp_triplet_sub.copy(&pos_sp_triplet)) { if (control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { _msg.landed_state = MAV_LANDED_STATE_TAKEOFF; @@ -4699,30 +4494,25 @@ class MavlinkStreamAltitude : public MavlinkStream unsigned get_size() override { - return (_local_pos_time > 0) ? MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _local_pos_sub.advertised() ? MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_local_pos_sub; - MavlinkOrbSubscription *_home_sub; - MavlinkOrbSubscription *_air_data_sub; - - uint64_t _local_pos_time{0}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _home_sub{ORB_ID(home_position)}; + uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; /* do not allow top copying this class */ MavlinkStreamAltitude(MavlinkStreamAltitude &) = delete; MavlinkStreamAltitude &operator = (const MavlinkStreamAltitude &) = delete; protected: - explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink), - _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), - _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), - _air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))) + explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - mavlink_altitude_t msg = {}; + mavlink_altitude_t msg{}; msg.altitude_monotonic = NAN; msg.altitude_amsl = NAN; @@ -4733,8 +4523,8 @@ class MavlinkStreamAltitude : public MavlinkStream // always update monotonic altitude bool air_data_updated = false; - vehicle_air_data_s air_data = {}; - _air_data_sub->update(&air_data); + vehicle_air_data_s air_data{}; + _air_data_sub.copy(&air_data); if (air_data.timestamp > 0) { msg.altitude_monotonic = air_data.baro_alt_meter; @@ -4746,7 +4536,7 @@ class MavlinkStreamAltitude : public MavlinkStream vehicle_local_position_s local_pos; - if (_local_pos_sub->update(&_local_pos_time, &local_pos)) { + if (_local_pos_sub.copy(&local_pos)) { if (local_pos.z_valid) { if (local_pos.z_global) { @@ -4758,8 +4548,8 @@ class MavlinkStreamAltitude : public MavlinkStream msg.altitude_local = -local_pos.z; - home_position_s home = {}; - _home_sub->update(&home); + home_position_s home{}; + _home_sub.copy(&home); if (home.valid_alt) { msg.altitude_relative = -(local_pos.z - home.z); @@ -4779,7 +4569,7 @@ class MavlinkStreamAltitude : public MavlinkStream // local position timeout after 10 ms // avoid publishing only baro altitude_monotonic if possible - bool lpos_timeout = (hrt_elapsed_time(&_local_pos_time) > 10000); + bool lpos_timeout = (hrt_elapsed_time(&local_pos.timestamp) > 10_ms); if (lpos_updated || (air_data_updated && lpos_timeout)) { msg.time_usec = hrt_absolute_time(); @@ -4822,32 +4612,27 @@ class MavlinkStreamWind : public MavlinkStream unsigned get_size() override { - return (_wind_estimate_time > 0) ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _wind_estimate_sub.advertised() ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_wind_estimate_sub; - uint64_t _wind_estimate_time; - - MavlinkOrbSubscription *_local_pos_sub; + uORB::Subscription _wind_estimate_sub{ORB_ID(wind_estimate)}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /* do not allow top copying this class */ MavlinkStreamWind(MavlinkStreamWind &) = delete; MavlinkStreamWind &operator = (const MavlinkStreamWind &) = delete; protected: - explicit MavlinkStreamWind(Mavlink *mavlink) : MavlinkStream(mavlink), - _wind_estimate_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))), - _wind_estimate_time(0), - _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))) + explicit MavlinkStreamWind(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { wind_estimate_s wind_estimate; - if (_wind_estimate_sub->update(&_wind_estimate_time, &wind_estimate)) { - mavlink_wind_cov_t msg = {}; + if (_wind_estimate_sub.update(&wind_estimate)) { + mavlink_wind_cov_t msg{}; msg.time_usec = wind_estimate.timestamp; @@ -4858,8 +4643,8 @@ class MavlinkStreamWind : public MavlinkStream msg.var_horiz = wind_estimate.variance_north + wind_estimate.variance_east; msg.var_vert = 0.0f; - vehicle_local_position_s lpos = {}; - _local_pos_sub->update(&lpos); + vehicle_local_position_s lpos{}; + _local_pos_sub.copy(&lpos); msg.wind_alt = (lpos.z_valid && lpos.z_global) ? (-lpos.z + lpos.ref_alt) : NAN; msg.horiz_accuracy = 0.0f; @@ -4904,38 +4689,35 @@ class MavlinkStreamMountOrientation : public MavlinkStream unsigned get_size() override { - return (_mount_orientation_time > 0) ? MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _mount_orientation_sub.advertised() ? MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_mount_orientation_sub; - MavlinkOrbSubscription *_gpos_sub; - uint64_t _mount_orientation_time{0}; + uORB::Subscription _mount_orientation_sub{ORB_ID(mount_orientation)}; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; /* do not allow top copying this class */ MavlinkStreamMountOrientation(MavlinkStreamMountOrientation &) = delete; MavlinkStreamMountOrientation &operator = (const MavlinkStreamMountOrientation &) = delete; protected: - explicit MavlinkStreamMountOrientation(Mavlink *mavlink) : MavlinkStream(mavlink), - _mount_orientation_sub(_mavlink->add_orb_subscription(ORB_ID(mount_orientation))), - _gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))) + explicit MavlinkStreamMountOrientation(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - mount_orientation_s mount_orientation{}; + mount_orientation_s mount_orientation; - if (_mount_orientation_sub->update(&_mount_orientation_time, &mount_orientation)) { - mavlink_mount_orientation_t msg = {}; + if (_mount_orientation_sub.update(&mount_orientation)) { + mavlink_mount_orientation_t msg{}; msg.roll = math::degrees(mount_orientation.attitude_euler_angle[0]); msg.pitch = math::degrees(mount_orientation.attitude_euler_angle[1]); msg.yaw = math::degrees(mount_orientation.attitude_euler_angle[2]); - vehicle_global_position_s gpos{}; - _gpos_sub->update(&gpos); - msg.yaw_absolute = math::degrees(matrix::wrap_2pi(gpos.yaw + mount_orientation.attitude_euler_angle[2])); + vehicle_local_position_s lpos{}; + _lpos_sub.copy(&lpos); + msg.yaw_absolute = math::degrees(matrix::wrap_2pi(lpos.yaw + mount_orientation.attitude_euler_angle[2])); mavlink_msg_mount_orientation_send_struct(_mavlink->get_channel(), &msg); @@ -4976,49 +4758,40 @@ class MavlinkStreamGroundTruth : public MavlinkStream unsigned get_size() override { - return (_att_time > 0 || _gpos_time > 0) ? MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN + - MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return (_att_sub.advertised() + || _gpos_sub.advertised()) ? MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_angular_velocity_sub; - MavlinkOrbSubscription *_att_sub; - MavlinkOrbSubscription *_gpos_sub; - MavlinkOrbSubscription *_lpos_sub; - - uint64_t _angular_velocity_time{0}; - uint64_t _att_time{0}; - uint64_t _gpos_time{0}; - uint64_t _lpos_time{0}; + uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; /* do not allow top copying this class */ MavlinkStreamGroundTruth(MavlinkStreamGroundTruth &) = delete; MavlinkStreamGroundTruth &operator = (const MavlinkStreamGroundTruth &) = delete; protected: - explicit MavlinkStreamGroundTruth(Mavlink *mavlink) : MavlinkStream(mavlink), - _angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity_groundtruth))), - _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_groundtruth))), - _gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position_groundtruth))), - _lpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_groundtruth))) + explicit MavlinkStreamGroundTruth(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - bool updated = false; + if (_angular_velocity_sub.updated() || _att_sub.updated() || _gpos_sub.updated() || _lpos_sub.updated()) { + vehicle_attitude_s att{}; + _att_sub.copy(&att); - vehicle_angular_velocity_s angular_velocity{}; - vehicle_attitude_s att{}; - vehicle_global_position_s gpos{}; - vehicle_local_position_s lpos{}; + vehicle_global_position_s gpos{}; + _gpos_sub.copy(&gpos); - updated |= _angular_velocity_sub->update(&_angular_velocity_time, &angular_velocity); - updated |= _att_sub->update(&_att_time, &att); - updated |= _gpos_sub->update(&_gpos_time, &gpos); - updated |= _lpos_sub->update(&_lpos_time, &lpos); + vehicle_local_position_s lpos{}; + _lpos_sub.copy(&lpos); - if (updated) { - mavlink_hil_state_quaternion_t msg = {}; + vehicle_angular_velocity_s angular_velocity{}; + _angular_velocity_sub.copy(&angular_velocity); + + mavlink_hil_state_quaternion_t msg{}; // vehicle_attitude -> hil_state_quaternion msg.attitude_quaternion[0] = att.q[0]; @@ -5147,34 +4920,30 @@ class MavlinkStreamOrbitStatus : public MavlinkStream unsigned get_size() override { - return MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN + - MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _orb_status_sub.advertised() ? MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_sub; - uint64_t _orbit_status_time; + uORB::Subscription _orb_status_sub{ORB_ID(orbit_status)}; /* do not allow top copying this class */ MavlinkStreamOrbitStatus(MavlinkStreamOrbitStatus &); MavlinkStreamOrbitStatus &operator = (const MavlinkStreamOrbitStatus &); protected: - explicit MavlinkStreamOrbitStatus(Mavlink *mavlink) : MavlinkStream(mavlink), - _sub(_mavlink->add_orb_subscription(ORB_ID(orbit_status))), - _orbit_status_time(0) + explicit MavlinkStreamOrbitStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { - struct orbit_status_s _orbit_status; + orbit_status_s _orbit_status; - if (_sub->update(&_orbit_status_time, &_orbit_status)) { - mavlink_orbit_execution_status_t _msg_orbit_execution_status = {}; + if (_orb_status_sub.update(&_orbit_status)) { + mavlink_orbit_execution_status_t _msg_orbit_execution_status{}; _msg_orbit_execution_status.time_usec = _orbit_status.timestamp; _msg_orbit_execution_status.radius = _orbit_status.radius; - _msg_orbit_execution_status.frame = _orbit_status.frame; + _msg_orbit_execution_status.frame = _orbit_status.frame; _msg_orbit_execution_status.x = _orbit_status.x * 1e7; _msg_orbit_execution_status.y = _orbit_status.y * 1e7; _msg_orbit_execution_status.z = _orbit_status.z; @@ -5216,31 +4985,27 @@ class MavlinkStreamObstacleDistance : public MavlinkStream unsigned get_size() override { - return _obstacle_distance_fused_sub->is_published() ? (MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN + - MAVLINK_NUM_NON_PAYLOAD_BYTES) : - 0; + return _obstacle_distance_fused_sub.advertised() ? (MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_obstacle_distance_fused_sub; - uint64_t _obstacle_distance_time; + uORB::Subscription _obstacle_distance_fused_sub{ORB_ID(obstacle_distance_fused)}; /* do not allow top copying this class */ MavlinkStreamObstacleDistance(MavlinkStreamObstacleDistance &) = delete; MavlinkStreamObstacleDistance &operator = (const MavlinkStreamObstacleDistance &) = delete; protected: - explicit MavlinkStreamObstacleDistance(Mavlink *mavlink) : MavlinkStream(mavlink), - _obstacle_distance_fused_sub(_mavlink->add_orb_subscription(ORB_ID(obstacle_distance_fused))), - _obstacle_distance_time(0) + explicit MavlinkStreamObstacleDistance(Mavlink *mavlink) : MavlinkStream(mavlink) {} bool send(const hrt_abstime t) override { obstacle_distance_s obstacle_distance; - if (_obstacle_distance_fused_sub->update(&_obstacle_distance_time, &obstacle_distance)) { - mavlink_obstacle_distance_t msg = {}; + if (_obstacle_distance_fused_sub.update(&obstacle_distance)) { + mavlink_obstacle_distance_t msg{}; msg.time_usec = obstacle_distance.timestamp; msg.sensor_type = obstacle_distance.sensor_type; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp deleted file mode 100644 index afe2358bcc25..000000000000 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_orb_subscription.cpp - * uORB subscription implementation. - * - */ - -#include "mavlink_orb_subscription.h" - -bool -MavlinkOrbSubscription::is_published() -{ - const bool published = _sub.advertised(); - - if (published) { - return true; - - } else if (!published && _subscribe_from_beginning) { - // For some topics like vehicle_command_ack, we want to subscribe - // from the beginning in order not to miss or delay the first publish respective advertise. - return _sub.subscribe(); - } - - return false; -} diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h deleted file mode 100644 index 5d2ab9545926..000000000000 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_orb_subscription.h - * uORB subscription definition. - * - * @author Anton Babushkin - */ - -#ifndef MAVLINK_ORB_SUBSCRIPTION_H_ -#define MAVLINK_ORB_SUBSCRIPTION_H_ - -#include -#include -#include - -class MavlinkOrbSubscription : public ListNode -{ -public: - - MavlinkOrbSubscription(const orb_id_t topic, int instance) : _sub(topic, instance) {} - ~MavlinkOrbSubscription() = default; - - /** - * Check if subscription updated based on timestamp. - * - * @return true only if topic was updated based on a timestamp and - * copied to buffer successfully. - * If topic was not updated since last check it will return false but - * still copy the data. - * If no data available data buffer will be filled with zeros. - */ - bool update(uint64_t *time, void *data) { return _sub.update(time, data); } - - /** - * Copy topic data to given buffer. - * - * @return true only if topic data copied successfully. - */ - bool update(void *data) { return _sub.copy(data); } - - /** - * Check if the subscription has been updated. - * - * @return true if there has been an update which has been - * copied successfully. - */ - bool update_if_changed(void *data) { return _sub.update(data); } - - /** - * Check if the topic has been published. - * - * This call will return true if the topic was ever published. - * @return true if the topic has been published at least once. - * If no data is available the buffer will be filled with zeros. - */ - bool is_published(); - - void subscribe_from_beginning(bool from_beginning) { _subscribe_from_beginning = from_beginning; } - - orb_id_t get_topic() const { return _sub.get_topic(); } - int get_instance() const { return _sub.get_instance(); } - -private: - - uORB::Subscription _sub; - - bool _subscribe_from_beginning{false}; ///< we need to subscribe from the beginning, e.g. for vehicle_command_acks -}; - - -#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */ diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 49b63aa619a2..0783ba1ffea5 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -94,19 +94,4 @@ void Subscription::unsubscribe() _last_generation = 0; } -bool Subscription::update(uint64_t *time, void *dst) -{ - if ((time != nullptr) && (dst != nullptr) && advertised()) { - // always copy data to dst regardless of update - const uint64_t t = _node->copy_and_get_timestamp(dst, _last_generation); - - if (*time == 0 || *time != t) { - *time = t; - return true; - } - } - - return false; -} - } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index f64d3fdd4670..00a9477e4b1a 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -118,17 +118,6 @@ class Subscription */ bool update(void *dst) { return updated() ? copy(dst) : false; } - /** - * Check if subscription updated based on timestamp. - * - * @return true only if topic was updated based on a timestamp and - * copied to buffer successfully. - * If topic was not updated since last check it will return false but - * still copy the data. - * If no data available data buffer will be filled with zeros. - */ - bool update(uint64_t *time, void *dst); - /** * Copy the struct * @param data The uORB message struct we are updating. diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index f962ed3e5d4d..f4921c85a71f 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -97,11 +97,6 @@ int orb_check(int handle, bool *updated) return uORB::Manager::get_instance()->orb_check(handle, updated); } -int orb_stat(int handle, uint64_t *time) -{ - return uORB::Manager::get_instance()->orb_stat(handle, time); -} - int orb_exists(const struct orb_metadata *meta, int instance) { return uORB::Manager::get_instance()->orb_exists(meta, instance); diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index af28dc61f7c2..462e5edc337b 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -221,11 +221,6 @@ extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) _ */ extern int orb_check(int handle, bool *updated) __EXPORT; -/** - * @see uORB::Manager::orb_stat() - */ -extern int orb_stat(int handle, uint64_t *time) __EXPORT; - /** * @see uORB::Manager::orb_exists() */ diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index b974c9f98f16..803cb4fc043e 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -183,19 +183,6 @@ uORB::DeviceNode::copy(void *dst, unsigned &generation) return updated; } -uint64_t -uORB::DeviceNode::copy_and_get_timestamp(void *dst, unsigned &generation) -{ - ATOMIC_ENTER; - - const hrt_abstime update_time = _last_update; - copy_locked(dst, generation); - - ATOMIC_LEAVE; - - return update_time; -} - ssize_t uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen) { @@ -216,13 +203,13 @@ uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen) */ ATOMIC_ENTER; - copy_locked(buffer, sd->generation); - // if subscriber has an interval track the last update time if (sd->update_interval) { - sd->update_interval->last_update = _last_update; + sd->update_interval->last_update = hrt_absolute_time(); } + copy_locked(buffer, sd->generation); + ATOMIC_LEAVE; return _meta->o_size; @@ -279,10 +266,6 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size); - /* update the timestamp and generation count */ - _last_update = hrt_absolute_time(); - - // callbacks for (auto item : _callbacks) { item->call(); @@ -302,13 +285,6 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) SubscriberData *sd = filp_to_sd(filp); switch (cmd) { - case ORBIOCLASTUPDATE: { - ATOMIC_ENTER; - *(hrt_abstime *)arg = _last_update; - ATOMIC_LEAVE; - return PX4_OK; - } - case ORBIOCUPDATED: { ATOMIC_ENTER; *(bool *)arg = appears_updated(sd); diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 4aef593ab0a6..e3800c61ea73 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -214,21 +214,6 @@ class uORB::DeviceNode : public cdev::CDev, public ListNode */ bool copy(void *dst, unsigned &generation); - /** - * Copies data and the corresponding generation - * from a node to the buffer provided. - * - * @param dst - * The buffer into which the data is copied. - * If topic was not updated since last check it will return false but - * still copy the data. - * @param generation - * The generation that was copied. - * @return uint64_t - * Returns the timestamp of the copied data. - */ - uint64_t copy_and_get_timestamp(void *dst, unsigned &generation); - // add item to list of work items to schedule on node update bool register_callback(SubscriptionCallback *callback_sub); @@ -269,20 +254,21 @@ class uORB::DeviceNode : public cdev::CDev, public ListNode }; const orb_metadata *_meta; /**< object metadata information */ - const uint8_t _instance; /**< orb multi instance identifier */ + uint8_t *_data{nullptr}; /**< allocated object buffer */ - hrt_abstime _last_update{0}; /**< time the object was last updated */ px4::atomic _generation{0}; /**< object generation count */ List _callbacks; - uint8_t _priority; /**< priority of the topic */ - bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */ - uint8_t _queue_size; /**< maximum number of elements in the queue */ - int8_t _subscriber_count{0}; // statistics uint32_t _lost_messages = 0; /**< nr of lost messages for all subscribers. If two subscribers lose the same message, it is counted as two. */ + const uint8_t _instance; /**< orb multi instance identifier */ + uint8_t _priority; /**< priority of the topic */ + bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */ + uint8_t _queue_size; /**< maximum number of elements in the queue */ + int8_t _subscriber_count{0}; + inline static SubscriberData *filp_to_sd(cdev::file_t *filp); /** diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index 10c5ea00e354..81e15ecfd619 100644 --- a/src/modules/uORB/uORBManager.cpp +++ b/src/modules/uORB/uORBManager.cpp @@ -305,11 +305,6 @@ int uORB::Manager::orb_check(int handle, bool *updated) return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); } -int uORB::Manager::orb_stat(int handle, uint64_t *time) -{ - return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); -} - int uORB::Manager::orb_priority(int handle, int32_t *priority) { return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index be5f42844aec..1ea0f7a82f05 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -177,7 +177,7 @@ class uORB::Manager * * The data is atomically published to the topic and any waiting subscribers * will be notified. Subscribers that are not waiting can check the topic - * for updates using orb_check and/or orb_stat. + * for updates using orb_check. * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. @@ -192,7 +192,7 @@ class uORB::Manager * * The returned value is a file descriptor that can be passed to poll() * in order to wait for updates to a topic, as well as topic_read, - * orb_check and orb_stat. + * orb_check. * * If there were any publications of the topic prior to the subscription, * an orb_check right after orb_subscribe will return true. @@ -222,7 +222,7 @@ class uORB::Manager * * The returned value is a file descriptor that can be passed to poll() * in order to wait for updates to a topic, as well as topic_read, - * orb_check and orb_stat. + * orb_check. * * If there were any publications of the topic prior to the subscription, * an orb_check right after orb_subscribe_multi will return true. @@ -289,9 +289,7 @@ class uORB::Manager * topic is likely to have updated. * * Updates are tracked on a per-handle basis; this call will continue to - * return true until orb_copy is called using the same handle. This interface - * should be preferred over calling orb_stat due to the race window between - * stat and copy that can lead to missed updates. + * return true until orb_copy is called using the same handle. * * @param handle A handle returned from orb_subscribe. * @param updated Set to true if the topic has been updated since the @@ -301,17 +299,6 @@ class uORB::Manager */ int orb_check(int handle, bool *updated); - /** - * Return the last time that the topic was updated. If a queue is used, it returns - * the timestamp of the latest element in the queue. - * - * @param handle A handle returned from orb_subscribe. - * @param time Returns the absolute time that the topic was updated, or zero if it has - * never been updated. Time is measured in microseconds. - * @return OK on success, PX4_ERROR otherwise with errno set accordingly. - */ - int orb_stat(int handle, uint64_t *time); - /** * Check if a topic has already been created and published (advertised) * diff --git a/src/systemcmds/tests/test_microbench_uorb.cpp b/src/systemcmds/tests/test_microbench_uorb.cpp index 9ae714233d9e..43ace7fdcfab 100644 --- a/src/systemcmds/tests/test_microbench_uorb.cpp +++ b/src/systemcmds/tests/test_microbench_uorb.cpp @@ -150,19 +150,16 @@ bool MicroBenchORB::time_px4_uorb() uint64_t time = 0; PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 1000); - PERF("orb_stat vehicle_status", ret = orb_stat(fd_status, &time), 1000); PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 1000); printf("\n"); PERF("orb_check vehicle_local_position", ret = orb_check(fd_lpos, &updated), 1000); - PERF("orb_stat vehicle_local_position", ret = orb_stat(fd_lpos, &time), 1000); PERF("orb_copy vehicle_local_position", ret = orb_copy(ORB_ID(vehicle_local_position), fd_lpos, &lpos), 1000); printf("\n"); PERF("orb_check sensor_gyro", ret = orb_check(fd_gyro, &updated), 1000); - PERF("orb_stat sensor_gyro", ret = orb_stat(fd_gyro, &time), 1000); PERF("orb_copy sensor_gyro", ret = orb_copy(ORB_ID(sensor_gyro), fd_gyro, &gyro), 1000); printf("\n");