Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1078 → Rev 1080

/branches/V0.71h Code Redesign killagreg/fc.c
73,8 → 73,9
#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
230,9 → 231,9
YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
YawGyroDrift = 0;
MKFlags |= MKFLAG_CALIBRATE;
FromNaviCtrl_Value.Kalman_K = -1;
FromNaviCtrl_Value.Kalman_MaxDrift = ParamSet.DriftComp * 16;
FromNaviCtrl_Value.Kalman_MaxFusion = 32;
FCParam.Kalman_K = -1;
FCParam.Kalman_MaxDrift = ParamSet.DriftComp * 16;
FCParam.Kalman_MaxFusion = 32;
}
 
/************************************************************************/
451,7 → 452,7
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,NaviSpeedCompensation.NaviGpsACC);
CHK_POTI(FCParam.NaviSpeedCompensation,ParamSet.NaviSpeedCompensation);
#endif
CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);
Ki = (float) FCParam.I_Factor * FACTOR_I;
889,13 → 890,13
if(!Looping_Nick && !Looping_Roll) // if not lopping in any direction
{
int32_t tmp_long, tmp_long2;
if(FromNaviCtrl_Value.Kalman_K != -1)
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 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
tmp_long = (tmp_long * FCParam.Kalman_K) / (32 * 16);
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
tmp_long2 = (tmp_long2 * FCParam.Kalman_K) / (32 * 16);
 
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
{
908,10 → 909,10
tmp_long2 /= 3;
}
// limit correction effect
if(tmp_long > (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;
if(tmp_long < -(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long =-(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;
if(tmp_long2 > (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;
if(tmp_long2 <-(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 =-(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;
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
{
974,7 → 975,7
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER;
 
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.Kalman_K == -1) )
{
AttitudeCorrectionNick /= 2;
AttitudeCorrectionRoll /= 2;
1000,7 → 1001,7
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralErrorNick) / 4096;
CorrectionNick = 0;
if((labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) || (FromNaviCtrl_Value.Kalman_MaxDrift > 3* 16))
if((labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) || (FCParam.Kalman_MaxDrift > 3* 16))
{
if(IntegralErrorNick > ERROR_LIMIT2)
{
1033,7 → 1034,7
BadCompassHeading = 1000;
}
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;
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;
1041,7 → 1042,7
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralErrorNick) / 4096;
CorrectionRoll = 0;
if((labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT) || (FromNaviCtrl_Value.Kalman_MaxDrift > 3* 16))
if((labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT) || (FCParam.Kalman_MaxDrift > 3 * 16))
{
if(IntegralErrorRoll > ERROR_LIMIT2)
{
1075,7 → 1076,7
}
// correct Gyro Offsets
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;
if(cnt * 16 > FCParam.Kalman_MaxDrift) cnt = FCParam.Kalman_MaxDrift / 16;
if(IntegralErrorRoll > ERROR_LIMIT) AdNeutralRoll += cnt;
if(IntegralErrorRoll < -ERROR_LIMIT) AdNeutralRoll -= cnt;
 
1239,11 → 1240,11
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[17] = FromNaviCtrl_Value.Distance;
//DebugOut.Analog[18] = FromNaviCtrl_Value.OsdBar;
DebugOut.Analog[20] = ServoValue;
DebugOut.Analog[27] = (int16_t)FromNaviCtrl_Value.Kalman_MaxDrift;
DebugOut.Analog[29] = (int16_t)FromNaviCtrl_Value.Kalman_K;
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;
 
/branches/V0.71h Code Redesign killagreg/fc.h
46,10 → 46,13
uint8_t NaviGpsI;
uint8_t NaviGpsD;
uint8_t NaviGpsACC;
uint8_t NaviOperationgRadius;
uint8_t NaviOperatingRadius;
uint8_t NaviWindCorrection;
uint8_t NaviSpeedCompensation;
#endif
int8_t Kalman_K;
int8_t Kalman_MaxDrift;
int8_t Kalman_MaxFusion;
} fc_param_t;
 
extern fc_param_t FCParam;
/branches/V0.71h Code Redesign killagreg/makefile
1,7 → 1,7
#--------------------------------------------------------------------
# MCU name
#MCU = atmega644
MCU = atmega644p
MCU = atmega644
#MCU = atmega644p
F_CPU = 20000000
#-------------------------------------------------------------------
VERSION_MAJOR = 0
15,8 → 15,8
#-------------------------------------------------------------------
#OPTIONS
# Use one of the extensions for a gps solution
#EXT = KILLAGREG
EXT = NAVICTRL
EXT = KILLAGREG
#EXT = NAVICTRL
#EXT = MK3MAG
#-------------------------------------------------------------------
# get SVN revision
/branches/V0.71h Code Redesign killagreg/spi.c
195,27 → 195,11
switch (FromNaviCtrl.Command)
{
case SPI_KALMAN:
FromNaviCtrl_Value.Kalman_K = FromNaviCtrl.Param.Byte[0];
FromNaviCtrl_Value.Kalman_MaxFusion = FromNaviCtrl.Param.Byte[1];
FromNaviCtrl_Value.Kalman_MaxDrift = FromNaviCtrl.Param.Byte[2];
FCParam.Kalman_K = FromNaviCtrl.Param.Byte[0];
FCParam.Kalman_MaxFusion = FromNaviCtrl.Param.Byte[1];
FCParam.Kalman_MaxDrift = FromNaviCtrl.Param.Byte[2];
break;
 
 
case SPI_CMD_OSD_DATA:
FromNaviCtrl_Value.OsdBar = FromNaviCtrl.Param.Byte[0];
FromNaviCtrl_Value.Distance = FromNaviCtrl.Param.Int[1];
break;
 
case SPI_CMD_GPS_POS:
//FromFlightCtrl.Param.Long[0] = GPS_Data.Longitude;
//FromFlightCtrl.Param.Long[1] = GPS_Data.Latitude;
break;
 
case SPI_CMD_GPS_TARGET:
//FromFlightCtrl.Param.Long[0] = GPS_Data.TargetLongitude;
//FromFlightCtrl.Param.Long[1] = GPS_Data.TargetLatitude;
break;
 
default:
break;
}
/branches/V0.71h Code Redesign killagreg/spi.h
106,16 → 106,7
uint8_t Chksum;
} __attribute__((packed)) FromNaviCtrl_t;
 
typedef struct
{
int16_t OsdBar;
int16_t Distance;
int8_t Kalman_K;
int8_t Kalman_MaxDrift;
int8_t Kalman_MaxFusion;
} __attribute__((packed)) FromNaviCtrl_Value_t;
 
 
typedef struct
{
uint8_t Major;
127,7 → 118,6
 
extern ToNaviCtrl_t ToNaviCtrl;
extern FromNaviCtrl_t FromNaviCtrl;
extern FromNaviCtrl_Value_t FromNaviCtrl_Value;
 
void SPI_MasterInit(void);
void SPI_StartTransmitPacket(void);