Update SIM_lander with new PIDController class #1006

trick_sims/SIM_lander/RUN_test/input.py

@@ -7,6 +7,7 @@ dyn.lander.vel[1] = 0.0
 dyn.lander.angleDot = 0.0
 dyn.lander.angle = 0.0
 
+trick.TMM_set_debug_level(1)
 #==========================================
 # Start the Satellite Graphics Client
 #==========================================

trick_sims/SIM_lander/S_define

@@ -2,7 +2,8 @@
 PURPOSE:
     ( Simulate a Lander. )
 LIBRARY DEPENDENCIES:
-    ((lander/src/Lander.cpp))
+    ((lander/src/Lander.cpp)
+     (PIDController/src/PIDController.cpp))
 *************************************************************/
 #include "sim_objects/default_trick_sys.sm"
 ##include "lander/include/Lander.hh"

trick_sims/SIM_lander/models/PIDController/README.md

@@ -0,0 +1,43 @@
+# class PIDController
+
+![PID Controller Diagram](images/PIDController.png)
+
+### Constructor
+
+```
+PIDController::PIDController(
+    double kp,   // proportional gain
+    double ki,   // integral gain
+    double kd,   // derivative gain
+    double omax, // limiter maximum
+    double omin, // limiter minimum
+    double dt,   // sample period
+    double tc    // filter time constant
+    );
+```
+
+
+For no filtering, set tc to the value of dt. To filter, set tc a value higher than dt.
+
+               
+```           
+double PIDController::getOutput( double setpoint_value,
+                                 double measured_value);
+```
+This function generates the control command (corresponding to **cmd** in the above diagram.) for the plant. The "plant" is the thing that you are controlling.
+
+For example, the plant could be a lunar lander. The measured value could be altitude, and the command could be motor thrust. The setpoint is the what you want the output of the plant to be. The setpoint could be an altitude of 100 feet. The measured_value is the actual output (the actual altitude) of the lander plus perhaps some noise (because sensors aren't perfect).
+
+### References
+
+The following videos provide some excellent instruction about PID control:
+
+[Understanding PID Control, Part 1: What is PID Control?]
+(https://www.youtube.com/watch?v=wkfEZmsQqiA)
+
+[Understanding PID Control, Part 2: Expanding Beyond a Simple Integral](https://www.youtube.com/watch?v=NVLXCwc8HzM)
+
+[Understanding PID Control, Part 3: Part 3: Expanding Beyond a Simple Derivative](https://www.youtube.com/watch?v=7dUVdrs1e18)
+
+[Understanding PID Control, Part 4: A PID Tuning Guide]
+(https://www.youtube.com/watch?v=sFOEsA0Irjs)

trick_sims/SIM_lander/models/PIDController/include/PIDController.hh

@@ -0,0 +1,23 @@
+#ifndef PIDCONTROLLER_HH
+#define PIDCONTROLLER_HH
+
+class PIDController {
+    public:
+    double Kp;
+    double Kd;
+    double Ki;
+    double Dt;
+    double k;
+    double error;
+    double integral;
+    double out_max;
+    double out_min;
+    double previous_error;
+    double prev_setpoint_value;
+    bool   integration_enabled;
+
+    PIDController( double kp, double ki, double kd, double omax, double omin, double dt, double tc );
+    double getOutput(double setpoint_value, double measured_value);
+};
+
+#endif

trick_sims/SIM_lander/models/PIDController/src/PIDController.cpp

@@ -0,0 +1,76 @@
+
+#include "PIDController.hh"
+
+PIDController::PIDController( double kp, double ki, double kd, double omax, double omin, double dt, double tc ) {
+    Kp=kp;
+    Ki=ki;
+    Kd=kd;
+    Dt=dt;
+    k = dt/tc;
+    integral = 0.0;
+    out_max = omax;
+    out_min = omin;
+    error = 0.0;
+    previous_error = 0.0;
+    prev_setpoint_value = 0.0;
+    integration_enabled = true;
+}
+
+double PIDController::getOutput(double setpoint_value, double measured_value) {
+
+    double error_unfiltered = setpoint_value - measured_value;
+
+    // Low pass filter for our error signal.
+    error = error + k * (error_unfiltered - error);
+
+    // If we suddenly change the set point value then we need to reset
+    // our previous_error, otherwise we'll get a spike in our derivative.
+    if (prev_setpoint_value != setpoint_value) {
+        previous_error = error;
+    }
+
+    // Integration is normally enabled unless we are mitigating integral windup.
+    if (integration_enabled) {
+        integral = integral + error * Dt;
+    }
+
+    double derivative = (error - previous_error) / Dt;
+
+    double pterm = Kp * error;
+    double iterm = Ki * integral;
+    double dterm = Kd * derivative;
+    double output = pterm + iterm + dterm;
+
+    bool same_sign;
+    // If the signs of the error and the output are the same,
+    // then the output isn't reducing the error.
+    if ((error * output) > 0.0) {
+        same_sign = 1;
+    } else {
+        same_sign = 0;
+    }
+
+    bool output_limited;
+    // Check for saturation.
+    if (output > out_max) {
+        output = out_max;
+        output_limited = 1;
+    }
+    if (output < out_min) {
+        output = out_min;
+        output_limited = 1;
+    }
+
+    // If the output isn't reducing the error, and is being limited,
+    // then we need to inhibit integration to prevent integral windup.
+    if (same_sign && output_limited) {
+        integration_enabled = false;
+    } else {
+        integration_enabled = true;
+    }
+
+    previous_error = error;
+    prev_setpoint_value = setpoint_value;
+
+    return ( output );
+}

trick_sims/SIM_lander/models/graphics/src/LanderDisplay.java

@@ -66,7 +66,9 @@ class RangeView extends JPanel {
     private double[] landerVel;
     private double   landerAngle;
     private double   landerAngleRate;
+    private double   nozzleAngle;
     private int      landerThrottle;    /* percent */
+    private int      rcs_state;
 
     private ScenePoly left_pad, right_pad;
     private ScenePoly left_L1,  right_L1;
@@ -77,6 +79,7 @@ class RangeView extends JPanel {
     private ScenePoly nozzle;
     private ScenePoly left_rcs_pod, right_rcs_pod;
     private ScenePoly left_top_rcs_nozzle, right_top_rcs_nozzle, left_bottom_rcs_nozzle, right_bottom_rcs_nozzle;
+    private ScenePoly left_top_rcs_plume, right_top_rcs_plume, left_bottom_rcs_plume, right_bottom_rcs_plume;
     private ScenePoly docking_ring;
     private ScenePoly flame;
 
@@ -84,7 +87,7 @@ class RangeView extends JPanel {
 
     // Controls
     private int throttleChange ;
-    private int rcs_state;
+    private int rcs_state_chg_cmd;
     private boolean altitudeCtrlEnabled;
     private double altitudeSetPoint;
     private boolean downRangeCtrlEnabled;
@@ -108,6 +111,7 @@ class RangeView extends JPanel {
         landerColor = new Color(150,10,10);
 
         landerAngle = 0.0;
+        nozzleAngle = 0.0;
         landerPos  = new double[]
             {5.0, 0.0};
         landerVel  = new double[]
@@ -204,24 +208,48 @@ class RangeView extends JPanel {
         left_top_rcs_nozzle.y  = new double[] { 0.1, 0.1, 0.2, 0.2 };
         left_top_rcs_nozzle.n = 4;
 
+        left_top_rcs_plume = new ScenePoly();
+        left_top_rcs_plume.color = new Color(255,255,255);
+        left_top_rcs_plume.x = new double[]  { -1.1, -1.2, -1.24, -1.06 };
+        left_top_rcs_plume.y  = new double[] { 0.2, 0.2, 0.4, 0.4 };
+        left_top_rcs_plume.n = 4;
+
         right_top_rcs_nozzle = new ScenePoly();
         right_top_rcs_nozzle.color = new Color(255,100,100);
         right_top_rcs_nozzle.x = new double[]  { 1.12, 1.18, 1.2, 1.1 };
         right_top_rcs_nozzle.y  = new double[] { 0.1, 0.1, 0.2, 0.2 };
         right_top_rcs_nozzle.n = 4;
 
+        right_top_rcs_plume = new ScenePoly();
+        right_top_rcs_plume.color = new Color(255,255,255);
+        right_top_rcs_plume.x = new double[]  { 1.1, 1.2, 1.24, 1.06 };
+        right_top_rcs_plume.y  = new double[] { 0.2, 0.2, 0.4, 0.4 };
+        right_top_rcs_plume.n = 4;
+
         left_bottom_rcs_nozzle = new ScenePoly();
         left_bottom_rcs_nozzle.color = new Color(255,100,100);
         left_bottom_rcs_nozzle.x = new double[]  { -1.12, -1.18, -1.2, -1.1 };
         left_bottom_rcs_nozzle.y  = new double[] { -0.1, -0.1, -0.2, -0.2 };
         left_bottom_rcs_nozzle.n = 4;
 
+        left_bottom_rcs_plume = new ScenePoly();
+        left_bottom_rcs_plume.color = new Color(255,255,255);
+        left_bottom_rcs_plume.x = new double[]  { -1.1, -1.2, -1.24, -1.06 };
+        left_bottom_rcs_plume.y  = new double[] { -0.2, -0.2, -0.4, -0.4 };
+        left_bottom_rcs_plume.n = 4;
+
         right_bottom_rcs_nozzle = new ScenePoly();
         right_bottom_rcs_nozzle.color = new Color(255,100,100);
         right_bottom_rcs_nozzle.x = new double[]  { 1.12, 1.18, 1.2, 1.1 };
         right_bottom_rcs_nozzle.y  = new double[] { -0.1, -0.1, -0.2, -0.2 };
         right_bottom_rcs_nozzle.n = 4;
 
+        right_bottom_rcs_plume = new ScenePoly();
+        right_bottom_rcs_plume.color = new Color(255,255,255);
+        right_bottom_rcs_plume.x = new double[]  {  1.1,  1.2,  1.24, 1.06 };
+        right_bottom_rcs_plume.y  = new double[] { -0.2, -0.2, -0.4, -0.4 };
+        right_bottom_rcs_plume.n = 4;
+
         docking_ring = new ScenePoly();
         docking_ring.color = new Color(100,100,200);
         docking_ring.x = new double[]  { 0.4, 0.4, -0.4, -0.4 };
@@ -249,10 +277,12 @@ class RangeView extends JPanel {
         g.fillPolygon(workPolyX, workPolyY, p.n);
     }
 
-    public void rcs_ccw()       { rcs_state = 1; }
+    public void rcs_ccw()         { rcs_state_chg_cmd = 1; }
-    public void rcs_cw()        { rcs_state = -1; }
+    public void rcs_cw()          { rcs_state_chg_cmd = -1; }
-    public void rcs_zero()      { rcs_state = 0; }
+    public void rcs_zero_change() { rcs_state_chg_cmd = 0; }
-    public int  get_rcs_state() { return rcs_state; }
+
+    public int  get_rcs_state_chg() { return rcs_state_chg_cmd; }
+    public void set_rcs_state(int state) { rcs_state = state; }
 
     public void throttleUp()         { throttleChange =    1; }
     public void throttleDown()       { throttleChange =   -1; }
@@ -274,12 +304,15 @@ class RangeView extends JPanel {
     public void setDownRangeSetPoint(double setpoint) { downRangeSetPoint = setpoint; }
     public double getDownRangeSetPoint()    { return downRangeSetPoint; }
 
-    public void setLanderAngle(double angle_r) {
+    public void setLanderAngle(double angle) {
-        landerAngle = angle_r;
+        landerAngle = angle;
     }
     public void setLanderAngleRate(double angle_rate) {
         landerAngleRate = angle_rate;
     }
+    public void setNozzleAngle(double angle) {
+        nozzleAngle = angle;
+    }
     public void setLanderPos(double x, double y) {
         landerPos[0] = x;
         landerPos[1] = y;
@@ -355,7 +388,7 @@ class RangeView extends JPanel {
         drawScenePoly(g2d, left_L4, landerAngle, landerPos[0], landerPos[1]);
         drawScenePoly(g2d, right_L4, landerAngle, landerPos[0], landerPos[1]);
         drawScenePoly(g2d, fuselage, landerAngle, landerPos[0], landerPos[1]);
-        drawScenePoly(g2d, nozzle, landerAngle, landerPos[0], landerPos[1]);
+        drawScenePoly(g2d, nozzle, (landerAngle + nozzleAngle), landerPos[0], landerPos[1]);
         drawScenePoly(g2d, left_rcs_pod, landerAngle, landerPos[0], landerPos[1]);
         drawScenePoly(g2d, right_rcs_pod, landerAngle, landerPos[0], landerPos[1]);
 
@@ -366,6 +399,14 @@ class RangeView extends JPanel {
         drawScenePoly(g2d, docking_ring, landerAngle, landerPos[0], landerPos[1]);
         drawScenePoly(g2d, flame, landerAngle, landerPos[0], landerPos[1]);
 
+        if ( rcs_state > 0) {
+            drawScenePoly(g2d, left_top_rcs_plume, landerAngle, landerPos[0], landerPos[1]);
+            drawScenePoly(g2d, right_bottom_rcs_plume, landerAngle, landerPos[0], landerPos[1]);
+        } else if ( rcs_state < 0) {
+            drawScenePoly(g2d, right_top_rcs_plume, landerAngle, landerPos[0], landerPos[1]);
+            drawScenePoly(g2d, left_bottom_rcs_plume, landerAngle, landerPos[0], landerPos[1]);
+        }
+
         // ===============================================================================
         // Move the first vertex of the triangular polygon that represents the main-engine
         // plume to make the triangle bigger or smaller depending on the throttle setting.
@@ -373,9 +414,9 @@ class RangeView extends JPanel {
 
         for (ii = 0; ii < flame.n; ii++) {
             workPolyX[ii] = (int)(worldOriginX + scale *
-                ( Math.cos(landerAngle) * flame.x[ii] - Math.sin(landerAngle) * flame.y[ii] + landerPos[0] ));
+                ( Math.cos(landerAngle+nozzleAngle) * flame.x[ii] - Math.sin(landerAngle+nozzleAngle) * flame.y[ii] + landerPos[0] ));
             workPolyY[ii] = (int)(worldOriginY - scale *
-                ( Math.sin(landerAngle) * flame.x[ii] + Math.cos(landerAngle) * flame.y[ii] + landerPos[1] ));
+                ( Math.sin(landerAngle+nozzleAngle) * flame.x[ii] + Math.cos(landerAngle+nozzleAngle) * flame.y[ii] + landerPos[1] ));
         }
         g2d.setPaint(flame.color);
         g2d.fillPolygon(workPolyX, workPolyY, flame.n);
@@ -401,8 +442,8 @@ class RangeView extends JPanel {
         double angle_rate_d = landerAngleRate * 57.29577;
         g2d.drawString ( String.format("SCALE: %d pixels/meter",scale), 20,20);
         g2d.drawString ( String.format("Throttle : [%d]", landerThrottle), 20,40);
-        g2d.drawString ( String.format("Lander Angle (Deg): [%.2f]", angle_d), 20,60);
+        g2d.drawString ( String.format("Lander Pitch (Deg): [%.2f]", angle_d), 20,60);
-        g2d.drawString ( String.format("Lander AngleRate (Deg/s): [%.2f]", angle_rate_d), 20,80);
+        g2d.drawString ( String.format("Lander PitchRate (Deg/s): [%.2f]", angle_rate_d), 20,80);
         g2d.drawString ( String.format("Lander Pos: [%.2f, %.2f]", landerPos[0], landerPos[1]), 20,100);
         g2d.drawString ( String.format("Lander Vel: [%.2f, %.2f]", landerVel[0], landerVel[1]), 20,120);
 
@@ -790,13 +831,15 @@ public class LanderDisplay extends JFrame {
         double posy = 0.0;
         double angle = 0.0;
         double angle_rate = 0.0;
+        double nozzle_angle = 0.0;
         double velx = 0.0;
         double vely = 0.0;
         int throttle = 0;
+        int rcs_state = 0;
 
         // Outbound command variables
         int throttle_delta;
-        int rcs_state;
+        int rcs_state_chg_cmd;
 
         int simMode = 0;
         boolean standalone = false;
@@ -844,11 +887,13 @@ public class LanderDisplay extends JFrame {
         landerDisplay.out.writeBytes( "trick.var_pause() \n" +
                             "trick.var_add(\"dyn.lander.pos[0]\")\n" +
                             "trick.var_add(\"dyn.lander.pos[1]\")\n" +
-                            "trick.var_add(\"dyn.lander.angle\")\n" +
+                            "trick.var_add(\"dyn.lander.pitch\")\n" +
                             "trick.var_add(\"dyn.lander.vel[0]\")\n" +
                             "trick.var_add(\"dyn.lander.vel[1]\")\n" +
-                            "trick.var_add(\"dyn.lander.angleDot\")\n" +
+                            "trick.var_add(\"dyn.lander.pitchDot\")\n" +
+                            "trick.var_add(\"dyn.lander.nozzle_angle\")\n" +
                             "trick.var_add(\"dyn.lander.throttle\")\n" +
+                            "trick.var_add(\"dyn.lander.rcs_command\")\n" +
                             "trick.var_add(\"trick_sys.sched.mode\")\n" +
         //  2) We want the responses in ASCII:
                             "trick.var_ascii() \n" +
@@ -872,8 +917,10 @@ public class LanderDisplay extends JFrame {
                 velx   = Double.parseDouble( field[4]);
                 vely   = Double.parseDouble( field[5]);
                 angle_rate = Double.parseDouble( field[6]);
-                throttle = Integer.parseInt( field[7]);
+                nozzle_angle = Double.parseDouble( field[7]);
-                simMode = Integer.parseInt( field[8]);
+                throttle = Integer.parseInt( field[8]);
+                rcs_state = Integer.parseInt( field[9]);
+                simMode = Integer.parseInt( field[10]);
             } catch (IOException | NullPointerException e ) {
                 go = false;
             }
@@ -882,16 +929,18 @@ public class LanderDisplay extends JFrame {
             rangeView.setLanderPos(posx, posy);
             rangeView.setLanderVel(velx, vely);
             rangeView.setLanderAngle(angle);
+            rangeView.setNozzleAngle(nozzle_angle);
             rangeView.setLanderAngleRate(angle_rate);
             rangeView.setLanderThrottle(throttle);
+            rangeView.set_rcs_state(rcs_state);
 
             throttle_delta = rangeView.getThrottleDelta();
             landerDisplay.out.writeBytes( String.format("dyn.lander.manual_throttle_change_command = %d ;\n", throttle_delta ));
             rangeView.throttleZeroChange();
 
-            rcs_state = rangeView.get_rcs_state();
+            rcs_state_chg_cmd = rangeView.get_rcs_state_chg();
-            landerDisplay.out.writeBytes( String.format("dyn.lander.manual_rcs_command = %d ;\n", rcs_state ));
+            landerDisplay.out.writeBytes( String.format("dyn.lander.manual_rcs_command = %d ;\n", rcs_state_chg_cmd ));
-            rangeView.rcs_zero();
+            rangeView.rcs_zero_change();
 
             if (rangeView.getAltitudeCtrlState()) {
                 landerDisplay.out.writeBytes("dyn.lander.altitudeCtrlEnabled = 1 ;\n");

trick_sims/SIM_lander/models/lander/include/Lander.hh

@@ -6,23 +6,7 @@ LIBRARY DEPENDENCIES:
 #ifndef _lander_hh_
 #define _lander_hh_
 
-class PIDController {
+#include "PIDController/include/PIDController.hh"
-    public:
-    double Kp;
-    double Kd;
-    double Ki;
-    double Dt;
-    double error;
-    double previous_error;
-    double integral;
-    double derivative;
-    //int    first_time;
-    double integral_max;
-    double prev_set_point_value;
-
-    PIDController( double kp, double ki, double kd, double imax, double dt );
-    double getOutput(double commanded_value, double measured_value);
-};
 
 class Lander {
 
@@ -30,31 +14,37 @@ class Lander {
         double pos[2] ;    /* (m)     xy-position */
         double vel[2] ;    /* (m/s)   xy-velocity */
         double acc[2] ;    /* (m/s2)  xy-acceleration  */
-        double angle;
+
-        double angleDot;
+        double pitch;      /* (rad)    pitch angle */
-        double angleDDot;
+        double pitchDot;   /* (rad/s)  pitch angular-rate */
+        double pitchDDot;  /* (rad/s2) pitch angular-acceleration */
+
+        double nozzle_angle;       /* (rad) */
+        double main_engine_offset; /* (m) distance of main engine swivel from vehicle center of gravity. */
         double thrust_max;         /* (N) Maximum thrust that the main engine can produce. */
-        double rcs_torque;         /* */
+        double rcs_thrust;         /* (N) Thrust produced when RCS motor is "On". */
+        double rcs_offset;         /* (m) distance of RCS motors from vehicle center of gravity. */
         double mass;               /* (kg) */
         double moment_of_inertia;  /* (kg/m2) */
         double g;                  /* (m/s2) - acceleration of gravity. */
-        int throttle;
+        int throttle;              /* -- Percentage of thrust_max. Range: 0 .. 100. */
-        int rcs_command;
+        int rcs_command;           /* -- -1 = counter-clockwise, 0 = no torque, 1 = clockwise */
 
         // Controls from the client
-        int manual_throttle_change_command;       /* add to throttle */
+        int manual_throttle_change_command;     /* add to throttle */
         int manual_rcs_command;                 /* -1 = counter-clockwise, 0 = no torque, 1 = clockwise */
-        int altitudeCtrlEnabled;
+
+        int altitudeCtrlEnabled;                /* -- 0= disabled, 1=ensabled */
         int downRangeCtrlEnabled;
 
         PIDController* posxCntrl;
         PIDController* posyCntrl;
-
+        PIDController* energy_controller;
-        PIDController* thrust_controller;
-
         PIDController* velxCntrl;
         PIDController* velyCntrl;
-        PIDController* angleCntrl;
+        PIDController* nozzlePitchController;
+        PIDController* nozzlePitchRateController;
+        PIDController* RCSpitchController;
 
         double x_cmd;
         double y_cmd;
@@ -62,8 +52,9 @@ class Lander {
         double vx_cmd;
         double ay_cmd;
         double ax_cmd;
-        double angle_cmd;
+        double pitch_cmd;
-        double angle_dot_cmd;
+        double pitch_dot_cmd;
+        double pitch_ddot_cmd;
 
         int default_data();
         int state_init();

trick_sims/SIM_lander/models/lander/src/Lander.cpp

@@ -3,7 +3,8 @@ PURPOSE: ( Simulate a satellite orbiting the Earth. )
 LIBRARY DEPENDENCY:
     ((Lander.o))
 *******************************************************************************/
-#include "../include/Lander.hh"
+#include "lander/include/Lander.hh"
+
 #include "trick/integrator_c_intf.h"
 #include <math.h>
 #include <iostream>
@@ -13,56 +14,25 @@ LIBRARY DEPENDENCY:
 
 int pid_debug = 0;
 
-PIDController::PIDController( double kp, double ki, double kd, double imax, double dt ) {
-    Kp=kp; Ki=ki; Kd=kd; Dt=dt;
-    previous_error = 0;
-    integral = 0;
-    integral_max = imax;
-    prev_set_point_value = 0.0;
-}
-
-double PIDController::getOutput(double set_point_value, double measured_value) {
-
-    error = set_point_value - measured_value;
-
-    if (prev_set_point_value != set_point_value) {
-        previous_error = error;
-    }
-
-    integral = integral + error * Dt;
-
-    if (integral < -integral_max) integral = -integral_max;
-    if (integral >  integral_max) integral =  integral_max;
-
-    derivative = (error - previous_error) / Dt;
-    previous_error = error;
-    prev_set_point_value = set_point_value;
-
-    double pterm = Kp * error;
-    double iterm = Ki * integral;
-    double dterm = Kd * derivative;
-    double output = pterm + iterm + dterm;
-
-    return ( output );
-}
-
 int Lander::default_data() {
 
     pos[0] = 0.0;
     pos[1] = 1.8;
     vel[0] = 0.0;
     vel[1] = 0.0;
-    angle = 0.0;
+    pitch = 0.0;
-    angleDot = 0.0;
+    pitchDot = 0.0;
-    angleDDot = 0.0;
+    pitchDDot = 0.0;
 
-    // thrust_max = 15000;
     thrust_max = 6500;
-    rcs_torque = 50;
+    rcs_thrust = 50;
+    rcs_offset = 1.15;
     mass = 2000;
     moment_of_inertia = 2000;
+    main_engine_offset = 1.0;
     g = 1.62;                  // Gravitional acceleration on the moon.
 
+    nozzle_angle = 0.0;
     throttle = 0;
     rcs_command = 0;
 
@@ -81,80 +51,130 @@ int Lander::default_data() {
 
 int Lander::state_init() {
 
-    posxCntrl  = new PIDController( 0.20, 0.01, 0.40, 50.0, 0.10 );
+    // =========================================================================
-    velxCntrl  = new PIDController( 0.20, 0.01, 0.40, 50.0, 0.10 );
+    //  Horizontal Position Controllers
+    // -------------------------------------------------------------------------
+    // Horizontal Position Controller
+    // Generates x-axis (horizontal) velocity commands.
+    posxCntrl  = new PIDController( 0.20, 0.0, 0.5 , 3.0,  -3.0,  0.10, 0.10 );
+    // -------------------------------------------------------------------------
+    // Horizontal Velocity Controller
+    // Generates x-axis (horizontal) acceleration commands.
+    // We chose to limit it to %5 of maximum possible acceleration.
+    double ax_limit = 0.05 * (thrust_max / mass);
+    velxCntrl  = new PIDController( 0.20, 0.0, 0.5, ax_limit,  -ax_limit,  0.10, 0.10 );
 
-    posyCntrl  = new PIDController( 0.40, 0.1,  1.0, 50.0, 0.10 );
+    // =========================================================================
-    velyCntrl  = new PIDController( 0.40, 0.00, 1.0, 10.0, 0.10 );
+    // Vertical Position Controllers (Below 100 meters)
+    // -------------------------------------------------------------------------
+    // Vertical Position Controller
+    // Generates y-axis (vertical) velocity commands.
+    posyCntrl  = new PIDController( 0.50, 0.1, 0.5, 5.0,  -2.0,  0.10, 0.10 );
+    // -------------------------------------------------------------------------
+    // Vertical Velocity Controller
+    // Generates y-axis (vertical) acceleration commands.
+    // We chose to limit it to %95 of maximum possible acceleration.
+    double ay_limit = 0.95 * (thrust_max / mass);
+    velyCntrl  = new PIDController( 0.50, 0.1, 0.5, ay_limit, -ay_limit,  0.10, 0.10 );
 
-    angleCntrl = new PIDController( 0.20, 0.02, 0.80, 10.0, 0.10 );
+    // =========================================================================
+    // Vertical Position Controller (Above 100 meters)
+    // Generates y-axis (vertical) acceleration commands.
+    // Setpoint: Potential Energy per Kg due to gravity between the commanded altitude and the surface.
+    // Measured value: Total Energy per Kg (Potential + Kinetic) due to the current altitude and altitude rate.
+    // Output: Commanded vertical acceleration.
+    energy_controller  = new PIDController( 0.1, 0.01, 0.1, 30.0,  0.00,   0.10, 0.10 );
 
-    thrust_controller  = new PIDController( 0.2, 0.001, 0.00, 100.0, 0.10 );
+    // =========================================================================
+    // Pitch Controllers (when throttle > 10)
+    // -------------------------------------------------------------------------
+    // Pitch Controller
+    // Generates pitch rate commands (radians / sec)
+    nozzlePitchController = new PIDController( 0.50, 0.0, 0.5, 0.2, -0.2, 0.10 , 0.10);
+
+    // -------------------------------------------------------------------------
+    // Pitch Rate Controller
+    // Generates pitch angular acceleration commands (radians /sec^2)
+    nozzlePitchRateController = new PIDController( 0.50, 0.1, 0.5, 0.2, -0.2, 0.10 , 0.10);
+
+    // =========================================================================
+    // Pitch Controller (when throttle < 10)
+    // -------------------------------------------------------------------------
+    // RCS Pitch Controller
+    // Generates pitch rate commands (radians / sec) for the RCS.
+    RCSpitchController = new PIDController( 0.10, 0.5, 0.0, (RAD_PER_DEG * 2.0), -(RAD_PER_DEG * 2.0), 0.10 , 0.10);
 
     return (0);
-
 }
 
-#define MAX_CTRL_ANGLE 30.0
 int Lander::control() {
 
-    if (altitudeCtrlEnabled == 1) {
+    if (altitudeCtrlEnabled) {
 
-        if (pos[1] < 50.0) {
+        double Eppkg = g * y_cmd;                                // Potential Energy per kg.
-             vy_cmd = posyCntrl->getOutput(y_cmd, pos[1]);
+        double Etpkg = g * pos[1] + 0.5 * fabs(vel[1]) * vel[1]; // Total Energy per kg.
-             if (vy_cmd > 50.0 ) vy_cmd = 50.0;
+        double Ediff = fabs(Eppkg - Etpkg);
-             if (vy_cmd < -5.0) vy_cmd = -5.0;
+
-             ay_cmd = g + velyCntrl->getOutput(vy_cmd, vel[1]);
+        if ((pos[1] < 100.0) && (Ediff < 50.0) ){
+             vy_cmd = posyCntrl->getOutput(y_cmd,  pos[1]);
+             ay_cmd = velyCntrl->getOutput(vy_cmd, vel[1]);
         } else {
             // This controller is intended to be more energy efficient.
-             ay_cmd = g + thrust_controller->getOutput(
+             ay_cmd = energy_controller->getOutput(
-                  g * y_cmd,
+                  g * y_cmd,                                   // Potential Energy
-                  g * pos[1] + 0.5 * fabs(vel[1]) * vel[1]
+                  g * pos[1] + 0.5 * fabs(vel[1]) * vel[1]     // Kinetic Energy
               );
         }
 
-         throttle = (ay_cmd * mass * 100) / thrust_max;
+        if ((downRangeCtrlEnabled) && (throttle >= 10)) {
-         if (throttle > 100 ) throttle = 100;
-         if (throttle <   0) throttle = 0;
-    }
 
-    if (downRangeCtrlEnabled == 1) {
-        if (throttle >= 10 ) {
             vx_cmd = posxCntrl->getOutput(x_cmd, pos[0]);
-            double thrust = (throttle / 100.0) * thrust_max;
-            double a_max = thrust / mass;
             ax_cmd = velxCntrl->getOutput(vx_cmd, vel[0]);
-            if (ax_cmd >  0.707 * a_max ) ax_cmd =  0.707 * a_max ;
+
-            if (ax_cmd < -0.707 * a_max ) ax_cmd = -0.707 * a_max ;
+            if (ay_cmd > 0.0) {
-            double acc_ratio = ax_cmd / a_max ; // must be between 0..1
+                pitch_cmd = -atan( ax_cmd / ay_cmd );
-            angle_cmd = -asin( acc_ratio );
+            } else {
+                pitch_cmd = 0.0;
+            }
+
+            rcs_command = 0;
+
+            pitch_dot_cmd  = nozzlePitchController->getOutput(pitch_cmd, pitch);
+            pitch_ddot_cmd = nozzlePitchRateController->getOutput(pitch_dot_cmd, pitchDot);
+            double thrust = (throttle / 100.0) * thrust_max;
+            // Calculate the nozzle angle that would give us the commanded pitch angular acceleration.
+            nozzle_angle = asin( (moment_of_inertia * pitch_ddot_cmd)/ (main_engine_offset * thrust) );
+
+
         } else {
-            angle_cmd = 0.0;
+            vx_cmd = 0.0;
+            ax_cmd = 0.0;
+            pitch_cmd = 0.0;
+
+            nozzle_angle = 0.0;
+
+            pitch_dot_cmd = RCSpitchController->getOutput(pitch_cmd, pitch);
+
+            if (( pitch_dot_cmd - pitchDot ) > (RAD_PER_DEG * 1.0) ) {
+                rcs_command = 1;
+            } else if (( pitch_dot_cmd - pitchDot ) < (-RAD_PER_DEG * 1.0) ) {
+                rcs_command = -1;
+            }
         }
 
-        if (angle_cmd >  MAX_CTRL_ANGLE * RAD_PER_DEG) angle_cmd =  MAX_CTRL_ANGLE * RAD_PER_DEG;
+        double a_cmd = sqrt(ax_cmd * ax_cmd + ay_cmd * ay_cmd);
-        if (angle_cmd < -MAX_CTRL_ANGLE * RAD_PER_DEG) angle_cmd = -MAX_CTRL_ANGLE * RAD_PER_DEG;
+        throttle = (a_cmd * mass * 100) / thrust_max;
 
-        pid_debug = 1;
-        angle_dot_cmd = angleCntrl->getOutput(angle_cmd, angle);
-        pid_debug = 0;
-
-        if ((angle_dot_cmd - angleDot) > 0.1 * RAD_PER_DEG) {
-            rcs_command =  1;
-        } else if ((angle_dot_cmd - angleDot) < -0.1 * RAD_PER_DEG) {
-            rcs_command = -1;
-        } else {
-            rcs_command =  0;
-        }
-
-    } else {
-        rcs_command = manual_rcs_command;
     }
 
     throttle += manual_throttle_change_command;
     if (throttle > 100 ) throttle = 100;
     if (throttle <   0) throttle = 0;
 
+    rcs_command += manual_rcs_command;
+    if (rcs_command >  1) rcs_command =  1;
+    if (rcs_command < -1) rcs_command = -1;
+
     return (0);
 }
 
@@ -162,24 +182,28 @@ int Lander::state_deriv() {
 
    double thrust = (throttle / 100.0) * thrust_max;
 
-   acc[0] = (thrust * -sin(angle)) / mass;
+    acc[0] = (thrust * -sin(pitch)) / mass;
-   acc[1] = (thrust *  cos(angle)) / mass - g;
+    acc[1] = (thrust *  cos(pitch)) / mass - g;
 
-   angleDDot = rcs_command * rcs_torque / moment_of_inertia;
+    double torque_main_engine = main_engine_offset * thrust * sin( nozzle_angle );
+    double torque_rcs = rcs_command * rcs_thrust * rcs_offset;
+    double torque_total = torque_main_engine + torque_rcs;
 
-   return(0);
+    pitchDDot = torque_total / moment_of_inertia;
+
+    return(0);
 }
 
 int Lander::state_integ() {
 
    int integration_step;
 
-   load_state ( &pos[0], &pos[1], &vel[0], &vel[1], &angle, &angleDot, (double*)0);
+   load_state ( &pos[0], &pos[1], &vel[0], &vel[1], &pitch, &pitchDot, (double*)0);
-   load_deriv ( &vel[0], &vel[1], &acc[0], &acc[1], &angleDot, &angleDDot, (double*)0);
+   load_deriv ( &vel[0], &vel[1], &acc[0], &acc[1], &pitchDot, &pitchDDot, (double*)0);
 
    integration_step = integrate();
 
-   unload_state( &pos[0], &pos[1], &vel[0], &vel[1], &angle, &angleDot, (double*)0);
+   unload_state( &pos[0], &pos[1], &vel[0], &vel[1], &pitch, &pitchDot, (double*)0);
 
    return(integration_step);
 }
@@ -189,8 +213,8 @@ int Lander::check_ground_contact() {
         pos[1] = 1.8;
         vel[0] = 0.0;
         vel[1] = 0.0;
-        angle = 0.0;
+        pitch = 0.0;
-        angleDot = 0.0;
+        pitchDot = 0.0;
     }
     return(0);
 }