| 103,8 → 103,10 |
|---|
| |
| // checks nick and roll sticks for manual control |
| uint8_t navi_isManuallyControlled(int16_t* PRTY) { |
| if (PRTY[CONTROL_PITCH] < staticParams.naviStickThreshold |
| && PRTY[CONTROL_ROLL] < staticParams.naviStickThreshold) |
| debugOut.analog[26] = PRTY[CONTROL_PITCH]; |
| debugOut.analog[27] = PRTY[CONTROL_ROLL]; |
| if (abs(PRTY[CONTROL_PITCH]) < staticParams.naviStickThreshold |
| && abs(PRTY[CONTROL_ROLL]) < staticParams.naviStickThreshold) |
| return 0; |
| else |
| return 1; |
| 252,9 → 254,6 |
|---|
| // limit resulting GPS control vector |
| navi_limitXY(&PID_Pitch, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN); |
| |
| debugOut.analog[26] = PID_Pitch; |
| debugOut.analog[27] = PID_Roll; |
| |
| naviSticks[CONTROL_PITCH] = PID_Pitch; |
| naviSticks[CONTROL_ROLL] = PID_Roll; |
| } else { // invalid GPS data or bad compass reading |
| 350,6 → 349,7 |
|---|
| navi_writeCurrPositionTo(&holdPosition); |
| if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
| navi_setNeutral(); |
| naviStatus = NAVI_STATUS_MANUAL_OVERRIDE; |
| } else {// GPS control active |
| navi_PIDController(&homePosition); |
| } |