Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Fix before squash

This commit is contained in:
Leonard Hall 2021-05-19 23:37:38 +09:30 committed by Andrew Tridgell
parent 8213fc5277
commit e294991b08
19 changed files with 89 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/land_detector.cpp
View File

@ -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

6
ArduCopter/mode.cpp
View File

@ -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();
}

2
ArduCopter/mode.h
View File

@ -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;

12
ArduCopter/mode_althold.cpp
View File

@ -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();
}

11
ArduCopter/mode_auto.cpp
View File

@ -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());
}

4
ArduCopter/mode_autorotate.cpp
View File

@ -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);

1
ArduCopter/mode_autotune.cpp
View File

@ -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);
}

4
ArduCopter/mode_brake.cpp
View File

@ -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();
}

4
ArduCopter/mode_circle.cpp
View File

@ -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);

14
ArduCopter/mode_flowhold.cpp
View File

@ -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();
}

20
ArduCopter/mode_guided.cpp
View File

@ -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());

4
ArduCopter/mode_land.cpp
View File

@ -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();
}

12
ArduCopter/mode_loiter.cpp
View File

@ -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();
}

14
ArduCopter/mode_poshold.cpp
View File

@ -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();
}

11
ArduCopter/mode_rtl.cpp
View File

@ -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();
}

14
ArduCopter/mode_sport.cpp
View File

@ -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();
}

2
ArduCopter/mode_throw.cpp
View File

@ -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;

15
ArduCopter/mode_zigzag.cpp
View File

@ -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();
}

18
ArduCopter/takeoff.cpp
View File

@ -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