mirror of https://github.com/ArduPilot/ardupilot
Copter: minor formatting fixes
This commit is contained in:
parent
91bff63267
commit
05fedbf98f
|
@ -323,12 +323,12 @@ static void auto_land_run()
|
||||||
// run loiter controller
|
// run loiter controller
|
||||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||||
|
|
||||||
float cmb_rate = get_land_descent_speed();
|
|
||||||
|
|
||||||
// call z-axis position controller
|
// call z-axis position controller
|
||||||
|
float cmb_rate = get_land_descent_speed();
|
||||||
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
|
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
|
||||||
pos_control.update_z_controller();
|
pos_control.update_z_controller();
|
||||||
|
|
||||||
|
// record desired climb rate for logging
|
||||||
desired_climb_rate = cmb_rate;
|
desired_climb_rate = cmb_rate;
|
||||||
|
|
||||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||||
|
|
|
@ -102,7 +102,7 @@ static void land_gps_run()
|
||||||
// call attitude controller
|
// 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(), target_yaw_rate);
|
||||||
|
|
||||||
//pause 4 seconds before beginning land descent
|
// pause 4 seconds before beginning land descent
|
||||||
float cmb_rate;
|
float cmb_rate;
|
||||||
if(land_pause && millis()-land_start_time < 4000) {
|
if(land_pause && millis()-land_start_time < 4000) {
|
||||||
cmb_rate = 0;
|
cmb_rate = 0;
|
||||||
|
@ -111,6 +111,7 @@ static void land_gps_run()
|
||||||
cmb_rate = get_land_descent_speed();
|
cmb_rate = get_land_descent_speed();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// record desired climb rate for logging
|
||||||
desired_climb_rate = cmb_rate;
|
desired_climb_rate = cmb_rate;
|
||||||
|
|
||||||
// update altitude target and call position controller
|
// update altitude target and call position controller
|
||||||
|
@ -162,7 +163,7 @@ static void land_nogps_run()
|
||||||
// call attitude controller
|
// call attitude controller
|
||||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||||
|
|
||||||
//pause 4 seconds before beginning land descent
|
// pause 4 seconds before beginning land descent
|
||||||
float cmb_rate;
|
float cmb_rate;
|
||||||
if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) {
|
if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) {
|
||||||
cmb_rate = 0;
|
cmb_rate = 0;
|
||||||
|
@ -171,6 +172,7 @@ static void land_nogps_run()
|
||||||
cmb_rate = get_land_descent_speed();
|
cmb_rate = get_land_descent_speed();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// record desired climb rate for logging
|
||||||
desired_climb_rate = cmb_rate;
|
desired_climb_rate = cmb_rate;
|
||||||
|
|
||||||
// call position controller
|
// call position controller
|
||||||
|
|
|
@ -380,6 +380,7 @@ static void rtl_land_run()
|
||||||
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
|
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
|
||||||
pos_control.update_z_controller();
|
pos_control.update_z_controller();
|
||||||
|
|
||||||
|
// record desired climb rate for logging
|
||||||
desired_climb_rate = cmb_rate;
|
desired_climb_rate = cmb_rate;
|
||||||
|
|
||||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||||
|
|
Loading…
Reference in New Issue