fixedpointnum navigation_getdistanceandbearing(fixedpointnum lat1,fixedpointnum lon1,fixedpointnum lat2,fixedpointnum lon2,fixedpointnum *bearing)
{ // returns fixedpointnum distance in meters and bearing in fixedpointnum degrees from point 1 to point 2
fixedpointnum latdiff=lat2-lat1;
fixedpointnum londiff=lib_fp_multiply(lon2-lon1,lib_fp_cosine(lat1>>LATLONGEXTRASHIFT));
*bearing = FIXEDPOINT90 + lib_fp_atan2(-latdiff, londiff);
if (*bearing >FIXEDPOINT180) *bearing -= FIXEDPOINT360;
// distance is 111319 meters per degree. This factor needs to be shifted 16 to make it a fixedpointnum.
// Since lat and lon are already shifted by 6,
// we will shift by 10 more total. Shift lat and long by 8 and the constant by 2 to get the extra 10.
// The squaring will overflow our fixedpointnum at distances greater than 1000 meters or so, so test for size and shift accordingly
if (lib_fp_abs(latdiff)+lib_fp_abs(londiff)>40000L)
{ // for big distances, don't shift lat and long. Instead shift the constant by 10.
// this will get us to 32 kilometers at which point our fixedpoingnum can't hold a larger distance.
return(lib_fp_multiply(lib_fp_sqrt(lib_fp_multiply(latdiff,latdiff)+lib_fp_multiply(londiff,londiff)),113990656L));
}
else
{
latdiff=latdiff<<8;
londiff=londiff<<8;
return(lib_fp_multiply(lib_fp_sqrt(lib_fp_multiply(latdiff,latdiff)+lib_fp_multiply(londiff,londiff)),445276L));
}
}
void imucalculateestimatedattitude(void)
{
float EstG[3];
float acc[3]; // holds float accel vector
float deltatime; // time in seconds
float gyros[3];
vectorcopy ( &EstG[0] , &GEstG[0] );
deltatime = (float)lib_timers_gettimermicrosecondsandreset(&gp_timer) * 0.000001f ; // time in seconds
deltatime = deltatime* 0.92; // correction factor
// unknown reason
readgyro();
readacc();
// correct the gyro and acc readings to remove error
// x & y accel offsets only
for ( int x = 0; x < 3; ++x) { // was 3
global.gyrorate[x] = global.gyrorate[x] + usersettings.gyrocalibration[x];
global.acc_g_vector[x] = global.acc_g_vector[x] + usersettings.acccalibration[x];
acc[x]= global.acc_g_vector[x]>>6;
}
// deadzone for yaw rate
//global.gyrorate[2] = ( 0 || global.gyrorate[2] > 40000 || global.gyrorate[2]< -40000 ) * global.gyrorate[2] ;
for ( int i = 0 ; i < 3; i++)
{
gyros[i] = tofloat( global.gyrorate[i] ) * deltatime * 0.01745329;
}
#ifndef SMALL_ANGLE_APPROX
// This does a "proper" matrix rotation using gyro deltas without small-angle approximation
float mat[3][3];
float tempvect[3];
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
vectorcopy ( &tempvect[0] , & EstG[0] );
// the signs are differnt due to different conventions
// for positive/negative angles in various multiwii forks this is based on
cosx = _cosf( gyros[1]);
sinx = _sinf( gyros[1]);
cosy = _cosf( -gyros[0]);
siny = _sinf( -gyros[0]);
cosz = _cosf( -gyros[2]);
sinz = _sinf( -gyros[2]);
coszcosx = cosz * cosx;
coszcosy = cosz * cosy;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
mat[0][0] = coszcosy;
mat[0][1] = -cosy * sinz;
mat[0][2] = siny;
mat[1][0] = sinzcosx + (coszsinx * siny);
mat[1][1] = coszcosx - (sinzsinx * siny);
mat[1][2] = -sinx * cosy;
mat[2][0] = (sinzsinx) - (coszcosx * siny);
mat[2][1] = (coszsinx) + (sinzcosx * siny);
mat[2][2] = cosy * cosx;
EstG[0] = tempvect[0] * mat[0][0] + tempvect[1] * mat[1][0] + tempvect[2] * mat[2][0];
EstG[1] = tempvect[0] * mat[0][1] + tempvect[1] * mat[1][1] + tempvect[2] * mat[2][1];
EstG[2] = tempvect[0] * mat[0][2] + tempvect[1] * mat[1][2] + tempvect[2] * mat[2][2];
#endif // end rotation matrix
#ifdef SMALL_ANGLE_APPROX
// this is a rotation with small angle approximation
float deltagyroangle; // holds float gyro angle in rad
// Rotate Estimated vector(s), ROLL
EstG[2] = scos(gyros[0]) * EstG[2] - ssin(gyros[0]) * EstG[0];
EstG[0] = ssin(gyros[0]) * EstG[2] + scos(gyros[0]) * EstG[0];
// Rotate Estimated vector(s), PITCH
EstG[1] = scos(gyros[1]) * EstG[1] + ssin(gyros[1]) * EstG[2];
EstG[2] = -ssin(gyros[1]) * EstG[1] + scos(gyros[1]) * EstG[2];
// Rotate Estimated vector(s), YAW
EstG[0] = scos(gyros[2]) * EstG[0] - ssin(gyros[2]) * EstG[1];
EstG[1] = ssin(gyros[2]) * EstG[0] + scos(gyros[2]) * EstG[1];
#endif // end small angle approx
// yaw not tested ( maybe for yaw hold? )
// includes deadzone
// global.currentestimatedeulerattitude[YAWINDEX] += ( 0 || global.gyrorate[2] > 40000 || global.gyrorate[2]< -40000 ) * ( (float) ( global.gyrorate[2] ) * deltatime + 0.5f);
// yaw without deadzone
// not tested , i am not sure what the yaw angle is even used for
global.currentestimatedeulerattitude[YAWINDEX] += ( (float) ( global.gyrorate[2] ) * deltatime);
lib_fp_constrain180(&global.currentestimatedeulerattitude[YAWINDEX]);
// global.estimateddownvector[ZINDEX] < 0
// in pilotcontrol.c fix for inverted(not tested)
global.estimateddownvector[ZINDEX] = (EstG[2]>0)? 1111:-1111 ;
// orientation vector magnitude
float mag = 0;
mag = calcmagnitude( &EstG[0] );
// normalize orientation vector
if (1)
{
for (int axis = 0; axis < 3; axis++)
{
EstG[axis] = EstG[axis] / ( mag / ACC_1G );
}
}
//debug4 = mag;
// calc acc mag
float accmag;
accmag = calcmagnitude( &acc[0] );
// debug2 = accmag;
//normvector( acc , accmag, normal);
// normalize acc
for (int axis = 0; axis < 3; axis++)
{
acc[axis] = acc[axis] / (accmag / ACC_1G) ;
}
// test acc mag
//debug5 = calcmagnitude( &acc[0] );
/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
// times for 3ms loop time
// filter time changes linearily with loop time
// 0.970 0.1s
// 0.988 0.25s
// 0.994 0.5 s
// 0.996 0.75 s
// 0.997 1.0 sec
// 0.998 1.5 sec
// 0.9985 2 sec
// 0.999 3 sec
// 0.99925 4 s
#define GYR_CMPF_FACTOR 0.998f // was 0.998
#define DISABLE_ACC 0
#define ACC_MIN 0.8f
#define ACC_MAX 1.2f
static unsigned int count = 0;
if ( ( accmag > ACC_MIN * ACC_1G ) && ( accmag < ACC_MAX * ACC_1G ) && !DISABLE_ACC )
{
//for (axis = 0; axis < 3; axis++)
if ( count >= 10 ) // loop time = 3ms so 30ms wait
{
//x4_set_leds( 0xFF);
EstG[0] = EstG[0] * GYR_CMPF_FACTOR + (float)acc[0] * ( 1.0f - GYR_CMPF_FACTOR );
EstG[1] = EstG[1] * GYR_CMPF_FACTOR + (float)acc[1] * ( 1.0f - GYR_CMPF_FACTOR );
EstG[2] = EstG[2] * GYR_CMPF_FACTOR + (float)acc[2] * ( 1.0f - GYR_CMPF_FACTOR );
}
count++;
}
else
{// acc mag out of bounds
//x4_set_leds( 0x00);
count = 0;
}
vectorcopy ( &GEstG[0] , &EstG[0]);
// convert our vectors to euler angles
global.currentestimatedeulerattitude[ROLLINDEX] = lib_fp_atan2(
FIXEDPOINTCONSTANT(EstG[0]*8 ),
FIXEDPOINTCONSTANT(EstG[2]*8 ) ) ;
/*
global.currentestimatedeulerattitude[PITCHINDEX] = lib_fp_atan2(
FIXEDPOINTCONSTANT( EstG[1]*8),
FIXEDPOINTCONSTANT( EstG[2]*8) );
*/
if (lib_fp_abs(global.currentestimatedeulerattitude[ROLLINDEX]) > FIXEDPOINT45 && lib_fp_abs(global.currentestimatedeulerattitude[ROLLINDEX]) < FIXEDPOINT135) {
global.currentestimatedeulerattitude[PITCHINDEX] =
lib_fp_atan2(EstG[1]*8, lib_fp_abs(EstG[0])*8);
} else {
global.currentestimatedeulerattitude[PITCHINDEX] =
lib_fp_atan2(
EstG[1]*8,
EstG[2]*8);
}
}
