WebSVN - FlightCtrl - Diff - Rev 746 and 750 - /branches/V0.68d Code Redesign killagreg/fc.c



#include "uart.h"
#include "rc.h"
#include "twimaster.h"
#include "mm3.h"

 
volatile uint16_t I2CTimeout = 100;
// gyro readings
volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw;
// gyro neutral readings
volatile int16_t AdNeutralPitch = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;

volatile int32_t Reading_IntegralGyroYaw = 0,   Reading_IntegralGyroYaw2 = 0;
volatile int32_t MeanIntegralPitch;
volatile int32_t MeanIntegralRoll;

 
// attitude acceleration integrals
volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0;

volatile int32_t Reading_Integral_Top = 0;
 
// compass course

        AdNeutralPitch = AdValueGyrPitch;
        AdNeutralRoll  = AdValueGyrRoll;
        AdNeutralYaw   = AdValueGyrYaw;
        StartNeutralRoll  = AdNeutralRoll;
        StartNeutralPitch = AdNeutralPitch;
    if(GetParamWord(PID_ACC_PITCH) > 1023)
    {
                NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY;
                NeutralAccX = abs(Mean_AccPitch) / ACC_AMPLIFY;
                NeutralAccZ = Current_AccZ;
    }

        Mean_AccTop   = ((int32_t)Mean_AccTop * 1 + ((int32_t)AdValueAccTop)) / 2L;

 
        // sum sensor readings for later averaging
    IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch;
 

    IntegralAccRoll  += ACC_AMPLIFY * AdValueAccRoll;
 
// Yaw
        // calculate yaw gyro intergral (~ to rotation angle)

        Reading_GyroYaw   = AdValueGyrYaw;

 
        Mean_AccPitch = ACC_AMPLIFY * (int32_t)AdValueAccPitch;
        Mean_AccRoll  = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
        Mean_AccTop   = (int32_t)AdValueAccTop;
    // start ADC (enables internal trigger so that the ISR in analog.c
    // updates the readings once)
    ADC_Enable();
    //update poti values by rc-signals
    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;

    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;

 
        TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
        TurnOver180Roll =  (int32_t) ParamSet.AngleTurnOverRoll  * 2500L;

}
 
/************************************************************************/

        Motor_Rear = 0;
        Motor_Front = 0;
        Motor_Right = 0;
        Motor_Left = 0;
        if(MotorTest[0]) Motor_Front = MotorTest[0];
        if(MotorTest[1]) Motor_Rear  = MotorTest[1];
        if(MotorTest[2]) Motor_Left  = MotorTest[2];
        if(MotorTest[3]) Motor_Right = MotorTest[3];
     }

 
    //DebugOut.Analog[12] = Motor_Front;

                        StickPitch += ExternStickPitch / 8;
                        StickRoll += ExternStickRoll / 8;
                        StickYaw += ExternStickYaw;

 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analog control via serial communication

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
                    if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128)

                        // reset averaging for acc and gyro integral as well as gyro integral acc correction
                        MeasurementCounter = 0;

 
                        IntegralAccPitch = 0;
 

                        IntegralAccRoll = 0;
 

                        MeanIntegralPitch = 0;

                        tmp_long   =  (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch);
                        tmp_long  /= 16;
                        tmp_long2  = (int32_t)(IntegralRoll   / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll);
                        tmp_long2 /= 16;

 
                        if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands
                        {
                                tmp_long  /= 3;
                                tmp_long2 /= 3;
                        }
                        if(MaxStickYaw > 25) // reduce further is yaw stick is active
                        {
                                tmp_long  /= 3;
                                tmp_long2 /= 3;

                        }
 
                        #define BALANCE 32
                        // limit correction effect
                        if(tmp_long >  BALANCE)  tmp_long  = BALANCE;
                        if(tmp_long < -BALANCE)  tmp_long  =-BALANCE;
                        if(tmp_long2 > BALANCE)  tmp_long2 = BALANCE;

                }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // MeasurementCounter is incremented in the isr of analog.c
                if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
                {
                        static int16_t cnt = 0;
                        static int8_t last_n_p, last_n_n, last_r_p, last_r_n;
                        static int32_t MeanIntegralPitch_old, MeanIntegralRoll_old;

 
                        // if not lopping in any direction (this should be alwais the case,
                        // because the Measurement counter is reset to 0 if looping in any direction is active.)
                        if(!Looping_Pitch && !Looping_Roll)

                                MeanIntegralRoll  /= BALANCE_NUMBER;

 
                                // Calculate mean of the acceleration values
                                IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER;
 

                                IntegralAccRoll  = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER;
 
                                // Pitch ++++++++++++++++++++++++++++++++++++++++++++++++
                                // Calculate deviation of the averaged gyro integral and the averaged acceleration integral

                        MeanIntegralRoll_old  = MeanIntegralRoll;
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++
                        // reset variables used for averaging
                        IntegralAccPitch = 0;
                        IntegralAccRoll = 0;
 
                        MeanIntegralPitch = 0;
                        MeanIntegralRoll = 0;
                        MeasurementCounter = 0;
                } // end of averaging

                        {
                                CompassCourse = CompassHeading;
                                StoreNewCompassCourse = 0;
                        }
                        w = (w * FCParam.CompassYawEffect) / 64;  // scale to parameter
                        w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination
                        if(w > 0) // if there is any compass effect (avoid negative compass feedback)
                        {
                                Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;
                        }
                }

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Hight Control
// The higth control algorithm reduces the thrust but does not increase the thrust.
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                // If hight control is activated and no emergency landing is active
                if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
                {
                        int tmp_int;
                        // if hight control is activated by an rc channel

Rev 746	Rev 750
Line 64...	Line 64...
64	#include "uart.h"	64	#include "uart.h"
65	#include "rc.h"	65	#include "rc.h"
66	#include "twimaster.h"	66	#include "twimaster.h"
67	#include "mm3.h"	67	#include "mm3.h"
Line 68...	Line 68...
68		68
69	volatile unsigned int I2CTimeout = 100;	69	volatile uint16_t I2CTimeout = 100;
70	// gyro readings	70	// gyro readings
71	volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw;	71	volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw;
72	// gyro neutral readings	72	// gyro neutral readings
73	volatile int16_t AdNeutralPitch = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;	73	volatile int16_t AdNeutralPitch = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
Line 88...	Line 88...
88	volatile int32_t Reading_IntegralGyroYaw = 0, Reading_IntegralGyroYaw2 = 0;	88	volatile int32_t Reading_IntegralGyroYaw = 0, Reading_IntegralGyroYaw2 = 0;
89	volatile int32_t MeanIntegralPitch;	89	volatile int32_t MeanIntegralPitch;
90	volatile int32_t MeanIntegralRoll;	90	volatile int32_t MeanIntegralRoll;
Line 91...	Line 91...
91		91
92	// attitude acceleration integrals	92	// attitude acceleration integrals
93	volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0, IntegralAccZ = 0;	93	volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0;
Line 94...	Line 94...
94	volatile int32_t Reading_Integral_Top = 0;	94	volatile int32_t Reading_Integral_Top = 0;
95		95
96	// compass course	96	// compass course
Line 175...	Line 175...
175	AdNeutralPitch = AdValueGyrPitch;	175	AdNeutralPitch = AdValueGyrPitch;
176	AdNeutralRoll = AdValueGyrRoll;	176	AdNeutralRoll = AdValueGyrRoll;
177	AdNeutralYaw = AdValueGyrYaw;	177	AdNeutralYaw = AdValueGyrYaw;
178	StartNeutralRoll = AdNeutralRoll;	178	StartNeutralRoll = AdNeutralRoll;
179	StartNeutralPitch = AdNeutralPitch;	179	StartNeutralPitch = AdNeutralPitch;
180	if(GetParamByte(PID_ACC_PITCH) > 4)	180	if(GetParamWord(PID_ACC_PITCH) > 1023)
181	{	181	{
182	NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY;	182	NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY;
183	NeutralAccX = abs(Mean_AccPitch) / ACC_AMPLIFY;	183	NeutralAccX = abs(Mean_AccPitch) / ACC_AMPLIFY;
184	NeutralAccZ = Current_AccZ;	184	NeutralAccZ = Current_AccZ;
185	}	185	}
Line 230...	Line 230...
230	Mean_AccTop = ((int32_t)Mean_AccTop * 1 + ((int32_t)AdValueAccTop)) / 2L;	230	Mean_AccTop = ((int32_t)Mean_AccTop * 1 + ((int32_t)AdValueAccTop)) / 2L;
Line 231...	Line 231...
231		231
232	// sum sensor readings for later averaging	232	// sum sensor readings for later averaging
233	IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch;	233	IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch;
234	IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll;	-
Line 235...	Line 234...
235	IntegralAccZ += Current_AccZ - NeutralAccZ;	234	IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll;
236		235
237	// Yaw	236	// Yaw
238	// calculate yaw gyro intergral (~ to rotation angle)	237	// calculate yaw gyro intergral (~ to rotation angle)
Line 348...	Line 347...
348	Reading_GyroYaw = AdValueGyrYaw;	347	Reading_GyroYaw = AdValueGyrYaw;
Line 349...	Line 348...
349		348
350	Mean_AccPitch = ACC_AMPLIFY * (int32_t)AdValueAccPitch;	349	Mean_AccPitch = ACC_AMPLIFY * (int32_t)AdValueAccPitch;
351	Mean_AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;	350	Mean_AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
-		351	Mean_AccTop = (int32_t)AdValueAccTop;
352	Mean_AccTop = (int32_t)AdValueAccTop;	352	// start ADC (enables internal trigger so that the ISR in analog.c
353	// start ADC	353	// updates the readings once)
354	ADC_Enable();	354	ADC_Enable();
355	//update poti values by rc-signals (why not +127?)	355	//update poti values by rc-signals
356	if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;	356	if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
357	if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;	357	if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
358	if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;	358	if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
359	if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;	359	if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
Line 362...	Line 362...
362	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;	362	if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
363	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;	363	if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
364	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;	364	if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
Line 365...	Line 365...
365		365
366	TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;	366	TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
367	TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;	367	TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
Line 368...	Line 368...
368	}	368	}
369		369
370	/************************************************************************/	370	/************************************************************************/
Line 377...	Line 377...
377	Motor_Rear = 0;	377	Motor_Rear = 0;
378	Motor_Front = 0;	378	Motor_Front = 0;
379	Motor_Right = 0;	379	Motor_Right = 0;
380	Motor_Left = 0;	380	Motor_Left = 0;
381	if(MotorTest[0]) Motor_Front = MotorTest[0];	381	if(MotorTest[0]) Motor_Front = MotorTest[0];
382	if(MotorTest[1]) Motor_Rear = MotorTest[1];	382	if(MotorTest[1]) Motor_Rear = MotorTest[1];
383	if(MotorTest[2]) Motor_Left = MotorTest[2];	383	if(MotorTest[2]) Motor_Left = MotorTest[2];
384	if(MotorTest[3]) Motor_Right = MotorTest[3];	384	if(MotorTest[3]) Motor_Right = MotorTest[3];
385	}	385	}
Line 386...	Line 386...
386		386
387	//DebugOut.Analog[12] = Motor_Front;	387	//DebugOut.Analog[12] = Motor_Front;
Line 682...	Line 682...
682	StickPitch += ExternStickPitch / 8;	682	StickPitch += ExternStickPitch / 8;
683	StickRoll += ExternStickRoll / 8;	683	StickRoll += ExternStickRoll / 8;
684	StickYaw += ExternStickYaw;	684	StickYaw += ExternStickYaw;
Line 685...	Line 685...
685		685
686	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	686	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
687	//+ Analoge Control via serial communication	687	//+ Analog control via serial communication
Line 688...	Line 688...
688	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	688	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
689		689
690	if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128)	690	if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128)
Line 781...	Line 781...
781	// reset averaging for acc and gyro integral as well as gyro integral acc correction	781	// reset averaging for acc and gyro integral as well as gyro integral acc correction
782	MeasurementCounter = 0;	782	MeasurementCounter = 0;
Line 783...	Line 783...
783		783
784	IntegralAccPitch = 0;	784	IntegralAccPitch = 0;
785	IntegralAccRoll = 0;	-
Line 786...	Line 785...
786	IntegralAccZ = 0;	785	IntegralAccRoll = 0;
787		786
Line 788...	Line 787...
788	MeanIntegralPitch = 0;	787	MeanIntegralPitch = 0;
Line 803...	Line 802...
803	tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch);	802	tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch);
804	tmp_long /= 16;	803	tmp_long /= 16;
805	tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll);	804	tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll);
806	tmp_long2 /= 16;	805	tmp_long2 /= 16;
Line 807...	Line 806...
807		806
808	if((MaxStickPitch > 15) \|\| (MaxStickRoll > 15))	807	if((MaxStickPitch > 15) \|\| (MaxStickRoll > 15)) // reduce effect during stick commands
809	{	808	{
810	tmp_long /= 3;	809	tmp_long /= 3;
811	tmp_long2 /= 3;	810	tmp_long2 /= 3;
812	}	811	}
813	if(MaxStickYaw > 25)	812	if(MaxStickYaw > 25) // reduce further is yaw stick is active
814	{	813	{
815	tmp_long /= 3;	814	tmp_long /= 3;
816	tmp_long2 /= 3;	815	tmp_long2 /= 3;
Line 817...	Line 816...
817	}	816	}
818		817
819	#define BALANCE 32	818	#define BALANCE 32
820	// limit correction	819	// limit correction effect
821	if(tmp_long > BALANCE) tmp_long = BALANCE;	820	if(tmp_long > BALANCE) tmp_long = BALANCE;
822	if(tmp_long < -BALANCE) tmp_long =-BALANCE;	821	if(tmp_long < -BALANCE) tmp_long =-BALANCE;
823	if(tmp_long2 > BALANCE) tmp_long2 = BALANCE;	822	if(tmp_long2 > BALANCE) tmp_long2 = BALANCE;
Line 828...	Line 827...
828	}	827	}
829	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	828	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
830	// MeasurementCounter is incremented in the isr of analog.c	829	// MeasurementCounter is incremented in the isr of analog.c
831	if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached	830	if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
832	{	831	{
833	static int cnt = 0;	832	static int16_t cnt = 0;
834	static char last_n_p, last_n_n, last_r_p, last_r_n;	833	static int8_t last_n_p, last_n_n, last_r_p, last_r_n;
835	static long MeanIntegralPitch_old, MeanIntegralRoll_old;	834	static int32_t MeanIntegralPitch_old, MeanIntegralRoll_old;
Line 836...	Line 835...
836		835
837	// if not lopping in any direction (this should be alwais the case,	836	// if not lopping in any direction (this should be alwais the case,
838	// because the Measurement counter is reset to 0 if looping in any direction is active.)	837	// because the Measurement counter is reset to 0 if looping in any direction is active.)
839	if(!Looping_Pitch && !Looping_Roll)	838	if(!Looping_Pitch && !Looping_Roll)
Line 843...	Line 842...
843	MeanIntegralRoll /= BALANCE_NUMBER;	842	MeanIntegralRoll /= BALANCE_NUMBER;
Line 844...	Line 843...
844		843
845	// Calculate mean of the acceleration values	844	// Calculate mean of the acceleration values
846	IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER;	845	IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER;
847	IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER;	-
Line 848...	Line 846...
848	IntegralAccZ = IntegralAccZ / BALANCE_NUMBER;	846	IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER;
849		847
850	// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++	848	// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++
851	// Calculate deviation of the averaged gyro integral and the averaged acceleration integral	849	// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
Line 981...	Line 979...
981	MeanIntegralRoll_old = MeanIntegralRoll;	979	MeanIntegralRoll_old = MeanIntegralRoll;
982	// +++++++++++++++++++++++++++++++++++++++++++++++++++++	980	// +++++++++++++++++++++++++++++++++++++++++++++++++++++
983	// reset variables used for averaging	981	// reset variables used for averaging
984	IntegralAccPitch = 0;	982	IntegralAccPitch = 0;
985	IntegralAccRoll = 0;	983	IntegralAccRoll = 0;
986	IntegralAccZ = 0;	-
987	MeanIntegralPitch = 0;	984	MeanIntegralPitch = 0;
988	MeanIntegralRoll = 0;	985	MeanIntegralRoll = 0;
989	MeasurementCounter = 0;	986	MeasurementCounter = 0;
990	} // end of averaging	987	} // end of averaging
Line 1037...	Line 1034...
1037	{	1034	{
1038	CompassCourse = CompassHeading;	1035	CompassCourse = CompassHeading;
1039	StoreNewCompassCourse = 0;	1036	StoreNewCompassCourse = 0;
1040	}	1037	}
1041	w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter	1038	w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter
1042	w = FCParam.CompassYawEffect - w; // reduce commpass effect with increasing declination	1039	w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination
1043	if(w > 0) // if there is any compass effect (avoid negative compass feedback)	1040	if(w > 0) // if there is any compass effect (avoid negative compass feedback)
1044	{	1041	{
1045	Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;	1042	Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;
1046	}	1043	}
1047	}	1044	}
Line 1127...	Line 1124...
1127		1124
1128	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1125	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1129	// Hight Control	1126	// Hight Control
1130	// The higth control algorithm reduces the thrust but does not increase the thrust.	1127	// The higth control algorithm reduces the thrust but does not increase the thrust.
1131	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1128	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1132	// If hight control is activated and no emergency landing is activre	1129	// If hight control is activated and no emergency landing is active
1133	if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )	1130	if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
1134	{	1131	{
1135	int tmp_int;	1132	int tmp_int;
1136	// if hight control is activated by an rc channel	1133	// if hight control is activated by an rc channel

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/fc.c - Rev 746 → 750