diff --git a/conf/airframes/RM/rm_ardrone2.xml b/conf/airframes/RM/rm_ardrone2.xml
index 361a9bc266..b342252369 100644
--- a/conf/airframes/RM/rm_ardrone2.xml
+++ b/conf/airframes/RM/rm_ardrone2.xml
@@ -35,7 +35,7 @@
     <load name="bat_voltage_ardrone2.xml"/>
 	<load name="uartrotation.xml">
 	<configure name="STEREO_UART" value="UART1"/>
-        <configure name="STEREO_BAUD" value="B115200"/>
+        <configure name="STEREO_BAUD" value="B1000000"/>
 	</load>
   </modules>
 
diff --git a/conf/messages.xml b/conf/messages.xml
index 9edc17ab9c..4fd11ae1ce 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -2092,8 +2092,7 @@
   </message>
 
   <!-- 245 is free -->
- <message name="SERIALRMAT" id="246">
-  </message>
+  <!-- 246 is free -->
 
 
   <message name="PPRZ_DEBUG" id="247">
diff --git a/conf/modules/uartrotation.xml b/conf/modules/uartrotation.xml
index 83b2aace2b..0c63ac3efe 100644
--- a/conf/modules/uartrotation.xml
+++ b/conf/modules/uartrotation.xml
@@ -2,33 +2,27 @@
 
 <module name="uartrotation">
   <doc>
-    <description>Writes rotation to the uart bus</description>
+    <description>Writes rotation to the uart bus. Meant for sending the rotation
+	to the ODROID board or to the stereo camera boards. 
+	Can be used to improve vision algorithms. 
+	The protocol that is used is the stereocommunication protocol. 
+    </description>
     <configure name="STEREO_UART" value="UARTX" description="Sets the UART port number of the connected camera (required)"/>
     <configure name="STEREO_BAUD" value="BXXXXX" description="Sets the BAUD rate of the connected camera (required: must be same as camera)"/>
   </doc>
   <header>
     <file name="uartrotation.h"/>
   </header>
-  <init fun="uart_rotation_init()"/>
+  <periodic fun="write_serial_rot()" freq="512."/>
   <makefile>
     <file name="uartrotation.c"/>
     <file name="stereoprotocol.c" dir="subsystems"/>
-<!--    <define name="USE_$(STEREO_UART)"/>
-    <define name="UART_LINK" value="$(STEREO_UART_LOWER)"/>
-    <define name="$(STEREO_UART)_BAUD" value="$(STEREO_BAUD)"/>-->
 	<raw>
 		STEREO_UART_LOWER=$(shell echo $(STEREO_UART) | tr A-Z a-z)
 		ap.CFLAGS += -DUSE_$(STEREO_UART)
 		ap.CFLAGS += -DUART_LINK=$(STEREO_UART_LOWER)
 		ap.CFLAGS += -D$(STEREO_UART)_BAUD=$(STEREO_BAUD)
 	</raw>
-<!--
-
-		ODROID_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
-		ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
-		ap.CFLAGS += -DUSE_GPS -DGPS_LINK=$(ODROID_GPS_PORT_LOWER)
-
--->
   </makefile>
 
 </module>
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index f8de10d0b2..fd59954d1d 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -26,7 +26,6 @@
       <message name="AIR_DATA"               period="1.3"/>
       <message name="SURVEY"                 period="2.5"/>
       <message name="OPTIC_FLOW_EST"         period="0.25"/>
-      <message name="SERIALRMAT"         period="0.25"/>
       <message name="VECTORNAV_INFO"      period="0.5"/>
     </mode>
 
diff --git a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
index 18cce940bb..bc9e3bc9d3 100644
--- a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
+++ b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
@@ -151,7 +151,6 @@ void guidance_h_module_run(bool_t in_flight)
 static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
                                            uint32_t stamp, float vel_x, float vel_y, float vel_z)
 {
-	printf("Stabilisation before %f %f \n", vel_x, vel_y);
   /* Check if we are in the correct AP_MODE before setting commands */
   if (autopilot_mode != AP_MODE_MODULE) {
     return;
diff --git a/sw/airborne/modules/optical_flow/px4flow.c b/sw/airborne/modules/optical_flow/px4flow.c
index 4512a85fe6..31ee1f0844 100644
--- a/sw/airborne/modules/optical_flow/px4flow.c
+++ b/sw/airborne/modules/optical_flow/px4flow.c
@@ -46,9 +46,8 @@ struct mavlink_msg_req req;
 static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((unused)))
 {
   optical_flow_available = TRUE;
- // printf("Stabilisation decoded %d %d height: %f \n", optical_flow.flow_x, optical_flow.flow_y,optical_flow.ground_distance);
 
-  // X and Y negated to get to the body of the drone
+  // Y negated to get to the body of the drone
   AbiSendMsgVELOCITY_ESTIMATE(PIX4FLOWSENDER_ID, 0,
                                   (optical_flow.flow_x/optical_flow.ground_distance),
 								  -1.0*(optical_flow.flow_y/optical_flow.ground_distance),
diff --git a/sw/airborne/modules/sensors/stereocam.c b/sw/airborne/modules/sensors/stereocam.c
index 8a062703a9..b7f5cc4c3f 100644
--- a/sw/airborne/modules/sensors/stereocam.c
+++ b/sw/airborne/modules/sensors/stereocam.c
@@ -37,12 +37,7 @@
 // define coms link for stereocam
 #define STEREO_PORT   (&((UART_LINK).device))
 struct link_device *dev = STEREO_PORT;
-
 #define StereoGetch() STEREO_PORT ->get_byte(STEREO_PORT->periph)
-#define StereoSend1(c) STEREO_PORT->put_byte(STEREO_PORT->periph, c)
-#define StereoUartSend1(c) StereoSend1(c)
-#define StereoSend(_dat,_len) { for (uint8_t i = 0; i< (_len); i++) StereoSend1(_dat[i]); };
-#define StereoUartSetBaudrate(_b) uart_periph_set_baudrate(STEREO_PORT, _b);
 
 //typedef struct MsgProperties {
 //  uint16_t positionImageStart;
diff --git a/sw/airborne/modules/uartrotation/uartrotation.c b/sw/airborne/modules/uartrotation/uartrotation.c
index 2b784b3a21..b06ed27bb4 100644
--- a/sw/airborne/modules/uartrotation/uartrotation.c
+++ b/sw/airborne/modules/uartrotation/uartrotation.c
@@ -19,47 +19,37 @@
  */
 /**
  * @file "modules/readlocationfromodroid/readlocationfromodroid.c"
- * @author Roland
- * reads from the odroid and sends information of the drone to the odroid using JSON messages
+ * @author Roland + Kirk
+ * Sends rotation using the stereoboard protocol over the UART.
  */
 
 #include "modules/uartrotation/uartrotation.h"
 #include "subsystems/abi.h"
 #include <serial_port.h>
-#include <stdio.h>
 #include <inttypes.h>
-#include <errno.h>
 #include "state.h"
 #include "subsystems/gps.h"
 #include "subsystems/ins/ins_int.h"
 #include "firmwares/rotorcraft/autopilot.h"
 #include "mcu_periph/uart.h"
-#include "subsystems/gps.h"
-#include "subsystems/datalink/telemetry.h"
 #include "subsystems/stereoprotocol.h"
 #include "navdata.h"
-int frameNumberSending=0;
+static int frame_number_sending=0;
 static abi_event odroid_agl_ev;
 float lastKnownHeight = 0.0;
 int pleaseResetOdroid = 0;
 
+//uart_periph_set_baudrate(UART_LINK, B115200);
 
-static void write_serial_rot(struct transport_tx *trans, struct link_device *devasdf) {
+void write_serial_rot() {
 	struct Int32RMat *ltp_to_body_mat = stateGetNedToBodyRMat_i();
-	int32_t lengthArrayInformation = 11*sizeof(int32_t);
+	static int32_t lengthArrayInformation = 11*sizeof(int32_t);
 	uint8_t ar[lengthArrayInformation];
 	int32_t *pointer = (int32_t*)ar;
 	for(int indexRot = 0; indexRot < 9; indexRot++){
 		pointer[indexRot]=ltp_to_body_mat->m[indexRot];
 	}
 	pointer[9]=(int32_t)(state.alt_agl_f*100); //height above ground level in CM. 
-	pointer[10]=frameNumberSending++;
-	printf("Whoo sending serial\n");
+	pointer[10]=frame_number_sending++;
 	stereoprot_sendArray( &((UART_LINK).device),ar, lengthArrayInformation, 1);
-	printf("Whoo sending serial2\n");
-}
-
-
-void uart_rotation_init() {
-	register_periodic_telemetry(DefaultPeriodic, "SERIALRMAT", write_serial_rot);
 }
diff --git a/sw/airborne/modules/uartrotation/uartrotation.h b/sw/airborne/modules/uartrotation/uartrotation.h
index 296818e9e7..503594bdba 100644
--- a/sw/airborne/modules/uartrotation/uartrotation.h
+++ b/sw/airborne/modules/uartrotation/uartrotation.h
@@ -18,15 +18,14 @@
  * <http://www.gnu.org/licenses/>.
  */
 /**
- * @file "modules/readlocationfromodroid/readlocationfromodroid.h"
+ * @file "modules/uartrotation/uartrotation.h"
  * @author Roland
- * reads from the odroid
+ * Sends rotation using the stereoboard protocol over the UART.
  */
 
-#ifndef READLOCATIONFROMODROID_H
-#define READLOCATIONFROMODROID_H
+#ifndef UARTROTATION_H
+#define UARTROTATION_H
 #include <inttypes.h>
-extern void odroid_loc_init(void);
-extern void odroid_loc_periodic(void);
+extern void write_serial_rot(void);
 #endif