SITL: Wire in basic support for SITL downward-looking sonar sensors

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -178,7 +178,7 @@ void SITL_State::_sitl_setup(void)
     if (_sitl != NULL) {
         // setup some initial values
         _update_barometer(_initial_height);
-        _update_ins(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0);
+        _update_ins(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0, _initial_height);
         _update_compass(0, 0, 0);
         _update_gps(0, 0, 0, 0, 0, 0, false);
     }
@@ -294,7 +294,7 @@ void SITL_State::_timer_handler(int signum)
         _update_ins(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg,
                     _sitl->state.rollRate, _sitl->state.pitchRate, _sitl->state.yawRate,
                     _sitl->state.xAccel, _sitl->state.yAccel, _sitl->state.zAccel,
-                    _sitl->state.airspeed);
+                    _sitl->state.airspeed, _sitl->state.altitude);
         _update_barometer(_sitl->state.altitude);
         _update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg);
     }

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -89,11 +89,12 @@ private:
     static void _update_ins(float roll, 	float pitch, 	float yaw,		// Relative to earth
 			    double rollRate, 	double pitchRate,double yawRate,	// Local to plane
 			    double xAccel, 	double yAccel, 	double zAccel,		// Local to plane
-			    float airspeed);
+			    float airspeed,	float altitude);
     static void _fdm_input(void);
     static void _simulator_output(void);
     static void _apply_servo_filter(float deltat);
     static uint16_t _airspeed_sensor(float airspeed);
+    static uint16_t _ground_sonar(float altitude);
     static float _gyro_drift(void);
     static float _rand_float(void);
     static Vector3f _rand_vec3f(void);

libraries/AP_HAL_AVR_SITL/sitl_ins.cpp

@@ -17,12 +17,14 @@
 #include <AP_Math.h>
 #include "../AP_Compass/AP_Compass.h"
 #include "../AP_Declination/AP_Declination.h"
+#include "../AP_RangeFinder/AP_RangeFinder.h"
 #include "../SITL/SITL.h"
 #include "Scheduler.h"
 #include <AP_Math.h>
 #include "../AP_ADC/AP_ADC.h"
 #include <SITL_State.h>
 
+
 using namespace AVR_SITL;
 
 /*
@@ -54,6 +56,25 @@ float SITL_State::_gyro_drift(void)
 
 }
 
+uint16_t SITL_State::_ground_sonar(float altitude)
+{
+	static float home_alt = -1;
+	// TODO Find the current sonar object and load these params from it
+	// rather than assuming XL type
+	float scaler = AP_RANGEFINDER_MAXSONARXL_SCALER;
+
+	if (home_alt == -1)
+		home_alt = altitude;
+
+	altitude = altitude - home_alt;
+	altitude = constrain_float(altitude,
+		AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE / 100.0f,
+		AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE / 100.0f);
+
+	// Altitude in in m, scaler relative to cm
+	return (uint16_t)(altitude * 100.0f / scaler);
+}
+
 /*
   setup the INS input channels with new input
 
@@ -84,7 +105,7 @@ float SITL_State::_gyro_drift(void)
 void SITL_State::_update_ins(float roll, 	float pitch, 	float yaw,		// Relative to earth
 			     double rollRate, 	double pitchRate,double yawRate,	// Local to plane
 			     double xAccel, 	double yAccel, 	double zAccel,		// Local to plane
-			     float airspeed)
+			     float airspeed,	float altitude)
 {
 	double p, q, r;
 
@@ -128,7 +149,12 @@ void SITL_State::_update_ins(float roll, 	float pitch, 	float yaw,		// Relative
 	_ins->set_gyro(Vector3f(p, q, r) + _ins->get_gyro_offsets());
 	_ins->set_accel(Vector3f(xAccel, yAccel, zAccel) + _ins->get_accel_offsets());
 
-	airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float()));
+	// Airspeed and Sonar share the same analog pin.  Connection type is
+	// manually selected.  Crude..
+	if(_sitl->sonar_connected)
+		airspeed_pin_value = _ground_sonar(altitude);
+	else
+		airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float()));
 }
 
 #endif

libraries/SITL/SITL.cpp

@@ -48,6 +48,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
     AP_GROUPINFO("ASPD_RND",      20, SITL,  aspd_noise,  0.5),
     AP_GROUPINFO("ACCEL_FAIL",    21, SITL,  accel_fail,  0),
     AP_GROUPINFO("BARO_DRIFT",    22, SITL,  baro_drift,  0),
+    AP_GROUPINFO("SONAR_CON",     23, SITL,  sonar_connected, 0),
     AP_GROUPEND
 };
 

libraries/SITL/SITL.h

@@ -55,6 +55,8 @@ public:
 	AP_Float mag_error;   // in degrees
     AP_Float servo_rate;  // servo speed in degrees/second
 
+    AP_Float sonar_connected; // Analogue sonar connected to AnalogPin 0 (else airspeed)
+
 	AP_Float drift_speed; // degrees/second/minute
 	AP_Float drift_time;  // period in minutes
     AP_Float engine_mul;  // engine multiplier