src/FDM/YASim/ControlMap.cpp

   1 #include "Jet.hpp"
   2 #include "Thruster.hpp"
   3 #include "PropEngine.hpp"
   4 #include "PistonEngine.hpp"
   5 #include "TurbineEngine.hpp"
   6 #include "Gear.hpp"
   7 #include "Hook.hpp"
   8 #include "Launchbar.hpp"
   9 #include "Wing.hpp"
  10 #include "Rotor.hpp"
  11 #include "Math.hpp"
  12 #include "Propeller.hpp"
  13 #include "Hitch.hpp"
  14
  15 #include "ControlMap.hpp"
  16 namespace yasim {
  17
  18 ControlMap::~ControlMap()
  19 {
  20     int i;
  21     for(i=0; i<_inputs.size(); i++) {
  22         Vector* v = (Vector*)_inputs.get(i);
  23         int j;
  24         for(j=0; j<v->size(); j++)
  25             delete (MapRec*)v->get(j);
  26         delete v;
  27     }
  28
  29     for(i=0; i<_outputs.size(); i++)
  30         delete (OutRec*)_outputs.get(i);
  31 }
  32
  33 int ControlMap::newInput()
  34 {
  35     Vector* v = new Vector();
  36     return _inputs.add(v);
  37 }
  38
  39 void ControlMap::addMapping(int input, int type, void* object, int options,
  40                             float src0, float src1, float dst0, float dst1)
  41 {
  42     addMapping(input, type, object, options);
  43
  44     // The one we just added is last in the list (ugly, awful hack!)
  45     Vector* maps = (Vector*)_inputs.get(input);
  46     MapRec* m = (MapRec*)maps->get(maps->size() - 1);
  47
  48     m->src0 = src0;
  49     m->src1 = src1;
  50     m->dst0 = dst0;
  51     m->dst1 = dst1;
  52 }
  53
  54 void ControlMap::addMapping(int input, int type, void* object, int options)
  55 {
  56     // See if the output object already exists
  57     OutRec* out = 0;
  58     int i;
  59     for(i=0; i<_outputs.size(); i++) {
  60         OutRec* o = (OutRec*)_outputs.get(i);
  61         if(o->object == object && o->type == type) {
  62             out = o;
  63             break;
  64         }
  65     }
  66
  67     // Create one if it doesn't
  68     if(out == 0) {
  69         out = new OutRec();
  70         out->type = type;
  71         out->object = object;
  72         out->oldL = out->oldR = out->time = 0;
  73         _outputs.add(out);
  74     }
  75
  76     // Make a new input record
  77     MapRec* map = new MapRec();
  78     map->out = out;
  79     map->opt = options;
  80     map->idx = out->maps.add(map);
  81
  82     // The default ranges differ depending on type!
  83     map->src1 = map->dst1 = rangeMax(type);
  84     map->src0 = map->dst0 = rangeMin(type);
  85
  86     // And add it to the approproate vectors.
  87     Vector* maps = (Vector*)_inputs.get(input);
  88     maps->add(map);
  89 }
  90
  91 void ControlMap::reset()
  92 {
  93     // Set all the values to zero
  94     for(int i=0; i<_outputs.size(); i++) {
  95         OutRec* o = (OutRec*)_outputs.get(i);
  96         for(int j=0; j<o->maps.size(); j++)
  97             ((MapRec*)(o->maps.get(j)))->val = 0;
  98     }
  99 }
 100
 101 void ControlMap::setInput(int input, float val)
 102 {
 103     Vector* maps = (Vector*)_inputs.get(input);
 104     for(int i=0; i<maps->size(); i++) {
 105         MapRec* m = (MapRec*)maps->get(i);
 106
 107         float val2 = val;
 108
 109         // Do the scaling operation.  Clamp to [src0:src1], rescale to
 110         // [0:1] within that range, then map to [dst0:dst1].
 111         if(val2 < m->src0) val2 = m->src0;
 112         if(val2 > m->src1) val2 = m->src1;
 113         val2 = (val2 - m->src0) / (m->src1 - m->src0);
 114         val2 = m->dst0 + val2 * (m->dst1 - m->dst0);
 115
 116         m->val = val2;
 117     }
 118 }
 119
 120 int ControlMap::getOutputHandle(void* obj, int type)
 121 {
 122     for(int i=0; i<_outputs.size(); i++) {
 123         OutRec* o = (OutRec*)_outputs.get(i);
 124         if(o->object == obj && o->type == type)
 125             return i;
 126     }
 127     return 0;
 128 }
 129
 130 void ControlMap::setTransitionTime(int handle, float time)
 131 {
 132     ((OutRec*)_outputs.get(handle))->time = time;
 133 }
 134
 135 float ControlMap::getOutput(int handle)
 136 {
 137     return ((OutRec*)_outputs.get(handle))->oldL;
 138 }
 139
 140 float ControlMap::getOutputR(int handle)
 141 {
 142     return ((OutRec*)_outputs.get(handle))->oldR;
 143 }
 144
 145 void ControlMap::applyControls(float dt)
 146 {
 147     int outrec;
 148     for(outrec=0; outrec<_outputs.size(); outrec++) {
 149         OutRec* o = (OutRec*)_outputs.get(outrec);
 150
 151         // Generate a summed value.  Note the check for "split"
 152         // control axes like ailerons.
 153         float lval = 0, rval = 0;
 154         int i;
 155         for(i=0; i<o->maps.size(); i++) {
 156             MapRec* m = (MapRec*)o->maps.get(i);
 157             float val = m->val;
 158
 159             if(m->opt & OPT_SQUARE)
 160                 val = val * Math::abs(val);
 161             if(m->opt & OPT_INVERT)
 162                 val = -val;
 163             lval += val;
 164             if(m->opt & OPT_SPLIT)
 165                 rval -= val;
 166             else
 167                 rval += val;
 168         }
 169
 170         // If there is a finite transition time, clamp the values to
 171         // the maximum travel allowed in this dt.
 172         if(o->time > 0) {
 173             float dl = lval - o->oldL;
 174             float dr = rval - o->oldR;
 175             float adl = Math::abs(dl);
 176             float adr = Math::abs(dr);
 177
 178             float max = (dt/o->time) * (rangeMax(o->type) - rangeMin(o->type));
 179             if(adl > max) dl = dl*max/adl;
 180             if(adr > max) dr = dr*max/adr;
 181
 182             lval = o->oldL + dl;
 183             rval = o->oldR + dr;
 184         }
 185
 186         o->oldL = lval;
 187         o->oldR = rval;
 188
 189         void* obj = o->object;
 190         switch(o->type) {
 191         case THROTTLE: ((Thruster*)obj)->setThrottle(lval);        break;
 192         case MIXTURE:  ((Thruster*)obj)->setMixture(lval);         break;
 193         case CONDLEVER: ((TurbineEngine*)((PropEngine*)obj)->getEngine())->setCondLever(lval); break;
 194         case STARTER:  ((Thruster*)obj)->setStarter(lval != 0.0);  break;
 195         case MAGNETOS: ((PropEngine*)obj)->setMagnetos((int)lval); break;
 196         case ADVANCE:  ((PropEngine*)obj)->setAdvance(lval);       break;
 197         case PROPPITCH: ((PropEngine*)obj)->setPropPitch(lval);    break;
 198         case PROPFEATHER: ((PropEngine*)obj)->setPropFeather((int)lval); break;
 199         case REHEAT:   ((Jet*)obj)->setReheat(lval);               break;
 200         case VECTOR:   ((Jet*)obj)->setRotation(lval);             break;
 201         case BRAKE:    ((Gear*)obj)->setBrake(lval);               break;
 202         case STEER:    ((Gear*)obj)->setRotation(lval);            break;
 203         case EXTEND:   ((Gear*)obj)->setExtension(lval);           break;
 204         case HEXTEND:  ((Hook*)obj)->setExtension(lval);           break;
 205         case LEXTEND:  ((Launchbar*)obj)->setExtension(lval);      break;
 206         case CASTERING:((Gear*)obj)->setCastering(lval != 0);      break;
 207         case SLAT:     ((Wing*)obj)->setSlat(lval);                break;
 208         case FLAP0:    ((Wing*)obj)->setFlap0(lval, rval);         break;
 209         case FLAP1:    ((Wing*)obj)->setFlap1(lval, rval);         break;
 210         case SPOILER:  ((Wing*)obj)->setSpoiler(lval, rval);       break;
 211         case COLLECTIVE:   ((Rotor*)obj)->setCollective(lval);     break;
 212         case CYCLICAIL:    ((Rotor*)obj)->setCyclicail(lval,rval); break;
 213         case CYCLICELE:    ((Rotor*)obj)->setCyclicele(lval,rval); break;
 214         case ROTORBRAKE:    ((Rotorgear*)obj)->setRotorBrake(lval); break;
 215         case ROTORENGINEON: ((Rotorgear*)obj)->setEngineOn((int)lval); break;
 216         case REVERSE_THRUST: ((Jet*)obj)->setReverse(lval != 0);   break;
 217         case BOOST:
 218             ((PistonEngine*)((Thruster*)obj)->getEngine())->setBoost(lval);
 219             break;
 220         case WASTEGATE:
 221             ((PistonEngine*)((Thruster*)obj)->getEngine())->setWastegate(lval);
 222             break;
 223         case WINCHRELSPEED: ((Hitch*)obj)->setWinchRelSpeed(lval); break;
 224         case HITCHOPEN:    ((Hitch*)obj)->setOpen(lval!=0);       break;
 225         case PLACEWINCH:    ((Hitch*)obj)->setWinchPositionAuto(lval!=0); break;
 226         case FINDAITOW:     ((Hitch*)obj)->findBestAIObject(lval!=0); break;
 227         }
 228     }
 229 }
 230
 231 float ControlMap::rangeMin(int type)
 232 {
 233     // The minimum of the range for each type of control
 234     switch(type) {
 235     case FLAP0:    return -1;  // [-1:1]
 236     case FLAP1:    return -1;
 237     case STEER:    return -1;
 238     case CYCLICELE: return -1;
 239     case CYCLICAIL: return -1;
 240     case COLLECTIVE: return -1;
 241     case WINCHRELSPEED: return -1;
 242     case MAGNETOS: return 0;   // [0:3]
 243     default:       return 0;   // [0:1]
 244     }
 245 }
 246
 247 float ControlMap::rangeMax(int type)
 248 {
 249     // The maximum of the range for each type of control
 250     switch(type) {
 251     case FLAP0:    return 1; // [-1:1]
 252     case FLAP1:    return 1;
 253     case STEER:    return 1;
 254     case MAGNETOS: return 3; // [0:3]
 255     default:       return 1; // [0:1]
 256     }
 257 }
 258
 259 } // namespace yasim