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(FGAIManager* mgr) {
54 _type_str = "multiplayer";
55 _otype = otMultiplayer;
57 _time_node = fgGetNode("/sim/time/elapsed-sec", true);
60 speedN = speedE = rateH = rateR = rateP = 0.0;
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;
70 //Exponentially weighted moving average time constants
71 speed_north_deg_sec_constant = speed_east_deg_sec_constant = 0.1;
81 FGAIMultiplayer::~FGAIMultiplayer() {
85 bool FGAIMultiplayer::init() {
86 return FGAIBase::init();
89 void FGAIMultiplayer::bind() {
91 props->setStringValue("callsign", company.c_str());
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));
117 props->setDoubleValue("sim/current-view/view-number", 1);
121 void FGAIMultiplayer::setCompany(string comp) {
124 props->setStringValue("callsign", company.c_str());
128 void FGAIMultiplayer::unbind() {
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");
145 void FGAIMultiplayer::update(double dt) {
147 FGAIBase::update(dt);
153 void FGAIMultiplayer::Run(double dt) {
155 // strangely, this is called with a dt of 0 quite often
157 SG_LOG( SG_GENERAL, SG_DEBUG, "AIMultiplayer::main loop dt " << dt ) ;
159 //if (dt == 0) return;
161 //FGAIMultiplayer::dt = dt;
163 //double rhr, rha; // "real" heading radius/angle
165 // get the current sim elapsed time
166 double time =_time_node->getDoubleValue(); //secs
170 //calulate the time difference, dt. Then use this value to extrapolate position and orientation
171 dt = time - time_stamp;
173 SG_LOG(SG_GENERAL, SG_DEBUG, "time: "
174 << time << " timestamp: " << time_stamp << " dt: " << dt << " freq Hz: " << 1/dt);
176 // change heading/roll/pitch
177 raw_hdg = hdg + rateH * dt;
178 raw_roll = roll + rateR * dt;
179 raw_pitch = pitch + rateP * dt;
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));
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;*/
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;
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);
205 // calculate new speed by acceleration
210 // convert speed to degrees per second
212 speed_north_deg_sec = speedN / ft_per_deg_lat;
213 speed_east_deg_sec = speedE / ft_per_deg_lon;
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);
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));
226 SG_LOG(SG_GENERAL, SG_DEBUG,"lat high pass filter");
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));
235 SG_LOG(SG_GENERAL, SG_DEBUG,"lon high pass filter");
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));
244 SG_LOG(SG_GENERAL, SG_DEBUG,"alt high pass filter");
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;
253 pos.setlat( damp_lat );
254 pos.setlon( damp_lon );
255 pos.setelev( damp_alt * SG_FEET_TO_METER );
260 //###########################//
261 // do calculations for radar //
262 //###########################//
263 //double range_ft2 = UpdateRadar(manager);
265 void FGAIMultiplayer::setTimeStamp()
267 // this function sets the timestamp as the sim elapsed time
268 time_stamp = _time_node->getDoubleValue(); //secs
270 //calculate the elapsed time since the latst update for display purposes only
271 double elapsed_time = time_stamp - last_time_stamp;
273 SG_LOG( SG_GENERAL, SG_DEBUG, " net input time s" << time_stamp << " freq Hz: " << 1/elapsed_time ) ;
276 last_time_stamp = time_stamp;