mirror of https://github.com/ArduPilot/ardupilot
Copter: explicitly set alt_target_from_climb_rate parameter
This commit is contained in:
parent
11fee21f06
commit
362a43c126
|
@ -80,7 +80,7 @@ void Copter::althold_run()
|
|||
}
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
|
||||
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
}
|
||||
|
|
|
@ -325,7 +325,7 @@ void Copter::autotune_run()
|
|||
}
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -34,9 +34,6 @@ bool Copter::brake_init(bool ignore_checks)
|
|||
// should be called at 100hz or more
|
||||
void Copter::brake_run()
|
||||
{
|
||||
float target_yaw_rate = 0;
|
||||
float target_climb_rate = 0;
|
||||
|
||||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed) {
|
||||
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
|
||||
|
@ -59,11 +56,11 @@ void Copter::brake_run()
|
|||
wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f);
|
||||
|
||||
// body-frame rate controller is run directly from 100hz loop
|
||||
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
|
||||
pos_control.set_alt_target_from_climb_rate(0.0f, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
}
|
||||
|
|
|
@ -78,6 +78,6 @@ void Copter::circle_run()
|
|||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
|
||||
}
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
}
|
||||
|
|
|
@ -104,7 +104,7 @@ void Copter::loiter_run()
|
|||
}
|
||||
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
|
||||
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
}
|
||||
|
|
|
@ -510,7 +510,7 @@ void Copter::poshold_run()
|
|||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
|
||||
}
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
|
||||
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
}
|
||||
|
|
|
@ -101,7 +101,7 @@ void Copter::sport_run()
|
|||
}
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
|
||||
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue