diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1f91fe0eca..39e1f8d36a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -54,44 +54,7 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include - -using matrix::Vector3f; -using matrix::wrap_2pi; #include "streams/ACTUATOR_CONTROL_TARGET.hpp" #include "streams/ACTUATOR_OUTPUT_STATUS.hpp" @@ -139,8 +102,8 @@ using matrix::wrap_2pi; #include "streams/SERVO_OUTPUT_RAW.hpp" #include "streams/STATUSTEXT.hpp" #include "streams/STORAGE_INFORMATION.hpp" -#include "streams/SYSTEM_TIME.hpp" #include "streams/SYS_STATUS.hpp" +#include "streams/SYSTEM_TIME.hpp" #include "streams/TIMESYNC.hpp" #include "streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp" #include "streams/VFR_HUD.hpp" diff --git a/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp b/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp index 588b5f052e..5c5cfbdefd 100644 --- a/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp +++ b/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp @@ -34,50 +34,29 @@ #ifndef ACTUATOR_OUTPUT_STATUS_HPP #define ACTUATOR_OUTPUT_STATUS_HPP +#include + class MavlinkStreamActuatorOutputStatus : public MavlinkStream { public: - const char *get_name() const override - { - return MavlinkStreamActuatorOutputStatus::get_name_static(); - } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamActuatorOutputStatus(mavlink); } - static constexpr const char *get_name_static() - { - return "ACTUATOR_OUTPUT_STATUS"; - } + static constexpr const char *get_name_static() { return "ACTUATOR_OUTPUT_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS; } - static constexpr uint16_t get_id_static() - { - return MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS; - } - - uint16_t get_id() override - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamActuatorOutputStatus(mavlink); - } + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } unsigned get_size() override { - return (_act_output_sub.advertised()) ? (MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _act_output_sub.advertised() ? (MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: + explicit MavlinkStreamActuatorOutputStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + uORB::Subscription _act_output_sub{ORB_ID(actuator_outputs)}; - /* do not allow top copying this class */ - MavlinkStreamActuatorOutputStatus(MavlinkStreamActuatorOutputStatus &) = delete; - MavlinkStreamActuatorOutputStatus &operator = (const MavlinkStreamActuatorOutputStatus &) = delete; - -protected: - explicit MavlinkStreamActuatorOutputStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - {} - bool send() override { actuator_outputs_s act; diff --git a/src/modules/mavlink/streams/ALTITUDE.hpp b/src/modules/mavlink/streams/ALTITUDE.hpp index 45eb67c9f9..f3c437d0d9 100644 --- a/src/modules/mavlink/streams/ALTITUDE.hpp +++ b/src/modules/mavlink/streams/ALTITUDE.hpp @@ -34,6 +34,8 @@ #ifndef ALTITUDE_HPP #define ALTITUDE_HPP +#include +#include #include #include @@ -56,9 +58,9 @@ public: private: explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} - 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)}; + uORB::Subscription _home_sub{ORB_ID(home_position)}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; bool send() override { diff --git a/src/modules/mavlink/streams/ATTITUDE.hpp b/src/modules/mavlink/streams/ATTITUDE.hpp index e36d4b4e6f..94c1f4a422 100644 --- a/src/modules/mavlink/streams/ATTITUDE.hpp +++ b/src/modules/mavlink/streams/ATTITUDE.hpp @@ -40,30 +40,13 @@ class MavlinkStreamAttitude : public MavlinkStream { public: - const char *get_name() const override - { - return MavlinkStreamAttitude::get_name_static(); - } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitude(mavlink); } - static constexpr const char *get_name_static() - { - return "ATTITUDE"; - } + static constexpr const char *get_name_static() { return "ATTITUDE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ATTITUDE; } - static constexpr uint16_t get_id_static() - { - return MAVLINK_MSG_ID_ATTITUDE; - } - - uint16_t get_id() override - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamAttitude(mavlink); - } + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } unsigned get_size() override { @@ -71,18 +54,11 @@ public: } private: + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} + 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; - MavlinkStreamAttitude &operator = (const MavlinkStreamAttitude &) = delete; - - -protected: - explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) - {} - bool send() override { vehicle_attitude_s att; diff --git a/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp b/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp index f05387c1bc..64189419ad 100644 --- a/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp +++ b/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp @@ -41,30 +41,13 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream { public: - const char *get_name() const override - { - return MavlinkStreamAttitudeQuaternion::get_name_static(); - } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitudeQuaternion(mavlink); } - static constexpr const char *get_name_static() - { - return "ATTITUDE_QUATERNION"; - } + static constexpr const char *get_name_static() { return "ATTITUDE_QUATERNION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } - static constexpr uint16_t get_id_static() - { - return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; - } - - uint16_t get_id() override - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamAttitudeQuaternion(mavlink); - } + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } unsigned get_size() override { @@ -72,18 +55,12 @@ public: } private: + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) {} + 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) - {} - bool send() override { vehicle_attitude_s att; diff --git a/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp b/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp index 5b7886d5ab..55c262862b 100644 --- a/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp +++ b/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp @@ -40,30 +40,13 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream { public: - const char *get_name() const override - { - return MavlinkStreamAttitudeTarget::get_name_static(); - } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitudeTarget(mavlink); } - static constexpr const char *get_name_static() - { - return "ATTITUDE_TARGET"; - } + static constexpr const char *get_name_static() { return "ATTITUDE_TARGET"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ATTITUDE_TARGET; } - static constexpr uint16_t get_id_static() - { - return MAVLINK_MSG_ID_ATTITUDE_TARGET; - } - - uint16_t get_id() override - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamAttitudeTarget(mavlink); - } + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } unsigned get_size() override { @@ -71,23 +54,16 @@ public: } private: + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _att_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; - /* do not allow top copying this class */ - MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &) = delete; - MavlinkStreamAttitudeTarget &operator = (const MavlinkStreamAttitudeTarget &) = delete; - -protected: - explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink) - {} - bool send() override { vehicle_attitude_setpoint_s att_sp; if (_att_sp_sub.update(&att_sp)) { - mavlink_attitude_target_t msg{}; msg.time_boot_ms = att_sp.timestamp / 1000; @@ -100,7 +76,7 @@ protected: msg.body_pitch_rate = att_rates_sp.pitch; msg.body_yaw_rate = att_rates_sp.yaw; - msg.thrust = Vector3f(att_sp.thrust_body).norm(); + msg.thrust = matrix::Vector3f(att_sp.thrust_body).norm(); mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/COLLISION.hpp b/src/modules/mavlink/streams/COLLISION.hpp index 455ac683fa..a3469c62d9 100644 --- a/src/modules/mavlink/streams/COLLISION.hpp +++ b/src/modules/mavlink/streams/COLLISION.hpp @@ -39,30 +39,13 @@ class MavlinkStreamCollision : public MavlinkStream { public: - const char *get_name() const override - { - return MavlinkStreamCollision::get_name_static(); - } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCollision(mavlink); } - static constexpr const char *get_name_static() - { - return "COLLISION"; - } + static constexpr const char *get_name_static() { return "COLLISION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_COLLISION; } - static constexpr uint16_t get_id_static() - { - return MAVLINK_MSG_ID_COLLISION; - } - - uint16_t get_id() override - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamCollision(mavlink); - } + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } unsigned get_size() override { @@ -70,16 +53,10 @@ public: } private: + explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink) {} + 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) - {} - bool send() override { collision_report_s report; diff --git a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp index b9ac0fa8d8..6d2997fe71 100644 --- a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp +++ b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp @@ -110,7 +110,7 @@ private: msg.vy = lpos.vy * 100.0f; msg.vz = lpos.vz * 100.0f; - msg.hdg = math::degrees(wrap_2pi(lpos.heading)) * 100.0f; + msg.hdg = math::degrees(matrix::wrap_2pi(lpos.heading)) * 100.0f; mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/GPS2_RAW.hpp b/src/modules/mavlink/streams/GPS2_RAW.hpp index 59cb182bd5..754740780a 100644 --- a/src/modules/mavlink/streams/GPS2_RAW.hpp +++ b/src/modules/mavlink/streams/GPS2_RAW.hpp @@ -79,7 +79,7 @@ private: msg.vel = UINT16_MAX; // If unknown, set to: UINT16_MAX } - msg.cog = math::degrees(wrap_2pi(gps.cog_rad)) * 1e2f; + msg.cog = math::degrees(matrix::wrap_2pi(gps.cog_rad)) * 1e2f; msg.satellites_visible = gps.satellites_used; //msg.dgps_numch = // Number of DGPS satellites diff --git a/src/modules/mavlink/streams/GPS_RAW_INT.hpp b/src/modules/mavlink/streams/GPS_RAW_INT.hpp index 0e8f90b528..80c17eccf8 100644 --- a/src/modules/mavlink/streams/GPS_RAW_INT.hpp +++ b/src/modules/mavlink/streams/GPS_RAW_INT.hpp @@ -79,7 +79,7 @@ private: msg.vel = UINT16_MAX; // If unknown, set to: UINT16_MAX } - msg.cog = math::degrees(wrap_2pi(gps.cog_rad)) * 1e2f; + msg.cog = math::degrees(matrix::wrap_2pi(gps.cog_rad)) * 1e2f; msg.satellites_visible = gps.satellites_used; msg.alt_ellipsoid = gps.alt_ellipsoid; msg.h_acc = gps.eph * 1e3f; // position uncertainty in mm diff --git a/src/modules/mavlink/streams/GPS_STATUS.hpp b/src/modules/mavlink/streams/GPS_STATUS.hpp index f6700f066d..82d36f2eae 100644 --- a/src/modules/mavlink/streams/GPS_STATUS.hpp +++ b/src/modules/mavlink/streams/GPS_STATUS.hpp @@ -39,35 +39,27 @@ class MavlinkStreamGPSStatus : public MavlinkStream { public: - const char *get_name() const override { return MavlinkStreamGPSStatus::get_name_static(); } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSStatus(mavlink); } static constexpr const char *get_name_static() { return "GPS_STATUS"; } - static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS_STATUS; } + const char *get_name() const override { return MavlinkStreamGPSStatus::get_name_static(); } uint16_t get_id() override { return get_id_static(); } - static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSStatus(mavlink); } - unsigned get_size() override { - return _satellite_info_sub.advertised() ? MAVLINK_MSG_ID_GPS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _satellite_info_sub.advertised() ? (MAVLINK_MSG_ID_GPS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: + explicit MavlinkStreamGPSStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + uORB::Subscription _satellite_info_sub{ORB_ID(satellite_info)}; - // Disallow copy and move construction. - MavlinkStreamGPSStatus(MavlinkStreamGPSStatus &) = delete; - MavlinkStreamGPSStatus &operator = (const MavlinkStreamGPSStatus &) = delete; - -protected: - explicit MavlinkStreamGPSStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - {} - bool send() override { - satellite_info_s sat {}; + satellite_info_s sat; if (_satellite_info_sub.update(&sat)) { mavlink_gps_status_t msg{}; diff --git a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp index eb0184947d..ebbe35ea1f 100644 --- a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp +++ b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp @@ -39,52 +39,29 @@ class MavlinkStreamOpticalFlowRad : public MavlinkStream { public: - const char *get_name() const override - { - return MavlinkStreamOpticalFlowRad::get_name_static(); - } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpticalFlowRad(mavlink); } - static constexpr const char *get_name_static() - { - return "OPTICAL_FLOW_RAD"; - } + static constexpr const char *get_name_static() { return "OPTICAL_FLOW_RAD"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } - static constexpr uint16_t get_id_static() - { - return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; - } - - uint16_t get_id() override - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamOpticalFlowRad(mavlink); - } + const char *get_name() const override { return MavlinkStreamOpticalFlowRad::get_name_static(); } + uint16_t get_id() override { return get_id_static(); } unsigned get_size() override { - return _flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - uORB::Subscription _flow_sub{ORB_ID(optical_flow)}; + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {} - /* do not allow top copying this class */ - MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &) = delete; - MavlinkStreamOpticalFlowRad &operator = (const MavlinkStreamOpticalFlowRad &) = delete; - -protected: - explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) - {} + uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; bool send() override { optical_flow_s flow; - if (_flow_sub.update(&flow)) { + if (_optical_flow_sub.update(&flow)) { mavlink_optical_flow_rad_t msg{}; msg.time_usec = flow.timestamp; diff --git a/src/modules/mavlink/streams/VFR_HUD.hpp b/src/modules/mavlink/streams/VFR_HUD.hpp index 28d9ef28bc..b5a038ab5d 100644 --- a/src/modules/mavlink/streams/VFR_HUD.hpp +++ b/src/modules/mavlink/streams/VFR_HUD.hpp @@ -86,7 +86,7 @@ private: mavlink_vfr_hud_t msg{}; msg.airspeed = airspeed_validated.calibrated_airspeed_m_s; msg.groundspeed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy); - msg.heading = math::degrees(wrap_2pi(lpos.heading)); + msg.heading = math::degrees(matrix::wrap_2pi(lpos.heading)); if (armed.armed) { actuator_controls_s act0{};