src/FDM/YASim/ControlMap.cpp

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