Page 361 - 2

// THROTTLE sticks during mission and RTH
#if GPS
if (GPS_conf.ignore_throttle == 1) {
if (f.GPS_mode == GPS_MODE_NAV || f.GPS_mode == GPS_MODE_RTH) {
//rcCommand[ROLL] = 0;
//rcCommand[PITCH] = 0;
//rcCommand[YAW] = 0;
f.THROTTLE_IGNORED = 1;
} else
f.THROTTLE_IGNORED = 0;
}

//Heading manipulation TODO: Do heading manipulation
#endif

if (abs(rcCommand[YAW]) <70 && f.MAG_MODE) {
int16_t dif = att.heading - magHold;
if (dif <= - 180) dif += 360;
if (dif >= + 180) dif -= 360;
if (f.SMALL_ANGLES_25 || (f.GPS_mode != 0)) rcCommand[YAW] -= dif*conf.pid[PIDMAG].
P8 >> 5; //Always correct maghold in GPS mode
} else magHold = att.heading;

#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
/* Smooth alt change routine , for slow auto and aerophoto modes (in general solution from al
exmos). It's slowly increase/decrease
* altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm in 1 second
with cycle time about 3-4ms)
*/
if (f.BARO_MODE) {
static uint8_t isAltHoldChanged = 0;
static int16_t AltHoldCorr = 0;

#if GPS
if (f.LAND_IN_PROGRESS) { //If autoland is in progress then take over and decrease alt slowly
AltHoldCorr -= GPS_conf.land_speed;
if(abs(AltHoldCorr) > 512) {
AltHold += AltHoldCorr/512;
AltHoldCorr %= 512;
}
}
#endif
//IF Throttle not ignored then allow change altitude with the stick....
if ( (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZO
NE) && !f.THROTTLE_IGNORED) {

- 361 -