Merge branch 'master' of github.com:PX4/Firmware into fmuv2_bringup

33 files changed, 887 insertions, 760 deletions

diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway
new file mode 100644
index 000000000..5742d685a
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/40_io_segway
@@ -0,0 +1,122 @@
+#!nsh
+#
+# Flight startup script for PX4FMU+PX4IO
+#
+ 
+# disable USB and autostart
+set USB no
+set MODE custom
+ 
+#
+# Start the ORB (first app to start)
+#
+uorb start
+ 
+#
+# Load microSD params
+#
+echo "[init] loading microSD params"
+param select /fs/microsd/params
+if [ -f /fs/microsd/params ]
+then
+	param load /fs/microsd/params
+fi
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	param set SYS_AUTOCONFIG 0
+	param save /fs/microsd/params
+fi
+ 
+#
+# Force some key parameters to sane values
+# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
+#              see https://pixhawk.ethz.ch/mavlink/
+#
+param set MAV_TYPE 10
+ 
+#
+# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
+#
+if [ -f /fs/microsd/px4io.bin ]
+then
+	echo "PX4IO Firmware found. Checking Upgrade.."
+	if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+	then
+		echo "No newer version, skipping upgrade."
+	else
+		echo "Loading /fs/microsd/px4io.bin"
+		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
+		then
+			cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
+		else
+			echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
+			echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
+		fi
+	fi
+fi
+ 
+#
+# Start MAVLink (depends on orb)
+#
+mavlink start -d /dev/ttyS1 -b 57600
+usleep 5000
+ 
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+ 
+#
+# Start PX4IO interface (depends on orb, commander)
+#
+px4io start
+ 
+#
+# Allow PX4IO to recover from midair restarts.
+# this is very unlikely, but quite safe and robust.
+px4io recovery
+
+#
+# Disable px4io topic limiting
+#
+px4io limit 200
+ 
+#
+# Start the sensors (depends on orb, px4io)
+#
+sh /etc/init.d/rc.sensors
+ 
+#
+# Start GPS interface (depends on orb)
+#
+gps start
+ 
+#
+# Start the attitude estimator (depends on orb)
+#
+attitude_estimator_ekf start
+ 
+#
+# Load mixer and start controllers (depends on px4io)
+#
+md25 start 3 0x58
+segway start
+ 
+#
+# Start logging
+#
+sdlog2 start -r 50 -a -b 14
+ 
+#
+# Start system state
+#
+if blinkm start
+then
+	blinkm systemstate
+fi
diff --git a/ROMFS/px4fmu_common/logging/conv.zip b/ROMFS/px4fmu_common/logging/conv.zip
new file mode 100644
index 000000000..7cb837e56
--- /dev/null
+++ b/ROMFS/px4fmu_common/logging/conv.zip
diff --git a/ROMFS/px4fmu_common/logging/logconv.m b/ROMFS/px4fmu_common/logging/logconv.m
deleted file mode 100644
index 3750ddae2..000000000
--- a/ROMFS/px4fmu_common/logging/logconv.m
+++ /dev/null
@@ -1,586 +0,0 @@
-% This Matlab Script can be used to import the binary logged values of the

-% PX4FMU into data that can be plotted and analyzed.

-

-%% ************************************************************************

-% PX4LOG_PLOTSCRIPT: Main function

-% ************************************************************************

-function PX4Log_Plotscript

-

-% Clear everything

-clc

-clear all

-close all

-

-% ************************************************************************

-% SETTINGS

-% ************************************************************************

-

-% Set the path to your sysvector.bin file here

-filePath = 'sysvector.bin';

-

-% Set the minimum and maximum times to plot here [in seconds]

-mintime=0;        %The minimum time/timestamp to display, as set by the user [0 for first element / start]

-maxtime=0;        %The maximum time/timestamp to display, as set by the user [0 for last element / end]

-

-%Determine which data to plot. Not completely implemented yet.

-bDisplayGPS=true;

-

-%conversion factors

-fconv_gpsalt=1E-3; %[mm] to [m]

-fconv_gpslatlong=1E-7; %[gps_raw_position_unit] to [deg]

-fconv_timestamp=1E-6; % [microseconds] to [seconds]

-

-% ************************************************************************

-% Import the PX4 logs

-% ************************************************************************

-ImportPX4LogData();

-

-%Translate min and max plot times to indices

-time=double(sysvector.timestamp) .*fconv_timestamp;

-mintime_log=time(1);        %The minimum time/timestamp found in the log

-maxtime_log=time(end);      %The maximum time/timestamp found in the log

-CurTime=mintime_log;        %The current time at which to draw the aircraft position

-

-[imintime,imaxtime]=FindMinMaxTimeIndices();

-

-% ************************************************************************

-% PLOT & GUI SETUP

-% ************************************************************************

-NrFigures=5;

-NrAxes=10;

-h.figures(1:NrFigures)=0.0;  % Temporary initialization of figure handle array - these are numbered consecutively

-h.axes(1:NrAxes)=0.0;        % Temporary initialization of axes handle array - these are numbered consecutively

-h.pathpoints=[];             % Temporary initiliazation of path points

-

-% Setup the GUI to control the plots

-InitControlGUI();

-% Setup the plotting-GUI (figures, axes) itself.

-InitPlotGUI();

-

-% ************************************************************************

-% DRAW EVERYTHING

-% ************************************************************************

-DrawRawData();

-DrawCurrentAircraftState();

-

-%% ************************************************************************

-%  *** END OF MAIN SCRIPT ***

-%  NESTED FUNCTION DEFINTIONS FROM HERE ON

-%  ************************************************************************

-

-

-%% ************************************************************************

-%  IMPORTPX4LOGDATA (nested function)

-%  ************************************************************************

-%  Attention:  This is the import routine for firmware from ca. 03/2013.

-%              Other firmware versions might require different import 

-%              routines.

-

-function ImportPX4LogData()

-    % Work around a Matlab bug (not related to PX4)

-    % where timestamps from 1.1.1970 do not allow to

-    % read the file's size

-    if ismac

-        system('touch -t 201212121212.12 sysvector.bin');

-    end

-

-    % ************************************************************************

-    % RETRIEVE SYSTEM VECTOR

-    % *************************************************************************

-    % //All measurements in NED frame

-    %

-    % uint64_t timestamp; //[us]

-    % float gyro[3]; //[rad/s]

-    % float accel[3]; //[m/s^2]

-    % float mag[3]; //[gauss] 

-    % float baro; //pressure [millibar]

-    % float baro_alt; //altitude above MSL [meter]

-    % float baro_temp; //[degree celcius]

-    % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]

-    % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)

-    % float vbat; //battery voltage in [volt]

-    % float bat_current - current drawn from battery at this time instant

-    % float bat_discharged - discharged energy in mAh

-    % float adc[4]; //remaining auxiliary ADC ports [volt]

-    % float local_position[3]; //tangent plane mapping into x,y,z [m]

-    % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]

-    % float attitude[3]; //pitch, roll, yaw [rad]

-    % float rotMatrix[9]; //unitvectors

-    % float actuator_control[4]; //unitvector

-    % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]

-    % float diff_pressure; - pressure difference in millibar

-    % float ind_airspeed;

-    % float true_airspeed;

-

-    % Definition of the logged values

-    logFormat{1} = struct('name', 'timestamp',             'bytes', 8, 'array', 1, 'precision', 'uint64',  'machineformat', 'ieee-le.l64');

-    logFormat{2} = struct('name', 'gyro',                  'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{3} = struct('name', 'accel',                 'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{4} = struct('name', 'mag',                   'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{5} = struct('name', 'baro',                  'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{6} = struct('name', 'baro_alt',              'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{7} = struct('name', 'baro_temp',             'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{8} = struct('name', 'control',               'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{9} = struct('name', 'actuators',             'bytes', 4, 'array', 8, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{10} = struct('name', 'vbat',                 'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{11} = struct('name', 'bat_current',          'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{12} = struct('name', 'bat_discharged',       'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{13} = struct('name', 'adc',                  'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{14} = struct('name', 'local_position',       'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{15} = struct('name', 'gps_raw_position',     'bytes', 4, 'array', 3, 'precision', 'uint32',  'machineformat', 'ieee-le');

-    logFormat{16} = struct('name', 'attitude',             'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{17} = struct('name', 'rot_matrix',           'bytes', 4, 'array', 9, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{18} = struct('name', 'vicon_position',       'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{19} = struct('name', 'actuator_control',     'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{20} = struct('name', 'optical_flow',         'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{21} = struct('name', 'diff_pressure',        'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{22} = struct('name', 'ind_airspeed',         'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-    logFormat{23} = struct('name', 'true_airspeed',        'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');

-

-    % First get length of one line

-    columns = length(logFormat);

-    lineLength = 0;

-

-    for i=1:columns

-        lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array;

-    end

-

-

-    if exist(filePath, 'file')

-

-        fileInfo = dir(filePath);

-        fileSize = fileInfo.bytes;

-

-        elements = int64(fileSize./(lineLength));

-

-        fid = fopen(filePath, 'r');

-        offset = 0;

-        for i=1:columns

-            % using fread with a skip speeds up the import drastically, do not

-            % import the values one after the other

-            sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(...

-                fid, ...

-                [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ...

-                lineLength - logFormat{i}.bytes*logFormat{i}.array, ...

-                logFormat{i}.machineformat) ...

-            );

-            offset = offset + logFormat{i}.bytes*logFormat{i}.array;

-            fseek(fid, offset,'bof');

-        end

-

-        % shot the flight time

-        time_us = sysvector.timestamp(end) - sysvector.timestamp(1);

-        time_s = time_us*1e-6;

-        time_m = time_s/60;

-

-        % close the logfile

-        fclose(fid);

-

-        disp(['end log2matlab conversion' char(10)]);

-    else

-        disp(['file: ' filePath ' does not exist' char(10)]);

-    end

-end

-

-%% ************************************************************************

-%  INITCONTROLGUI (nested function)

-%  ************************************************************************

-%Setup central control GUI components to control current time where data is shown

-function InitControlGUI()

-    %**********************************************************************

-    % GUI size definitions

-    %**********************************************************************

-    dxy=5; %margins

-    %Panel: Plotctrl

-    dlabels=120;

-    dsliders=200;

-    dedits=80;

-    hslider=20;

-        

-    hpanel1=40; %panel1

-    hpanel2=220;%panel2

-    hpanel3=3*hslider+4*dxy+3*dxy;%panel3.

-    

-    width=dlabels+dsliders+dedits+4*dxy+2*dxy;  %figure width

-    height=hpanel1+hpanel2+hpanel3+4*dxy;       %figure height

-        

-    %**********************************************************************

-    % Create GUI

-    %**********************************************************************

-    h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI');

-    h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1));

-    h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1));

-    h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1));

-    

-    %%Control GUI-elements

-    %Slider: Current time

-    h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left');

-    h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],...

-        'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel);

-    temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min');

-    set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);

-    h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),...

-        'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel);

-

-    %Slider: MaxTime

-    h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left');

-    h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],...

-        'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

-    h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),...

-        'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

-    

-    %Slider: MinTime

-    h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left');

-    h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],...

-        'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

-    h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),...

-        'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);

-

-    %%Current data/state GUI-elements (Multiline-edit-box)

-    h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',...

-                                    'HorizontalAlignment','left','parent',h.aircraftstatepanel);

-    

-    h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',...

-                                'HorizontalAlignment','left','parent',h.guistatepanel);

-    

-end

-

-%% ************************************************************************

-%  INITPLOTGUI (nested function)

-%  ************************************************************************

-function InitPlotGUI()

-    

-    % Setup handles to lines and text

-    h.markertext=[];

-    templinehandle=0.0;%line([0 1],[0 5]);   % Just a temporary handle to init array

-    h.markerline(1:NrAxes)=templinehandle;   % the actual handle-array to the lines  - these are numbered consecutively

-    h.markerline(1:NrAxes)=0.0;

-        

-    % Setup all other figures and axes for plotting

-    %  PLOT WINDOW 1: GPS POSITION

-    h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position');

-    h.axes(1)=axes();

-    set(h.axes(1),'Parent',h.figures(2));

-

-    %  PLOT WINDOW 2: IMU, baro altitude

-    h.figures(3)=figure('Name', 'IMU / Baro Altitude');

-    h.axes(2)=subplot(4,1,1);

-    h.axes(3)=subplot(4,1,2);

-    h.axes(4)=subplot(4,1,3);

-    h.axes(5)=subplot(4,1,4);

-    set(h.axes(2:5),'Parent',h.figures(3));

-    

-    %  PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...

-    h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds');

-    h.axes(6)=subplot(4,1,1);

-    h.axes(7)=subplot(4,1,2);

-    h.axes(8)=subplot(4,1,3);

-    h.axes(9)=subplot(4,1,4);

-    set(h.axes(6:9),'Parent',h.figures(4));

-    

-    %  PLOT WINDOW 4: LOG STATS

-    h.figures(5) = figure('Name', 'Log Statistics');

-    h.axes(10)=subplot(1,1,1);

-    set(h.axes(10:10),'Parent',h.figures(5));

-    

-end

-

-%% ************************************************************************

-%  DRAWRAWDATA (nested function)

-%  ************************************************************************

-%Draws the raw data from the sysvector, but does not add any

-%marker-lines or so

-function DrawRawData() 

-    % ************************************************************************

-    %  PLOT WINDOW 1: GPS POSITION & GUI

-    %  ************************************************************************

-    figure(h.figures(2));

-    % Only plot GPS data if available

-    if (sum(double(sysvector.gps_raw_position(imintime:imaxtime,1)))>0) && (bDisplayGPS)

-        %Draw data

-        plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:imaxtime,1))*fconv_gpslatlong, ...

-                        double(sysvector.gps_raw_position(imintime:imaxtime,2))*fconv_gpslatlong, ...

-                        double(sysvector.gps_raw_position(imintime:imaxtime,3))*fconv_gpsalt,'r.');

-        title(h.axes(1),'GPS Position Data(if available)');

-        xlabel(h.axes(1),'Latitude [deg]');

-        ylabel(h.axes(1),'Longitude [deg]');

-        zlabel(h.axes(1),'Altitude above MSL [m]');

-        grid on

-        

-        %Reset path

-        h.pathpoints=0;

-    end

-

-    % ************************************************************************

-    %  PLOT WINDOW 2: IMU, baro altitude

-    %  ************************************************************************

-    figure(h.figures(3));

-    plot(h.axes(2),time(imintime:imaxtime),sysvector.mag(imintime:imaxtime,:));

-    title(h.axes(2),'Magnetometers [Gauss]');

-    legend(h.axes(2),'x','y','z');

-    plot(h.axes(3),time(imintime:imaxtime),sysvector.accel(imintime:imaxtime,:));

-    title(h.axes(3),'Accelerometers [m/s²]');

-    legend(h.axes(3),'x','y','z');

-    plot(h.axes(4),time(imintime:imaxtime),sysvector.gyro(imintime:imaxtime,:));

-    title(h.axes(4),'Gyroscopes [rad/s]');

-    legend(h.axes(4),'x','y','z');

-    plot(h.axes(5),time(imintime:imaxtime),sysvector.baro_alt(imintime:imaxtime),'color','blue');

-    if(bDisplayGPS)

-        hold on;

-        plot(h.axes(5),time(imintime:imaxtime),double(sysvector.gps_raw_position(imintime:imaxtime,3)).*fconv_gpsalt,'color','red');

-        hold off

-        legend('Barometric Altitude [m]','GPS Altitude [m]');

-    else

-        legend('Barometric Altitude [m]');

-    end

-    title(h.axes(5),'Altitude above MSL [m]');

-

-    % ************************************************************************

-    %  PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...

-    % ************************************************************************

-    figure(h.figures(4));

-    %Attitude Estimate

-    plot(h.axes(6),time(imintime:imaxtime), sysvector.attitude(imintime:imaxtime,:).*180./3.14159);

-    title(h.axes(6),'Estimated attitude [deg]');

-    legend(h.axes(6),'roll','pitch','yaw');

-    %Actuator Controls

-    plot(h.axes(7),time(imintime:imaxtime), sysvector.actuator_control(imintime:imaxtime,:));

-    title(h.axes(7),'Actuator control [-]');

-    legend(h.axes(7),'0','1','2','3');

-    %Actuator Controls

-    plot(h.axes(8),time(imintime:imaxtime), sysvector.actuators(imintime:imaxtime,1:8));

-    title(h.axes(8),'Actuator PWM (raw-)outputs [µs]');

-    legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8');

-    set(h.axes(8), 'YLim',[800 2200]);

-    %Airspeeds

-    plot(h.axes(9),time(imintime:imaxtime), sysvector.ind_airspeed(imintime:imaxtime));

-    hold on

-    plot(h.axes(9),time(imintime:imaxtime), sysvector.true_airspeed(imintime:imaxtime));

-    hold off

-    %add GPS total airspeed here

-    title(h.axes(9),'Airspeed [m/s]');

-    legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed');

-    %calculate time differences and plot them

-    intervals = zeros(0,imaxtime - imintime);

-    for k = imintime+1:imaxtime

-        intervals(k) = time(k) - time(k-1);

-    end

-    plot(h.axes(10), time(imintime:imaxtime), intervals);

-

-    %Set same timescale for all plots

-    for i=2:NrAxes

-        set(h.axes(i),'XLim',[mintime maxtime]);

-    end

-    

-    set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);

-end

-

-%% ************************************************************************

-%  DRAWCURRENTAIRCRAFTSTATE(nested function)

-%  ************************************************************************

-function DrawCurrentAircraftState()

-    %find current data index        

-    i=find(time>=CurTime,1,'first');

-

-    %**********************************************************************

-    % Current aircraft state label update

-    %**********************************************************************

-    acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong),'°, ',...

-                        'lon=',num2str(double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong),'°, ',...

-                        'alt=',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]'];

-    acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.mag(i,1)),...

-                               ', y=',num2str(sysvector.mag(i,2)),...

-                               ', z=',num2str(sysvector.mag(i,3)),']'];

-    acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.accel(i,1)),...

-                               ', y=',num2str(sysvector.accel(i,2)),...

-                               ', z=',num2str(sysvector.accel(i,3)),']'];

-    acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.gyro(i,1)),...

-                               ', y=',num2str(sysvector.gyro(i,2)),...

-                               ', z=',num2str(sysvector.gyro(i,3)),']'];

-    acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.baro_alt(i)),'m, GPS: ',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]'];

-    acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.attitude(i,1).*180./3.14159),...

-                               ', Pitch=',num2str(sysvector.attitude(i,2).*180./3.14159),...

-                               ', Yaw=',num2str(sysvector.attitude(i,3).*180./3.14159),']'];

-    acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:');

-    for j=1:4

-        acstate{7,:}=[acstate{7,:},num2str(sysvector.actuator_control(i,j)),','];

-    end

-    acstate{7,:}=[acstate{7,:},']'];

-    acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:');

-    for j=1:8

-        acstate{8,:}=[acstate{8,:},num2str(sysvector.actuators(i,j)),','];

-    end

-    acstate{8,:}=[acstate{8,:},']'];

-    acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.ind_airspeed(i)),', TAS: ',num2str(sysvector.true_airspeed(i)),']'];

-    

-    set(h.edits.AircraftState,'String',acstate);

-    

-    %**********************************************************************

-    % GPS Plot Update

-    %**********************************************************************

-    %Plot traveled path, and  and time.

-    figure(h.figures(2));

-    hold on;

-    if(CurTime>mintime+1) %the +1 is only a small bugfix

-        h.pathline=plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:i,1))*fconv_gpslatlong, ...

-                                    double(sysvector.gps_raw_position(imintime:i,2))*fconv_gpslatlong, ...

-                                    double(sysvector.gps_raw_position(imintime:i,3))*fconv_gpsalt,'b','LineWidth',2); 

-    end;

-    hold off

-    %Plot current position

-    newpoint=[double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong double(sysvector.gps_raw_position(i,3))*fconv_gpsalt];

-    if(numel(h.pathpoints)<=3) %empty path

-        h.pathpoints(1,1:3)=newpoint;

-    else %Not empty, append new point 

-        h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint;

-    end

-    axes(h.axes(1));    

-    line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20);

-    

-    

-    % Plot current time (small label next to current gps position) 

-    textdesc=strcat('  t=',num2str(time(i)),'s');

-    if(isvalidhandle(h.markertext))

-            delete(h.markertext); %delete old text

-    end 

-    h.markertext=text(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong,double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong,...

-                        double(sysvector.gps_raw_position(i,3))*fconv_gpsalt,textdesc);

-    set(h.edits.CurTime,'String',CurTime);

-        

-    %**********************************************************************

-    % Plot the lines showing the current time in the 2-d plots

-    %**********************************************************************

-    for i=2:NrAxes

-        if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end

-        ylims=get(h.axes(i),'YLim');

-        h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black');

-        set(h.markerline(i),'parent',h.axes(i));

-    end

-    

-    set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);

-end

-

-%% ************************************************************************

-%  MINMAXTIME CALLBACK (nested function)

-%  ************************************************************************

-function minmaxtime_callback(hObj,event) %#ok<INUSL>

-    new_mintime=get(h.sliders.MinTime,'Value');

-    new_maxtime=get(h.sliders.MaxTime,'Value');

-

-    %Safety checks:

-    bErr=false;

-    %1: mintime must be < maxtime

-    if((new_mintime>maxtime) || (new_maxtime<mintime))

-        set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than maxtime! Values were not changed.','BackgroundColor','red');

-        bErr=true;

-    else

-        %2: MinTime must be <=CurTime

-        if(new_mintime>CurTime)

-            set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red');

-            mintime=new_mintime;

-            CurTime=mintime;

-            bErr=true;

-        end

-        %3: MaxTime must be >CurTime

-        if(new_maxtime<CurTime)

-            set(h.labels.GUIState,'String','Error: Maxtime cannot be smaller than CurTime! CurTime set to new maxtime.','BackgroundColor','red');

-            maxtime=new_maxtime;

-            CurTime=maxtime;

-            bErr=true;

-        end

-    end

-

-    if(bErr==false)

-        maxtime=new_maxtime;

-        mintime=new_mintime;

-    end

-

-    %Needs to be done in case values were reset above

-    set(h.sliders.MinTime,'Value',mintime);

-    set(h.sliders.MaxTime,'Value',maxtime);

-

-    %Update curtime-slider

-    set(h.sliders.CurTime,'Value',CurTime);

-    set(h.sliders.CurTime,'Max',maxtime);

-    set(h.sliders.CurTime,'Min',mintime);

-    temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); 

-    set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);  %Set Stepsize to constant [in seconds]

-

-    %update edit fields

-    set(h.edits.CurTime,'String',get(h.sliders.CurTime,'Value'));

-    set(h.edits.MinTime,'String',get(h.sliders.MinTime,'Value'));

-    set(h.edits.MaxTime,'String',get(h.sliders.MaxTime,'Value'));

-

-    %Finally, we have to redraw. Update time indices first.

-    [imintime,imaxtime]=FindMinMaxTimeIndices();

-    DrawRawData();                  %Rawdata only

-    DrawCurrentAircraftState();     %path info & markers

-end

-

-

-%% ************************************************************************

-%  CURTIME CALLBACK (nested function)

-%  ************************************************************************

-function curtime_callback(hObj,event) %#ok<INUSL>

-    %find current time

-    if(hObj==h.sliders.CurTime)

-        CurTime=get(h.sliders.CurTime,'Value');

-    elseif (hObj==h.edits.CurTime)

-        temp=str2num(get(h.edits.CurTime,'String'));

-        if(temp<maxtime && temp>mintime)

-            CurTime=temp;

-        else

-            %Error

-            set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red');

-        end

-    else

-        %Error

-        set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red');        

-    end

-    

-    set(h.sliders.CurTime,'Value',CurTime);

-    set(h.edits.CurTime,'String',num2str(CurTime));

-    

-    %Redraw time markers, but don't have to redraw the whole raw data

-    DrawCurrentAircraftState();

-end

-

-%% ************************************************************************

-%  FINDMINMAXINDICES (nested function)

-%  ************************************************************************

-function [idxmin,idxmax] = FindMinMaxTimeIndices()

-    for i=1:size(sysvector.timestamp,1)

-        if time(i)>=mintime; idxmin=i; break; end

-    end

-    for i=1:size(sysvector.timestamp,1)

-        if maxtime==0; idxmax=size(sysvector.timestamp,1); break; end

-        if time(i)>=maxtime; idxmax=i; break; end

-    end

-    mintime=time(idxmin); 

-    maxtime=time(idxmax);

-end

-

-%% ************************************************************************

-%  ISVALIDHANDLE (nested function)

-%  ************************************************************************

-function isvalid = isvalidhandle(handle)

-   if(exist(varname(handle))>0 && length(ishandle(handle))>0) 

-        if(ishandle(handle)>0)

-            if(handle>0.0)

-                isvalid=true;

-                return;

-            end

-        end

-   end

-   isvalid=false;

-end

-

-%% ************************************************************************

-%  JUST SOME SMALL HELPER FUNCTIONS (nested function)

-%  ************************************************************************

-function out = varname(var)

-   out = inputname(1);

-end

-

-%This is the end of the matlab file / the main function

-end

diff --git a/Tools/README.txt b/Tools/README.txt
new file mode 100644
index 000000000..abeb9a4c7
--- /dev/null
+++ b/Tools/README.txt
@@ -0,0 +1,13 @@
+====== PX4 LOG CONVERSION ======
+
+On each log session (commonly started and stopped by arming and disarming the vehicle) a new file logxxx.bin is created. In many cases there will be only one logfile named log001.bin (only one flight).
+
+There are two conversion scripts in this ZIP file:
+
+logconv.m: This is a MATLAB script which will automatically convert and display the flight data with a GUI. If running this script, the second script can be ignored.
+
+sdlog2_dump.py: This is a Python script (compatible with v2 and v3) which converts the self-describing binary log format to a CSV file. To export a CSV file from within a shell (Windows CMD or BASH on Linux / Mac OS), run:
+
+python sdlog2_dump.py log001.bin -f "export.csv" -t "TIME" -d "," -n ""
+
+Python can be downloaded from http://python.org, but is available as default on Mac OS and Linux.
\ No newline at end of file
diff --git a/Tools/logconv.m b/Tools/logconv.m
index f2ae6e5f3..c416b8095 100644
--- a/Tools/logconv.m
+++ b/Tools/logconv.m
@@ -111,9 +111,9 @@ function ImportPX4LogData()
         time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1);

         time_s = uint64(time_us*1e-6);

         time_m = uint64(time_s/60);

-        time_s = time_s - time_m * 60

+        time_s = time_s - time_m * 60;

         

-        disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s)]);

+        disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]);

 

         disp(['logfile conversion finished.' char(10)]);

     else

diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py
index ebc04f4d0..7fefc5908 100644
--- a/Tools/sdlog2_dump.py
+++ b/Tools/sdlog2_dump.py
@@ -1,6 +1,8 @@
 #!/usr/bin/env python
 
-"""Dump binary log generated by sdlog2 or APM as CSV
+from __future__ import print_function
+
+"""Dump binary log generated by PX4's sdlog2 or APM as CSV
     
 Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]]
     
@@ -21,6 +23,11 @@ __version__ = "1.2"
 
 import struct, sys
 
+if sys.hexversion >= 0x030000F0:
+    runningPython3 = True
+else:
+    runningPython3 = False
+
 class SDLog2Parser:
     BLOCK_SIZE = 8192
     MSG_HEADER_LEN = 3
@@ -65,7 +72,7 @@ class SDLog2Parser:
         self.__msg_descrs = {}      # message descriptions by message type map
         self.__msg_labels = {}      # message labels by message name map
         self.__msg_names = []       # message names in the same order as FORMAT messages
-        self.__buffer = ""          # buffer for input binary data
+        self.__buffer = bytearray() # buffer for input binary data
         self.__ptr = 0              # read pointer in buffer
         self.__csv_columns = []     # CSV file columns in correct order in format "MSG.label"
         self.__csv_data = {}        # current values for all columns
@@ -105,7 +112,7 @@ class SDLog2Parser:
             for msg_name, show_fields in self.__msg_filter:
                 self.__msg_filter_map[msg_name] = show_fields
         first_data_msg = True
-        f = open(fn, "r")
+        f = open(fn, "rb")
         bytes_read = 0
         while True:
             chunk = f.read(self.BLOCK_SIZE)
@@ -114,15 +121,15 @@ class SDLog2Parser:
             self.__buffer = self.__buffer[self.__ptr:] + chunk
             self.__ptr = 0
             while self.__bytesLeft() >= self.MSG_HEADER_LEN:
-                head1 = ord(self.__buffer[self.__ptr])
-                head2 = ord(self.__buffer[self.__ptr+1])
+                head1 = self.__buffer[self.__ptr]
+                head2 = self.__buffer[self.__ptr+1]
                 if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2):
                     if self.__correct_errors:
                         self.__ptr += 1
                         continue
                     else:
                         raise Exception("Invalid header at %i (0x%X): %02X %02X, must be %02X %02X" % (bytes_read + self.__ptr, bytes_read + self.__ptr, head1, head2, self.MSG_HEAD1, self.MSG_HEAD2))
-                msg_type = ord(self.__buffer[self.__ptr+2])
+                msg_type = self.__buffer[self.__ptr+2]
                 if msg_type == self.MSG_TYPE_FORMAT:
                     # parse FORMAT message
                     if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN:
@@ -168,9 +175,9 @@ class SDLog2Parser:
                 self.__csv_columns.append(full_label)
                 self.__csv_data[full_label] = None
         if self.__file != None:
-                print >> self.__file, self.__csv_delim.join(self.__csv_columns)
+                print(self.__csv_delim.join(self.__csv_columns), file=self.__file)
         else:
-                print self.__csv_delim.join(self.__csv_columns)
+                print(self.__csv_delim.join(self.__csv_columns))
 
     def __printCSVRow(self):
         s = []
@@ -183,18 +190,26 @@ class SDLog2Parser:
             s.append(v)
 
         if self.__file != None:
-                print >> self.__file, self.__csv_delim.join(s)
+                print(self.__csv_delim.join(s), file=self.__file)
         else:
-                print self.__csv_delim.join(s)
+                print(self.__csv_delim.join(s))
 
     def __parseMsgDescr(self):
-        data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])
+        if runningPython3:
+            data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])
+        else:
+            data = struct.unpack(self.MSG_FORMAT_STRUCT, str(self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]))
         msg_type = data[0]
         if msg_type != self.MSG_TYPE_FORMAT:
             msg_length = data[1]
-            msg_name = data[2].strip("\0")
-            msg_format = data[3].strip("\0")
-            msg_labels = data[4].strip("\0").split(",")
+            if runningPython3:
+                msg_name = str(data[2], 'ascii').strip("\0")
+                msg_format = str(data[3], 'ascii').strip("\0")
+                msg_labels = str(data[4], 'ascii').strip("\0").split(",")
+            else:
+                msg_name = str(data[2]).strip("\0")
+                msg_format = str(data[3]).strip("\0")
+                msg_labels = str(data[4]).strip("\0").split(",")
             # Convert msg_format to struct.unpack format string
             msg_struct = ""
             msg_mults = []
@@ -211,8 +226,8 @@ class SDLog2Parser:
             self.__msg_names.append(msg_name)
             if self.__debug_out:
                 if self.__filterMsg(msg_name) != None:
-                    print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % (
-                                msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults)
+                    print("MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % (
+                                msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults))
         self.__ptr += self.MSG_FORMAT_PACKET_LEN
     
     def __parseMsg(self, msg_descr):
@@ -222,8 +237,11 @@ class SDLog2Parser:
             self.__csv_updated = False
         show_fields = self.__filterMsg(msg_name)
         if (show_fields != None):
-            data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))
-            for i in xrange(len(data)):
+            if runningPython3:
+                data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))
+            else:
+                data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])))
+            for i in range(len(data)):
                 if type(data[i]) is str:
                     data[i] = data[i].strip("\0")
                 m = msg_mults[i]
@@ -231,14 +249,14 @@ class SDLog2Parser:
                     data[i] = data[i] * m
             if self.__debug_out:
                 s = []
-                for i in xrange(len(data)):
+                for i in range(len(data)):
                     label = msg_labels[i]
                     if show_fields == "*" or label in show_fields:
                         s.append(label + "=" + str(data[i]))
-                print "MSG %s: %s" % (msg_name, ", ".join(s))
+                print("MSG %s: %s" % (msg_name, ", ".join(s)))
             else:
                 # update CSV data buffer
-                for i in xrange(len(data)):
+                for i in range(len(data)):
                     label = msg_labels[i]
                     if label in show_fields:
                         self.__csv_data[msg_name + "_" + label] = data[i]
@@ -250,14 +268,14 @@ class SDLog2Parser:
 
 def _main():
     if len(sys.argv) < 2:
-        print "Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n"
-        print "\t-v\tUse plain debug output instead of CSV.\n"
-        print "\t-e\tRecover from errors.\n"
-        print "\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n"
-        print "\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n"
-        print "\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed."
-        print "\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n"
-        print "\t-fPrint to file instead of stdout"
+        print("Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n")
+        print("\t-v\tUse plain debug output instead of CSV.\n")
+        print("\t-e\tRecover from errors.\n")
+        print("\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n")
+        print("\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n")
+        print("\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed.")
+        print("\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n")
+        print("\t-fPrint to file instead of stdout")
         return
     fn = sys.argv[1]
     debug_out = False
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index f1cfc45e6..db13cc197 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -75,6 +75,7 @@ MODULES		+= examples/flow_position_estimator
 #
 # Vehicle Control
 #
+MODULES		+= modules/segway
 MODULES		+= modules/fixedwing_backside
 MODULES		+= modules/fixedwing_att_control
 MODULES		+= modules/fixedwing_pos_control
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index bcf456c56..6a4795c46 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -534,9 +534,10 @@ CONFIG_USBDEV=y
 #
 # CONFIG_USBDEV_ISOCHRONOUS is not set
 # CONFIG_USBDEV_DUALSPEED is not set
-CONFIG_USBDEV_SELFPOWERED=y
-# CONFIG_USBDEV_BUSPOWERED is not set
+# CONFIG_USBDEV_SELFPOWERED is not set
+CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
+# CONFIG_USBDEV_REMOTEWAKEUP is not set
 # CONFIG_USBDEV_DMA is not set
 # CONFIG_USBDEV_TRACE is not set
 
diff --git a/src/drivers/md25/BlockSysIdent.cpp b/src/drivers/md25/BlockSysIdent.cpp
new file mode 100644
index 000000000..23b0724d8
--- /dev/null
+++ b/src/drivers/md25/BlockSysIdent.cpp
@@ -0,0 +1,8 @@
+#include "BlockSysIdent.hpp"
+
+BlockSysIdent::BlockSysIdent() :
+	Block(NULL, "SYSID"),
+	_freq(this, "FREQ"),
+	_ampl(this, "AMPL")
+{
+}
diff --git a/src/drivers/md25/BlockSysIdent.hpp b/src/drivers/md25/BlockSysIdent.hpp
new file mode 100644
index 000000000..270635f40
--- /dev/null
+++ b/src/drivers/md25/BlockSysIdent.hpp
@@ -0,0 +1,10 @@
+#include <controllib/block/Block.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+class BlockSysIdent : public control::Block {
+public:
+	BlockSysIdent();
+private:
+	control::BlockParam<float> _freq;
+	control::BlockParam<float> _ampl;
+};
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
index 71932ad65..d43e3aef9 100644
--- a/src/drivers/md25/md25.cpp
+++ b/src/drivers/md25/md25.cpp
@@ -45,9 +45,16 @@
 #include "md25.hpp"
 #include <poll.h>
 #include <stdio.h>
+#include <math.h>
+#include <string.h>
 
 #include <systemlib/err.h>
 #include <arch/board/board.h>
+#include <mavlink/mavlink_log.h>
+
+#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/topics/debug_key_value.h>
+#include <drivers/drv_hrt.h>
 
 // registers
 enum {
@@ -72,6 +79,9 @@ enum {
 	REG_COMMAND_RW,
 };
 
+// File descriptors
+static int mavlink_fd;
+
 MD25::MD25(const char *deviceName, int bus,
 	   uint16_t address, uint32_t speed) :
 	I2C("MD25", deviceName, bus, address, speed),
@@ -106,7 +116,8 @@ MD25::MD25(const char *deviceName, int bus,
 	setMotor2Speed(0);
 	resetEncoders();
 	_setMode(MD25::MODE_UNSIGNED_SPEED);
-	setSpeedRegulation(true);
+	setSpeedRegulation(false);
+	setMotorAccel(10);
 	setTimeout(true);
 }
 
@@ -298,6 +309,12 @@ int MD25::setDeviceAddress(uint8_t address)
 	return OK;
 }
 
+int MD25::setMotorAccel(uint8_t accel)
+{
+	return _writeUint8(REG_ACCEL_RATE_RW,
+			   accel);
+}
+
 int MD25::setMotor1Speed(float value)
 {
 	return _writeUint8(REG_SPEED1_RW,
@@ -451,12 +468,12 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 	MD25 md25("/dev/md25", bus, address);
 
 	// print status
-	char buf[200];
+	char buf[400];
 	md25.status(buf, sizeof(buf));
 	printf("%s\n", buf);
 
 	// setup for test
-	md25.setSpeedRegulation(true);
+	md25.setSpeedRegulation(false);
 	md25.setTimeout(true);
 	float dt = 0.1;
 	float speed = 0.2;
@@ -550,4 +567,68 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 	return 0;
 }
 
+int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency)
+{
+	printf("md25 sine: starting\n");
+
+	// setup
+	MD25 md25("/dev/md25", bus, address);
+
+	// print status
+	char buf[400];
+	md25.status(buf, sizeof(buf));
+	printf("%s\n", buf);
+
+	// setup for test
+	md25.setSpeedRegulation(false);
+	md25.setTimeout(true);
+	float dt = 0.01;
+	float t_final = 60.0;
+	float prev_revolution = md25.getRevolutions1();
+
+	// debug publication
+	control::UOrbPublication<debug_key_value_s> debug_msg(NULL,
+			ORB_ID(debug_key_value));
+
+	// sine wave for motor 1
+	md25.resetEncoders();
+	while (true) {
+
+		// input
+		uint64_t timestamp = hrt_absolute_time();
+		float t = timestamp/1000000.0f;
+
+		float input_value = amplitude*sinf(2*M_PI*frequency*t);
+		md25.setMotor1Speed(input_value);
+
+		// output
+		md25.readData();
+		float current_revolution = md25.getRevolutions1();
+
+		// send input message
+		//strncpy(debug_msg.key, "md25 in   ", 10);
+		//debug_msg.timestamp_ms = 1000*timestamp;
+		//debug_msg.value = input_value;
+		//debug_msg.update();
+
+		// send output message
+		strncpy(debug_msg.key, "md25 out  ", 10);
+		debug_msg.timestamp_ms = 1000*timestamp;
+		debug_msg.value = current_revolution;
+		debug_msg.update();
+
+		if (t > t_final) break;
+
+		// update for next step
+		prev_revolution = current_revolution;
+
+		// sleep
+		usleep(1000000 * dt);
+	}
+	md25.setMotor1Speed(0);
+
+	printf("md25 sine complete\n");
+	return 0;
+}
+
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp
index e77511b16..1661f67f9 100644
--- a/src/drivers/md25/md25.hpp
+++ b/src/drivers/md25/md25.hpp
@@ -46,7 +46,7 @@
 
 #include <poll.h>
 #include <stdio.h>
-#include <controllib/block/UOrbSubscription.hpp>
+#include <controllib/uorb/UOrbSubscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <drivers/device/i2c.h>
 
@@ -213,6 +213,19 @@ public:
 	int setDeviceAddress(uint8_t address);
 
 	/**
+	 * set motor acceleration
+	 * @param accel
+	 * controls motor speed change (1-10)
+	 * accel rate  | time for full fwd. to full rev.
+	 *  1          | 6.375 s
+	 *  2          | 1.6 s
+	 *  3          | 0.675 s
+	 *  5(default) | 1.275 s
+	 * 10          | 0.65 s
+	 */
+	int setMotorAccel(uint8_t accel);
+
+	/**
 	 * set motor 1 speed
 	 * @param normSpeed normalize speed between -1 and 1
 	 * @return non-zero -> error
@@ -290,4 +303,7 @@ private:
 // unit testing
 int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
 
+// sine testing
+int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency);
+
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp
index e62c46b0d..7e5904d05 100644
--- a/src/drivers/md25/md25_main.cpp
+++ b/src/drivers/md25/md25_main.cpp
@@ -82,7 +82,7 @@ usage(const char *reason)
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
 
-	fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n");
+	fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n");
 	exit(1);
 }
 
@@ -136,6 +136,28 @@ int md25_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "sine")) {
+
+		if (argc < 6) {
+			printf("usage: md25 sine bus address amp freq\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		float amplitude = atof(argv[4]);
+
+		float frequency = atof(argv[5]);
+
+		md25Sine(deviceName, bus, address, amplitude, frequency);
+
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "probe")) {
 		if (argc < 4) {
 			printf("usage: md25 probe bus address\n");
@@ -162,6 +184,29 @@ int md25_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "read")) {
+		if (argc < 4) {
+			printf("usage: md25 read bus address\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		MD25 md25(deviceName, bus, address);
+
+		// print status
+		char buf[400];
+		md25.status(buf, sizeof(buf));
+		printf("%s\n", buf);
+
+		exit(0);
+	}
+	
+
 	if (!strcmp(argv[1], "search")) {
 		if (argc < 3) {
 			printf("usage: md25 search bus\n");
@@ -246,7 +291,7 @@ int md25_thread_main(int argc, char *argv[])
 	uint8_t address = strtoul(argv[4], nullptr, 0);
 
 	// start
-	MD25 md25("/dev/md25", bus, address);
+	MD25 md25(deviceName, bus, address);
 
 	thread_running = true;
 
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index 49d0d157d..f01ee0355 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -47,8 +47,8 @@
 #include <mathlib/mathlib.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
-#include <controllib/block/UOrbSubscription.hpp>
-#include <controllib/block/UOrbPublication.hpp>
+#include <controllib/uorb/UOrbSubscription.hpp>
+#include <controllib/uorb/UOrbPublication.hpp>
 
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp
index 5994d2315..b964d40a3 100644
--- a/src/modules/controllib/block/Block.cpp
+++ b/src/modules/controllib/block/Block.cpp
@@ -43,8 +43,8 @@
 
 #include "Block.hpp"
 #include "BlockParam.hpp"
-#include "UOrbSubscription.hpp"
-#include "UOrbPublication.hpp"
+#include "../uorb/UOrbSubscription.hpp"
+#include "../uorb/UOrbPublication.hpp"
 
 namespace control
 {
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 7a785d12e..fefe99702 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -42,6 +42,7 @@
 #include <assert.h>
 #include <time.h>
 #include <stdlib.h>
+#include <math.h>
 #include <mathlib/math/test/test.hpp>
 
 #include "block/Block.hpp"
diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk
index 13d1069c7..d815a8feb 100644
--- a/src/modules/controllib/module.mk
+++ b/src/modules/controllib/module.mk
@@ -37,7 +37,7 @@
 SRCS		 = test_params.c \
 		   block/Block.cpp \
 		   block/BlockParam.cpp \
-		   block/UOrbPublication.cpp \
-		   block/UOrbSubscription.cpp \
-		   blocks.cpp \
-		   fixedwing.cpp
+		   uorb/UOrbPublication.cpp \
+		   uorb/UOrbSubscription.cpp \
+		   uorb/blocks.cpp \
+		   blocks.cpp
diff --git a/src/modules/controllib/block/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp
index f69b39d90..f69b39d90 100644
--- a/src/modules/controllib/block/UOrbPublication.cpp
+++ b/src/modules/controllib/uorb/UOrbPublication.cpp
diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/uorb/UOrbPublication.hpp
index 0a8ae2ff7..6f1f3fc1c 100644
--- a/src/modules/controllib/block/UOrbPublication.hpp
+++ b/src/modules/controllib/uorb/UOrbPublication.hpp
@@ -39,8 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include "Block.hpp"
-#include "List.hpp"
+#include "../block/Block.hpp"
+#include "../block/List.hpp"
 
 
 namespace control
diff --git a/src/modules/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/uorb/UOrbSubscription.cpp
index 022cadd24..022cadd24 100644
--- a/src/modules/controllib/block/UOrbSubscription.cpp
+++ b/src/modules/controllib/uorb/UOrbSubscription.cpp
diff --git a/src/modules/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/uorb/UOrbSubscription.hpp
index 22cc2e114..d337d89a8 100644
--- a/src/modules/controllib/block/UOrbSubscription.hpp
+++ b/src/modules/controllib/uorb/UOrbSubscription.hpp
@@ -39,8 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include "Block.hpp"
-#include "List.hpp"
+#include "../block/Block.hpp"
+#include "../block/List.hpp"
 
 
 namespace control
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
new file mode 100644
index 000000000..448a42a99
--- /dev/null
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uorb_blocks.cpp
+ *
+ * uorb block library code
+ */
+
+#include "blocks.hpp"
+
+namespace control
+{
+
+BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_xtYawLimit(this, "XT2YAW"),
+	_xt2Yaw(this, "XT2YAW"),
+	_psiCmd(0)
+{
+}
+
+BlockWaypointGuidance::~BlockWaypointGuidance() {};
+
+void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
+				   vehicle_attitude_s &att,
+				   vehicle_global_position_setpoint_s &posCmd,
+				   vehicle_global_position_setpoint_s &lastPosCmd)
+{
+
+	// heading to waypoint
+	float psiTrack = get_bearing_to_next_waypoint(
+				 (double)pos.lat / (double)1e7d,
+				 (double)pos.lon / (double)1e7d,
+				 (double)posCmd.lat / (double)1e7d,
+				 (double)posCmd.lon / (double)1e7d);
+
+	// cross track
+	struct crosstrack_error_s xtrackError;
+	get_distance_to_line(&xtrackError,
+			     (double)pos.lat / (double)1e7d,
+			     (double)pos.lon / (double)1e7d,
+			     (double)lastPosCmd.lat / (double)1e7d,
+			     (double)lastPosCmd.lon / (double)1e7d,
+			     (double)posCmd.lat / (double)1e7d,
+			     (double)posCmd.lon / (double)1e7d);
+
+	_psiCmd = _wrap_2pi(psiTrack -
+			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
+}
+
+BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	// subscriptions
+	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
+	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
+	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
+	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
+	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
+	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
+	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
+	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	// publications
+	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
+{
+}
+
+BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
+
+} // namespace control
+
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
new file mode 100644
index 000000000..9c0720aa5
--- /dev/null
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uorb_blocks.h
+ *
+ * uorb block library code
+ */
+
+#pragma once
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_set_triplet.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <drivers/drv_hrt.h>
+#include <poll.h>
+
+extern "C" {
+#include <systemlib/geo/geo.h>
+}
+
+#include "../blocks.hpp"
+#include "UOrbSubscription.hpp"
+#include "UOrbPublication.hpp"
+
+namespace control
+{
+
+/**
+ * Waypoint Guidance block
+ */
+class __EXPORT BlockWaypointGuidance : public SuperBlock
+{
+private:
+	BlockLimitSym _xtYawLimit;
+	BlockP _xt2Yaw;
+	float _psiCmd;
+public:
+	BlockWaypointGuidance(SuperBlock *parent, const char *name);
+	virtual ~BlockWaypointGuidance();
+	void update(vehicle_global_position_s &pos,
+		    vehicle_attitude_s &att,
+		    vehicle_global_position_setpoint_s &posCmd,
+		    vehicle_global_position_setpoint_s &lastPosCmd);
+	float getPsiCmd() { return _psiCmd; }
+};
+
+/**
+ * UorbEnabledAutopilot
+ */
+class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
+{
+protected:
+	// subscriptions
+	UOrbSubscription<vehicle_attitude_s> _att;
+	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
+	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
+	UOrbSubscription<vehicle_global_position_s> _pos;
+	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
+	UOrbSubscription<manual_control_setpoint_s> _manual;
+	UOrbSubscription<vehicle_status_s> _status;
+	UOrbSubscription<parameter_update_s> _param_update;
+	// publications
+	UOrbPublication<actuator_controls_s> _actuators;
+public:
+	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
+	virtual ~BlockUorbEnabledAutopilot();
+};
+
+} // namespace control
+
diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index 77b2ac806..f655a13bd 100644
--- a/src/modules/controllib/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -86,61 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
 	_yawDamper.update(rCmd, r, outputScale);
 }
 
-BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	_xtYawLimit(this, "XT2YAW"),
-	_xt2Yaw(this, "XT2YAW"),
-	_psiCmd(0)
-{
-}
-
-BlockWaypointGuidance::~BlockWaypointGuidance() {};
-
-void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
-				   vehicle_attitude_s &att,
-				   vehicle_global_position_setpoint_s &posCmd,
-				   vehicle_global_position_setpoint_s &lastPosCmd)
-{
-
-	// heading to waypoint
-	float psiTrack = get_bearing_to_next_waypoint(
-				 (double)pos.lat / (double)1e7d,
-				 (double)pos.lon / (double)1e7d,
-				 (double)posCmd.lat / (double)1e7d,
-				 (double)posCmd.lon / (double)1e7d);
-
-	// cross track
-	struct crosstrack_error_s xtrackError;
-	get_distance_to_line(&xtrackError,
-			     (double)pos.lat / (double)1e7d,
-			     (double)pos.lon / (double)1e7d,
-			     (double)lastPosCmd.lat / (double)1e7d,
-			     (double)lastPosCmd.lon / (double)1e7d,
-			     (double)posCmd.lat / (double)1e7d,
-			     (double)posCmd.lon / (double)1e7d);
-
-	_psiCmd = _wrap_2pi(psiTrack -
-			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
-}
-
-BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	// subscriptions
-	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
-	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
-	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
-	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
-	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
-	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
-	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
-	// publications
-	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
-{
-}
-
-BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
-
 BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
 	BlockUorbEnabledAutopilot(parent, name),
 	_stabilization(this, ""), // no name needed, already unique
diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp
index e4028c40d..3876e4630 100644
--- a/src/modules/controllib/fixedwing.hpp
+++ b/src/modules/fixedwing_backside/fixedwing.hpp
@@ -39,31 +39,8 @@
 
 #pragma once
 
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_set_triplet.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <drivers/drv_hrt.h>
-#include <poll.h>
-
-#include "blocks.hpp"
-#include "block/UOrbSubscription.hpp"
-#include "block/UOrbPublication.hpp"
-
-extern "C" {
-#include <systemlib/geo/geo.h>
-}
+#include <controllib/blocks.hpp>
+#include <controllib/uorb/blocks.hpp>
 
 namespace control
 {
@@ -251,47 +228,6 @@ public:
  */
 
 /**
- * Waypoint Guidance block
- */
-class __EXPORT BlockWaypointGuidance : public SuperBlock
-{
-private:
-	BlockLimitSym _xtYawLimit;
-	BlockP _xt2Yaw;
-	float _psiCmd;
-public:
-	BlockWaypointGuidance(SuperBlock *parent, const char *name);
-	virtual ~BlockWaypointGuidance();
-	void update(vehicle_global_position_s &pos,
-		    vehicle_attitude_s &att,
-		    vehicle_global_position_setpoint_s &posCmd,
-		    vehicle_global_position_setpoint_s &lastPosCmd);
-	float getPsiCmd() { return _psiCmd; }
-};
-
-/**
- * UorbEnabledAutopilot
- */
-class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
-{
-protected:
-	// subscriptions
-	UOrbSubscription<vehicle_attitude_s> _att;
-	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
-	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
-	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
-	UOrbSubscription<manual_control_setpoint_s> _manual;
-	UOrbSubscription<vehicle_status_s> _status;
-	UOrbSubscription<parameter_update_s> _param_update;
-	// publications
-	UOrbPublication<actuator_controls_s> _actuators;
-public:
-	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
-	virtual ~BlockUorbEnabledAutopilot();
-};
-
-/**
  * Multi-mode Autopilot
  */
 class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index 4803a526e..f4ea05088 100644
--- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -45,12 +45,13 @@
 #include <stdlib.h>
 #include <string.h>
 #include <systemlib/systemlib.h>
-#include <controllib/fixedwing.hpp>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 #include <math.h>
 
+#include "fixedwing.hpp"
+
 static bool thread_should_exit = false;     /**< Deamon exit flag */
 static bool thread_running = false;     /**< Deamon status flag */
 static int deamon_task;             /**< Handle of deamon task / thread */
diff --git a/src/modules/fixedwing_backside/module.mk b/src/modules/fixedwing_backside/module.mk
index ec958d7cb..133728781 100644
--- a/src/modules/fixedwing_backside/module.mk
+++ b/src/modules/fixedwing_backside/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= fixedwing_backside
 
 SRCS		= fixedwing_backside_main.cpp \
+		  fixedwing.cpp \
 		  params.c
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 3713e0b30..ba7cdd91c 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -604,6 +604,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "unable to create logging folder, exiting.");
 	}
 
+	const char *converter_in = "/etc/logging/conv.zip";
+	char* converter_out = malloc(150);
+	sprintf(converter_out, "%s/conv.zip", folder_path);
+
+	if (file_copy(converter_in, converter_out)) {
+		errx(1, "unable to copy conversion scripts, exiting.");
+	}
+	free(converter_out);
+
 	/* only print logging path, important to find log file later */
 	warnx("logging to directory: %s", folder_path);
 
@@ -1256,7 +1265,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return ret;
+	return OK;
 }
 
 void handle_command(struct vehicle_command_s *cmd)
diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp
new file mode 100644
index 000000000..b1dc39445
--- /dev/null
+++ b/src/modules/segway/BlockSegwayController.cpp
@@ -0,0 +1,57 @@
+#include "BlockSegwayController.hpp"
+
+void BlockSegwayController::update() {
+	// wait for a sensor update, check for exit condition every 100 ms
+	if (poll(&_attPoll, 1, 100) < 0) return; // poll error
+
+	uint64_t newTimeStamp = hrt_absolute_time();
+	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
+	_timeStamp = newTimeStamp;
+
+	// check for sane values of dt
+	// to prevent large control responses
+	if (dt > 1.0f || dt < 0) return;
+
+	// set dt for all child blocks
+	setDt(dt);
+
+	// check for new updates
+	if (_param_update.updated()) updateParams();
+
+	// get new information from subscriptions
+	updateSubscriptions();
+
+	// default all output to zero unless handled by mode
+	for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	// only update guidance in auto mode
+	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+		// update guidance
+	}
+
+	// compute speed command
+	float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
+
+	// handle autopilot modes
+	if (_status.state_machine == SYSTEM_STATE_AUTO ||
+	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+		_actuators.control[0] = spdCmd;
+		_actuators.control[1] = spdCmd;
+
+	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
+		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+			_actuators.control[CH_LEFT] = _manual.throttle;
+			_actuators.control[CH_RIGHT] = _manual.pitch;
+
+		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+			_actuators.control[0] = spdCmd;
+			_actuators.control[1] = spdCmd;
+		}
+	}
+
+	// update all publications
+	updatePublications();
+
+}
+
diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp
new file mode 100644
index 000000000..4a01f785c
--- /dev/null
+++ b/src/modules/segway/BlockSegwayController.hpp
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <controllib/uorb/blocks.hpp>
+
+using namespace control;
+
+class BlockSegwayController : public control::BlockUorbEnabledAutopilot {
+public:
+	BlockSegwayController() :
+		BlockUorbEnabledAutopilot(NULL,"SEG"),
+		th2v(this, "TH2V"),
+		q2v(this, "Q2V"),
+		_attPoll(),
+		_timeStamp(0)
+	{
+		_attPoll.fd = _att.getHandle();
+		_attPoll.events = POLLIN;
+	}
+	void update();
+private:
+	enum {CH_LEFT, CH_RIGHT};
+	BlockPI th2v;
+	BlockP q2v;
+	struct pollfd _attPoll;
+	uint64_t _timeStamp;
+};
+
diff --git a/src/modules/segway/module.mk b/src/modules/segway/module.mk
new file mode 100644
index 000000000..d5da85601
--- /dev/null
+++ b/src/modules/segway/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# segway controller
+#
+
+MODULE_COMMAND	= segway
+
+SRCS		= segway_main.cpp \
+		  BlockSegwayController.cpp \
+		  params.c
diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c
new file mode 100644
index 000000000..d72923717
--- /dev/null
+++ b/src/modules/segway/params.c
@@ -0,0 +1,8 @@
+#include <systemlib/param/param.h>
+
+// 16 is max name length
+PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage
+PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage
+PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter
+PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage
+
diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp
new file mode 100644
index 000000000..061fbf9b9
--- /dev/null
+++ b/src/modules/segway/segway_main.cpp
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: James Goppert
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file segway_main.cpp
+ * @author James Goppert
+ *
+ * Segway controller using control library
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <math.h>
+
+#include "BlockSegwayController.hpp"
+
+static bool thread_should_exit = false;     /**< Deamon exit flag */
+static bool thread_running = false;     /**< Deamon status flag */
+static int deamon_task;             /**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int segway_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int segway_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: segway {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int segway_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+
+		deamon_task = task_spawn_cmd("segway",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 5120,
+					 segway_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int segway_thread_main(int argc, char *argv[])
+{
+
+	warnx("starting");
+
+	using namespace control;
+
+	BlockSegwayController autopilot;
+
+	thread_running = true;
+
+	while (!thread_should_exit) {
+		autopilot.update();
+	}
+
+	warnx("exiting.");
+
+	thread_running = false;
+
+	return 0;
+}