Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: fix guided mode handling of terrain altitudes

Browse Source
This commit is contained in:
Randy Mackay 2020-02-27 20:23:45 +09:00
parent d5c0c62699
commit b21ecda76f
1 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopter/GCS_Mavlink.cpp
View File

@ -1084,9 +1084,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
* bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
*/
Vector3f pos_neu_cm; // position (North, East, Up coordinates) in centimeters
bool terrain_alt = false;
// extract location from message
Location loc;
if (!pos_ignore) {
// sanity check location
if (!check_latlng(packet.lat_int, packet.lon_int)) {
@ -1097,16 +1096,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
// unknown coordinate frame
break;
}
const Location loc{
packet.lat_int,
packet.lon_int,
int32_t(packet.alt*100),
frame,
};
terrain_alt = (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN);
if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
break;
}
loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};
}
// prepare yaw
@ -1121,12 +1111,22 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// send targets to the appropriate guided mode controller
if (!pos_ignore && !vel_ignore && acc_ignore) {
// convert Location to vector from ekf origin for posvel controller
if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {
// posvel controller does not support alt-above-terrain
break;
}
Vector3f pos_neu_cm;
if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
break;
}
copter.mode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (pos_ignore && !vel_ignore && acc_ignore) {
copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (!pos_ignore && vel_ignore && acc_ignore) {
copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, terrain_alt);
copter.mode_guided.set_destination(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
}
break;