src/FDM/YASim/ControlMap.cpp

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