Merge branch 'master' of https://code.google.com/p/ardupilot-mega

ArduCopter/ArduCopter.pde

@@ -418,6 +418,7 @@ static byte	led_mode = NORMAL_LEDS;
 static const 	float t7			= 10000000.0;
 // We use atan2 and other trig techniques to calaculate angles
 // We need to scale the longitude up to make these calcs work
+// to account for decreasing distance between lines of longitude away from the equator
 static float 	scaleLongUp			= 1;
 // Sometimes we need to remove the scaling for distance calcs
 static float 	scaleLongDown 		= 1;
@@ -529,20 +530,12 @@ static int8_t CH7_wp_index;
 ////////////////////////////////////////////////////////////////////////////////
 // Battery Sensors
 ////////////////////////////////////////////////////////////////////////////////
-// Battery Voltage of total battery, initialized above threshold for filter
-static float	battery_voltage		= LOW_VOLTAGE * 1.05;
-// Battery Voltage of cell 1, initialized above threshold for filter
-static float 	battery_voltage1 	= LOW_VOLTAGE * 1.05;
-// Battery Voltage of cells 1 + 2, initialized above threshold for filter
-static float 	battery_voltage2 	= LOW_VOLTAGE * 1.05;
-// Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
-static float 	battery_voltage3 	= LOW_VOLTAGE * 1.05;
-// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
-static float 	battery_voltage4 	= LOW_VOLTAGE * 1.05;
+// Battery Voltage of battery, initialized above threshold for filter
+static float	battery_voltage1		= LOW_VOLTAGE * 1.05;
 // refers to the instant amp draw – based on an Attopilot Current sensor
-static float	current_amps;
+static float	current_amps1;
 // refers to the total amps drawn – based on an Attopilot Current sensor
-static float	current_total;
+static float	current_total1;
 // Used to track if the battery is low - LED output flashes when the batt is low
 static bool		low_batt = false;
 

ArduCopter/GCS_Mavlink.pde

@@ -78,7 +78,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
         status 		= MAV_STATE_STANDBY;
     }
 
-    uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity;	//Mavlink scaling 100% = 1000
+    uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity;	//Mavlink scaling 100% = 1000
 
     mavlink_msg_sys_status_send(
         chan,
@@ -86,7 +86,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
         nav_mode,
         status,
         0,
-        battery_voltage * 1000,
+        battery_voltage1 * 1000,
         battery_remaining,
         packet_drops);
 }

ArduCopter/Log.pde

@@ -321,9 +321,9 @@ static void Log_Write_Current()
 
 	DataFlash.WriteInt(g.rc_3.control_in);			// 1
 	DataFlash.WriteLong(throttle_integrator);		// 2
-	DataFlash.WriteInt(battery_voltage 	* 100.0);	// 3
-	DataFlash.WriteInt(current_amps 	* 100.0);	// 4
-	DataFlash.WriteInt(current_total);				// 5
+	DataFlash.WriteInt(battery_voltage1 	* 100.0);	// 3
+	DataFlash.WriteInt(current_amps1 	* 100.0);	// 4
+	DataFlash.WriteInt(current_total1);				// 5
 
 	DataFlash.WriteByte(END_BYTE);
 }

ArduCopter/Parameters.h

@@ -198,7 +198,7 @@ public:
 	AP_Int16	RTL_altitude;
 	AP_Int8		sonar_enabled;
 	AP_Int8		sonar_type;   // 0 = XL, 1 = LV, 2 = XLL (XL with 10m range)
-	AP_Int8		battery_monitoring;	// 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
+	AP_Int8		battery_monitoring;	// 0=disabled, 3=voltage only, 4=voltage and current
 	AP_Int16	pack_capacity;		// Battery pack capacity less reserve
 	AP_Int8		compass_enabled;
     AP_Int8		optflow_enabled;

ArduCopter/config.h

@@ -118,6 +118,8 @@
 # define USB_MUX_PIN      -1
 # define CLI_SLIDER_ENABLED DISABLED
 # define OPTFLOW_CS_PIN   34
+# define BATTERY_PIN_1      0
+# define CURRENT_PIN_1      1
 #elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
 # define A_LED_PIN        27
 # define B_LED_PIN        26
@@ -129,6 +131,8 @@
 # define CLI_SLIDER_ENABLED DISABLED
 # define USB_MUX_PIN      23
 # define OPTFLOW_CS_PIN   A6
+# define BATTERY_PIN_1      1
+# define CURRENT_PIN_1      2
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/config_channels.h

@@ -61,10 +61,10 @@
 //
 //
 #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
-/* Camera Pitch and Camera Roll: Not yet defined for APM2 
+/* Camera Pitch and Camera Roll: Not yet defined for APM2
  * They will likely be dependent on the frame config */
-# define CH_CAM_PITCH (-1)
-# define CH_CAM_ROLL  (-1)
+# define CH_CAM_PITCH CH_11
+# define CH_CAM_ROLL  CH_10
 #elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
 # define CH_CAM_PITCH CH_5
 # define CH_CAM_ROLL  CH_6

ArduCopter/defines.h

@@ -325,15 +325,8 @@ enum gcs_severity {
 #define AN14  68 // NC
 #define AN15  69 // NC
 
-#define VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage
-#define CURRENT_PIN_1 1 // and current
-
 #define RELAY_PIN 47
 
-#define BATTERY_PIN1 0		        // These are the pins for the voltage dividers
-#define BATTERY_PIN2 1
-#define BATTERY_PIN3 2
-#define BATTERY_PIN4 3
 #define PIEZO_PIN AN5           //Last pin on the back ADC connector
 
 

ArduCopter/sensors.pde

@@ -100,30 +100,25 @@ static void init_optflow()
 
 static void read_battery(void)
 {
-	battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
-	battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
-	battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
-	battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
 
-	if(g.battery_monitoring == 1)
-		battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
-
-	if(g.battery_monitoring == 2)
-		battery_voltage = battery_voltage4;
-
-	if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
-		battery_voltage = battery_voltage1;
+	if(g.battery_monitoring == 0){
+		battery_voltage1 = 0;
+		return;
+	}
 
+	if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
+		battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9;
+	}
 	if(g.battery_monitoring == 4) {
-		current_amps	 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
-		current_total	 += current_amps * 0.0278;	// called at 100ms on average
+		current_amps1	 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin
+		current_total1	 += current_amps1 * 0.02778;	// called at 100ms on average, .0002778 is 1/3600 (conversion to hours)
 	}
 
 	#if BATTERY_EVENT == 1
 	//if(battery_voltage < g.low_voltage)
 	//	low_battery_event();
 
-	if((battery_voltage < g.low_voltage) || (g.battery_monitoring == 4 && current_total > g.pack_capacity)){
+	if((battery_voltage1 < g.low_voltage) || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity)){
 		low_battery_event();
 
 		#if PIEZO_LOW_VOLTAGE == 1

ArduCopter/setup.pde

@@ -395,7 +395,7 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
 		g.battery_monitoring.set_and_save(argv[1].i);
 
 	} else {
-		Serial.printf_P(PSTR("\nOp: off, 1-4"));
+		Serial.printf_P(PSTR("\nOp: off, 3-4"));
 	}
 
 	report_batt_monitor();
@@ -778,8 +778,6 @@ static void report_batt_monitor()
 	Serial.printf_P(PSTR("\nBatt Mon:\n"));
 	print_divider();
 	if(g.battery_monitoring == 0)	print_enabled(false);
-	if(g.battery_monitoring == 1)	Serial.printf_P(PSTR("3c"));
-	if(g.battery_monitoring == 2)	Serial.printf_P(PSTR("4c"));
 	if(g.battery_monitoring == 3)	Serial.printf_P(PSTR("volts"));
 	if(g.battery_monitoring == 4)	Serial.printf_P(PSTR("volts and cur"));
 	print_blanks(2);

ArduCopter/test.pde

@@ -21,7 +21,6 @@ static int8_t	test_battery(uint8_t argc, 		const Menu::arg *argv);
 //static int8_t	test_wp_nav(uint8_t argc, 		const Menu::arg *argv);
 //static int8_t	test_reverse(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_tuning(uint8_t argc, 		const Menu::arg *argv);
-static int8_t	test_current(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_relay(uint8_t argc,	 	const Menu::arg *argv);
 static int8_t	test_wp(uint8_t argc, 			const Menu::arg *argv);
 #if HIL_MODE != HIL_MODE_ATTITUDE
@@ -69,7 +68,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
 	{"battery",		test_battery},
 	{"tune",		test_tuning},
 	//{"tri",			test_tri},
-	{"current",		test_current},
 	{"relay",		test_relay},
 	{"wp",			test_wp},
 	//{"nav",			test_nav},
@@ -713,26 +711,6 @@ test_gps(uint8_t argc, const Menu::arg *argv)
 }
 //*/
 
-static int8_t
-test_battery(uint8_t argc, const Menu::arg *argv)
-{
-#if BATTERY_EVENT == 1
-	for (int i = 0; i < 20; i++){
-		delay(20);
-		read_battery();
-	}
-	Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"),
-			battery_voltage1,
-			battery_voltage2,
-			battery_voltage3,
-			battery_voltage4);
-#else
-	Serial.printf_P(PSTR("Not enabled\n"));
-
-#endif
-	return (0);
-}
-
 static int8_t
 test_tuning(uint8_t argc, const Menu::arg *argv)
 {
@@ -751,7 +729,7 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
 }
 
 static int8_t
-test_current(uint8_t argc, const Menu::arg *argv)
+test_battery(uint8_t argc, const Menu::arg *argv)
 {
 	print_hit_enter();
 	//delta_ms_medium_loop = 100;
@@ -760,11 +738,17 @@ test_current(uint8_t argc, const Menu::arg *argv)
 		delay(100);
 		read_radio();
 		read_battery();
-		Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
-						battery_voltage,
-						current_amps,
-						current_total);
-
+		if (g.battery_monitoring == 3){
+			Serial.printf_P(PSTR("V: %4.4f\n"),
+								battery_voltage1,
+								current_amps1,
+								current_total1);
+		} else {
+			Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"),
+								battery_voltage1,
+								current_amps1,
+								current_total1);
+		}
 		APM_RC.OutputCh(MOT_1, g.rc_3.radio_in);
 		APM_RC.OutputCh(MOT_2, g.rc_3.radio_in);
 		APM_RC.OutputCh(MOT_3, g.rc_3.radio_in);

ArduPlane/ArduPlane.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduPlane V2.27"
+#define THISFIRMWARE "ArduPlane V2.28"
 /*
 Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler
 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 
@@ -240,6 +240,15 @@ AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0)
 	AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter);
 #endif
 
+AP_Relay relay;
+
+// Camera/Antenna mount tracking and stabilisation stuff
+// --------------------------------------
+#if MOUNT == ENABLED
+AP_Mount camera_mount(g_gps, &dcm);
+#endif
+
+
 ////////////////////////////////////////////////////////////////////////////////
 // Global variables
 ////////////////////////////////////////////////////////////////////////////////
@@ -354,162 +363,263 @@ static int     ground_start_avg;
 // If we do not detect GPS at startup, we stop trying and assume GPS is not connected	
 static bool	GPS_enabled 	= false;			
 
+////////////////////////////////////////////////////////////////////////////////
 // Location & Navigation
-// ---------------------
+////////////////////////////////////////////////////////////////////////////////
+// Constants
 const	float radius_of_earth 	= 6378100;	// meters
 const	float gravity 			= 9.81;		// meters/ sec^2
-static long	nav_bearing;						// deg * 100 : 0 to 360 current desired bearing to navigate
-static long	target_bearing;						// deg * 100 : 0 to 360 location of the plane to the target
-static long	crosstrack_bearing;					// deg * 100 : 0 to 360 desired angle of plane to target
-static float	nav_gain_scaler 		= 1;		// Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
+// This is the currently calculated direction to fly.  
+// deg * 100 : 0 to 360
+static long	nav_bearing;
+// This is the direction to the next waypoint or loiter center 
+// deg * 100 : 0 to 360
+static long	target_bearing;	
+//This is the direction from the last waypoint to the next waypoint 
+// deg * 100 : 0 to 360
+static long	crosstrack_bearing;
+// A gain scaler to account for ground speed/headwind/tailwind
+static float	nav_gain_scaler 		= 1;		
+// Direction held during phases of takeoff and landing
+// deg * 100 dir of plane,  A value of -1 indicates the course has not been set/is not in use
 static long    hold_course       	 	= -1;		// deg * 100 dir of plane
 
-static byte	nav_command_index;					// active nav command memory location
-static byte	non_nav_command_index;				// active non-nav command memory location
-static byte	nav_command_ID		= NO_COMMAND;	// active nav command ID
-static byte	non_nav_command_ID	= NO_COMMAND;	// active non-nav command ID
+// There may be two active commands in Auto mode.  
+// This indicates the active navigation command by index number
+static byte	nav_command_index;					
+// This indicates the active non-navigation command by index number
+static byte	non_nav_command_index;				
+// This is the command type (eg navigate to waypoint) of the active navigation command
+static byte	nav_command_ID		= NO_COMMAND;	
+static byte	non_nav_command_ID	= NO_COMMAND;	
 
+////////////////////////////////////////////////////////////////////////////////
 // Airspeed
-// --------
-static int		airspeed;							// m/s * 100
-static int		airspeed_nudge;  					// m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
-static long		target_airspeed;					// m/s * 100 (used for Auto-flap deployment in FBW_B mode)
-static float	airspeed_error;						// m/s * 100
-static long 	energy_error;                       // energy state error (kinetic + potential) for altitude hold
-static long		airspeed_energy_error;              // kinetic portion of energy error (m^2/s^2)
+////////////////////////////////////////////////////////////////////////////////
+// The current airspeed estimate/measurement in centimeters per second
+static int		airspeed;
+// The calculated airspeed to use in FBW-B.  Also used in higher modes for insuring min ground speed is met.
+// Also used for flap deployment criteria.  Centimeters per second.static long		target_airspeed;	
+static long		target_airspeed;
+// The difference between current and desired airspeed.  Used in the pitch controller.  Centimeters per second.
+static float	airspeed_error;	
+// The calculated total energy error (kinetic (altitude) plus potential (airspeed)).  
+// Used by the throttle controller
+static long 	energy_error;
+// kinetic portion of energy error (m^2/s^2)
+static long		airspeed_energy_error;
+// An amount that the airspeed should be increased in auto modes based on the user positioning the 
+// throttle stick in the top half of the range.  Centimeters per second.
+static int		airspeed_nudge;
+// Similar to airspeed_nudge, but used when no airspeed sensor.
+// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
+static int     throttle_nudge = 0;                 
 
+////////////////////////////////////////////////////////////////////////////////
 // Ground speed
-static long		groundspeed_undershoot = 0;				// m/s * 100  (>=0, where > 0 => amount below min ground speed)
-
+////////////////////////////////////////////////////////////////////////////////
+// The amount current ground speed is below min ground speed.  Centimeters per second
+static long		groundspeed_undershoot = 0;
 
+////////////////////////////////////////////////////////////////////////////////
 // Location Errors
-// ---------------
-static long	bearing_error;						// deg * 100 : 0 to 36000
-static long	altitude_error;						// meters * 100 we are off in altitude
-static float	crosstrack_error;					// meters we are off trackline
+////////////////////////////////////////////////////////////////////////////////
+// Difference between current bearing and desired bearing.  Hundredths of a degree
+static long	bearing_error;
+// Difference between current altitude and desired altitude.  Centimeters
+static long	altitude_error;
+// Distance perpandicular to the course line that we are off trackline.  Meters 
+static float	crosstrack_error;
 
+////////////////////////////////////////////////////////////////////////////////
 // Battery Sensors
-// ---------------
-static float	battery_voltage		= LOW_VOLTAGE * 1.05;		// Battery Voltage of total battery, initialized above threshold for filter
-static float 	battery_voltage1 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cell 1, initialized above threshold for filter
-static float 	battery_voltage2 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2, initialized above threshold for filter
-static float 	battery_voltage3 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
-static float 	battery_voltage4 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
+////////////////////////////////////////////////////////////////////////////////
+// Battery pack 1 voltage.  Initialized above the low voltage threshold to pre-load the filter and prevent low voltage events at startup.
+static float 	battery_voltage1 	= LOW_VOLTAGE * 1.05;
+// Battery pack 1 instantaneous currrent draw.  Amperes
+static float	current_amps1;
+// Totalized current (Amp-hours) from battery 1
+static float	current_total1;									
 
-static float	current_amps;
-static float	current_total;
+// To Do - Add support for second battery pack
+//static float 	battery_voltage2 	= LOW_VOLTAGE * 1.05;		// Battery 2 Voltage, initialized above threshold for filter
+//static float	current_amps2;									// Current (Amperes) draw from battery 2
+//static float	current_total2;									// Totalized current (Amp-hours) from battery 2
 
+////////////////////////////////////////////////////////////////////////////////
 // Airspeed Sensors
-// ----------------
-static float   airspeed_raw;                       // Airspeed Sensor - is a float to better handle filtering
-static float   airspeed_pressure;					// airspeed as a pressure value
-
-// Barometer Sensor variables
-// --------------------------
-static unsigned long 	abs_pressure;
+////////////////////////////////////////////////////////////////////////////////
+// Raw differential pressure measurement (filtered).  ADC units
+static float   airspeed_raw; 
+// Raw differential pressure less the zero pressure offset.  ADC units
+static float   airspeed_pressure;
 
+////////////////////////////////////////////////////////////////////////////////
 // Altitude Sensor variables
-// ----------------------
+////////////////////////////////////////////////////////////////////////////////
+// Raw absolute pressure measurement (filtered).  ADC units
+static unsigned long 	abs_pressure;
+// Altitude from the sonar sensor.  Meters.  Not yet implemented.
 static int		sonar_alt;
 
+////////////////////////////////////////////////////////////////////////////////
 // flight mode specific
-// --------------------
-static bool takeoff_complete    = true;         // Flag for using gps ground course instead of IMU yaw.  Set false when takeoff command processes.
+////////////////////////////////////////////////////////////////////////////////
+// Flag for using gps ground course instead of IMU yaw.  Set false when takeoff command in process.
+static bool takeoff_complete    = true;   
+// Flag to indicate if we have landed.
+//Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
 static bool	land_complete;
+// Altitude threshold to complete a takeoff command in autonomous modes.  Centimeters
 static long	takeoff_altitude;
-// static int			landing_distance;					// meters;
-static int			landing_pitch;						// pitch for landing set by commands
+// Pitch to hold during landing command in the no airspeed sensor case.  Hundredths of a degree
+static int			landing_pitch;
+// Minimum pitch to hold during takeoff command execution.  Hundredths of a degree
 static int			takeoff_pitch;
 
+////////////////////////////////////////////////////////////////////////////////
 // Loiter management
-// -----------------
-static long 	old_target_bearing;					// deg * 100
-static int		loiter_total; 						// deg : how many times to loiter * 360
-static int 	loiter_delta;						// deg : how far we just turned
-static int		loiter_sum;							// deg : how far we have turned around a waypoint
-static long 	loiter_time;						// millis : when we started LOITER mode
-static int 	loiter_time_max;					// millis : how long to stay in LOITER mode
+////////////////////////////////////////////////////////////////////////////////
+// Previous target bearing.  Used to calculate loiter rotations.  Hundredths of a degree
+static long 	old_target_bearing;
+// Total desired rotation in a loiter.  Used for Loiter Turns commands.  Degrees
+static int		loiter_total; 
+// The amount in degrees we have turned since recording old_target_bearing
+static int 	loiter_delta;
+// Total rotation in a loiter.  Used for Loiter Turns commands and to check for missed waypoints.  Degrees
+static int		loiter_sum;
+// The amount of time we have been in a Loiter.  Used for the Loiter Time command.  Milliseconds.
+static long 	loiter_time;
+// The amount of time we should stay in a loiter for the Loiter Time command.  Milliseconds.
+static int 	loiter_time_max;
 
-// these are the values for navigation control functions
-// ----------------------------------------------------
-static long	nav_roll;							// deg * 100 : target roll angle
-static long	nav_pitch;							// deg * 100 : target pitch angle
-static int     throttle_nudge = 0;                 // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
+////////////////////////////////////////////////////////////////////////////////
+// Navigation control variables
+////////////////////////////////////////////////////////////////////////////////
+// The instantaneous desired bank angle.  Hundredths of a degree
+static long	nav_roll;
+// The instantaneous desired pitch angle.  Hundredths of a degree
+static long	nav_pitch;
 
-// Waypoints
-// ---------
-static long	wp_distance;						// meters - distance between plane and next waypoint
-static long	wp_totalDistance;					// meters - distance between old and next waypoint
+////////////////////////////////////////////////////////////////////////////////
+// Waypoint distances
+////////////////////////////////////////////////////////////////////////////////
+// Distance between plane and next waypoint.  Meters
+static long	wp_distance;
+// Distance between previous and next waypoint.  Meters
+static long	wp_totalDistance;
 
+////////////////////////////////////////////////////////////////////////////////
 // repeating event control
-// -----------------------
-static byte 		event_id; 							// what to do - see defines
-static long 		event_timer; 						// when the event was asked for in ms
-static uint16_t 	event_delay; 						// how long to delay the next firing of event in millis
-static int 		event_repeat = 0;					// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
-static int 		event_value; 						// per command value, such as PWM for servos
-static int 		event_undo_value;					// the value used to cycle events (alternate value to event_value)
+////////////////////////////////////////////////////////////////////////////////
+// Flag indicating current event type
+static byte 		event_id;
+// when the event was started in ms
+static long 		event_timer;
+// how long to delay the next firing of event in millis
+static uint16_t 	event_delay;					
+// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
+static int 		event_repeat = 0;
+// per command value, such as PWM for servos
+static int 		event_value; 
+// the value used to cycle events (alternate value to event_value)
+static int 		event_undo_value;
 
-// delay command
-// --------------
-static long 	condition_value;						// used in condition commands (eg delay, change alt, etc.)
+////////////////////////////////////////////////////////////////////////////////
+// Conditional command
+////////////////////////////////////////////////////////////////////////////////
+// A value used in condition commands (eg delay, change alt, etc.)
+// For example in a change altitude command, it is the altitude to change to.
+static long 	condition_value;
+// A starting value used to check the status of a conditional command.
+// For example in a delay command the condition_start records that start time for the delay
 static long 	condition_start;
-static int 	condition_rate;
+// A value used in condition commands.  For example the rate at which to change altitude.
+static int 		condition_rate;
 
+////////////////////////////////////////////////////////////////////////////////
 // 3D Location vectors
-// -------------------
-static struct 	Location home;						// home location
-static struct 	Location prev_WP;					// last waypoint
-static struct 	Location current_loc;				// current location
-static struct 	Location next_WP;					// next waypoint
-static struct  	Location guided_WP;					// guided mode waypoint
-static struct 	Location next_nav_command;			// command preloaded
-static struct 	Location next_nonnav_command;		// command preloaded
-static long 	target_altitude;					// used for altitude management between waypoints
-static long 	offset_altitude;					// used for altitude management between waypoints
-static bool	home_is_set; 						// Flag for if we have g_gps lock and have set the home location
+// Location structure defined in AP_Common
+////////////////////////////////////////////////////////////////////////////////
+// The home location used for RTL.  The location is set when we first get stable GPS lock
+static struct 	Location home;
+// Flag for if we have g_gps lock and have set the home location
+static bool	home_is_set;
+// The location of the previous waypoint.  Used for track following and altitude ramp calculations
+static struct 	Location prev_WP;
+// The plane's current location
+static struct 	Location current_loc;
+// The location of the current/active waypoint.  Used for altitude ramp, track following and loiter calculations.
+static struct 	Location next_WP;
+// The location of the active waypoint in Guided mode.
+static struct  	Location guided_WP;
+// 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
+static struct 	Location next_nonnav_command;
 
+////////////////////////////////////////////////////////////////////////////////
+// Altitude / Climb rate control
+////////////////////////////////////////////////////////////////////////////////
+// The current desired altitude.  Altitude is linearly ramped between waypoints.  Centimeters
+static long 	target_altitude;
+// Altitude difference between previous and current waypoint.  Centimeters
+static long 	offset_altitude;
 
+////////////////////////////////////////////////////////////////////////////////
 // IMU variables
-// -------------
-static float G_Dt						= 0.02;		// Integration time for the gyros (DCM algorithm)
-
+////////////////////////////////////////////////////////////////////////////////
+// The main loop execution time.  Seconds
+//This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
+static float G_Dt						= 0.02;		
 
+////////////////////////////////////////////////////////////////////////////////
 // Performance monitoring
-// ----------------------
-static long 	perf_mon_timer;						// Metric based on accel gain deweighting
-static int 	G_Dt_max = 0;						// Max main loop cycle time in milliseconds
+////////////////////////////////////////////////////////////////////////////////
+// Timer used to accrue data and trigger recording of the performanc monitoring log message
+static long 	perf_mon_timer;
+// The maximum main loop execution time recorded in the current performance monitoring interval
+static int 	G_Dt_max = 0;
+// The number of gps fixes recorded in the current performance monitoring interval
 static int 	gps_fix_count = 0;
+// A variable used by developers to track performanc metrics.
+// Currently used to record the number of GCS heartbeat messages received
 static int		pmTest1 = 0;
 
 
+////////////////////////////////////////////////////////////////////////////////
 // System Timers
-// --------------
-static unsigned long 	fast_loopTimer;				// Time in miliseconds of main control loop
-static unsigned long 	fast_loopTimeStamp;			// Time Stamp when fast loop was complete
-static uint8_t 		delta_ms_fast_loop; 		// Delta Time in miliseconds
+////////////////////////////////////////////////////////////////////////////////
+// Time in miliseconds of start of main control loop.  Milliseconds
+static unsigned long 	fast_loopTimer;
+// Time Stamp when fast loop was complete.  Milliseconds
+static unsigned long 	fast_loopTimeStamp;
+// Number of milliseconds used in last main loop cycle
+static uint8_t 		delta_ms_fast_loop;
+// Counter of main loop executions.  Used for performance monitoring and failsafe processing
 static uint16_t			mainLoop_count;
 
-static unsigned long 	medium_loopTimer;			// Time in miliseconds of medium loop
-static byte 			medium_loopCounter;			// Counters for branching from main control loop to slower loops
+// Time in miliseconds of start of medium control loop.  Milliseconds
+static unsigned long 	medium_loopTimer;
+// Counters for branching from main control loop to slower loops
+static byte 			medium_loopCounter;	
+// Number of milliseconds used in last medium loop cycle
 static uint8_t			delta_ms_medium_loop;
 
+// Counters for branching from medium control loop to slower loops
 static byte 			slow_loopCounter;
+// Counter to trigger execution of very low rate processes
 static byte 			superslow_loopCounter;
-static byte			counter_one_herz;
+// Counter to trigger execution of 1 Hz processes
+static byte				counter_one_herz;
 
-static unsigned long 	nav_loopTimer;				// used to track the elapsed time for GPS nav
+// used to track the elapsed time for navigation PID integral terms
+static unsigned long 	nav_loopTimer;				
+// Elapsed time since last call to navigation pid functions
+static unsigned long 	dTnav;
+// % MCU cycles used
+static float 			load;
 
-static unsigned long 	dTnav;						// Delta Time in milliseconds for navigation computations
-static float 			load;						// % MCU cycles used
-
-AP_Relay relay;
-
-// Camera/Antenna mount tracking and stabilisation stuff
-// --------------------------------------
-#if MOUNT == ENABLED
-AP_Mount camera_mount(g_gps, &dcm);
-#endif
 
 
 ////////////////////////////////////////////////////////////////////////////////

ArduPlane/GCS_Mavlink.pde

@@ -206,11 +206,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
     uint16_t battery_current = -1;
     uint8_t  battery_remaining = -1;
 
-    if (current_total != 0 && g.pack_capacity != 0) {
-        battery_remaining = (100.0 * (g.pack_capacity - current_total) / g.pack_capacity);
+    if (current_total1 != 0 && g.pack_capacity != 0) {
+        battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity);
     }
-    if (current_total != 0) {
-        battery_current = current_amps * 100;
+    if (current_total1 != 0) {
+        battery_current = current_amps1 * 100;
     }
 
     mavlink_msg_sys_status_send(
@@ -219,7 +219,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
         control_sensors_enabled,
         control_sensors_health,
         (uint16_t)(load * 1000),
-        battery_voltage * 1000, // mV
+        battery_voltage1 * 1000, // mV
         battery_current,        // in 10mA units
         battery_remaining,      // in %
         0, // comm drops %,
@@ -270,7 +270,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
         }
 
         uint8_t status 		= MAV_STATE_ACTIVE;
-        uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity;	//Mavlink scaling 100% = 1000
+        uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity;	//Mavlink scaling 100% = 1000
 
         mavlink_msg_sys_status_send(
             chan,
@@ -278,7 +278,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
             nav_mode,
             status,
             load * 1000,
-            battery_voltage * 1000,
+            battery_voltage1 * 1000,
             battery_remaining,
             packet_drops);
 #endif // MAVLINK10

ArduPlane/Log.pde

@@ -386,9 +386,9 @@ static void Log_Write_Current()
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_CURRENT_MSG);
 	DataFlash.WriteInt(g.channel_throttle.control_in);
-	DataFlash.WriteInt((int)(battery_voltage 	* 100.0));
-	DataFlash.WriteInt((int)(current_amps 		* 100.0));
-	DataFlash.WriteInt((int)current_total);
+	DataFlash.WriteInt((int)(battery_voltage1 	* 100.0));
+	DataFlash.WriteInt((int)(current_amps1 		* 100.0));
+	DataFlash.WriteInt((int)current_total1);
 	DataFlash.WriteByte(END_BYTE);
 }
 

ArduPlane/Parameters.h

@@ -330,7 +330,7 @@ public:
     AP_Int32    ground_pressure;
     AP_Int8		compass_enabled;
     AP_Int16    angle_of_attack;
-    AP_Int8		battery_monitoring;	// 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
+    AP_Int8		battery_monitoring;	// 0=disabled, 3=voltage only, 4=voltage and current
     AP_Float	volt_div_ratio;
     AP_Float	curr_amp_per_volt;
     AP_Float	input_voltage;

ArduPlane/commands_logic.pde

@@ -64,20 +64,6 @@ handle_process_condition_command()
 			do_change_alt();
 			break;
 
-	/*	case MAV_CMD_NAV_LAND_OPTIONS:	//    TODO - Add the command or equiv to MAVLink (repair in verify_condition() also)
-			gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set"));
-
-			// pitch in deg, airspeed  m/s, throttle %, track WP 1 or 0
-			landing_pitch 		= next_nav_command.lng * 100;
-			g.airspeed_cruise   =  next_nav_command.alt * 100;
-			g.throttle_cruise   = next_nav_command.lat;
-			landing_distance 	= next_nav_command.p1;
-
-			SendDebug_P("MSG: throttle_cruise = ");
-			SendDebugln(g.throttle_cruise,DEC);
-			break;
-	*/
-
 		default:
 			break;
 	}

ArduPlane/config.h

@@ -87,6 +87,8 @@
 # define PUSHBUTTON_PIN   41
 # define USB_MUX_PIN      -1
 # define CONFIG_RELAY     ENABLED
+# define BATTERY_PIN_1	  0
+# define CURRENT_PIN_1	  1
 #elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
 # define A_LED_PIN        27
 # define B_LED_PIN        26
@@ -97,6 +99,8 @@
 # define PUSHBUTTON_PIN   (-1)
 # define CLI_SLIDER_ENABLED DISABLED
 # define USB_MUX_PIN 23
+# define BATTERY_PIN_1	  1
+# define CURRENT_PIN_1	  2
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -161,10 +165,10 @@
 # endif
 #elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN
 # ifndef CONFIG_PITOT_SOURCE_ANALOG_PIN
-#  define CONFIG_PITOT_SOURCE_ANALOG_PIN AN4
+#  define CONFIG_PITOT_SOURCE_ANALOG_PIN 0
 # endif
 #else
-# warning Invalid value for CONFIG_PITOT_SOURCE, disabling sonar
+# warning Invalid value for CONFIG_PITOT_SOURCE, disabling airspeed
 # undef PITOT_ENABLED
 # define PITOT_ENABLED DISABLED
 #endif
@@ -173,10 +177,9 @@
 # define SONAR_TYPE             MAX_SONAR_LV	// MAX_SONAR_XL,  
 #endif
 
-/* In ArduPlane PITOT usually takes the place of SONAR, but some bits
- * still depend on SONAR.
- */
-#define SONAR_ENABLED PITOT_ENABLED
+#ifndef SONAR_ENABLED
+#define SONAR_ENABLED DISABLED
+#endif
 
 //////////////////////////////////////////////////////////////////////////////
 // HIL_MODE                                 OPTIONAL
@@ -225,12 +228,17 @@
 # define LOW_VOLTAGE			9.6
 #endif
 #ifndef VOLT_DIV_RATIO
-# define VOLT_DIV_RATIO			3.56
+# define VOLT_DIV_RATIO			3.56	// This is the proper value for an on-board APM1 voltage divider with a 3.9kOhm resistor
+//# define VOLT_DIV_RATIO		15.70	// This is the proper value for the AttoPilot 50V/90A sensor
+//# define VOLT_DIV_RATIO		4.127	// This is the proper value for the AttoPilot 13.6V/45A sensor
+
 #endif
 
 #ifndef CURR_AMP_PER_VOLT
-# define CURR_AMP_PER_VOLT		27.32
+# define CURR_AMP_PER_VOLT		27.32	// This is the proper value for the AttoPilot 50V/90A sensor
+//# define CURR_AMP_PER_VOLT	13.66	// This is the proper value for the AttoPilot 13.6V/45A sensor
 #endif
+
 #ifndef CURR_AMPS_OFFSET
 # define CURR_AMPS_OFFSET		0.0
 #endif

ArduPlane/defines.h

@@ -183,18 +183,8 @@ enum gcs_severity {
 
 
 #define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio
-
 #define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
 
-#define	AIRSPEED_CH 7			// The external ADC channel for the airspeed sensor
-#define BATTERY_PIN1 0		        // These are the pins for the voltage dividers
-#define BATTERY_PIN2 1
-#define BATTERY_PIN3 2
-#define BATTERY_PIN4 3
-
-#define VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage
-#define CURRENT_PIN_1 1 // and current
-
 #define RELAY_PIN 47
 
 

ArduPlane/sensors.pde

@@ -106,25 +106,21 @@ static void zero_airspeed(void)
 
 static void read_battery(void)
 {
-	battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
-	battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
-	battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
-	battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
-
-	if(g.battery_monitoring == 1) 
-		battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream	
-	if(g.battery_monitoring == 2) 
-		battery_voltage = battery_voltage4;
+	if(g.battery_monitoring == 0) {
+		battery_voltage1 = 0;
+		return;
+	}
+	
 	if(g.battery_monitoring == 3 || g.battery_monitoring == 4) 
-		battery_voltage = battery_voltage1;
+		battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9;
 	if(g.battery_monitoring == 4) {
-		current_amps	 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
-		current_total	 += current_amps * (float)delta_ms_medium_loop * 0.000278;
+		current_amps1	 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; 	//reads power sensor current pin
+		current_total1	 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778;				// .0002778 is 1/3600 (conversion to hours)
 	}
 	
 	#if BATTERY_EVENT == ENABLED
-		if(battery_voltage < LOW_VOLTAGE)	low_battery_event();
-		if(g.battery_monitoring == 4 && current_total > g.pack_capacity)	low_battery_event();
+		if(battery_voltage1 < LOW_VOLTAGE)	low_battery_event();
+		if(g.battery_monitoring == 4 && current_total1 > g.pack_capacity)	low_battery_event();
 	#endif
 }
 

ArduPlane/setup.pde

@@ -301,8 +301,11 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
 	} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
 		g.compass_enabled = false;
 
+	} else if (!strcmp_P(argv[1].str, PSTR("reset"))) {
+		compass.set_offsets(0,0,0);
+
 	} else {
-		Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
+		Serial.printf_P(PSTR("\nOptions:[on,off,reset]\n"));
 		report_compass();
 		return 0;
 	}
@@ -319,7 +322,7 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
 		g.battery_monitoring.set_and_save(argv[1].i);
 
 	} else {
-		Serial.printf_P(PSTR("\nOptions: 0-4"));
+		Serial.printf_P(PSTR("\nOptions: 3-4"));
 	}
 
 	report_batt_monitor();
@@ -336,8 +339,6 @@ static void report_batt_monitor()
 	Serial.printf_P(PSTR("Batt Mointor\n"));
 	print_divider();
 	if(g.battery_monitoring == 0)	Serial.printf_P(PSTR("Batt monitoring disabled"));
-	if(g.battery_monitoring == 1)	Serial.printf_P(PSTR("Monitoring 3 cell"));
-	if(g.battery_monitoring == 2)	Serial.printf_P(PSTR("Monitoring 4 cell"));
 	if(g.battery_monitoring == 3)	Serial.printf_P(PSTR("Monitoring batt volts"));
 	if(g.battery_monitoring == 4)	Serial.printf_P(PSTR("Monitoring volts and current"));
 	print_blanks(2);

ArduPlane/test.pde

@@ -14,7 +14,6 @@ static int8_t	test_adc(uint8_t argc, 			const Menu::arg *argv);
 #endif
 static int8_t	test_imu(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_battery(uint8_t argc, 		const Menu::arg *argv);
-static int8_t	test_current(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_relay(uint8_t argc,	 	const Menu::arg *argv);
 static int8_t	test_wp(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_airspeed(uint8_t argc, 	const Menu::arg *argv);
@@ -34,18 +33,18 @@ static int8_t	test_logging(uint8_t argc, 		const Menu::arg *argv);
 // User enters the string in the console to call the functions on the right.
 // See class Menu in AP_Common for implementation details
 static const struct Menu::command test_menu_commands[] PROGMEM = {
-	{"pwm",			test_radio_pwm},
-	{"radio",		test_radio},
-	{"passthru",	test_passthru},
-	{"failsafe",	test_failsafe},
-	{"battery",		test_battery},
-	{"relay",		test_relay},
-	{"waypoints",	test_wp},
-	{"xbee",		test_xbee},
-	{"eedump",		test_eedump},
-	{"modeswitch",	test_modeswitch},
+	{"pwm",				test_radio_pwm},
+	{"radio",			test_radio},
+	{"passthru",		test_passthru},
+	{"failsafe",		test_failsafe},
+	{"battery",	test_battery},
+	{"relay",			test_relay},
+	{"waypoints",		test_wp},
+	{"xbee",			test_xbee},
+	{"eedump",			test_eedump},
+	{"modeswitch",		test_modeswitch},
 #if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
-	{"dipswitches",	test_dipswitches},
+	{"dipswitches",		test_dipswitches},
 #endif
 
 	// Tests below here are for hardware sensors only present
@@ -60,7 +59,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
 	{"airspeed",	test_airspeed},
 	{"airpressure",	test_pressure},
 	{"compass",		test_mag},
-	{"current",		test_current},
 #elif HIL_MODE == HIL_MODE_SENSORS
 	{"adc", 		test_adc},
 	{"gps",			test_gps},
@@ -260,26 +258,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_battery(uint8_t argc, const Menu::arg *argv)
 {
-if (g.battery_monitoring >=1 && g.battery_monitoring < 4) {
-	for (int i = 0; i < 80; i++){	//  Need to get many samples for filter to stabilize
-		delay(20);
-		read_battery();
-	}
-	Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"),
-			battery_voltage1,
-			battery_voltage2,
-			battery_voltage3,
-			battery_voltage4);
-} else {
-	Serial.printf_P(PSTR("Not enabled\n"));
-}
-	return (0);
-}
-
-static int8_t
-test_current(uint8_t argc, const Menu::arg *argv)
-{
-if (g.battery_monitoring == 4) {
+if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
 	print_hit_enter();
 	delta_ms_medium_loop = 100;
 
@@ -287,10 +266,17 @@ if (g.battery_monitoring == 4) {
 		delay(100);
 		read_radio();
 		read_battery();
-		Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
-						battery_voltage,
-						current_amps,
-						current_total);
+		if (g.battery_monitoring == 3){
+			Serial.printf_P(PSTR("V: %4.4f\n"),
+						battery_voltage1,
+						current_amps1,
+						current_total1);
+		} else {
+			Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
+						battery_voltage1,
+						current_amps1,
+						current_total1);
+		}
 
 		// write out the servo PWM values
 		// ------------------------------