Index: libUDB/libUDB.h
===================================================================
--- libUDB/libUDB.h	(revision 1692)
+++ libUDB/libUDB.h	(working copy)
@@ -135,6 +135,10 @@
 } UDB_SKIP_FLAGS;
 
 extern UDB_SKIP_FLAGS udb_skip_flags;
+#endif
+
+#if (USE_PPM_INPUT == 1)
+extern int udb_pwm_sonar ;
 #endif
 
 // Implement this callback to prepare the pwOut values.
Index: libUDB/radioIn_udb4.c
===================================================================
--- libUDB/radioIn_udb4.c	(revision 1692)
+++ libUDB/radioIn_udb4.c	(working copy)
@@ -34,6 +34,11 @@
 int udb_pwIn[NUM_INPUTS+1] ;	// pulse widths of radio inputs
 int udb_pwTrim[NUM_INPUTS+1] ;	// initial pulse widths for trimming
 
+#if ( USE_SONAR_ON_PWM_INPUT_8	== 1 )
+int udb_pwm_sonar  ;		// pulse width of sonar signal
+int udb_pwm_sonar_rise ;
+#endif
+
 int failSafePulses = 0 ;
 
 
@@ -78,14 +83,45 @@
 	_TRISD8 = 1 ;
 	_IC1IP = 6 ;
 	_IC1IF = 0 ;
+
+#if ( USE_SONAR_ON_PWM_INPUT_8	== 1 )
+	// Setup Channel 8 for Sonar
+    // Sonar PWM Pulses are at 58 micro seconds per cm measured. Maximum is 765 cm. So Max Pulse is 44370 micro seconds.
+    // Clock of timer is running at 16,000,000 Hz. So Max Sonar Pulse is 16000000 * 0.044379 clock pulses whih is 710064 pulses. 
+    // If prescales of the timer is set to 64, then maxumum sonar measurement within matrixPIlot is 710064 / 64 = 11095.
+    // If minimum reading is 0.2 meters, then minimum PWM is  (20 * 58) = 1160 micro seconds. So the
+    // minimum integer in MatrixPilot should then be (16000000 * 0.001160) / 64 = 290
+	// Each unit of UDB PWM sonar pulse is 64 / 16000000 seconds which is 0.000004 seconds in length.
+    // Therefore each centimeter of measured distance will show 0.000058 / 0.000004 or 58 or 14.5 UDB PWM sonar units / centimeter.
+
+	TMR3 = 0 ; 				// initialize timer
+	T3CONbits.TCKPS = 2 ;	// prescaler = 64,  see page 175 at http://ww1.microchip.com/downloads/en/DeviceDoc/70593C.pdf
+	T3CONbits.TCS = 0 ;		// use the internal clock
+	T3CONbits.TON = 1 ;		// turn on timer 3
+
+    IC8CONbits.ICTMR = 0 ;  // use timer 3
+	IC8CONbits.ICM = 1 ; // capture every edge
+	_TRISD15 = 1 ;
+	_IC8IP = 6 ;
+	_IC8IF = 0 ; 
+#endif
+    
 	if (NUM_INPUTS > 0) _IC1IE = 1 ;
 	
 #if (USE_PPM_INPUT != 1)
-	IC8CON  = IC7CON  = IC6CON   = IC5CON   = IC4CON   = IC3CON   = IC2CON   = IC1CON ;
-	_TRISD9 = _TRISD10 = _TRISD11 = _TRISD12 = _TRISD13 = _TRISD14 = _TRISD15 = _TRISD8 ;
+	IC7CON  = IC6CON   = IC5CON   = IC4CON   = IC3CON   = IC2CON   = IC1CON ;
+	_TRISD9 = _TRISD10 = _TRISD11 = _TRISD12 = _TRISD13 = _TRISD14 = _TRISD8 ;
+
+#if ( USE_SONAR_ON_PWM_INPUT_8	!= 1 )
+	 _TRISD15 = _TRISD8 ;
+#endif
 	
 	//	set the interrupt priorities to 6
-	_IC2IP = _IC3IP = _IC4IP = _IC5IP = _IC6IP = _IC7IP = _IC8IP = _IC1IP ; 
+	_IC2IP = _IC3IP = _IC4IP = _IC5IP = _IC6IP = _IC7IP  = _IC1IP ;
+
+#if ( USE_SONAR_ON_PWM_INPUT_8	!= 1 )
+	 _IC8IP = _IC1IP ;
+#endif 
 	
 	//	clear the interrupts:
 	_IC2IF = _IC3IF = _IC4IF = _IC5IF = _IC6IF = _IC7IF = _IC8IF = _IC1IF ;
@@ -97,7 +133,7 @@
 	if (NUM_INPUTS > 4) _IC5IE = 1 ; 
 	if (NUM_INPUTS > 5) _IC6IE = 1 ; 
 	if (NUM_INPUTS > 6) _IC7IE = 1 ; 
-	if (NUM_INPUTS > 7) _IC8IE = 1 ;
+	if ((NUM_INPUTS > 7) || ( USE_SONAR_ON_PWM_INPUT_8 == 1)) _IC8IE = 1 ;
 #endif
 	
 	return ;
@@ -406,8 +442,9 @@
 	return ;
 }
 
-
+  
 // Input Channel 8
+#if ( USE_SONAR_ON_PWM_INPUT_8	!= 1 )
 void __attribute__((__interrupt__,__no_auto_psv__)) _IC8Interrupt(void)
 {
 	indicate_loading_inter ;
@@ -449,6 +486,32 @@
 	return ;
 }
 
+#else // ( USE_SONAR_ON_PWM_INPUT_8	is True )
+void __attribute__((__interrupt__,__no_auto_psv__)) _IC8Interrupt(void)
+{
+	indicate_loading_inter ;
+	interrupt_save_set_corcon ;
+	
+	unsigned int time ;
+	_IC8IF =  0 ; // clear the interrupt
+	while ( IC8CONbits.ICBNE )
+	{
+		time = IC8BUF ;
+	}
+	if (PORTDbits.RD15)
+	{
+		 udb_pwm_sonar_rise = time ;
+	}
+	else
+	{
+		udb_pwm_sonar = time - udb_pwm_sonar_rise ;	
+		// NOTE: This value should be sensed check for max, min, and "Not Trusted"
+	}	
+	interrupt_restore_corcon ;
+	return ;
+}
+#endif
+
 #else // #if (USE_PPM_INPUT == 1)
 
 #if (PPM_SIGNAL_INVERTED == 1)
Index: MatrixPilot/config_tests.c
===================================================================
--- MatrixPilot/config_tests.c	(revision 1692)
+++ MatrixPilot/config_tests.c	(working copy)
@@ -215,6 +215,11 @@
 	#error("ANALOG_RSSI_INPUT_CHANNEL > NUM_ANALOG_INPUTS.")
 #endif
 
+// Sonar settings
+#if (USE_PPM_INPUT != 1 && NUM_INPUTS > 7 && USE_SONAR_ON_PWM_INPUT_8 == 1 )
+	#error("SONAR INPUT on PWM INPUT 8 in Conflict with NUMBER of PWM NUM_INPUTS which is 8 or more")
+#endif
+
 // Check Magnetometer Options
 #if ( MAG_YAW_DRIFT == 1 )
 #ifdef MAG_DIRECT
Index: MatrixPilot/options.h
===================================================================
--- MatrixPilot/options.h	(revision 1692)
+++ MatrixPilot/options.h	(working copy)
@@ -369,7 +370,37 @@
 #define RSSI_MIN_SIGNAL_VOLTAGE				0.5		// Voltage when RSSI should show 0%
 #define RSSI_MAX_SIGNAL_VOLTAGE				3.3		// Voltage when RSSI should show 100%
 
+////////////////////////////////////////////////////////////////////////////////
+// MAXBOTIX SONAR TERRAIN FOLLOWING AND LANDING FLARE
+// Designed for use with the following device:-
+// http://www.maxbotix.com/Ultrasonic_Sensors/MB1230.htm
+// Can be used on INPUT 8 of the UDB4 if that is not used for a channel input.
+// Will return distance to ground in meters and compensate for roll subject to 
+// receiving a returned sonar signal.
+// 
+// Currently works with UDB4 only
+// Height is returned 4 times / second in centimers in SERIAL_UDB_EXTRA PWM channel 8.
+// 
 
+// NOTE: Requires Auto Pitch PID adjustment to be enabled.
+// Auto Pitch PID will auto adjust your plane for the best possible PID control of height.
+
+#define AUTO_ADJUST_PITCH_PID					1
+
+#define USE_SONAR_ON_PWM_INPUT_8				1
+
+// SONAR_STABILIZED_TERRAIN_FOLLOWING
+// In stabilized mode, the plane will attempt to stay above a minimum sonar height from ground
+// using Throttle and Pitch. 
+#define SONAR_STABILIZED_TERRAIN_FOLLOWING 		1
+#define SONAR_STABILIZED_MINIMUM_HEIGHT			2.0  // Meters. Minimum 1.0, Maximum 6.0
+
+// SONAR LANDING FLARE
+#define SONAR_LANDING_FLARE 			0
+#define SONAR_FLARE_DESCENT_RATE		0.3	// Meters / Second. 0.3 is default value.
+#define SONAR_FLARE_START_HEIGHT		4.0 // Meters. Minimum 1.0, Maximum 6.0. Start High !
+
+
 ////////////////////////////////////////////////////////////////////////////////
 // Trigger Action
 // Use the trigger to do things like drop an item at a certain waypoint, or take a photo every

Index: MatrixPilot/telemetry.c
===================================================================
--- MatrixPilot/telemetry.c	(revision 1692)
+++ MatrixPilot/telemetry.c	(working copy)
@@ -707,7 +707,34 @@
 	return ;
 }
 
+#elif ( SERIAL_OUTPUT_FORMAT == SERIAL_SONAR )
 
+extern int udb_pwm_sonar ;
+#define UDB_SONAR_PWM_UNITS_TO_CENTIMETERS	 4451  // 64536.0 / 14.5
+fractional z_vector[] = {0 , 0 , RMAX} ;
+fractional cos_pitch_roll ;
+
+
+void serial_output_8hz( void )
+{
+	union longbbbb accum ;
+	int sonar_distance ; // distance to object regardless of orientation
+	int sonar_height_to_ground ; // distance to object in Earth's Z Plane
+	accum.WW = __builtin_mulss( udb_pwm_sonar, UDB_SONAR_PWM_UNITS_TO_CENTIMETERS ) + 32768 ;
+	sonar_distance = accum._.W1 ;
+	
+	cos_pitch_roll = VectorDotProduct( 3 , &rmat[6] , z_vector )<<1	;
+	if ( cos_pitch_roll > 16383 )
+	{
+		cos_pitch_roll = 16383 ;
+	}
+    cos_pitch_roll = cos_pitch_roll << 1 ;
+	accum.WW = __builtin_mulss(cos_pitch_roll, sonar_distance) ;
+	sonar_height_to_ground = accum._.W1 << 1 ;
+	serial_output("%i,%i,%i\r\n",sonar_distance, cos_pitch_roll, sonar_height_to_ground) ;
+	return ;
+}
+ 
 #else // If SERIAL_OUTPUT_FORMAT is set to SERIAL_NONE, or is not set
 
 void serial_output_8hz( void )
