rc_pilot is working in the full simulation!

Controllers/setPIDgains.m

@@ -10,23 +10,23 @@ function [vehicle_params] = setPIDgains(vehicle_params)
     
     % z
     vehicle_params.PIDctrl.z.kP = 8;
-    vehicle_params.PIDctrl.z.kI = 0;
+    vehicle_params.PIDctrl.z.kI = 0.1;
     vehicle_params.PIDctrl.z.kD = 1;
     
     %% Velocity -> Acceleration
     % x-dot
     vehicle_params.PIDctrl.x_dot.kP = 5;
-    vehicle_params.PIDctrl.x_dot.kI = 0;
+    vehicle_params.PIDctrl.x_dot.kI = 0.1;
     vehicle_params.PIDctrl.x_dot.kD = 0;
     
     % y-dot
     vehicle_params.PIDctrl.y_dot.kP = 5;
-    vehicle_params.PIDctrl.y_dot.kI = 0;
+    vehicle_params.PIDctrl.y_dot.kI = 0.1;
     vehicle_params.PIDctrl.y_dot.kD = 0;
     
     % z-dot
     vehicle_params.PIDctrl.z_dot.kP = 5;
-    vehicle_params.PIDctrl.z_dot.kI = 0;
+    vehicle_params.PIDctrl.z_dot.kI = 0.1;
     vehicle_params.PIDctrl.z_dot.kD = 0;
     
     %% Attitude -> Attitude Rate

Parameters/parameters_Quad.m

@@ -25,6 +25,7 @@ function [params] = parameters_Quad(global_params)
     
     % Thrust Value needed to maintain hover and fight gravity (N)
     params.hoverThrust =   (params.m * params.g);
+    params.hoverThrust = params.hoverThrust .* 1.26; % eps*m*g
     
     %% Disturbances
     params.dist_F_power = 0.1 .* [1; 1; 1];   % 0-mean Band-limited, White Gaussian Dist (Ext Force)
@@ -44,6 +45,9 @@ function [params] = parameters_Quad(global_params)
     % Controller Rate (s)
     params.ctlDt = 0.01;
     
+    % rc_pilot cascaded PID controller (specify gains)
+    params = setPIDgains(params);
+        
     %% Initial Conditions         
     params.x0 = [0;...      % x position [m]
                  0;...      % y position [m]