aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: fix set_position_target altitudes

Browse Source
This commit is contained in:
Jonathan Challinger 2015-04-13 14:10:02 -07:00 committed by Randy Mackay
parent 301d4cc4a6
commit b344a7ed17
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/GCS_Mavlink.pde
View File

@ -1360,11 +1360,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
*/
if (!pos_ignore && !vel_ignore && acc_ignore) {
guided_set_destination_posvel(Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f), Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
Vector3f pos_ned = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
pos_ned.z = pv_alt_above_origin(pos_ned.z);
guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (pos_ignore && !vel_ignore && acc_ignore) {
guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (!pos_ignore && vel_ignore && acc_ignore) {
guided_set_destination(Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f));
Vector3f pos_ned = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
pos_ned.z = pv_alt_above_origin(pos_ned.z);
guided_set_destination(pos_ned);
} else {
result = MAV_RESULT_FAILED;
}