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