diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index d03fd30132..62f7e56af7 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -486,13 +486,13 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw if (copter.flightmode == &copter.mode_zigzag) { switch (ch_flag) { case LOW: - copter.mode_zigzag.save_or_move_to_destination(0); + copter.mode_zigzag.save_or_move_to_destination(ModeZigZag::Destination::A); break; case MIDDLE: copter.mode_zigzag.return_to_manual_control(false); break; case HIGH: - copter.mode_zigzag.save_or_move_to_destination(1); + copter.mode_zigzag.save_or_move_to_destination(ModeZigZag::Destination::B); break; } } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 529d557eb2..601beae5dd 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1387,6 +1387,11 @@ public: // Inherit constructor using Mode::Mode; + enum class Destination : uint8_t { + A, // Destination A + B, // Destination B + }; + bool init(bool ignore_checks) override; void exit(); void run() override; @@ -1396,8 +1401,8 @@ public: bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return true; } - // save current position as A (dest_num = 0) or B (dest_num = 1). If both A and B have been saved move to the one specified - void save_or_move_to_destination(uint8_t dest_num); + // save current position as A or B. If both A and B have been saved move to the one specified + void save_or_move_to_destination(Destination ab_dest); // return manual control to the pilot void return_to_manual_control(bool maintain_target); @@ -1412,7 +1417,7 @@ private: void auto_control(); void manual_control(); bool reached_destination(); - bool calculate_next_dest(uint8_t position_num, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const; + bool calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const; Vector2f dest_A; // in NEU frame in cm relative to ekf origin Vector2f dest_B; // in NEU frame in cm relative to ekf origin diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 7a9db0c14b..0477dceabb 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -81,14 +81,9 @@ void ModeZigZag::run() } } -// save current position as A (dest_num = 0) or B (dest_num = 1). If both A and B have been saved move to the one specified -void ModeZigZag::save_or_move_to_destination(uint8_t dest_num) +// save current position as A or B. If both A and B have been saved move to the one specified +void ModeZigZag::save_or_move_to_destination(Destination ab_dest) { - // sanity check - if (dest_num > 1) { - return; - } - // get current position as an offset from EKF origin const Vector3f curr_pos = inertial_nav.get_position(); @@ -96,7 +91,7 @@ void ModeZigZag::save_or_move_to_destination(uint8_t dest_num) switch (stage) { case STORING_POINTS: - if (dest_num == 0) { + if (ab_dest == Destination::A) { // store point A dest_A.x = curr_pos.x; dest_A.y = curr_pos.y; @@ -120,7 +115,7 @@ void ModeZigZag::save_or_move_to_destination(uint8_t dest_num) // A and B have been defined, move vehicle to destination A or B Vector3f next_dest; bool terr_alt; - if (calculate_next_dest(dest_num, stage == AUTO, next_dest, terr_alt)) { + if (calculate_next_dest(ab_dest, stage == AUTO, next_dest, terr_alt)) { wp_nav->wp_and_spline_init(); if (wp_nav->set_wp_destination(next_dest, terr_alt)) { stage = AUTO; @@ -131,7 +126,7 @@ void ModeZigZag::save_or_move_to_destination(uint8_t dest_num) } #endif reach_wp_time_ms = 0; - if (dest_num == 0) { + if (ab_dest == Destination::A) { gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to A"); } else { gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to B"); @@ -329,15 +324,10 @@ bool ModeZigZag::reached_destination() // calculate next destination according to vector A-B and current position // use_wpnav_alt should be true if waypoint controller's altitude target should be used, false for position control or current altitude target // terrain_alt is returned as true if the next_dest should be considered a terrain alt -bool ModeZigZag::calculate_next_dest(uint8_t dest_num, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const +bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const { - // sanity check dest_num - if (dest_num > 1) { - return false; - } - - // define start_pos as either A or B depending upon dest_num - Vector2f start_pos = dest_num == 0 ? dest_A : dest_B; + // define start_pos as either destination A or B + Vector2f start_pos = (ab_dest == Destination::A) ? dest_A : dest_B; // calculate vector from A to B Vector2f AB_diff = dest_B - dest_A;