HAL_SITL: initial support for internal simulators

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -22,6 +22,8 @@
 #include <AP_Param.h>
 #include <pthread.h>
 
+#include <SIM_Multicopter.h>
+
 typedef void *(*pthread_startroutine_t)(void *);
 
 #ifdef __CYGWIN__
@@ -75,6 +77,8 @@ void SITL_State::_usage(void)
 void SITL_State::_parse_command_line(int argc, char * const argv[])
 {
 	int opt;
+    const char *home_str = NULL;
+    const char *model_str = NULL;
 
 	signal(SIGFPE, _sig_fpe);
 	// No-op SIGPIPE handler
@@ -88,7 +92,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
     _rcout_port = 5502;
     _simin_port = 5501;
 
-	while ((opt = getopt(argc, argv, "swhr:H:CI:P:S")) != -1) {
+	while ((opt = getopt(argc, argv, "swhr:H:CI:P:SO:M:")) != -1) {
 		switch (opt) {
 		case 'w':
 			AP_Param::erase_all();
@@ -116,12 +120,23 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
 		case 'S':
             _synthetic_clock_mode = true;
 			break;
+        case 'O':
+            home_str = optarg;
+            break;
+        case 'M':
+            model_str = optarg;
+            break;
 		default:
 			_usage();
 			exit(1);
 		}
 	}
 
+    if (model_str && home_str) {
+        sitl_model = new MultiCopter(home_str, model_str);
+        printf("Started model %s at %s\n", model_str, home_str);
+    }
+
 	fprintf(stdout, "Starting sketch '%s'\n", SKETCH);
 
 	if (strcmp(SKETCH, "ArduCopter") == 0) {
@@ -292,25 +307,29 @@ void SITL_State::_fdm_thread(void)
             next_stop_clock = 0;
         }
 
-        tv.tv_sec = 1;
+        if (sitl_model != NULL) {
-        tv.tv_usec = 0;
+            _fdm_input_local();
+        } else {
+            tv.tv_sec = 1;
+            tv.tv_usec = 0;
 
-        FD_ZERO(&fds);
+            FD_ZERO(&fds);
-        FD_SET(_sitl_fd, &fds);
+            FD_SET(_sitl_fd, &fds);
-        if (select(_sitl_fd+1, &fds, NULL, NULL, &tv) != 1) {
+            if (select(_sitl_fd+1, &fds, NULL, NULL, &tv) != 1) {
-            // internal error
+                // internal error
-            _simulator_output(true);
+                _simulator_output(true);
-            continue;
+                continue;
+            }
+
+            /* check for packet from flight sim */
+            _fdm_input();
         }
 
-        /* check for packet from flight sim */
-        _fdm_input();
-
         /* make sure we die if our parent dies */
         if (kill(_parent_pid, 0) != 0) {
             exit(1);
         }
-
+        
         if (_scheduler->interrupts_are_blocked() || _sitl == NULL) {
             continue;
         }
@@ -372,10 +391,6 @@ void SITL_State::_fdm_input(void)
 		uint16_t pwm[8];
 	};
 	union {
-        struct {
-            uint64_t timestamp;
-            struct sitl_fdm fg_pkt;
-        } fg_pkt_timestamped;
 		struct sitl_fdm fg_pkt;
 		struct pwm_packet pwm_pkt;
 	} d;
@@ -385,13 +400,6 @@ void SITL_State::_fdm_input(void)
 	size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT);
 	switch (size) {
     case 148:
-        /* a fg packate with a timestamp */
-        next_stop_clock = d.fg_pkt_timestamped.timestamp;
-        memmove(&d.fg_pkt, &d.fg_pkt_timestamped.fg_pkt, sizeof(d.fg_pkt));
-        _synthetic_clock_mode = true;
-        // fall through
-
-	case 140:
 		static uint32_t last_report;
 		static uint32_t count;
 
@@ -400,6 +408,8 @@ void SITL_State::_fdm_input(void)
 			return;
 		}
 
+        next_stop_clock = d.fg_pkt.timestamp_us;
+        _synthetic_clock_mode = true;
         got_fg_input = true;
 
 		if (d.fg_pkt.latitude == 0 ||
@@ -447,6 +457,22 @@ void SITL_State::_fdm_input(void)
         _simulator_output(_synthetic_clock_mode);
     }
 }
+
+
+/*
+  get FDM input from a local model
+ */
+void SITL_State::_fdm_input_local(void)
+{
+    Aircraft::sitl_input input;
+
+    _simulator_servos(input);
+    sitl_model->update(input);
+    sitl_model->fill_fdm(_sitl->state);
+    next_stop_clock = _sitl->state.timestamp_us;
+    _synthetic_clock_mode = true;
+    _update_count++;
+}
 #endif
 
 /*
@@ -476,15 +502,12 @@ void SITL_State::_apply_servo_filter(float deltat)
 
 
 /*
-  send RC outputs to simulator
+  create sitl_input structure for sending to FDM
  */
-void SITL_State::_simulator_output(bool synthetic_clock_mode)
+void SITL_State::_simulator_servos(Aircraft::sitl_input &input)
 {
-	static uint32_t last_update_usec;
+    static uint32_t last_update_usec;
-	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 */
@@ -507,15 +530,8 @@ void SITL_State::_simulator_output(bool synthetic_clock_mode)
         }
 	}
 
-    if (_sitl == NULL) {
-        return;
-    }
-
 	// output at chosen framerate
     uint32_t now = hal.scheduler->micros();
-	if (!synthetic_clock_mode && last_update_usec != 0 && now - last_update_usec < 1000000/_framerate) {
-		return;
-	}
     float deltat = (now - last_update_usec) * 1.0e-6f;
 	last_update_usec = now;
 
@@ -523,52 +539,75 @@ void SITL_State::_simulator_output(bool synthetic_clock_mode)
 
 	for (i=0; i<11; i++) {
 		if (pwm_output[i] == 0xFFFF) {
-			control.pwm[i] = 0;
+			input.servos[i] = 0;
 		} else {
-			control.pwm[i] = pwm_output[i];
+			input.servos[i] = pwm_output[i];
 		}
         last_pwm_output[i] = pwm_output[i];
 	}
 
 	if (_vehicle == ArduPlane) {
 		// add in engine multiplier
-		if (control.pwm[2] > 1000) {
+		if (input.servos[2] > 1000) {
-			control.pwm[2] = ((control.pwm[2]-1000) * _sitl->engine_mul) + 1000;
+			input.servos[2] = ((input.servos[2]-1000) * _sitl->engine_mul) + 1000;
-			if (control.pwm[2] > 2000) control.pwm[2] = 2000;
+			if (input.servos[2] > 2000) input.servos[2] = 2000;
 		}
-		_motors_on = ((control.pwm[2]-1000)/1000.0f) > 0;
+		_motors_on = ((input.servos[2]-1000)/1000.0f) > 0;
 	} else if (_vehicle == APMrover2) {
 		// add in engine multiplier
-		if (control.pwm[2] != 1500) {
+		if (input.servos[2] != 1500) {
-			control.pwm[2] = ((control.pwm[2]-1500) * _sitl->engine_mul) + 1500;
+			input.servos[2] = ((input.servos[2]-1500) * _sitl->engine_mul) + 1500;
-			if (control.pwm[2] > 2000) control.pwm[2] = 2000;
+			if (input.servos[2] > 2000) input.servos[2] = 2000;
-			if (control.pwm[2] < 1000) control.pwm[2] = 1000;
+			if (input.servos[2] < 1000) input.servos[2] = 1000;
 		}
-		_motors_on = ((control.pwm[2]-1500)/500.0f) != 0;
+		_motors_on = ((input.servos[2]-1500)/500.0f) != 0;
 	} else {
 		_motors_on = false;
         // apply engine multiplier to first motor
-        control.pwm[0] = ((control.pwm[0]-1000) * _sitl->engine_mul) + 1000;
+        input.servos[0] = ((input.servos[0]-1000) * _sitl->engine_mul) + 1000;
         // run checks on each motor
 		for (i=0; i<4; i++) {
             // check motors do not exceed their limits
-            if (control.pwm[i] > 2000) control.pwm[i] = 2000;
+            if (input.servos[i] > 2000) input.servos[i] = 2000;
-            if (control.pwm[i] < 1000) control.pwm[i] = 1000;
+            if (input.servos[i] < 1000) input.servos[i] = 1000;
             // update motor_on flag
-			if ((control.pwm[i]-1000)/1000.0f > 0) {
+			if ((input.servos[i]-1000)/1000.0f > 0) {
                 _motors_on = true;
 			}
 		}
 	}
 
-    float throttle = _motors_on?(control.pwm[2]-1000) / 1000.0f:0;
+    float throttle = _motors_on?(input.servos[2]-1000) / 1000.0f:0;
     // lose 0.7V at full throttle
     float voltage = _sitl->batt_voltage - 0.7f*throttle;
+
     // assume 50A at full throttle
     _current = 50.0f * throttle;
+
     // assume 3DR power brick
     voltage_pin_value = ((voltage / 10.1f) / 5.0f) * 1024;
     current_pin_value = ((_current / 17.0f) / 5.0f) * 1024;
+}
+
+
+/*
+  send RC outputs to simulator
+ */
+void SITL_State::_simulator_output(bool synthetic_clock_mode)
+{
+	struct {
+		uint16_t pwm[11];
+		uint16_t speed, direction, turbulance;
+	} control;
+    Aircraft::sitl_input input;
+
+    _simulator_servos(input);
+
+    if (_sitl == NULL) {
+        return;
+    }
+
+    memcpy(control.pwm, input.servos, sizeof(control.pwm));
 
 	// setup wind control
     float wind_speed = _sitl->wind_speed * 100;

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -22,6 +22,7 @@
 #include "../AP_OpticalFlow/AP_OpticalFlow.h"
 #include "../AP_Terrain/AP_Terrain.h"
 #include "../SITL/SITL.h"
+#include "../SITL/SIM_Multicopter.h"
 
 class HAL_AVR_SITL;
 
@@ -101,6 +102,8 @@ private:
 			    double xAccel, 	double yAccel, 	double zAccel,		// Local to plane
 			    float airspeed,	float altitude);
     void _fdm_input(void);
+    void _fdm_input_local(void);
+    void _simulator_servos(Aircraft::sitl_input &input);
     void _simulator_output(bool synthetic_clock_mode);
     void _apply_servo_filter(float deltat);
     uint16_t _airspeed_sensor(float airspeed);
@@ -176,6 +179,9 @@ private:
     VectorN<readings_baro,baro_buffer_length> buffer_baro;
     uint32_t time_delta_baro;
     uint32_t delayed_time_baro;
+
+    // internal SITL model
+    Aircraft *sitl_model;
 };
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL