diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-28 15:13:14 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-28 15:13:14 +0100 |
commit | 9c355d280eb379c72df636c1d47e5b14ae3e3e6e (patch) | |
tree | e224a00b8bd506e2a78cad6886262c9968cb9bc5 /src/modules/mavlink | |
parent | ac77fe9c27d7253b01805ff94d3e0f8e21017709 (diff) | |
download | px4-firmware-9c355d280eb379c72df636c1d47e5b14ae3e3e6e.tar.gz px4-firmware-9c355d280eb379c72df636c1d47e5b14ae3e3e6e.tar.bz2 px4-firmware-9c355d280eb379c72df636c1d47e5b14ae3e3e6e.zip |
Merged beta into mavlink rework branch
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_orb_listener.cpp | 38 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 18 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 3 |
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> |