Rev 2074 | Rev 2086 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2074 | Rev 2076 | ||
|---|---|---|---|
| Line 101... | Line 101... | ||
| 101 | return 0; |
101 | return 0; |
| 102 | } |
102 | } |
| Line 103... | Line 103... | ||
| 103 | 103 | ||
| 104 | // checks nick and roll sticks for manual control |
104 | // checks nick and roll sticks for manual control |
| - | 105 | uint8_t navi_isManuallyControlled(int16_t* PRTY) { |
|
| - | 106 | debugOut.analog[26] = PRTY[CONTROL_PITCH]; |
|
| 105 | uint8_t navi_isManuallyControlled(int16_t* PRTY) { |
107 | debugOut.analog[27] = PRTY[CONTROL_ROLL]; |
| 106 | if (PRTY[CONTROL_PITCH] < staticParams.naviStickThreshold |
108 | if (abs(PRTY[CONTROL_PITCH]) < staticParams.naviStickThreshold |
| 107 | && PRTY[CONTROL_ROLL] < staticParams.naviStickThreshold) |
109 | && abs(PRTY[CONTROL_ROLL]) < staticParams.naviStickThreshold) |
| 108 | return 0; |
110 | return 0; |
| 109 | else |
111 | else |
| 110 | return 1; |
112 | return 1; |
| Line 250... | Line 252... | ||
| 250 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
252 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
| Line 251... | Line 253... | ||
| 251 | 253 | ||
| 252 | // limit resulting GPS control vector |
254 | // limit resulting GPS control vector |
| Line 253... | Line -... | ||
| 253 | navi_limitXY(&PID_Pitch, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN); |
- | |
| 254 | - | ||
| 255 | debugOut.analog[26] = PID_Pitch; |
- | |
| 256 | debugOut.analog[27] = PID_Roll; |
255 | navi_limitXY(&PID_Pitch, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN); |
| 257 | 256 | ||
| 258 | naviSticks[CONTROL_PITCH] = PID_Pitch; |
257 | naviSticks[CONTROL_PITCH] = PID_Pitch; |
| 259 | naviSticks[CONTROL_ROLL] = PID_Roll; |
258 | naviSticks[CONTROL_ROLL] = PID_Roll; |
| 260 | } else { // invalid GPS data or bad compass reading |
259 | } else { // invalid GPS data or bad compass reading |
| Line 348... | Line 347... | ||
| 348 | // update hold point to current gps position |
347 | // update hold point to current gps position |
| 349 | // to avoid a flight back if home comming is deactivated |
348 | // to avoid a flight back if home comming is deactivated |
| 350 | navi_writeCurrPositionTo(&holdPosition); |
349 | navi_writeCurrPositionTo(&holdPosition); |
| 351 | if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
350 | if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
| 352 | navi_setNeutral(); |
351 | navi_setNeutral(); |
| - | 352 | naviStatus = NAVI_STATUS_MANUAL_OVERRIDE; |
|
| 353 | } else {// GPS control active |
353 | } else {// GPS control active |
| 354 | navi_PIDController(&homePosition); |
354 | navi_PIDController(&homePosition); |
| 355 | } |
355 | } |
| 356 | naviStatus = NAVI_STATUS_RTH; |
356 | naviStatus = NAVI_STATUS_RTH; |
| 357 | } else { |
357 | } else { |