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