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

replace geo _wrap_pi with matrix::wrap_pi

Browse Source
This commit is contained in:
Daniel Agar
2018-06-12 09:00:52 +02:00
committed by Lorenz Meier
parent 518daa4a8d
commit d0bde9ab2a
14 changed files with 60 additions and 313 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_high_latency2.cpp
+6 -3
View File
@@ -40,6 +40,7 @@
#include "mavlink_high_latency2.h"
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <lib/ecl/geo/geo.h>
#include <commander/px4_custom_mode.h>
@@ -59,6 +60,8 @@
#include <uORB/topics/wind_estimate.h>
#include <uORB/uORB.h>
using matrix::wrap_2pi;
MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : MavlinkStream(mavlink),
_actuator_sub_0(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_actuator_time_0(0),
@@ -238,7 +241,7 @@ bool MavlinkStreamHighLatency2::write_attitude_sp(mavlink_high_latency2_t *msg)
const bool updated = _attitude_sp_sub->update(&_attitude_sp_time, &attitude_sp);
if (_attitude_sp_time > 0) {
msg->target_heading = static_cast<uint8_t>(math::degrees(_wrap_2pi(attitude_sp.yaw_body)) * 0.5f);
msg->target_heading = static_cast<uint8_t>(math::degrees(wrap_2pi(attitude_sp.yaw_body)) * 0.5f);
}
return updated;
@@ -330,7 +333,7 @@ bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *m
msg->altitude = altitude;
msg->heading = static_cast<uint8_t>(math::degrees(_wrap_2pi(global_pos.yaw)) * 0.5f);
msg->heading = static_cast<uint8_t>(math::degrees(wrap_2pi(global_pos.yaw)) * 0.5f);
}
return updated;
@@ -451,7 +454,7 @@ bool MavlinkStreamHighLatency2::write_wind_estimate(mavlink_high_latency2_t *msg
if (_wind_time > 0) {
msg->wind_heading = static_cast<uint8_t>(
math::degrees(_wrap_2pi(atan2f(wind.windspeed_east, wind.windspeed_north))) * 0.5f);
math::degrees(wrap_2pi(atan2f(wind.windspeed_east, wind.windspeed_north))) * 0.5f);
}
return updated;