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);
}
}
// read the acc and gyro a bunch of times and get an average of how far off they are.
// assumes the aircraft is sitting level and still.
// If both==false, only gyro is calibrated and accelerometer calibration not touched.
void calibrategyroandaccelerometer(bool both)
{
#ifdef X4_BUILD
uint8_t ledstatus;
#endif
// do some readings to initialize the gyro filters
for (int x = 0; x < 15; ++x) {
readgyro();
readacc();
}
// start the low pass at the first readout rather then at zero
for (int x = 0; x < 3; ++x) {
usersettings.gyrocalibration[x]= -global.gyrorate[x];
if(both)
usersettings.acccalibration[x]= -global.acc_g_vector[x];
}
/*
for (int x = 0; x < 3; ++x) {
usersettings.gyrocalibration[x] = 0;
if(both)
usersettings.acccalibration[x] = 0;
}
*/
fixedpointnum totaltime = 0;
// calibrate the gyro and acc
while (totaltime < (FIXEDPOINTCONSTANT(4) << TIMESLIVEREXTRASHIFT)) // 4 seconds
{
readgyro();
if(both) {
readacc();
global.acc_g_vector[ZINDEX] -= FIXEDPOINTONE; // vertical vector should be at 1g
}
calculatetimesliver();
totaltime += global.timesliver;
#ifdef X4_BUILD
// Rotating LED pattern
ledstatus = (uint8_t)((totaltime >> (FIXEDPOINTSHIFT+TIMESLIVEREXTRASHIFT-3))& 0x3);
switch(ledstatus) {
case 0:
x4_set_leds(X4_LED_FL);
break;
case 1:
x4_set_leds(X4_LED_FR);
break;
case 2:
x4_set_leds(X4_LED_RR);
break;
case 3:
x4_set_leds(X4_LED_RL);
break;
}
#endif
for (int x = 0; x < 3; ++x) {
lib_fp_lowpassfilter(&usersettings.gyrocalibration[x], -global.gyrorate[x], global.timesliver, FIXEDPOINTONEOVERONE, TIMESLIVEREXTRASHIFT);
if(both)
lib_fp_lowpassfilter(&usersettings.acccalibration[x], -global.acc_g_vector[x], global.timesliver, FIXEDPOINTONEOVERONE, TIMESLIVEREXTRASHIFT);
}
}
}
// It all starts here:
int main(void) {
// start with default user settings in case there's nothing in eeprom
default_user_settings();
// try to load settings from eeprom
read_user_settings_from_eeprom();
// set our LED as a digital output
lib_digitalio_initpin(LED1_OUTPUT,DIGITALOUTPUT);
//initialize the libraries that require initialization
lib_timers_init();
lib_i2c_init();
// pause a moment before initializing everything. To make sure everything is powered up
lib_timers_delaymilliseconds(100);
// initialize all other modules
init_rx();
init_outputs();
serial_init();
init_gyro();
init_acc();
init_baro();
init_compass();
init_gps();
init_imu();
// set the default i2c speed to 400 KHz. If a device needs to slow it down, it can, but it should set it back.
lib_i2c_setclockspeed(I2C_400_KHZ);
// initialize state
global.state.armed=0;
global.state.calibratingCompass=0;
global.state.calibratingAccAndGyro=0;
global.state.navigationMode=NAVIGATION_MODE_OFF;
global.failsafeTimer=lib_timers_starttimer();
// run loop
for(;;) {
// check to see what switches are activated
check_checkbox_items();
// check for config program activity
serial_check_for_action();
calculate_timesliver();
// run the imu to estimate the current attitude of the aircraft
imu_calculate_estimated_attitude();
// arm and disarm via rx aux switches
if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) { // see if we want to change armed modes
if (!global.state.armed) {
if (global.activeCheckboxItems & CHECKBOX_MASK_ARM) {
global.state.armed=1;
#if (GPS_TYPE!=NO_GPS)
navigation_set_home_to_current_location();
#endif
global.home.heading=global.currentEstimatedEulerAttitude[YAW_INDEX];
global.home.location.altitude=global.baroRawAltitude;
}
} else if (!(global.activeCheckboxItems & CHECKBOX_MASK_ARM)) global.state.armed=0;
}
#if (GPS_TYPE!=NO_GPS)
// turn on or off navigation when appropriate
if (global.state.navigationMode==NAVIGATION_MODE_OFF) {
if (global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME) { // return to home switch turned on
navigation_set_destination(global.home.location.latitude,global.home.location.longitude);
global.state.navigationMode=NAVIGATION_MODE_RETURN_TO_HOME;
} else if (global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD) { // position hold turned on
navigation_set_destination(global.gps.currentLatitude,global.gps.currentLongitude);
global.state.navigationMode=NAVIGATION_MODE_POSITION_HOLD;
}
} else { // we are currently navigating
// turn off navigation if desired
if ((global.state.navigationMode==NAVIGATION_MODE_RETURN_TO_HOME && !(global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME)) || (global.state.navigationMode==NAVIGATION_MODE_POSITION_HOLD && !(global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD))) {
global.state.navigationMode=NAVIGATION_MODE_OFF;
// we will be turning control back over to the pilot.
reset_pilot_control();
}
}
#endif
// read the receiver
read_rx();
// turn on the LED when we are stable and the gps has 5 satelites or more
#if (GPS_TYPE==NO_GPS)
lib_digitalio_setoutput(LED1_OUTPUT, (global.state.stable==0)==LED1_ON);
#else
lib_digitalio_setoutput(LED1_OUTPUT, (!(global.state.stable && global.gps.numSatelites>=5))==LED1_ON);
#endif
// get the angle error. Angle error is the difference between our current attitude and our desired attitude.
// It can be set by navigation, or by the pilot, etc.
fixedpointnum angleError[3];
// let the pilot control the aircraft.
get_angle_error_from_pilot_input(angleError);
#if (GPS_TYPE!=NO_GPS)
// read the gps
unsigned char gotNewGpsReading=read_gps();
// if we are navigating, use navigation to determine our desired attitude (tilt angles)
if (global.state.navigationMode!=NAVIGATION_MODE_OFF) { // we are navigating
navigation_set_angle_error(gotNewGpsReading,angleError);
}
#endif
if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) {
// We are probably on the ground. Don't accumnulate error when we can't correct it
reset_pilot_control();
// bleed off integrated error by averaging in a value of zero
lib_fp_lowpassfilter(&global.integratedAngleError[ROLL_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
lib_fp_lowpassfilter(&global.integratedAngleError[PITCH_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
lib_fp_lowpassfilter(&global.integratedAngleError[YAW_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
}
#ifndef NO_AUTOTUNE
// let autotune adjust the angle error if the pilot has autotune turned on
if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOTUNE) {
if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE)) {
autotune(angleError,AUTOTUNE_STARTING); // tell autotune that we just started autotuning
} else {
autotune(angleError,AUTOTUNE_TUNING); // tell autotune that we are in the middle of autotuning
}
} else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE) {
autotune(angleError,AUTOTUNE_STOPPING); // tell autotune that we just stopped autotuning
}
#endif
// This gets reset every loop cycle
// keep a flag to indicate whether we shoud apply altitude hold. The pilot can turn it on or
// uncrashability/autopilot mode can turn it on.
global.state.altitudeHold=0;
if (global.activeCheckboxItems & CHECKBOX_MASK_ALTHOLD) {
global.state.altitudeHold=1;
if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_ALTHOLD)) {
// we just turned on alt hold. Remember our current alt. as our target
global.altitudeHoldDesiredAltitude=global.altitude;
global.integratedAltitudeError=0;
}
}
fixedpointnum throttleOutput;
#ifndef NO_AUTOPILOT
// autopilot is available
if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOPILOT) {
if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT)) {
// let autopilot know to transition to the starting state
autopilot(AUTOPILOT_STARTING);
} else {
// autopilot normal run state
autopilot(AUTOPILOT_RUNNING);
}
} else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT) {
// tell autopilot that we just stopped autotuning
autopilot(AUTOPILOT_STOPPING);
} else {
// get the pilot's throttle component
// convert from fixedpoint -1 to 1 to fixedpoint 0 to 1
throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET;
}
#else
// get the pilot's throttle component
// convert from fixedpoint -1 to 1 to fixedpoint 0 to 1
throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET;
#endif
#ifndef NO_UNCRASHABLE
uncrashable(gotNewGpsReading,angleError,&throttleOutput);
#endif
#if (BAROMETER_TYPE!=NO_BAROMETER)
// check for altitude hold and adjust the throttle output accordingly
if (global.state.altitudeHold) {
global.integratedAltitudeError+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,global.timesliver);
lib_fp_constrain(&global.integratedAltitudeError,-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT); // don't let the integrated error get too high
// do pid for the altitude hold and add it to the throttle output
throttleOutput+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,settings.pid_pgain[ALTITUDE_INDEX])-lib_fp_multiply(global.altitudeVelocity,settings.pid_dgain[ALTITUDE_INDEX])+lib_fp_multiply(global.integratedAltitudeError,settings.pid_igain[ALTITUDE_INDEX]);
}
#endif
#ifndef NO_AUTOTHROTTLE
if ((global.activeCheckboxItems & CHECKBOX_MASK_AUTOTHROTTLE) || global.state.altitudeHold) {
if (global.estimatedDownVector[Z_INDEX]>FIXEDPOINTCONSTANT(.3)) {
// Divide the throttle by the throttleOutput by the z component of the down vector
// This is probaly the slow way, but it's a way to do fixed point division
fixedpointnum recriprocal=lib_fp_invsqrt(global.estimatedDownVector[Z_INDEX]);
recriprocal=lib_fp_multiply(recriprocal,recriprocal);
throttleOutput=lib_fp_multiply(throttleOutput-AUTOTHROTTLE_DEAD_AREA,recriprocal)+AUTOTHROTTLE_DEAD_AREA;
}
}
#endif
#ifndef NO_FAILSAFE
// if we don't hear from the receiver for over a second, try to land safely
if (lib_timers_gettimermicroseconds(global.failsafeTimer)>1000000L) {
throttleOutput=FPFAILSAFEMOTOROUTPUT;
// make sure we are level!
angleError[ROLL_INDEX]=-global.currentEstimatedEulerAttitude[ROLL_INDEX];
angleError[PITCH_INDEX]=-global.currentEstimatedEulerAttitude[PITCH_INDEX];
}
#endif
// calculate output values. Output values will range from 0 to 1.0
// calculate pid outputs based on our angleErrors as inputs
fixedpointnum pidOutput[3];
// Gain Scheduling essentialy modifies the gains depending on
// throttle level. If GAIN_SCHEDULING_FACTOR is 1.0, it multiplies PID outputs by 1.5 when at full throttle,
// 1.0 when at mid throttle, and .5 when at zero throttle. This helps
// eliminate the wobbles when decending at low throttle.
fixedpointnum gainSchedulingMultiplier=lib_fp_multiply(throttleOutput-FIXEDPOINTCONSTANT(.5),FIXEDPOINTCONSTANT(GAIN_SCHEDULING_FACTOR))+FIXEDPOINTONE;
for (int x=0;x<3;++x) {
global.integratedAngleError[x]+=lib_fp_multiply(angleError[x],global.timesliver);
// don't let the integrated error get too high (windup)
lib_fp_constrain(&global.integratedAngleError[x],-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT);
// do the attitude pid
pidOutput[x]=lib_fp_multiply(angleError[x],settings.pid_pgain[x])-lib_fp_multiply(global.gyrorate[x],settings.pid_dgain[x])+(lib_fp_multiply(global.integratedAngleError[x],settings.pid_igain[x])>>4);
// add gain scheduling.
pidOutput[x]=lib_fp_multiply(gainSchedulingMultiplier,pidOutput[x]);
}
lib_fp_constrain(&throttleOutput,0,FIXEDPOINTONE); // Keep throttle output between 0 and 1
compute_mix(throttleOutput, pidOutput); // aircraft type dependent mixes
#if (NUM_SERVOS>0)
// do not update servos during unarmed calibration of sensors which are sensitive to vibration
if (global.state.armed || (!global.state.calibratingAccAndGyro)) write_servo_outputs();
#endif
write_motor_outputs();
}
