SITL: fill in a lot more of the AP_HAL SITL backend

libraries/AP_HAL_AVR_SITL/AnalogIn.cpp

@@ -0,0 +1,43 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+
+#include "HAL_AVR.h"
+#include "AP_HAL_AVR_SITL.h"
+#include "AnalogIn.h"
+#include <stdint.h>
+
+using namespace AVR_SITL;
+
+extern const AP_HAL::HAL& hal;
+
+ADCSource::ADCSource(uint8_t pin, float prescale) :
+    _prescale(prescale)
+{}
+
+float ADCSource::read_average() {
+	return 0;
+}
+
+float ADCSource::read_latest() {
+	return 0;
+}
+
+void ADCSource::set_pin(uint8_t pin) {
+}
+
+SITLAnalogIn::SITLAnalogIn() {}
+
+void SITLAnalogIn::init(void *ap_hal_scheduler) {
+}
+
+AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t n) {
+    return NULL;
+}
+
+AP_HAL::AnalogSource* SITLAnalogIn::channel(int16_t n, float prescale) {
+    return NULL;	
+}
+
+#endif

libraries/AP_HAL_AVR_SITL/AnalogIn.h

@@ -0,0 +1,41 @@
+
+#ifndef __AP_HAL_AVR_SITL_ANALOG_IN_H__
+#define __AP_HAL_AVR_SITL_ANALOG_IN_H__
+
+#include <AP_HAL.h>
+#include "AP_HAL_AVR_SITL_Namespace.h"
+
+#define SITL_INPUT_MAX_CHANNELS 12
+
+class AVR_SITL::ADCSource : public AP_HAL::AnalogSource {
+public:
+    friend class AVR_SITL::SITLAnalogIn;
+    /* pin designates the ADC input number, or when == AVR_ANALOG_PIN_VCC,
+     * board vcc */
+    ADCSource(uint8_t pin, float prescale = 1.0);
+
+    /* implement AnalogSource virtual api: */
+    float read_average();
+    float read_latest();
+    void set_pin(uint8_t p);
+
+private:
+    /* prescale scales the raw measurments for read()*/
+    const float _prescale;
+};
+
+/* AVRAnalogIn : a concrete class providing the implementations of the 
+ * timer event and the AP_HAL::AnalogIn interface */
+class AVR_SITL::SITLAnalogIn : public AP_HAL::AnalogIn {
+public:
+    SITLAnalogIn();
+    void init(void* ap_hal_scheduler);
+    AP_HAL::AnalogSource* channel(int16_t n);
+    AP_HAL::AnalogSource* channel(int16_t n, float prescale);
+
+private:
+    static ADCSource* _channels[SITL_INPUT_MAX_CHANNELS];
+
+};
+
+#endif // __AP_HAL_AVR_SITL_ANALOG_IN_H__

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -0,0 +1,452 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_AVR_SITL.h>
+#include "AP_HAL_AVR_SITL_Namespace.h"
+#include "HAL_AVR_SITL_Class.h"
+#include "UARTDriver.h"
+#include "Scheduler.h"
+
+#include <stdio.h>
+#include <signal.h>
+#include <getopt.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <errno.h>
+
+#include <AP_Param.h>
+
+extern const AP_HAL::HAL& hal;
+
+// emulate RC input
+struct RC_ICR4 ICR4;
+
+using namespace AVR_SITL;
+
+enum SITL_State::vehicle_type SITL_State::_vehicle;
+uint16_t SITL_State::_framerate;
+struct sockaddr_in SITL_State::_rcout_addr;
+pid_t SITL_State::_parent_pid;
+uint32_t SITL_State::_update_count;
+bool SITL_State::_motors_on;
+
+AP_Baro_BMP085_HIL *SITL_State::_barometer;
+AP_InertialSensor_Stub *SITL_State::_ins;
+SITLScheduler *SITL_State::_scheduler;
+AP_Compass_HIL *SITL_State::_compass;
+
+int SITL_State::_sitl_fd;
+SITL *SITL_State::_sitl;
+uint16_t SITL_State::_pwm_output[11];
+
+// catch floating point exceptions
+void SITL_State::_sig_fpe(int signum)
+{
+	printf("ERROR: Floating point exception\n");
+	exit(1);
+}
+
+void SITL_State::_usage(void)
+{
+	printf("Options:\n");
+	printf("\t-w          wipe eeprom and dataflash\n");
+	printf("\t-r RATE     set SITL framerate\n");
+	printf("\t-H HEIGHT   initial barometric height\n");
+	printf("\t-C          use console instead of TCP ports\n");
+}
+
+void SITL_State::_parse_command_line(int argc, char * const argv[])
+{
+	int opt;
+
+	signal(SIGFPE, _sig_fpe);
+
+	while ((opt = getopt(argc, argv, "swhr:H:C")) != -1) {
+		switch (opt) {
+		case 'w':
+			AP_Param::erase_all();
+			unlink("dataflash.bin");
+			break;
+		case 'r':
+			_framerate = (unsigned)atoi(optarg);
+			break;
+		case 'H':
+			_initial_height = atof(optarg);
+			break;
+		case 'C':
+			AVR_SITL::SITLUARTDriver::_console = true;
+			break;
+		default:
+			_usage();
+			exit(1);
+		}
+	}
+
+	printf("Starting sketch '%s'\n", SKETCH);
+
+	if (strcmp(SKETCH, "ArduCopter") == 0) {
+		_vehicle = ArduCopter;
+		if (_framerate == 0) {
+			_framerate = 200;
+		}
+	} else if (strcmp(SKETCH, "APMrover2") == 0) {
+		_vehicle = APMrover2;
+		if (_framerate == 0) {
+			_framerate = 50;
+		}
+		// set right default throttle for rover (allowing for reverse)
+		ICR4.set(2, 1500);
+	} else {
+		_vehicle = ArduPlane;
+		if (_framerate == 0) {
+			_framerate = 50;
+		}
+	}
+
+	_sitl_setup();
+}
+
+
+/*
+  setup for SITL handling
+ */
+void SITL_State::_sitl_setup(void)
+{
+#ifndef __CYGWIN__
+	_parent_pid = getppid();
+#endif
+	_rcout_addr.sin_family = AF_INET;
+	_rcout_addr.sin_port = htons(_rcout_port);
+	inet_pton(AF_INET, "127.0.0.1", &_rcout_addr.sin_addr);
+
+	_setup_timer();
+	_setup_fdm();
+	printf("Starting SITL input\n");
+
+	// find the barometer object if it exists
+	_sitl = (SITL *)AP_Param::find_object("SIM_");
+	_barometer = (AP_Baro_BMP085_HIL *)AP_Param::find_object("GND_");
+	_ins = (AP_InertialSensor_Stub *)AP_Param::find_object("INS_");
+	_compass = (AP_Compass_HIL *)AP_Param::find_object("COMPASS_");
+
+    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_compass(0, 0, 0);
+        _update_gps(0, 0, 0, 0, 0, false);
+    }
+}
+
+
+/*
+  setup a SITL FDM listening UDP port
+ */
+void SITL_State::_setup_fdm(void)
+{
+	int one=1, ret;
+	struct sockaddr_in sockaddr;
+
+	memset(&sockaddr,0,sizeof(sockaddr));
+
+#ifdef HAVE_SOCK_SIN_LEN
+	sockaddr.sin_len = sizeof(sockaddr);
+#endif
+	sockaddr.sin_port = htons(_simin_port);
+	sockaddr.sin_family = AF_INET;
+
+	_sitl_fd = socket(AF_INET, SOCK_DGRAM, 0);
+	if (_sitl_fd == -1) {
+		fprintf(stderr, "SITL: socket failed - %s\n", strerror(errno));
+		exit(1);
+	}
+
+	/* we want to be able to re-use ports quickly */
+	setsockopt(_sitl_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
+
+	ret = bind(_sitl_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
+	if (ret == -1) {
+		fprintf(stderr, "SITL: bind failed on port %u - %s\n",
+			(unsigned)ntohs(sockaddr.sin_port), strerror(errno));
+		exit(1);
+	}
+
+	AVR_SITL::SITLUARTDriver::_set_nonblocking(_sitl_fd);
+}
+
+
+/*
+  timer called at 1kHz
+ */
+void SITL_State::_timer_handler(int signum)
+{
+	static uint32_t last_update_count;
+	static bool in_timer;
+
+	if (in_timer || ((SITLScheduler *)hal.scheduler)->interrupts_are_blocked()) {
+		return;
+	}
+	_scheduler->begin_atomic();
+
+	in_timer = true;
+
+#ifndef __CYGWIN__
+	/* make sure we die if our parent dies */
+	if (kill(_parent_pid, 0) != 0) {
+		exit(1);
+	}
+#else
+    
+	static uint16_t count = 0;
+	static uint32_t last_report;
+        
+    	count++;
+	if (millis() - last_report > 1000) {
+		printf("TH %u cps\n", count);
+		count = 0;
+		last_report = millis();
+	}
+#endif
+
+	/* check for packet from flight sim */
+	_fdm_input();
+
+	// send RC output to flight sim
+	_simulator_output();
+
+	if (_update_count == 0 && _sitl != NULL) {
+		_update_gps(0, 0, 0, 0, 0, false);
+		_scheduler->timer_event();
+		_scheduler->end_atomic();
+		in_timer = false;
+		return;
+	}
+
+	if (_update_count == last_update_count) {
+		_scheduler->timer_event();
+		_scheduler->end_atomic();
+		in_timer = false;
+		return;
+	}
+	last_update_count = _update_count;
+
+    if (_sitl != NULL) {
+        _update_gps(_sitl->state.latitude, _sitl->state.longitude,
+                    _sitl->state.altitude,
+                    _sitl->state.speedN, _sitl->state.speedE, !_sitl->gps_disable);
+        _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);
+        _update_barometer(_sitl->state.altitude);
+        _update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.heading);
+    }
+
+	// trigger all APM timers. We do this last as it can re-enable
+	// interrupts, which can lead to recursion
+	_scheduler->timer_event();
+
+	_scheduler->end_atomic();
+	in_timer = false;
+}
+
+
+/*
+  check for a SITL FDM packet
+ */
+void SITL_State::_fdm_input(void)
+{
+	ssize_t size;
+	struct pwm_packet {
+		uint16_t pwm[8];
+	};
+	union {
+		struct sitl_fdm fg_pkt;
+		struct pwm_packet pwm_pkt;
+	} d;
+
+	size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT);
+	switch (size) {
+	case 132:
+		static uint32_t last_report;
+		static uint32_t count;
+
+		if (d.fg_pkt.magic != 0x4c56414e) {
+			printf("Bad FDM packet - magic=0x%08x\n", d.fg_pkt.magic);
+			return;
+		}
+
+		if (d.fg_pkt.latitude == 0 ||
+		    d.fg_pkt.longitude == 0 ||
+		    d.fg_pkt.altitude <= 0) {
+			// garbage input
+			return;
+		}
+
+		_sitl->state = d.fg_pkt;
+		_update_count++;
+
+		count++;
+		if (hal.scheduler->millis() - last_report > 1000) {
+			//printf("SIM %u FPS\n", count);
+			count = 0;
+			last_report = hal.scheduler->millis();
+		}
+		break;
+
+	case 16: {
+		// a packet giving the receiver PWM inputs
+		uint8_t i;
+		for (i=0; i<8; i++) {
+			// setup the ICR4 register for the RC channel
+			// inputs
+			if (d.pwm_pkt.pwm[i] != 0) {
+				ICR4.set(i, d.pwm_pkt.pwm[i]);
+			}
+		}
+		break;
+	}
+	}
+
+}
+
+/*
+  send RC outputs to simulator
+ */
+void SITL_State::_simulator_output(void)
+{
+	static uint32_t last_update;
+	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 */
+	uint8_t i;
+
+	if (last_update == 0) {
+		for (i=0; i<11; i++) {
+			_pwm_output[i] = 1000;
+		}
+		if (_vehicle == ArduPlane) {
+			_pwm_output[0] = _pwm_output[1] = _pwm_output[3] = 1500;
+			_pwm_output[7] = 1800;
+		}
+		if (_vehicle == APMrover2) {
+			_pwm_output[0] = _pwm_output[1] = _pwm_output[2] = _pwm_output[3] = 1500;
+			_pwm_output[7] = 1800;
+		}
+	}
+
+    if (_sitl == NULL) {
+        return;
+    }
+
+	// output at chosen framerate
+	if (last_update != 0 && hal.scheduler->millis() - last_update < 1000/_framerate) {
+		return;
+	}
+	last_update = hal.scheduler->millis();
+
+	for (i=0; i<11; i++) {
+		if (_pwm_output[i] == 0xFFFF) {
+			control.pwm[i] = 0;
+		} else {
+			control.pwm[i] = _pwm_output[i];
+		}
+	}
+
+	if (_vehicle == ArduPlane) {
+		// add in engine multiplier
+		if (control.pwm[2] > 1000) {
+			control.pwm[2] = ((control.pwm[2]-1000) * _sitl->engine_mul) + 1000;
+			if (control.pwm[2] > 2000) control.pwm[2] = 2000;
+		}
+		_motors_on = ((control.pwm[2]-1000)/1000.0) > 0;
+	} else if (_vehicle == APMrover2) {
+		// add in engine multiplier
+		if (control.pwm[2] != 1500) {
+			control.pwm[2] = ((control.pwm[2]-1500) * _sitl->engine_mul) + 1500;
+			if (control.pwm[2] > 2000) control.pwm[2] = 2000;
+			if (control.pwm[2] < 1000) control.pwm[2] = 1000;
+		}
+		_motors_on = ((control.pwm[2]-1500)/500.0) != 0;
+	} else {
+		_motors_on = false;
+		for (i=0; i<4; i++) {
+			if ((control.pwm[i]-1000)/1000.0 > 0) {
+				_motors_on = true;
+			}
+		}
+	}
+
+	// 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;
+
+	// zero the wind for the first 15s to allow pitot calibration
+	if (hal.scheduler->millis() < 15000) {
+		control.speed = 0;
+	}
+
+	sendto(_sitl_fd, (void*)&control, sizeof(control), MSG_DONTWAIT, (const sockaddr *)&_rcout_addr, sizeof(_rcout_addr));
+}
+
+
+/*
+  setup a timer used to prod the ISRs
+ */
+void SITL_State::_setup_timer(void)
+{
+	struct itimerval it;
+	struct sigaction act;
+
+	act.sa_handler = _timer_handler;
+        act.sa_flags = SA_RESTART|SA_NODEFER;
+        sigemptyset(&act.sa_mask);
+        sigaddset(&act.sa_mask, SIGALRM);
+        sigaction(SIGALRM, &act, NULL);
+
+	it.it_interval.tv_sec = 0;
+	it.it_interval.tv_usec = 1000; // 1KHz
+	it.it_value = it.it_interval;
+
+	setitimer(ITIMER_REAL, &it, NULL);
+}
+
+// generate a random float between -1 and 1
+float SITL_State::_rand_float(void)
+{
+    return ((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6;
+}
+
+// generate a random Vector3f of size 1
+Vector3f SITL_State::_rand_vec3f(void)
+{
+	Vector3f v = Vector3f(_rand_float(),
+                          _rand_float(),
+                          _rand_float());
+	if (v.length() != 0.0) {
+		v.normalize();
+	}
+	return v;
+}
+
+
+void SITL_State::init(int argc, char * const argv[])
+{
+    _scheduler = (SITLScheduler *)hal.scheduler;
+	_parse_command_line(argc, argv);
+}
+
+#endif

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -0,0 +1,96 @@
+
+#ifndef __AP_HAL_AVR_SITL_STATE_H__
+#define __AP_HAL_AVR_SITL_STATE_H__
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+
+#include <AP_HAL_AVR_SITL.h>
+#include "AP_HAL_AVR_SITL_Namespace.h"
+#include "HAL_AVR_SITL_Class.h"
+#include "sitl_rc.h"
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/udp.h>
+#include <arpa/inet.h>
+
+#include "../AP_Baro/AP_Baro.h"
+#include "../AP_InertialSensor/AP_InertialSensor.h"
+#include "../AP_Compass/AP_Compass.h"
+#include "../SITL/SITL.h"
+
+extern struct RC_ICR4 ICR4;
+
+class HAL_AVR_SITL;
+
+class AVR_SITL::SITL_State {
+public:
+    void init(int argc, char * const argv[]);
+
+    enum vehicle_type {
+	    ArduCopter,
+	    APMrover2,
+	    ArduPlane
+    };
+
+private:
+    void _parse_command_line(int argc, char * const argv[]);
+    void _usage(void);
+    void _sitl_setup(void);
+    void _setup_fdm(void);
+    void _setup_timer(void);
+    void _setup_adc(void);
+
+    // these methods are static as they are called
+    // from the timer
+    static void _update_barometer(float height);
+    static void _update_compass(float roll, float pitch, float yaw);
+    static void _update_gps(double latitude, double longitude, float altitude,
+			    double speedN, double speedE, bool have_lock);
+    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);
+    static void _fdm_input(void);
+    static void _simulator_output(void);
+    static uint16_t _airspeed_sensor(float airspeed);
+    static float _gyro_drift(void);
+    static float _rand_float(void);
+    static Vector3f _rand_vec3f(void);
+    static Vector3f _heading_to_mag(float roll, float pitch, float yaw);
+    static void _gps_send(uint8_t msgid, uint8_t *buf, uint16_t size);
+    ssize_t _gps_read(int fd, void *buf, size_t count);
+    int _gps_pipe(void);
+
+    // signal handlers
+    static void _sig_fpe(int signum);
+    static void _timer_handler(int signum);
+
+    // internal state
+    static enum vehicle_type _vehicle;
+    static uint16_t _framerate;
+    float _initial_height;
+    static struct sockaddr_in _rcout_addr;
+    static pid_t _parent_pid;
+    static uint32_t _update_count;
+    static bool _motors_on;
+
+    static AP_Baro_BMP085_HIL *_barometer;
+    static AP_InertialSensor_Stub *_ins;
+    static SITLScheduler *_scheduler;
+    static AP_Compass_HIL *_compass;
+
+    static int _sitl_fd;
+    static SITL *_sitl;
+    static const uint16_t _rcout_port = 5502;
+    static const uint16_t _simin_port = 5501;
+
+    static uint16_t _pwm_output[11];
+};
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+#endif // __AP_HAL_AVR_SITL_STATE_H__
+

libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp

@@ -0,0 +1,57 @@
+/*
+  SITL handling
+
+  This simulates a barometer
+
+  Andrew Tridgell November 2011
+ */
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+
+#include "HAL_AVR.h"
+#include "AP_HAL_AVR_SITL.h"
+
+using namespace AVR_SITL;
+
+extern const AP_HAL::HAL& hal;
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <math.h>
+
+/*
+  setup the barometer with new input
+  altitude is in meters
+ */
+void SITL_State::_update_barometer(float altitude)
+{
+	double Temp, Press, y;
+	static uint32_t last_update;
+
+	if (_barometer == NULL) {
+		// this sketch doesn't use a barometer
+		return;
+	}
+
+	// 80Hz, to match the real APM2 barometer
+	if (hal.scheduler->millis() - last_update < 12) {
+		return;
+	}
+	last_update = hal.scheduler->millis();
+
+	Temp = 312;
+
+	y = ((altitude-584.0) * 1000.0) / 29271.267;
+	y /= (Temp / 10.0) + 273.15;
+	y = 1.0/exp(y);
+	y *= 95446.0;
+
+	Press = y + (_rand_float() * _sitl->baro_noise);
+
+	_barometer->setHIL(Temp, Press);
+}
+
+#endif

libraries/AP_HAL_AVR_SITL/sitl_compass.cpp

@@ -0,0 +1,80 @@
+/*
+  SITL handling
+
+  This simulates a compass
+
+  Andrew Tridgell November 2011
+ */
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_AVR_SITL.h>
+#include "AP_HAL_AVR_SITL_Namespace.h"
+#include "HAL_AVR_SITL_Class.h"
+
+#include <AP_Math.h>
+#include "../AP_Compass/AP_Compass.h"
+#include "../AP_Declination/AP_Declination.h"
+#include "../SITL/SITL.h"
+
+using namespace AVR_SITL;
+
+#define MAG_OFS_X 5.0
+#define MAG_OFS_Y 13.0
+#define MAG_OFS_Z -18.0
+
+// inclination in Canberra (degrees)
+#define MAG_INCLINATION -66
+
+// magnetic field strength in Canberra as observed
+// using an APM1 with 5883L compass
+#define MAG_FIELD_STRENGTH 818
+
+/*
+  given a magnetic heading, and roll, pitch, yaw values,
+  calculate consistent magnetometer components
+
+  All angles are in radians
+ */
+Vector3f SITL_State::_heading_to_mag(float roll, float pitch, float yaw)
+{
+	Vector3f Bearth, m;
+	Matrix3f R;
+	float declination = AP_Declination::get_declination(_sitl->state.latitude, _sitl->state.longitude);
+
+	// Bearth is the magnetic field in Canberra. We need to adjust
+	// it for inclination and declination
+	Bearth(MAG_FIELD_STRENGTH, 0, 0);
+	R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(declination));
+	Bearth = R * Bearth;
+
+	// create a rotation matrix for the given attitude
+	R.from_euler(roll, pitch, yaw);
+
+	// convert the earth frame magnetic vector to body frame, and
+	// apply the offsets
+	m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
+	return m + (_rand_vec3f() * _sitl->mag_noise);
+}
+
+
+
+/*
+  setup the compass with new input
+  all inputs are in degrees
+ */
+void SITL_State::_update_compass(float roll, float pitch, float yaw)
+{
+	if (_compass == NULL) {
+		// no compass in this sketch
+		return;
+	}
+	Vector3f m = _heading_to_mag(ToRad(roll),
+				     ToRad(pitch),
+				     ToRad(yaw));
+	_compass->setHIL(m.x, m.y, m.z);
+}
+
+#endif

libraries/AP_HAL_AVR_SITL/sitl_gps.cpp

@@ -0,0 +1,250 @@
+/*
+  SITL handling
+
+  This simulates a GPS on a serial port
+
+  Andrew Tridgell November 2011
+ */
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_AVR_SITL.h>
+#include "AP_HAL_AVR_SITL_Namespace.h"
+#include "HAL_AVR_SITL_Class.h"
+
+#include <AP_Math.h>
+#include "../SITL/SITL.h"
+#include "Scheduler.h"
+#include "UARTDriver.h"
+#include "../AP_GPS/AP_GPS.h"
+#include "../AP_GPS/AP_GPS_UBLOX.h"
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+using namespace AVR_SITL;
+extern const AP_HAL::HAL& hal;
+
+#define MAX_GPS_DELAY 100
+
+struct gps_data {
+	double latitude;
+	double longitude;
+	float altitude;
+	double speedN;
+	double speedE;
+	bool have_lock;
+} gps_data[MAX_GPS_DELAY];
+
+static uint8_t next_gps_index;
+static uint8_t gps_delay;
+
+// state of GPS emulation
+static struct {
+	/* pipe emulating UBLOX GPS serial stream */
+	int gps_fd, client_fd;
+	uint32_t last_update; // milliseconds
+} gps_state;
+
+/*
+  hook for reading from the GPS pipe
+ */
+ssize_t SITL_State::_gps_read(int fd, void *buf, size_t count)
+{
+#ifdef FIONREAD
+	// use FIONREAD to get exact value if possible
+	int num_ready;
+	while (ioctl(fd, FIONREAD, &num_ready) == 0 && num_ready > 256) {
+		// the pipe is filling up - drain it
+		uint8_t tmp[128];
+		if (read(fd, tmp, sizeof(tmp)) != sizeof(tmp)) {
+			break;
+		}
+	}
+#endif
+	return read(fd, buf, count);
+}
+
+/*
+  setup GPS input pipe
+ */
+int SITL_State::_gps_pipe(void)
+{
+	int fd[2];
+	if (gps_state.client_fd != 0) {
+		return gps_state.client_fd;
+	}
+	pipe(fd);
+	gps_state.gps_fd    = fd[1];
+	gps_state.client_fd = fd[0];
+	gps_state.last_update = _scheduler->millis();
+	AVR_SITL::SITLUARTDriver::_set_nonblocking(gps_state.gps_fd);
+	AVR_SITL::SITLUARTDriver::_set_nonblocking(fd[0]);
+	return gps_state.client_fd;
+}
+
+
+/*
+  send a UBLOX GPS message
+ */
+void SITL_State::_gps_send(uint8_t msgid, uint8_t *buf, uint16_t size)
+{
+        const uint8_t PREAMBLE1 = 0xb5;
+        const uint8_t PREAMBLE2 = 0x62;
+        const uint8_t CLASS_NAV = 0x1;
+	uint8_t hdr[6], chk[2];
+	hdr[0] = PREAMBLE1;
+	hdr[1] = PREAMBLE2;
+	hdr[2] = CLASS_NAV;
+	hdr[3] = msgid;
+	hdr[4] = size & 0xFF;
+	hdr[5] = size >> 8;
+	chk[0] = chk[1] = hdr[2];
+	chk[1] += (chk[0] += hdr[3]);
+	chk[1] += (chk[0] += hdr[4]);
+	chk[1] += (chk[0] += hdr[5]);
+	for (uint8_t i=0; i<size; i++) {
+		chk[1] += (chk[0] += buf[i]);
+	}
+	write(gps_state.gps_fd, hdr, sizeof(hdr));
+	write(gps_state.gps_fd, buf, size);
+	write(gps_state.gps_fd, chk, sizeof(chk));
+}
+
+
+/*
+  possibly send a new GPS UBLOX packet
+ */
+void SITL_State::_update_gps(double latitude, double longitude, float altitude,
+			     double speedN, double speedE, bool have_lock)
+{
+	struct ubx_nav_posllh {
+		uint32_t	time; // GPS msToW
+		int32_t		longitude;
+		int32_t		latitude;
+		int32_t		altitude_ellipsoid;
+		int32_t		altitude_msl;
+		uint32_t	horizontal_accuracy;
+		uint32_t	vertical_accuracy;
+	} pos;
+	struct ubx_nav_status {
+		uint32_t	time;				// GPS msToW
+		uint8_t		fix_type;
+		uint8_t		fix_status;
+		uint8_t		differential_status;
+		uint8_t		res;
+		uint32_t	time_to_first_fix;
+		uint32_t	uptime;				// milliseconds
+	} status;
+	struct ubx_nav_velned {
+		uint32_t	time;				// GPS msToW
+		int32_t		ned_north;
+		int32_t		ned_east;
+		int32_t		ned_down;
+		uint32_t	speed_3d;
+		uint32_t	speed_2d;
+		int32_t		heading_2d;
+		uint32_t	speed_accuracy;
+		uint32_t	heading_accuracy;
+	} velned;
+	struct ubx_nav_solution {
+		uint32_t time;
+		int32_t time_nsec;
+		int16_t week;
+		uint8_t fix_type;
+		uint8_t fix_status;
+		int32_t ecef_x;
+		int32_t ecef_y;
+		int32_t ecef_z;
+		uint32_t position_accuracy_3d;
+		int32_t ecef_x_velocity;
+		int32_t ecef_y_velocity;
+		int32_t ecef_z_velocity;
+		uint32_t speed_accuracy;
+		uint16_t position_DOP;
+		uint8_t res;
+		uint8_t satellites;
+		uint32_t res2;
+	} sol;
+        const uint8_t MSG_POSLLH = 0x2;
+	const uint8_t MSG_STATUS = 0x3;
+	const uint8_t MSG_VELNED = 0x12;
+        const uint8_t MSG_SOL = 0x6;
+	struct gps_data d;
+
+	// 5Hz, to match the real UBlox config in APM
+	if (hal.scheduler->millis() - gps_state.last_update < 200) {
+		return;
+	}
+	gps_state.last_update = hal.scheduler->millis();
+
+	d.latitude = latitude;
+	d.longitude = longitude;
+	d.altitude = altitude;
+	d.speedN = speedN;
+	d.speedE = speedE;
+	d.have_lock = have_lock;
+
+	// add in some GPS lag
+	gps_data[next_gps_index++] = d;
+	if (next_gps_index >= gps_delay) {
+		next_gps_index = 0;
+	}
+
+	d = gps_data[next_gps_index];
+
+	if (_sitl->gps_delay != gps_delay) {
+		// cope with updates to the delay control
+		gps_delay = _sitl->gps_delay;
+		for (uint8_t i=0; i<gps_delay; i++) {
+			gps_data[i] = d;
+		}
+	}
+
+	pos.time = hal.scheduler->millis(); // FIX
+	pos.longitude = d.longitude * 1.0e7;
+	pos.latitude  = d.latitude * 1.0e7;
+	pos.altitude_ellipsoid = d.altitude*1000.0;
+	pos.altitude_msl = d.altitude*1000.0;
+	pos.horizontal_accuracy = 5;
+	pos.vertical_accuracy = 10;
+
+	status.time = pos.time;
+	status.fix_type = d.have_lock?3:0;
+	status.fix_status = d.have_lock?1:0;
+	status.differential_status = 0;
+	status.res = 0;
+	status.time_to_first_fix = 0;
+	status.uptime = hal.scheduler->millis();
+
+	velned.time = pos.time;
+	velned.ned_north = 100.0 * d.speedN;
+	velned.ned_east  = 100.0 * d.speedE;
+	velned.ned_down  = 0;
+#define sqr(x) ((x)*(x))
+	velned.speed_2d = sqrt(sqr(d.speedN)+sqr(d.speedE)) * 100;
+	velned.speed_3d = velned.speed_2d;
+	velned.heading_2d = ToDeg(atan2(d.speedE, d.speedN)) * 100000.0;
+	if (velned.heading_2d < 0.0) {
+		velned.heading_2d += 360.0 * 100000.0;
+	}
+	velned.speed_accuracy = 2;
+	velned.heading_accuracy = 4;
+
+	memset(&sol, 0, sizeof(sol));
+	sol.fix_type = d.have_lock?3:0;
+	sol.fix_status = 221;
+	sol.satellites = d.have_lock?10:3;
+
+	if (gps_state.gps_fd == 0) {
+		return;
+	}
+
+	_gps_send(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
+	_gps_send(MSG_STATUS, (uint8_t*)&status, sizeof(status));
+	_gps_send(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
+	_gps_send(MSG_SOL,    (uint8_t*)&sol, sizeof(sol));
+}
+
+#endif

libraries/AP_HAL_AVR_SITL/sitl_ins.cpp

@@ -0,0 +1,124 @@
+/*
+  SITL handling
+
+  This emulates the ADS7844 ADC
+
+  Andrew Tridgell November 2011
+ */
+
+#include <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_AVR_SITL.h>
+#include "AP_HAL_AVR_SITL_Namespace.h"
+#include "HAL_AVR_SITL_Class.h"
+
+#include <AP_Math.h>
+#include "../AP_Compass/AP_Compass.h"
+#include "../AP_Declination/AP_Declination.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;
+
+/*
+  convert airspeed in m/s to an airspeed sensor value
+ */
+uint16_t SITL_State::_airspeed_sensor(float airspeed)
+{
+	const float airspeed_ratio = 1.9936;
+	const float airspeed_offset = 2820;
+	float airspeed_pressure, airspeed_raw;
+
+	airspeed_pressure = (airspeed*airspeed) / airspeed_ratio;
+	airspeed_raw = airspeed_pressure + airspeed_offset;
+	return airspeed_raw;
+}
+
+
+float SITL_State::_gyro_drift(void)
+{
+	if (_sitl->drift_speed == 0.0) {
+		return 0;
+	}
+	double period  = _sitl->drift_time * 2;
+	double minutes = fmod(_scheduler->_micros() / 60.0e6, period);
+	if (minutes < period/2) {
+		return minutes * ToRad(_sitl->drift_speed);
+	}
+	return (period - minutes) * ToRad(_sitl->drift_speed);
+
+}
+
+/*
+  setup the INS input channels with new input
+
+  Note that this uses roll, pitch and yaw only as inputs. The
+  simulator rollrates are instantaneous, whereas we need to use
+  average rates to cope with slow update rates.
+
+  inputs are in degrees
+
+	phi - roll
+	theta - pitch
+	psi - true heading
+	alpha - angle of attack
+	beta - side slip
+	phidot - roll rate
+	thetadot - pitch rate
+	psidot - yaw rate
+	v_north - north velocity in local/body frame
+	v_east - east velocity in local/body frame
+	v_down - down velocity in local/body frame
+	A_X_pilot - X accel in body frame
+	A_Y_pilot - Y accel in body frame
+	A_Z_pilot - Z accel in body frame
+
+  Note: doubles on high prec. stuff are preserved until the last moment
+
+ */
+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)
+{
+	double p, q, r;
+
+	if (_ins == NULL) {
+		// no inertial sensor in this sketch
+		return;
+	}
+
+	SITL::convert_body_frame(roll, pitch,
+				 rollRate, pitchRate, yawRate,
+				 &p, &q, &r);
+
+	// minimum noise levels are 2 bits
+	float accel_noise = 0.1;
+	float gyro_noise = ToRad(0.4);
+	if (_motors_on) {
+		// add extra noise when the motors are on
+		accel_noise += _sitl->accel_noise;
+		gyro_noise += ToRad(_sitl->gyro_noise);
+	}
+	xAccel += accel_noise * _rand_float();
+	yAccel += accel_noise * _rand_float();
+	zAccel += accel_noise * _rand_float();
+
+	p += gyro_noise * _rand_float();
+	q += gyro_noise * _rand_float();
+	r += gyro_noise * _rand_float();
+
+	p += _gyro_drift();
+	q += _gyro_drift();
+	r += _gyro_drift();
+
+	_ins->set_gyro(Vector3f(p, q, r));
+	_ins->set_accel(Vector3f(xAccel, yAccel, zAccel));
+}
+
+#endif

libraries/AP_HAL_AVR_SITL/sitl_rc.h

@@ -0,0 +1,52 @@
+/*
+  RC input emulation
+  Code by Andrew Tridgell November 2011
+ */
+
+#ifndef _SITL_RC_H
+#define _SITL_RC_H
+
+struct RC_ICR4 {
+	uint16_t channels[9]; // 9th channel is sync
+	uint32_t value;
+	uint8_t idx;
+
+	RC_ICR4() {
+		// constructor
+		channels[0] = channels[1] = channels[3] = 1500;
+		channels[4] = channels[7] = 1800;
+		channels[2] = channels[5] = channels[6] = 1000;
+		channels[8] = 4500; // sync
+		idx = 0;
+	}
+
+	/*
+	  read from ICR4 fetches next channel
+	 */
+	operator int() {
+		value += channels[idx++]*2;
+		if (idx == 9) {
+			idx = 0;
+		}
+		value = value % 40000;
+		return (uint16_t)value;
+	}
+
+
+	/*
+	   ignore rate assignment for now (needed for apm2
+	   emulation)
+	*/
+	RC_ICR4& operator=(uint16_t rate) {
+		return *this;
+	}
+
+	/*
+	  interface to set a channel value from SITL
+	 */
+	void set(uint8_t i, uint16_t v) {
+		channels[i] = v;
+	}
+};
+
+#endif