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

adapted to new vehicle attitude message

Browse Source
This commit is contained in:
tumbili
2016-10-18 20:46:08 +02:00
committed by Lorenz Meier
parent 215bfaa377
commit 5e0e522903
16 changed files with 142 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+8 -6
View File
@@ -841,11 +841,12 @@ protected:
if (_att_sub->update(&_att_time, &att)) {
mavlink_attitude_t msg;
matrix::Quaternion<float> q(&att.q[0]);
matrix::Euler<float> euler(q);
msg.time_boot_ms = att.timestamp / 1000;
msg.roll = att.roll;
msg.pitch = att.pitch;
msg.yaw = att.yaw;
msg.roll = euler(0);
msg.pitch = euler(1);
msg.yaw = euler(2);
msg.rollspeed = att.rollspeed;
msg.pitchspeed = att.pitchspeed;
msg.yawspeed = att.yawspeed;
@@ -1014,10 +1015,11 @@ protected:
if (updated) {
mavlink_vfr_hud_t msg;
matrix::Quaternion<float> q(&att.q[0]);
matrix::Euler<float> euler(q);
msg.airspeed = airspeed.indicated_airspeed_m_s;
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
msg.heading = _wrap_2pi(euler(2)) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
if (_pos_time > 0) {
src/modules/mavlink/mavlink_receiver.cpp
+11 -8
View File
@@ -1920,8 +1920,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
math::Vector<3> euler = C_nb.to_euler();
hil_attitude.timestamp = timestamp;
memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R));
hil_attitude.R_valid = true;
//memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R));
//hil_attitude.R_valid = true;
hil_attitude.q[0] = q(0);
hil_attitude.q[1] = q(1);
@@ -1929,9 +1929,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_attitude.q[3] = q(3);
hil_attitude.q_valid = true;
hil_attitude.roll = euler(0);
hil_attitude.pitch = euler(1);
hil_attitude.yaw = euler(2);
//hil_attitude.roll = euler(0);
//hil_attitude.pitch = euler(1);
//hil_attitude.yaw = euler(2);
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
@@ -1948,7 +1948,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
{
struct vehicle_global_position_s hil_global_pos;
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
matrix::Quaternion<float> q(&hil_attitude.q[0]);
matrix::Euler<float> euler(q);
hil_global_pos.timestamp = timestamp;
hil_global_pos.lat = hil_state.lat / ((double)1e7);
hil_global_pos.lon = hil_state.lon / ((double)1e7);
@@ -1956,7 +1957,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_n = hil_state.vx / 100.0f;
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = hil_attitude.yaw;
hil_global_pos.yaw = euler(1);
hil_global_pos.eph = 2.0f;
hil_global_pos.epv = 4.0f;
@@ -1997,7 +1998,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
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;
hil_local_pos.yaw = hil_attitude.yaw;
matrix::Quaternion<float> q(&hil_attitude.q[0]);
matrix::Euler<float> euler(q);
hil_local_pos.yaw = euler(2);
hil_local_pos.xy_global = true;
hil_local_pos.z_global = true;