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);
}
}
void getangleerrorfrompilotinput(fixedpointnum * angleerror)
{
// sets the ange errors for roll, pitch, and yaw based on where the pilot has the tx sticks.
fixedpointnum rxrollvalue;
fixedpointnum rxpitchvalue;
// if in headfree mode, rotate the pilot's stick inputs by the angle that is the difference between where we are currently heading and where we were heading when we armed.
if (global.activecheckboxitems & CHECKBOXMASKHEADFREE) {
fixedpointnum angledifference = global.currentestimatedeulerattitude[YAWINDEX] - global.heading_when_armed;
fixedpointnum cosangledifference = lib_fp_cosine(angledifference);
fixedpointnum sinangledifference = lib_fp_sine(angledifference);
rxpitchvalue = lib_fp_multiply(global.rxvalues[PITCHINDEX], cosangledifference) + lib_fp_multiply(global.rxvalues[ROLLINDEX], sinangledifference);
rxrollvalue = lib_fp_multiply(global.rxvalues[ROLLINDEX], cosangledifference) - lib_fp_multiply(global.rxvalues[PITCHINDEX], sinangledifference);
#ifdef INVERTED
rxrollvalue = - rxrollvalue;
#endif
} else {
rxpitchvalue = global.rxvalues[PITCHINDEX];
rxrollvalue = global.rxvalues[ROLLINDEX];
#ifdef INVERTED
rxrollvalue = - rxrollvalue;
#endif
}
// first, calculate level mode values
// how far is our estimated current attitude from our desired attitude?
// desired angle is rxvalue (-1 to 1) times LEVEL_MODE_MAX_TILT
// First, figure out which max angle we are using depending on aux switch settings.
fixedpointnum levelmodemaxangle;
if (global.activecheckboxitems & CHECKBOXMASKHIGHANGLE)
levelmodemaxangle = FP_LEVEL_MODE_MAX_TILT_HIGH_ANGLE;
else
levelmodemaxangle = FP_LEVEL_MODE_MAX_TILT;
// the angle error is how much our current angles differ from our desired angles.
fixedpointnum levelmoderollangleerror = lib_fp_multiply(rxrollvalue, levelmodemaxangle) - global.currentestimatedeulerattitude[ROLLINDEX];
fixedpointnum levelmodepitchangleerror = lib_fp_multiply(rxpitchvalue, levelmodemaxangle) - global.currentestimatedeulerattitude[PITCHINDEX];
// In acro mode, we want the rotation rate to be proportional to the pilot's stick movement. The desired rotation rate is
// the stick movement * a multiplier.
// Fill angleerror with acro values. If we are currently rotating at rate X and
// we want to be rotating at rate Y, then our angle should be off by (Y-X)*timesliver. In theory, we should probably accumulate
// this error over time, but our I term will do that anyway and we are talking about rates, which don't need to be perfect.
// First figure out what max rotation rates we are using depending on our aux switch settings.
fixedpointnum maxyawrate;
fixedpointnum maxpitchandrollrate;
if (global.activecheckboxitems & CHECKBOXMASKHIGHRATES) {
maxyawrate = highyawrate;
maxpitchandrollrate = highpitchandrollrate;
} else {
maxyawrate = usersettings.maxyawrate;
maxpitchandrollrate = usersettings.maxpitchandrollrate;
}
angleerror[ROLLINDEX] = lib_fp_multiply(lib_fp_multiply(rxrollvalue, maxpitchandrollrate) - global.gyrorate[ROLLINDEX], global.timesliver);
angleerror[PITCHINDEX] = lib_fp_multiply(lib_fp_multiply(rxpitchvalue, maxpitchandrollrate) - global.gyrorate[PITCHINDEX], global.timesliver);
// put a low pass filter on the yaw gyro. If we don't do this, things can get jittery.
lib_fp_lowpassfilter(&filteredyawgyrorate, global.gyrorate[YAWINDEX], global.timesliver >> (TIMESLIVEREXTRASHIFT - 3), FIXEDPOINTONEOVERONESIXTYITH, 3);
if(global.activecheckboxitems & CHECKBOXMASKYAWHOLD) {
// Yaw hold: control yaw angle instead of yaw rate by accumulating the yaw errors.
// This is similar to compass mode, but no hardware compass needed.
if(!(global.previousactivecheckboxitems & CHECKBOXMASKYAWHOLD)) {
// This mode was just switched on. Use current position as reference by setting error to zero.
accumulatedyawerror = 0;
}
// Accumulate yaw angle error
accumulatedyawerror += lib_fp_multiply(lib_fp_multiply(global.rxvalues[YAWINDEX], maxyawrate) - filteredyawgyrorate, global.timesliver) >> TIMESLIVEREXTRASHIFT;
// Make sure it does not get too high
lib_fp_constrain180(&accumulatedyawerror);
angleerror[YAWINDEX] = accumulatedyawerror;
} else {
