diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index 483e5b8798b3..59d692d2a188 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include /* Configuration Constants */ @@ -117,7 +117,7 @@ class PAW3902 : public device::SPI, public px4::ScheduledWorkItem bool changeMode(Mode newMode); - uORB::Publication _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; perf_counter_t _sample_perf; perf_counter_t _interval_perf; diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 604c8291e5f3..55af0320c614 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include class PX4Accelerometer : public cdev::CDev, public ModuleParams @@ -68,7 +68,7 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } - uORB::PublicationData _sensor_accel_pub; + uORB::PublicationMultiData _sensor_accel_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; Integrator _integrator{4000, false}; diff --git a/src/lib/drivers/barometer/PX4Barometer.hpp b/src/lib/drivers/barometer/PX4Barometer.hpp index a72e4038d17e..ca48928119a3 100644 --- a/src/lib/drivers/barometer/PX4Barometer.hpp +++ b/src/lib/drivers/barometer/PX4Barometer.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include class PX4Barometer : public cdev::CDev @@ -59,7 +59,7 @@ class PX4Barometer : public cdev::CDev private: - uORB::PublicationData _sensor_baro_pub; + uORB::PublicationMultiData _sensor_baro_pub; int _class_device_instance{-1}; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 6868038b46e9..50e99e4655b4 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include class PX4Gyroscope : public cdev::CDev, public ModuleParams @@ -68,7 +68,7 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } - uORB::PublicationData _sensor_gyro_pub; + uORB::PublicationMultiData _sensor_gyro_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; Integrator _integrator{4000, true}; diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index 545a10ace4f4..e909602dd881 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include class PX4Magnetometer : public cdev::CDev @@ -63,7 +63,7 @@ class PX4Magnetometer : public cdev::CDev private: - uORB::PublicationData _sensor_mag_pub; + uORB::PublicationMultiData _sensor_mag_pub; const enum Rotation _rotation; diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index c1d484c921ac..c9cb7bb08e61 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include class PX4Rangefinder : public cdev::CDev @@ -64,7 +64,7 @@ class PX4Rangefinder : public cdev::CDev private: - uORB::PublicationData _distance_sensor_pub; + uORB::PublicationMultiData _distance_sensor_pub; int _class_device_instance{-1}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8c0383dde91c..b23945d8eb8a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -43,12 +43,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include #include #include @@ -87,18 +81,17 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), _mavlink_ftp(parent), _mavlink_log_handler(parent), - _mavlink_timesync(parent), _mission_manager(parent), - _parameters_manager(parent) + _parameters_manager(parent), + _mavlink_timesync(parent) { - /* Make the attitude quaternion valid */ - _att.q[0] = 1.0f; } void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result) { vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); command_ack.command = command; command_ack.result = result; @@ -386,7 +379,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ @@ -415,7 +409,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) mavlink_command_int_t cmd_mavlink; mavlink_msg_command_int_decode(msg, &cmd_mavlink); - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; vcmd.timestamp = hrt_absolute_time(); /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ @@ -526,7 +520,8 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink->get_channel()); - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); command_ack.result_param2 = ack.result_param2; command_ack.command = ack.command; @@ -553,7 +548,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); - optical_flow_s f = {}; + optical_flow_s f{}; f.timestamp = hrt_absolute_time(); f.time_since_last_sonar_update = flow.time_delta_distance_us; @@ -579,17 +574,13 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val); rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral); - if (_flow_pub == nullptr) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - - } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &f); - } + _flow_pub.publish(f); /* Use distance value for distance sensor topic */ - distance_sensor_s d = {}; - if (flow.distance > 0.0f) { // negative values signal invalid data + + distance_sensor_s d{}; + d.timestamp = f.timestamp; d.min_distance = 0.3f; d.max_distance = 5.0f; @@ -599,13 +590,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; d.variance = 0.0; - if (_flow_distance_sensor_pub == nullptr) { - _flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); - - } else { - orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d); - } + _flow_distance_sensor_pub.publish(d); } } @@ -616,8 +601,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) mavlink_hil_optical_flow_t flow; mavlink_msg_hil_optical_flow_decode(msg, &flow); - struct optical_flow_s f; - memset(&f, 0, sizeof(f)); + optical_flow_s f{}; f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; f.integration_timespan = flow.integration_time_us; @@ -632,16 +616,10 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) f.sensor_id = flow.sensor_id; f.gyro_temperature = flow.temperature; - if (_flow_pub == nullptr) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - - } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &f); - } + _flow_pub.publish(f); /* Use distance value for distance sensor topic */ - struct distance_sensor_s d; - memset(&d, 0, sizeof(d)); + distance_sensor_s d{}; d.timestamp = hrt_absolute_time(); d.min_distance = 0.3f; @@ -652,13 +630,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; d.variance = 0.0; - if (_hil_distance_sensor_pub == nullptr) { - _hil_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); - - } else { - orb_publish(ORB_ID(distance_sensor), _hil_distance_sensor_pub, &d); - } + _flow_distance_sensor_pub.publish(d); } void @@ -671,6 +643,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) custom_mode.data = new_mode.custom_mode; vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ @@ -696,8 +669,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) mavlink_distance_sensor_t dist_sensor; mavlink_msg_distance_sensor_decode(msg, &dist_sensor); - struct distance_sensor_s d; - memset(&d, 0, sizeof(d)); + distance_sensor_s d{}; d.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */ d.min_distance = float(dist_sensor.min_distance) * 1e-2f; /* cm to m */ @@ -709,14 +681,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) d.variance = dist_sensor.covariance * 1e-4f; // cm^2 to m^2 /// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum - - if (_distance_sensor_pub == nullptr) { - _distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); - - } else { - orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); - } + _distance_sensor_pub.publish(d); } void @@ -725,7 +690,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) mavlink_att_pos_mocap_t mocap; mavlink_msg_att_pos_mocap_decode(msg, &mocap); - struct vehicle_odometry_s mocap_odom = {}; + vehicle_odometry_s mocap_odom{}; mocap_odom.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec); mocap_odom.x = mocap.x; @@ -752,9 +717,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) mocap_odom.yawspeed = NAN; mocap_odom.velocity_covariance[0] = NAN; - int instance_id = 0; - - orb_publish_auto(ORB_ID(vehicle_mocap_odometry), &_mocap_odometry_pub, &mocap_odom, &instance_id, ORB_PRIO_HIGH); + _mocap_odometry_pub.publish(mocap_odom); } void @@ -763,9 +726,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t mavlink_set_position_target_local_ned_t set_position_target_local_ned; mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned); - struct offboard_control_mode_s offboard_control_mode = {}; - - bool values_finite = + const bool values_finite = PX4_ISFINITE(set_position_target_local_ned.x) && PX4_ISFINITE(set_position_target_local_ned.y) && PX4_ISFINITE(set_position_target_local_ned.z) && @@ -784,39 +745,37 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t set_position_target_local_ned.target_component == 0) && values_finite) { + offboard_control_mode_s offboard_control_mode{}; + /* convert mavlink type (local, NED) to uORB offboard control struct */ offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & 0x4); offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38); offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0); - bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); - /* yaw ignore flag mapps to ignore_attitude */ offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_local_ned.type_mask & 0x800); offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_local_ned.type_mask & 0x800); offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_local_ned.type_mask & 0x800); + /* yaw ignore flag mapps to ignore_attitude */ + bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000); bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000); bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000); bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000); offboard_control_mode.timestamp = hrt_absolute_time(); - - if (_offboard_control_mode_pub == nullptr) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); - - } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); - } + _offboard_control_mode_pub.publish(offboard_control_mode); /* If we are in offboard control mode and offboard control loop through is enabled * also publish the setpoint topic which is read by the controller */ if (_mavlink->get_forward_externalsp()) { - _control_mode_sub.update(&_control_mode); + vehicle_control_mode_s control_mode{}; + _control_mode_sub.copy(&control_mode); - if (_control_mode.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { if (is_force_sp && offboard_control_mode.ignore_position && offboard_control_mode.ignore_velocity) { @@ -824,7 +783,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } else { /* It's not a pure force setpoint: publish to setpoint triplet topic */ - struct position_setpoint_triplet_s pos_sp_triplet = {}; + position_setpoint_triplet_s pos_sp_triplet{}; + pos_sp_triplet.timestamp = hrt_absolute_time(); pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; @@ -850,6 +810,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values */ if (!offboard_control_mode.ignore_position) { + pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = set_position_target_local_ned.x; pos_sp_triplet.current.y = set_position_target_local_ned.y; @@ -861,13 +822,13 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local vel values */ if (!offboard_control_mode.ignore_velocity) { + pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = set_position_target_local_ned.vx; pos_sp_triplet.current.vy = set_position_target_local_ned.vy; pos_sp_triplet.current.vz = set_position_target_local_ned.vz; - pos_sp_triplet.current.velocity_frame = - set_position_target_local_ned.coordinate_frame; + pos_sp_triplet.current.velocity_frame = set_position_target_local_ned.coordinate_frame; } else { pos_sp_triplet.current.velocity_valid = false; @@ -884,12 +845,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local acceleration values if the setpoint type is 'local pos' and none * of the accelerations fields is set to 'ignore' */ if (!offboard_control_mode.ignore_acceleration_force) { + pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = set_position_target_local_ned.afx; pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; - pos_sp_triplet.current.acceleration_is_force = - is_force_sp; + pos_sp_triplet.current.acceleration_is_force = is_force_sp; } else { pos_sp_triplet.current.acceleration_valid = false; @@ -908,6 +869,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t if (!(offboard_control_mode.ignore_bodyrate_x || offboard_control_mode.ignore_bodyrate_y || offboard_control_mode.ignore_bodyrate_z)) { + pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; @@ -916,20 +878,9 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } //XXX handle global pos setpoints (different MAV frames) - - if (_pos_sp_triplet_pub == nullptr) { - _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), - &pos_sp_triplet); - - } else { - orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, - &pos_sp_triplet); - } - + _pos_sp_triplet_pub.publish(pos_sp_triplet); } - } - } } } @@ -940,10 +891,6 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m mavlink_set_actuator_control_target_t set_actuator_control_target; mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target); - struct offboard_control_mode_s offboard_control_mode = {}; - - struct actuator_controls_s actuator_controls = {}; - bool values_finite = PX4_ISFINITE(set_actuator_control_target.controls[0]) && PX4_ISFINITE(set_actuator_control_target.controls[1]) && @@ -959,8 +906,12 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m (mavlink_system.compid == set_actuator_control_target.target_component || set_actuator_control_target.target_component == 0) && values_finite) { + /* Ignore all setpoints except when controlling the gimbal(group_mlx==2) as we are setting raw actuators here */ bool ignore_setpoints = bool(set_actuator_control_target.group_mlx != 2); + + offboard_control_mode_s offboard_control_mode{}; + offboard_control_mode.ignore_thrust = ignore_setpoints; offboard_control_mode.ignore_attitude = ignore_setpoints; offboard_control_mode.ignore_bodyrate_x = ignore_setpoints; @@ -972,18 +923,15 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.timestamp = hrt_absolute_time(); - if (_offboard_control_mode_pub == nullptr) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); - - } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); - } + _offboard_control_mode_pub.publish(offboard_control_mode); /* If we are in offboard control mode, publish the actuator controls */ - _control_mode_sub.update(&_control_mode); + vehicle_control_mode_s control_mode{}; + _control_mode_sub.copy(&control_mode); - if (_control_mode.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { + actuator_controls_s actuator_controls{}; actuator_controls.timestamp = hrt_absolute_time(); /* Set duty cycles for the servos in the actuator_controls message */ @@ -993,43 +941,19 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m switch (set_actuator_control_target.group_mlx) { case 0: - if (_actuator_controls_pubs[0] == nullptr) { - _actuator_controls_pubs[0] = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); - - } else { - orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pubs[0], &actuator_controls); - } - + _actuator_controls_pubs[0].publish(actuator_controls); break; case 1: - if (_actuator_controls_pubs[1] == nullptr) { - _actuator_controls_pubs[1] = orb_advertise(ORB_ID(actuator_controls_1), &actuator_controls); - - } else { - orb_publish(ORB_ID(actuator_controls_1), _actuator_controls_pubs[1], &actuator_controls); - } - + _actuator_controls_pubs[1].publish(actuator_controls); break; case 2: - if (_actuator_controls_pubs[2] == nullptr) { - _actuator_controls_pubs[2] = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls); - - } else { - orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_pubs[2], &actuator_controls); - } - + _actuator_controls_pubs[2].publish(actuator_controls); break; case 3: - if (_actuator_controls_pubs[3] == nullptr) { - _actuator_controls_pubs[3] = orb_advertise(ORB_ID(actuator_controls_3), &actuator_controls); - - } else { - orb_publish(ORB_ID(actuator_controls_3), _actuator_controls_pubs[3], &actuator_controls); - } - + _actuator_controls_pubs[3].publish(actuator_controls); break; default: @@ -1061,7 +985,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) mavlink_vision_position_estimate_t ev; mavlink_msg_vision_position_estimate_decode(msg, &ev); - struct vehicle_odometry_s visual_odom = {}; + vehicle_odometry_s visual_odom{}; visual_odom.timestamp = _mavlink_timesync.sync_stamp(ev.usec); visual_odom.x = ev.x; @@ -1092,8 +1016,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) visual_odom.yawspeed = NAN; visual_odom.velocity_covariance[0] = NAN; - int instance_id = 0; - orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &visual_odom, &instance_id, ORB_PRIO_HIGH); + _visual_odometry_pub.publish(visual_odom); } void @@ -1186,15 +1109,17 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) } } else if (odom.child_frame_id == MAV_FRAME_BODY_NED) { /* WRT to vehicle body-NED frame */ - if (_vehicle_attitude_sub.copy(&_att)) { + vehicle_attitude_s att{}; + + if (_vehicle_attitude_sub.copy(&att)) { /* Get quaternion from vehicle_attitude quaternion and build DCM matrix from it * Note that the msg quaternion represents the rotation of the msg frame to the msg child frame * but rotates msg child frame *data* into the msg frame */ - const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(_att.q)); + const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(att.q)); /* the linear velocities needs to be transformed to the local NED frame */ - matrix::Vector3 linvel_local(Rbl * matrix::Vector3(odom.vx, odom.vy, odom.vz)); + matrix::Vector3f linvel_local(Rbl * matrix::Vector3f(odom.vx, odom.vy, odom.vz)); odometry.vx = linvel_local(0); odometry.vy = linvel_local(1); odometry.vz = linvel_local(2); @@ -1213,17 +1138,19 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) } else if (odom.child_frame_id == MAV_FRAME_VISION_NED || /* WRT to vehicle local NED frame */ odom.child_frame_id == MAV_FRAME_MOCAP_NED) { - if (_vehicle_attitude_sub.copy(&_att)) { + vehicle_attitude_s att{}; + + if (_vehicle_attitude_sub.copy(&att)) { /* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */ - matrix::Dcmf Rlb = matrix::Quatf(_att.q); + matrix::Dcmf Rlb = matrix::Quatf(att.q); odometry.vx = odom.vx; odometry.vy = odom.vy; odometry.vz = odom.vz; /* the angular rates need to be transformed to the body frame */ - matrix::Vector3 angvel_local(Rlb * matrix::Vector3(odom.rollspeed, odom.pitchspeed, odom.yawspeed)); + matrix::Vector3f angvel_local(Rlb * matrix::Vector3f(odom.rollspeed, odom.pitchspeed, odom.yawspeed)); odometry.rollspeed = angvel_local(0); odometry.pitchspeed = angvel_local(1); odometry.yawspeed = angvel_local(2); @@ -1232,20 +1159,17 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) for (size_t i = 0; i < VEL_URT_SIZE; i++) { odometry.velocity_covariance[i] = odom.velocity_covariance[i]; } - } } else { PX4_ERR("Body frame %u not supported. Unable to publish velocity", odom.child_frame_id); } - int instance_id = 0; - if (odom.frame_id == MAV_FRAME_VISION_NED) { - orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &odometry, &instance_id, ORB_PRIO_HIGH); + _visual_odometry_pub.publish(odometry); } else if (odom.frame_id == MAV_FRAME_MOCAP_NED) { - orb_publish_auto(ORB_ID(vehicle_mocap_odometry), &_mocap_odometry_pub, &odometry, &instance_id, ORB_PRIO_HIGH); + _mocap_odometry_pub.publish(odometry); } else { PX4_ERR("Local frame %u not supported. Unable to publish pose and velocity", odom.frame_id); @@ -1275,8 +1199,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) set_attitude_target.target_component == 0) && values_finite) { + offboard_control_mode_s offboard_control_mode{}; + /* set correct ignore flags for thrust field: copy from mavlink message */ - _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); /* * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust @@ -1298,46 +1224,40 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if ((ignore_bodyrate_msg_x || ignore_bodyrate_msg_y || ignore_bodyrate_msg_z) && - ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) { + ignore_attitude_msg && !offboard_control_mode.ignore_thrust) { + /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - _offboard_control_mode.ignore_bodyrate_x = - ignore_bodyrate_msg_x && _offboard_control_mode.ignore_bodyrate_x; - _offboard_control_mode.ignore_bodyrate_y = - ignore_bodyrate_msg_y && _offboard_control_mode.ignore_bodyrate_y; - _offboard_control_mode.ignore_bodyrate_z = - ignore_bodyrate_msg_z && _offboard_control_mode.ignore_bodyrate_z; - _offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude; + offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x && offboard_control_mode.ignore_bodyrate_x; + offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y && offboard_control_mode.ignore_bodyrate_y; + offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z && offboard_control_mode.ignore_bodyrate_z; + offboard_control_mode.ignore_attitude = ignore_attitude_msg && offboard_control_mode.ignore_attitude; } else { - _offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x; - _offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y; - _offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z; - _offboard_control_mode.ignore_attitude = ignore_attitude_msg; + offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x; + offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y; + offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z; + offboard_control_mode.ignore_attitude = ignore_attitude_msg; } - _offboard_control_mode.ignore_position = true; - _offboard_control_mode.ignore_velocity = true; - _offboard_control_mode.ignore_acceleration_force = true; + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; - _offboard_control_mode.timestamp = hrt_absolute_time(); + offboard_control_mode.timestamp = hrt_absolute_time(); - if (_offboard_control_mode_pub == nullptr) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode); - - } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &_offboard_control_mode); - } + _offboard_control_mode_pub.publish(offboard_control_mode); /* If we are in offboard control mode and offboard control loop through is enabled * also publish the setpoint topic which is read by the controller */ if (_mavlink->get_forward_externalsp()) { - _control_mode_sub.update(&_control_mode); + vehicle_control_mode_s control_mode{}; + _control_mode_sub.copy(&control_mode); - if (_control_mode.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ - if (!(_offboard_control_mode.ignore_attitude)) { + if (!(offboard_control_mode.ignore_attitude)) { vehicle_attitude_setpoint_s att_sp = {}; att_sp.timestamp = hrt_absolute_time(); @@ -1353,7 +1273,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.yaw_sp_move_rate = 0.0f; } - if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid // Fill correct field by checking frametype // TODO: add as needed switch (_mavlink->get_system_type()) { @@ -1378,19 +1298,16 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } } - if (_att_sp_pub == nullptr) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - } else { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); - } + _att_sp_pub.publish(att_sp); } /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ - if (!_offboard_control_mode.ignore_bodyrate_x || - !_offboard_control_mode.ignore_bodyrate_y || - !_offboard_control_mode.ignore_bodyrate_z) { - vehicle_rates_setpoint_s rates_sp = {}; + if (!offboard_control_mode.ignore_bodyrate_x || + !offboard_control_mode.ignore_bodyrate_y || + !offboard_control_mode.ignore_bodyrate_z) { + + vehicle_rates_setpoint_s rates_sp{}; + rates_sp.timestamp = hrt_absolute_time(); // only copy att rates sp if message contained new data @@ -1406,7 +1323,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) rates_sp.yaw = set_attitude_target.body_yaw_rate; } - if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid switch (_mavlink->get_system_type()) { case MAV_TYPE_GENERIC: break; @@ -1430,15 +1347,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } - if (_rates_sp_pub == nullptr) { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - } else { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); - } + _rates_sp_pub.publish(rates_sp); } } - } } } @@ -1451,7 +1362,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); - radio_status_s status = {}; + radio_status_s status{}; + status.timestamp = hrt_absolute_time(); status.rssi = rstatus.rssi; status.remote_rssi = rstatus.remrssi; @@ -1463,8 +1375,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) _mavlink->update_radio_status(status); - int multi_instance; - orb_publish_auto(ORB_ID(radio_status), &_radio_status_pub, &status, &multi_instance, ORB_PRIO_HIGH); + _radio_status_pub.publish(status); } } @@ -1483,7 +1394,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) } else if ((ping.target_system == mavlink_system.sysid) && (ping.target_component == - mavlink_system.compid)) { // This is a returned ping message from this system. Calculate latency from it. + mavlink_system.compid)) { // This is a returned ping message from this system. Calculate latency from it. const hrt_abstime now = hrt_absolute_time(); @@ -1518,7 +1429,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) /* Ping status is supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */ if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { - struct ping_s uorb_ping_msg = {}; + ping_s uorb_ping_msg{}; uorb_ping_msg.timestamp = now; uorb_ping_msg.ping_time = ping.time_usec; @@ -1528,13 +1439,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) uorb_ping_msg.system_id = msg->sysid; uorb_ping_msg.component_id = msg->compid; - if (_ping_pub == nullptr) { - int multi_instance; - _ping_pub = orb_advertise_multi(ORB_ID(ping), &uorb_ping_msg, &multi_instance, ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(ping), _ping_pub, &uorb_ping_msg); - } + _ping_pub.publish(uorb_ping_msg); } } } @@ -1551,7 +1456,7 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) mavlink_battery_status_t battery_mavlink; mavlink_msg_battery_status_decode(msg, &battery_mavlink); - battery_status_s battery_status = {}; + battery_status_s battery_status{}; battery_status.timestamp = hrt_absolute_time(); float voltage_sum = 0.0f; @@ -1583,12 +1488,7 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) battery_status.warning = battery_status_s::BATTERY_WARNING_LOW; } - if (_battery_pub == nullptr) { - _battery_pub = orb_advertise(ORB_ID(battery_status), &battery_status); - - } else { - orb_publish(ORB_ID(battery_status), _battery_pub, &battery_status); - } + _battery_pub.publish(battery_status); } void @@ -1661,7 +1561,8 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg) mavlink_obstacle_distance_t mavlink_obstacle_distance; mavlink_msg_obstacle_distance_decode(msg, &mavlink_obstacle_distance); - obstacle_distance_s obstacle_distance = {}; + obstacle_distance_s obstacle_distance{}; + obstacle_distance.timestamp = hrt_absolute_time(); obstacle_distance.sensor_type = mavlink_obstacle_distance.sensor_type; memcpy(obstacle_distance.distances, mavlink_obstacle_distance.distances, sizeof(obstacle_distance.distances)); @@ -1677,12 +1578,7 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg) obstacle_distance.max_distance = mavlink_obstacle_distance.max_distance; obstacle_distance.angle_offset = mavlink_obstacle_distance.angle_offset; - if (_obstacle_distance_pub == nullptr) { - _obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &obstacle_distance); - - } else { - orb_publish(ORB_ID(obstacle_distance), _obstacle_distance_pub, &obstacle_distance); - } + _obstacle_distance_pub.publish(obstacle_distance); } void @@ -1691,8 +1587,7 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess mavlink_trajectory_representation_waypoints_t trajectory; mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory); - - struct vehicle_trajectory_waypoint_s trajectory_waypoint = {}; + vehicle_trajectory_waypoint_s trajectory_waypoint{}; trajectory_waypoint.timestamp = hrt_absolute_time(); const int number_valid_points = trajectory.valid_points; @@ -1724,13 +1619,7 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess trajectory_waypoint.waypoints[i].point_valid = false; } - if (_trajectory_waypoint_pub == nullptr) { - _trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &trajectory_waypoint); - - } else { - orb_publish(ORB_ID(vehicle_trajectory_waypoint), _trajectory_waypoint_pub, &trajectory_waypoint); - } - + _trajectory_waypoint_pub.publish(trajectory_waypoint); } switch_pos_t @@ -1743,7 +1632,6 @@ MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) int MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw) { - bool on = (buttons & (1 << sw)); if (sw < MOM_SWITCH_COUNT) { @@ -1788,7 +1676,8 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) } // fill uORB message - struct input_rc_s rc = {}; + input_rc_s rc{}; + // metadata rc.timestamp = hrt_absolute_time(); rc.timestamp_last_signal = rc.timestamp; @@ -1799,6 +1688,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) rc.rc_total_frame_count = 1; rc.rc_ppm_frame_length = 0; rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; + // channels rc.values[0] = man.chan1_raw; rc.values[1] = man.chan2_raw; @@ -1836,9 +1726,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) } // publish uORB message - int instance; // provides the instance ID or the publication - ORB_PRIO priority = ORB_PRIO_HIGH; // since it is an override, set priority high - orb_publish_auto(ORB_ID(input_rc), &_rc_pub, &rc, &instance, priority); + _rc_pub.publish(rc); } void @@ -1854,7 +1742,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) if (_mavlink->get_manual_input_mode_generation()) { - struct input_rc_s rc = {}; + input_rc_s rc{}; rc.timestamp = hrt_absolute_time(); rc.timestamp_last_signal = rc.timestamp; @@ -1867,14 +1755,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; rc.rssi = RC_INPUT_RSSI_MAX; - /* roll */ - rc.values[0] = man.x / 2 + 1500; - /* pitch */ - rc.values[1] = man.y / 2 + 1500; - /* yaw */ - rc.values[2] = man.r / 2 + 1500; - /* throttle */ - rc.values[3] = fminf(fmaxf(man.z / 0.9f + 800, 1000.0f), 2000.0f); + rc.values[0] = man.x / 2 + 1500; // roll + rc.values[1] = man.y / 2 + 1500; // pitch + rc.values[2] = man.r / 2 + 1500; // yaw + rc.values[3] = math::constrain(man.z / 0.9f + 800.0f, 1000.0f, 2000.0f); // throttle /* decode all switches which fit into the channel mask */ unsigned max_switch = (sizeof(man.buttons) * 8); @@ -1891,15 +1775,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) _mom_switch_state = man.buttons; - if (_rc_pub == nullptr) { - _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); - - } else { - orb_publish(ORB_ID(input_rc), _rc_pub, &rc); - } + _rc_pub.publish(rc); } else { - struct manual_control_setpoint_s manual = {}; + manual_control_setpoint_s manual{}; manual.timestamp = hrt_absolute_time(); manual.x = man.x / 1000.0f; @@ -1908,8 +1787,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.z = man.z / 1000.0f; manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); - int m_inst; - orb_publish_auto(ORB_ID(manual_control_setpoint), &_manual_pub, &manual, &m_inst, ORB_PRIO_LOW); + _manual_pub.publish(manual); } } @@ -2003,11 +1881,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - uint64_t timestamp = hrt_absolute_time(); + const uint64_t timestamp = hrt_absolute_time(); /* airspeed */ { - struct airspeed_s airspeed = {}; + airspeed_s airspeed{}; float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); float tas = calc_true_airspeed_from_indicated(ias, imu.abs_pressure * 100, imu.temperature); @@ -2016,17 +1894,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) airspeed.indicated_airspeed_m_s = ias; airspeed.true_airspeed_m_s = tas; - if (_airspeed_pub == nullptr) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } + _airspeed_pub.publish(airspeed); } /* gyro */ { - sensor_gyro_s gyro = {}; + sensor_gyro_s gyro{}; gyro.timestamp = timestamp; gyro.x_raw = imu.xgyro * 1000.0f; @@ -2037,17 +1910,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) gyro.z = imu.zgyro; gyro.temperature = imu.temperature; - if (_gyro_pub == nullptr) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - - } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); - } + _gyro_pub.publish(gyro); } /* accelerometer */ { - sensor_accel_s accel = {}; + sensor_accel_s accel{}; accel.timestamp = timestamp; accel.x_raw = imu.xacc / (CONSTANTS_ONE_G / 1000.0f); @@ -2058,17 +1926,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) accel.z = imu.zacc; accel.temperature = imu.temperature; - if (_accel_pub == nullptr) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } + _accel_pub.publish(accel); } /* magnetometer */ { - struct mag_report mag = {}; + sensor_mag_s mag{}; mag.timestamp = timestamp; mag.x_raw = imu.xmag * 1000.0f; @@ -2078,18 +1941,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mag.y = imu.ymag; mag.z = imu.zmag; - if (_mag_pub == nullptr) { - /* publish to the first mag topic */ - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - - } else { - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } + _mag_pub.publish(mag); } /* baro */ { - sensor_baro_s baro = {}; + sensor_baro_s baro{}; baro.timestamp = timestamp; baro.pressure = imu.abs_pressure; @@ -2098,17 +1955,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* fake device ID */ baro.device_id = 1234567; - if (_baro_pub == nullptr) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - - } else { - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - } + _baro_pub.publish(baro); } /* battery status */ { - struct battery_status_s hil_battery_status = {}; + battery_status_s hil_battery_status{}; hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.5f; @@ -2116,12 +1968,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub == nullptr) { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - - } else { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - } + _battery_pub.publish(hil_battery_status); } } @@ -2131,7 +1978,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) mavlink_hil_gps_t gps; mavlink_msg_hil_gps_decode(msg, &gps); - uint64_t timestamp = hrt_absolute_time(); + const uint64_t timestamp = hrt_absolute_time(); struct vehicle_gps_position_s hil_gps = {}; @@ -2160,57 +2007,41 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.heading = NAN; hil_gps.heading_offset = NAN; - if (_gps_pub == nullptr) { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); - - } else { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); - } + _gps_pub.publish(hil_gps); } void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) { mavlink_follow_target_t follow_target_msg; - follow_target_s follow_target_topic = {}; - mavlink_msg_follow_target_decode(msg, &follow_target_msg); - follow_target_topic.timestamp = hrt_absolute_time(); + follow_target_s follow_target_topic{}; + follow_target_topic.timestamp = hrt_absolute_time(); follow_target_topic.lat = follow_target_msg.lat * 1e-7; follow_target_topic.lon = follow_target_msg.lon * 1e-7; follow_target_topic.alt = follow_target_msg.alt; - if (_follow_target_pub == nullptr) { - _follow_target_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic); - - } else { - orb_publish(ORB_ID(follow_target), _follow_target_pub, &follow_target_topic); - } + _follow_target_pub.publish(follow_target_topic); } void MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg) { mavlink_landing_target_t landing_target; - mavlink_msg_landing_target_decode(msg, &landing_target); if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) { - landing_target_pose_s landing_target_pose = {}; + landing_target_pose_s landing_target_pose{}; + landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(landing_target.time_usec); landing_target_pose.abs_pos_valid = true; landing_target_pose.x_abs = landing_target.x; landing_target_pose.y_abs = landing_target.y; landing_target_pose.z_abs = landing_target.z; - if (_landing_target_pose_pub == nullptr) { - _landing_target_pose_pub = orb_advertise(ORB_ID(landing_target_pose), &landing_target_pose); - - } else { - orb_publish(ORB_ID(landing_target_pose), _landing_target_pose_pub, &landing_target_pose); - } + _landing_target_pose_pub.publish(landing_target_pose); } } @@ -2218,10 +2049,10 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) { mavlink_adsb_vehicle_t adsb; - transponder_report_s t = {}; - mavlink_msg_adsb_vehicle_decode(msg, &adsb); + transponder_report_s t{}; + t.timestamp = hrt_absolute_time(); t.icao_address = adsb.ICAO_address; @@ -2259,9 +2090,7 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) void MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) { - mavlink_utm_global_position_t utm_pos{}; - transponder_report_s t{}; - + mavlink_utm_global_position_t utm_pos; mavlink_msg_utm_global_position_decode(msg, &utm_pos); // Convert cm/s to m/s @@ -2269,6 +2098,7 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) float vy = utm_pos.vy / 100.0f; float vz = utm_pos.vz / 100.0f; + transponder_report_s t{}; t.timestamp = hrt_absolute_time(); // TODO: ID t.lat = utm_pos.lat * 1e-7; @@ -2326,34 +2156,30 @@ void MavlinkReceiver::handle_message_collision(mavlink_message_t *msg) { mavlink_collision_t collision; - collision_report_s t = {}; - mavlink_msg_collision_decode(msg, &collision); - t.timestamp = hrt_absolute_time(); - t.src = collision.src; - t.id = collision.id; - t.action = collision.action; - t.threat_level = collision.threat_level; - t.time_to_minimum_delta = collision.time_to_minimum_delta; - t.altitude_minimum_delta = collision.altitude_minimum_delta; - t.horizontal_minimum_delta = collision.horizontal_minimum_delta; + collision_report_s collision_report{}; - if (_collision_report_pub == nullptr) { - _collision_report_pub = orb_advertise(ORB_ID(collision_report), &t); + collision_report.timestamp = hrt_absolute_time(); + collision_report.src = collision.src; + collision_report.id = collision.id; + collision_report.action = collision.action; + collision_report.threat_level = collision.threat_level; + collision_report.time_to_minimum_delta = collision.time_to_minimum_delta; + collision_report.altitude_minimum_delta = collision.altitude_minimum_delta; + collision_report.horizontal_minimum_delta = collision.horizontal_minimum_delta; - } else { - orb_publish(ORB_ID(collision_report), _collision_report_pub, &t); - } + _collision_report_pub.publish(collision_report); } void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg) { - mavlink_gps_rtcm_data_t gps_rtcm_data_msg = {}; + mavlink_gps_rtcm_data_t gps_rtcm_data_msg; mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg); - gps_inject_data_s gps_inject_data_topic = {}; + gps_inject_data_s gps_inject_data_topic{}; + gps_inject_data_topic.len = math::min((int)sizeof(gps_rtcm_data_msg.data), (int)sizeof(uint8_t) * gps_rtcm_data_msg.len); gps_inject_data_topic.flags = gps_rtcm_data_msg.flags; @@ -2369,44 +2195,34 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) mavlink_hil_state_quaternion_t hil_state; mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); - uint64_t timestamp = hrt_absolute_time(); + const uint64_t timestamp = hrt_absolute_time(); /* airspeed */ { - struct airspeed_s airspeed = {}; + airspeed_s airspeed{}; airspeed.timestamp = timestamp; airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - if (_airspeed_pub == nullptr) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } + _airspeed_pub.publish(airspeed); } /* attitude */ - struct vehicle_attitude_s hil_attitude; { - hil_attitude = {}; + vehicle_attitude_s hil_attitude{}; + hil_attitude.timestamp = timestamp; matrix::Quatf q(hil_state.attitude_quaternion); q.copyTo(hil_attitude.q); - if (_attitude_pub == nullptr) { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); - - } else { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); - } + _attitude_pub.publish(hil_attitude); } /* global position */ { - struct vehicle_global_position_s hil_global_pos = {}; + vehicle_global_position_s hil_global_pos{}; hil_global_pos.timestamp = timestamp; hil_global_pos.lat = hil_state.lat / ((double)1e7); @@ -2418,12 +2234,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.eph = 2.0f; hil_global_pos.epv = 4.0f; - if (_global_pos_pub == nullptr) { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - - } else { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); - } + _global_pos_pub.publish(hil_global_pos); } /* local position */ @@ -2434,64 +2245,47 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; + map_projection_init(&_hil_local_proj_ref, lat, lon); - _hil_local_pos.ref_timestamp = timestamp; - _hil_local_pos.ref_lat = lat; - _hil_local_pos.ref_lon = lon; - _hil_local_pos.ref_alt = _hil_local_alt0; } float x = 0.0f; float y = 0.0f; map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); - _hil_local_pos.timestamp = timestamp; - _hil_local_pos.xy_valid = true; - _hil_local_pos.z_valid = true; - _hil_local_pos.v_xy_valid = true; - _hil_local_pos.v_z_valid = true; - _hil_local_pos.x = x; - _hil_local_pos.y = y; - _hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; - _hil_local_pos.vx = hil_state.vx / 100.0f; - _hil_local_pos.vy = hil_state.vy / 100.0f; - _hil_local_pos.vz = hil_state.vz / 100.0f; - matrix::Eulerf euler = matrix::Quatf(hil_attitude.q); - _hil_local_pos.yaw = euler.psi(); - _hil_local_pos.xy_global = true; - _hil_local_pos.z_global = true; - _hil_local_pos.vxy_max = INFINITY; - _hil_local_pos.vz_max = INFINITY; - _hil_local_pos.hagl_min = INFINITY; - _hil_local_pos.hagl_max = INFINITY; - - if (_local_pos_pub == nullptr) { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_hil_local_pos); - } else { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_hil_local_pos); - } - } - - /* land detector */ - { - bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - - if (_hil_land_detector.landed != landed) { - _hil_land_detector.landed = landed; - _hil_land_detector.timestamp = hrt_absolute_time(); - - if (_land_detector_pub == nullptr) { - _land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &_hil_land_detector); - - } else { - orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &_hil_land_detector); - } - } + vehicle_local_position_s hil_local_pos{}; + hil_local_pos.timestamp = timestamp; + + hil_local_pos.ref_timestamp = _hil_local_proj_ref.timestamp; + hil_local_pos.ref_lat = math::radians(_hil_local_proj_ref.lat_rad); + hil_local_pos.ref_lon = math::radians(_hil_local_proj_ref.lon_rad); + hil_local_pos.ref_alt = _hil_local_alt0; + hil_local_pos.xy_valid = true; + hil_local_pos.z_valid = true; + hil_local_pos.v_xy_valid = true; + hil_local_pos.v_z_valid = true; + hil_local_pos.x = x; + hil_local_pos.y = y; + hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; + hil_local_pos.vx = hil_state.vx / 100.0f; + hil_local_pos.vy = hil_state.vy / 100.0f; + hil_local_pos.vz = hil_state.vz / 100.0f; + + matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)}; + hil_local_pos.yaw = euler.psi(); + hil_local_pos.xy_global = true; + hil_local_pos.z_global = true; + hil_local_pos.vxy_max = INFINITY; + hil_local_pos.vz_max = INFINITY; + hil_local_pos.hagl_min = INFINITY; + hil_local_pos.hagl_max = INFINITY; + + _local_pos_pub.publish(hil_local_pos); } /* accelerometer */ { - sensor_accel_s accel = {}; + sensor_accel_s accel{}; accel.timestamp = timestamp; accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f; @@ -2502,17 +2296,12 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) accel.z = hil_state.zacc; accel.temperature = 25.0f; - if (_accel_pub == nullptr) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } + _accel_pub.publish(accel); } /* gyroscope */ { - sensor_gyro_s gyro = {}; + sensor_gyro_s gyro{}; gyro.timestamp = timestamp; gyro.x_raw = hil_state.rollspeed * 1e3f; @@ -2523,17 +2312,12 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) gyro.z = hil_state.yawspeed; gyro.temperature = 25.0f; - if (_gyro_pub == nullptr) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - - } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); - } + _gyro_pub.publish(gyro); } /* battery status */ { - struct battery_status_s hil_battery_status = {}; + battery_status_s hil_battery_status{}; hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.1f; @@ -2541,12 +2325,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub == nullptr) { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - - } else { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - } + _battery_pub.publish(hil_battery_status); } } @@ -2554,51 +2333,41 @@ void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg) { mavlink_named_value_float_t debug_msg; - debug_key_value_s debug_topic; - mavlink_msg_named_value_float_decode(msg, &debug_msg); + debug_key_value_s debug_topic{}; + debug_topic.timestamp = hrt_absolute_time(); memcpy(debug_topic.key, debug_msg.name, sizeof(debug_topic.key)); debug_topic.key[sizeof(debug_topic.key) - 1] = '\0'; // enforce null termination debug_topic.value = debug_msg.value; - if (_debug_key_value_pub == nullptr) { - _debug_key_value_pub = orb_advertise(ORB_ID(debug_key_value), &debug_topic); - - } else { - orb_publish(ORB_ID(debug_key_value), _debug_key_value_pub, &debug_topic); - } + _debug_key_value_pub.publish(debug_topic); } void MavlinkReceiver::handle_message_debug(mavlink_message_t *msg) { mavlink_debug_t debug_msg; - debug_value_s debug_topic; - mavlink_msg_debug_decode(msg, &debug_msg); + debug_value_s debug_topic{}; + debug_topic.timestamp = hrt_absolute_time(); debug_topic.ind = debug_msg.ind; debug_topic.value = debug_msg.value; - if (_debug_value_pub == nullptr) { - _debug_value_pub = orb_advertise(ORB_ID(debug_value), &debug_topic); - - } else { - orb_publish(ORB_ID(debug_value), _debug_value_pub, &debug_topic); - } + _debug_value_pub.publish(debug_topic); } void MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg) { mavlink_debug_vect_t debug_msg; - debug_vect_s debug_topic; - mavlink_msg_debug_vect_decode(msg, &debug_msg); + debug_vect_s debug_topic{}; + debug_topic.timestamp = hrt_absolute_time(); memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name)); debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination @@ -2606,22 +2375,17 @@ MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg) debug_topic.y = debug_msg.y; debug_topic.z = debug_msg.z; - if (_debug_vect_pub == nullptr) { - _debug_vect_pub = orb_advertise(ORB_ID(debug_vect), &debug_topic); - - } else { - orb_publish(ORB_ID(debug_vect), _debug_vect_pub, &debug_topic); - } + _debug_vect_pub.publish(debug_topic); } void MavlinkReceiver::handle_message_debug_float_array(mavlink_message_t *msg) { mavlink_debug_float_array_t debug_msg; - debug_array_s debug_topic = {}; - mavlink_msg_debug_float_array_decode(msg, &debug_msg); + debug_array_s debug_topic{}; + debug_topic.timestamp = hrt_absolute_time(); debug_topic.id = debug_msg.array_id; memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name)); @@ -2631,21 +2395,15 @@ MavlinkReceiver::handle_message_debug_float_array(mavlink_message_t *msg) debug_topic.data[i] = debug_msg.data[i]; } - if (_debug_array_pub == nullptr) { - _debug_array_pub = orb_advertise(ORB_ID(debug_array), &debug_topic); - - } else { - orb_publish(ORB_ID(debug_array), _debug_array_pub, &debug_topic); - } + _debug_array_pub.publish(debug_topic); } /** * Receive data from UART/UDP */ -void * -MavlinkReceiver::receive_thread(void *arg) +void +MavlinkReceiver::Run() { - /* set thread name */ { char thread_name[24]; @@ -2737,18 +2495,21 @@ MavlinkReceiver::receive_thread(void *arg) // the system within the first N seconds hrt_abstime stime = _mavlink->get_start_time(); - if ((stime != 0 && (hrt_elapsed_time(&stime) > 3 * 1000 * 1000)) + if ((stime != 0 && (hrt_elapsed_time(&stime) > 3_s)) || (srcaddr_last.sin_addr.s_addr == htonl(localhost))) { + srcaddr_last.sin_addr.s_addr = srcaddr.sin_addr.s_addr; srcaddr_last.sin_port = srcaddr.sin_port; + _mavlink->set_client_source_initialized(); + PX4_INFO("partner IP: %s", inet_ntoa(srcaddr.sin_addr)); } } #endif - // only start accepting messages once we're sure who we talk to + // only start accepting messages once we're sure who we talk to if (_mavlink->get_client_source_initialized()) { /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { @@ -2810,26 +2571,15 @@ MavlinkReceiver::receive_thread(void *arg) } } - - return nullptr; } void * MavlinkReceiver::start_helper(void *context) { + MavlinkReceiver rcv{(Mavlink *)context}; + rcv.Run(); - MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); - - if (!rcv) { - PX4_ERR("alloc failed"); - return nullptr; - } - - void *ret = rcv->receive_thread(nullptr); - - delete rcv; - - return ret; + return nullptr; } void @@ -2843,7 +2593,9 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent) param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, PX4_STACK_ADJUSTED(2840 + MAVLINK_RECEIVER_NET_ADDED_STACK)); + pthread_attr_setstacksize(&receiveloop_attr, + PX4_STACK_ADJUSTED(sizeof(MavlinkReceiver) + 2840 + MAVLINK_RECEIVER_NET_ADDED_STACK)); + pthread_create(thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); pthread_attr_destroy(&receiveloop_attr); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 08144cb427e5..21de7bb057f2 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -41,13 +41,21 @@ #pragma once -#include +#include "mavlink_ftp.h" +#include "mavlink_log_handler.h" +#include "mavlink_mission.h" +#include "mavlink_parameters.h" +#include "mavlink_timesync.h" +#include +#include #include -#include +#include #include #include #include +#include +#include #include #include #include @@ -58,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -65,8 +74,13 @@ #include #include #include +#include #include +#include +#include #include +#include +#include #include #include #include @@ -74,8 +88,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -84,20 +98,11 @@ #include #include -#include "mavlink_ftp.h" -#include "mavlink_log_handler.h" -#include "mavlink_mission.h" -#include "mavlink_parameters.h" -#include "mavlink_timesync.h" - class Mavlink; class MavlinkReceiver : public ModuleParams { public: - /** - * Constructor - */ MavlinkReceiver(Mavlink *parent); ~MavlinkReceiver() = default; @@ -120,8 +125,8 @@ class MavlinkReceiver : public ModuleParams const vehicle_command_s &vehicle_command); void handle_message(mavlink_message_t *msg); + void handle_message_adsb_vehicle(mavlink_message_t *msg); - void handle_message_utm_global_position(mavlink_message_t *msg); void handle_message_att_pos_mocap(mavlink_message_t *msg); void handle_message_battery_status(mavlink_message_t *msg); void handle_message_collision(mavlink_message_t *msg); @@ -157,9 +162,11 @@ class MavlinkReceiver : public ModuleParams void handle_message_set_mode(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); + void handle_message_utm_global_position(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); - void *receive_thread(void *arg); + + void Run(); /** * Set the interval at which the given message stream is published. @@ -187,7 +194,6 @@ class MavlinkReceiver : public ModuleParams bool evaluate_target_ok(int command, int target_system, int target_component); void send_flight_information(); - void send_storage_information(int storage_id); /** @@ -195,96 +201,87 @@ class MavlinkReceiver : public ModuleParams */ void update_params(); - Mavlink *_mavlink; + Mavlink *_mavlink; MavlinkFTP _mavlink_ftp; MavlinkLogHandler _mavlink_log_handler; - MavlinkTimesync _mavlink_timesync; - MavlinkMissionManager _mission_manager; MavlinkParametersManager _parameters_manager; + MavlinkTimesync _mavlink_timesync; - mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char() - - map_projection_reference_s _hil_local_proj_ref {}; - offboard_control_mode_s _offboard_control_mode{}; - - vehicle_attitude_s _att {}; - vehicle_local_position_s _hil_local_pos {}; - vehicle_land_detected_s _hil_land_detector {}; - vehicle_control_mode_s _control_mode {}; - - uORB::PublicationQueued _gps_inject_data_pub{ORB_ID(gps_inject_data)}; - uORB::PublicationQueued _transponder_report_pub{ORB_ID(transponder_report)}; - uORB::PublicationQueued _cmd_pub{ORB_ID(vehicle_command)}; - uORB::PublicationQueued _cmd_ack_pub{ORB_ID(vehicle_command_ack)}; - - orb_advert_t _accel_pub{nullptr}; - orb_advert_t _actuator_controls_pubs[4] {nullptr, nullptr, nullptr, nullptr}; - orb_advert_t _airspeed_pub{nullptr}; - orb_advert_t _att_sp_pub{nullptr}; - orb_advert_t _attitude_pub{nullptr}; - orb_advert_t _baro_pub{nullptr}; - orb_advert_t _battery_pub{nullptr}; - orb_advert_t _collision_report_pub{nullptr}; - orb_advert_t _debug_array_pub{nullptr}; - orb_advert_t _debug_key_value_pub{nullptr}; - orb_advert_t _debug_value_pub{nullptr}; - orb_advert_t _debug_vect_pub{nullptr}; - orb_advert_t _distance_sensor_pub{nullptr}; - orb_advert_t _flow_distance_sensor_pub{nullptr}; - orb_advert_t _flow_pub{nullptr}; - orb_advert_t _follow_target_pub{nullptr}; - orb_advert_t _global_pos_pub{nullptr}; - orb_advert_t _gps_pub{nullptr}; - orb_advert_t _gyro_pub{nullptr}; - orb_advert_t _hil_distance_sensor_pub{nullptr}; - orb_advert_t _land_detector_pub{nullptr}; - orb_advert_t _landing_target_pose_pub{nullptr}; - orb_advert_t _local_pos_pub{nullptr}; - orb_advert_t _mag_pub{nullptr}; - orb_advert_t _manual_pub{nullptr}; - orb_advert_t _mocap_odometry_pub{nullptr}; - orb_advert_t _obstacle_distance_pub{nullptr}; - orb_advert_t _offboard_control_mode_pub{nullptr}; - orb_advert_t _ping_pub{nullptr}; - orb_advert_t _pos_sp_triplet_pub{nullptr}; - orb_advert_t _radio_status_pub{nullptr}; - orb_advert_t _rates_sp_pub{nullptr}; - orb_advert_t _rc_pub{nullptr}; - orb_advert_t _trajectory_waypoint_pub{nullptr}; - orb_advert_t _visual_odometry_pub{nullptr}; - - uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; - uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - - static constexpr unsigned int MOM_SWITCH_COUNT{8}; - - int _orb_class_instance{-1}; - - uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; - - uint16_t _mom_switch_state{0}; - - uint64_t _global_ref_timestamp{0}; - - float _hil_local_alt0{0.0f}; - - bool _hil_local_proj_inited{false}; - - uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; - - hrt_abstime _last_utm_global_pos_com{0}; + mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char() + + // ORB publications + uORB::Publication _actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)}; + uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; + uORB::Publication _battery_pub{ORB_ID(battery_status)}; + uORB::Publication _collision_report_pub{ORB_ID(collision_report)}; + uORB::Publication _debug_array_pub{ORB_ID(debug_array)}; + uORB::Publication _debug_key_value_pub{ORB_ID(debug_key_value)}; + uORB::Publication _debug_value_pub{ORB_ID(debug_value)}; + uORB::Publication _debug_vect_pub{ORB_ID(debug_vect)}; + uORB::Publication _follow_target_pub{ORB_ID(follow_target)}; + uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; + uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; + uORB::Publication _flow_pub{ORB_ID(optical_flow)}; + uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; + uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; + uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _global_pos_pub{ORB_ID(vehicle_global_position)}; + uORB::Publication _gps_pub{ORB_ID(vehicle_gps_position)}; + uORB::Publication _land_detector_pub{ORB_ID(vehicle_land_detected)}; + uORB::Publication _local_pos_pub{ORB_ID(vehicle_local_position)}; + uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; + uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; + + // ORB publications (multi) + uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; + uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; + uORB::PublicationMulti _rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW}; + uORB::PublicationMulti _manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW}; + uORB::PublicationMulti _ping_pub{ORB_ID(ping), ORB_PRIO_LOW}; + uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW}; + uORB::PublicationMulti _accel_pub{ORB_ID(sensor_accel), ORB_PRIO_LOW}; + uORB::PublicationMulti _baro_pub{ORB_ID(sensor_baro), ORB_PRIO_LOW}; + uORB::PublicationMulti _gyro_pub{ORB_ID(sensor_gyro), ORB_PRIO_LOW}; + uORB::PublicationMulti _mag_pub{ORB_ID(sensor_mag), ORB_PRIO_LOW}; + + // ORB publications (queue length > 1) + uORB::PublicationQueued _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::PublicationQueued _transponder_report_pub{ORB_ID(transponder_report)}; + uORB::PublicationQueued _cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationQueued _cmd_pub{ORB_ID(vehicle_command)}; + + // ORB subscriptions + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + + + static constexpr unsigned int MOM_SWITCH_COUNT{8}; + uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; + uint16_t _mom_switch_state{0}; + + uint64_t _global_ref_timestamp{0}; + + map_projection_reference_s _hil_local_proj_ref{}; + float _hil_local_alt0{0.0f}; + bool _hil_local_proj_inited{false}; + + hrt_abstime _last_utm_global_pos_com{0}; DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, (ParamFloat) _param_bat_low_thr, + (ParamInt) _param_com_flight_uuid, (ParamFloat) _param_sens_flow_maxhgt, (ParamFloat) _param_sens_flow_maxr, (ParamFloat) _param_sens_flow_minhgt, - (ParamInt) _param_com_flight_uuid, (ParamInt) _param_sens_flow_rot ); diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 4965901da24d..e871c16c155e 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -155,7 +156,7 @@ class MulticopterAttitudeControl : public ModuleBase uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; - uORB::Publication _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index 6b3f4f0f818c..38ca2f9f5f0d 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -41,14 +41,30 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat MAIN uorb STACK_MAIN 2100 SRCS + ORBSet.hpp + Publication.hpp + PublicationMulti.hpp + PublicationQueued.hpp Subscription.cpp + Subscription.hpp + SubscriptionCallback.hpp + SubscriptionInterval.hpp SubscriptionPollable.cpp + SubscriptionPollable.hpp uORB.cpp + uORB.h + uORBCommon.hpp + uORBCommunicator.hpp uORBDeviceMaster.cpp + uORBDeviceMaster.hpp uORBDeviceNode.cpp + uORBDeviceNode.hpp uORBMain.cpp uORBManager.cpp + uORBManager.hpp + uORBTopics.h uORBUtils.cpp + uORBUtils.hpp DEPENDS cdev uorb_msgs diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index debdce3f4470..eac2a8f52d00 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -38,8 +38,9 @@ #pragma once -#include +#include #include +#include namespace uORB { @@ -55,13 +56,9 @@ class Publication /** * Constructor * - * @param meta The uORB metadata (usually from - * the ORB_ID() macro) for the topic. - * @param priority The priority for multi pub/sub, 0-based, -1 means - * don't publish as multi + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. */ - Publication(const orb_metadata *meta, uint8_t priority = 0) : _meta(meta), _priority(priority) {} - + Publication(const orb_metadata *meta) : _meta(meta) {} ~Publication() { orb_unadvertise(_handle); } /** @@ -70,44 +67,29 @@ class Publication */ bool publish(const T &data) { - bool updated = false; - if (_handle != nullptr) { - updated = (orb_publish(_meta, _handle, &data) == PX4_OK); + return (orb_publish(_meta, _handle, &data) == PX4_OK); } else { - orb_advert_t handle = nullptr; - - if (_priority > 0) { - int instance; - handle = orb_advertise_multi(_meta, &data, &instance, _priority); - - } else { - handle = orb_advertise(_meta, &data); - } + orb_advert_t handle = orb_advertise(_meta, &data); if (handle != nullptr) { _handle = handle; - updated = true; - - } else { - PX4_ERR("%s advert fail", _meta->o_name); + return true; } } - return updated; + return false; } protected: const orb_metadata *_meta; orb_advert_t _handle{nullptr}; - - const uint8_t _priority; }; /** - * The publication class with data. + * The publication class with data embedded. */ template class PublicationData : public Publication @@ -116,11 +98,9 @@ class PublicationData : public Publication /** * Constructor * - * @param meta The uORB metadata (usually from - * the ORB_ID() macro) for the topic. - * @param priority The priority for multi pub, 0-based. + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. */ - PublicationData(const orb_metadata *meta, int priority = -1) : Publication(meta, priority) {} + PublicationData(const orb_metadata *meta) : Publication(meta) {} ~PublicationData() = default; T &get() { return _data; } diff --git a/src/modules/uORB/PublicationMulti.hpp b/src/modules/uORB/PublicationMulti.hpp new file mode 100644 index 000000000000..f00acfa753ca --- /dev/null +++ b/src/modules/uORB/PublicationMulti.hpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 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 Publication.hpp + * + */ + +#pragma once + +#include +#include +#include + +namespace uORB +{ + +/** + * Base publication multi wrapper class + */ +template +class PublicationMulti +{ +public: + + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub/sub, 0 means don't publish as multi + */ + PublicationMulti(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) : + _meta(meta), + _priority(priority) + {} + + ~PublicationMulti() { orb_unadvertise(_handle); } + + /** + * Publish the struct + * @param data The uORB message struct we are updating. + */ + bool publish(const T &data) + { + if (_handle != nullptr) { + return (orb_publish(_meta, _handle, &data) == PX4_OK); + + } else { + int instance = 0; + orb_advert_t handle = orb_advertise_multi(_meta, &data, &instance, _priority); + + if (handle != nullptr) { + _handle = handle; + return true; + } + } + + return false; + } + +protected: + const orb_metadata *_meta; + + orb_advert_t _handle{nullptr}; + + const uint8_t _priority; +}; + +/** + * The publication multi class with data embedded. + */ +template +class PublicationMultiData : public PublicationMulti +{ +public: + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub + */ + PublicationMultiData(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) : + PublicationMulti(meta, priority) + {} + + ~PublicationMultiData() = default; + + T &get() { return _data; } + void set(const T &data) { _data = data; } + + // Publishes the embedded struct. + bool update() { return PublicationMulti::publish(_data); } + bool update(const T &data) + { + _data = data; + return PublicationMulti::publish(_data); + } + +private: + T _data{}; +}; + +} // namespace uORB diff --git a/src/modules/uORB/PublicationQueued.hpp b/src/modules/uORB/PublicationQueued.hpp index d3db3331069a..c8080878df9e 100644 --- a/src/modules/uORB/PublicationQueued.hpp +++ b/src/modules/uORB/PublicationQueued.hpp @@ -38,8 +38,9 @@ #pragma once -#include #include +#include +#include namespace uORB { @@ -55,11 +56,9 @@ class PublicationQueued /** * Constructor * - * @param meta The uORB metadata (usually from - * the ORB_ID() macro) for the topic. + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. */ PublicationQueued(const orb_metadata *meta) : _meta(meta) {} - ~PublicationQueued() { orb_unadvertise(_handle); } /** @@ -68,24 +67,19 @@ class PublicationQueued */ bool publish(const T &data) { - bool updated = false; - if (_handle != nullptr) { - updated = (orb_publish(_meta, _handle, &data) == PX4_OK); + return (orb_publish(_meta, _handle, &data) == PX4_OK); } else { orb_advert_t handle = orb_advertise_queue(_meta, &data, T::ORB_QUEUE_LENGTH); if (handle != nullptr) { _handle = handle; - updated = true; - - } else { - PX4_ERR("%s advertise failed", _meta->o_name); + return true; } } - return updated; + return false; } protected: