SITL: added SIM_WIND_* parameters

this allows control of the simulated wind during a flight

Tools/autotest/jsbsim/runsim.py

@@ -53,7 +53,15 @@ def setup_template(home):
 
 def process_sitl_input(buf):
     '''process control changes from SITL sim'''
-    pwm = list(struct.unpack('<11H', buf))
+    control = list(struct.unpack('<14H', buf))
+    pwm = control[:11]
+    (speed, direction, turbulance) = control[11:]
+
+    global wind
+    wind.speed      = speed*0.01
+    wind.direction  = direction*0.01
+    wind.turbulance = turbulance*0.01
+    
     aileron  = (pwm[0]-1500)/500.0
     elevator = (pwm[1]-1500)/500.0
     throttle = (pwm[2]-1000)/1000.0
@@ -237,7 +245,7 @@ def main_loop():
             frame_count += 1
 
         if sim_in.fileno() in rin:
-            simbuf = sim_in.recv(22)
+            simbuf = sim_in.recv(28)
             process_sitl_input(simbuf)
             last_sim_input = tnow
 

Tools/autotest/pysim/sim_multicopter.py

@@ -56,18 +56,29 @@ def sim_send(m, a):
 def sim_recv(m):
     '''receive control information from SITL'''
     try:
-        buf = sim_in.recv(22)
+        buf = sim_in.recv(28)
     except socket.error as e:
         if not e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
             raise
         return
         
-    if len(buf) != 22:
+    if len(buf) != 28:
         return
-    pwm = list(struct.unpack('<11H', buf))
+    control = list(struct.unpack('<14H', buf))
+    pwm = control[0:11]
+
+    # update motors
     for i in range(11):
         m[i] = (pwm[i]-1000)/1000.0
 
+    # update wind
+    global a
+    (speed, direction, turbulance) = control[11:]
+    a.wind.speed = speed*0.01
+    a.wind.direction = direction*0.01
+    a.wind.turbulance = turbulance*0.01
+    
+
 
 def interpret_address(addrstr):
     '''interpret a IP:port string'''

libraries/Desktop/support/sitl.cpp

@@ -153,7 +153,10 @@ float sitl_motor_speed[4] = {0,0,0,0};
 static void sitl_simulator_output(void)
 {
 	static uint32_t last_update;
-	uint16_t pwm[11];
+	struct {
+		uint16_t pwm[11];
+		uint16_t speed, direction, turbulance;
+	} control;
 	/* this maps the registers used for PWM outputs. The RC
 	 * driver updates these whenever it wants the channel output
 	 * to change */
@@ -180,29 +183,38 @@ static void sitl_simulator_output(void)
 
 	for (i=0; i<11; i++) {
 		if (*reg[i] == 0xFFFF) {
-			pwm[i] = 0;
+			control.pwm[i] = 0;
 		} else {
-			pwm[i] = (*reg[i])/2;
+			control.pwm[i] = (*reg[i])/2;
 		}
 	}
 
 	if (!desktop_state.quadcopter) {
 		// add in engine multiplier
-		if (pwm[2] > 1000) {
+		if (control.pwm[2] > 1000) {
-			pwm[2] = ((pwm[2]-1000) * sitl.engine_mul) + 1000;
+			control.pwm[2] = ((control.pwm[2]-1000) * sitl.engine_mul) + 1000;
-			if (pwm[2] > 2000) pwm[2] = 2000;
+			if (control.pwm[2] > 2000) control.pwm[2] = 2000;
 		}
 
 		// 400kV motor, 16V
-		sitl_motor_speed[0] = ((pwm[2]-1000)/1000.0) * 400 * 16 / 60.0;
+		sitl_motor_speed[0] = ((control.pwm[2]-1000)/1000.0) * 400 * 16 / 60.0;
 	} else {
 		// 850kV motor, 16V
 		for (i=0; i<4; i++) {
-			sitl_motor_speed[i] = ((pwm[i]-1000)/1000.0) * 850 * 12 / 60.0;
+			sitl_motor_speed[i] = ((control.pwm[i]-1000)/1000.0) * 850 * 12 / 60.0;
 		}
 	}
 
-	sendto(sitl_fd, (void*)pwm, sizeof(pwm), MSG_DONTWAIT, (const sockaddr *)&rcout_addr, sizeof(rcout_addr));
+	// setup wind control
+	control.speed      = sitl.wind_speed * 100;
+	float direction = sitl.wind_direction;
+	if (direction < 0) {
+		direction += 360;
+	}
+	control.direction  = direction * 100;
+	control.turbulance = sitl.wind_turbulance * 100;
+
+	sendto(sitl_fd, (void*)&control, sizeof(control), MSG_DONTWAIT, (const sockaddr *)&rcout_addr, sizeof(rcout_addr));
 }
 
 /*

libraries/SITL/SITL.cpp

@@ -24,6 +24,9 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
     AP_GROUPINFO("DRIFT_TIME", 6, SITL,  drift_time,  5),
     AP_GROUPINFO("GPS_DELAY",  7, SITL,  gps_delay,   4),
     AP_GROUPINFO("ENGINE_MUL", 8, SITL,  engine_mul,  1),
+    AP_GROUPINFO("WIND_SPD",   9, SITL,  wind_speed,  5),
+    AP_GROUPINFO("WIND_DIR",  10, SITL,  wind_direction,  180),
+    AP_GROUPINFO("WIND_TURB", 11, SITL,  wind_turbulance,  0.2),
     AP_GROUPEND
 };
 

libraries/SITL/SITL.h

@@ -27,16 +27,6 @@ struct sitl_fdm {
 class SITL
 {
 public:
-	SITL() {
-		baro_noise  = 3;   // Pascals
-		gyro_noise  = 30;   // degrees/s
-		accel_noise = 3;    // m/s/s
-		mag_noise   = 10;   // mag units
-		aspd_noise  = 2;    // m/s
-		drift_speed = 0.2;  // dps/min
-		drift_time  = 5;    // minutes
-        gps_delay   = 4;    // 0.8 seconds
-	}
 	struct sitl_fdm state;
 
 	static const struct AP_Param::GroupInfo var_info[];
@@ -54,6 +44,11 @@ public:
 	AP_Int8  gps_disable; // disable simulated GPS
 	AP_Int8  gps_delay;   // delay in samples
 
+    // wind control
+    AP_Float wind_speed;
+    AP_Float wind_direction;
+    AP_Float wind_turbulance;
+    
 	void simstate_send(mavlink_channel_t chan);
 
 	// convert a set of roll rates from earth frame to body frame