void HOATDispatcher::sendExtendedResponse(
CigiOutgoingMsg *outgoing,
HOTRequest *request,
HOTResponse *response, int numResponses )
{
CigiHatHotXRespV3_2 packet;
packet.SetHatHotID( request->id );
packet.SetValid( true );
packet.SetHostFrame( request->hostFrameNumber & 0x0f );
Vect3 delta = response->hitLocation - request->locationMSL;
//FIXME - negative heights must be handled!!!
double hot = delta.mag();
packet.SetHot( hot );
delta = request->location - response->hitLocation;
//FIXME - negative heights must be handled!!!
double hat = delta.mag();
packet.SetHat( hat );
if( terrainMaterialOverride )
packet.SetMaterial( terrainMaterialOverrideCode );
else
packet.SetMaterial( response->materialCode );
// fixme - does this work w/ geocentric?
double heading = 180.0/M_PI * normalize_angle( atan2(response->normal[1], response->normal[0]) );
double pitch = 180.0/M_PI * normalize_angle( atan2(response->normal[1], response->normal[2]) );
// Convert database heading/pitch into geodetic
CoordinateSet lru_pos;
lru_pos.LatX = response->hitLocation[0];
lru_pos.LonY = response->hitLocation[1];
lru_pos.AltZ = response->hitLocation[2];
lru_pos.Yaw = heading;
lru_pos.Pitch = pitch;
lru_pos.Roll = 0.0;
CoordinateSet gd_pos;
coordinateConverter->performReverseConversion( lru_pos, gd_pos );
packet.SetNormAz( normalize_angle( gd_pos.Yaw ) );
packet.SetNormEl( clamp_pitch( gd_pos.Pitch ) );
*outgoing << packet;
}
void HOATDispatcher::sendResponse(
CigiOutgoingMsg *outgoing,
HOTRequest *request,
HOTResponse *response, int numResponses )
{
CigiHatHotRespV3_2 packet;
packet.SetHatHotID( request->id );
packet.SetValid( true );
packet.SetHostFrame( request->hostFrameNumber & 0x0f );
if( request->type == CigiBaseHatHotReq::HOT )
{
// HOT
packet.SetReqType( CigiBaseHatHotResp::HOT );
Vect3 delta = response->hitLocation - request->locationMSL;
//FIXME - negative heights must be handled!!!
double hot = delta.mag();
packet.SetHot( hot );
}
else
{
// HAT
packet.SetReqType( CigiBaseHatHotResp::HAT );
Vect3 delta = request->location - response->hitLocation;
//FIXME - negative heights must be handled!!!
double hat = delta.mag();
packet.SetHat( hat );
}
*outgoing << packet;
}
// ================================================
// Extrapolate
// vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
void CoordinateConverterTM::Extrapolate( CoordinateSet &CigiCoord,
CoordinateSet &DBaseCoord,
ExtrapolationSet &ExtrapData,
float dTime,
bool TrajEn )
{
int stat;
double lla[3];
double xyz[3];
Vect3 RateV; // Rate vector
Vect3 RateVb; // Rate vector (in body)
Vect3 RateVa; // Rate vector - from accel parameters
Vect3 EnuRate; // Rate vector - East-North-Up referance
double tRate;
double mRate;
double cRate;
float ypr[3];
float nedv[3];
float neda[3];
float gypr[3];
float gccv[3];
float gcca[3];
double gridDeclination = sin( CigiCoord.LatX ) *
( CigiCoord.LatX - TMcentMeridian );
// All rates are in entity body coordinates
// All accels are based on the North-East-Down
// coordinate system.
// Get the current rate
RateVb.Set( ExtrapData.XRate, ExtrapData.YRate, ExtrapData.ZRate );
// Generate current rotation matrix
Mtx3 RotMtx;
RotMtx.GenRot( CigiCoord.Yaw, CigiCoord.Pitch, CigiCoord.Roll );
// Convert Rates to ned
RateV = RotMtx * RateVb;
if( TrajEn )
{
mRate = RateV.mag();
RateVa.Set( ExtrapData.AccelX, ExtrapData.AccelY, ExtrapData.AccelZ );
// Determine drag coef.
if( ExtrapData.TermVel > 0.000001 )
ExtrapData.DragF = ( double )RateVa.mag() / ExtrapData.TermVel;
else if( mRate > 0.000001 )
ExtrapData.DragF = ( double )ExtrapData.RetardationRate / mRate;
else
ExtrapData.DragF = 0.0;
RateVa = RateVa * dTime;
RateV = RateV + RateVa;
cRate = RateV.mag();
tRate = cRate - ( cRate * ExtrapData.DragF * dTime );
RateV = RateV.Unit() * tRate;
// Convert back to body coordinate system
RateVb = RateV * RotMtx;
// Store current rates
ExtrapData.XRate = RateVb.getx();
ExtrapData.YRate = RateVb.gety();
ExtrapData.ZRate = RateVb.getz();
}
// Convert Linear Rates to act in the Database Coordinate system
// and apply them to Database Corrdinate system entity position
if( ConvMeth == 1 ) // WCS
{
// Not Supported at this time
}
else
{
if( ConvMeth == 2 ) // TM
{
// Convert from NED to ENU
EnuRate.Set( RateV.gety(), RateV.getx(), -RateV.getz() );
// this only affects the x and y rates
Vect3 XYNorm( 0.0, 0.0, 1.0 );
EnuRate.Rotate( XYNorm, gridDeclination );
}
else // LCC (ConvMeth == 3)
{
// Not Supported at this time
}
ApplyAttitudeRates( DBaseCoord, ExtrapData, dTime );
}
// Note: This is in east-north-up
DBaseCoord.LatX += EnuRate.getx() * dTime;
DBaseCoord.LonY += EnuRate.gety() * dTime;
DBaseCoord.AltZ += EnuRate.getz() * dTime;
// Generate lla position and attitude
CoordDBase2Cigi( DBaseCoord, CigiCoord );
}
