fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 21:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: move VFR_HUD to separate stream header

Browse Source
This commit is contained in:
Daniel Agar
2021-03-11 11:46:14 -05:00
parent d46ae7983f
commit c6311324d2
2 changed files with 140 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+3 -118
View File
@@ -138,6 +138,7 @@ using matrix::wrap_2pi;
#include "streams/SYSTEM_TIME.hpp"
#include "streams/TIMESYNC.hpp"
#include "streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp"
#include "streams/VFR_HUD.hpp"
#include "streams/VIBRATION.hpp"
#include "streams/WIND_COV.hpp"
@@ -1212,124 +1213,6 @@ public:
}
};
class MavlinkStreamVFRHUD : public MavlinkStream
{
public:
const char *get_name() const override
{
return MavlinkStreamVFRHUD::get_name_static();
}
static constexpr const char *get_name_static()
{
return "VFR_HUD";
}
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_VFR_HUD;
}
uint16_t get_id() override
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamVFRHUD(mavlink);
}
unsigned get_size() override
{
if (_lpos_sub.advertised() || _airspeed_validated_sub.advertised()) {
return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
return 0;
}
private:
uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _act0_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _act1_sub{ORB_ID(actuator_controls_1)};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)};
/* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &) = delete;
MavlinkStreamVFRHUD &operator = (const MavlinkStreamVFRHUD &) = delete;
protected:
explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
bool send() override
{
if (_lpos_sub.updated() || _airspeed_validated_sub.updated()) {
vehicle_local_position_s lpos{};
_lpos_sub.copy(&lpos);
actuator_armed_s armed{};
_armed_sub.copy(&armed);
airspeed_validated_s airspeed_validated{};
_airspeed_validated_sub.copy(&airspeed_validated);
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));
if (armed.armed) {
actuator_controls_s act0{};
actuator_controls_s act1{};
_act0_sub.copy(&act0);
_act1_sub.copy(&act1);
// VFR_HUD throttle should only be used for operator feedback.
// VTOLs switch between actuator_controls_0 and actuator_controls_1. During transition there isn't a
// a single throttle value, but this should still be a useful heuristic for operator awareness.
//
// Use ACTUATOR_CONTROL_TARGET if accurate states are needed.
msg.throttle = 100 * math::max(
act0.control[actuator_controls_s::INDEX_THROTTLE],
act1.control[actuator_controls_s::INDEX_THROTTLE]);
} else {
msg.throttle = 0.0f;
}
if (lpos.z_valid && lpos.z_global) {
/* use local position estimate */
msg.alt = -lpos.z + lpos.ref_alt;
} else {
vehicle_air_data_s air_data{};
_air_data_sub.copy(&air_data);
/* fall back to baro altitude */
if (air_data.timestamp > 0) {
msg.alt = air_data.baro_alt_meter;
}
}
if (lpos.v_z_valid) {
msg.climb = -lpos.vz;
}
mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamCameraTrigger : public MavlinkStream
{
public:
@@ -1538,7 +1421,9 @@ static const StreamListItem streams_list[] = {
#if defined(ATTITUDE_QUATERNION_HPP)
create_stream_list_item<MavlinkStreamAttitudeQuaternion>(),
#endif // ATTITUDE_QUATERNION_HPP
#if defined(VFR_HUD_HPP)
create_stream_list_item<MavlinkStreamVFRHUD>(),
#endif // VFR_HUD_HPP
#if defined(GPS_GLOBAL_ORIGIN_HPP)
create_stream_list_item<MavlinkStreamGpsGlobalOrigin>(),
#endif // GPS_GLOBAL_ORIGIN_HPP