Merge branch 'master' of https://code.google.com/p/ardupilot-mega

ArduCopter/Makefile

@@ -22,7 +22,7 @@ apm2:
 	make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DCLI_SLIDER_ENABLED=DISABLED"
 
 mavlink10:
-	make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DMAVLINK10 -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
+	make -f Makefile EXTRAFLAGS="-DMAVLINK10"
 
 etags:
 	cd .. && etags -f ArduCopter/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduCopter libraries)

ArduPlane/GCS_Mavlink.pde

@@ -1071,6 +1071,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 case MAV_ACTION_SET_AUTO:
                     set_mode(AUTO);
                     result=1;
+                    // clearing failsafe should not be needed
+                    // here. Added based on some puzzling results in
+                    // the simulator (tridge)
+                    failsafe = FAILSAFE_NONE;
                     break;
 
                 case MAV_ACTION_STORAGE_READ:

ArduPlane/Makefile

@@ -22,7 +22,7 @@ apm2:
 	make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
 
 mavlink10:
-	make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DMAVLINK10 -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
+	make -f Makefile EXTRAFLAGS="-DMAVLINK10"
 
 etags:
 	cd .. && etags -f ArduPlane/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduPlane libraries)

Tools/autotest/arduplane.py

@@ -286,6 +286,7 @@ def fly_ArduPlane(viewerip=None):
         print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
         raise
     mav.message_hooks.append(message_hook)
+    mav.idle_hooks.append(idle_hook)
 
     failed = False
     e = 'None'

Tools/autotest/autotest.py

@@ -53,6 +53,7 @@ def dump_logs(atype):
         lognums.append(int(mavproxy.match.group(1)))
     mavproxy.expect("Log]")
     for i in range(numlogs):
+        print("Dumping log %u (i=%u)" % (lognums[i], i))
         mavproxy.send("dump %u\n" % lognums[i])
         mavproxy.expect("logs enabled:", timeout=400)
         mavproxy.expect("Log]")
@@ -259,12 +260,11 @@ def run_tests(steps):
     if not passed:
         print("FAILED %u tests: %s" % (len(failed), failed))
 
-    results.addglob("Google Earth track", '*.kml')
+    results.addglob("Google Earth track", '*.kmz')
     results.addfile('Full Logs', 'autotest-output.txt')
     results.addglob('DataFlash Log', '*.flashlog')
     results.addglob("MAVLink log", '*.mavlog')
     results.addglob("GPX track", '*.gpx')
-    results.addglob("KMZ track", '*.kmz')
     results.addfile('ArduPlane build log', 'ArduPlane.txt')
     results.addfile('ArduPlane code size', 'ArduPlane.sizes.txt')
     results.addfile('ArduPlane stack sizes', 'ArduPlane.framesizes.txt')

libraries/AP_ADC/AP_ADC_ADS7844.cpp

@@ -212,7 +212,7 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
 	// division. That costs us 36 bytes of stack, but I think its
 	// worth it.
 	for (i = 0; i < 6; i++) {
-		result[i] = sum[i] / count[i];
+		result[i] = (sum[i] + (count[i]/2)) / count[i];
 	}
 
 

libraries/Desktop/support/sitl.cpp

@@ -41,7 +41,16 @@ struct ADC_UDR2 UDR2;
 struct RC_ICR4 ICR4;
 extern AP_TimerProcess timer_scheduler;
 extern Arduino_Mega_ISR_Registry isr_registry;
-static volatile uint32_t sim_input_count;
+
+static volatile struct {
+	double latitude, longitude; // degrees
+	double altitude; // MSL
+	double heading; // degrees
+	double speedN, speedE; // m/s
+	double roll, pitch, yaw; // degrees
+	double airspeed; // m/s
+	uint32_t update_count;
+} sim_state;
 
 
 /*
@@ -123,14 +132,19 @@ static void sitl_fgear_input(void)
 			return;
 		}
 
-		sitl_update_gps(d.fg_pkt.latitude, d.fg_pkt.longitude,
-				ft2m(d.fg_pkt.altitude),
-				ft2m(d.fg_pkt.speedN), ft2m(d.fg_pkt.speedE), true);
-		sitl_update_adc(d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading, kt2mps(d.fg_pkt.airspeed));
-		sitl_update_barometer(ft2m(d.fg_pkt.altitude));
-		sitl_update_compass(d.fg_pkt.heading, d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading);
+		sim_state.latitude = d.fg_pkt.latitude;
+		sim_state.longitude = d.fg_pkt.longitude;
+		sim_state.altitude  = ft2m(d.fg_pkt.altitude);
+		sim_state.speedN = ft2m(d.fg_pkt.speedN);
+		sim_state.speedE = ft2m(d.fg_pkt.speedE);
+		sim_state.roll = d.fg_pkt.rollDeg;
+		sim_state.pitch = d.fg_pkt.pitchDeg;
+		sim_state.yaw = d.fg_pkt.yawDeg;
+		sim_state.heading = d.fg_pkt.heading;
+		sim_state.airspeed = kt2mps(d.fg_pkt.airspeed);
+		sim_state.update_count++;
+
 		count++;
-		sim_input_count++;
 		if (millis() - last_report > 1000) {
 			printf("SIM %u FPS\n", count);
 			count = 0;
@@ -239,6 +253,8 @@ static void sitl_simulator_output(void)
  */
 static void timer_handler(int signum)
 {
+	static uint32_t last_update_count;
+
 	/* make sure we die if our parent dies */
 	if (kill(parent_pid, 0) != 0) {
 		exit(1);
@@ -258,9 +274,22 @@ static void timer_handler(int signum)
 	// send RC output to flight sim
 	sitl_simulator_output();
 
-	if (sim_input_count == 0) {
+	if (sim_state.update_count == 0) {
 		sitl_update_gps(0, 0, 0, 0, 0, false);
+		return;
+	}
+
+	if (sim_state.update_count == last_update_count) {
+		return;
 	}
+	last_update_count = sim_state.update_count;
+
+	sitl_update_gps(sim_state.latitude, sim_state.longitude,
+			sim_state.altitude,
+			sim_state.speedN, sim_state.speedE, true);
+	sitl_update_adc(sim_state.roll, sim_state.pitch, sim_state.heading, sim_state.airspeed);
+	sitl_update_barometer(sim_state.altitude);
+	sitl_update_compass(sim_state.heading, sim_state.roll, sim_state.pitch, sim_state.heading);
 }
 
 

libraries/Desktop/support/sitl_adc.cpp

@@ -52,16 +52,21 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
 	const float _gyro_gain_y = 0.41;
 	const float _gyro_gain_z = 0.41;
 	const float _accel_scale = 9.80665 / 423.8;
-	uint16_t adc[7];
+	float adc[7];
 	float xAccel, yAccel, zAccel, scale;
 	float rollRate, pitchRate, yawRate;
 	static uint32_t last_update;
 	static float last_roll, last_pitch, last_yaw;
 	unsigned long delta_t;
 
+	// 200Hz max
+	if (micros() - last_update < 5000) {
+		return;
+	}
+
 	/* map roll/pitch/yaw to values the accelerometer would see */
-	xAccel = sin(ToRad(pitch));
-	yAccel = -sin(ToRad(roll));
+	xAccel = sin(ToRad(pitch)) * cos(ToRad(roll));
+	yAccel = -sin(ToRad(roll)) * cos(ToRad(pitch));
 	zAccel = -cos(ToRad(roll)) * cos(ToRad(pitch));
 	scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel));
 	xAccel *= scale;
@@ -76,12 +81,13 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
 		delta_t = micros();
 	} else {
 		delta_t = micros() - last_update;
-		rollRate  = 1.0e6 * (roll  - last_roll)  / delta_t;
-		pitchRate = 1.0e6 * (pitch - last_pitch) / delta_t;
-		yawRate   = 1.0e6 * (yaw   - last_yaw)   / delta_t;
-		rollRate = normalise(rollRate, -180, 180);
-		pitchRate = normalise(pitchRate, -180, 180);
-		yawRate = normalise(yawRate, -180, 180);
+		float rollChange, pitchChange, yawChange;
+		rollChange  = normalise180(roll - last_roll);
+		pitchChange = normalise180(pitch - last_pitch);
+		yawChange   = normalise180(yaw - last_yaw);
+		rollRate  = 1.0e6 * rollChange  / delta_t;
+		pitchRate = 1.0e6 * pitchChange / delta_t;
+		yawRate   = 1.0e6 * yawChange   / delta_t;
 	}
 	last_update += delta_t;
 
@@ -100,20 +106,30 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
 
 	/* tell the UDR2 register emulation what values to give to the driver */
 	for (uint8_t i=0; i<6; i++) {
-		UDR2.set(sensor_map[i], (adc[i]<<3) & 0x7FFF);
+		UDR2.set(sensor_map[i], adc[i]);
 	}
 
 
 	// set the airspeed sensor input
-	UDR2.set(7, airspeed_sensor(airspeed)<<3);
+	UDR2.set(7, airspeed_sensor(airspeed));
 
 	/* FIX: rubbish value for temperature for now */
-	UDR2.set(3, 2000<<3);
+	UDR2.set(3, 2000);
+
+#if 0
+	extern AP_DCM_HIL dcm;
+	dcm.setHil(ToRad(roll), ToRad(pitch), ToRad(yaw),
+		   ToRad(rollRate), ToRad(pitchRate), ToRad(yawRate));
+
+#endif
 
 #if 0
 	extern AP_DCM dcm;
-	printf("roll=%6.1f / %6.1f pitch=%6.1f / %6.1f  rollRate=%6.1f pitchRate=%6.1f\n",
-	       roll, dcm.roll_sensor/100.0, pitch, dcm.pitch_sensor/100.0, rollRate, pitchRate);
+	Vector3f omega = dcm.get_gyro();
+	printf("dt=%5u adc[2]=%6.1f roll=%6.1f / %6.1f yaw=%6.1f / %6.1f  yawRate=%6.3f / %6.3f\n",
+	       (unsigned)delta_t,
+	       adc[2],
+	       roll, dcm.roll_sensor/100.0, yaw, dcm.yaw_sensor/100.0, yawRate, ToDeg(omega.z));
 #endif
 }
 

libraries/Desktop/support/sitl_adc.h

@@ -6,11 +6,13 @@
 #ifndef _SITL_ADC_H
 #define _SITL_ADC_H
 
+#include <stdlib.h>
+
 // this implements the UDR2 register
 struct ADC_UDR2 {
 	uint16_t value, next_value;
 	uint8_t idx;
-	uint16_t channels[8];
+	float channels[8];
 
 	ADC_UDR2() {
 		// constructor
@@ -26,19 +28,26 @@ struct ADC_UDR2 {
 	   to output next
 	*/
 	ADC_UDR2& operator=(uint8_t cmd) {
+		float next_analog;
 		switch (cmd) {
-		case 0x87: next_value = channels[0]; break;
-		case 0xC7: next_value = channels[1]; break;
-		case 0x97: next_value = channels[2]; break;
-		case 0xD7: next_value = channels[3]; break;
-		case 0xA7: next_value = channels[4]; break;
-		case 0xE7: next_value = channels[5]; break;
-		case 0xB7: next_value = channels[6]; break;
-		case 0xF7: next_value = channels[7]; break;
+		case 0x87: next_analog = channels[0]; break;
+		case 0xC7: next_analog = channels[1]; break;
+		case 0x97: next_analog = channels[2]; break;
+		case 0xD7: next_analog = channels[3]; break;
+		case 0xA7: next_analog = channels[4]; break;
+		case 0xE7: next_analog = channels[5]; break;
+		case 0xB7: next_analog = channels[6]; break;
+		case 0xF7: next_analog = channels[7]; break;
+		case 0:
+		default: return *this;
 		}
-		if (cmd != 0) {
-			idx = 1;
+		idx = 1;
+		int noise = (((unsigned long)random()) % 3) - 1;
+		next_value = (unsigned)(next_analog + noise + 0.5);
+		if (next_value >= 0x1000) {
+			next_value = 0xFFF;
 		}
+		next_value = (next_value << 3);
 		return *this;
 	}
 
@@ -60,7 +69,7 @@ struct ADC_UDR2 {
 	/*
 	  interface to set a channel value from SITL
 	 */
-	void set(uint8_t i, uint16_t v) {
+	void set(uint8_t i, float v) {
 		channels[i] = v;
 	}
 };

libraries/Desktop/support/util.cpp

@@ -90,3 +90,8 @@ double normalise(double v, double min, double max)
 	}
 	return v;
 }
+
+double normalise180(double v)
+{
+	return normalise(v, -180, 180);
+}

libraries/Desktop/support/util.h

@@ -9,3 +9,4 @@ float swap_float(float f);
 void swap_floats(float *f, unsigned count);
 void set_nonblocking(int fd);
 double normalise(double v, double min, double max);
+double normalise180(double v);