Rev 2074 | Rev 2086 | Go to most recent revision | Show entire file | Regard 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 { |