inline uint16_t getGpsDistance()
{
uint32_t lat = (((uint32_t)gpsLatitude_bp / 100) * 1000000) + (((gpsLatitude_bp % 100) * 10000 + gpsLatitude_ap)*5)/3;
uint32_t lon = (((uint32_t)gpsLongitude_bp / 100) * 1000000) + (((gpsLongitude_bp % 100) * 10000 + gpsLongitude_ap)*5)/3;
// lat=50247528; //50247528 1000 = ca 111 m
// lon=11387561; //11397561 1000 = ca 70 m
uint32_t lat_len = (lat > pilotLatitude) ? lat - pilotLatitude : pilotLatitude - lat;
uint32_t lon_len = (lon > pilotLongitude) ? lon - pilotLongitude : pilotLongitude - lon;
uint32_t alt_len = (abs_alt > pilotAltitude) ? abs_alt - pilotAltitude : pilotAltitude - abs_alt;
lat_len = (lat_len * metersPerDegreeLat) / 1000; // * 1113
lon_len = (lon_len * metersPerDegreeLon) / 10000; // * 1113 * cos(lat)
uint32_t temp_len = (sqrt32(lat_len*lat_len + lon_len*lon_len));
//um den Gesamtabstand mit der Höhe zu rechnen muß erneut quadriert und Wurzel gezogen werden
lon_len = sqrt32(temp_len*temp_len + alt_len*alt_len); // Ergebnis in 1/10tel Meter
return lon_len;
}
bool InputManager::moveToPosition(Vector3<s16>* currentPosition, float32 speed, Vector3<s16>& destinationOut)
{
s16 from_x = currentPosition->x();
s16 from_y = currentPosition->y();
touchPosition heldPosition;
if(keysHeld() & KEY_TOUCH){
touchRead(&heldPosition);
s16 diff_x = (heldPosition.px - from_x);
s16 diff_y = (heldPosition.py - from_y);
u32 magnitude = sqrt32(diff_x * diff_x + diff_y * diff_y);
if (magnitude < 2)
return NULL;
diff_x = round(diff_x * speed / magnitude);
diff_y = round(diff_y * speed / magnitude);
destinationOut.setX((u16)(from_x + diff_x));
destinationOut.setY((u16)(from_y + diff_y));
return true;
}
else
{
destinationOut = *currentPosition;
return false;
}
}
uint16_t acc_magnitude(int8_t* angle)
{
int16_t acc[3], a, alpha;
/* briefly turn on accelerometer */
acc_write(ACC_REG_CTRL_REG1, 0x97);
timer_wait(MILLISECONDS(2));
acc_read(ACC_REG_OUT_X, sizeof(acc), (uint8_t*)&acc);
acc_write(ACC_REG_CTRL_REG1, 0x00);
/* get acceleration vector magnitude */
a = sqrt32(
((int)acc[0])*acc[0] +
((int)acc[1])*acc[1] +
((int)acc[2])*acc[2]
)/64;
/* calculate tag angle */
if(angle)
{
if(!a)
alpha = 127;
else
{
alpha = (acc[2]*2)/a;
if(alpha>127)
alpha=127;
else
if(alpha<-127)
alpha=-127;
}
*angle = asin7deg(alpha);
}
return a;
}
/* Triangle median */
static int triangle_median(int f1, int f2, int f3)
{
// Median to a = sqrt((2*b*b + 2*c*c - *a*a) / 4)
// Median to b = sqrt((2*a*a + 2*c*c - *b*b) / 4)
// Median to c = sqrt((2*a*a + 2*b*b - *c*c) / 4)
ret = ( 2*f1*f1 + 2*f2*f2 - f3*f3 ) / 4;
return sqrt32(ret);
}
dm_time_t
dm_mech_g1_seek_3pt_curve(struct dm_disk_if *d,
struct dm_mech_state *begin,
struct dm_mech_state *end,
int rw)
{
dm_time_t result;
int cyls;
struct dm_mech_g1 *m = (struct dm_mech_g1 *)d->mech;
cyls = abs(end->cyl - begin->cyl);
if (cyls == 0) {
result = 0;
}
else {
result = m->seekone;
result += m->seekavg * (cyls - 1);
result += m->seekfull * sqrt32(cyls - 1);
}
return result;
}
void genTextures()
{
// int pal_addr = gluTexLoadPal( &paldata[0], 8, GL_RGB8_A5 );
u16 paldata[8];
for(int i = 0; i <8; i++)
paldata[i] = 0xFFFF;
u8 texdata[64*64];
for(int x = 0; x < 64; x++)
for(int y = 0; y < 64; y++)
{
texdata[x+y*64] = (u8) 1;
}
glBindTexture(0, textureIDS[0]);
glTexImage2D(0, 0, GL_RGB256, TEXTURE_SIZE_64, TEXTURE_SIZE_64, 0, TEXGEN_TEXCOORD, &texdata);
glColorTableEXT( 0, 0, 8, 0, 0, paldata );
genTextureFromImg(1, 0);
genTextureFromImg(2, 1);
genTextureFromImg(3, 2);
u8* texdata2 = new u8[256*64];
u8* text = (u8*) introtextBitmap;
for(int x = 0; x < 256; x++)
for(int y = 0; y < 64; y++)
{
u8 val = 0;
if(text[x+y*256] == 0x01)
val = 31<<3;
texdata2[x+y*256] = val;
}
glBindTexture(4, textureIDS[4]);
glTexImage2D(0, 0, GL_RGB8_A5, TEXTURE_SIZE_256, TEXTURE_SIZE_64, 0, TEXGEN_TEXCOORD, texdata2);
glColorTableEXT( 0, 0, 8, 0, 0, paldata );
genTextureFromImg(6, 3);
genTextureFromImg(8, 4);
for(int x = 0; x < 64; x++)
for(int y = 0; y < 64; y++)
texdata[x+y*64] = (u8) (31)<< 3 | 1;
glBindTexture(7, textureIDS[7]);
glTexImage2D(0, 0, GL_RGB8_A5, TEXTURE_SIZE_64, TEXTURE_SIZE_64, 0, TEXGEN_TEXCOORD, &texdata);
glColorTableEXT( 0, 0, 8, 0, 0, paldata );
for(int x = 0; x < 64; x++)
for(int y = 0; y < 64; y++)
{
int dist = (x-32)*(x-32)+(y-32)*(y-32);
dist = sqrt32(dist);
// dist /= 32;
if(dist>31) dist = 31;
if(dist<0) dist = 0;
if(dist !=31)
texdata[x+y*64] = (u8) 31<< 3 | 1;
else
texdata[x+y*64] = (u8) 0;
}
glBindTexture(9, textureIDS[9]);
glTexImage2D(0, 0, GL_RGB8_A5, TEXTURE_SIZE_64, TEXTURE_SIZE_64, 0, TEXGEN_TEXCOORD, &texdata);
glColorTableEXT( 0, 0, 9, 0, 0, paldata );
genTextureFromImg(10, 5);
genTextureFromImg(11, 6);
genTextureFromImg(12, 7);
genTextureFromImg(13, 8);
delete[] texdata2;
}
void_t vt_inclined_plane(virtualtouch_s* vt)
{
dbg_function();
int tmp;
int_t alpha = mm_alpha(vt->countalpha);
int_t ahpla = 1000 - mm_alpha(vt->countalpha);
tmp = (vt->distance.dx + vt->distance.sx + vt->distance.dw + vt->distance.up) / 4;
if ( vt->eamflags & VT_EAMF_PLANE )
vt->p3d.z = (alpha * tmp + ahpla * vt->p3d.z)/1000;
else
vt->p3d.z = tmp;
dbg_info("plane: [z=%3d]", vt->p3d.z);
int_t h, f, l;
int_t b = vt->vxdistance.x;
if (vt->distance.sx > vt->distance.dx)
{
h = vt->distance.sx - vt->distance.dx;
f = 1;
}
else
{
h = vt->distance.dx - vt->distance.sx;
f = -1;
}
l = sqrt32(b*b + h*h);
tmp = ((vt->peso.x * h) / l) * f;
if ( vt->eamflags & VT_EAMF_PLANE )
vt->p3d.x = (alpha * tmp + ahpla * vt->p3d.x)/1000;
else
vt->p3d.x = tmp;
dbg_info("plane: [p=%3d z=%3d h=%3d b=%3d l=%3d f=%3d][px=%3d]",
vt->peso.x, vt->p3d.z, h, b, l, f, vt->p3d.x);
b = vt->vxdistance.y;
if (vt->distance.up > vt->distance.dw)
{
h = vt->distance.up - vt->distance.dw;
f = -1;
}
else
{
h = vt->distance.dw - vt->distance.up;
f = 1;
}
l = sqrt32(b*b + h*h);
tmp = ((vt->peso.y * h) / l) * f;
if ( vt->eamflags & VT_EAMF_PLANE )
vt->p3d.y = (alpha * tmp + ahpla * vt->p3d.y)/1000;
else
vt->p3d.y = tmp;
dbg_info("plane: [p=%3d z=%3d h=%3d b=%3d l=%3d f=%3d][px=%3d]",
vt->peso.y, vt->p3d.z, h, b, l, f, vt->p3d.y);
dbg_return(0);
}
static int calc_distance(point_2d* a, point_2d* b)
{
ret = (a->x - b->x) * (a->x - b->x) + (a->y - b->y) * (a->y - b->y);
return sqrt32(ret);
}
