diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index e6f8a1bfaa..8564690453 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -287,15 +287,19 @@ static void set_servos(void)
 		g.channel_rudder.radio_out 		= g.channel_rudder.radio_in;
 		// FIXME To me it does not make sense to control the aileron using radio_in in manual mode
 		// Doug could you please take a look at this ?
-		if (g_rc_function[RC_Channel_aux::k_aileron] != rc_array[g.flight_mode_channel-1]) {
-			G_RC_AUX(k_aileron)->radio_out	= g_rc_function[RC_Channel_aux::k_aileron]->radio_in;
-        }
+		if (g_rc_function[RC_Channel_aux::k_aileron]) {
+			if (g_rc_function[RC_Channel_aux::k_aileron] != rc_array[g.flight_mode_channel-1]) {
+				g_rc_function[RC_Channel_aux::k_aileron]->radio_out	= g_rc_function[RC_Channel_aux::k_aileron]->radio_in;
+			}
+		}
 		// only use radio_in if the channel is not used as flight_mode_channel
-		if (g_rc_function[RC_Channel_aux::k_flap_auto] != rc_array[g.flight_mode_channel-1]) {
-			G_RC_AUX(k_flap_auto)->radio_out	= g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in;
-		} else {
-			G_RC_AUX(k_flap_auto)->radio_out	= g_rc_function[RC_Channel_aux::k_flap_auto]->radio_trim;
-        }
+		if (g_rc_function[RC_Channel_aux::k_flap_auto]) {
+			if (g_rc_function[RC_Channel_aux::k_flap_auto] != rc_array[g.flight_mode_channel-1]) {
+				g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out	= g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in;
+			} else {
+				g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out	= g_rc_function[RC_Channel_aux::k_flap_auto]->radio_trim;
+			}
+		}
 	} else {
 		if (g.mix_mode == 0) {
 			g.channel_roll.calc_pwm();
diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h
index a74b58535e..b378d5ded2 100644
--- a/libraries/APO/APO_DefaultSetup.h
+++ b/libraries/APO/APO_DefaultSetup.h
@@ -23,6 +23,7 @@ void setup() {
 	AP_Guide * guide = NULL;
 	AP_Controller * controller = NULL;
 	AP_HardwareAbstractionLayer * hal = NULL;
+
 	/*
 	 * Communications
 	 */
@@ -50,6 +51,7 @@ void setup() {
 			hal->debug->println_P(PSTR("initializing gps"));
 			AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
 			hal->gps = &gpsDriver;
+			hal->gps->callback = delay;
 			hal->gps->init();
 		}
 
@@ -123,7 +125,7 @@ void setup() {
 			hal->rangeFinders.push_back(rangeFinder);
 		}
 
-	} else 
+	}
 
 	/*
 	 * Select guidance, navigation, control algorithms
@@ -155,7 +157,6 @@ void setup() {
 		hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
 	}
 
-
 	/*
 	 * Start the autopilot
 	 */
diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp
index 55251c720d..fa796d0626 100644
--- a/libraries/APO/AP_Autopilot.cpp
+++ b/libraries/APO/AP_Autopilot.cpp
@@ -45,6 +45,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
 				if (hal->gps->fix) {
 					break;
 				} else {
+					hal->gps->update();
 					hal->gcs->sendText(SEVERITY_LOW,
 							PSTR("waiting for gps lock\n"));
 					hal->debug->printf_P(PSTR("waiting for gps lock\n"));
@@ -67,8 +68,8 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
 				break;
 			}
 			hal->debug->println_P(PSTR("waiting for hil packet"));
-			delay(1000);
 		}
+		delay(1000);
 	}
 	
 	AP_MavlinkCommand::home.setAlt(_navigator->getAlt());