fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-14 13:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: move ATTITUDE to streams hpp

Browse Source 
Signed-off-by: Antonio Sanjurjo Cortés <74329840+antonio-sc66@users.noreply.github.com>
This commit is contained in:
Antonio Sanjurjo Cortés
2020-11-29 20:26:07 -05:00
committed by Daniel Agar
parent 0bf622e211
commit 8ac2ada0e6
2 changed files with 118 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+3 -77
View File
@@ -106,6 +106,7 @@ using matrix::Vector3f;
using matrix::wrap_2pi;
#include "streams/ALTITUDE.hpp"
#include "streams/ATTITUDE.hpp"
#include "streams/AUTOPILOT_VERSION.hpp"
#include "streams/DISTANCE_SENSOR.hpp"
#include "streams/ESC_INFO.hpp"
@@ -1432,83 +1433,6 @@ protected:
}
};
class MavlinkStreamAttitude : public MavlinkStream
{
public:
const char *get_name() const override
{
return MavlinkStreamAttitude::get_name_static();
}
static constexpr const char *get_name_static()
{
return "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);
}
unsigned get_size() override
{
return _att_sub.advertised() ? MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
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;
if (_att_sub.update(&att)) {
vehicle_angular_velocity_s angular_velocity{};
_angular_velocity_sub.copy(&angular_velocity);
mavlink_attitude_t msg{};
const matrix::Eulerf euler = matrix::Quatf(att.q);
msg.time_boot_ms = att.timestamp / 1000;
msg.roll = euler.phi();
msg.pitch = euler.theta();
msg.yaw = euler.psi();
msg.rollspeed = angular_velocity.xyz[0];
msg.pitchspeed = angular_velocity.xyz[1];
msg.yawspeed = angular_velocity.xyz[2];
mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamAttitudeQuaternion : public MavlinkStream
{
public:
@@ -4225,7 +4149,9 @@ static const StreamListItem streams_list[] = {
create_stream_list_item<MavlinkStreamScaledPressure<0> >(),
// create_stream_list_item<MavlinkStreamScaledPressure<1> >(),
// create_stream_list_item<MavlinkStreamScaledPressure<2> >(),
#if defined(ATTITUDE_HPP)
create_stream_list_item<MavlinkStreamAttitude>(),
#endif // ATTITUDE_HPP
create_stream_list_item<MavlinkStreamAttitudeQuaternion>(),
create_stream_list_item<MavlinkStreamVFRHUD>(),
#if defined(GPS_GLOBAL_ORIGIN_HPP)