git.gag.com Git - debian/openrocket/blob - core/src/net/sf/openrocket/simulation/listeners/example/RollControlListener.java

   1 package net.sf.openrocket.simulation.listeners.example;
   2
   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;
  12
  13 /**
  14  * An example listener that applies a PI-controller to adjust the cant of fins
  15  * named "CONTROL" to stop the rocket from rolling.
  16  *
  17  * @author Sampo Niskanen <sampo.niskanen@iki.fi>
  18  */
  19 public class RollControlListener extends AbstractSimulationListener {
  20
  21         // Name of control fin set
  22         private static final String CONTROL_FIN_NAME = "CONTROL";
  23
  24         // Define custom flight data type
  25         private static final FlightDataType FIN_CANT_TYPE = FlightDataType.getType("Control fin cant", "\u03B1fc", UnitGroup.UNITS_ANGLE);
  26
  27         // Simulation time at which PID controller is activated
  28         private static final double START_TIME = 0.5;
  29
  30         // Desired roll rate (rad/sec)
  31         private static final double SETPOINT = 0.0;
  32
  33         // Maximum control fin turn rate (rad/sec)
  34         private static final double TURNRATE = 10 * Math.PI / 180;
  35
  36         // Maximum control fin angle (rad)
  37         private static final double MAX_ANGLE = 15 * Math.PI / 180;
  38
  39
  40         /*
  41          * PID parameters
  42          *
  43          * At M=0.3 KP oscillation threshold between 0.35 and 0.4.    Good KI=3
  44          * At M=0.6 KP oscillation threshold between 0.07 and 0.08    Good KI=2
  45          * At M=0.9 KP oscillation threshold between 0.013 and 0.014  Good KI=0.5
  46          */
  47         private static final double KP = 0.007;
  48         private static final double KI = 0.2;
  49
  50
  51
  52
  53         private double rollrate;
  54
  55         private double prevTime = 0;
  56         private double intState = 0;
  57
  58         private double finPosition = 0;
  59
  60
  61
  62         @Override
  63         public FlightConditions postFlightConditions(SimulationStatus status, FlightConditions flightConditions) {
  64                 // Store the current roll rate for later use
  65                 rollrate = flightConditions.getRollRate();
  66                 return null;
  67         }
  68
  69
  70         @Override
  71         public void postStep(SimulationStatus status) throws SimulationException {
  72
  73                 // Activate PID controller only after a specific time
  74                 if (status.getSimulationTime() < START_TIME) {
  75                         prevTime = status.getSimulationTime();
  76                         return;
  77                 }
  78
  79                 // Find the fin set named CONTROL
  80                 FinSet finset = null;
  81                 for (RocketComponent c : status.getConfiguration()) {
  82                         if ((c instanceof FinSet) && (c.getName().equals(CONTROL_FIN_NAME))) {
  83                                 finset = (FinSet) c;
  84                                 break;
  85                         }
  86                 }
  87                 if (finset == null) {
  88                         throw new SimulationException("A fin set with name '" + CONTROL_FIN_NAME + "' was not found");
  89                 }
  90
  91
  92                 // Determine time step
  93                 double deltaT = status.getSimulationTime() - prevTime;
  94                 prevTime = status.getSimulationTime();
  95
  96
  97                 // PID controller
  98                 double error = SETPOINT - rollrate;
  99
 100                 double p = KP * error;
 101                 intState += error * deltaT;
 102                 double i = KI * intState;
 103
 104                 double value = p + i;
 105
 106
 107                 // Clamp the fin angle between -MAX_ANGLE and MAX_ANGLE
 108                 if (Math.abs(value) > MAX_ANGLE) {
 109                         System.err.printf("Attempting to set angle %.1f at t=%.3f, clamping.\n",
 110                                         value * 180 / Math.PI, status.getSimulationTime());
 111                         value = MathUtil.clamp(value, -MAX_ANGLE, MAX_ANGLE);
 112                 }
 113
 114
 115                 // Limit the fin turn rate
 116                 if (finPosition < value) {
 117                         finPosition = Math.min(finPosition + TURNRATE * deltaT, value);
 118                 } else {
 119                         finPosition = Math.max(finPosition - TURNRATE * deltaT, value);
 120                 }
 121
 122                 // Set the control fin cant and store the data
 123                 finset.setCantAngle(finPosition);
 124                 status.getFlightData().setValue(FIN_CANT_TYPE, finPosition);
 125
 126         }
 127 }