From e294991b08941d17da5c4a4d9c554404f55d802c Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 19 May 2021 23:37:38 +0930 Subject: [PATCH] Copter: Fix before squash --- ArduCopter/land_detector.cpp | 2 +- ArduCopter/mode.cpp | 6 +++--- ArduCopter/mode.h | 2 +- ArduCopter/mode_althold.cpp | 12 ++++++------ ArduCopter/mode_auto.cpp | 11 +++++++---- ArduCopter/mode_autorotate.cpp | 4 ++-- ArduCopter/mode_autotune.cpp | 1 + ArduCopter/mode_brake.cpp | 4 ++-- ArduCopter/mode_circle.cpp | 4 ++-- ArduCopter/mode_flowhold.cpp | 14 +++++++------- ArduCopter/mode_guided.cpp | 20 +++++++++----------- ArduCopter/mode_land.cpp | 4 ++-- ArduCopter/mode_loiter.cpp | 12 ++++++------ ArduCopter/mode_poshold.cpp | 14 +++++++------- ArduCopter/mode_rtl.cpp | 11 +++++++---- ArduCopter/mode_sport.cpp | 14 +++++++------- ArduCopter/mode_throw.cpp | 2 +- ArduCopter/mode_zigzag.cpp | 15 ++++++++------- ArduCopter/takeoff.cpp | 18 ++++++++++-------- 19 files changed, 89 insertions(+), 81 deletions(-) diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index c411eefb91..644ee52932 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -186,7 +186,7 @@ void Copter::update_throttle_mix() // check for large acceleration - falling or high turbulence const bool accel_moving = (land_accel_ef_filter.get().length() > LAND_CHECK_ACCEL_MOVING); - // check for requested decent + // check for requested descent bool descent_not_demanded = pos_control->get_vel_desired_cms().z >= 0.0f; // check if landing diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index b32e14420a..e4f3d68e5f 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -468,7 +468,7 @@ void Mode::make_safe_spool_down() break; } - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero pos_control->update_z_controller(); // we may need to move this out attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); @@ -622,7 +622,7 @@ void Mode::land_run_horizontal_control() thrust_vector.y *= ratio; // tell position controller we are applying an external limit - pos_control->set_limit_xy(); + pos_control->set_externally_limited_xy(); } } @@ -801,7 +801,7 @@ GCS_Copter &Mode::gcs() // are taking off so I terms can be cleared void Mode::set_throttle_takeoff() { - // tell position controller to reset alt target and reset I terms + // initialise the vertical position controller pos_control->init_z_controller(); } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 60b848c4c8..3128263c98 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -164,7 +164,7 @@ protected: private: bool _running; float take_off_start_alt; - float take_off_alt; + float take_off_complete_alt ; }; static _TakeOff takeoff; diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index cf6658a6f2..37cf46a1da 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -8,7 +8,7 @@ // althold_init - initialise althold controller bool ModeAltHold::init(bool ignore_checks) { - // initialise position and desired velocity + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } @@ -20,7 +20,7 @@ bool ModeAltHold::init(bool ignore_checks) // should be called at 100hz or more void ModeAltHold::run() { - // initialize vertical speeds and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // apply SIMPLE mode transform to pilot inputs @@ -46,7 +46,7 @@ void ModeAltHold::run() case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero break; case AltHold_Landed_Ground_Idle: @@ -55,7 +55,7 @@ void ModeAltHold::run() case AltHold_Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero break; case AltHold_Takeoff: @@ -67,7 +67,7 @@ void ModeAltHold::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // get take-off adjusted pilot and takeoff climb rates + // set position controller targets adjusted for pilot input takeoff.do_pilot_takeoff(target_climb_rate); break; @@ -92,6 +92,6 @@ void ModeAltHold::run() // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - // call z-axis position controller + // run the vertical position controller and set output throttle pos_control->update_z_controller(); } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b9f3722b7b..27332609a3 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -270,7 +270,7 @@ void ModeAuto::land_start(const Vector3f& destination) // initialise loiter target destination loiter_nav->init_target(destination); - // initialise position and desired velocity + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } @@ -825,7 +825,8 @@ void ModeAuto::wp_run() // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); - // call z-axis position controller (wpnav should have already updated it's alt target) + // WP_Nav has set the vertical position control targets + // run the vertical position controller and set output throttle pos_control->update_z_controller(); // call attitude controller @@ -883,7 +884,8 @@ void ModeAuto::circle_run() // call circle controller copter.failsafe_terrain_set_status(copter.circle_nav->update()); - // call z-axis position controller + // WP_Nav has set the vertical position control targets + // run the vertical position controller and set output throttle pos_control->update_z_controller(); if (auto_yaw.mode() == AUTO_YAW_HOLD) { @@ -994,7 +996,7 @@ void ModeAuto::payload_place_start(const Vector3f& destination) // initialise loiter target destination loiter_nav->init_target(destination); - // initialise position and desired velocity + // initialise the vertical position controller pos_control->init_z_controller(); // initialise yaw @@ -1326,6 +1328,7 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) loiter_to_alt.reached_alt = false; loiter_to_alt.alt_error_cm = 0; + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); } diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 84162c67bc..a60ab9ea51 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -231,13 +231,13 @@ void ModeAutorotate::run() float pilot_des_v_z = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); pilot_des_v_z = constrain_float(pilot_des_v_z, pilot_spd_dn, pilot_spd_up); - // Calculate target climb rate adjustment to transition from bail out decent speed to requested climb rate on stick. + // Calculate target climb rate adjustment to transition from bail out descent speed to requested climb rate on stick. _target_climb_rate_adjust = (curr_vel_z - pilot_des_v_z)/(_bail_time - BAILOUT_MOTOR_RAMP_TIME); //accounting for 0.5s motor spool time // Calculate pitch target adjustment rate to return to level _target_pitch_adjust = _pitch_target/_bail_time; - // Set speed and acceleration limit + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index f87b51fb6d..1700c23b0b 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -93,6 +93,7 @@ void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitc */ void AutoTune::init_z_limits() { + // set vertical speed and acceleration limits copter.pos_control->set_max_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z); } diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index c34c3271b4..77837cd6af 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -15,10 +15,10 @@ bool ModeBrake::init(bool ignore_checks) // initialise position controller pos_control->init_xy_controller(); - // initialize vertical speed and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE); - // initialise position and desired velocity + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 19102318d6..6a0b8822ed 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -12,7 +12,7 @@ bool ModeCircle::init(bool ignore_checks) pilot_yaw_override = false; speed_changing = false; - // initialize speeds and accelerations + // set speed and acceleration limits pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); @@ -26,7 +26,7 @@ bool ModeCircle::init(bool ignore_checks) // should be called at 100hz or more void ModeCircle::run() { - // initialize speeds and accelerations + // set speed and acceleration limits pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 83ebaa4184..2a24983e5f 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -88,10 +88,10 @@ bool ModeFlowHold::init(bool ignore_checks) return false; } - // initialize vertical maximum speeds and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - // initialise position and desired velocity + // initialise the vertical position controller if (!copter.pos_control->is_active_z()) { pos_control->init_z_controller(); } @@ -229,7 +229,7 @@ void ModeFlowHold::run() { update_height_estimate(); - // initialize vertical speeds and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // apply SIMPLE mode transform to pilot inputs @@ -264,7 +264,7 @@ void ModeFlowHold::run() copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); copter.attitude_control->reset_rate_controller_I_terms(); copter.attitude_control->set_yaw_target_to_current_heading(); - copter.pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + copter.pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero flow_pi_xy.reset_I(); break; @@ -280,7 +280,7 @@ void ModeFlowHold::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // get take-off adjusted pilot and takeoff climb rates + // set position controller targets adjusted for pilot input takeoff.do_pilot_takeoff(target_climb_rate); break; @@ -290,7 +290,7 @@ void ModeFlowHold::run() case AltHold_Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero break; case AltHold_Flying: @@ -336,7 +336,7 @@ void ModeFlowHold::run() // call attitude controller copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); - // call z-axis position controller + // run the vertical position controller and set output throttle pos_control->update_z_controller(); } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index a2d9ea5388..f8c502e13a 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -164,10 +164,10 @@ void ModeGuided::vel_control_start() // initialise horizontal speed, acceleration pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); - // initialize vertical speeds and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); - // initialise velocity controller + // initialise the position controller pos_control->init_z_controller(); pos_control->init_xy_controller(); } @@ -181,10 +181,10 @@ void ModeGuided::posvel_control_start() // initialise horizontal speed, acceleration pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); - // initialize vertical speeds and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); - // initialise velocity controller + // initialise the position controller pos_control->init_z_controller(); pos_control->init_xy_controller(); @@ -203,10 +203,10 @@ void ModeGuided::angle_control_start() // set guided_mode to velocity controller guided_mode = SubMode::Angle; - // set vertical speed and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); - // initialise position and desired velocity + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } @@ -439,7 +439,8 @@ void ModeGuided::pos_control_run() // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); - // call z-axis position controller (wpnav should have already updated it's alt target) + // WP_Nav has set the vertical position control targets + // run the vertical position controller and set output throttle pos_control->update_z_controller(); // call attitude controller @@ -552,11 +553,8 @@ void ModeGuided::posvel_control_run() } } - // calculate dt - float dt = pos_control->get_dt(); - // advance position target using velocity target - guided_pos_target_cm += guided_vel_target_cms * dt; + guided_pos_target_cm += guided_vel_target_cms * pos_control->get_dt(); // send position and velocity targets to position controller pos_control->input_pos_vel_accel_xy(guided_pos_target_cm, guided_vel_target_cms, Vector3f()); diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index d8630edb51..5d7e6e6940 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -12,10 +12,10 @@ bool ModeLand::init(bool ignore_checks) loiter_nav->init_target(stopping_point); } - // initialize vertical maximum speeds and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); - // initialise position and desired velocity + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 4faa30dac9..cd9b259313 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -25,7 +25,7 @@ bool ModeLoiter::init(bool ignore_checks) } loiter_nav->init_target(); - // initialise position and desired velocity + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } @@ -77,7 +77,7 @@ void ModeLoiter::run() float target_yaw_rate = 0.0f; float target_climb_rate = 0.0f; - // initialize vertical speed and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // process pilot inputs unless we are in radio failsafe @@ -116,7 +116,7 @@ void ModeLoiter::run() case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero loiter_nav->init_target(); attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); break; @@ -130,7 +130,7 @@ void ModeLoiter::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // get take-off adjusted pilot and takeoff climb rates + // set position controller targets adjusted for pilot input takeoff.do_pilot_takeoff(target_climb_rate); // run loiter controller @@ -148,7 +148,7 @@ void ModeLoiter::run() attitude_control->reset_rate_controller_I_terms_smoothly(); loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero break; case AltHold_Flying: @@ -177,7 +177,7 @@ void ModeLoiter::run() break; } - // call z-axis position controller + // run the vertical position controller and set output throttle pos_control->update_z_controller(); } diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index fd020e110c..f85f584ae3 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -27,10 +27,10 @@ // poshold_init - initialise PosHold controller bool ModePosHold::init(bool ignore_checks) { - // initialize vertical speeds and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - // initialise position and desired velocity + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } @@ -70,7 +70,7 @@ void ModePosHold::run() float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls const Vector3f& vel = inertial_nav.get_velocity(); - // initialize vertical speeds and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); loiter_nav->clear_pilot_desired_acceleration(); @@ -102,7 +102,7 @@ void ModePosHold::run() case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); loiter_nav->update(false); @@ -124,7 +124,7 @@ void ModePosHold::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // get take-off adjusted pilot and takeoff climb rates + // set position controller targets adjusted for pilot input takeoff.do_pilot_takeoff(target_climb_rate); // init and update loiter although pilot is controlling lean angles @@ -147,7 +147,7 @@ void ModePosHold::run() case AltHold_Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero // set poshold state to pilot override roll_mode = RPMode::PILOT_OVERRIDE; @@ -490,7 +490,7 @@ void ModePosHold::run() // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll, pitch, target_yaw_rate); - // call z-axis position controller + // run the vertical position controller and set output throttle pos_control->update_z_controller(); } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 598ca9843f..c491b826bf 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -171,7 +171,8 @@ void ModeRTL::climb_return_run() // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); - // call z-axis position controller (wpnav should have already updated it's alt target) + // WP_Nav has set the vertical position control targets + // run the vertical position controller and set output throttle pos_control->update_z_controller(); // call attitude controller @@ -228,7 +229,8 @@ void ModeRTL::loiterathome_run() // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); - // call z-axis position controller (wpnav should have already updated it's alt target) + // WP_Nav has set the vertical position control targets + // run the vertical position controller and set output throttle pos_control->update_z_controller(); // call attitude controller @@ -335,7 +337,8 @@ void ModeRTL::descent_run() // run loiter controller loiter_nav->update(); - // call z-axis position controller + // WP_Nav has set the vertical position control targets + // run the vertical position controller and set output throttle pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt); pos_control->update_z_controller(); @@ -355,7 +358,7 @@ void ModeRTL::land_start() // Set wp navigation target to above home loiter_nav->init_target(wp_nav->get_wp_destination()); - // initialise position and desired velocity + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index e971588c45..9c36aea33a 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -9,10 +9,10 @@ // sport_init - initialise sport controller bool ModeSport::init(bool ignore_checks) { - // initialize vertical speed and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - // initialise position and desired velocity + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } @@ -24,7 +24,7 @@ bool ModeSport::init(bool ignore_checks) // should be called at 100hz or more void ModeSport::run() { - // initialize vertical speed and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // apply SIMPLE mode transform @@ -76,7 +76,7 @@ void ModeSport::run() case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero break; case AltHold_Takeoff: @@ -88,7 +88,7 @@ void ModeSport::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // get take-off adjusted pilot and takeoff climb rates + // set position controller targets adjusted for pilot input takeoff.do_pilot_takeoff(target_climb_rate); break; @@ -98,7 +98,7 @@ void ModeSport::run() case AltHold_Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero break; case AltHold_Flying: @@ -117,7 +117,7 @@ void ModeSport::run() // call attitude controller attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); - // call z-axis position controller + // run the vertical position controller and set output throttle pos_control->update_z_controller(); } diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index ea94d3f0fe..427835b224 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -22,7 +22,7 @@ bool ModeThrow::init(bool ignore_checks) // initialise pos controller speed and acceleration pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), BRAKE_MODE_DECEL_RATE); - // initialize vertical speed and acceleration + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE); return true; diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index a46aee01e5..37a60ff2fc 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -83,7 +83,7 @@ bool ModeZigZag::init(bool ignore_checks) } loiter_nav->init_target(); - // initialise position_z and desired velocity_z + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } @@ -110,7 +110,7 @@ void ModeZigZag::exit() // should be called at 100hz or more void ModeZigZag::run() { - // initialize vertical speed and acceleration's range + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // set the direction and the total number of lines @@ -269,7 +269,8 @@ void ModeZigZag::auto_control() // run waypoint controller const bool wpnav_ok = wp_nav->update_wpnav(); - // call z-axis position controller (wp_nav should have already updated its alt target) + // WP_Nav has set the vertical position control targets + // run the vertical position controller and set output throttle pos_control->update_z_controller(); // call attitude controller @@ -326,7 +327,7 @@ void ModeZigZag::manual_control() case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); break; @@ -346,7 +347,7 @@ void ModeZigZag::manual_control() // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); - // get take-off adjusted pilot and takeoff climb rates + // set position controller targets adjusted for pilot input takeoff.do_pilot_takeoff(target_climb_rate); break; @@ -358,7 +359,7 @@ void ModeZigZag::manual_control() attitude_control->reset_rate_controller_I_terms_smoothly(); loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero break; case AltHold_Flying: @@ -381,7 +382,7 @@ void ModeZigZag::manual_control() break; } - // call z-axis position controller + // run the vertical position controller and set output throttle pos_control->update_z_controller(); } diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 40e684b755..7c38cd44ff 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -48,7 +48,7 @@ bool Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) return true; } -// start takeoff to specified altitude above home in centimetres +// start takeoff to specified altitude above home in centimeters void Mode::_TakeOff::start(float alt_cm) { // indicate we are taking off @@ -59,7 +59,7 @@ void Mode::_TakeOff::start(float alt_cm) // initialise takeoff state _running = true; take_off_start_alt = copter.pos_control->get_pos_target_z_cm(); - take_off_alt = take_off_start_alt + alt_cm; + take_off_complete_alt = take_off_start_alt + alt_cm; } // stop takeoff @@ -68,7 +68,8 @@ void Mode::_TakeOff::stop() _running = false; } -// do_pilot_takeoff - commands the aircraft to the take off altitude +// do_pilot_takeoff - controls the vertical position controller during the process of taking off +// take off is complete when the vertical target reaches the take off altitude. // climb is cancelled if pilot_climb_rate_cm becomes negative // sets take off to complete when target altitude is within 1% of the take off altitude void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) @@ -82,7 +83,7 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) Vector3f vel; Vector3f accel; - pos.z = take_off_alt; + pos.z = take_off_complete_alt ; vel.z = pilot_climb_rate_cm; // command the aircraft to the take off altitude and current pilot climb rate @@ -90,7 +91,7 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) // stop take off early and return if negative climb rate is commanded or we are within 0.1% of our take off altitude if (is_negative(pilot_climb_rate_cm) || - (take_off_alt - take_off_start_alt) * 0.999f < copter.pos_control->get_pos_target_z_cm() - take_off_start_alt) { + (take_off_complete_alt - take_off_start_alt) * 0.999f < copter.pos_control->get_pos_target_z_cm() - take_off_start_alt) { stop(); } } @@ -120,7 +121,7 @@ void Mode::auto_takeoff_run() } else { // motors have not completed spool up yet so relax navigation and position controllers wp_nav->shift_wp_origin_and_destination_to_current_pos_xy(); - pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero pos_control->update_z_controller(); attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); @@ -139,7 +140,7 @@ void Mode::auto_takeoff_run() wp_nav->shift_wp_origin_and_destination_to_current_pos_xy(); } // tell the position controller that we have limited roll/pitch demand to prevent integrator buildup - pos_control->set_limit_xy(); + pos_control->set_externally_limited_xy(); } // run waypoint controller @@ -150,7 +151,8 @@ void Mode::auto_takeoff_run() thrustvector = wp_nav->get_thrust_vector(); } - // call z-axis position controller (wpnav should have already updated it's alt target) + // WP_Nav has set the vertical position control targets + // run the vertical position controller and set output throttle copter.pos_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot