fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 00:53: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;
src/modules/mavlink/mavlink_messages.cpp
+5 -9
View File
@@ -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);
src/modules/mavlink/mavlink_mission.cpp
+13 -12
View File
@@ -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:
src/modules/mavlink/mavlink_receiver.cpp
+3 -1
View File
@@ -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?