Copter: use the return code of the guided_set_destination_posvel() function calls

This commit is contained in:
Dr.-Ing. Amilcar Do Carmo Lucas 2017-11-29 11:19:42 +01:00 committed by Randy Mackay
parent c7a2fcdc37
commit ce3197e0ac
1 changed files with 10 additions and 4 deletions

View File

@ -1446,8 +1446,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// send request
if (!pos_ignore && !vel_ignore && acc_ignore) {
copter.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
result = MAV_RESULT_ACCEPTED;
if (copter.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
copter.guided_set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
result = MAV_RESULT_ACCEPTED;
@ -1545,8 +1548,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
}
if (!pos_ignore && !vel_ignore && acc_ignore) {
copter.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);
result = MAV_RESULT_ACCEPTED;
if (copter.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)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
copter.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);
result = MAV_RESULT_ACCEPTED;