Merge remote-tracking branch 'upstream/master' into ros

5 files changed, 69 insertions, 3 deletions

diff --git a/Tools/usb_serialload.py b/Tools/usb_serialload.py
new file mode 100644
index 000000000..5c864532f
--- /dev/null
+++ b/Tools/usb_serialload.py
@@ -0,0 +1,55 @@
+import serial, time
+
+
+port = serial.Serial('/dev/ttyACM0', baudrate=57600, timeout=2)
+
+data = '01234567890123456789012345678901234567890123456789'
+#data = 'hellohello'
+outLine = 'echo %s\n' % data
+
+port.write('\n\n\n')
+port.write('free\n')
+line = port.readline(80)
+while line != '':
+    print(line)
+    line = port.readline(80)
+    
+
+i = 0
+bytesOut = 0
+bytesIn = 0
+
+startTime = time.time()
+lastPrint = startTime
+while True:
+    bytesOut += port.write(outLine)
+    line = port.readline(80)
+    bytesIn += len(line)
+    # check command line echo
+    if (data not in line):
+        print('command error %d: %s' % (i,line))
+        #break
+    # read echo output
+    line = port.readline(80)
+    if (data not in line):
+        print('echo output error %d: %s' % (i,line))
+        #break
+    bytesIn += len(line)
+    #print('%d: %s' % (i,line))
+    #print('%d: bytesOut: %d, bytesIn: %d' % (i, bytesOut, bytesIn))
+    
+    elapsedT = time.time() - lastPrint
+    if (time.time() - lastPrint >= 5):
+        outRate = bytesOut / elapsedT
+        inRate = bytesIn / elapsedT
+        usbRate = (bytesOut + bytesIn) / elapsedT
+        lastPrint = time.time()
+        print('elapsed time: %f' % (time.time() - startTime))
+        print('data rates (bytes/sec): out: %f, in: %f, total: %f' % (outRate, inRate, usbRate))
+        
+        bytesOut = 0
+        bytesIn = 0
+        
+    i += 1    
+    #if (i > 2): break
+    
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index cfae8761c..82fa5cc6e 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -176,6 +176,7 @@ static const int ERROR = -1;
 #define L3G4200D_DEFAULT_RATE			800
 #define L3GD20_DEFAULT_RANGE_DPS		2000
 #define L3GD20_DEFAULT_FILTER_FREQ		30
+#define L3GD20_TEMP_OFFSET_CELSIUS		40
 
 #ifndef SENSOR_BOARD_ROTATION_DEFAULT
 #define SENSOR_BOARD_ROTATION_DEFAULT		SENSOR_BOARD_ROTATION_270_DEG
@@ -856,7 +857,7 @@ L3GD20::measure()
 #pragma pack(push, 1)
 	struct {
 		uint8_t		cmd;
-		uint8_t		temp;
+		int8_t		temp;
 		uint8_t		status;
 		int16_t		x;
 		int16_t		y;
@@ -930,6 +931,8 @@ L3GD20::measure()
 
 	report.z_raw = raw_report.z;
 
+	report.temperature_raw = raw_report.temp;
+
 	report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
 	report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
 	report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
@@ -938,6 +941,8 @@ L3GD20::measure()
 	report.y = _gyro_filter_y.apply(report.y);
 	report.z = _gyro_filter_z.apply(report.z);
 
+	report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
+
 	// apply user specified rotation
 	rotate_3f(_rotation, report.x, report.y, report.z);
 
@@ -1091,9 +1096,11 @@ test()
 	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
 	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
 	warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
+	warnx("temp: \t%d\tC", (int)g_report.temperature);
 	warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
 	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
 	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
+	warnx("temp: \t%d\traw", (int)g_report.temperature_raw);
 	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
 	      (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
 
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 5187b448f..917c7f830 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -74,6 +74,7 @@ enum Rotation {
 	ROTATION_ROLL_270_YAW_135    = 23,
 	ROTATION_PITCH_90            = 24,
 	ROTATION_PITCH_270           = 25,
+	ROTATION_ROLL_270_YAW_270    = 26,
 	ROTATION_MAX
 };
 
@@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = {
 	{270,   0,  90 },
 	{270,   0, 135 },
 	{  0,  90,   0 },
-	{  0, 270,   0 }
+	{  0, 270,   0 },
+	{270,   0, 270 }
 };
 
 /**
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 9247f999a..bc092c7e9 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1396,7 +1396,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
-	param.sched_priority = SCHED_PRIORITY_MAX - 40;
+	param.sched_priority = SCHED_PRIORITY_MAX - 80;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
 	pthread_attr_setstacksize(&receiveloop_attr, 2900);
diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk
index 41706e137..a12bc369e 100644
--- a/src/systemcmds/nshterm/module.mk
+++ b/src/systemcmds/nshterm/module.mk
@@ -41,3 +41,5 @@ SRCS		 = nshterm.c
 MODULE_STACKSIZE = 1600
 
 MAXOPTIMIZATION	 = -Os
+
+MODULE_PRIORITY	= "SCHED_PRIORITY_DEFAULT-30"