diff --git a/src/examples/bottle_drop/bottle_drop.cpp b/src/examples/bottle_drop/bottle_drop.cpp
index 6a444e921e4221d6ddfc503860406f50364d561f..e8f4dca07e0169625815087b2ab2ae385e6d1df9 100644
--- a/src/examples/bottle_drop/bottle_drop.cpp
+++ b/src/examples/bottle_drop/bottle_drop.cpp
@@ -67,6 +67,9 @@
#include <lib/ecl/geo/geo.h>
#include <dataman/dataman.h>
#include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
+
+using matrix::wrap_pi;
/**
@@ -526,7 +529,7 @@ BottleDrop::task_main()
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat,
flight_vector_e.lon);
- approach_error = _wrap_pi(ground_direction - approach_direction);
+ approach_error = wrap_pi(ground_direction - approach_direction);
if (counter % 90 == 0) {
mavlink_log_info(&_mavlink_log_pub, "drop distance %u, heading error %u", (unsigned)distance_real,
diff --git a/src/examples/segway/blocks.cpp b/src/examples/segway/blocks.cpp
index b751137da02009a2391071ac3e841f5ce13f912c..da0cf91f710928a7ad81a90b5cb2822a85a6e179 100644
--- a/src/examples/segway/blocks.cpp
+++ b/src/examples/segway/blocks.cpp
@@ -39,6 +39,9 @@
#include "blocks.hpp"
#include <lib/ecl/geo/geo.h>
+#include <matrix/math.hpp>
+
+using matrix::wrap_2pi;
namespace control
{
@@ -77,8 +80,8 @@ void BlockWaypointGuidance::update(
missionCmd.lat,
missionCmd.lon);
- _psiCmd = _wrap_2pi(psiTrack -
- _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
+ _psiCmd = wrap_2pi(psiTrack -
+ _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
}
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
diff --git a/src/lib/ecl b/src/lib/ecl
index d177e96508d2572f6fa8eb7ff41852749c882548..26dcf05d80ff98f72c79099901d84bd3986b9291 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit d177e96508d2572f6fa8eb7ff41852749c882548
+Subproject commit 26dcf05d80ff98f72c79099901d84bd3986b9291
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 55a39cfb2792aed64215332cda9051feed22acd1..6c0a01131bfdf7b5f108aa6e4d3a8d090f7e53cb 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -65,6 +65,7 @@ using matrix::Dcmf;
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector3f;
+using matrix::wrap_pi;
class AttitudeEstimatorQ;
@@ -572,7 +573,7 @@ bool AttitudeEstimatorQ::update(float dt)
// Vision heading correction
// Project heading to global frame and extract XY component
Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg);
- float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
+ float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
}
@@ -581,7 +582,7 @@ bool AttitudeEstimatorQ::update(float dt)
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
- float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
+ float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
}
@@ -591,7 +592,7 @@ bool AttitudeEstimatorQ::update(float dt)
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector3f mag_earth = _q.conjugate(_mag);
- float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
+ float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
float gainMult = 1.0f;
const float fifty_dps = 0.873f;
diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp
index d9cac1fc5468c59692643321d5050418c177625b..c187408b7d0ac07bd179331404d4bed95a1c2467 100644
--- a/src/modules/mavlink/mavlink_high_latency2.cpp
+++ b/src/modules/mavlink/mavlink_high_latency2.cpp
@@ -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;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 131fdf0b2fe5d28ef82889e0b1a7a8be715d6fe1..1833a93e251932419a6dd618b3bbb55c36c47e1b 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -39,11 +39,6 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
-#include <stdio.h>
-#include <errno.h>
-#include <math.h>
-#include <float.h>
-
#include "mavlink_main.h"
#include "mavlink_messages.h"
#include "mavlink_command_sender.h"
@@ -57,7 +52,6 @@
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_time.h>
-#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/actuator_armed.h>
@@ -109,6 +103,8 @@
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/uORB.h>
+using matrix::wrap_2pi;
+
static uint16_t cm_uint16_from_m_float(float m);
static void get_mavlink_mode_state(const struct vehicle_status_s *const status, uint8_t *mavlink_state,
@@ -1082,7 +1078,7 @@ protected:
matrix::Eulerf euler = matrix::Quatf(att.q);
msg.airspeed = airspeed.indicated_airspeed_m_s;
msg.groundspeed = sqrtf(pos.vx * pos.vx + pos.vy * pos.vy);
- msg.heading = _wrap_2pi(euler.psi()) * M_RAD_TO_DEG_F;
+ msg.heading = math::degrees(euler.psi());
if (armed.armed) {
actuator_controls_s act0 = {};
@@ -1198,7 +1194,7 @@ protected:
msg.vel_acc = gps.s_variance_m_s * 1e3f;
msg.hdg_acc = gps.c_variance_rad * 1e5f / M_DEG_TO_RAD_F;
msg.vel = cm_uint16_from_m_float(gps.vel_m_s);
- msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f;
+ msg.cog = math::degrees(wrap_2pi(gps.cog_rad)) * 1e2f;
msg.satellites_visible = gps.satellites_used;
mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
@@ -1795,7 +1791,7 @@ protected:
msg.vy = lpos.vy * 100.0f;
msg.vz = lpos.vz * 100.0f;
- msg.hdg = _wrap_2pi(lpos.yaw) * M_RAD_TO_DEG_F * 100.0f;
+ msg.hdg = math::degrees(lpos.yaw) * 100.0f;
mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 6be7429c741db0b74f7b297f25da8443bff66aeb..18680ad4a446ce7a47dace7217d1de33f91e6973 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -43,17 +43,18 @@
#include "mavlink_mission.h"
#include "mavlink_main.h"
-#include <errno.h>
-#include <math.h>
#include <lib/ecl/geo/geo.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <px4_defines.h>
-
+#include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
#include <navigator/navigation.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
+using matrix::wrap_pi;
+
dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
bool MavlinkMissionManager::_dataman_init = false;
uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
@@ -1315,13 +1316,13 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
mission_item->nav_cmd = NAV_CMD_WAYPOINT;
mission_item->time_inside = mavlink_mission_item->param1;
mission_item->acceptance_radius = mavlink_mission_item->param2;
- mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+ mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4));
break;
case MAV_CMD_NAV_LOITER_UNLIM:
mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
mission_item->loiter_radius = mavlink_mission_item->param3;
- mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+ mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4));
break;
case MAV_CMD_NAV_LOITER_TIME:
@@ -1334,14 +1335,14 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case MAV_CMD_NAV_LAND:
mission_item->nav_cmd = NAV_CMD_LAND;
// TODO: abort alt param1
- mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+ mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4));
mission_item->land_precision = mavlink_mission_item->param2;
break;
case MAV_CMD_NAV_TAKEOFF:
mission_item->nav_cmd = NAV_CMD_TAKEOFF;
mission_item->pitch_min = mavlink_mission_item->param1;
- mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+ mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4));
break;
case MAV_CMD_NAV_LOITER_TO_ALT:
@@ -1381,7 +1382,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case MAV_CMD_NAV_VTOL_TAKEOFF:
case MAV_CMD_NAV_VTOL_LAND:
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
- mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+ mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4));
break;
case MAV_CMD_NAV_FENCE_RETURN_POINT:
@@ -1586,12 +1587,12 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_WAYPOINT:
mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->param2 = mission_item->acceptance_radius;
- mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+ mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
break;
case NAV_CMD_LOITER_UNLIMITED:
mavlink_mission_item->param3 = mission_item->loiter_radius;
- mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+ mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
break;
case NAV_CMD_LOITER_TIME_LIMIT:
@@ -1602,12 +1603,12 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_LAND:
// TODO: param1 abort alt
- mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+ mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
break;
case NAV_CMD_TAKEOFF:
mavlink_mission_item->param1 = mission_item->pitch_min;
- mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+ mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
break;
case NAV_CMD_LOITER_TO_ALT:
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 5bdc543d9d1d18b10a449d34695f873274962d48..7ac5e21a9c16231fd87a7f07db931ffeb4e26a78 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -98,6 +98,8 @@
#include "mavlink_main.h"
#include "mavlink_command_sender.h"
+using matrix::wrap_2pi;
+
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mavlink(parent),
_mission_manager(nullptr),
@@ -2066,7 +2068,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
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
hil_gps.vel_ned_valid = true;
- hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
+ hil_gps.cog_rad = ((gps.cog == 65535) ? NAN : wrap_2pi(math::radians(gps.cog * 1e-2f)));
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used?
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 63d550ae08d8df7b0e0cf3b73faa35942f620250..64bad8f70b1f212004e9ce51bac9849a04d54600 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1563,8 +1563,8 @@ MulticopterPositionControl::control_offboard()
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_pos_sp_triplet.current.yawspeed_valid) {
- float yaw_target = _wrap_pi(_att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * _dt);
- float yaw_offs = _wrap_pi(yaw_target - _yaw);
+ float yaw_target = wrap_pi(_att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * _dt);
+ float yaw_offs = wrap_pi(yaw_target - _yaw);
const float yaw_rate_max = (_man_yaw_max < _global_yaw_max) ? _man_yaw_max : _global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _mc_att_yaw_p.get();
@@ -1800,7 +1800,7 @@ void MulticopterPositionControl::control_auto()
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
- float yaw_diff = _wrap_pi(_att_sp.yaw_body - _yaw);
+ float yaw_diff = wrap_pi(_att_sp.yaw_body - _yaw);
/* only follow previous-current-line for specific triplet type */
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
@@ -2767,8 +2767,8 @@ MulticopterPositionControl::generate_attitude_setpoint()
const float yaw_offset_max = yaw_rate_max / _mc_att_yaw_p.get();
_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
- float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * _dt);
- float yaw_offs = _wrap_pi(yaw_target - _yaw);
+ float yaw_target = wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * _dt);
+ float yaw_offs = wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it, only allow if it would make the offset smaller again.
@@ -2856,7 +2856,7 @@ MulticopterPositionControl::generate_attitude_setpoint()
// - look at the roll and pitch angles: they should stay pretty much the same as when not yawing
// calculate our current yaw error
- float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw);
+ float yaw_error = wrap_pi(_att_sp.yaw_body - _yaw);
// compute the vector obtained by rotating a z unit vector by the rotation
// given by the roll and pitch commands of the user
diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index 5cbe3190e5bf0c962a89dce5886e6ba9c758a7c6..53e68a72fa16958d91bbf2291f6806bd389d419f 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -56,6 +56,8 @@
#include "navigator.h"
+using matrix::wrap_pi;
+
constexpr float FollowTarget::_follow_position_matricies[4][9];
FollowTarget::FollowTarget(Navigator *navigator) :
@@ -200,7 +202,7 @@ void FollowTarget::on_active()
_current_target_motion.lat,
_current_target_motion.lon);
- _yaw_rate = _wrap_pi((_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0f));
+ _yaw_rate = wrap_pi((_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0f));
} else {
_yaw_angle = _yaw_rate = NAN;
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index f057308494b9374d3c0728f01130f6519c9fb822..2e4b5521967211e5b9c374195afcada03b1c57e5 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -60,6 +60,8 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
+using matrix::wrap_pi;
+
Mission::Mission(Navigator *navigator) :
MissionBlock(navigator),
ModuleParams(navigator)
@@ -1245,7 +1247,7 @@ Mission::heading_sp_update()
/* always keep the back of the rotary wing pointing towards home */
if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
- _mission_item.yaw = _wrap_pi(yaw + M_PI_F);
+ _mission_item.yaw = wrap_pi(yaw + M_PI_F);
pos_sp_triplet->current.yaw = _mission_item.yaw;
} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index ee8c3f17b3e5bf99bb3f707b57a98d5132afeb0f..b2753fda0430d0093cf5b9962b8cd2db8683c935 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -54,6 +54,8 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vtol_vehicle_status.h>
+using matrix::wrap_pi;
+
MissionBlock::MissionBlock(Navigator *navigator) :
NavigatorMode(navigator)
{
@@ -326,7 +328,8 @@ MissionBlock::is_mission_item_reached()
float cog = _navigator->get_vstatus()->is_rotary_wing ? _navigator->get_global_position()->yaw : atan2f(
_navigator->get_global_position()->vel_e,
_navigator->get_global_position()->vel_n);
- float yaw_err = _wrap_pi(_mission_item.yaw - cog);
+
+ float yaw_err = wrap_pi(_mission_item.yaw - cog);
/* accept yaw if reached or if timeout is set in which case we ignore not forced headings */
if (fabsf(yaw_err) < math::radians(_navigator->get_yaw_threshold())
diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 7565ce37eff7931079845eb3ac9ee0f6a22ea9c9..24b240114c3b4df374c88d7aad00b685c2182842 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -46,6 +46,8 @@
#include <float.h>
+using matrix::wrap_pi;
+
Standard::Standard(VtolAttitudeControl *attc) :
VtolType(attc),
_pusher_throttle(0.0f),
@@ -368,7 +370,7 @@ void Standard::update_mc_state()
// rotate the vector into a new frame which is rotated in z by the desired heading
// with respect to the earh frame.
- float yaw_error = matrix::wrap_pi(euler_sp(2) - euler(2));
+ const float yaw_error = wrap_pi(euler_sp(2) - euler(2));
matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
tilt_new = R_yaw_correction * tilt_new;
diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp
deleted file mode 100644
index 5f3307010e7f337fc5b8ff0b952c513d30dcb5fe..0000000000000000000000000000000000000000
--- a/src/platforms/ros/geo.cpp
+++ /dev/null
@@ -1,271 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file geo.c
- *
- * Geo / math functions to perform geodesic calculations
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#include <lib/ecl/geo/geo.h>
-#include <px4.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <string.h>
-#include <float.h>
-
-__EXPORT float _wrap_pi(float bearing)
-{
- /* value is inf or NaN */
- if (!PX4_ISFINITE(bearing)) {
- return bearing;
- }
-
- int c = 0;
-
- while (bearing >= M_PI_F) {
- bearing -= M_TWOPI_F;
-
- if (c++ > 3) {
- return NAN;
- }
- }
-
- c = 0;
-
- while (bearing < -M_PI_F) {
- bearing += M_TWOPI_F;
-
- if (c++ > 3) {
- return NAN;
- }
- }
-
- return bearing;
-}
-
-__EXPORT float _wrap_2pi(float bearing)
-{
- /* value is inf or NaN */
- if (!PX4_ISFINITE(bearing)) {
- return bearing;
- }
-
- int c = 0;
-
- while (bearing >= M_TWOPI_F) {
- bearing -= M_TWOPI_F;
-
- if (c++ > 3) {
- return NAN;
- }
- }
-
- c = 0;
-
- while (bearing < 0.0f) {
- bearing += M_TWOPI_F;
-
- if (c++ > 3) {
- return NAN;
- }
- }
-
- return bearing;
-}
-
-__EXPORT float _wrap_180(float bearing)
-{
- /* value is inf or NaN */
- if (!PX4_ISFINITE(bearing)) {
- return bearing;
- }
-
- int c = 0;
-
- while (bearing >= 180.0f) {
- bearing -= 360.0f;
-
- if (c++ > 3) {
- return NAN;
- }
- }
-
- c = 0;
-
- while (bearing < -180.0f) {
- bearing += 360.0f;
-
- if (c++ > 3) {
- return NAN;
- }
- }
-
- return bearing;
-}
-
-__EXPORT float _wrap_360(float bearing)
-{
- /* value is inf or NaN */
- if (!PX4_ISFINITE(bearing)) {
- return bearing;
- }
-
- int c = 0;
-
- while (bearing >= 360.0f) {
- bearing -= 360.0f;
-
- if (c++ > 3) {
- return NAN;
- }
- }
-
- c = 0;
-
- while (bearing < 0.0f) {
- bearing += 360.0f;
-
- if (c++ > 3) {
- return NAN;
- }
- }
-
- return bearing;
-}
-
-__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref)
-{
- return ref->init_done;
-}
-
-__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
-{
- return ref->timestamp;
-}
-
-__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0,
- uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
-{
-
- ref->lat_rad = lat_0 * M_DEG_TO_RAD;
- ref->lon_rad = lon_0 * M_DEG_TO_RAD;
- ref->sin_lat = sin(ref->lat_rad);
- ref->cos_lat = cos(ref->lat_rad);
-
- ref->timestamp = timestamp;
- ref->init_done = true;
-
- return 0;
-}
-
-__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0,
- double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
-{
- return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros());
-}
-
-__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad,
- double *ref_lon_rad)
-{
- if (!map_projection_initialized(ref)) {
- return -1;
- }
-
- *ref_lat_rad = ref->lat_rad;
- *ref_lon_rad = ref->lon_rad;
-
- return 0;
-}
-
-__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
- float *y)
-{
- if (!map_projection_initialized(ref)) {
- return -1;
- }
-
- double lat_rad = lat * M_DEG_TO_RAD;
- double lon_rad = lon * M_DEG_TO_RAD;
-
- double sin_lat = sin(lat_rad);
- double cos_lat = cos(lat_rad);
- double cos_d_lon = cos(lon_rad - ref->lon_rad);
-
- double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
- double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
-
- *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
- *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
-
- return 0;
-}
-
-__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
- double *lon)
-{
- if (!map_projection_initialized(ref)) {
- return -1;
- }
-
- double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
- double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
- double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
- double sin_c = sin(c);
- double cos_c = cos(c);
-
- double lat_rad;
- double lon_rad;
-
- if (fabs(c) > DBL_EPSILON) {
- lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
- lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
-
- } else {
- lat_rad = ref->lat_rad;
- lon_rad = ref->lon_rad;
- }
-
- *lat = lat_rad * 180.0 / M_PI;
- *lon = lon_rad * 180.0 / M_PI;
-
- return 0;
-}