+// FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
+//
+// Based on FGAIAircraft
+// Written by David Culp, started October 2003.
+// Also by Gregor Richards, started December 2005.
+// With additions by Vivian Meazza, January 2006
+//
+// Copyright (C) 2003 David P. Culp - davidculp2@comcast.net
+// Copyright (C) 2005 Gregor Richards
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include <simgear/math/point3d.hxx>
+#include <Main/fg_props.hxx>
+#include <Main/globals.hxx>
+#include <Main/viewer.hxx>
+#include <Scenery/scenery.hxx>
+#include <Scenery/tilemgr.hxx>
+#include <simgear/route/waypoint.hxx>
+#include <string>
+#include <math.h>
+#include <time.h>
+#ifdef _MSC_VER
+# include <float.h>
+# define finite _finite
+#elif defined(__sun) || defined(sgi)
+# include <ieeefp.h>
+#endif
+
+SG_USING_STD(string);
+
+#include "AIMultiplayer.hxx"
+ static string tempReg;
+
+
+FGAIMultiplayer::FGAIMultiplayer(FGAIManager* mgr) {
+ manager = mgr;
+ _type_str = "multiplayer";
+ _otype = otMultiplayer;
+
+ _time_node = fgGetNode("/sim/time/elapsed-sec", true);
+
+ //initialise values
+ speedN = speedE = rateH = rateR = rateP = 0.0;
+ raw_hdg = hdg;
+ raw_roll = roll;
+ raw_pitch = pitch;
+ raw_speed_east_deg_sec = speedE / ft_per_deg_lon;
+ raw_speed_north_deg_sec = speedN / ft_per_deg_lat;
+ raw_lon = damp_lon = pos.lon();
+ raw_lat = damp_lat = pos.lat();
+ raw_alt = damp_alt = pos.elev() / SG_FEET_TO_METER;
+
+ //Exponentially weighted moving average time constants
+ speed_north_deg_sec_constant = speed_east_deg_sec_constant = 0.1;
+ alt_constant = 0.1;
+ lat_constant = 0.05;
+ lon_constant = 0.05;
+ hdg_constant = 0.1;
+ roll_constant = 0.1;
+ pitch_constant = 0.1;
+}
+
+
+FGAIMultiplayer::~FGAIMultiplayer() {
+}
+
+
+bool FGAIMultiplayer::init() {
+ return FGAIBase::init();
+}
+
+void FGAIMultiplayer::bind() {
+ FGAIBase::bind();
+ props->setStringValue("callsign", company.c_str());
+
+ props->tie("controls/constants/roll",
+ SGRawValuePointer<double>(&roll_constant));
+ props->tie("controls/constants/pitch",
+ SGRawValuePointer<double>(&pitch_constant));
+ props->tie("controls/constants/hdg",
+ SGRawValuePointer<double>(&hdg_constant));
+ props->tie("controls/constants/altitude",
+ SGRawValuePointer<double>(&alt_constant));
+ /*props->tie("controls/constants/speedE",
+ SGRawValuePointer<double>(&speed_east_deg_sec_constant));
+ props->tie("controls/constants/speedN",
+ SGRawValuePointer<double>(&speed_north_deg_sec_constant));*/
+ props->tie("controls/constants/lat",
+ SGRawValuePointer<double>(&lat_constant));
+ props->tie("controls/constants/lon",
+ SGRawValuePointer<double>(&lon_constant));
+ props->tie("surface-positions/rudder-pos-norm",
+ SGRawValuePointer<double>(&rudder));
+ props->tie("surface-positions/elevator-pos-norm",
+ SGRawValuePointer<double>(&elevator));
+ props->tie("velocities/speedE-fps",
+ SGRawValuePointer<double>(&speedE));
+
+
+ props->setDoubleValue("sim/current-view/view-number", 1);
+
+}
+
+void FGAIMultiplayer::setCompany(string comp) {
+ company = comp;
+ if (props)
+ props->setStringValue("callsign", company.c_str());
+
+}
+
+void FGAIMultiplayer::unbind() {
+ FGAIBase::unbind();
+
+ props->untie("controls/constants/roll");
+ props->untie("controls/constants/pitch");
+ props->untie("controls/constants/hdg");
+ props->untie("controls/constants/altitude");
+ /*props->untie("controls/constants/speedE");
+ props->untie("controls/constants/speedN");*/
+ props->untie("controls/constants/lat");
+ props->untie("controls/constants/lon");
+ props->untie("surface-positions/rudder-pos-norm");
+ props->untie("surface-positions/elevator-pos-norm");
+ props->untie("velocities/speedE-fps");
+}
+
+
+void FGAIMultiplayer::update(double dt) {
+
+ FGAIBase::update(dt);
+ Run(dt);
+ Transform();
+}
+
+
+void FGAIMultiplayer::Run(double dt) {
+
+ // strangely, this is called with a dt of 0 quite often
+
+ SG_LOG( SG_GENERAL, SG_DEBUG, "AIMultiplayer::main loop dt " << dt ) ;
+
+ //if (dt == 0) return;
+
+ //FGAIMultiplayer::dt = dt;
+
+ //double rhr, rha; // "real" heading radius/angle
+
+ // get the current sim elapsed time
+ double time =_time_node->getDoubleValue(); //secs
+
+ dt = 0;
+
+ //calulate the time difference, dt. Then use this value to extrapolate position and orientation
+ dt = time - time_stamp;
+
+ SG_LOG(SG_GENERAL, SG_DEBUG, "time: "
+ << time << " timestamp: " << time_stamp << " dt: " << dt << " freq Hz: " << 1/dt);
+
+ // change heading/roll/pitch
+ raw_hdg = hdg + rateH * dt;
+ raw_roll = roll + rateR * dt;
+ raw_pitch = pitch + rateP * dt;
+
+ //apply lowpass filters
+ hdg = (raw_hdg * hdg_constant) + (hdg * (1 - hdg_constant));
+ roll = (raw_roll * roll_constant) + (roll * (1 - roll_constant));
+ pitch = (raw_pitch * pitch_constant) + (pitch * (1 - pitch_constant));
+
+ /*cout << "raw roll " << raw_roll <<" damp hdg " << roll << endl;
+ cout << "raw hdg" << raw_hdg <<" damp hdg " << hdg << endl;
+ cout << "raw pitch " << raw_pitch <<" damp pitch " << pitch << endl;*/
+
+ // sanitize HRP
+ while (hdg < 0) hdg += 360;
+ while (hdg >= 360) hdg -= 360;
+ while (roll <= -180) roll += 360;
+ while (roll > 180) roll -= 360;
+ while (pitch <= -180) pitch += 360;
+ while (pitch > 180) pitch -= 360;
+
+ // calculate the new accelerations by change in the rate of heading
+ /*rhr = sqrt(pow(accN,2) + pow(accE,2));
+ rha = atan2(accN, accE);
+ rha += rateH * dt;
+ accN = sin(rha);
+ accE = cos(rha);*/
+
+ // calculate new speed by acceleration
+ speedN += accN * dt;
+ speedE += accE * dt;
+ speedD += accD * dt;
+
+ // convert speed to degrees per second
+ // 1.686
+ speed_north_deg_sec = speedN / ft_per_deg_lat;
+ speed_east_deg_sec = speedE / ft_per_deg_lon;
+
+ // calculate new position by speed
+ raw_lat = pos.lat() + speed_north_deg_sec * dt;
+ raw_lon = pos.lon() + speed_east_deg_sec * dt ;
+ raw_alt = (pos.elev() / SG_FEET_TO_METER) + (speedD * dt);
+
+ //apply lowpass filters if the difference is small
+ if ( fabs ( pos.lat() - raw_lat) < 0.001 ) {
+ SG_LOG(SG_GENERAL, SG_DEBUG,"lat lowpass filter");
+ damp_lat = (raw_lat * lat_constant) + (damp_lat * (1 - lat_constant));
+ }else {
+ // skip the filter
+ SG_LOG(SG_GENERAL, SG_DEBUG,"lat high pass filter");
+ damp_lat = raw_lat;
+ }
+
+ if ( fabs ( pos.lon() - raw_lon) < 0.001 ) {
+ SG_LOG(SG_GENERAL, SG_DEBUG,"lon lowpass filter");
+ damp_lon = (raw_lon * lon_constant) + (damp_lon * (1 - lon_constant));
+ }else {
+ // skip the filter
+ SG_LOG(SG_GENERAL, SG_DEBUG,"lon high pass filter");
+ damp_lon = raw_lon;
+ }
+
+ if ( fabs ( (pos.elev()/SG_FEET_TO_METER) - raw_alt) < 10 ) {
+ SG_LOG(SG_GENERAL, SG_DEBUG,"alt lowpass filter");
+ damp_alt = (raw_alt * alt_constant) + (damp_alt * (1 - alt_constant));
+ }else {
+ // skip the filter
+ SG_LOG(SG_GENERAL, SG_DEBUG,"alt high pass filter");
+ damp_alt = raw_alt;
+ }
+
+ // cout << "raw lat" << raw_lat <<" damp lat " << damp_lat << endl;
+ //cout << "raw lon" << raw_lon <<" damp lon " << damp_lon << endl;
+ //cout << "raw alt" << raw_alt <<" damp alt " << damp_alt << endl;
+
+ // set new position
+ pos.setlat( damp_lat );
+ pos.setlon( damp_lon );
+ pos.setelev( damp_alt * SG_FEET_TO_METER );
+
+ //save the values
+ time_stamp = time;
+
+ //###########################//
+ // do calculations for radar //
+ //###########################//
+ //double range_ft2 = UpdateRadar(manager);
+}
+void FGAIMultiplayer::setTimeStamp()
+{
+ // this function sets the timestamp as the sim elapsed time
+ time_stamp = _time_node->getDoubleValue(); //secs
+
+ //calculate the elapsed time since the latst update for display purposes only
+ double elapsed_time = time_stamp - last_time_stamp;
+
+ SG_LOG( SG_GENERAL, SG_DEBUG, " net input time s" << time_stamp << " freq Hz: " << 1/elapsed_time ) ;
+
+ //save the values
+ last_time_stamp = time_stamp;
+}
+