Skip to content

Commit

Permalink
Rover: Determine common conditions first
Browse files Browse the repository at this point in the history
  • Loading branch information
@muramura
muramura committed May 2, 2024
1 parent 45114d8 commit a84c844
Showing 1 changed file with 34 additions and 30 deletions.
64 changes: 34 additions & 30 deletions Rover/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
}

Expand Down Expand Up @@ -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);
}
}
}

Expand Down

0 comments on commit a84c844

Please sign in to comment.