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