Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Math: use pythagorous2() in some more places

Browse Source
This commit is contained in:
Andrew Tridgell 2012-12-20 15:16:18 +11:00
parent 78d98023f4
commit 9931009db2
3 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/GCS_Mavlink.pde
View File

@ -1631,7 +1631,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
float vel = sqrt((packet.vx * (float)packet.vx) + (packet.vy * (float)packet.vy));
float vel = pythagorous2(packet.vx, packet.vy);
float cog = wrap_360_cd(ToDeg(atan2(packet.vy, packet.vx)) * 100);
// set gps hil sensor

2
ArduCopter/GCS_Mavlink.pde
View File

@ -1753,7 +1753,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
float vel = sqrt((packet.vx * (float)packet.vx) + (packet.vy * (float)packet.vy));
float vel = pythagorous2(packet.vx, packet.vy);
float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100);
// set gps hil sensor

3
libraries/AP_HAL_AVR_SITL/sitl_gps.cpp
View File

@ -222,8 +222,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
velned.ned_north = 100.0 * d.speedN;
velned.ned_east = 100.0 * d.speedE;
velned.ned_down = 0;
#define sqr(x) ((x)*(x))
velned.speed_2d = sqrt(sqr(d.speedN)+sqr(d.speedE)) * 100;
velned.speed_2d = pythagorous2(d.speedN, d.speedE) * 100;
velned.speed_3d = velned.speed_2d;
velned.heading_2d = ToDeg(atan2(d.speedE, d.speedN)) * 100000.0;
if (velned.heading_2d < 0.0) {