From 5a8c542d4623f096e2ebbab77a50f8a61c2a7b51 Mon Sep 17 00:00:00 2001 From: devbharat Date: Tue, 6 Dec 2016 10:11:48 +0100 Subject: [PATCH] Added checks for setting mpc saturation flags --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 1bb0bc069d..408b9d4f39 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1880,7 +1880,8 @@ MulticopterPositionControl::control_position(float dt) /* limit min lift */ if (-thrust_sp(2) < thr_min) { thrust_sp(2) = -thr_min; - saturation_z = true; + /* Don't freeze altitude integral if it wants to throttle up */ + saturation_z = vel_err(2) > 0.0f ? true : saturation_z; } if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) { @@ -1898,7 +1899,8 @@ MulticopterPositionControl::control_position(float dt) float k = thrust_xy_max / thrust_sp_xy_len; thrust_sp(0) *= k; thrust_sp(1) *= k; - saturation_xy = true; + /* Don't freeze x,y integrals if they both want to throttle down */ + saturation_xy = ((vel_err(0) * _vel_sp(0) < 0.0f) && (vel_err(1) * _vel_sp(1) < 0.0f)) ? saturation_xy : true; } } } @@ -1934,7 +1936,8 @@ MulticopterPositionControl::control_position(float dt) thrust_sp(1) = 0.0f; thrust_sp(2) = -thr_max; saturation_xy = true; - saturation_z = true; + /* Don't freeze altitude integral if it wants to throttle down */ + saturation_z = vel_err(2) < 0.0f ? true : saturation_z; } else { /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ @@ -1943,7 +1946,8 @@ MulticopterPositionControl::control_position(float dt) float k = thrust_xy_max / thrust_xy_abs; thrust_sp(0) *= k; thrust_sp(1) *= k; - saturation_xy = true; + /* Don't freeze x,y integrals if they both want to throttle down */ + saturation_xy = ((vel_err(0) * _vel_sp(0) < 0.0f) && (vel_err(1) * _vel_sp(1) < 0.0f)) ? saturation_xy : true; } } else { @@ -2154,7 +2158,6 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) _att_sp.landing_gear = -1.0f; } - _att_sp.timestamp = hrt_absolute_time(); } @@ -2178,7 +2181,6 @@ MulticopterPositionControl::task_main() _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); _global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); - parameters_update(true); /* initialize values of critical structs until first regular update */