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() : FGAIBase(otMultiplayer) {
  53    _time_node = fgGetNode("/sim/time/elapsed-sec", true);
  54
  55    //initialise values
  56    speedN = speedE = rateH = rateR = rateP = 0.0;
  57    raw_hdg = hdg;
  58    raw_roll = roll;
  59    raw_pitch = pitch;
  60    raw_speed_east_deg_sec = speedE / ft_per_deg_lon;
  61    raw_speed_north_deg_sec = speedN / ft_per_deg_lat;
  62    raw_lon = damp_lon = pos.lon();
  63    raw_lat = damp_lat = pos.lat();
  64    raw_alt = damp_alt = pos.elev() / SG_FEET_TO_METER;
  65
  66    //Exponentially weighted moving average time constants
  67    speed_north_deg_sec_constant = speed_east_deg_sec_constant = 0.1;
  68    alt_constant = 0.1;
  69    lat_constant = 0.05;
  70    lon_constant = 0.05;
  71    hdg_constant = 0.1;
  72    roll_constant = 0.1;
  73    pitch_constant = 0.1;
  74 }
  75
  76
  77 FGAIMultiplayer::~FGAIMultiplayer() {
  78 }
  79
  80 bool FGAIMultiplayer::init() {
  81    return FGAIBase::init();
  82 }
  83
  84 void FGAIMultiplayer::bind() {
  85     FGAIBase::bind();
  86     props->setStringValue("callsign", company.c_str());
  87
  88     props->tie("controls/constants/roll",
  89                 SGRawValuePointer<double>(&roll_constant));
  90     props->tie("controls/constants/pitch",
  91                 SGRawValuePointer<double>(&pitch_constant));
  92     props->tie("controls/constants/hdg",
  93                 SGRawValuePointer<double>(&hdg_constant));
  94     props->tie("controls/constants/altitude",
  95                 SGRawValuePointer<double>(&alt_constant));
  96     /*props->tie("controls/constants/speedE",
  97                 SGRawValuePointer<double>(&speed_east_deg_sec_constant));
  98     props->tie("controls/constants/speedN",
  99                 SGRawValuePointer<double>(&speed_north_deg_sec_constant));*/
 100     props->tie("controls/constants/lat",
 101                 SGRawValuePointer<double>(&lat_constant));
 102     props->tie("controls/constants/lon",
 103                 SGRawValuePointer<double>(&lon_constant));
 104     props->tie("surface-positions/rudder-pos-norm",
 105                 SGRawValuePointer<double>(&rudder));
 106     props->tie("surface-positions/elevator-pos-norm",
 107                 SGRawValuePointer<double>(&elevator));
 108     props->tie("velocities/speedE-fps",
 109                 SGRawValuePointer<double>(&speedE));
 110
 111
 112     props->setDoubleValue("sim/current-view/view-number", 1);
 113
 114 }
 115
 116 void FGAIMultiplayer::setCompany(string comp) {
 117     company = comp;
 118     if (props)
 119         props->setStringValue("callsign", company.c_str());
 120
 121 }
 122
 123 void FGAIMultiplayer::unbind() {
 124     FGAIBase::unbind();
 125
 126     props->untie("controls/constants/roll");
 127     props->untie("controls/constants/pitch");
 128     props->untie("controls/constants/hdg");
 129     props->untie("controls/constants/altitude");
 130     /*props->untie("controls/constants/speedE");
 131     props->untie("controls/constants/speedN");*/
 132     props->untie("controls/constants/lat");
 133     props->untie("controls/constants/lon");
 134     props->untie("surface-positions/rudder-pos-norm");
 135     props->untie("surface-positions/elevator-pos-norm");
 136     props->untie("velocities/speedE-fps");
 137 }
 138
 139
 140 void FGAIMultiplayer::update(double dt) {
 141
 142     FGAIBase::update(dt);
 143     Run(dt);
 144     Transform();
 145 }
 146
 147
 148 void FGAIMultiplayer::Run(double dt) {
 149
 150     // strangely, this is called with a dt of 0 quite often
 151
 152     SG_LOG( SG_GENERAL, SG_DEBUG, "AIMultiplayer::main loop dt " << dt ) ;
 153
 154     //if (dt == 0) return;
 155
 156     //FGAIMultiplayer::dt = dt;
 157
 158     //double rhr, rha; // "real" heading radius/angle
 159
 160     // get the current sim elapsed time
 161     double time =_time_node->getDoubleValue();                        //secs
 162
 163     dt = 0;
 164
 165     //calulate the time difference, dt. Then use this value to extrapolate position and orientation
 166     dt = time - time_stamp;
 167
 168     SG_LOG(SG_GENERAL, SG_DEBUG, "time: "
 169         << time << " timestamp: " << time_stamp << " dt: " << dt << " freq Hz: " << 1/dt);
 170
 171     // change heading/roll/pitch
 172     raw_hdg = hdg + rateH * dt;
 173     raw_roll = roll + rateR * dt;
 174     raw_pitch = pitch + rateP * dt;
 175
 176     //apply lowpass filters
 177     hdg = (raw_hdg * hdg_constant) + (hdg * (1 - hdg_constant));
 178     roll = (raw_roll * roll_constant) + (roll * (1 - roll_constant));
 179     pitch = (raw_pitch * pitch_constant) + (pitch * (1 - pitch_constant));
 180
 181     /*cout << "raw roll " << raw_roll <<" damp hdg " << roll << endl;
 182     cout << "raw hdg" << raw_hdg <<" damp hdg " << hdg << endl;
 183     cout << "raw pitch " << raw_pitch <<" damp pitch " << pitch << endl;*/
 184
 185     // sanitize HRP
 186     while (hdg < 0) hdg += 360;
 187     while (hdg >= 360) hdg -= 360;
 188     while (roll <= -180) roll += 360;
 189     while (roll > 180) roll -= 360;
 190     while (pitch <= -180) pitch += 360;
 191     while (pitch > 180) pitch -= 360;
 192
 193     // calculate the new accelerations by change in the rate of heading
 194     /*rhr = sqrt(pow(accN,2) + pow(accE,2));
 195     rha = atan2(accN, accE);
 196     rha += rateH * dt;
 197     accN = sin(rha);
 198     accE = cos(rha);*/
 199
 200     // calculate new speed by acceleration
 201     speedN += accN * dt;
 202     speedE += accE * dt;
 203     speedD += accD * dt;
 204
 205     // convert speed to degrees per second
 206     // 1.686
 207     speed_north_deg_sec = speedN / ft_per_deg_lat;
 208     speed_east_deg_sec  = speedE / ft_per_deg_lon;
 209
 210     // calculate new position by speed
 211     raw_lat = pos.lat() + speed_north_deg_sec * dt;
 212     raw_lon = pos.lon() + speed_east_deg_sec * dt ;
 213     raw_alt = (pos.elev() / SG_FEET_TO_METER) +  (speedD * dt);
 214
 215     //apply lowpass filters if the difference is small
 216     if ( fabs ( pos.lat() - raw_lat) < 0.001 ) {
 217         SG_LOG(SG_GENERAL, SG_DEBUG,"lat lowpass filter");
 218         damp_lat = (raw_lat * lat_constant) + (damp_lat * (1 - lat_constant));
 219     }else {
 220         // skip the filter
 221         SG_LOG(SG_GENERAL, SG_DEBUG,"lat high pass filter");
 222         damp_lat = raw_lat;
 223     }
 224
 225     if ( fabs ( pos.lon() - raw_lon) < 0.001 ) {
 226         SG_LOG(SG_GENERAL, SG_DEBUG,"lon lowpass filter");
 227         damp_lon = (raw_lon * lon_constant) + (damp_lon * (1 - lon_constant));
 228     }else {
 229         // skip the filter
 230         SG_LOG(SG_GENERAL, SG_DEBUG,"lon high pass filter");
 231         damp_lon = raw_lon;
 232     }
 233
 234     if ( fabs ( (pos.elev()/SG_FEET_TO_METER) - raw_alt) < 10 ) {
 235         SG_LOG(SG_GENERAL, SG_DEBUG,"alt lowpass filter");
 236         damp_alt = (raw_alt * alt_constant) + (damp_alt * (1 - alt_constant));
 237     }else {
 238         // skip the filter
 239         SG_LOG(SG_GENERAL, SG_DEBUG,"alt high pass filter");
 240         damp_alt = raw_alt;
 241     }
 242
 243  //      cout << "raw lat" << raw_lat <<" damp lat " << damp_lat << endl;
 244     //cout << "raw lon" << raw_lon <<" damp lon " << damp_lon << endl;
 245     //cout << "raw alt" << raw_alt <<" damp alt " << damp_alt << endl;
 246
 247     // set new position
 248     pos.setlat( damp_lat );
 249     pos.setlon( damp_lon );
 250     pos.setelev( damp_alt * SG_FEET_TO_METER );
 251
 252     //save the values
 253     time_stamp = time;
 254
 255     //###########################//
 256     // do calculations for radar //
 257     //###########################//
 258     //double range_ft2 = UpdateRadar(manager);
 259 }
 260
 261 void FGAIMultiplayer::setTimeStamp()
 262 {
 263     // this function sets the timestamp as the sim elapsed time
 264     time_stamp = _time_node->getDoubleValue();            //secs
 265
 266     //calculate the elapsed time since the latst update for display purposes only
 267     double elapsed_time = time_stamp - last_time_stamp;
 268
 269     SG_LOG( SG_GENERAL, SG_DEBUG, " net input time s" << time_stamp << " freq Hz: " << 1/elapsed_time ) ;
 270
 271     //save the values
 272     last_time_stamp = time_stamp;
 273 }
 274