Subversion Repositories FlightCtrl

Rev

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

Rev 830 Rev 844
Line 10... Line 10...
10
 
10
 
11
#define TSK_IDLE                0
11
#define TSK_IDLE                0
12
#define TSK_HOLD                1
12
#define TSK_HOLD                1
Line 13... Line 13...
13
#define TSK_HOME                2
13
#define TSK_HOME                2
14
 
14
 
15
#define GPS_STICK_SENSE         20              // must be at least in a range where 90% of the trimming does not switch of the GPS function
15
#define GPS_STICK_SENSE         15              // must be at least in a range where 90% of the trimming does not switch of the GPS function
16
#define GPS_STICK_LIMIT         45              // limit of gps stick control to avoid critical flight attitudes
16
#define GPS_STICK_LIMIT         35              // limit of gps stick control to avoid critical flight attitudes
Line 17... Line 17...
17
#define GPS_POSDEV_INTEGRAL_LIMIT  32000  // limit for the position error integral
17
#define GPS_POSDEV_INTEGRAL_LIMIT  32000  // limit for the position error integral
18
#define MAX_VELOCITY            300     // max ground speed in cm/s during position control
18
#define MAX_VELOCITY            700     // max ground speed in cm/s during position control
Line 19... Line 19...
19
 
19
 
20
 
20
 
Line 160... Line 160...
160
                }
160
                }
Line 161... Line 161...
161
 
161
 
Line 162... Line 162...
162
                //Calculate PID-components of the controller (negative sign for compensation)
162
                //Calculate PID-components of the controller (negative sign for compensation)
163
 
163
 
164
                // P-Part
164
                // P-Part
Line 165... Line 165...
165
                P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
165
                P_North = -((int32_t)GPS_P_Factor * GPSPosDev_North)/2048;
166
                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
166
                P_East =  -((int32_t)GPS_P_Factor * GPSPosDev_East)/2048;
167
 
167
 
Line 168... Line 168...
168
                // I-Part
168
                // I-Part
169
                I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192;
169
                I_North = -((int32_t)GPS_I_Factor * GPSPosDevIntegral_North)/8192;
170
                I_East =  -(GPS_I_Factor * GPSPosDevIntegral_East)/8192;
170
                I_East =  -((int32_t)GPS_I_Factor * GPSPosDevIntegral_East)/8192;
Line 171... Line 171...
171
 
171
 
172
                // combine P- & I-Part
172
                // combine P- & I-Part
173
                PID_North = P_North + I_North;
173
                PID_North = P_North + I_North;
174
                PID_East  = P_East  + I_East;
174
                PID_East  = P_East  + I_East;
175
 
175
 
176
                //limit PI-Part to limit the max velocity
176
                //limit PI-Part to limit the max velocity
177
                temp1 = (GPS_D_Factor * MAX_VELOCITY)/512; // the PI-Part limit
177
                temp1 = ((int32_t)GPS_D_Factor * MAX_VELOCITY)/512; // the PI-Part limit
178
                temp = (int32_t)c_sqrt(PID_North*PID_North + PID_East*PID_East); // the current PI-Part
178
                temp = (int32_t)c_sqrt(PID_North*PID_North + PID_East*PID_East); // the current PI-Part
179
                if(temp > temp1) // P-Part limit is reached
179
                if(temp > temp1) // P-Part limit is reached
180
                {
180
                {
181
                        // normalize P-part components to the P-Part limit
181
                        // normalize P-part components to the P-Part limit
182
                        PID_North  = (PID_North  * temp1)/temp;
182
                        PID_North  = (PID_North  * temp1)/temp;
183
                        PID_East   = (PID_East * temp1)/temp;
183
                        PID_East   = (PID_East * temp1)  /temp;
Line 192... Line 192...
192
                        if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT;
192
                        if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT;
193
                        else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT;
193
                        else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT;
194
                }
194
                }
Line 195... Line 195...
195
 
195
 
196
                // D-Part
196
                // D-Part
197
                D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
197
                D_North = -((int32_t)GPS_D_Factor * GPSInfo.velnorth)/512;
Line 198... Line 198...
198
                D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
198
                D_East =  -((int32_t)GPS_D_Factor * GPSInfo.veleast)/512;
199
 
199
 
200
 
200