mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-13 10:03:57 -03:00
Plane: invert auto_state.no_crosstrack flag to be auto_state.crosstrack. Non-functional change
This commit is contained in:
parent
e49d29f8cd
commit
7271586a47
@ -1276,7 +1276,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
|
||||
plane.auto_state.takeoff_altitude_rel_cm = packet.param1 * 100;
|
||||
}
|
||||
if (plane.landing.request_go_around()) {
|
||||
plane.auto_state.next_wp_no_crosstrack = true;
|
||||
plane.auto_state.next_wp_crosstrack = false;
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
}
|
||||
@ -1923,7 +1923,7 @@ AP_Mission *GCS_MAVLINK_Plane::get_mission()
|
||||
|
||||
void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg)
|
||||
{
|
||||
plane.auto_state.next_wp_no_crosstrack = true;
|
||||
plane.auto_state.next_wp_crosstrack = false;
|
||||
GCS_MAVLINK::handle_mission_set_current(mission, msg);
|
||||
if (plane.control_mode == AUTO && plane.mission.state() == AP_Mission::MISSION_STOPPED) {
|
||||
plane.mission.resume();
|
||||
|
@ -28,8 +28,6 @@ Plane::Plane(void)
|
||||
{
|
||||
// C++11 doesn't allow in-class initialisation of bitfields
|
||||
auto_state.takeoff_complete = true;
|
||||
auto_state.next_wp_no_crosstrack = true;
|
||||
auto_state.no_crosstrack = true;
|
||||
}
|
||||
|
||||
Plane plane;
|
||||
|
@ -452,11 +452,11 @@ private:
|
||||
// should we fly inverted?
|
||||
bool inverted_flight:1;
|
||||
|
||||
// should we disable cross-tracking for the next waypoint?
|
||||
bool next_wp_no_crosstrack:1;
|
||||
// should we enable cross-tracking for the next waypoint?
|
||||
bool next_wp_crosstrack:1;
|
||||
|
||||
// should we use cross-tracking for this waypoint?
|
||||
bool no_crosstrack:1;
|
||||
bool crosstrack:1;
|
||||
|
||||
// in FBWA taildragger takeoff mode
|
||||
bool fbwa_tdrag_takeoff_mode:1;
|
||||
|
@ -9,16 +9,16 @@
|
||||
*/
|
||||
void Plane::set_next_WP(const struct Location &loc)
|
||||
{
|
||||
if (auto_state.next_wp_no_crosstrack) {
|
||||
if (auto_state.next_wp_crosstrack) {
|
||||
// copy the current WP into the OldWP slot
|
||||
prev_WP_loc = next_WP_loc;
|
||||
auto_state.crosstrack = true;
|
||||
} else {
|
||||
// we should not try to cross-track for this waypoint
|
||||
prev_WP_loc = current_loc;
|
||||
// use cross-track for the next waypoint
|
||||
auto_state.next_wp_no_crosstrack = false;
|
||||
auto_state.no_crosstrack = true;
|
||||
} else {
|
||||
// copy the current WP into the OldWP slot
|
||||
prev_WP_loc = next_WP_loc;
|
||||
auto_state.no_crosstrack = false;
|
||||
auto_state.next_wp_crosstrack = true;
|
||||
auto_state.crosstrack = false;
|
||||
}
|
||||
|
||||
// Load the next_WP slot
|
||||
|
@ -344,8 +344,8 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
|
||||
|
||||
void Plane::do_RTL(int32_t rtl_altitude)
|
||||
{
|
||||
auto_state.next_wp_no_crosstrack = true;
|
||||
auto_state.no_crosstrack = true;
|
||||
auto_state.next_wp_crosstrack = false;
|
||||
auto_state.crosstrack = false;
|
||||
prev_WP_loc = current_loc;
|
||||
next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude);
|
||||
setup_terrain_target_alt(next_WP_loc);
|
||||
@ -575,7 +575,7 @@ bool Plane::verify_takeoff()
|
||||
|
||||
// don't cross-track on completion of takeoff, as otherwise we
|
||||
// can end up doing too sharp a turn
|
||||
auto_state.next_wp_no_crosstrack = true;
|
||||
auto_state.next_wp_crosstrack = false;
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
@ -608,10 +608,10 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
}
|
||||
}
|
||||
|
||||
if (auto_state.no_crosstrack) {
|
||||
nav_controller->update_waypoint(current_loc, flex_next_WP_loc);
|
||||
} else {
|
||||
if (auto_state.crosstrack) {
|
||||
nav_controller->update_waypoint(prev_WP_loc, flex_next_WP_loc);
|
||||
} else {
|
||||
nav_controller->update_waypoint(current_loc, flex_next_WP_loc);
|
||||
}
|
||||
|
||||
// see if the user has specified a maximum distance to waypoint
|
||||
|
@ -187,7 +187,7 @@ void Plane::update_loiter(uint16_t radius)
|
||||
}
|
||||
} else if ((loiter.start_time_ms == 0 &&
|
||||
control_mode == AUTO &&
|
||||
!auto_state.no_crosstrack &&
|
||||
auto_state.crosstrack &&
|
||||
get_distance(current_loc, next_WP_loc) > radius*3) ||
|
||||
(control_mode == RTL && quadplane.available() && quadplane.rtl_mode == 1)) {
|
||||
/*
|
||||
|
@ -270,7 +270,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
||||
auto_state.inverted_flight = false;
|
||||
|
||||
// don't cross-track when starting a mission
|
||||
auto_state.next_wp_no_crosstrack = true;
|
||||
auto_state.next_wp_crosstrack = false;
|
||||
|
||||
// reset landing check
|
||||
auto_state.checked_for_autoland = false;
|
||||
|
Loading…
Reference in New Issue
Block a user