Rev 2057 | Rev 2074 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2057 | Rev 2058 | ||
---|---|---|---|
Line 18... | Line 18... | ||
18 | GPS_FLIGHT_MODE_AID, |
18 | GPS_FLIGHT_MODE_AID, |
19 | GPS_FLIGHT_MODE_HOME, |
19 | GPS_FLIGHT_MODE_HOME, |
20 | } FlightMode_t; |
20 | } FlightMode_t; |
Line 21... | Line 21... | ||
21 | 21 | ||
22 | #define GPS_POSINTEGRAL_LIMIT 32000 |
22 | #define GPS_POSINTEGRAL_LIMIT 32000 |
23 | #define LOG_NAVI_STICK_GAIN 2 |
23 | #define LOG_NAVI_STICK_GAIN 3 |
Line 24... | Line 24... | ||
24 | #define GPS_P_LIMIT 100 |
24 | #define GPS_P_LIMIT 100 |
25 | 25 | ||
26 | typedef struct { |
26 | typedef struct { |
Line 34... | Line 34... | ||
34 | GPS_Pos_t holdPosition = { 0, 0, 0, INVALID }; |
34 | GPS_Pos_t holdPosition = { 0, 0, 0, INVALID }; |
35 | // GPS coordinates for home position |
35 | // GPS coordinates for home position |
36 | GPS_Pos_t homePosition = { 0, 0, 0, INVALID }; |
36 | GPS_Pos_t homePosition = { 0, 0, 0, INVALID }; |
37 | // the current flight mode |
37 | // the current flight mode |
38 | FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF; |
38 | FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF; |
- | 39 | int16_t naviSticks[2] = {0,0}; |
|
Line 39... | Line 40... | ||
39 | 40 | ||
40 | // --------------------------------------------------------------------------------- |
41 | // --------------------------------------------------------------------------------- |
41 | void navi_updateFlightMode(void) { |
42 | void navi_updateFlightMode(void) { |
Line 42... | Line 43... | ||
42 | static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF; |
43 | static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF; |
43 | 44 | ||
44 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD || (MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
45 | if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
45 | flightMode = GPS_FLIGHT_MODE_HOME; |
46 | flightMode = GPS_FLIGHT_MODE_FREE; |
46 | } else { |
47 | } else { |
47 | if (dynamicParams.naviMode < 50) |
48 | if (dynamicParams.naviMode < 50) |
48 | flightMode = GPS_FLIGHT_MODE_FREE; |
49 | flightMode = GPS_FLIGHT_MODE_FREE; |
Line 122... | Line 123... | ||
122 | pGPSPos->status = INVALID; |
123 | pGPSPos->status = INVALID; |
123 | } |
124 | } |
124 | return 1; |
125 | return 1; |
125 | } |
126 | } |
Line -... | Line 127... | ||
- | 127 | ||
- | 128 | void navi_setNeutral(void) { |
|
- | 129 | naviSticks[CONTROL_PITCH] = naviSticks[CONTROL_ROLL] = 0; |
|
- | 130 | } |
|
126 | 131 | ||
127 | // calculates the GPS control stick values from the deviation to target position |
132 | // calculates the GPS control stick values from the deviation to target position |
128 | // if the pointer to the target positin is NULL or is the target position invalid |
133 | // if the pointer to the target positin is NULL or is the target position invalid |
129 | // then the P part of the controller is deactivated. |
134 | // then the P part of the controller is deactivated. |
130 | void navi_PIDController(GPS_Pos_t *pTargetPos, int16_t *PRTY) { |
135 | void navi_PIDController(GPS_Pos_t *pTargetPos) { |
131 | static int32_t PID_Nick, PID_Roll; |
136 | static int32_t PID_Pitch, PID_Roll; |
132 | int32_t coscompass, sincompass; |
137 | int32_t coscompass, sincompass; |
133 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
138 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
134 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
139 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
135 | int32_t PID_North = 0, PID_East = 0; |
140 | int32_t PID_North = 0, PID_East = 0; |
Line 176... | Line 181... | ||
176 | GPSPosDevIntegral_North = 0; |
181 | GPSPosDevIntegral_North = 0; |
177 | GPSPosDevIntegral_East = 0; |
182 | GPSPosDevIntegral_East = 0; |
178 | } |
183 | } |
Line 179... | Line 184... | ||
179 | 184 | ||
180 | //Calculate PID-components of the controller |
- | |
181 | 185 | //Calculate PID-components of the controller |
|
182 | // D-Part |
186 | // D-Part |
183 | D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) >> 9; |
187 | D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) >> 9; |
Line 184... | Line 188... | ||
184 | D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) >> 9; |
188 | D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) >> 9; |
Line 192... | Line 196... | ||
192 | I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) >> 13; |
196 | I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) >> 13; |
Line 193... | Line 197... | ||
193 | 197 | ||
194 | // combine P & I |
198 | // combine P & I |
195 | PID_North = P_North + I_North; |
199 | PID_North = P_North + I_North; |
- | 200 | PID_East = P_East + I_East; |
|
196 | PID_East = P_East + I_East; |
201 | |
197 | if (!navi_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) { |
202 | if (!navi_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) { |
198 | // within limit |
203 | // within limit |
199 | GPSPosDevIntegral_North += GPSPosDev_North >> 4; |
204 | GPSPosDevIntegral_North += GPSPosDev_North >> 4; |
200 | GPSPosDevIntegral_East += GPSPosDev_East >> 4; |
205 | GPSPosDevIntegral_East += GPSPosDev_East >> 4; |
Line 224... | Line 229... | ||
224 | // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
229 | // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
Line 225... | Line 230... | ||
225 | 230 | ||
226 | coscompass = -cos_360(heading / GYRO_DEG_FACTOR_YAW); |
231 | coscompass = -cos_360(heading / GYRO_DEG_FACTOR_YAW); |
Line 227... | Line 232... | ||
227 | sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW); |
232 | sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW); |
228 | 233 | ||
Line 229... | Line 234... | ||
229 | PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
234 | PID_Pitch = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
230 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
235 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
Line 231... | Line 236... | ||
231 | 236 | ||
232 | // limit resulting GPS control vector |
237 | // limit resulting GPS control vector |
Line -... | Line 238... | ||
- | 238 | navi_limitXY(&PID_Pitch, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN); |
|
- | 239 | ||
233 | navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN); |
240 | debugOut.analog[26] = PID_Pitch; |
234 | 241 | debugOut.analog[27] = PID_Roll; |
|
- | 242 | ||
235 | PRTY[CONTROL_PITCH] += PID_Nick; |
243 | naviSticks[CONTROL_PITCH] = PID_Pitch; |
236 | PRTY[CONTROL_ROLL] += PID_Roll; |
244 | naviSticks[CONTROL_ROLL] = PID_Roll; |
237 | 245 | } else { // invalid GPS data or bad compass reading |
|
238 | } else { // invalid GPS data or bad compass reading |
246 | // reset error integral |
Line 250... | Line 258... | ||
250 | 258 | ||
251 | // store home position if start of flight flag is set |
259 | // store home position if start of flight flag is set |
252 | if (MKFlags & MKFLAG_CALIBRATE) { |
260 | if (MKFlags & MKFLAG_CALIBRATE) { |
253 | MKFlags &= ~(MKFLAG_CALIBRATE); |
261 | MKFlags &= ~(MKFLAG_CALIBRATE); |
- | 262 | if (navi_writeCurrPositionTo(&homePosition)) { |
|
254 | if (navi_writeCurrPositionTo(&homePosition)) { |
263 | // shift north to simulate an offset. |
255 | homePosition.latitude += 10000L; |
264 | // homePosition.latitude += 10000L; |
256 | beep(500); |
265 | beep(500); |
257 | } |
266 | } |
Line 258... | Line 267... | ||
258 | } |
267 | } |
259 | 268 | ||
- | 269 | switch (GPSInfo.status) { |
|
260 | switch (GPSInfo.status) { |
270 | case INVALID: // invalid gps data |
261 | case INVALID: // invalid gps data |
271 | navi_setNeutral(); |
262 | if (flightMode != GPS_FLIGHT_MODE_FREE) { |
272 | if (flightMode != GPS_FLIGHT_MODE_FREE) { |
263 | beep(100); // beep if signal is neccesary |
273 | beep(100); // beep if signal is neccesary |
264 | } |
274 | } |
Line 268... | Line 278... | ||
268 | if (GPSTimeout) |
278 | if (GPSTimeout) |
269 | GPSTimeout--; |
279 | GPSTimeout--; |
270 | // if no new data arrived within timeout set current data invalid |
280 | // if no new data arrived within timeout set current data invalid |
271 | // and therefore disable GPS |
281 | // and therefore disable GPS |
272 | else { |
282 | else { |
- | 283 | navi_setNeutral(); |
|
273 | GPSInfo.status = INVALID; |
284 | GPSInfo.status = INVALID; |
274 | } |
285 | } |
275 | break; |
286 | break; |
276 | case NEWDATA: // new valid data from gps device |
287 | case NEWDATA: // new valid data from gps device |
277 | // if the gps data quality is good |
288 | // if the gps data quality is good |
Line 280... | Line 291... | ||
280 | switch (flightMode) { // check what's to do |
291 | switch (flightMode) { // check what's to do |
281 | case GPS_FLIGHT_MODE_FREE: |
292 | case GPS_FLIGHT_MODE_FREE: |
282 | // update hold position to current gps position |
293 | // update hold position to current gps position |
283 | navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad |
294 | navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad |
284 | // disable gps control |
295 | // disable gps control |
- | 296 | navi_setNeutral(); |
|
285 | break; |
297 | break; |
Line 286... | Line 298... | ||
286 | 298 | ||
287 | case GPS_FLIGHT_MODE_AID: |
299 | case GPS_FLIGHT_MODE_AID: |
288 | if (holdPosition.status != INVALID) { |
300 | if (holdPosition.status != INVALID) { |
289 | if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
301 | if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
290 | // update hold point to current gps position |
302 | // update hold point to current gps position |
291 | navi_writeCurrPositionTo(&holdPosition); |
303 | navi_writeCurrPositionTo(&holdPosition); |
- | 304 | // disable gps control |
|
292 | // disable gps control |
305 | navi_setNeutral(); |
293 | GPS_P_Delay = 0; |
306 | GPS_P_Delay = 0; |
294 | } else { // GPS control active |
307 | } else { // GPS control active |
295 | if (GPS_P_Delay < 7) { |
308 | if (GPS_P_Delay < 7) { |
296 | // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
309 | // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
297 | GPS_P_Delay++; |
310 | GPS_P_Delay++; |
298 | navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position |
311 | navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position |
299 | navi_PIDController(NULL, PRTY); // activates only the D-Part |
312 | navi_PIDController(NULL); // activates only the D-Part |
300 | } else |
313 | } else |
301 | navi_PIDController(&holdPosition, PRTY); // activates the P&D-Part |
314 | navi_PIDController(&holdPosition); // activates the P&D-Part |
302 | } |
315 | } |
303 | } else // invalid Hold Position |
316 | } else { // invalid Hold Position |
304 | { // try to catch a valid hold position from gps data input |
317 | // try to catch a valid hold position from gps data input |
- | 318 | navi_writeCurrPositionTo(&holdPosition); |
|
305 | navi_writeCurrPositionTo(&holdPosition); |
319 | navi_setNeutral(); |
306 | } |
320 | } |
Line 307... | Line 321... | ||
307 | break; |
321 | break; |
308 | 322 | ||
309 | case GPS_FLIGHT_MODE_HOME: |
323 | case GPS_FLIGHT_MODE_HOME: |
310 | if (homePosition.status != INVALID) { |
324 | if (homePosition.status != INVALID) { |
311 | // update hold point to current gps position |
325 | // update hold point to current gps position |
312 | // to avoid a flight back if home comming is deactivated |
326 | // to avoid a flight back if home comming is deactivated |
313 | navi_writeCurrPositionTo(&holdPosition); |
327 | navi_writeCurrPositionTo(&holdPosition); |
314 | if (navi_isManuallyControlled(PRTY)) // MK controlled by user |
328 | if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
315 | { |
329 | navi_setNeutral(); |
316 | } else {// GPS control active |
330 | } else {// GPS control active |
317 | navi_PIDController(&homePosition, PRTY); |
331 | navi_PIDController(&homePosition); |
318 | } |
332 | } |
319 | } else { |
333 | } else { |
320 | // bad home position |
334 | // bad home position |
Line 321... | Line 335... | ||
321 | beep(50); // signal invalid home position |
335 | beep(50); // signal invalid home position |
322 | // try to hold at least the position as a fallback option |
336 | // try to hold at least the position as a fallback option |
323 | 337 | ||
- | 338 | if (holdPosition.status != INVALID) { |
|
324 | if (holdPosition.status != INVALID) { |
339 | if (navi_isManuallyControlled(PRTY)) { |
325 | if (navi_isManuallyControlled(PRTY)) { |
340 | // MK controlled by user |
326 | // MK controlled by user |
341 | navi_setNeutral(); |
327 | } else { |
342 | } else { |
328 | // GPS control active |
343 | // GPS control active |
329 | navi_PIDController(&holdPosition, PRTY); |
344 | navi_PIDController(&holdPosition); |
- | 345 | } |
|
330 | } |
346 | } else { // try to catch a valid hold position |
331 | } else { // try to catch a valid hold position |
347 | navi_writeCurrPositionTo(&holdPosition); |
332 | navi_writeCurrPositionTo(&holdPosition); |
348 | navi_setNeutral(); |
333 | } |
349 | } |
- | 350 | } |
|
334 | } |
351 | break; // eof TSK_HOME |
335 | break; // eof TSK_HOME |
352 | default: // unhandled task |
336 | default: // unhandled task |
353 | navi_setNeutral(); |
337 | break; // eof default |
354 | break; // eof default |
338 | } // eof switch GPS_Task |
355 | } // eof switch GPS_Task |
- | 356 | } // eof gps data quality is good |
|
339 | } // eof gps data quality is good |
357 | else { // gps data quality is bad |
340 | else // gps data quality is bad |
358 | // disable gps control |
341 | { // disable gps control |
359 | navi_setNeutral(); |
342 | if (flightMode != GPS_FLIGHT_MODE_FREE) { |
360 | if (flightMode != GPS_FLIGHT_MODE_FREE) { |
343 | // beep if signal is not sufficient |
361 | // beep if signal is not sufficient |
Line 350... | Line 368... | ||
350 | } |
368 | } |
351 | // set current data as processed to avoid further calculations on the same gps data |
369 | // set current data as processed to avoid further calculations on the same gps data |
352 | GPSInfo.status = PROCESSED; |
370 | GPSInfo.status = PROCESSED; |
353 | break; |
371 | break; |
354 | } // eof GPSInfo.status |
372 | } // eof GPSInfo.status |
- | 373 | ||
- | 374 | PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH]; |
|
- | 375 | PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL]; |
|
355 | } |
376 | } |