|
|
|
@ -674,10 +674,10 @@ void Mode::land_run_horizontal_control()
@@ -674,10 +674,10 @@ void Mode::land_run_horizontal_control()
|
|
|
|
|
// there is any position estimate drift after touchdown. We
|
|
|
|
|
// limit attitude to 7 degrees below this limit and linearly
|
|
|
|
|
// interpolate for 1m above that
|
|
|
|
|
float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(), |
|
|
|
|
const float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(), |
|
|
|
|
g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); |
|
|
|
|
float thrust_vector_max = sinf(radians(attitude_limit_cd / 100.0f)) * GRAVITY_MSS * 100.0f; |
|
|
|
|
float thrust_vector_mag = norm(thrust_vector.x, thrust_vector.y); |
|
|
|
|
const float thrust_vector_max = sinf(radians(attitude_limit_cd / 100.0f)) * GRAVITY_MSS * 100.0f; |
|
|
|
|
const float thrust_vector_mag = thrust_vector.xy().length(); |
|
|
|
|
if (thrust_vector_mag > thrust_vector_max) { |
|
|
|
|
float ratio = thrust_vector_max / thrust_vector_mag; |
|
|
|
|
thrust_vector.x *= ratio; |
|
|
|
|