src/AIModel/AIMultiplayer.cxx

   1 // FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
   2 //
   3 // Based on FGAIAircraft
   4 // Written by David Culp, started October 2003.
   5 // Also by Gregor Richards, started December 2005.
   6 // With additions by Vivian Meazza, January 2006
   7 //
   8 // Copyright (C) 2003  David P. Culp - davidculp2@comcast.net
   9 // Copyright (C) 2005  Gregor Richards
  10 //
  11 // This program is free software; you can redistribute it and/or
  12 // modify it under the terms of the GNU General Public License as
  13 // published by the Free Software Foundation; either version 2 of the
  14 // License, or (at your option) any later version.
  15 //
  16 // This program is distributed in the hope that it will be useful, but
  17 // WITHOUT ANY WARRANTY; without even the implied warranty of
  18 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  19 // General Public License for more details.
  20 //
  21 // You should have received a copy of the GNU General Public License
  22 // along with this program; if not, write to the Free Software
  23 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  24
  25 #ifdef HAVE_CONFIG_H
  26 #  include <config.h>
  27 #endif
  28
  29 #include <simgear/math/point3d.hxx>
  30 #include <Main/fg_props.hxx>
  31 #include <Main/globals.hxx>
  32 #include <Main/viewer.hxx>
  33 #include <Scenery/scenery.hxx>
  34 #include <Scenery/tilemgr.hxx>
  35 #include <simgear/route/waypoint.hxx>
  36 #include <string>
  37 #include <math.h>
  38 #include <time.h>
  39 #ifdef _MSC_VER
  40 #  include <float.h>
  41 #  define finite _finite
  42 #elif defined(__sun) || defined(sgi)
  43 #  include <ieeefp.h>
  44 #endif
  45
  46 SG_USING_STD(string);
  47
  48 #include "AIMultiplayer.hxx"
  49    static string tempReg;
  50
  51
  52 FGAIMultiplayer::FGAIMultiplayer(FGAIManager* mgr) {
  53    manager = mgr;
  54    _type_str = "multiplayer";
  55    _otype = otMultiplayer;
  56
  57    _time_node = fgGetNode("/sim/time/elapsed-sec", true);
  58
  59    //initialise values
  60    speedN = speedE = rateH = rateR = rateP = 0.0;
  61    raw_hdg = hdg;
  62    raw_roll = roll;
  63    raw_pitch = pitch;
  64    raw_speed_east_deg_sec = speedE / ft_per_deg_lon;
  65    raw_speed_north_deg_sec = speedN / ft_per_deg_lat;
  66    raw_lon = damp_lon = pos.lon();
  67    raw_lat = damp_lat = pos.lat();
  68    raw_alt = damp_alt = pos.elev() / SG_FEET_TO_METER;
  69
  70    //Exponentially weighted moving average time constants
  71    speed_north_deg_sec_constant = speed_east_deg_sec_constant = 0.1;
  72    alt_constant = 0.1;
  73    lat_constant = 0.05;
  74    lon_constant = 0.05;
  75    hdg_constant = 0.1;
  76    roll_constant = 0.1;
  77    pitch_constant = 0.1;
  78 }
  79
  80
  81 FGAIMultiplayer::~FGAIMultiplayer() {
  82 }
  83
  84
  85 bool FGAIMultiplayer::init() {
  86    return FGAIBase::init();
  87 }
  88
  89 void FGAIMultiplayer::bind() {
  90     FGAIBase::bind();
  91     props->setStringValue("callsign", company.c_str());
  92
  93     props->tie("controls/constants/roll",
  94                 SGRawValuePointer<double>(&roll_constant));
  95     props->tie("controls/constants/pitch",
  96                 SGRawValuePointer<double>(&pitch_constant));
  97     props->tie("controls/constants/hdg",
  98                 SGRawValuePointer<double>(&hdg_constant));
  99     props->tie("controls/constants/altitude",
 100                 SGRawValuePointer<double>(&alt_constant));
 101     /*props->tie("controls/constants/speedE",
 102                 SGRawValuePointer<double>(&speed_east_deg_sec_constant));
 103     props->tie("controls/constants/speedN",
 104                 SGRawValuePointer<double>(&speed_north_deg_sec_constant));*/
 105     props->tie("controls/constants/lat",
 106                 SGRawValuePointer<double>(&lat_constant));
 107     props->tie("controls/constants/lon",
 108                 SGRawValuePointer<double>(&lon_constant));
 109     props->tie("surface-positions/rudder-pos-norm",
 110                 SGRawValuePointer<double>(&rudder));
 111     props->tie("surface-positions/elevator-pos-norm",
 112                 SGRawValuePointer<double>(&elevator));
 113     props->tie("velocities/speedE-fps",
 114                 SGRawValuePointer<double>(&speedE));
 115
 116
 117     props->setDoubleValue("sim/current-view/view-number", 1);
 118
 119 }
 120
 121 void FGAIMultiplayer::setCompany(string comp) {
 122     company = comp;
 123     if (props)
 124         props->setStringValue("callsign", company.c_str());
 125
 126 }
 127
 128 void FGAIMultiplayer::unbind() {
 129     FGAIBase::unbind();
 130
 131     props->untie("controls/constants/roll");
 132     props->untie("controls/constants/pitch");
 133     props->untie("controls/constants/hdg");
 134     props->untie("controls/constants/altitude");
 135     /*props->untie("controls/constants/speedE");
 136     props->untie("controls/constants/speedN");*/
 137     props->untie("controls/constants/lat");
 138     props->untie("controls/constants/lon");
 139     props->untie("surface-positions/rudder-pos-norm");
 140     props->untie("surface-positions/elevator-pos-norm");
 141     props->untie("velocities/speedE-fps");
 142 }
 143
 144
 145 void FGAIMultiplayer::update(double dt) {
 146
 147     FGAIBase::update(dt);
 148     Run(dt);
 149     Transform();
 150 }
 151
 152
 153 void FGAIMultiplayer::Run(double dt) {
 154
 155     // strangely, this is called with a dt of 0 quite often
 156
 157     SG_LOG( SG_GENERAL, SG_DEBUG, "AIMultiplayer::main loop dt " << dt ) ;
 158
 159     //if (dt == 0) return;
 160
 161     //FGAIMultiplayer::dt = dt;
 162
 163     //double rhr, rha; // "real" heading radius/angle
 164
 165     // get the current sim elapsed time
 166     double time =_time_node->getDoubleValue();                        //secs
 167
 168     dt = 0;
 169
 170     //calulate the time difference, dt. Then use this value to extrapolate position and orientation
 171     dt = time - time_stamp;
 172
 173     SG_LOG(SG_GENERAL, SG_DEBUG, "time: "
 174         << time << " timestamp: " << time_stamp << " dt: " << dt << " freq Hz: " << 1/dt);
 175
 176     // change heading/roll/pitch
 177     raw_hdg = hdg + rateH * dt;
 178     raw_roll = roll + rateR * dt;
 179     raw_pitch = pitch + rateP * dt;
 180
 181     //apply lowpass filters
 182     hdg = (raw_hdg * hdg_constant) + (hdg * (1 - hdg_constant));
 183     roll = (raw_roll * roll_constant) + (roll * (1 - roll_constant));
 184     pitch = (raw_pitch * pitch_constant) + (pitch * (1 - pitch_constant));
 185
 186     /*cout << "raw roll " << raw_roll <<" damp hdg " << roll << endl;
 187     cout << "raw hdg" << raw_hdg <<" damp hdg " << hdg << endl;
 188     cout << "raw pitch " << raw_pitch <<" damp pitch " << pitch << endl;*/
 189
 190     // sanitize HRP
 191     while (hdg < 0) hdg += 360;
 192     while (hdg >= 360) hdg -= 360;
 193     while (roll <= -180) roll += 360;
 194     while (roll > 180) roll -= 360;
 195     while (pitch <= -180) pitch += 360;
 196     while (pitch > 180) pitch -= 360;
 197
 198     // calculate the new accelerations by change in the rate of heading
 199     /*rhr = sqrt(pow(accN,2) + pow(accE,2));
 200     rha = atan2(accN, accE);
 201     rha += rateH * dt;
 202     accN = sin(rha);
 203     accE = cos(rha);*/
 204
 205     // calculate new speed by acceleration
 206     speedN += accN * dt;
 207     speedE += accE * dt;
 208     speedD += accD * dt;
 209
 210     // convert speed to degrees per second
 211     // 1.686
 212     speed_north_deg_sec = speedN / ft_per_deg_lat;
 213     speed_east_deg_sec  = speedE / ft_per_deg_lon;
 214
 215     // calculate new position by speed
 216     raw_lat = pos.lat() + speed_north_deg_sec * dt;
 217     raw_lon = pos.lon() + speed_east_deg_sec * dt ;
 218     raw_alt = (pos.elev() / SG_FEET_TO_METER) +  (speedD * dt);
 219
 220     //apply lowpass filters if the difference is small
 221     if ( fabs ( pos.lat() - raw_lat) < 0.001 ) {
 222         SG_LOG(SG_GENERAL, SG_DEBUG,"lat lowpass filter");
 223         damp_lat = (raw_lat * lat_constant) + (damp_lat * (1 - lat_constant));
 224     }else {
 225         // skip the filter
 226         SG_LOG(SG_GENERAL, SG_DEBUG,"lat high pass filter");
 227         damp_lat = raw_lat;
 228     }
 229
 230     if ( fabs ( pos.lon() - raw_lon) < 0.001 ) {
 231         SG_LOG(SG_GENERAL, SG_DEBUG,"lon lowpass filter");
 232         damp_lon = (raw_lon * lon_constant) + (damp_lon * (1 - lon_constant));
 233     }else {
 234         // skip the filter
 235         SG_LOG(SG_GENERAL, SG_DEBUG,"lon high pass filter");
 236         damp_lon = raw_lon;
 237     }
 238
 239     if ( fabs ( (pos.elev()/SG_FEET_TO_METER) - raw_alt) < 10 ) {
 240         SG_LOG(SG_GENERAL, SG_DEBUG,"alt lowpass filter");
 241         damp_alt = (raw_alt * alt_constant) + (damp_alt * (1 - alt_constant));
 242     }else {
 243         // skip the filter
 244         SG_LOG(SG_GENERAL, SG_DEBUG,"alt high pass filter");
 245         damp_alt = raw_alt;
 246     }
 247
 248  //      cout << "raw lat" << raw_lat <<" damp lat " << damp_lat << endl;
 249     //cout << "raw lon" << raw_lon <<" damp lon " << damp_lon << endl;
 250     //cout << "raw alt" << raw_alt <<" damp alt " << damp_alt << endl;
 251
 252     // set new position
 253     pos.setlat( damp_lat );
 254     pos.setlon( damp_lon );
 255     pos.setelev( damp_alt * SG_FEET_TO_METER );
 256
 257     //save the values
 258     time_stamp = time;
 259
 260     //###########################//
 261     // do calculations for radar //
 262     //###########################//
 263     //double range_ft2 = UpdateRadar(manager);
 264 }
 265 void FGAIMultiplayer::setTimeStamp()
 266 {
 267     // this function sets the timestamp as the sim elapsed time
 268     time_stamp = _time_node->getDoubleValue();            //secs
 269
 270     //calculate the elapsed time since the latst update for display purposes only
 271     double elapsed_time = time_stamp - last_time_stamp;
 272
 273     SG_LOG( SG_GENERAL, SG_DEBUG, " net input time s" << time_stamp << " freq Hz: " << 1/elapsed_time ) ;
 274
 275     //save the values
 276     last_time_stamp = time_stamp;
 277 }
 278