APMrover v2.0c - tested with APM v2 full kit (Oilpan) - dualversion (IMUless + IMUfull)

Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>

APMrover2/APM_Config.h

@@ -10,11 +10,16 @@
 
 #define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
 
+#define LITE  DISABLED    // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329
+                          // if LITE is DISABLED, this is for a full APM v1 (Oilpan + GPS MT3329 + Magnetometer HMC5883L) or APM v2
+
 #define CLI_ENABLED         ENABLED
 #define CLI_SLIDER_ENABLED  DISABLED
 #define CLOSED_LOOP_NAV     ENABLED
 #define AUTO_WP_RADIUS      ENABLED
 
+#define TRACE               DISABLED
+
 //#include "APM_Config_HILmode.h"  // for test in HIL mode with AeroSIM Rc 3.83
 #include "APM_Config_Rover.h"      // to be used with the real Traxxas model Monster Jam Grinder
 

APMrover2/APM_Config_Rover.h

@@ -11,12 +11,14 @@
 //#define GROUND_START_DELAY  1
 
 #define AIRSPEED_SENSOR	    DISABLED
+
+#if LITE == DISABLED
   #define MAGNETOMETER	    ENABLED
   #define LOGGING_ENABLED	    ENABLED
 
   #define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_UP_PINS_FORWARD
   #define PARAM_DECLINATION       0.18  // Paris
-
+#endif
 //////////////////////////////////////////////////////////////////////////////
 // Serial port speeds.
 //

APMrover2/APMrover2.pde

@@ -1,11 +1,11 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "APMrover v2.0b JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer
+#define THISFIRMWARE "APMrover v2.0c JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer
 
 // This is a full version of Arduplane v2.32 specially adapted for a Rover by Jean-Louis Naudin (JLN) 
 
 /*
-Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler
+Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin
 Thanks to:  Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier 
 Please contribute your ideas!
 
@@ -16,7 +16,7 @@ License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.
 
 //
-// JLN updates: last update 2012-05-01
+// JLN updates: last update 2012-05-04
 //
 // DOLIST:
 //  
@@ -24,6 +24,9 @@ version 2.1 of the License, or (at your option) any later version.
 //-------------------------------------------------------------------------------------------------------------------------
 // Dev Startup : 2012-04-21
 //
+//  2012-05-04: Added #define LITE ENABLED  for the APM1280 or APM2560 CPU IMUless version
+//  2012-05-03: Successful missions tests with a full APM2560 kit (GPS MT3329 + magnetometer HMC5883L)
+//  2012-05-03: removing stick mixing in auto mode
 //  2012-05-01: special update for rover about ground_course if compass is enabled
 //  2012-04-30: Successfully tested in autonomous nav with a waypoints list recorded in live mode
 //  2012-04-30: Now a full version for APM v1 or APM v2 with magnetometer
@@ -586,7 +589,6 @@ static long	nav_pitch;
 // Calculated radius for the wp turn based on ground speed and max turn angle
 static long    wp_radius;
 static long	toff_yaw;			// deg * 100 : yaw angle for takeoff
-static long     nav_yaw = 1;                    // deg * 100 : target yaw angle  - used only for thermal hunt
 static long	altitude_estimate = 0;		// for smoothing GPS output
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -643,8 +645,7 @@ static struct 	Location current_loc;
 static struct 	Location next_WP;
 // The location of the active waypoint in Guided mode.
 static struct  	Location guided_WP;
-static struct   Location soarwp1_WP = {0,0,0};	// temp waypoint for Ridge Soaring
-static struct   Location soarwp2_WP = {0,0,0};	// temp waypoint for Ridge Soaring
+
 // The location structure information from the Nav command being processed
 static struct 	Location next_nav_command;	
 // The location structure information from the Non-Nav command being processed
@@ -749,11 +750,12 @@ void loop()
 
 		if (millis() - perf_mon_timer > 20000) {
 			if (mainLoop_count != 0) {
+  #if LITE == DISABLED
 				if (g.log_bitmask & MASK_LOG_PM)
 					#if HIL_MODE != HIL_MODE_ATTITUDE
 					Log_Write_Performance();
 					#endif
-
+ #endif
 				resetPerfData();
 			}
 		}
@@ -787,11 +789,14 @@ static void fast_loop()
 		gcs_update();
 	#endif
 
+#if LITE == DISABLED
 	ahrs.update();
+#endif 
 
 	// uses the yaw from the DCM to give more accurate turns
 	calc_bearing_error();
 
+#if LITE == DISABLED
 	# if HIL_MODE == HIL_MODE_DISABLED
 		if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
 			Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
@@ -799,7 +804,7 @@ static void fast_loop()
 		if (g.log_bitmask & MASK_LOG_RAW)
 			Log_Write_Raw();
 	#endif
-
+#endif
 	// inertial navigation
 	// ------------------
 	#if INERTIAL_NAVIGATION == ENABLED
@@ -846,6 +851,7 @@ static void medium_loop()
                 calc_gndspeed_undershoot();
             }
             
+#if LITE == DISABLED
 			#if HIL_MODE != HIL_MODE_ATTITUDE
             if (g.compass_enabled && compass.read()) {
                 ahrs.set_compass(&compass);
@@ -857,6 +863,7 @@ static void medium_loop()
                 ahrs.set_compass(NULL);
             }
 			#endif
+#endif
 /*{
 Serial.print(ahrs.roll_sensor, DEC);	Serial.printf_P(PSTR("\t"));
 Serial.print(ahrs.pitch_sensor, DEC);	Serial.printf_P(PSTR("\t"));
@@ -901,7 +908,7 @@ Serial.println(tempaccel.z, DEC);
 		//-------------------------------------------------
 		case 3:
 			medium_loopCounter++;
-
+#if LITE == DISABLED
 			#if HIL_MODE != HIL_MODE_ATTITUDE
 				if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
 					Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
@@ -915,7 +922,7 @@ Serial.println(tempaccel.z, DEC);
 
 			if (g.log_bitmask & MASK_LOG_GPS)
 				Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
-
+#endif
             // send all requested output streams with rates requested
             // between 5 and 45 Hz
             gcs_data_stream_send(5,45);
@@ -949,12 +956,13 @@ static void slow_loop()
 			check_long_failsafe();
 			superslow_loopCounter++;
 			if(superslow_loopCounter >=200) {				//	200 = Execute every minute
+#if LITE == DISABLED
 				#if HIL_MODE != HIL_MODE_ATTITUDE
 					if(g.compass_enabled) {
 						compass.save_offsets();
 					}
 				#endif
-
+#endif
 				superslow_loopCounter = 0;
 			}
 			break;
@@ -988,22 +996,31 @@ static void slow_loop()
 #if USB_MUX_PIN > 0
             check_usb_mux();
 #endif
+
+#if TRACE == ENABLED
+              Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"), 
+                    (float)ground_course/100, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);           
+             // Serial.printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"), 
+                //    g.command_total, g.command_index, nav_command_index);                                 
+#endif                          
 			break;
 	}
 }
 
 static void one_second_loop()
 {
+#if LITE == DISABLED
 	if (g.log_bitmask & MASK_LOG_CUR)
 		Log_Write_Current();
-
+#endif
 	// send a heartbeat
 	gcs_send_message(MSG_HEARTBEAT);
     gcs_data_stream_send(1,3);
 }
 
 static void update_GPS(void)
-{
+{      static uint16_t hdg;
+  
 	g_gps->update();
 	update_GPS_light();
 
@@ -1026,19 +1043,20 @@ static void update_GPS(void)
 			} else {
 				if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){
 					startup_ground();
-
+#if LITE == DISABLED
 					if (g.log_bitmask & MASK_LOG_CMD)
 						Log_Write_Startup(TYPE_GROUNDSTART_MSG);
-
+#endif
 					init_home();
 				} else if (ENABLE_AIR_START == 0) {
 					init_home();
 				}
-
+#if LITE == DISABLED
 				if (g.compass_enabled) {
 					// Set compass declination automatically
 					compass.set_initial_location(g_gps->latitude, g_gps->longitude);
 				}
+#endif
 				ground_start_count = 0;
 			}
 		}
@@ -1048,13 +1066,16 @@ static void update_GPS(void)
 		current_loc.lat = g_gps->latitude;     // Lat * 10**7
                 current_loc.alt = max((g_gps->altitude - home.alt),0);
                 ground_speed   = g_gps->ground_speed;
+#if LITE == DISABLED                
                 if (g.compass_enabled) {
-                  ground_course = (wrap_360(ToDeg(compass.heading) * 100));
+                  hdg=(ahrs.yaw_sensor / 100) % 360;
+                  ground_course = hdg * 100;
                 } else {
-                  ground_course = g_gps->ground_course;
+#endif            
+                  ground_course = abs(g_gps->ground_course);
+#if LITE == DISABLED                  
                 }
-
-
+#endif
         // see if we've breached the geo-fence
         geofence_check(false);
 	}

APMrover2/Attitude.pde

@@ -6,39 +6,10 @@
 
 static void stabilize()
 {
-	float ch1_inf = 1.0;
-
 	// Calculate desired servo output for the turn  // Wheels Direction
 	// ---------------------------------------------
-         g.channel_roll.servo_out = nav_roll;
-
-	// Mix Stick input to allow users to override control surfaces
-	// -----------------------------------------------------------
-	if ((control_mode < FLY_BY_WIRE_A) ||
-        (ENABLE_STICK_MIXING == 1 &&
-         geofence_stickmixing() &&
-         control_mode > FLY_BY_WIRE_B &&
-         failsafe == FAILSAFE_NONE)) {
-
-		// TODO: use RC_Channel control_mix function?
-		ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
-		ch1_inf = fabs(ch1_inf);
-		ch1_inf = min(ch1_inf, 400.0);
-		ch1_inf = ((400.0 - ch1_inf) /400.0);
-
-		// scale the sensor input based on the stick input
-		// -----------------------------------------------
-		g.channel_roll.servo_out *= ch1_inf;
-
-		// Mix in stick inputs
-		// -------------------
-		g.channel_roll.servo_out +=	g.channel_roll.pwm_to_angle();
-
-		//Serial.printf_P(PSTR(" servo_out[CH_ROLL] "));
-		//Serial.println(servo_out[CH_ROLL],DEC);
-
-	}
         
+      g.channel_roll.servo_out = nav_roll;
       g.channel_roll.servo_out = g.channel_roll.servo_out * g.turn_gain;
       g.channel_rudder.servo_out = g.channel_roll.servo_out;
 }

APMrover2/GCS_Mavlink.pde

@@ -114,7 +114,8 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
         micros(),
         ahrs.roll,
         ahrs.pitch - radians(g.pitch_trim*0.01),
-        ahrs.yaw,
+        ToRad(ground_course/100.0),
+        //ahrs.yaw,
         omega.x,
         omega.y,
         omega.z);
@@ -317,9 +318,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
         g_gps->ground_speed * rot.a.x,  // X speed cm/s
         g_gps->ground_speed * rot.b.x,  // Y speed cm/s
         g_gps->ground_speed * rot.c.x,
-        //g_gps->ground_course);          // course in 1/100 degree
-        ground_course);          // course in 1/100 degree
-
+        g_gps->ground_course);          // course in 1/100 degree
 }
 
 static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
@@ -333,7 +332,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
         target_bearing / 100,
         wp_distance,
         altitude_error / 1.0e2,
-        airspeed_error,
+        groundspeed_error,
         crosstrack_error);
 }
 
@@ -357,8 +356,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
         g_gps->hdop,
         65535,
         g_gps->ground_speed,  // cm/s
-        //g_gps->ground_course, // 1/100 degrees,
-        ground_course, // 1/100 degrees,
+        g_gps->ground_course, // 1/100 degrees,
         g_gps->num_sats);
 
 #else // MAVLINK10
@@ -372,9 +370,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
             g_gps->hdop,
             0.0,
             g_gps->ground_speed / 100.0,
-            //ground_course = (wrap_360(ToDeg(compass.heading) * 100));
-            //g_gps->ground_course / 100.0);
-            ground_course / 100.0);
+            g_gps->ground_course / 100.0);
 #endif  // MAVLINK10
 }
 
@@ -484,13 +480,13 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
 
 static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
 {
-    int32_t pressure = barometer.get_pressure();
+    int32_t pressure = 0;
     mavlink_msg_scaled_pressure_send(
         chan,
         micros(),
         pressure/100.0,
         (pressure - g.ground_pressure)/100.0,
-        barometer.get_temperature());
+        0);
 }
 
 static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
@@ -502,8 +498,8 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
                                     mag_offsets.y,
                                     mag_offsets.z,
                                     compass.get_declination(),
-                                    barometer.get_raw_pressure(),
-                                    barometer.get_raw_temp(),
+                                    0,
+                                    0,
                                     imu.gx(), imu.gy(), imu.gz(),
                                     imu.ax(), imu.ay(), imu.az());
 }
@@ -1048,7 +1044,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 if (packet.param1 == 1 ||
                     packet.param2 == 1 ||
                     packet.param3 == 1) {
-                    startup_IMU_ground(false);
                 }
                 if (packet.param4 == 1) {
                     trim_radio();
@@ -1153,7 +1148,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 case MAV_ACTION_CALIBRATE_ACC:
                 case MAV_ACTION_CALIBRATE_PRESSURE:
                 case MAV_ACTION_REBOOT:  // this is a rough interpretation
-                    startup_IMU_ground(true);
                     result=1;
                     break;
 

APMrover2/Log.pde

@@ -1,5 +1,5 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
+#if LITE == DISABLED
 #if LOGGING_ENABLED == ENABLED
 
 // Code to Write and Read packets from DataFlash log memory
@@ -694,3 +694,4 @@ static void Log_Write_Raw() {}
 
 
 #endif // LOGGING_ENABLED
+#endif

APMrover2/config.h

@@ -50,6 +50,10 @@
 # define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
 #endif
 
+#ifndef LITE
+# define LITE DISABLED
+#endif
+
 #if defined( __AVR_ATmega1280__ )
 #define CLI_ENABLED DISABLED
 #define LOGGING_ENABLED DISABLED
@@ -828,6 +832,10 @@
 # define HIL_SERVOS DISABLED
 #endif
 
+#ifndef TRACE
+# define TRACE DISABLED
+#endif
+
 // use this to completely disable the CLI
 #ifndef CLI_ENABLED
 # define CLI_ENABLED ENABLED

APMrover2/navigation.pde

@@ -77,31 +77,10 @@ static void calc_gndspeed_undershoot()
 
 static void calc_bearing_error()
 {
-	if(takeoff_complete == true  || g.compass_enabled == true) {
-        /*
-          most of the time we use the yaw sensor for heading, even if
-          we don't have a compass. The yaw sensor is drift corrected
-          in the DCM library. We only use the gps ground course
-          directly if we haven't completed takeoff, as the yaw drift
-          correction won't have had a chance to kick in. Drift
-          correction using the GPS typically takes 10 seconds or so
-          for a 180 degree correction.
-         */
-		bearing_error = nav_bearing - ahrs.yaw_sensor;
-	} else {
-
-		// TODO: we need to use the Yaw gyro for in between GPS reads,
-		// maybe as an offset from a saved gryo value.
 	bearing_error = nav_bearing - ground_course;
-	}
-
 	bearing_error = wrap_180(bearing_error);
 }
 
-static void calc_altitude_error()
-{
-}
-
 static long wrap_360(long error)
 {
 	if (error > 36000)	error -= 36000;

APMrover2/radio.pde

@@ -86,27 +86,8 @@ static void read_radio()
 
 	g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
 	g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4));
-
-    #if FLAPERON == ENABLED      
-        // JLN update for true flaperons
-        if (control_mode == MANUAL) {
-          g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
- 	  g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
-        } else {
-        aileron1 = g.rc_5.radio_trim + (BOOL_TO_SIGN(-g.rc_5.get_reverse()) *g.channel_roll.angle_to_pwm());
-        aileron2 = g.rc_6.radio_trim + (BOOL_TO_SIGN(-g.rc_6.get_reverse()) *g.channel_roll.angle_to_pwm());
-        
-        aileron1 = constrain(aileron1,(uint16_t)g.rc_5.radio_min,(uint16_t)g.rc_5.radio_max);
-        aileron2 = constrain(aileron2,(uint16_t)g.rc_6.radio_min,(uint16_t)g.rc_6.radio_max);
-                  
-        g.rc_5.set_pwm(aileron1);
-        g.rc_6.set_pwm(aileron2);
-        }
-    #else
   	g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
  	g.rc_6.set_pwm(APM_RC.InputCh(CH_6));        
-    #endif
-
 	g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
 	g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
 

APMrover2/sensors.pde

@@ -1,5 +1,5 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
+#if LITE == DISABLED
 // Sensors are not available in HIL_MODE_ATTITUDE
 #if HIL_MODE != HIL_MODE_ATTITUDE
 
@@ -83,6 +83,7 @@ static void zero_airspeed(void)
 }
 
 #endif // HIL_MODE != HIL_MODE_ATTITUDE
+#endif
 
 static void read_battery(void)
 {

APMrover2/system.pde

@@ -9,7 +9,9 @@ The init_ardupilot function processes everything we need for an in - air restart
 #if CLI_ENABLED == ENABLED
 
 // Functions called from the top-level menu
+#if LITE == DISABLED
 static int8_t	process_logs(uint8_t argc, const Menu::arg *argv);	// in Log.pde
+#endif
 static int8_t	setup_mode(uint8_t argc, const Menu::arg *argv);	// in setup.pde
 static int8_t	test_mode(uint8_t argc, const Menu::arg *argv);		// in test.cpp
 static int8_t	planner_mode(uint8_t argc, const Menu::arg *argv);	// in planner.pde
@@ -33,7 +35,9 @@ static int8_t	main_menu_help(uint8_t argc, const Menu::arg *argv)
 static const struct Menu::command main_menu_commands[] PROGMEM = {
 //   command		function called
 //   =======        ===============
+#if LITE == DISABLED
 	{"logs",		process_logs},
+#endif
 	{"setup",		setup_mode},
 	{"test",		test_mode},
 	{"help",		main_menu_help},
@@ -155,6 +159,7 @@ static void init_ardupilot()
 
 	mavlink_system.sysid = g.sysid_this_mav;
 
+#if LITE == DISABLED
 #if LOGGING_ENABLED == ENABLED
 	DataFlash.Init(); 	// DataFlash log initialization
     if (!DataFlash.CardInserted()) {
@@ -168,6 +173,7 @@ static void init_ardupilot()
 		DataFlash.start_new_log();
 	}
 #endif
+#endif
 
 #if HIL_MODE != HIL_MODE_ATTITUDE
 
@@ -175,6 +181,7 @@ static void init_ardupilot()
     adc.Init(&timer_scheduler);      // APM ADC library initialization
 #endif
 
+#if LITE == DISABLED
 	barometer.init(&timer_scheduler);
 
 	if (g.compass_enabled==true) {
@@ -189,7 +196,7 @@ static void init_ardupilot()
         }
 	}
 #endif
-
+#endif
 	// Do GPS init
 	g_gps = &g_gps_driver;
 	g_gps->init();			// GPS Initialization
@@ -257,9 +264,10 @@ static void init_ardupilot()
 
 	startup_ground();
 
+#if LITE == DISABLED
 	if (g.log_bitmask & MASK_LOG_CMD)
 			Log_Write_Startup(TYPE_GROUNDSTART_MSG);
-
+#endif
         set_mode(MANUAL);
 
 	// set the correct flight mode
@@ -286,12 +294,13 @@ static void startup_ground(void)
 	// -----------------------
 	demo_servos(1);
 
+#if LITE == DISABLED
 	//IMU ground start
 	//------------------------
     //
 
 	startup_IMU_ground(false);
-
+#endif
 	// read the radio to set trims
 	// ---------------------------
 	trim_radio();		// This was commented out as a HACK.  Why?  I don't find a problem.
@@ -370,8 +379,11 @@ static void set_mode(byte mode)
 			break;
 	}
 
+#if LITE == DISABLED
 	if (g.log_bitmask & MASK_LOG_MODE)
 		Log_Write_Mode(control_mode);
+#endif
+
 }
 
 static void check_long_failsafe()
@@ -413,7 +425,7 @@ static void check_short_failsafe()
 	}
 }
 
-
+#if LITE == DISABLED
 static void startup_IMU_ground(bool force_accel_level)
 {
 #if HIL_MODE != HIL_MODE_ATTITUDE
@@ -455,7 +467,7 @@ static void startup_IMU_ground(bool force_accel_level)
 	digitalWrite(A_LED_PIN, LED_OFF);
 	digitalWrite(C_LED_PIN, LED_OFF);
 }
-
+#endif
 
 static void update_GPS_light(void)
 {