diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index c9eb66f410..ff056d6456 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -187,6 +187,25 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate) #endif } +// get surfacing tracking alt +// returns true if there is a valid target +bool Copter::get_surface_tracking_target_alt_cm(float &target_alt_cm) const +{ + // check target has been updated recently + if (AP_HAL::millis() - surface_tracking.last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) { + return false; + } + target_alt_cm = surface_tracking.target_alt_cm; + return true; +} + +// set surface tracking target altitude +void Copter::set_surface_tracking_target_alt_cm(float target_alt_cm) +{ + surface_tracking.target_alt_cm = target_alt_cm; + surface_tracking.last_update_ms = AP_HAL::millis(); +} + // get target climb rate reduced to avoid obstacles and altitude fence float Copter::get_avoidance_adjusted_climbrate(float target_rate) { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index eadfc0e897..1a044086ce 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -622,6 +622,8 @@ private: float get_pilot_desired_climb_rate(float throttle_control); float get_non_takeoff_throttle(); float get_surface_tracking_climb_rate(int16_t target_rate); + bool get_surface_tracking_target_alt_cm(float &target_alt_cm) const; + void set_surface_tracking_target_alt_cm(float target_alt_cm); float get_avoidance_adjusted_climbrate(float target_rate); void set_accel_throttle_I_from_pilot_throttle(); void rotate_body_frame_to_NE(float &x, float &y); diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index a90c1a8d8b..1fce0831db 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -522,7 +522,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw copter.mode_zigzag.save_or_move_to_destination(0); break; case MIDDLE: - copter.mode_zigzag.return_to_manual_control(); + copter.mode_zigzag.return_to_manual_control(false); break; case HIGH: copter.mode_zigzag.save_or_move_to_destination(1); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 0a3a458832..4856cfbadb 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -693,6 +693,16 @@ float Copter::Mode::get_surface_tracking_climb_rate(int16_t target_rate) return copter.get_surface_tracking_climb_rate(target_rate); } +bool Copter::Mode::get_surface_tracking_target_alt_cm(float &target_alt_cm) const +{ + return copter.get_surface_tracking_target_alt_cm(target_alt_cm); +} + +void Copter::Mode::set_surface_tracking_target_alt_cm(float target_alt_cm) +{ + copter.set_surface_tracking_target_alt_cm(target_alt_cm); +} + float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle) { return copter.get_pilot_desired_yaw_rate(stick_angle); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 984113fafb..e4838532a1 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -198,6 +198,8 @@ protected: // these are candidates for moving into the Mode base // class. float get_surface_tracking_climb_rate(int16_t target_rate); + bool get_surface_tracking_target_alt_cm(float &target_alt_cm) const; + void set_surface_tracking_target_alt_cm(float target_alt_cm); float get_pilot_desired_yaw_rate(int16_t stick_angle); float get_pilot_desired_climb_rate(float throttle_control); float get_pilot_desired_throttle() const; @@ -1206,7 +1208,7 @@ public: void save_or_move_to_destination(uint8_t dest_num); // return manual control to the pilot - void return_to_manual_control(); + void return_to_manual_control(bool maintain_target); protected: @@ -1218,7 +1220,7 @@ private: void auto_control(); void manual_control(); bool reached_destination(); - bool calculate_next_dest(uint8_t position_num, Vector3f& next_dest) const; + bool calculate_next_dest(uint8_t position_num, 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 7149645412..3091fea4c3 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -48,8 +48,7 @@ void Copter::ModeZigZag::run() // if vehicle has reached destination switch to manual control if (reached_destination()) { AP_Notify::events.waypoint_complete = 1; - stage = MANUAL_REGAIN; - loiter_nav->init_target(wp_nav->get_wp_destination()); + return_to_manual_control(true); } else { auto_control(); } @@ -100,10 +99,10 @@ void Copter::ModeZigZag::save_or_move_to_destination(uint8_t dest_num) case MANUAL_REGAIN: // A and B have been defined, move vehicle to destination A or B Vector3f next_dest; - if (calculate_next_dest(dest_num, next_dest)) { - // initialise waypoint controller + bool terr_alt; + if (calculate_next_dest(dest_num, stage == AUTO, next_dest, terr_alt)) { wp_nav->wp_and_spline_init(); - if (wp_nav->set_wp_destination(next_dest, false)) { + if (wp_nav->set_wp_destination(next_dest, terr_alt)) { stage = AUTO; reach_wp_time_ms = 0; if (dest_num == 0) { @@ -118,12 +117,16 @@ void Copter::ModeZigZag::save_or_move_to_destination(uint8_t dest_num) } // return manual control to the pilot -void Copter::ModeZigZag::return_to_manual_control() +void Copter::ModeZigZag::return_to_manual_control(bool maintain_target) { if (stage == AUTO) { stage = MANUAL_REGAIN; loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); + const Vector3f wp_dest = wp_nav->get_wp_destination(); + loiter_nav->init_target(wp_dest); + if (maintain_target && wp_nav->origin_and_destination_are_terrain_alt()) { + set_surface_tracking_target_alt_cm(wp_dest.z); + } gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control"); } } @@ -142,14 +145,19 @@ void Copter::ModeZigZag::auto_control() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller - copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); + const bool wpnav_ok = wp_nav->update_wpnav(); // call z-axis position controller (wp_nav should have already updated its alt target) pos_control->update_z_controller(); // call attitude controller // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + + // if wpnav failed (because of lack of terrain data) switch back to pilot control for next iteration + if (!wpnav_ok) { + return_to_manual_control(false); + } } // manual_control - process manual control @@ -226,7 +234,9 @@ bool Copter::ModeZigZag::reached_destination() } // calculate next destination according to vector A-B and current position -bool Copter::ModeZigZag::calculate_next_dest(uint8_t dest_num, Vector3f& next_dest) const +// 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 Copter::ModeZigZag::calculate_next_dest(uint8_t dest_num, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const { // sanity check dest_num if (dest_num > 1) { @@ -264,7 +274,22 @@ bool Copter::ModeZigZag::calculate_next_dest(uint8_t dest_num, Vector3f& next_de const Vector2f closest2d = Vector2f::closest_point(curr_pos2d, perp1, perp2); next_dest.x = closest2d.x; next_dest.y = closest2d.y; - next_dest.z = pos_control->is_active_z() ? pos_control->get_alt_target() : curr_pos.z; + + if (use_wpnav_alt) { + // get altitude target from waypoint controller + terrain_alt = wp_nav->origin_and_destination_are_terrain_alt(); + next_dest.z = wp_nav->get_wp_destination().z; + } else { + // if we have a downward facing range finder then use terrain altitude targets + terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used(); + if (terrain_alt) { + if (!get_surface_tracking_target_alt_cm(next_dest.z)) { + next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); + } + } else { + next_dest.z = pos_control->is_active_z() ? pos_control->get_alt_target() : curr_pos.z; + } + } return true; }