Inertial Navigation Script

[VeCTeR76 0]

Here's some code I posted in the lo-mac.com forum, but for those who are interested, here's a basic inertial map, using speed to calculate position. It's basic, and it doesn't calculate for wind yet.

 

struct mapData
{
 long x;
 long y;
 long z;
 unsigned long zoom;

 long xWp[ NUMBEROFWAYPOINTS ];
 long yWp[ NUMBEROFWAYPOINTS ];
 long zWp[ NUMBEROFWAYPOINTS ];
 unsigned int lastWp;
 unsigned int currWp;

};

// map.cpp

#define wpSize 6
#define mySize 6
#define minLineFactor 0.75
#define minZoom 16


// InitMap ====================================================================================================

void ssWindow::InitMap()
{
 mfdMap.x = 0;
 mfdMap.y = 0;
 mfdMap.z = 0;
 mfdMap.zoom = minZoom;
 mfdMap.currWp = 0;

 mfdMap.lastWp = 0;

}

// AddMapDistance ====================================================================================================

void ssWindow::AddMapDistance( unsigned int speed, unsigned int alt, int pitch, int bank, int yaw, int aoa, unsigned int currTime )
{
int horzSpeed;
 unsigned int heading;
 static unsigned int lastTime = 0;

 // calculate horizontal speed
 // d = t * v
horzSpeed = ( currTime - lastTime ) * speed * tcos( abs( pitch ) ) / 360;


 // adjust heading for AOA
 if ( bank > 0 )
  heading = yaw;
 else
   heading = yaw; 

 heading = heading % 360;

 // record every tenth of a kilometer
 mfdMap.x += horzSpeed * tcos( heading ) / 10;
 mfdMap.y += horzSpeed * tsin( heading ) / 10;
 mfdMap.z = alt;

 lastTime = currTime;

}

// DrawMap ====================================================================================================

void ssWindow::DrawMap( int x, int y, int w, int h, int heading )
{
 int i;
 int cX = x + w / 3;
 int cY = y + h / 2;
 int wpX, wpY;
 long zoomX, zoomY;
 char tempStr[ 32 ];
 unsigned long rangeToWp = 0;
 int wpHeading = 0;

 setlinestyle( SOLID_LINE, 0, NORM_WIDTH );
setcolor( BackGrndColor );

 // clear old map waypoints
 setfillstyle( SOLID_FILL, BackGrndColor );
 bar( x + 1, y + 1, x + w - 1, y + h - 1 );

 setlinestyle( SOLID_LINE, 0, NORM_WIDTH );
setcolor( TextColor );

 // do this everytime??? fix this
 rectangle( x, y, x + w, y + h );

 line( cX - mySize, cY - mySize, cX + mySize, cY );
 line( cX - mySize, cY + mySize, cX + mySize, cY );
 line( cX - mySize, cY + mySize, cX - mySize, cY - mySize );

 setcolor( TextColor );

 // draw waypoints
 for ( i = 0; i < mfdMap.lastWp; i++ )
 {
   // calculate difference from plane x and y
   zoomX = ( mfdMap.xWp[ i ] - mfdMap.x ) * 10 / mfdMap.zoom;
   zoomY = ( mfdMap.yWp[ i ] - mfdMap.y ) * 10 / mfdMap.zoom;

   // calculate angle
	 wpX = calcXComp( zoomX, zoomY, heading ) + cX;
   wpY = calcYComp( zoomX, zoomY, heading ) + cY;

   if ( wpX + wpSize < x + w && wpX - wpSize > x && wpY + wpSize < y + h && wpY - wpSize > y  )
   {
  	 sprintf( tempStr, "%d", i );
     bmpTable.bmpTextxy( wpX - 6, wpY - 9, tempStr, TextColor, BackGrndColor );
  	 circle( wpX, wpY, wpSize );
   }

   //DrawSmallNumber( (unsigned) mfdMap.xWp[ i ], 30, 50, "xWp[ i ]", "" );
   //DrawSmallNumber( (unsigned) zoomX, 20, 50, "zoomX", "" );
   //DrawSmallNumber( (unsigned) (100 * tcos( heading )), 10, 50, "tcos()", "" );

   // is this the waypoint we're looking for??
   if ( mfdMap.currWp == i && mfdMap.lastWp > 0 )
  {
     // calculate range
     zoomX = ( mfdMap.xWp[ i ] - mfdMap.x ) / 100;
     zoomY = ( mfdMap.yWp[ i ] - mfdMap.y ) / 100;
     if ( zoomX >= 200 || zoomY >= 200 )
     {
       zoomX /= 10;
       zoomY /= 10;
       rangeToWp = sqrt_wchecking( (long) zoomX * zoomX + zoomY * zoomY  ) * 10;
     }
     else
       rangeToWp = sqrt_wchecking( (long) zoomX * zoomX + zoomY * zoomY  );

     if ( wpX > x + w || wpX < x || wpY > y + h || wpY < y  )
  	 {
       zoomX = ( mfdMap.xWp[ i ] - mfdMap.x ) / mfdMap.zoom;
     zoomY = ( mfdMap.yWp[ i ] - mfdMap.y ) / mfdMap.zoom;

       if ( rangeToWp == 0 )
         rangeToWp = 1;

   wpX = minLineFactor * mfdMap.zoom * calcXComp( zoomX, zoomY, heading ) / rangeToWp;
       wpY = minLineFactor * mfdMap.zoom * calcYComp( zoomX, zoomY, heading ) / rangeToWp;
       wpX += cX;
       wpY += cY;

     }
     line( cX, cY, wpX, wpY );
     //sprintf( tempStr, "%d", i );
     //bmpTable.bmpTextxy( wpX - 6, wpY - 9, tempStr, TextColor2, BackGrndColor );
   }
 }

 sprintf( tempStr, "%lukm  ", rangeToWp );
 padLeft( tempStr, 5 );
 bmpTable.bmpTextxy( x + w, y, tempStr, TextColor, BackGrndColor );
 bmpTable.bmpTextxy( x + w, cY - 25, "[   ]", TextColor, BackGrndColor );
 sprintf( tempStr, "%u", heading );
 padRight( tempStr, 3, '0' );
 bmpTable.bmpTextxy( x + w, cY - 18, tempStr, TextColor2, BackGrndColor );
 sprintf( tempStr, "Z:%lu ", ( mfdMap.zoom / minZoom ) );
 padRight( tempStr, 7 );
 bmpTable.bmpTextxy( x + w, y + h - 50, tempStr, TextColor, BackGrndColor );

 rectangle( x + w, y, x + w + 13, y + h );
}

// IncMapZoom ====================================================================================================

void ssWindow::IncMapZoom()
{
 if ( mfdMap.zoom >= 65536L )
   mfdMap.zoom = 32768L;
 else
  mfdMap.zoom /= 2;

 if ( mfdMap.zoom < minZoom )
   mfdMap.zoom = minZoom;

}

// DecMapZoom ====================================================================================================

void ssWindow::DecMapZoom()
{
 mfdMap.zoom *= 2;
 if ( mfdMap.zoom >= 65536L )
   mfdMap.zoom = 65536L;
}

// ChangeMapWP ====================================================================================================

void ssWindow::ChangeMapWP()
{
 mfdMap.currWp++;
 if ( mfdMap.currWp >= mfdMap.lastWp )
   mfdMap.currWp = 0;
}

// add waypoint ============================================================================

void ssWindow::AddMapWP()
{
 mfdMap.xWp[ mfdMap.lastWp ] = mfdMap.x;
 mfdMap.yWp[ mfdMap.lastWp ] = mfdMap.y;
 mfdMap.zWp[ mfdMap.lastWp ] = mfdMap.z;
 mfdMap.lastWp++;

}

// restart =====================================================================================

void ssWindow::RestartMap()
{
 mfdMap.x = 0;
 mfdMap.y = 0;
 mfdMap.z = 0;
}

[MadJeff 3]

Cool! I'm gonna start messing with this a little to get a feel for how things work. It would be nice to get a FAQ thread going to get people started, I guess I better work on that. ;)

[PG_Raptor 0]

Man... coding... I havnt' done that in forever. I do remember it being very fun though :)

[VeCTeR76 0]

Crap, I messed up.

 

First off, this code isn't really for the script, it's for a c++ program. Sorry, my bad. Secondly, if you cut and paste what I wrote above, your map will be rotated 90 clockwise. Sorry, that's the way I have mine... my screen is a laptop screen laying on its side.

[GhostDog 0]

Hey thats the good thing about being able to code yourself: you can do it just the way you like it and the rest can just **** *** :D