Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: set-pos-target-global-int fails sooner if accel given 

same change for set-position-target-local-ned processing
This commit is contained in:
Randy Mackay 2020-02-27 20:34:32 +09:00
parent b21ecda76f
commit 255c5d4126
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

@ -994,10 +994,10 @@ 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_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; bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
/* // exit immediately if acceleration provided
* for future use: if (!acc_ignore) {
* bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE; break;
*/ }
// prepare position // prepare position
Vector3f pos_vector; Vector3f pos_vector;
@ -1051,11 +1051,11 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
} }
// send request // send request
if (!pos_ignore && !vel_ignore && acc_ignore) { if (!pos_ignore && !vel_ignore) {
copter.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); copter.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (pos_ignore && !vel_ignore && acc_ignore) { } else if (pos_ignore && !vel_ignore) {
copter.mode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); copter.mode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} else if (!pos_ignore && vel_ignore && acc_ignore) { } else if (!pos_ignore && vel_ignore) {
copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} }
@ -1079,10 +1079,10 @@ 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_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; bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
/* // exit immediately if acceleration provided
* for future use: if (!acc_ignore) {
* bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE; break;
*/ }
// extract location from message // extract location from message
Location loc; Location loc;
@ -1112,7 +1112,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
} }
// send targets to the appropriate guided mode controller // send targets to the appropriate guided mode controller
if (!pos_ignore && !vel_ignore && acc_ignore) { if (!pos_ignore && !vel_ignore) {
// convert Location to vector from ekf origin for posvel controller // convert Location to vector from ekf origin for posvel controller
if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) { if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {
// posvel controller does not support alt-above-terrain // posvel controller does not support alt-above-terrain
@ -1123,9 +1123,9 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
break; 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, 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) { } 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); 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) { } else if (!pos_ignore && vel_ignore) {
copter.mode_guided.set_destination(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); copter.mode_guided.set_destination(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
} }