Page 362 - 2

// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives
~ +50 cm in 1 second with cycle time about 3-4ms)
AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold;
if(abs(AltHoldCorr) > 512) {
AltHold += AltHoldCorr/512;
AltHoldCorr %= 512;
}
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = alt.EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
debug[2] = BaroPID;
debug[3] = rcCommand[THROTTLE];
}
#endif //BARO

#if defined(THROTTLE_ANGLE_CORRECTION)
if(f.ANGLE_MODE || f.HORIZON_MODE) {
rcCommand[THROTTLE]+= throttleAngleCorrection;
}
#endif

#if GPS
//TODO: split cos_yaw calculations into two phases (X and Y)
if (( f.GPS_mode != GPS_MODE_NONE ) && f.GPS_FIX_HOME ) {
float sin_yaw_y = sin(att.heading*0.0174532925f);
float cos_yaw_x = cos(att.heading*0.0174532925f);
GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
} else {
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
}

//Used to communicate back nav angles to the GPS simulator (for HIL testing)
#if defined(GPS_SIMULATOR)
SerialWrite(2,0xa5);
SerialWrite16(2,nav[LAT]+rcCommand[PITCH]);
SerialWrite16(2,nav[LON]+rcCommand[ROLL]);
SerialWrite16(2,(nav[LAT]+rcCommand[PITCH])-(nav[LON]+rcCommand[ROLL])); //check
#endif

#endif //GPS

- 362 -