From 7271586a47249518635311aef64693bbbc4c5691 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 21 Nov 2017 11:41:45 -0800 Subject: [PATCH] Plane: invert auto_state.no_crosstrack flag to be auto_state.crosstrack. Non-functional change --- ArduPlane/GCS_Mavlink.cpp | 4 ++-- ArduPlane/Plane.cpp | 2 -- ArduPlane/Plane.h | 6 +++--- ArduPlane/commands.cpp | 14 +++++++------- ArduPlane/commands_logic.cpp | 12 ++++++------ ArduPlane/navigation.cpp | 2 +- ArduPlane/system.cpp | 2 +- 7 files changed, 20 insertions(+), 22 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 272df1c70f..93d7f3dbcb 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -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(); diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index 44d47348a1..587a59529d 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -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; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 31c7d83560..9c08ca23d5 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -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; diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index a741337a43..de75ebdc20 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -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 diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 73877c0985..e230c28fbf 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -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 diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 0b874bea1f..a209f5b67a 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -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)) { /* diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index cfdd28aab3..95fc1b72ad 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -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;