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; |
|