1 package net.sf.openrocket.simulation.listeners.example;
3 import net.sf.openrocket.aerodynamics.FlightConditions;
4 import net.sf.openrocket.rocketcomponent.FinSet;
5 import net.sf.openrocket.rocketcomponent.RocketComponent;
6 import net.sf.openrocket.simulation.FlightDataType;
7 import net.sf.openrocket.simulation.SimulationStatus;
8 import net.sf.openrocket.simulation.exception.SimulationException;
9 import net.sf.openrocket.simulation.listeners.AbstractSimulationListener;
10 import net.sf.openrocket.unit.UnitGroup;
11 import net.sf.openrocket.util.MathUtil;
14 * An example listener that applies a PI-controller to adjust the cant of fins
15 * named "CONTROL" to stop the rocket from rolling.
17 * @author Sampo Niskanen <sampo.niskanen@iki.fi>
19 public class RollControlListener extends AbstractSimulationListener {
21 // Name of control fin set
22 private static final String CONTROL_FIN_NAME = "CONTROL";
24 // Define custom flight data type
25 private static final FlightDataType FIN_CANT_TYPE = FlightDataType.getType("Control fin cant",
26 UnitGroup.UNITS_ANGLE);
28 // Simulation time at which PID controller is activated
29 private static final double START_TIME = 0.5;
31 // Desired roll rate (rad/sec)
32 private static final double SETPOINT = 0.0;
34 // Maximum control fin turn rate (rad/sec)
35 private static final double TURNRATE = 10 * Math.PI / 180;
37 // Maximum control fin angle (rad)
38 private static final double MAX_ANGLE = 15 * Math.PI / 180;
44 * At M=0.3 KP oscillation threshold between 0.35 and 0.4. Good KI=3
45 * At M=0.6 KP oscillation threshold between 0.07 and 0.08 Good KI=2
46 * At M=0.9 KP oscillation threshold between 0.013 and 0.014 Good KI=0.5
48 private static final double KP = 0.007;
49 private static final double KI = 0.2;
54 private double rollrate;
56 private double prevTime = 0;
57 private double intState = 0;
59 private double finPosition = 0;
64 public FlightConditions postFlightConditions(SimulationStatus status, FlightConditions flightConditions) {
65 // Store the current roll rate for later use
66 rollrate = flightConditions.getRollRate();
72 public void postStep(SimulationStatus status) throws SimulationException {
74 // Activate PID controller only after a specific time
75 if (status.getSimulationTime() < START_TIME) {
76 prevTime = status.getSimulationTime();
80 // Find the fin set named CONTROL
82 for (RocketComponent c : status.getConfiguration()) {
83 if ((c instanceof FinSet) && (c.getName().equals(CONTROL_FIN_NAME))) {
89 throw new SimulationException("A fin set with name '" + CONTROL_FIN_NAME + "' was not found");
93 // Determine time step
94 double deltaT = status.getSimulationTime() - prevTime;
95 prevTime = status.getSimulationTime();
99 double error = SETPOINT - rollrate;
101 double p = KP * error;
102 intState += error * deltaT;
103 double i = KI * intState;
105 double value = p + i;
108 // Clamp the fin angle between -MAX_ANGLE and MAX_ANGLE
109 if (Math.abs(value) > MAX_ANGLE) {
110 System.err.printf("Attempting to set angle %.1f at t=%.3f, clamping.\n",
111 value * 180 / Math.PI, status.getSimulationTime());
112 value = MathUtil.clamp(value, -MAX_ANGLE, MAX_ANGLE);
116 // Limit the fin turn rate
117 if (finPosition < value) {
118 finPosition = Math.min(finPosition + TURNRATE * deltaT, value);
120 finPosition = Math.max(finPosition - TURNRATE * deltaT, value);
123 // Set the control fin cant and store the data
124 finset.setCantAngle(finPosition);
125 status.getFlightData().setValue(FIN_CANT_TYPE, finPosition);