fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 15: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/navigator/follow_target.cpp
+3 -1
View File
@@ -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;
src/modules/navigator/mission.cpp
+3 -1
View File
@@ -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
src/modules/navigator/mission_block.cpp
+4 -1
View File
@@ -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())