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;
}