Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Guided close gap between TARGET_LOCAL_NED and TARGET_GLOBAL_INT

Browse Source
This commit is contained in:
Leonard Hall 2021-07-08 13:51:38 +09:30 committed by Randy Mackay
parent 4995add509
commit b43f5cb2a3
1 changed files with 29 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduCopter/GCS_Mavlink.cpp
View File

@ -1128,11 +1128,6 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
// exit immediately if neither position nor velocity is provided
if (pos_ignore && vel_ignore) {
break;
}
// prepare position
Vector3f pos_vector;
if (!pos_ignore) {
@ -1188,12 +1183,15 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
// send request
if (!pos_ignore && !vel_ignore) {
copter.mode_guided.set_destination_posvelaccel(pos_vector, vel_vector, accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (pos_ignore && vel_ignore && !acc_ignore) {
copter.mode_guided.set_accel(accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (pos_ignore && !vel_ignore) {
copter.mode_guided.set_velaccel(vel_vector, accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (pos_ignore && vel_ignore && !acc_ignore) {
copter.mode_guided.set_accel(accel_vector, !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_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, false);
} else {
// input is not valid so stop
copter.mode_guided.init(true);
}
break;
@ -1210,17 +1208,14 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
break;
}
// todo: do we need to check for supported coordinate frames
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
// exit immediately if acceleration provided
if (!acc_ignore) {
break;
}
// extract location from message
Location loc;
if (!pos_ignore) {
@ -1236,6 +1231,20 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};
}
// prepare velocity
Vector3f vel_vector;
if (!vel_ignore) {
// convert to cm
vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f);
}
// prepare acceleration
Vector3f accel_vector;
if (!acc_ignore) {
// convert to cm
accel_vector = Vector3f(packet.afx * 100.0f, packet.afy * 100.0f, -packet.afz * 100.0f);
}
// prepare yaw
float yaw_cd = 0.0f;
bool yaw_relative = false;
@ -1259,11 +1268,16 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
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);
copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (pos_ignore && !vel_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) {
copter.mode_guided.set_velaccel(vel_vector, accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (pos_ignore && vel_ignore && !acc_ignore) {
copter.mode_guided.set_accel(accel_vector, !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(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else {
// input is not valid so stop
copter.mode_guided.init(true);
}
break;