mirror of https://github.com/ArduPilot/ardupilot
Copter: use vector.xy().length() instead of norm(x,y)
This commit is contained in:
parent
6dfbd9ce31
commit
46ec70f9fc
|
@ -11,7 +11,7 @@ float Mode::AutoYaw::roi_yaw() const
|
||||||
float Mode::AutoYaw::look_ahead_yaw()
|
float Mode::AutoYaw::look_ahead_yaw()
|
||||||
{
|
{
|
||||||
const Vector3f& vel = copter.inertial_nav.get_velocity();
|
const Vector3f& vel = copter.inertial_nav.get_velocity();
|
||||||
float speed = norm(vel.x,vel.y);
|
float speed = vel.xy().length();
|
||||||
// Commanded Yaw to automatically look ahead.
|
// Commanded Yaw to automatically look ahead.
|
||||||
if (copter.position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
|
if (copter.position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
|
||||||
_look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f;
|
_look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f;
|
||||||
|
|
|
@ -177,7 +177,7 @@ void Copter::update_throttle_mix()
|
||||||
|
|
||||||
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
|
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
|
||||||
const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
|
const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
|
||||||
bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD);
|
bool large_angle_request = angle_target.xy().length() > LAND_CHECK_LARGE_ANGLE_CD;
|
||||||
|
|
||||||
// check for large external disturbance - angle error over 30 degrees
|
// check for large external disturbance - angle error over 30 degrees
|
||||||
const float angle_error = attitude_control->get_att_error_angle_deg();
|
const float angle_error = attitude_control->get_att_error_angle_deg();
|
||||||
|
|
|
@ -674,10 +674,10 @@ void Mode::land_run_horizontal_control()
|
||||||
// there is any position estimate drift after touchdown. We
|
// there is any position estimate drift after touchdown. We
|
||||||
// limit attitude to 7 degrees below this limit and linearly
|
// limit attitude to 7 degrees below this limit and linearly
|
||||||
// interpolate for 1m above that
|
// 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);
|
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;
|
const 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_mag = thrust_vector.xy().length();
|
||||||
if (thrust_vector_mag > thrust_vector_max) {
|
if (thrust_vector_mag > thrust_vector_max) {
|
||||||
float ratio = thrust_vector_max / thrust_vector_mag;
|
float ratio = thrust_vector_max / thrust_vector_mag;
|
||||||
thrust_vector.x *= ratio;
|
thrust_vector.x *= ratio;
|
||||||
|
|
Loading…
Reference in New Issue