diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index f38e8f3dd0498..006b6378036e2 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -941,21 +941,23 @@ void GCS_MAVLINK_Rover::handle_set_position_target_local_ned(const mavlink_messa // GCS will need to monitor desired location to // see if they are having an effect. } - } else if (!vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { - // consume velocity - rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); - } else if (!vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { - // consume velocity and turn rate - rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed); - } else if (!vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { - // consume velocity and heading - rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); - } else if (vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { - // consume just target heading (probably only skid steering vehicles can do this) - rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f); - } else if (vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { - // consume just turn rate (probably only skid steering vehicles can do this) - rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f); + } else if (acc_ignore) { + if (!vel_ignore && yaw_ignore && yaw_rate_ignore) { + // consume velocity + rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); + } else if (!vel_ignore && yaw_ignore && !yaw_rate_ignore) { + // consume velocity and turn rate + rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed); + } else if (!vel_ignore && !yaw_ignore && yaw_rate_ignore) { + // consume velocity and heading + rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); + } else if (vel_ignore && !yaw_ignore && yaw_rate_ignore) { + // consume just target heading (probably only skid steering vehicles can do this) + rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f); + } else if (vel_ignore && yaw_ignore && !yaw_rate_ignore) { + // consume just turn rate (probably only skid steering vehicles can do this) + rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f); + } } } @@ -1037,21 +1039,23 @@ void GCS_MAVLINK_Rover::handle_set_position_target_global_int(const mavlink_mess // GCS will just need to look at desired location // outputs to see if it having an effect. } - } else if (!vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { - // consume velocity - rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); - } else if (!vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { - // consume velocity and turn rate - rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed); - } else if (!vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { - // consume velocity - rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); - } else if (vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { - // consume just target heading (probably only skid steering vehicles can do this) - rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f); - } else if (vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { - // consume just turn rate(probably only skid steering vehicles can do this) - rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f); + } else if (acc_ignore) { + if (!vel_ignore && yaw_ignore && yaw_rate_ignore) { + // consume velocity + rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); + } else if (!vel_ignore && yaw_ignore && !yaw_rate_ignore) { + // consume velocity and turn rate + rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed); + } else if (!vel_ignore && !yaw_ignore && yaw_rate_ignore) { + // consume velocity + rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); + } else if (vel_ignore && !yaw_ignore && yaw_rate_ignore) { + // consume just target heading (probably only skid steering vehicles can do this) + rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f); + } else if (vel_ignore && yaw_ignore && !yaw_rate_ignore) { + // consume just turn rate(probably only skid steering vehicles can do this) + rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f); + } } }