aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink_orb_listener.cpp38
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp18
-rw-r--r--src/modules/mavlink/mavlink_receiver.h3
3 files changed, 26 insertions, 33 deletions
diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp
index e2675dfa7..44bf77bb0 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_orb_listener.cpp
@@ -59,7 +59,6 @@
#include "mavlink_orb_listener.h"
#include "mavlink_main.h"
-//#include "util.h"
void *uorb_receive_thread(void *arg);
@@ -200,10 +199,10 @@ MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
hrt_abstime t = hrt_absolute_time();
if (t >= last_sent_vfr + 100000) {
last_sent_vfr = t;
- float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
+ float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
- mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
+ mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d);
}
/* send quaternion values if it exists */
@@ -333,13 +332,13 @@ MavlinkOrbListener::l_global_position(const struct listener *l)
mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(),
global_pos.timestamp / 1000,
- global_pos.lat,
- global_pos.lon,
+ global_pos.lat * 1e7,
+ global_pos.lon * 1e7,
global_pos.alt * 1000.0f,
- global_pos.relative_alt * 1000.0f,
- global_pos.vx * 100.0f,
- global_pos.vy * 100.0f,
- global_pos.vz * 100.0f,
+ (global_pos.alt - home.alt) * 1000.0f,
+ global_pos.vel_n * 100.0f,
+ global_pos.vel_e * 100.0f,
+ global_pos.vel_d * 100.0f,
_wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
@@ -363,23 +362,18 @@ MavlinkOrbListener::l_local_position(const struct listener *l)
void
MavlinkOrbListener::l_global_position_setpoint(const struct listener *l)
{
- struct mission_item_triplet_s triplet;
- orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
+ struct position_setpoint_triplet_s triplet;
+ orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet);
- uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
-
- if (!triplet.current_valid)
+ if (!triplet.current.valid)
return;
- if (triplet.current.altitude_is_relative)
- coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
-
if (gcs_link)
mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(),
- coordinate_frame,
+ MAV_FRAME_GLOBAL,
(int32_t)(triplet.current.lat * 1e7d),
(int32_t)(triplet.current.lon * 1e7d),
- (int32_t)(triplet.current.altitude * 1e3f),
+ (int32_t)(triplet.current.alt * 1e3f),
(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
}
@@ -615,11 +609,9 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l)
void
MavlinkOrbListener::l_home(const struct listener *l)
{
- struct home_position_s home;
-
orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
- mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f);
+ mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
}
void
@@ -757,7 +749,7 @@ MavlinkOrbListener::uorb_receive_start(void)
orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */
/* --- GLOBAL SETPOINT VALUE --- */
- mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
+ mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */
/* --- LOCAL SETPOINT VALUE --- */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index ab4074558..4f763c3c6 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -555,6 +555,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_gps.alt = gps.alt;
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
+ hil_gps.timestamp_variance = gps.time_usec;
hil_gps.s_variance_m_s = 5.0f;
hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
@@ -566,6 +567,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
if (heading_rad > M_PI_F)
heading_rad -= 2.0f * M_PI_F;
+ hil_gps.timestamp_velocity = gps.time_usec;
hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
@@ -613,9 +615,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
- hil_global_pos.vx = hil_state.vx / 100.0f;
- hil_global_pos.vy = hil_state.vy / 100.0f;
- hil_global_pos.vz = hil_state.vz / 100.0f;
+ 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;
} else {
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
@@ -659,8 +661,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
- math::Dcm C_nb(q);
- math::EulerAngles euler(C_nb);
+ math::Matrix<3,3> C_nb = q.to_dcm();
+ math::Vector<3> euler = C_nb.to_euler();
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
@@ -675,9 +677,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_attitude.q[3] = q(3);
hil_attitude.q_valid = true;
- hil_attitude.roll = euler.getPhi();
- hil_attitude.pitch = euler.getTheta();
- hil_attitude.yaw = euler.getPsi();
+ 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;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 556b6f8ad..ea57714d2 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -53,9 +53,8 @@
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-20 15:19:09 +0000