Subversion Repositories FlightCtrl

Rev

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

Rev 764 Rev 767
Line 8... Line 8...
8
 
8
 
9
#define TSK_IDLE                0
9
#define TSK_IDLE                0
10
#define TSK_HOLD                1
10
#define TSK_HOLD                1
Line 11... Line 11...
11
#define TSK_HOME                2
11
#define TSK_HOME                2
12
 
12
 
13
#define GPS_STICK_SENSE 12
13
#define GPS_STICK_SENSE         20              // must be at least in a range where 90% of the trimming does not switch of the GPS function
14
#define GPS_STICK_LIMIT 45
14
#define GPS_STICK_LIMIT         45              // limit of gps stick control to avoid critical flight attitudes
Line 15... Line 15...
15
#define GPS_D_LIMIT_DIST 500 // for position deviations grater than 500 cm (5m) the D-Part of the PD-Controller is limited
15
#define GPS_D_LIMIT_DIST        2000    // for position deviations grater than 500 cm (20m) the D-Part of the PD-Controller is limited
16
#define GPS_D_LIMIT 50 // PD-Controntroller D-Limit.
16
#define GPS_D_LIMIT                     50              // PD-Controntroller D-Limit.
Line 17... Line 17...
17
 
17
 
Line 73... Line 73...
73
                GPSPosDev_East  = GPSInfo.utmeast  - TargetPos->Easting;
73
                GPSPosDev_East  = GPSInfo.utmeast  - TargetPos->Easting;
Line 74... Line 74...
74
 
74
 
75
                DebugOut.Analog[12] = GPSPosDev_North;
75
                DebugOut.Analog[12] = GPSPosDev_North;
Line -... Line 76...
-
 
76
                DebugOut.Analog[13] = GPSPosDev_East;
-
 
77
 
-
 
78
                //DebugOut.Analog[12] = GPSInfo.velnorth;
76
                DebugOut.Analog[13] = GPSPosDev_East;
79
                //DebugOut.Analog[13] = GPSInfo.veleast;
77
 
80
 
78
                //Calculate PD-components of the controller (negative sign for compensation)
81
                //Calculate PD-components of the controller (negative sign for compensation)
79
                P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
82
                P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
80
                D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
83
                D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
Line 210... Line 213...
210
                                                // if sticks are centered (no manual control)
213
                                                // if sticks are centered (no manual control)
211
                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
214
                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
212
                                                {
215
                                                {
213
                                                        GPS_PDController(&HomePosition);
216
                                                        GPS_PDController(&HomePosition);
214
                                                }
217
                                                }
-
 
218
                                                else // manual control
-
 
219
                                                {
-
 
220
                                                        GPS_Neutral();
-
 
221
                                                }
215
                                        }
222
                                        }
216
                                        else // bad home position
223
                                        else // bad home position
217
                                        {
224
                                        {
218
                                                BeepTime = 50; // signal invalid home position
225
                                                BeepTime = 50; // signal invalid home position
219
                                                // try to hold at least the position as a fallback option
226
                                                // try to hold at least the position as a fallback option
220
                                                // if sticks are centered (no manual control)
227
                                                // if sticks are centered (no manual control)
221
                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
228
                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
222
                                                {
229
                                                {
223
                                                        GPS_PDController(&HoldPosition);
230
                                                        GPS_PDController(&HoldPosition);
224
                                                }
231
                                                }
-
 
232
                                                else // manual control or no rteference position
-
 
233
                                                {
-
 
234
                                                        GPS_Neutral();
-
 
235
                                                }
225
                                        }
236
                                        }
226
                                        break; // eof TSK_HOME
237
                                        break; // eof TSK_HOME
227
                                default: // unhandled task
238
                                default: // unhandled task
228
                                        GPS_Neutral();
239
                                        GPS_Neutral();
229
                                        break; // eof default
240
                                        break; // eof default
Line 233... Line 244...
233
                else // no 3D-SATFIX
244
                else // no 3D-SATFIX
234
                {       // disable gps control
245
                {       // disable gps control
235
                        GPS_Neutral();
246
                        GPS_Neutral();
236
                        if(GPS_Task != TSK_IDLE)
247
                        if(GPS_Task != TSK_IDLE)
237
                        {
248
                        {
238
                                satbeep = 2048 - (int16_t)GPSInfo.satnum * 256; // is zero at 8 sats
249
                                satbeep = 1800 - (int16_t)GPSInfo.satnum * 225; // is zero at 8 sats
239
                                if (satbeep < 0) satbeep = 0;
250
                                if (satbeep < 0) satbeep = 0;
240
                                BeepTime = 50 + (uint16_t)satbeep;
251
                                BeepTime = 50 + (uint16_t)satbeep; // max 1850 * 0.1 ms =
241
                        }
252
                        }
242
                }
253
                }
243
                // set current data as processed to avoid further calculations on the same gps data
254
                // set current data as processed to avoid further calculations on the same gps data
244
                GPSInfo.status = PROCESSED;
255
                GPSInfo.status = PROCESSED;
245
                break;
256
                break;