From 3922c121631a159d911a96353aad06a3d8896769 Mon Sep 17 00:00:00 2001 From: Tim Moore Date: Wed, 9 Dec 2009 07:44:50 +0100 Subject: [PATCH] Vivian Meazza: AI escorts --- src/AIModel/AIBase.hxx | 2 +- src/AIModel/AICarrier.cxx | 12 +- src/AIModel/AICarrier.hxx | 2 +- src/AIModel/AIEscort.cxx | 441 ++++++++++++++++++++++++++++++++ src/AIModel/AIEscort.hxx | 109 ++++++++ src/AIModel/AIManager.cxx | 10 +- src/AIModel/AIShip.cxx | 12 +- src/AIModel/AIShip.hxx | 3 +- src/AIModel/Makefile.am | 1 + src/Instrumentation/wxradar.cxx | 214 +++++++++++----- src/Instrumentation/wxradar.hxx | 29 ++- 11 files changed, 756 insertions(+), 79 deletions(-) create mode 100644 src/AIModel/AIEscort.cxx create mode 100644 src/AIModel/AIEscort.hxx diff --git a/src/AIModel/AIBase.hxx b/src/AIModel/AIBase.hxx index 8840136da..1ab3a640e 100644 --- a/src/AIModel/AIBase.hxx +++ b/src/AIModel/AIBase.hxx @@ -45,7 +45,7 @@ class FGAIBase : public osg::Referenced { public: enum object_type { otNull = 0, otAircraft, otShip, otCarrier, otBallistic, otRocket, otStorm, otThermal, otStatic, otWingman, otGroundVehicle, - otMultiplayer, + otEscort, otMultiplayer, MAX_OBJECTS }; // Needs to be last!!! FGAIBase(object_type ot); diff --git a/src/AIModel/AICarrier.cxx b/src/AIModel/AICarrier.cxx index a0dc5effd..ff2a42014 100644 --- a/src/AIModel/AICarrier.cxx +++ b/src/AIModel/AICarrier.cxx @@ -127,8 +127,8 @@ void FGAICarrier::update(double dt) { FGAIShip::update(dt); //automatic turn into wind with a target wind of 25 kts otd - //SG_LOG(SG_GENERAL, SG_ALERT, "AICarrier: MPControl " << MPControl ); - if (!MPControl){ + //SG_LOG(SG_GENERAL, SG_ALERT, "AICarrier: MPControl " << MPControl << " AIControl " << AIControl); + if (!MPControl && AIControl){ if(turn_to_launch_hdg){ TurnToLaunch(); @@ -259,9 +259,9 @@ void FGAICarrier::bind() { props->tie("controls/start-pos-long-deg", SGRawValueMethods(pos, &SGGeod::getLongitudeDeg)); props->tie("controls/mp-control", - SGRawValuePointer(&MPControl)); - props->tie("velocities/speed-kts", - SGRawValuePointer(&speed)); + SGRawValuePointer(&MPControl)); + props->tie("controls/ai-control", + SGRawValuePointer(&AIControl)); props->tie("environment/surface-wind-speed-true-kts", SGRawValuePointer(&wind_speed_kts)); props->tie("environment/surface-wind-from-true-degs", @@ -317,7 +317,6 @@ void FGAICarrier::unbind() { props->untie("controls/flols/distance-m"); props->untie("controls/flols/angle-degs"); props->untie("controls/turn-to-launch-hdg"); - props->untie("velocities/speed-kts"); props->untie("environment/wind-speed-true-kts"); props->untie("environment/wind-from-true-degs"); props->untie("environment/rel-wind-from-degs"); @@ -333,6 +332,7 @@ void FGAICarrier::unbind() { props->untie("controls/constants/jbd/trans-time-s"); props->untie("controls/jbd-time-constant"); props->untie("controls/mp-control"); + props->untie("controls/ai-control"); props->untie("controls/turn-to-recovery-hdg"); props->untie("controls/turn-to-base-course"); } diff --git a/src/AIModel/AICarrier.hxx b/src/AIModel/AICarrier.hxx index 239e302d9..9df40db41 100644 --- a/src/AIModel/AICarrier.hxx +++ b/src/AIModel/AICarrier.hxx @@ -123,7 +123,7 @@ private: bool turn_to_base_course; bool returning; // set if the carrier is returning to an operating box bool InToWind(); // set if the carrier is in to wind - bool MPControl; + bool MPControl, AIControl; SGPropertyNode_ptr _longitude_node; diff --git a/src/AIModel/AIEscort.cxx b/src/AIModel/AIEscort.cxx new file mode 100644 index 000000000..80ef32325 --- /dev/null +++ b/src/AIModel/AIEscort.cxx @@ -0,0 +1,441 @@ +// FGAIEscort - FGAIShip-derived class creates an AI Ground Vehicle +// by adding a ground following utility +// +// Written by Vivian Meazza, started August 2009. +// - vivian.meazza at lineone.net +// +// 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include +#include +#include + +#include +#include +#include + +#include +#include
+#include
+ +#include +#include + +#include "AIEscort.hxx" + +FGAIEscort::FGAIEscort() : +FGAIShip(otEscort), + +_selected_ac(0), +_relbrg (0), +_stn_truebrg(0), +_parent_speed(0), +_stn_limit(0), +_stn_angle_limit(0), +_stn_speed(0), +_stn_height(0), +_max_speed(0), +_interval(0), +_MPControl(false), +_patrol(false), +_stn_deg_true(false), +_parent("") + +{ + invisible = false; +} + +FGAIEscort::~FGAIEscort() {} + +void FGAIEscort::readFromScenario(SGPropertyNode* scFileNode) { + if (!scFileNode) + return; + + FGAIShip::readFromScenario(scFileNode); + + setName(scFileNode->getStringValue("name", "Escort")); + setSMPath(scFileNode->getStringValue("submodel-path", "")); + setStnRange(scFileNode->getDoubleValue("station/range-nm", 1)); + setStnBrg(scFileNode->getDoubleValue("station/brg-deg", 0.0)); + setStnLimit(scFileNode->getDoubleValue("station/range-limit-nm", 0.2)); + setStnAngleLimit(scFileNode->getDoubleValue("station/angle-limit-deg", 15.0)); + setStnSpeed(scFileNode->getDoubleValue("station/speed-kts", 2.5)); + setStnPatrol(scFileNode->getBoolValue("station/patrol", false)); + setStnHtFt(scFileNode->getDoubleValue("station/height-ft", 0.0)); + setStnDegTrue(scFileNode->getBoolValue("station/deg-true", false)); + setParentName(scFileNode->getStringValue("station/parent", "")); + setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 30.0)); + setUpdateInterval(scFileNode->getDoubleValue("update-interval-sec", 10.0)); + setCallSign(scFileNode->getStringValue("callsign", "")); + + if(_patrol) + sg_srandom_time(); + +} + +void FGAIEscort::bind() { + FGAIShip::bind(); + + props->tie("station/rel-bearing-deg", + SGRawValuePointer(&_stn_relbrg)); + props->tie("station/true-bearing-deg", + SGRawValuePointer(&_stn_truebrg)); + props->tie("station/range-nm", + SGRawValuePointer(&_stn_range)); + props->tie("station/range-limit-nm", + SGRawValuePointer(&_stn_limit)); + props->tie("station/angle-limit-deg", + SGRawValuePointer(&_stn_angle_limit)); + props->tie("station/speed-kts", + SGRawValuePointer(&_stn_speed)); + props->tie("station/height-ft", + SGRawValuePointer(&_stn_height)); + props->tie("controls/update-interval-sec", + SGRawValuePointer(&_interval)); + props->tie("controls/parent-mp-control", + SGRawValuePointer(&_MPControl)); + props->tie("station/target-range-nm", + SGRawValuePointer(&_tgtrange)); + props->tie("station/target-brg-deg-t", + SGRawValuePointer(&_tgtbrg)); + props->tie("station/patrol", + SGRawValuePointer(&_patrol)); +} + +void FGAIEscort::unbind() { + FGAIShip::unbind(); + + props->untie("station/rel-bearing-deg"); + props->untie("station/true-bearing-deg"); + props->untie("station/range-nm"); + props->untie("station/range-limit-nm"); + props->untie("station/angle-limit-deg"); + props->untie("station/speed-kts"); + props->untie("station/height-ft"); + props->untie("controls/update-interval-sec"); + +} + +bool FGAIEscort::init(bool search_in_AI_path) { + if (!FGAIShip::init(search_in_AI_path)) + return false; + + invisible = false; + no_roll = false; + + props->setStringValue("controls/parent-name", _parent.c_str()); + setParent(); + pos = _tgtpos; + speed = _parent_speed; + hdg = _parent_hdg; + + return true; +} + +void FGAIEscort::update(double dt) { + FGAIShip::update(dt); + + RunEscort(dt); +} + +void FGAIEscort::setStnRange(double r) { + _stn_range = r; +} + +void FGAIEscort::setStnBrg(double b) { + _stn_brg = b; +} + +void FGAIEscort::setStnLimit(double l) { + _stn_limit = l; +} + +void FGAIEscort::setStnAngleLimit(double al) { + _stn_angle_limit = al; +} + +void FGAIEscort::setStnSpeed(double s) { + _stn_speed = s; +} + +void FGAIEscort::setStnHtFt(double h) { + _stn_height = h; +} + +void FGAIEscort::setStnDegTrue(bool t) { + _stn_deg_true = t; +} + +void FGAIEscort::setMaxSpeed(double m) { + _max_speed = m; +} + +void FGAIEscort::setUpdateInterval(double i) { + _interval = i; +} + +void FGAIEscort::setStnPatrol(bool p) { + _patrol = p; +} + +void FGAIEscort::setParentName(const string& p) { + _parent = p; +} + +bool FGAIEscort::getGroundElev(SGGeod inpos) { + + double height_m ; + + if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 3000), height_m, &_material,0)){ + _ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET; + + if (_material) { + const vector& names = _material->get_names(); + + _solid = _material->get_solid(); + + if (!names.empty()) + props->setStringValue("material/name", names[0].c_str()); + else + props->setStringValue("material/name", ""); + + //cout << "material " << names[0].c_str() + // << " _elevation_m " << _elevation_m + // << " solid " << _solid + // << " load " << _load_resistance + // << " frictionFactor " << _frictionFactor + // << endl; + + } + + return true; + } else { + return false; + } + +} + +void FGAIEscort::setParent() { + + const SGPropertyNode *ai = fgGetNode("/ai/models", true); + + for (int i = ai->nChildren() - 1; i >= -1; i--) { + const SGPropertyNode *model; + + if (i < 0) { // last iteration: selected model + model = _selected_ac; + } else { + model = ai->getChild(i); + string path = ai->getPath(); + const string name = model->getStringValue("name"); + + if (!model->nChildren()){ + continue; + } + if (name == _parent) { + _selected_ac = model; // save selected model for last iteration + break; + } + + } + if (!model) + continue; + + }// end for loop + + if (_selected_ac != 0){ + const string name = _selected_ac->getStringValue("name"); + double lat = _selected_ac->getDoubleValue("position/latitude-deg"); + double lon = _selected_ac->getDoubleValue("position/longitude-deg"); + double elevation = _selected_ac->getDoubleValue("position/altitude-ft"); + _MPControl = _selected_ac->getBoolValue("controls/mp-control"); + + _selectedpos.setLatitudeDeg(lat); + _selectedpos.setLongitudeDeg(lon); + _selectedpos.setElevationFt(elevation); + + _parent_speed = _selected_ac->getDoubleValue("velocities/speed-kts"); + _parent_hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg"); + + if(!_stn_deg_true){ + _stn_truebrg = calcTrueBearingDeg(_stn_brg, _parent_hdg); + _stn_relbrg = _stn_brg; + //cout << _name <<" set rel"<getDoubleValue("orientation/true-heading-deg"); + double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg"); + double roll = _selected_ac->getDoubleValue("orientation/roll-deg"); + + // Transform that one to the horizontal local coordinate system. + SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos); + + // and postrotate the orientation of the AIModel wrt the horizontal + // local frame + hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll); + + // The offset converted to the usual body fixed coordinate system + // rotated to the earth fiexed coordinates axis + SGVec3d off = hlTrans.backTransform(_off); + + // Add the position offset of the AIModel to gain the earth centered position + SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos); + + return cartPos + off; +} + + +void FGAIEscort::setStationSpeed(){ + + double speed = 0; + double angle = 0; + + // these are the AI rules for the manoeuvring of escorts + + if (_MPControl && _tgtrange > 4 * _stn_limit){ + SG_LOG(SG_GENERAL, SG_ALERT, "AIEscort: " << _name + << " re-aligning to MP pos"); + pos = _tgtpos; + speed = 0; + angle = 0; + }else if ((_relbrg < -90 || _relbrg > 90) && _tgtrange > _stn_limit ){ + angle =_relbrg; + + if(_tgtrange > 4 * _stn_limit) + speed = 4 * -_stn_speed; + else + speed = -_stn_speed; + + }else if ((_relbrg >= -90 || _relbrg <= 90) && _tgtrange > _stn_limit){ + angle = _relbrg; + + if(_tgtrange > 4 * _stn_limit) + speed = 4 * _stn_speed; + else + speed = _stn_speed; + + } else { + + if(_patrol){ + angle = 15 * sg_random(); + speed = 5 * sg_random(); + } else { + angle = 0; + speed = 0; + } + + } + + double station_speed = _parent_speed + speed; + + SG_CLAMP_RANGE(station_speed, 5.0, _max_speed); + SG_CLAMP_RANGE(angle, -_stn_angle_limit, _stn_angle_limit); + + AccelTo(station_speed); + TurnTo(_parent_hdg + angle); + ClimbTo(_stn_height); + +} + +void FGAIEscort::RunEscort(double dt){ + + _dt_count += dt; + + + + /////////////////////////////////////////////////////////////////////////// + // Check execution time (currently once every 0.05 sec or 20 fps) + // Add a bit of randomization to prevent the execution of all flight plans + // in synchrony, which can add significant periodic framerate flutter. + // Randomization removed to get better appearance + /////////////////////////////////////////////////////////////////////////// + + //cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl; + if (_dt_count < _next_run) + return; + _next_run = _interval /*+ (0.015 * sg_random())*/; + + if(_parent == ""){ + return; + } + + setParent(); + setStationSpeed(); + //getGroundElev(pos); + + _dt_count = 0; + +} + +// end AIGroundvehicle diff --git a/src/AIModel/AIEscort.hxx b/src/AIModel/AIEscort.hxx new file mode 100644 index 000000000..fec849867 --- /dev/null +++ b/src/AIModel/AIEscort.hxx @@ -0,0 +1,109 @@ +// FGAIGroundVehicle - FGAIShip-derived class creates an AI Ground Vehicle +// by adding a ground following utility +// +// Written by Vivian Meazza, started August 2009. +// - vivian.meazza at lineone.net +// +// 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifndef _FG_AIESCORT_HXX +#define _FG_AIESCORT_HXX + +#include +#include + +#include + +using std::string; +using std::list; + +#include "AIBase.hxx" + +#include "AIShip.hxx" + +#include "AIManager.hxx" +#include "AIBase.hxx" + +class FGAIEscort : public FGAIShip { +public: + FGAIEscort(); + virtual ~FGAIEscort(); + + virtual void readFromScenario(SGPropertyNode* scFileNode); + virtual void bind(); + virtual void unbind(); + virtual const char* getTypeString(void) const { return "escort"; } + + bool init(bool search_in_AI_path=false); + +private: + + virtual void reinit() { init(); } + virtual void update (double dt); + + void setParentName(const string& p); + void setParent(); + void setStnRange(double r); + void setStnBrg(double y); + void setStationSpeed(); + void setStnLimit(double l); + void setStnAngleLimit(double l); + void setStnSpeed(double s); + void setStnHtFt(double h); + void setStnPatrol(bool p); + void setStnDegTrue(bool t); + + void setMaxSpeed(double m); + void setUpdateInterval(double i); + + void RunEscort(double dt); + + bool getGroundElev(SGGeod inpos); + + SGVec3d getCartHitchPosAt(const SGVec3d& off) const; + + void calcRangeBearing(double lat, double lon, double lat2, double lon2, + double &range, double &bearing) const; + double calcRelBearingDeg(double bearing, double heading); + double calcTrueBearingDeg(double bearing, double heading); + double calcRecipBearingDeg(double bearing); + + SGGeod _selectedpos; + SGGeod _tgtpos; + + bool _solid; // if true ground is solid for FDMs + double _load_resistance; // ground load resistanc N/m^2 + double _frictionFactor; // dimensionless modifier for Coefficient of Friction + double _tgtrange, _tgtbrg; + double _ht_agl_ft; + double _relbrg, _truebrg; + double _parent_speed, _parent_hdg; + double _interval; + + double _stn_relbrg, _stn_truebrg, _stn_brg, _stn_range, _stn_height; + double _stn_speed, _stn_angle_limit, _stn_limit; + + double _max_speed; + + const SGMaterial* _material; + const SGPropertyNode *_selected_ac; + + bool _MPControl, _patrol, _stn_deg_true; + + string _parent; + +}; + +#endif // FG_AIGROUNDVEHICLE_HXX diff --git a/src/AIModel/AIManager.cxx b/src/AIModel/AIManager.cxx index 74d19e6a2..ed4f4a534 100644 --- a/src/AIModel/AIManager.cxx +++ b/src/AIModel/AIManager.cxx @@ -39,6 +39,7 @@ #include "AITanker.hxx" #include "AIWingman.hxx" #include "AIGroundVehicle.hxx" +#include "AIEscort.hxx" FGAIManager::FGAIManager() { _dt = 0.0; @@ -311,6 +312,11 @@ FGAIManager::processScenario( const string &filename ) { groundvehicle->readFromScenario(scEntry); attach(groundvehicle); + } else if (type == "escort") { + FGAIEscort* escort = new FGAIEscort; + escort->readFromScenario(scEntry); + attach(escort); + } else if (type == "thunderstorm") { FGAIStorm* storm = new FGAIStorm; storm->readFromScenario(scEntry); @@ -395,8 +401,8 @@ const FGAIBase * FGAIManager::calcCollision(double alt, double lat, double lon, double fuse_range) { // we specify tgt extent (ft) according to the AIObject type - double tgt_ht[] = {0, 50, 100, 250, 0, 100, 0, 0, 50, 50, 20, 50}; - double tgt_length[] = {0, 100, 200, 750, 0, 50, 0, 0, 200, 100, 40, 100}; + double tgt_ht[] = {0, 50, 100, 250, 0, 100, 0, 0, 50, 50, 20, 100, 50}; + double tgt_length[] = {0, 100, 200, 750, 0, 50, 0, 0, 200, 100, 40, 200, 100}; ai_list_iterator ai_list_itr = ai_list.begin(); ai_list_iterator end = ai_list.end(); diff --git a/src/AIModel/AIShip.cxx b/src/AIModel/AIShip.cxx index 8fbb4beb5..b0e0eb65f 100644 --- a/src/AIModel/AIShip.cxx +++ b/src/AIModel/AIShip.cxx @@ -43,7 +43,7 @@ FGAIShip::FGAIShip(object_type ot) : FGAIBase(ot), -_limit(40), +_limit(100), _elevation_m(0), _elevation_ft(0), _tow_angle(0), @@ -200,6 +200,8 @@ void FGAIShip::bind() { SGRawValuePointer(&_fixed_turn_radius)); props->tie("controls/restart", SGRawValuePointer(&_restart)); + props->tie("velocities/speed-kts", + SGRawValuePointer(&speed)); } void FGAIShip::unbind() { @@ -236,6 +238,7 @@ void FGAIShip::unbind() { props->untie("controls/fixed-turn-radius-ft"); props->untie("controls/constants/speed"); props->untie("controls/restart"); + props->untie("velocities/speed-kts"); } void FGAIShip::update(double dt) { @@ -309,8 +312,7 @@ void FGAIShip::Run(double dt) { } // do not allow unreasonable speeds - if (speed > _limit) - speed = _limit; + SG_CLAMP_RANGE(speed, -_limit * 0.75, _limit); // convert speed to degrees per second speed_north_deg_sec = cos(hdg / SGD_RADIANS_TO_DEGREES) @@ -337,7 +339,7 @@ void FGAIShip::Run(double dt) { //we assume that at slow speed ships will manoeuvre using engines/bow thruster - if(type == "ship" || type == "carrier"){ + if(type == "ship" || type == "carrier" || type == "escort"){ if (fabs(speed)<=5) _sp_turn_radius_ft = _fixed_turn_radius; @@ -418,7 +420,7 @@ void FGAIShip::Run(double dt) { } // set the _rudder limit by speed - if (type == "ship" || type == "carrier"){ + if (type == "ship" || type == "carrier" || type == "escort"){ if (speed <= 40) rudder_limit = (-0.825 * speed) + 35; diff --git a/src/AIModel/AIShip.hxx b/src/AIModel/AIShip.hxx index 37689856a..c240dabf3 100644 --- a/src/AIModel/AIShip.hxx +++ b/src/AIModel/AIShip.hxx @@ -79,6 +79,7 @@ public: double _rudder_constant, _speed_constant, _hdg_constant, _limit ; double _elevation_m, _elevation_ft; double _missed_range, _tow_angle, _wait_count, _missed_count,_wp_range; + double _dt_count, _next_run; FGAIFlightPlan::waypoint* prev; // the one behind you FGAIFlightPlan::waypoint* curr; // the one ahead @@ -123,8 +124,6 @@ private: double _roll_constant, _roll_factor; double _sp_turn_radius_ft, _rd_turn_radius_ft, _fixed_turn_radius; double _old_range, _range_rate; - double _dt_count; - double _next_run; double _missed_time_sec; double _start_sec; double _day; diff --git a/src/AIModel/Makefile.am b/src/AIModel/Makefile.am index 7cced80f1..17b63583e 100644 --- a/src/AIModel/Makefile.am +++ b/src/AIModel/Makefile.am @@ -18,6 +18,7 @@ libAIModel_a_SOURCES = submodel.cxx submodel.hxx \ AITanker.cxx AITanker.hxx \ AIWingman.cxx AIWingman.hxx\ AIGroundVehicle.cxx AIGroundVehicle.hxx \ + AIEscort.cxx AIEscort.hxx \ performancedata.cxx performancedata.hxx \ performancedb.cxx performancedb.hxx diff --git a/src/Instrumentation/wxradar.cxx b/src/Instrumentation/wxradar.cxx index 6c2187ae7..c8259e28a 100644 --- a/src/Instrumentation/wxradar.cxx +++ b/src/Instrumentation/wxradar.cxx @@ -63,22 +63,26 @@ using std::setfill; #include "od_gauge.hxx" #include "wxradar.hxx" +#include // for cout, endl +using std::cout; +using std::endl; static const float UNIT = 1.0f / 8.0f; // 8 symbols in a row/column in the texture static const char *DEFAULT_FONT = "typewriter.txf"; wxRadarBg::wxRadarBg(SGPropertyNode *node) : - _name(node->getStringValue("name", "radar")), - _num(node->getIntValue("number", 0)), - _interval(node->getDoubleValue("update-interval-sec", 1.0)), - _time(0.0), - _sim_init_done(false), - _odg(0), - _last_switchKnob("off"), - _resultTexture(0), - _wxEcho(0) +_name(node->getStringValue("name", "radar")), +_num(node->getIntValue("number", 0)), +_interval(node->getDoubleValue("update-interval-sec", 1.0)), +_time(0.0), +_sim_init_done(false), +_odg(0), +_last_switchKnob("off"), +_resultTexture(0), +_wxEcho(0), +_antenna_ht(node->getDoubleValue("antenna-ht-ft", 0.0)) { string branch; branch = "/instrumentation/" + _name; @@ -116,12 +120,12 @@ wxRadarBg::init () // texture name to use in 2D and 3D instruments _texture_path = _Instrument->getStringValue("radar-texture-path", - "Aircraft/Instruments/Textures/od_wxradar.rgb"); + "Aircraft/Instruments/Textures/od_wxradar.rgb"); _resultTexture = FGTextureManager::createTexture(_texture_path.c_str(), false); SGPath tpath(globals->get_fg_root()); string path = _Instrument->getStringValue("echo-texture-path", - "Aircraft/Instruments/Textures/wxecho.rgb"); + "Aircraft/Instruments/Textures/wxecho.rgb"); tpath.append(path); // no mipmap or else alpha will mix with pixels on the border of shapes, ruining the effect @@ -219,7 +223,7 @@ wxRadarBg::init () pset->setDataVariance(osg::Object::DYNAMIC); _geom->addPrimitiveSet(pset); _geom->setInitialBound(osg::BoundingBox(osg::Vec3f(-256.0f, -256.0f, 0.0f), - osg::Vec3f(256.0f, 256.0f, 0.0f))); + osg::Vec3f(256.0f, 256.0f, 0.0f))); _radarGeode->addDrawable(_geom); _odg->allocRT(); // Texture in the 2D panel system @@ -281,8 +285,11 @@ wxRadarBg::update (double delta_time_sec) return; } _time += delta_time_sec; - if (_time < _interval) + if (_time < _interval){ +// cout << "WXradar update too soon " << _time << endl; return; + } +// cout << "WXradar updating" << _time<< endl; _time = 0.0; @@ -293,7 +300,9 @@ wxRadarBg::update (double delta_time_sec) center_map(); } } else if (mode == "plan") { - _display_mode = PLAN; + _display_mode = PLAN;} + else if (mode == "bscan") { + _display_mode = BSCAN; } else { _display_mode = ARC; } @@ -358,6 +367,8 @@ wxRadarBg::update (double delta_time_sec) if (_radar_rotate_node->getBoolValue()) { _angle_offset = -_view_heading; } + } else if (_display_mode == BSCAN) { + _angle_offset = -_view_heading; } else { // rose } @@ -371,25 +382,25 @@ wxRadarBg::update (double delta_time_sec) osg::DrawArrays *quadPSet - = static_cast(_geom->getPrimitiveSet(0)); + = static_cast(_geom->getPrimitiveSet(0)); quadPSet->set(osg::PrimitiveSet::QUADS, 0, _vertices->size()); quadPSet->dirty(); // erase what is out of sight of antenna /* - |\ /| - | \ / | - | \ / | - --------- - | | - | | - --------- + |\ /| + | \ / | + | \ / | + --------- + | | + | | + --------- */ osg::DrawArrays *maskPSet - = static_cast(_geom->getPrimitiveSet(1)); + = static_cast(_geom->getPrimitiveSet(1)); osg::DrawArrays *trimaskPSet - = static_cast(_geom->getPrimitiveSet(2)); + = static_cast(_geom->getPrimitiveSet(2)); if (_display_mode == ARC) { float xOffset = 256.0f; @@ -458,7 +469,7 @@ wxRadarBg::update (double delta_time_sec) void wxRadarBg::update_weather() { - string modeButton = _Instrument->getStringValue("mode", "wx"); + string modeButton = _Instrument->getStringValue("mode", "WX"); _radarEchoBuffer = *sgEnviro.get_radar_echo(); // pretend we have a scan angle bigger then the FOV @@ -498,7 +509,7 @@ wxRadarBg::update_weather() continue; float angle = (iradarEcho->heading - _angle_offset) //* fovFactor - + 0.5 * SG_PI; + + 0.5 * SG_PI; // Rotate echo into position, and rotate echo to have // a constant orientation towards the @@ -508,8 +519,8 @@ wxRadarBg::update_weather() const osg::Vec2f texBase(col, (UNIT * (float) (4 + (cloudId & 3)))); osg::Matrixf m(osg::Matrixf::scale(size, size, 1.0f) - * osg::Matrixf::translate(0.0f, radius, 0.0f) - * wxRotate(angle) * _centerTrans); + * osg::Matrixf::translate(0.0f, radius, 0.0f) + * wxRotate(angle) * _centerTrans); addQuad(_vertices, _texCoords, m, texBase); //SG_LOG(SG_GENERAL, SG_DEBUG, "Radar: drawing clouds" @@ -536,12 +547,12 @@ wxRadarBg::update_weather() float size = UNIT * 0.5f; float radius = iradarEcho->dist * _scale; float angle = iradarEcho->heading * SG_DEGREES_TO_RADIANS - - _angle_offset; + - _angle_offset; osg::Matrixf m(osg::Matrixf::scale(size, size, 1.0f) - * wxRotate(-angle) - * osg::Matrixf::translate(0.0f, radius, 0.0f) - * wxRotate(angle) * _centerTrans); + * wxRotate(-angle) + * osg::Matrixf::translate(0.0f, radius, 0.0f) + * wxRotate(angle) * _centerTrans); addQuad(_vertices, _texCoords, m, texBase); } } @@ -550,7 +561,7 @@ wxRadarBg::update_weather() void wxRadarBg::update_data(const SGPropertyNode *ac, double altitude, double heading, - double radius, double bearing, bool selected) + double radius, double bearing, bool selected) { osgText::Text *callsign = new osgText::Text; callsign->setFont(_font.get()); @@ -558,8 +569,8 @@ wxRadarBg::update_data(const SGPropertyNode *ac, double altitude, double heading callsign->setCharacterSize(_font_size); callsign->setColor(selected ? osg::Vec4(1, 1, 1, 1) : _font_color); osg::Matrixf m(wxRotate(-bearing) - * osg::Matrixf::translate(0.0f, radius, 0.0f) - * wxRotate(bearing) * _centerTrans); + * osg::Matrixf::translate(0.0f, radius, 0.0f) + * wxRotate(bearing) * _centerTrans); osg::Vec3 pos = m.preMult(osg::Vec3(16, 16, 0)); // cast to int's, otherwise text comes out ugly @@ -573,10 +584,10 @@ wxRadarBg::update_data(const SGPropertyNode *ac, double altitude, double heading stringstream text; text << identity << endl - << setprecision(0) << fixed - << setw(3) << setfill('0') << heading * SG_RADIANS_TO_DEGREES << "\xB0 " - << setw(0) << altitude << "ft" << endl - << ac->getDoubleValue("velocities/true-airspeed-kt") << "kts"; + << setprecision(0) << fixed + << setw(3) << setfill('0') << heading * SG_RADIANS_TO_DEGREES << "\xB0 " + << setw(0) << altitude << "ft" << endl + << ac->getDoubleValue("velocities/true-airspeed-kt") << "kts"; callsign->setText(text.str()); _textGeode->addDrawable(callsign); @@ -586,6 +597,75 @@ wxRadarBg::update_data(const SGPropertyNode *ac, double altitude, double heading void wxRadarBg::update_aircraft() { + double diff; + double age_factor; + double test_rng; + double test_brg; + double range; + double bearing; + float echo_radius; + double angle; + + if (!ground_echoes.empty()){ + ground_echoes_iterator = ground_echoes.begin(); + + while(ground_echoes_iterator != ground_echoes.end()) { + diff = _elapsed_time - (*ground_echoes_iterator)->elapsed_time; + + if( diff > _persistance) { + ground_echoes.erase(ground_echoes_iterator); + } else { +// double test_brg = (*ground_echoes_iterator)->bearing; +// double bearing = test_brg * SG_DEGREES_TO_RADIANS; +// float angle = calcRelBearing(bearing, _view_heading); + double bumpinessFactor = (*ground_echoes_iterator)->bumpiness; + float heading = get_heading(); + if ( _display_mode == BSCAN ){ + test_rng = (*ground_echoes_iterator)->elevation * 6; + test_brg = (*ground_echoes_iterator)->bearing; + angle = calcRelBearingDeg(test_brg, heading) * 6; + range = sqrt(test_rng * test_rng + angle * angle); + bearing = atan2(angle, test_rng); + //cout << "angle " << angle <<" bearing " + // << bearing / SG_DEGREES_TO_RADIANS << endl; + echo_radius = (0.1 + (1.9 * bumpinessFactor)) * 240 * age_factor; + } else { + test_rng = (*ground_echoes_iterator)->range; + range = test_rng * SG_METER_TO_NM; + test_brg = (*ground_echoes_iterator)->bearing; + bearing = test_brg * SG_DEGREES_TO_RADIANS; + echo_radius = (0.1 + (1.9 * bumpinessFactor)) * 120 * age_factor; + bearing += _angle_offset; + } + + float radius = range * _scale; + //double heading = 90 * SG_DEGREES_TO_RADIANS; + //heading += _angle_offset; + + age_factor = 1; + + if (diff != 0) + age_factor = 1 - (0.5 * diff/_persistance); + + float size = echo_radius * UNIT; + + const osg::Vec2f texBase(3 * UNIT, 3 * UNIT); + osg::Matrixf m(osg::Matrixf::scale(size, size, 1.0f) + * osg::Matrixf::translate(0.0f, radius, 0.0f) + * wxRotate(bearing) * _centerTrans); + addQuad(_vertices, _texCoords, m, texBase); + + ++ground_echoes_iterator; + + //cout << "test bearing " << test_brg + //<< " test_rng " << test_rng * SG_METER_TO_NM + //<< " persistance " << _persistance + //<< endl; + } + + } + + } if (!_ai_enabled_node->getBoolValue()) return; @@ -630,11 +710,13 @@ wxRadarBg::update_aircraft() double echo_radius, sigma; const string name = model->getName(); + + //cout << "name "<getDoubleValue("position/latitude-deg"); double lon = model->getDoubleValue("position/longitude-deg"); @@ -652,7 +734,7 @@ wxRadarBg::update_aircraft() double range, bearing; calcRangeBearing(user_lat, user_lon, lat, lon, range, bearing); - + //cout << _antenna_ht << _interval<< endl; bool isVisible = withinRadarHorizon(user_alt, alt, range); if (!isVisible) @@ -679,8 +761,8 @@ wxRadarBg::update_aircraft() const osg::Vec2f texBase(3 * UNIT, 3 * UNIT); osg::Matrixf m(osg::Matrixf::scale(size, size, 1.0f) - * osg::Matrixf::translate(0.0f, radius, 0.0f) - * wxRotate(bearing) * _centerTrans); + * osg::Matrixf::translate(0.0f, radius, 0.0f) + * wxRotate(bearing) * _centerTrans); addQuad(_vertices, _texCoords, m, texBase); } @@ -689,9 +771,9 @@ wxRadarBg::update_aircraft() const osg::Vec2f texBase(0, 3 * UNIT); float size = 600 * UNIT; osg::Matrixf m(osg::Matrixf::scale(size, size, 1.0f) - * wxRotate(heading - bearing) - * osg::Matrixf::translate(0.0f, radius, 0.0f) - * wxRotate(bearing) * _centerTrans); + * wxRotate(heading - bearing) + * osg::Matrixf::translate(0.0f, radius, 0.0f) + * wxRotate(bearing) * _centerTrans); addQuad(_vertices, _texCoords, m, texBase); } @@ -714,13 +796,13 @@ wxRadarBg::update_tacan() float size = 600 * UNIT; float radius = _tacan_distance_node->getFloatValue() * _scale; float angle = _tacan_bearing_node->getFloatValue() * SG_DEGREES_TO_RADIANS - + _angle_offset; + + _angle_offset; const osg::Vec2f texBase(1 * UNIT, 3 * UNIT); osg::Matrixf m(osg::Matrixf::scale(size, size, 1.0f) - * wxRotate(-angle) - * osg::Matrixf::translate(0.0f, radius, 0.0f) - * wxRotate(angle) * _centerTrans); + * wxRotate(-angle) + * osg::Matrixf::translate(0.0f, radius, 0.0f) + * wxRotate(angle) * _centerTrans); addQuad(_vertices, _texCoords, m, texBase); //SG_LOG(SG_GENERAL, SG_DEBUG, "Radar: drawing TACAN" @@ -741,7 +823,7 @@ wxRadarBg::update_heading_marker() const osg::Vec2f texBase(2 * UNIT, 3 * UNIT); float size = 600 * UNIT; osg::Matrixf m(osg::Matrixf::scale(size, size, 1.0f) - * wxRotate(_view_heading + _angle_offset)); + * wxRotate(_view_heading + _angle_offset)); m *= _centerTrans; addQuad(_vertices, _texCoords, m, texBase); @@ -785,15 +867,17 @@ wxRadarBg::withinRadarHorizon(double user_alt, double alt, double range_nm) { // Radar Horizon = 1.23(ht^1/2 + hr^1/2), //don't allow negative altitudes (an approximation - yes altitudes can be negative) + // Allow antenna ht to be set, but only on ground + _antenna_ht = _Instrument->getDoubleValue("antenna-ht-ft"); - if (user_alt < 0) - user_alt = 0; + if (user_alt <= 0) + user_alt = _antenna_ht; - if (alt < 0) - alt = 0; + if (alt <= 0) + alt = 0; // to allow some vertical extent of target double radarhorizon = 1.23 * (sqrt(alt) + sqrt(user_alt)); - //SG_LOG(SG_GENERAL, SG_DEBUG, "Radar: horizon " << radarhorizon); +// SG_LOG(SG_GENERAL, SG_ALERT, "Radar: radar horizon " << radarhorizon); return radarhorizon >= range_nm; } @@ -828,7 +912,7 @@ wxRadarBg::inRadarRange(double sigma, double range_nm) void wxRadarBg::calcRangeBearing(double lat, double lon, double lat2, double lon2, - double &range, double &bearing) const + double &range, double &bearing) const { // calculate the bearing and range of the second pos from the first double az2, distance; @@ -851,6 +935,20 @@ wxRadarBg::calcRelBearing(float bearing, float heading) return angle; } +float +wxRadarBg::calcRelBearingDeg(float bearing, float heading) +{ + float angle = bearing - heading; + + if (angle >= 180) + return angle -= 360; + + if (angle < -180) + return angle += 360; + + return angle; +} + void wxRadarBg::updateFont() diff --git a/src/Instrumentation/wxradar.hxx b/src/Instrumentation/wxradar.hxx index 533a45872..1bac75e8b 100644 --- a/src/Instrumentation/wxradar.hxx +++ b/src/Instrumentation/wxradar.hxx @@ -33,8 +33,11 @@ #include #include +#include #include + using std::vector; +using std::queue; using std::string; class FGODGauge; @@ -53,8 +56,10 @@ public: protected: string _name; int _num; - double _interval; double _time; + double _interval; + double _elapsed_time; + double _persistance; bool _sim_init_done; SGPropertyNode_ptr _serviceable_node; @@ -68,6 +73,20 @@ protected: FGODGauge *_odg; + typedef struct { + double bearing; + double range; + double elevation; + double bumpiness; + double elapsed_time; + }ground_echo; + + typedef vector ground_echo_vector_type; + typedef ground_echo_vector_type::iterator ground_echo_vector_iterator; + + ground_echo_vector_type ground_echoes; + ground_echo_vector_iterator ground_echoes_iterator; + // Convenience function for creating a property node with a // default value template @@ -76,7 +95,7 @@ protected: private: string _texture_path; - typedef enum { ARC, MAP, PLAN, ROSE } DisplayMode; + typedef enum { ARC, MAP, PLAN, ROSE, BSCAN} DisplayMode; DisplayMode _display_mode; string _last_switchKnob; @@ -89,6 +108,7 @@ private: double _radar_ref_rng; double _lat, _lon; + double _antenna_ht; SGPropertyNode_ptr _Tacan; SGPropertyNode_ptr _Radar_controls; @@ -136,17 +156,18 @@ private: void update_tacan(); void update_heading_marker(); void update_data(const SGPropertyNode *ac, double alt, double heading, - double radius, double bearing, bool selected); + double radius, double bearing, bool selected); void center_map(); void apply_map_offset(); void updateFont(); void calcRangeBearing(double lat, double lon, double lat2, double lon2, - double &range, double &bearing) const; + double &range, double &bearing) const; bool withinRadarHorizon(double user_alt, double alt, double range); bool inRadarRange(double sigma, double range); float calcRelBearing(float bearing, float heading); + float calcRelBearingDeg(float bearing, float heading); }; -- 2.39.5