1 // FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
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
8 // Copyright (C) 2003 David P. Culp - davidculp2@comcast.net
9 // Copyright (C) 2005 Gregor Richards
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.
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.
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.
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>
41 # define finite _finite
42 #elif defined(__sun) || defined(sgi)
48 #include "AIMultiplayer.hxx"
49 static string tempReg;
52 FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) {
53 _time_node = fgGetNode("/sim/time/elapsed-sec", true);
56 speedN = speedE = rateH = rateR = rateP = 0.0;
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;
66 //Exponentially weighted moving average time constants
67 speed_north_deg_sec_constant = speed_east_deg_sec_constant = 0.1;
77 FGAIMultiplayer::~FGAIMultiplayer() {
80 bool FGAIMultiplayer::init() {
81 return FGAIBase::init();
84 void FGAIMultiplayer::bind() {
86 props->setStringValue("callsign", company.c_str());
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));
112 props->setDoubleValue("sim/current-view/view-number", 1);
116 void FGAIMultiplayer::setCompany(string comp) {
119 props->setStringValue("callsign", company.c_str());
123 void FGAIMultiplayer::unbind() {
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");
140 void FGAIMultiplayer::update(double dt) {
142 FGAIBase::update(dt);
148 void FGAIMultiplayer::Run(double dt) {
150 // strangely, this is called with a dt of 0 quite often
152 SG_LOG( SG_GENERAL, SG_DEBUG, "AIMultiplayer::main loop dt " << dt ) ;
154 //if (dt == 0) return;
156 //FGAIMultiplayer::dt = dt;
158 //double rhr, rha; // "real" heading radius/angle
160 // get the current sim elapsed time
161 double time =_time_node->getDoubleValue(); //secs
165 //calulate the time difference, dt. Then use this value to extrapolate position and orientation
166 dt = time - time_stamp;
168 SG_LOG(SG_GENERAL, SG_DEBUG, "time: "
169 << time << " timestamp: " << time_stamp << " dt: " << dt << " freq Hz: " << 1/dt);
171 // change heading/roll/pitch
172 raw_hdg = hdg + rateH * dt;
173 raw_roll = roll + rateR * dt;
174 raw_pitch = pitch + rateP * dt;
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));
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;*/
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;
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);
200 // calculate new speed by acceleration
205 // convert speed to degrees per second
207 speed_north_deg_sec = speedN / ft_per_deg_lat;
208 speed_east_deg_sec = speedE / ft_per_deg_lon;
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);
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));
221 SG_LOG(SG_GENERAL, SG_DEBUG,"lat high pass filter");
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));
230 SG_LOG(SG_GENERAL, SG_DEBUG,"lon high pass filter");
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));
239 SG_LOG(SG_GENERAL, SG_DEBUG,"alt high pass filter");
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;
248 pos.setlat( damp_lat );
249 pos.setlon( damp_lon );
250 pos.setelev( damp_alt * SG_FEET_TO_METER );
255 //###########################//
256 // do calculations for radar //
257 //###########################//
258 //double range_ft2 = UpdateRadar(manager);
261 void FGAIMultiplayer::setTimeStamp()
263 // this function sets the timestamp as the sim elapsed time
264 time_stamp = _time_node->getDoubleValue(); //secs
266 //calculate the elapsed time since the latst update for display purposes only
267 double elapsed_time = time_stamp - last_time_stamp;
269 SG_LOG( SG_GENERAL, SG_DEBUG, " net input time s" << time_stamp << " freq Hz: " << 1/elapsed_time ) ;
272 last_time_stamp = time_stamp;