Core Functionality

build.xml

@@ -11,7 +11,9 @@
 	<target name="build" description="Builds the plugin.">
 		<echo>Building...</echo>
         <mkdir dir="${build.dir}"/>
-        <javac debug="true" srcdir="${src.dir}" destdir="${build.dir}" classpath="OpenRocket-15.03.jar" includeantruntime="false" source="1.8" target="1.8"/>
+        <javac debug="true" srcdir="${src.dir}" destdir="${build.dir}" classpath="OpenRocket-15.03.jar" includeantruntime="false" source="1.8" target="1.8">
+        	<exclude name="**/ORBrakeTest.java"/>
+    	</javac>
 		<echo>Done</echo>
     </target>
 

src/net/sf/openrocket/ORBrake/ORBrake.java

@@ -21,6 +21,67 @@ public String getDescription()
     @Override
     public void initialize(SimulationConditions conditions) throws SimulationException
     {
-        conditions.getSimulationListenerList().add(new ORBrakeSimulationListener());
+        conditions.getSimulationListenerList().add(new ORBrakeSimulationListener(getSetpoint(), getKp(), getKi(), getKd(), getTau(), getT()));
     }
+
+    public double getSetpoint()
+    {
+    	return config.getDouble("setpoint", 100.0);
+    }
+    public void setSetpoint(double setpoint)
+    {
+    	config.put("setpoint", setpoint);
+    	fireChangeEvent();
+    }
+
+    public double getKp()
+    {
+    	return config.getDouble("Kp", 5.0);
+    }
+    public void setKp(double Kp)
+    {
+    	config.put("Kp", Kp);
+    	fireChangeEvent();
+    }
+
+    public double getKi()
+    {
+    	return config.getDouble("Ki", 0.0);
+    }
+    public void setKi(double Ki)
+    {
+    	config.put("Ki", Ki);
+    	fireChangeEvent();
+    }
+
+    public double getKd()
+    {
+    	return config.getDouble("Kd", 0.0);
+    }
+    public void setKd(double Kd)
+    {
+    	config.put("Kd", Kd);
+    	fireChangeEvent();
+    }
+
+    public double getTau()
+    {
+    	return config.getDouble("tau", 1.0);
+    }
+    public void setTau(double tau)
+    {
+    	config.put("tau", tau);
+    	fireChangeEvent();
+    }
+
+    public double getT()
+    {
+    	return config.getDouble("T", 1.0);
+    }
+    public void setT(double T)
+    {
+    	config.put("T", T);
+    	fireChangeEvent();
+    }
+
 }

src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java

@@ -23,19 +23,90 @@ public ORBrakeConfigurator() {
 
 	@Override
 	protected JComponent getConfigurationComponent(ORBrake extension, Simulation simulation, JPanel panel) {
-		// panel.add(new JLabel("Thrust multiplier:"));
 
-		// DoubleModel m = new DoubleModel(extension, "Multiplier", UnitGroup.UNITS_RELATIVE, 0);
+		panel.add(new JLabel("Setpoint:"));
+		DoubleModel S = new DoubleModel(extension, "Setpoint", UnitGroup.UNITS_DISTANCE, 0);
 
-		// JSpinner spin = new JSpinner(m.getSpinnerModel());
-		// spin.setEditor(new SpinnerEditor(spin));
-		// panel.add(spin, "w 65lp!");
+		JSpinner spinS = new JSpinner(S.getSpinnerModel());
+		spinS.setEditor(new SpinnerEditor(spinS));
+		panel.add(spinS, "w 65lp!");
 
-		// UnitSelector unit = new UnitSelector(m);
-		// panel.add(unit, "w 25");
+		UnitSelector unitS = new UnitSelector(S);
+		panel.add(unitS, "w 25");
+
+		BasicSlider sliderS = new BasicSlider(S.getSliderModel(0, 3100));
+		panel.add(sliderS, "w 75lp, wrap");
+
+
+		panel.add(new JLabel("Proportional Gain:"));
+		DoubleModel P = new DoubleModel(extension, "Kp", UnitGroup.UNITS_COEFFICIENT, 0);
+
+		JSpinner spinP = new JSpinner(P.getSpinnerModel());
+		spinP.setEditor(new SpinnerEditor(spinP));
+		panel.add(spinP, "w 65lp!");
+
+		UnitSelector unitP = new UnitSelector(P);
+		panel.add(unitP, "w 25");
+
+		BasicSlider sliderP = new BasicSlider(P.getSliderModel(0, 10));
+		panel.add(sliderP, "w 75lp, wrap");
+
+
+		panel.add(new JLabel("Integral Gain:"));
+		DoubleModel I = new DoubleModel(extension, "Ki", UnitGroup.UNITS_COEFFICIENT, 0);
+
+		JSpinner spinI = new JSpinner(I.getSpinnerModel());
+		spinI.setEditor(new SpinnerEditor(spinI));
+		panel.add(spinI, "w 65lp!");
+
+		UnitSelector unitI = new UnitSelector(I);
+		panel.add(unitI, "w 25");
+
+		BasicSlider sliderI = new BasicSlider(I.getSliderModel(0, 5));
+		panel.add(sliderI, "w 75lp, wrap");
+
+
+		panel.add(new JLabel("Differential Gain:"));
+		DoubleModel D = new DoubleModel(extension, "Kd", UnitGroup.UNITS_COEFFICIENT, 0);
+
+		JSpinner spinD = new JSpinner(D.getSpinnerModel());
+		spinD.setEditor(new SpinnerEditor(spinD));
+		panel.add(spinD, "w 65lp!");
+
+		UnitSelector unitD = new UnitSelector(D);
+		panel.add(unitD, "w 25");
+
+		BasicSlider sliderD = new BasicSlider(D.getSliderModel(0, 5));
+		panel.add(sliderD, "w 75lp, wrap");
+
+
+		panel.add(new JLabel("Time Constant:"));
+		DoubleModel Tau = new DoubleModel(extension, "Tau", UnitGroup.UNITS_COEFFICIENT, 0);
+
+		JSpinner spinTau = new JSpinner(Tau.getSpinnerModel());
+		spinTau.setEditor(new SpinnerEditor(spinTau));
+		panel.add(spinTau, "w 65lp!");
+
+		UnitSelector unitTau = new UnitSelector(Tau);
+		panel.add(unitTau, "w 25");
+
+		BasicSlider sliderTau = new BasicSlider(Tau.getSliderModel(0, 3));
+		panel.add(sliderTau, "w 75lp, wrap");
+
+
+		panel.add(new JLabel("Sample Time:"));
+		DoubleModel T = new DoubleModel(extension, "T", UnitGroup.UNITS_TIME_STEP, 0);
+
+		JSpinner spinT = new JSpinner(T.getSpinnerModel());
+		spinT.setEditor(new SpinnerEditor(spinT));
+		panel.add(spinT, "w 65lp!");
+
+		UnitSelector unitT = new UnitSelector(T);
+		panel.add(unitT, "w 25");
+
+		BasicSlider sliderT = new BasicSlider(T.getSliderModel(0, 3));
+		panel.add(sliderT, "w 75lp, wrap");
 
-		// BasicSlider slider = new BasicSlider(m.getSliderModel(0, 3));
-		// panel.add(slider, "w 75lp, wrap");
 
 		return panel;
 	}

src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java

@@ -1,38 +1,201 @@
 package net.sf.openrocket.ORBrake;
 
 import java.lang.Math;
+import java.util.stream.IntStream; 
 import net.sf.openrocket.simulation.SimulationStatus;
 import net.sf.openrocket.simulation.exception.SimulationException;
 import net.sf.openrocket.simulation.listeners.AbstractSimulationListener;
 import net.sf.openrocket.aerodynamics.FlightConditions;
 
 public class ORBrakeSimulationListener extends AbstractSimulationListener {
 
-    private double velocity;
-    private double drag_coef = 1.3;
-    private double density = 1.225;
+//    double velocity;
+//    double altitude;
+//    double thrust;
+    double setpoint; //desired altitude in feet
+
+    // Input parameters for PID controller
+	double Kp; //proportional gain constant
+    double Ki; //integral gain constant
+    double Kd; //derivative gain constant
+    double tau; //low pass filter time constant
+//	double min_inte = 1; //integral min limit
+//	double max_inte = 5; //integral max limit
+    double T; //sample time in sec
+
+    // Memory variables for PID controller
+    double inte = 0; //integral term
+    double prev_err = 0; //previous error
+    double diff = 0; //differential term
+    double prev_measure = 0; //previous measurement
+
+    private static final double surfConst[][] = {	// Surface constants for presimulated airbrake extensions.
+    		{-0.000000000, 0.000000000, -0.000000000, 0.0000000000, 0.000000000},	// 0  %
+    		{-0.102912726, -0.000151700, 0.001042665, 0.0000041259, 0.000415087},	// 20 %
+    		{-0.183309557, -0.000256191, 0.008246675, 0.0000120688, 0.000861634},	// 40 %
+    		{-0.378713528, -0.000554695, 0.004619966, 0.0000177406, 0.001427697},	// 60 %    		
+    		{-0.679742164, -0.000995227, 0.001080119, 0.0000233844, 0.002092339},	// 80 %
+    		{-1.161195104, -0.001690272, -0.003398721, 0.0000376809, 0.002936851}	// 100%
+    };
 
-	public ORBrakeSimulationListener() {
+	public ORBrakeSimulationListener(double setpoint, double Kp, double Ki, double Kd, double tau, double T) {
 		super();
+		this.setpoint = setpoint;
+		this.Kp = Kp;
+		this.Ki = Ki;
+		this.Kd = Kd;
+		this.tau = tau;
+		this.T = T;
 	}
 
 	@Override
     public FlightConditions postFlightConditions(SimulationStatus status, FlightConditions conditions)
+    /**
+     * Called immediately after the flight conditions of the current time 
+     * step so that relevant ones can be extracted.
+     * 
+     * @param	status		Object that contains simulation status details.
+     * @param	conditions	Object that contains flight condition details.
+     * @return	null.
+     */
     {
-        velocity = conditions.getVelocity();
+//        velocity = conditions.getVelocity();
         return null; 
     }
 
     @Override
-    public double postSimpleThrustCalculation(SimulationStatus status, double thrust) throws SimulationException
+    public double postSimpleThrustCalculation(SimulationStatus status, double thrust) // throws SimulationException
+    /**
+     * Influences the thrust after it is computed at each time step 
+     * but before it is applied to the vehicle.
+     * 
+     * @param status	Object that contains simulation status details.
+     * @param thrust	The computed motor thrust.
+     * @return	The modified thrust to be actually applied.
+     */
     {
         // if (status.getSimulationTime() )
-        return thrust + AirbrakeForce(velocity);
+//    	status.getRocketVelocity().normalize();
+//    	this.thrust = thrust;
+//    	this.altitude = status.getRocketPosition().z;
+        return thrust + airbrakeForce(status, thrust);
+    }
+
+    double airbrakeForce(SimulationStatus status, double thrust)
+    {
+    	double requiredDrag = requiredDrag(setpoint, status, thrust);
+    	double surf = dragSurface(5, status.getRocketPosition().z, status.getRocketVelocity().length());
+    	if (requiredDrag > surf) {
+    		requiredDrag = surf;
+    	} else if (requiredDrag < 0) {
+    		requiredDrag = 0;
+    	}
+        return -requiredDrag;
     }
 
-    private double AirbrakeForce(double vel)
+    double requiredDrag(double SP,SimulationStatus status, double thrust) //PID controller to get updated drag coefficient
+    /**
+     * SP = desired altitude setpoint
+     * measure = actual altitude
+     */
+    {
+    	// Initial conditions  	
+    	double out = 0;
+    	double measure = status.getRocketPosition().z;
+    	double velocity = status.getRocketVelocity().length();
+
+    	if (thrust == 0)
+    	{    		
+    		// Error function
+	    	double err = SP - measure;
+
+	    	// Proportional term
+	    	double prop = Kp*err;
+
+	    	// Integral term
+	    	inte += 0.5*Ki*T*(err+prev_err);
+
+	    	// Anti-wind up (dynamic integral clamping)
+	    	double min_inte; //integral min limit
+	    	double max_inte; //integral max limit
+	    	if (dragSurface(5, measure, velocity) > prop) {
+	    		max_inte = dragSurface(5, measure, velocity) - prop;
+	    	} else {
+	    		max_inte = 0;
+	    	}
+	    	if (0 < prop) {
+	    		min_inte = 0 - prop;
+	    	} else {
+	    		min_inte = 0;
+	    	}
+	    	if (inte > max_inte) {
+	    		inte = max_inte;
+	    	} else if (inte < min_inte) {
+	    		inte = min_inte;
+	    	}
+
+	    	// Differential term
+	    	diff = ( -2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T);
+
+	    	// Output 
+	    	out = prop + inte + diff;
+
+	    	// Update memory
+	    	prev_err = err;
+	    	prev_measure = measure;
+	    }
+
+    	return out; //required drag
+    }
+
+//    double extensionFromDrag(double requiredDrag)
+//    /**
+//     * Computes the required extension to achieve a required drag.
+//     * 
+//     * @param requiredDrag	The desired drag from the control system.
+//     * @return	The percentage deployment that will produce that drag.
+//     */
+//    {
+//    	double[] drag = new double[6];
+//
+//    	// Compute drag for each known extension.
+//    	IntStream.range(0, 5).forEachOrdered(n -> {
+//    	    drag[n] = this.dragSurface(n);
+//    	});
+//    	
+//    	// Interpolate to find desired extension
+//    	double extension = 0;
+//    	double term;
+//    	
+//    	for (int i = 0; i < 5; ++i)
+//    	{
+//    		term = i;
+//    		for (int j = 0; j < 5; ++j)
+//    		{
+//        		if(j != i)
+//        		{
+//        			term *= (requiredDrag - drag[j]) / (drag[i] - drag[j]);
+//        		}
+//        	};
+//        	
+//        	extension += term;
+//    	};
+//    	extension *= 20;
+//    	return extension;
+//    }
+
+    double dragSurface(int extNum, double altitude, double velocity)
+    /**
+     * Finds the drag force at one of the 6 drag surfaces given the current 
+     * velocity and altitude.
+     * 
+     * @param	extNum	The index of the extension percentage.  Each increment 
+     * 					represents a 20% increase.
+     * @return	The drag given altitude, velocity, and extension.
+     */
     {
-        return drag_coef * density * Math.pow(vel, 2) / 2;
+    	return surfConst[extNum][0] + surfConst[extNum][1]*altitude + surfConst[extNum][2]*velocity + 
+    			surfConst[extNum][3]*altitude*velocity + surfConst[extNum][4]*Math.pow(velocity, 2);
     }
 
 }