Subversion Repositories FlightCtrl

Rev

Rev 942 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 942 Rev 950
Line 14... Line 14...
14
        GPS_FLIGHT_MODE_FREE,
14
        GPS_FLIGHT_MODE_FREE,
15
        GPS_FLIGHT_MODE_AID,
15
        GPS_FLIGHT_MODE_AID,
16
        GPS_FLIGHT_MODE_HOME,
16
        GPS_FLIGHT_MODE_HOME,
17
} FlightMode_t;
17
} FlightMode_t;
Line 18... Line 18...
18
 
18
 
19
#define GPS_POSINTEGRAL_LIMIT 3000
19
#define GPS_POSINTEGRAL_LIMIT 32000
20
#define GPS_STICK_LIMIT         500             // limit of gps stick control to avoid critical flight attitudes
-
 
21
#define GPS_I_LIMIT             156             // limit for the position error integral
20
#define GPS_STICK_LIMIT         45              // limit of gps stick control to avoid critical flight attitudes
22
#define GPS_D_LIMIT                     625
-
 
Line 23... Line 21...
23
#define GPS_P_LIMIT                     312
21
#define GPS_P_LIMIT                     25
24
 
22
 
25
 
23
 
Line 206... Line 204...
206
                        // reset error integral
204
                        // reset error integral
207
                        GPSPosDevIntegral_North = 0;
205
                        GPSPosDevIntegral_North = 0;
208
                        GPSPosDevIntegral_East = 0;
206
                        GPSPosDevIntegral_East = 0;
209
                }
207
                }
Line 210... Line -...
210
 
-
 
211
                GPSPosDevIntegral_North += GPSPosDev_North;
-
 
212
                GPSPosDevIntegral_East  += GPSPosDev_East;
-
 
213
                GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT);
-
 
214
 
208
 
Line 215... Line 209...
215
                //Calculate PID-components of the controller
209
                //Calculate PID-components of the controller
216
 
210
 
217
                // D-Part
211
                // D-Part
218
                D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/256;
-
 
Line 219... Line 212...
219
                D_East =  ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/256;
212
                D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/512;
220
                GPS_LimitXY(&D_North, &D_East, GPS_D_LIMIT);
213
                D_East =  ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/512;
221
 
214
 
222
                // P-Part
-
 
Line 223... Line 215...
223
                P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/1024;
215
                // P-Part
224
                P_East =  ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/1024;
216
                P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/2048;
225
                GPS_LimitXY(&P_North, &P_East, GPS_P_LIMIT);
217
                P_East =  ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/2048;
226
 
-
 
Line -... Line 218...
-
 
218
 
227
                // I-Part
219
                // I-Part
228
                I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/8192;
220
                I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/8192;
229
                I_East =  ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/8192;
221
                I_East =  ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/8192;
-
 
222
 
-
 
223
 
-
 
224
                // combine P & I
-
 
225
                PID_North = P_North + I_North;
-
 
226
                PID_East  = P_East  + I_East;
-
 
227
                if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT))
-
 
228
                {
-
 
229
                        GPSPosDevIntegral_North += GPSPosDev_North/16;
-
 
230
                        GPSPosDevIntegral_East  += GPSPosDev_East/16;
-
 
231
                        GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_North, GPS_POSINTEGRAL_LIMIT);
-
 
232
                }
Line 230... Line 233...
230
                GPS_LimitXY(&I_North, &I_East, GPS_I_LIMIT);
233
 
231
 
234
                // combine PI- and D-Part
232
                // combine P & I & D-Part
235
                PID_North += D_North;
Line 251... Line 254...
251
                // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
254
                // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
252
                // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll.
255
                // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll.
Line 253... Line 256...
253
 
256
 
254
                coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);
257
                coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);
255
                sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);
258
                sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);
256
                PID_Nick =   (PID_Nick + (coscompass * PID_North + sincompass * PID_East) / 8192)/2;
259
                PID_Nick =   (coscompass * PID_North + sincompass * PID_East) / 8192;
Line 257... Line 260...
257
                PID_Roll  =  (PID_Roll + (sincompass * PID_North - coscompass * PID_East) / 8192)/2;
260
                PID_Roll  =  (sincompass * PID_North - coscompass * PID_East) / 8192;
258
 
261