WebSVN - FlightCtrl - Diff - Rev 1078 and 1080 - /branches/V0.71h Code Redesign killagreg/fc.c



#ifdef USE_MK3MAG
#include "mk3mag.h"
#include "gps.h"
#endif
#include "led.h"
#ifdef USE_NAVICTRL
#include "spi.h"
#endif
// gyro readings
int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
// gyro neutral readings
int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
int16_t StartNeutralRoll = 0, StartNeutralNick = 0;

    GPS_Nick = 0;
    GPS_Roll = 0;
    YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
    YawGyroDrift = 0;
    MKFlags |= MKFLAG_CALIBRATE;
        FCParam.Kalman_K = -1;
        FCParam.Kalman_MaxDrift = ParamSet.DriftComp * 16;
        FCParam.Kalman_MaxFusion = 32;
}

 
/************************************************************************/
/*  Averaging Measurement Readings                                      */

                CHK_POTI(FCParam.NaviGpsI,ParamSet.NaviGpsI);
                CHK_POTI(FCParam.NaviGpsD,ParamSet.NaviGpsD);
                CHK_POTI(FCParam.NaviGpsACC,ParamSet.NaviGpsACC);
                CHK_POTI_MM(FCParam.NaviOperatingRadius,ParamSet.NaviOperatingRadius,10, 255);
                CHK_POTI(FCParam.NaviWindCorrection,ParamSet.NaviWindCorrection);
                CHK_POTI(FCParam.NaviSpeedCompensation,ParamSet.NaviSpeedCompensation);
                #endif
                CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);
                Ki = (float) FCParam.I_Factor * FACTOR_I;
        }
}

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(!Looping_Nick && !Looping_Roll) // if not lopping in any direction
        {
                int32_t tmp_long, tmp_long2;
                if(FCParam.Kalman_K != -1)
                {
                        // determine the deviation of gyro integral from averaged acceleration sensor
                        tmp_long   = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
                        tmp_long   = (tmp_long * FCParam.Kalman_K) / (32 * 16);
                        tmp_long2  = (int32_t)(IntegralRoll   / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);

                        tmp_long2  = (tmp_long2 * FCParam.Kalman_K) / (32 * 16);
 
                        if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
                        {

                        {
                                tmp_long  /= 3;
                                tmp_long2 /= 3;
                        }
                        // limit correction effect
                        if(tmp_long >  (int32_t)FCParam.Kalman_MaxFusion)  tmp_long  = (int32_t)FCParam.Kalman_MaxFusion;
                        if(tmp_long < -(int32_t)FCParam.Kalman_MaxFusion)  tmp_long  =-(int32_t)FCParam.Kalman_MaxFusion;
                        if(tmp_long2 > (int32_t)FCParam.Kalman_MaxFusion)  tmp_long2 = (int32_t)FCParam.Kalman_MaxFusion;
                        if(tmp_long2 <-(int32_t)FCParam.Kalman_MaxFusion)  tmp_long2 =-(int32_t)FCParam.Kalman_MaxFusion;
                }
                else
                {
                        // determine the deviation of gyro integral from averaged acceleration sensor
                        tmp_long   =  (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);

                        // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
                        IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
                        CorrectionRoll  = IntegralErrorRoll / ParamSet.GyroAccTrim;
                        AttitudeCorrectionRoll  = CorrectionRoll  / BALANCE_NUMBER;

 
                        if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.Kalman_K == -1) )
                        {
                                AttitudeCorrectionNick /= 2;
                                AttitudeCorrectionRoll /= 2;

                        #define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
                        #define MOVEMENT_LIMIT 20000
        // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
                        cnt = 1;// + labs(IntegralErrorNick) / 4096;
                        CorrectionNick = 0;
                        if((labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) || (FCParam.Kalman_MaxDrift > 3* 16))
                        {
                                if(IntegralErrorNick >  ERROR_LIMIT2)
                                {
                                        if(last_n_p)
                                        {

                        {
                                cnt = 0;
                                BadCompassHeading = 1000;
                        }
                        if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
                        if(cnt * 16 > FCParam.Kalman_MaxDrift) cnt = FCParam.Kalman_MaxDrift / 16;
                        // correct Gyro Offsets
                        if(IntegralErrorNick >  ERROR_LIMIT)   AdNeutralNick += cnt;
                        if(IntegralErrorNick < -ERROR_LIMIT)   AdNeutralNick -= cnt;

 
        // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
                        cnt = 1;// + labs(IntegralErrorNick) / 4096;
                        CorrectionRoll = 0;
                        if((labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT) || (FCParam.Kalman_MaxDrift > 3 * 16))
                        {
                                if(IntegralErrorRoll >  ERROR_LIMIT2)
                                {
                                        if(last_r_p)

                                cnt = 0;
                                BadCompassHeading = 1000;
                        }
                        // correct Gyro Offsets
                        if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
                        if(cnt * 16 > FCParam.Kalman_MaxDrift) cnt = FCParam.Kalman_MaxDrift / 16;
                        if(IntegralErrorRoll >  ERROR_LIMIT)   AdNeutralRoll += cnt;
                        if(IntegralErrorRoll < -ERROR_LIMIT)   AdNeutralRoll -= cnt;

 
                }

                DebugOut.Analog[8]  = CompassHeading;
                DebugOut.Analog[9]  = UBat;
                DebugOut.Analog[10] = RC_Quality;
                DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
                //DebugOut.Analog[16] = Mean_AccTop;
                //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
                //DebugOut.Analog[18] = FromNaviCtrl_Value.OsdBar;
                DebugOut.Analog[20] = ServoValue;
                DebugOut.Analog[27] = (int16_t)FCParam.Kalman_MaxDrift;
                DebugOut.Analog[29] = (int16_t)FCParam.Kalman_K;
                DebugOut.Analog[30] = GPS_Nick;
                DebugOut.Analog[31] = GPS_Roll;

 

Rev 1078	Rev 1080
Line 71...	Line 71...
71	#ifdef USE_MK3MAG	71	#ifdef USE_MK3MAG
72	#include "mk3mag.h"	72	#include "mk3mag.h"
73	#include "gps.h"	73	#include "gps.h"
74	#endif	74	#endif
75	#include "led.h"	75	#include "led.h"
-		76	#ifdef USE_NAVICTRL
76	#include "spi.h"	77	#include "spi.h"
77		78	#endif
78	// gyro readings	79	// gyro readings
79	int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;	80	int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
80	// gyro neutral readings	81	// gyro neutral readings
81	int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;	82	int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
82	int16_t StartNeutralRoll = 0, StartNeutralNick = 0;	83	int16_t StartNeutralRoll = 0, StartNeutralNick = 0;
Line 228...	Line 229...
228	GPS_Nick = 0;	229	GPS_Nick = 0;
229	GPS_Roll = 0;	230	GPS_Roll = 0;
230	YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;	231	YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
231	YawGyroDrift = 0;	232	YawGyroDrift = 0;
232	MKFlags \|= MKFLAG_CALIBRATE;	233	MKFlags \|= MKFLAG_CALIBRATE;
233	FromNaviCtrl_Value.Kalman_K = -1;	234	FCParam.Kalman_K = -1;
234	FromNaviCtrl_Value.Kalman_MaxDrift = ParamSet.DriftComp * 16;	235	FCParam.Kalman_MaxDrift = ParamSet.DriftComp * 16;
235	FromNaviCtrl_Value.Kalman_MaxFusion = 32;	236	FCParam.Kalman_MaxFusion = 32;
236	}	237	}
Line 237...	Line 238...
237		238
238	/************************************************************************/	239	/************************************************************************/
239	/* Averaging Measurement Readings */	240	/* Averaging Measurement Readings */
Line 449...	Line 450...
449	CHK_POTI(FCParam.NaviGpsI,ParamSet.NaviGpsI);	450	CHK_POTI(FCParam.NaviGpsI,ParamSet.NaviGpsI);
450	CHK_POTI(FCParam.NaviGpsD,ParamSet.NaviGpsD);	451	CHK_POTI(FCParam.NaviGpsD,ParamSet.NaviGpsD);
451	CHK_POTI(FCParam.NaviGpsACC,ParamSet.NaviGpsACC);	452	CHK_POTI(FCParam.NaviGpsACC,ParamSet.NaviGpsACC);
452	CHK_POTI_MM(FCParam.NaviOperatingRadius,ParamSet.NaviOperatingRadius,10, 255);	453	CHK_POTI_MM(FCParam.NaviOperatingRadius,ParamSet.NaviOperatingRadius,10, 255);
453	CHK_POTI(FCParam.NaviWindCorrection,ParamSet.NaviWindCorrection);	454	CHK_POTI(FCParam.NaviWindCorrection,ParamSet.NaviWindCorrection);
454	CHK_POTI(FCParam.NaviSpeedCompensation,NaviSpeedCompensation.NaviGpsACC);	455	CHK_POTI(FCParam.NaviSpeedCompensation,ParamSet.NaviSpeedCompensation);
455	#endif	456	#endif
456	CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);	457	CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);
457	Ki = (float) FCParam.I_Factor * FACTOR_I;	458	Ki = (float) FCParam.I_Factor * FACTOR_I;
458	}	459	}
459	}	460	}
Line 887...	Line 888...
887		888
888	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	889	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
889	if(!Looping_Nick && !Looping_Roll) // if not lopping in any direction	890	if(!Looping_Nick && !Looping_Roll) // if not lopping in any direction
890	{	891	{
891	int32_t tmp_long, tmp_long2;	892	int32_t tmp_long, tmp_long2;
892	if(FromNaviCtrl_Value.Kalman_K != -1)	893	if(FCParam.Kalman_K != -1)
893	{	894	{
894	// determine the deviation of gyro integral from averaged acceleration sensor	895	// determine the deviation of gyro integral from averaged acceleration sensor
895	tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);	896	tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
896	tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);	897	tmp_long = (tmp_long * FCParam.Kalman_K) / (32 * 16);
897	tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);	898	tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
Line 898...	Line 899...
898	tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);	899	tmp_long2 = (tmp_long2 * FCParam.Kalman_K) / (32 * 16);
899		900
900	if((MaxStickNick > 64) \|\| (MaxStickRoll > 64)) // reduce effect during stick commands	901	if((MaxStickNick > 64) \|\| (MaxStickRoll > 64)) // reduce effect during stick commands
901	{	902	{
Line 906...	Line 907...
906	{	907	{
907	tmp_long /= 3;	908	tmp_long /= 3;
908	tmp_long2 /= 3;	909	tmp_long2 /= 3;
909	}	910	}
910	// limit correction effect	911	// limit correction effect
911	if(tmp_long > (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;	912	if(tmp_long > (int32_t)FCParam.Kalman_MaxFusion) tmp_long = (int32_t)FCParam.Kalman_MaxFusion;
912	if(tmp_long < -(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long =-(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;	913	if(tmp_long < -(int32_t)FCParam.Kalman_MaxFusion) tmp_long =-(int32_t)FCParam.Kalman_MaxFusion;
913	if(tmp_long2 > (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;	914	if(tmp_long2 > (int32_t)FCParam.Kalman_MaxFusion) tmp_long2 = (int32_t)FCParam.Kalman_MaxFusion;
914	if(tmp_long2 <-(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 =-(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;	915	if(tmp_long2 <-(int32_t)FCParam.Kalman_MaxFusion) tmp_long2 =-(int32_t)FCParam.Kalman_MaxFusion;
915	}	916	}
916	else	917	else
917	{	918	{
918	// determine the deviation of gyro integral from averaged acceleration sensor	919	// determine the deviation of gyro integral from averaged acceleration sensor
919	tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);	920	tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
Line 972...	Line 973...
972	// Calculate deviation of the averaged gyro integral and the averaged acceleration integral	973	// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
973	IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);	974	IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
974	CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;	975	CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;
975	AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER;	976	AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER;
Line 976...	Line 977...
976		977
977	if(((MaxStickNick > 64) \|\| (MaxStickRoll > 64) \|\| (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))	978	if(((MaxStickNick > 64) \|\| (MaxStickRoll > 64) \|\| (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.Kalman_K == -1) )
978	{	979	{
979	AttitudeCorrectionNick /= 2;	980	AttitudeCorrectionNick /= 2;
980	AttitudeCorrectionRoll /= 2;	981	AttitudeCorrectionRoll /= 2;
Line 998...	Line 999...
998	#define ERROR_LIMIT2 (BALANCE_NUMBER * 16)	999	#define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
999	#define MOVEMENT_LIMIT 20000	1000	#define MOVEMENT_LIMIT 20000
1000	// Nick +++++++++++++++++++++++++++++++++++++++++++++++++	1001	// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
1001	cnt = 1;// + labs(IntegralErrorNick) / 4096;	1002	cnt = 1;// + labs(IntegralErrorNick) / 4096;
1002	CorrectionNick = 0;	1003	CorrectionNick = 0;
1003	if((labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) \|\| (FromNaviCtrl_Value.Kalman_MaxDrift > 3* 16))	1004	if((labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) \|\| (FCParam.Kalman_MaxDrift > 3* 16))
1004	{	1005	{
1005	if(IntegralErrorNick > ERROR_LIMIT2)	1006	if(IntegralErrorNick > ERROR_LIMIT2)
1006	{	1007	{
1007	if(last_n_p)	1008	if(last_n_p)
1008	{	1009	{
Line 1031...	Line 1032...
1031	{	1032	{
1032	cnt = 0;	1033	cnt = 0;
1033	BadCompassHeading = 1000;	1034	BadCompassHeading = 1000;
1034	}	1035	}
1035	if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;	1036	if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
1036	if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;	1037	if(cnt * 16 > FCParam.Kalman_MaxDrift) cnt = FCParam.Kalman_MaxDrift / 16;
1037	// correct Gyro Offsets	1038	// correct Gyro Offsets
1038	if(IntegralErrorNick > ERROR_LIMIT) AdNeutralNick += cnt;	1039	if(IntegralErrorNick > ERROR_LIMIT) AdNeutralNick += cnt;
1039	if(IntegralErrorNick < -ERROR_LIMIT) AdNeutralNick -= cnt;	1040	if(IntegralErrorNick < -ERROR_LIMIT) AdNeutralNick -= cnt;
Line 1040...	Line 1041...
1040		1041
1041	// Roll +++++++++++++++++++++++++++++++++++++++++++++++++	1042	// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
1042	cnt = 1;// + labs(IntegralErrorNick) / 4096;	1043	cnt = 1;// + labs(IntegralErrorNick) / 4096;
1043	CorrectionRoll = 0;	1044	CorrectionRoll = 0;
1044	if((labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT) \|\| (FromNaviCtrl_Value.Kalman_MaxDrift > 3* 16))	1045	if((labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT) \|\| (FCParam.Kalman_MaxDrift > 3 * 16))
1045	{	1046	{
1046	if(IntegralErrorRoll > ERROR_LIMIT2)	1047	if(IntegralErrorRoll > ERROR_LIMIT2)
1047	{	1048	{
1048	if(last_r_p)	1049	if(last_r_p)
Line 1073...	Line 1074...
1073	cnt = 0;	1074	cnt = 0;
1074	BadCompassHeading = 1000;	1075	BadCompassHeading = 1000;
1075	}	1076	}
1076	// correct Gyro Offsets	1077	// correct Gyro Offsets
1077	if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;	1078	if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
1078	if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;	1079	if(cnt * 16 > FCParam.Kalman_MaxDrift) cnt = FCParam.Kalman_MaxDrift / 16;
1079	if(IntegralErrorRoll > ERROR_LIMIT) AdNeutralRoll += cnt;	1080	if(IntegralErrorRoll > ERROR_LIMIT) AdNeutralRoll += cnt;
1080	if(IntegralErrorRoll < -ERROR_LIMIT) AdNeutralRoll -= cnt;	1081	if(IntegralErrorRoll < -ERROR_LIMIT) AdNeutralRoll -= cnt;
Line 1081...	Line 1082...
1081		1082
1082	}	1083	}
Line 1237...	Line 1238...
1237	DebugOut.Analog[8] = CompassHeading;	1238	DebugOut.Analog[8] = CompassHeading;
1238	DebugOut.Analog[9] = UBat;	1239	DebugOut.Analog[9] = UBat;
1239	DebugOut.Analog[10] = RC_Quality;	1240	DebugOut.Analog[10] = RC_Quality;
1240	DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR;	1241	DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
1241	//DebugOut.Analog[16] = Mean_AccTop;	1242	//DebugOut.Analog[16] = Mean_AccTop;
1242	DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;	1243	//DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
1243	DebugOut.Analog[18] = FromNaviCtrl_Value.OsdBar;	1244	//DebugOut.Analog[18] = FromNaviCtrl_Value.OsdBar;
1244	DebugOut.Analog[20] = ServoValue;	1245	DebugOut.Analog[20] = ServoValue;
1245	DebugOut.Analog[27] = (int16_t)FromNaviCtrl_Value.Kalman_MaxDrift;	1246	DebugOut.Analog[27] = (int16_t)FCParam.Kalman_MaxDrift;
1246	DebugOut.Analog[29] = (int16_t)FromNaviCtrl_Value.Kalman_K;	1247	DebugOut.Analog[29] = (int16_t)FCParam.Kalman_K;
1247	DebugOut.Analog[30] = GPS_Nick;	1248	DebugOut.Analog[30] = GPS_Nick;
1248	DebugOut.Analog[31] = GPS_Roll;	1249	DebugOut.Analog[31] = GPS_Roll;
Line 1249...	Line 1250...
1249		1250

Subversion Repositories FlightCtrl

(root)/branches/V0.71h Code Redesign killagreg/fc.c @ 1358 - Rev 1078 → 1080