Browse Source

Plane: invert auto_state.no_crosstrack flag to be auto_state.crosstrack. Non-functional change

mission-4.1.18
Tom Pittenger 7 years ago committed by Tom Pittenger
parent
commit
7271586a47
  1. 4
      ArduPlane/GCS_Mavlink.cpp
  2. 2
      ArduPlane/Plane.cpp
  3. 6
      ArduPlane/Plane.h
  4. 14
      ArduPlane/commands.cpp
  5. 12
      ArduPlane/commands_logic.cpp
  6. 2
      ArduPlane/navigation.cpp
  7. 2
      ArduPlane/system.cpp

4
ArduPlane/GCS_Mavlink.cpp

@ -1276,7 +1276,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) @@ -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() @@ -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();

2
ArduPlane/Plane.cpp

@ -28,8 +28,6 @@ Plane::Plane(void) @@ -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;

6
ArduPlane/Plane.h

@ -452,11 +452,11 @@ private: @@ -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;

14
ArduPlane/commands.cpp

@ -9,16 +9,16 @@ @@ -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

12
ArduPlane/commands_logic.cpp

@ -344,8 +344,8 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret @@ -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() @@ -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) @@ -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

2
ArduPlane/navigation.cpp

@ -187,7 +187,7 @@ void Plane::update_loiter(uint16_t radius) @@ -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)) {
/*

2
ArduPlane/system.cpp

@ -270,7 +270,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) @@ -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…
Cancel
Save