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

Makefile

@@ -117,7 +117,7 @@ upload:		$(FIRMWARE_BUNDLE) $(UPLOADER)
 	
 upload-jtag-px4fmu:
 	@echo Attempting to flash PX4FMU board via JTAG
-	@openocd -f interface/olimex-jtag-tiny.cfg -f Tools/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
+	@openocd -f interface/olimex-jtag-tiny.cfg -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
 
 #
 # Hacks and fixups

ROMFS/logging/logconv.m

@@ -1,5 +1,72 @@
 clear all
-clc
+close all
+
+%%%%%%%%%%%%%%%%%%%%%%%
+% SYSTEM VECTOR
+%
+% 			All measurements in NED frame
+% 			
+% 			uint64_t timestamp;
+% 			float gyro[3]; in rad/s
+% 			float accel[3]; in m/s^2
+% 			float mag[3]; in Gauss
+% 			float baro; pressure in millibar
+% 			float baro_alt; altitude above MSL in meters
+% 			float baro_temp; in degrees 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 adc[3]; remaining auxiliary ADC ports in volt
+%           float local_position
+%           int32 gps_raw_position
+
+
+if exist('sysvector.bin', 'file')
+    % Read actuators file
+    myFile = java.io.File('sysvector.bin')
+    fileSize = length(myFile)
+
+    fid = fopen('sysvector.bin', 'r');
+    elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
+
+    for i=1:elements
+        % timestamp
+        sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
+        % actuators 1-16
+        % quadrotor: motor 1-4 on the first four positions
+        sysvector(i, 2:32) = fread(fid,  28+3, 'float', 'ieee-le');
+        sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
+    end
+    
+    sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
+    sysvector_minutes = sysvector_interval_seconds / 60
+    
+    % Normalize time
+    sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
+    
+    % Create some basic plots
+    
+    % Remove zero rows from GPS
+    gps = sysvector(:,33:35);
+    gps(~any(gps,2), :) = [];
+    
+    all_data = figure('Name', 'GPS RAW');
+    gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
+    
+    
+    all_data = figure('Name', 'Complete Log Data (exc. GPS)');
+    plot(sysvector(:,1), sysvector(:,2:32));
+    
+    actuator_inputs = figure('Name', 'Attitude controller outputs');
+    plot(sysvector(:,1), sysvector(:,14:17));
+    legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
+    
+    actuator_outputs = figure('Name', 'Actuator outputs');
+    plot(sysvector(:,1), sysvector(:,18:25));
+    legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
+    
+end
 
 if exist('actuator_outputs0.bin', 'file')
     % Read actuators file
@@ -9,7 +76,7 @@ if exist('actuator_outputs0.bin', 'file')
     fid = fopen('actuator_outputs0.bin', 'r');
     elements = int64(fileSize./(16*4+8))
 
-    for i=1:(elements-2)
+    for i=1:elements
         % timestamp
         actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
         % actuators 1-16

apps/ardrone_interface/ardrone_interface.c

@@ -51,7 +51,7 @@
 #include <time.h>
 #include <systemlib/err.h>
 #include <sys/prctl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>

apps/ardrone_interface/ardrone_motor_control.c

@@ -42,7 +42,7 @@
 #include <fcntl.h>
 #include <unistd.h>
 #include <drivers/drv_gpio.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <systemlib/err.h>

apps/attitude_estimator_ekf/attitudeKalmanfilter.m

@@ -1,108 +0,0 @@
-function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
-%#codegen
-
-
-%Extended Attitude Kalmanfilter
-%   
-    %state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]'
-    %measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]'
-    %knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW]
-    %
-    %[x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
-    %
-    %Example.... 
-    %
-    % $Author: Tobias Naegeli $    $Date: 2012 $    $Revision: 1 $
-   
-
-    %%define the matrices
-    acc_ProcessNoise=knownConst(1);
-    mag_ProcessNoise=knownConst(2); 
-    ratesOffset_ProcessNoise=knownConst(3);
-    rates_ProcessNoise=knownConst(4);
-   
-    
-    acc_MeasurementNoise=knownConst(5);
-    mag_MeasurementNoise=knownConst(6);
-    rates_MeasurementNoise=knownConst(7);
-
-     %process noise covariance matrix
-     Q = [      eye(3)*acc_ProcessNoise,    zeros(3),                   zeros(3),                           zeros(3);
-                zeros(3),                   eye(3)*mag_ProcessNoise,    zeros(3),                           zeros(3);
-                zeros(3),                   zeros(3),                   eye(3)*ratesOffset_ProcessNoise,    zeros(3);
-                zeros(3),                   zeros(3),                   zeros(3),                           eye(3)*rates_ProcessNoise];
-    
-     %measurement noise covariance matrix
-     R = [   eye(3)*acc_MeasurementNoise,       zeros(3),                       zeros(3);
-                 zeros(3),                          eye(3)*mag_MeasurementNoise,    zeros(3);
-                 zeros(3),                          zeros(3),                       eye(3)*rates_MeasurementNoise];
-    
-
-    %observation matrix
-    H_k=[   eye(3),     zeros(3),   zeros(3),   zeros(3);
-            zeros(3),   eye(3),     zeros(3),   zeros(3);
-            zeros(3),   zeros(3),   eye(3),     eye(3)];
-        
-    %compute A(t,w)
-    
-    %x_aposteriori_k[10,11,12] should be [p,q,r]
-    %R_temp=[1,-r, q
-    %        r, 1, -p
-    %       -q, p, 1]
-
-    R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11);
-        dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10);
-        -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1];
-    
-    %strange, should not be transposed
-    A_pred=[R_temp',     zeros(3),   zeros(3),   zeros(3);
-        zeros(3),   R_temp',     zeros(3),   zeros(3);
-        zeros(3),   zeros(3),   eye(3),     zeros(3);
-        zeros(3),   zeros(3),   zeros(3),   eye(3)];
-    
-    %%prediction step
-    x_apriori=A_pred*x_aposteriori_k;
-
-    %linearization
-    acc_temp_mat=[0,              dt*x_aposteriori_k(3),    -dt*x_aposteriori_k(2);
-        -dt*x_aposteriori_k(3), 0,                  dt*x_aposteriori_k(1);
-        dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1),    0];
-    
-    mag_temp_mat=[0,              dt*x_aposteriori_k(6),    -dt*x_aposteriori_k(5);
-        -dt*x_aposteriori_k(6), 0,                  dt*x_aposteriori_k(4);
-        dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4),    0];
-    
-    A_lin=[R_temp',     zeros(3),   zeros(3),   acc_temp_mat';
-        zeros(3),   R_temp',     zeros(3),   mag_temp_mat';
-        zeros(3),   zeros(3),   eye(3),     zeros(3);
-        zeros(3),   zeros(3),   zeros(3),   eye(3)];
-    
-    
-    P_apriori=A_lin*P_aposteriori_k*A_lin'+Q;
-    
-    
-    %%update step
-
-    y_k=z_k-H_k*x_apriori;
-    S_k=H_k*P_apriori*H_k'+R;
-    K_k=(P_apriori*H_k'/(S_k));
-
-    
-    x_aposteriori=x_apriori+K_k*y_k;
-    P_aposteriori=(eye(12)-K_k*H_k)*P_apriori;
-
-
-    %%Rotation matrix generation
-    
-    earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3));
-    earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6)));
-    earth_y=cross(earth_x,earth_z);
-    
-    Rot_matrix=[earth_x,earth_y,earth_z];
-    
-
-
-
-
-
-

apps/attitude_estimator_ekf/attitudeKalmanfilter.prj

@@ -1,270 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<deployment-project>
-  <configuration target="target.matlab.ecoder" target-name="MATLAB Embedded Coder Target" name="attitudeKalmanfilter" location="F:\codegenerationMatlabAttFilter" file="F:\codegenerationMatlabAttFilter\attitudeKalmanfilter.prj" build-checksum="2344418414">
-    <param.mex.general.TargetLang>option.general.TargetLang.C</param.mex.general.TargetLang>
-    <param.mex.general.IntegrityChecks>true</param.mex.general.IntegrityChecks>
-    <param.mex.general.ResponsivenessChecks>true</param.mex.general.ResponsivenessChecks>
-    <param.mex.general.EnableBLAS>true</param.mex.general.EnableBLAS>
-    <param.mex.general.ExtrinsicCalls>true</param.mex.general.ExtrinsicCalls>
-    <param.mex.general.EchoExpressions>true</param.mex.general.EchoExpressions>
-    <param.mex.general.EnableDebugging>true</param.mex.general.EnableDebugging>
-    <param.mex.general.SaturateOnIntegerOverflow>true</param.mex.general.SaturateOnIntegerOverflow>
-    <param.mex.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.mex.general.FilePartitionMethod>
-    <param.mex.general.GlobalDataSyncMethod>option.general.GlobalDataSyncMethod.SyncAlways</param.mex.general.GlobalDataSyncMethod>
-    <param.mex.general.EnableVariableSizing>true</param.mex.general.EnableVariableSizing>
-    <param.mex.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.mex.general.DynamicMemoryAllocation>
-    <param.mex.paths.working>option.paths.working.project</param.mex.paths.working>
-    <param.mex.paths.working.specified />
-    <param.mex.paths.build>option.paths.build.project</param.mex.paths.build>
-    <param.mex.paths.build.specified />
-    <param.mex.paths.search />
-    <param.mex.report.GenerateReport>true</param.mex.report.GenerateReport>
-    <param.mex.report.LaunchReport>false</param.mex.report.LaunchReport>
-    <param.mex.comments.GenerateComments>true</param.mex.comments.GenerateComments>
-    <param.mex.comments.MATLABSourceComments>true</param.mex.comments.MATLABSourceComments>
-    <param.mex.symbols.ReservedNameArray />
-    <param.mex.customcode.CustomSourceCode />
-    <param.mex.customcode.CustomHeaderCode />
-    <param.mex.customcode.CustomInitializer />
-    <param.mex.customcode.CustomTerminator />
-    <param.mex.customcode.CustomInclude />
-    <param.mex.customcode.CustomSource />
-    <param.mex.customcode.CustomLibrary />
-    <param.mex.PostCodeGenCommand />
-    <param.mex.EnableMemcpy>true</param.mex.EnableMemcpy>
-    <param.mex.MemcpyThreshold>64</param.mex.MemcpyThreshold>
-    <param.mex.InitFltsAndDblsToZero>true</param.mex.InitFltsAndDblsToZero>
-    <param.mex.InlineThreshold>10</param.mex.InlineThreshold>
-    <param.mex.InlineThresholdMax>200</param.mex.InlineThresholdMax>
-    <param.mex.InlineStackLimit>4000</param.mex.InlineStackLimit>
-    <param.mex.StackUsageMax>200000</param.mex.StackUsageMax>
-    <param.mex.ConstantFoldingTimeout>10000</param.mex.ConstantFoldingTimeout>
-    <param.grt.general.TargetLang>option.general.TargetLang.C</param.grt.general.TargetLang>
-    <param.general.target.description>MATLAB Embedded Coder Target</param.general.target.description>
-    <param.grt.CCompilerOptimization>option.CCompilerOptimization.On</param.grt.CCompilerOptimization>
-    <param.grt.CCompilerCustomOptimizations />
-    <param.grt.general.GenerateMakefile>true</param.grt.general.GenerateMakefile>
-    <param.grt.general.MakeCommand>make_rtw</param.grt.general.MakeCommand>
-    <param.grt.general.TemplateMakefile>default_tmf</param.grt.general.TemplateMakefile>
-    <param.grt.general.SaturateOnIntegerOverflow>true</param.grt.general.SaturateOnIntegerOverflow>
-    <param.grt.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.grt.general.FilePartitionMethod>
-    <param.grt.general.EnableVariableSizing>true</param.grt.general.EnableVariableSizing>
-    <param.grt.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.grt.general.DynamicMemoryAllocation>
-    <param.grt.paths.working>option.paths.working.project</param.grt.paths.working>
-    <param.grt.paths.working.specified />
-    <param.grt.paths.build>option.paths.build.project</param.grt.paths.build>
-    <param.grt.paths.build.specified />
-    <param.grt.paths.search />
-    <param.grt.report.GenerateReport>true</param.grt.report.GenerateReport>
-    <param.grt.report.LaunchReport>false</param.grt.report.LaunchReport>
-    <param.grt.GenerateComments>true</param.grt.GenerateComments>
-    <param.grt.MATLABSourceComments>true</param.grt.MATLABSourceComments>
-    <param.ert.MATLABFcnDesc>false</param.ert.MATLABFcnDesc>
-    <param.ert.CustomSymbolStrGlobalVar>$M$N</param.ert.CustomSymbolStrGlobalVar>
-    <param.ert.CustomSymbolStrType>$M$N</param.ert.CustomSymbolStrType>
-    <param.ert.CustomSymbolStrField>$M$N</param.ert.CustomSymbolStrField>
-    <param.ert.CustomSymbolStrFcn>$M$N</param.ert.CustomSymbolStrFcn>
-    <param.ert.CustomSymbolStrTmpVar>$M$N</param.ert.CustomSymbolStrTmpVar>
-    <param.ert.CustomSymbolStrMacro>$M$N</param.ert.CustomSymbolStrMacro>
-    <param.ert.CustomSymbolStrEMXArray>emxArray_$M$N</param.ert.CustomSymbolStrEMXArray>
-    <param.grt.MaxIdLength>32</param.grt.MaxIdLength>
-    <param.grt.ReservedNameArray />
-    <param.grt.customcode.CustomSourceCode />
-    <param.grt.customcode.CustomHeaderCode />
-    <param.grt.customcode.CustomInitializer />
-    <param.grt.customcode.CustomTerminator />
-    <param.grt.customcode.CustomInclude />
-    <param.grt.customcode.CustomSource />
-    <param.grt.customcode.CustomLibrary />
-    <param.grt.PostCodeGenCommand />
-    <param.grt.Verbose>false</param.grt.Verbose>
-    <param.grt.TargetFunctionLibrary>C89/C90 (ANSI)</param.grt.TargetFunctionLibrary>
-    <param.grt.SupportNonFinite>true</param.grt.SupportNonFinite>
-    <param.ert.TargetFunctionLibrary>C99 (ISO)</param.ert.TargetFunctionLibrary>
-    <param.ert.PurelyIntegerCode>false</param.ert.PurelyIntegerCode>
-    <param.ert.SupportNonFinite>true</param.ert.SupportNonFinite>
-    <param.ert.IncludeTerminateFcn>true</param.ert.IncludeTerminateFcn>
-    <param.ert.MultiInstanceCode>false</param.ert.MultiInstanceCode>
-    <param.ert.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ert.ParenthesesLevel>
-    <param.ert.ConvertIfToSwitch>false</param.ert.ConvertIfToSwitch>
-    <param.ert.PreserveExternInFcnDecls>true</param.ert.PreserveExternInFcnDecls>
-    <param.grt.EnableMemcpy>true</param.grt.EnableMemcpy>
-    <param.grt.MemcpyThreshold>64</param.grt.MemcpyThreshold>
-    <param.grt.InitFltsAndDblsToZero>true</param.grt.InitFltsAndDblsToZero>
-    <param.grt.InlineThreshold>10</param.grt.InlineThreshold>
-    <param.grt.InlineThresholdMax>200</param.grt.InlineThresholdMax>
-    <param.grt.InlineStackLimit>4000</param.grt.InlineStackLimit>
-    <param.grt.StackUsageMax>200000</param.grt.StackUsageMax>
-    <param.grt.ConstantFoldingTimeout>10000</param.grt.ConstantFoldingTimeout>
-    <param.UseECoderFeatures>true</param.UseECoderFeatures>
-    <param.mex.outputfile>attitudeKalmanfilter_mex</param.mex.outputfile>
-    <param.grt.outputfile>attitudeKalmanfilter</param.grt.outputfile>
-    <param.artifact>option.target.artifact.lib</param.artifact>
-    <param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
-    <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
-    <param.version>R2011a</param.version>
-    <param.HasECoderFeatures>true</param.HasECoderFeatures>
-    <param.mex.mainhtml />
-    <param.grt.mainhtml>F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter\html\index.html</param.grt.mainhtml>
-    <unset>
-      <param.mex.general.TargetLang />
-      <param.mex.general.IntegrityChecks />
-      <param.mex.general.ResponsivenessChecks />
-      <param.mex.general.EnableBLAS />
-      <param.mex.general.ExtrinsicCalls />
-      <param.mex.general.EchoExpressions />
-      <param.mex.general.EnableDebugging />
-      <param.mex.general.SaturateOnIntegerOverflow />
-      <param.mex.general.FilePartitionMethod />
-      <param.mex.general.GlobalDataSyncMethod />
-      <param.mex.general.EnableVariableSizing />
-      <param.mex.general.DynamicMemoryAllocation />
-      <param.mex.paths.working />
-      <param.mex.paths.working.specified />
-      <param.mex.paths.build />
-      <param.mex.paths.build.specified />
-      <param.mex.paths.search />
-      <param.mex.report.GenerateReport />
-      <param.mex.report.LaunchReport />
-      <param.mex.comments.GenerateComments />
-      <param.mex.comments.MATLABSourceComments />
-      <param.mex.symbols.ReservedNameArray />
-      <param.mex.customcode.CustomSourceCode />
-      <param.mex.customcode.CustomHeaderCode />
-      <param.mex.customcode.CustomInitializer />
-      <param.mex.customcode.CustomTerminator />
-      <param.mex.customcode.CustomInclude />
-      <param.mex.customcode.CustomSource />
-      <param.mex.customcode.CustomLibrary />
-      <param.mex.PostCodeGenCommand />
-      <param.mex.EnableMemcpy />
-      <param.mex.MemcpyThreshold />
-      <param.mex.InitFltsAndDblsToZero />
-      <param.mex.InlineThreshold />
-      <param.mex.InlineThresholdMax />
-      <param.mex.InlineStackLimit />
-      <param.mex.StackUsageMax />
-      <param.mex.ConstantFoldingTimeout />
-      <param.grt.general.TargetLang />
-      <param.grt.CCompilerCustomOptimizations />
-      <param.grt.general.GenerateMakefile />
-      <param.grt.general.MakeCommand />
-      <param.grt.general.TemplateMakefile />
-      <param.grt.general.SaturateOnIntegerOverflow />
-      <param.grt.general.FilePartitionMethod />
-      <param.grt.general.EnableVariableSizing />
-      <param.grt.general.DynamicMemoryAllocation />
-      <param.grt.paths.working />
-      <param.grt.paths.working.specified />
-      <param.grt.paths.build />
-      <param.grt.paths.build.specified />
-      <param.grt.paths.search />
-      <param.grt.report.GenerateReport />
-      <param.grt.report.LaunchReport />
-      <param.grt.GenerateComments />
-      <param.ert.MATLABFcnDesc />
-      <param.ert.CustomSymbolStrGlobalVar />
-      <param.ert.CustomSymbolStrType />
-      <param.ert.CustomSymbolStrField />
-      <param.ert.CustomSymbolStrFcn />
-      <param.ert.CustomSymbolStrTmpVar />
-      <param.ert.CustomSymbolStrMacro />
-      <param.ert.CustomSymbolStrEMXArray />
-      <param.grt.MaxIdLength />
-      <param.grt.ReservedNameArray />
-      <param.grt.customcode.CustomHeaderCode />
-      <param.grt.customcode.CustomInitializer />
-      <param.grt.customcode.CustomTerminator />
-      <param.grt.customcode.CustomInclude />
-      <param.grt.customcode.CustomSource />
-      <param.grt.customcode.CustomLibrary />
-      <param.grt.PostCodeGenCommand />
-      <param.grt.Verbose />
-      <param.grt.SupportNonFinite />
-      <param.ert.PurelyIntegerCode />
-      <param.ert.SupportNonFinite />
-      <param.ert.IncludeTerminateFcn />
-      <param.ert.MultiInstanceCode />
-      <param.ert.ParenthesesLevel />
-      <param.ert.ConvertIfToSwitch />
-      <param.ert.PreserveExternInFcnDecls />
-      <param.grt.EnableMemcpy />
-      <param.grt.MemcpyThreshold />
-      <param.grt.InitFltsAndDblsToZero />
-      <param.grt.InlineThreshold />
-      <param.grt.InlineThresholdMax />
-      <param.grt.InlineStackLimit />
-      <param.grt.StackUsageMax />
-      <param.grt.ConstantFoldingTimeout />
-      <param.UseECoderFeatures />
-      <param.mex.outputfile />
-      <param.grt.outputfile />
-      <param.mex.GenCodeOnly />
-      <param.version />
-      <param.HasECoderFeatures />
-      <param.mex.mainhtml />
-    </unset>
-    <fileset.entrypoints>
-      <file value="${PROJECT_ROOT}\attitudeKalmanfilter.m" custom-data-expanded="true">
-        <Inputs fileName="attitudeKalmanfilter.m" functionName="attitudeKalmanfilter">
-          <Input Name="dt">
-            <Class>single</Class>
-            <Size>1 x 1</Size>
-            <Value />
-            <InitialValue />
-            <Complex>false</Complex>
-          </Input>
-          <Input Name="z_k">
-            <Class>single</Class>
-            <Size>9 x 1</Size>
-            <Value />
-            <InitialValue />
-            <Complex>false</Complex>
-          </Input>
-          <Input Name="x_aposteriori_k">
-            <Class>single</Class>
-            <Size>12 x 1</Size>
-            <Value />
-            <InitialValue />
-            <Complex>false</Complex>
-          </Input>
-          <Input Name="P_aposteriori_k">
-            <Class>single</Class>
-            <Size>12 x 12</Size>
-            <Value />
-            <InitialValue />
-            <Complex>false</Complex>
-          </Input>
-          <Input Name="knownConst">
-            <Class>single</Class>
-            <Size>7 x 1</Size>
-            <Value />
-            <InitialValue />
-            <Complex>false</Complex>
-          </Input>
-        </Inputs>
-      </file>
-    </fileset.entrypoints>
-    <fileset.package />
-    <build-deliverables />
-    <matlab>
-      <root>C:\Program Files\MATLAB\R2011a</root>
-    </matlab>
-    <platform>
-      <unix>false</unix>
-      <mac>false</mac>
-      <windows>true</windows>
-      <win2k>false</win2k>
-      <winxp>false</winxp>
-      <vista>false</vista>
-      <linux>false</linux>
-      <solaris>false</solaris>
-      <osver>6.1</osver>
-      <os32>false</os32>
-      <os64>true</os64>
-      <arch>win64</arch>
-      <matlab>true</matlab>
-    </platform>
-  </configuration>
-</deployment-project>
-

apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c

@@ -59,9 +59,10 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/parameter_update.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
 
 #include "codegen/attitudeKalmanfilter_initialize.h"
 #include "codegen/attitudeKalmanfilter.h"
@@ -200,7 +201,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	uint64_t last_run = hrt_absolute_time();
 
 	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
 	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
@@ -350,7 +353,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					static bool const_initialized = false;
 
 					/* initialize with good values once we have a reasonable dt estimate */
-					if (!const_initialized && dt < 0.05 && dt > 0.005)
+					if (!const_initialized && dt < 0.05f && dt > 0.005f)
 					{
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
@@ -445,8 +448,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
 					att.R_valid = true;
 
+					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
 						// Broadcast
 						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
+					} else {
+						warnx("NaN in roll/pitch/yaw estimate!");
+					}
 				}
 			}
 		}

apps/commander/calibration_routines.c

@@ -1,3 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 calibration_routines.c
+ * Calibration routines implementations.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
 #include <math.h>
 
 #include "calibration_routines.h"

apps/commander/calibration_routines.h

@@ -1,3 +1,43 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 calibration_routines.h
+ * Calibration routines definitions.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
 
 /**
  * Least-squares fit of a sphere to a set of points.

apps/commander/commander.c

@@ -1,10 +1,10 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,6 +38,12 @@
 /**
  * @file commander.c
  * Main system state machine implementation.
+ *
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ *
  */
 
 #include "commander.h"
@@ -56,8 +62,8 @@
 #include <v1.0/common/mavlink.h>
 #include <string.h>
 #include <arch/board/drv_led.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
 #include "state_machine_helper.h"
 #include "systemlib/systemlib.h"
@@ -89,7 +95,7 @@
 PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
 //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0);	/**< Go into high-level failsafe after 0 ms */
 
-#include <arch/board/up_cpuload.h>
+#include <systemlib/cpuload.h>
 extern struct system_load_s system_load;
 
 /* Decouple update interval and hysteris counters, all depends on intervals */
@@ -121,9 +127,9 @@ static orb_advert_t stat_pub;
 
 static unsigned int failsafe_lowlevel_timeout_ms;
 
-static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_should_exit = false;		/**< daemon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
+static bool thread_running = false;		/**< daemon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
+static int daemon_task;				/**< Handle of daemon task / thread */
 
 /* pthread loops */
 static void *orb_receive_loop(void *arg);
@@ -942,6 +948,8 @@ static void *orb_receive_loop(void *arg)  //handles status information coming fr
 	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
 	struct subsystem_info_s info;
 
+	struct vehicle_status_s *vstatus = (struct vehicle_status_s*)arg;
+
 	while (!thread_should_exit) {
 		struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } };
 
@@ -952,6 +960,27 @@ static void *orb_receive_loop(void *arg)  //handles status information coming fr
 			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
 
 			printf("Subsys changed: %d\n", (int)info.subsystem_type);
+
+			/* mark / unmark as present */
+			if (info.present) {
+				vstatus->onboard_control_sensors_present |= info.subsystem_type;
+			} else {
+				vstatus->onboard_control_sensors_present &= ~info.subsystem_type;
+			}
+
+			/* mark / unmark as enabled */
+			if (info.enabled) {
+				vstatus->onboard_control_sensors_enabled |= info.subsystem_type;
+			} else {
+				vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type;
+			}
+
+			/* mark / unmark as ok */
+			if (info.ok) {
+				vstatus->onboard_control_sensors_health |= info.subsystem_type;
+			} else {
+				vstatus->onboard_control_sensors_health &= ~info.subsystem_type;
+			}
 		}
 	}
 
@@ -960,12 +989,6 @@ static void *orb_receive_loop(void *arg)  //handles status information coming fr
 	return NULL;
 }
 
-
-
-enum BAT_CHEM {
-	BAT_CHEM_LITHIUM_POLYMERE = 0,
-};
-
 /*
  * Provides a coarse estimate of remaining battery power.
  *
@@ -973,35 +996,41 @@ enum BAT_CHEM {
  *
  * @return the estimated remaining capacity in 0..1
  */
-float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage);
+float battery_remaining_estimate_voltage(float voltage);
 
 PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
 PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
+PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
 
-float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage)
+float battery_remaining_estimate_voltage(float voltage)
 {
 	float ret = 0;
 	static param_t bat_volt_empty;
 	static param_t bat_volt_full;
+	static param_t bat_n_cells;
 	static bool initialized = false;
 	static unsigned int counter = 0;
+	static float ncells = 3;
+	// XXX change cells to int (and param to INT32)
 
 	if (!initialized) {
 		bat_volt_empty = param_find("BAT_V_EMPTY");
 		bat_volt_full = param_find("BAT_V_FULL");
+		bat_n_cells = param_find("BAT_N_CELLS");
 		initialized = true;
 	}
 
-	float chemistry_voltage_empty[1] = { 3.2f };
+	static float chemistry_voltage_empty = 3.2f;
-	float chemistry_voltage_full[1] = { 4.05f };
+	static float chemistry_voltage_full = 4.05f;
 
 	if (counter % 100 == 0) {
-		param_get(bat_volt_empty, &(chemistry_voltage_empty[0]));
+		param_get(bat_volt_empty, &chemistry_voltage_empty);
-		param_get(bat_volt_full, &(chemistry_voltage_full[0]));
+		param_get(bat_volt_full, &chemistry_voltage_full);
+		param_get(bat_n_cells, &ncells);
 	}
 	counter++;
 
-	ret = (voltage - cells * chemistry_voltage_empty[chemistry]) / (cells * (chemistry_voltage_full[chemistry] - chemistry_voltage_empty[chemistry]));
+	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
 
 	/* limit to sane values */
 	ret = (ret < 0) ? 0 : ret;
@@ -1014,12 +1043,12 @@ usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
+	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
 	exit(1);
 }
 
 /**
- * The deamon app only briefly exists to start
+ * The daemon app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
  * 
@@ -1040,7 +1069,7 @@ int commander_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("commander",
+		daemon_task = task_spawn("commander",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 50,
 					 8000,
@@ -1126,7 +1155,7 @@ int commander_thread_main(int argc, char *argv[])
 	pthread_attr_t subsystem_info_attr;
 	pthread_attr_init(&subsystem_info_attr);
 	pthread_attr_setstacksize(&subsystem_info_attr, 2048);
-	pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, NULL);
+	pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, &current_status);
 
 	/* Start monitoring loop */
 	uint16_t counter = 0;
@@ -1218,7 +1247,7 @@ int commander_thread_main(int argc, char *argv[])
 		 * valid and system has been running for two and a half seconds
 		 */
 		if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) {
-			bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
+			bat_remain = battery_remaining_estimate_voltage(battery_voltage);
 		}
 
 		/* Slow but important 8 Hz checks */

apps/commander/commander.h

@@ -1,10 +1,11 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *           Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
  *
- *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
@@ -34,12 +35,20 @@
  *
  ****************************************************************************/
 
-/* @file Main system state machine definition */
+/**
+ * @file commander.h
+ * Main system state machine definition.
+ *
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ *
+ */
 
 #ifndef COMMANDER_H_
 #define COMMANDER_H_
 
-#define VOLTAGE_BATTERY_CRITICAL_VOLTS 10.0f
 #define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
 #define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
 

apps/commander/state_machine_helper.c

@@ -45,7 +45,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <systemlib/systemlib.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"

apps/drivers/bma180/bma180.cpp

@@ -58,7 +58,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 
 #include <drivers/device/spi.h>
@@ -289,6 +289,7 @@ BMA180::init()
 	_num_reports = 2;
 	_oldest_report = _next_report = 0;
 	_reports = new struct accel_report[_num_reports];
+
 	if (_reports == nullptr)
 		goto out;
 
@@ -321,13 +322,16 @@ BMA180::init()
 	modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
 
 	if (set_range(4)) warnx("Failed setting range");
+
 	if (set_lowpass(75)) warnx("Failed setting lowpass");
 
 	if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
 		ret = OK;
+
 	} else {
 		ret = ERROR;
 	}
+
 out:
 	return ret;
 }
@@ -441,6 +445,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGPOLLRATE:
 		if (_call_interval == 0)
 			return SENSOR_POLLRATE_MANUAL;
+
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
@@ -549,24 +554,30 @@ BMA180::set_range(unsigned max_g)
 
 	if (max_g == 0)
 		max_g = 16;
+
 	if (max_g > 16)
 		return -ERANGE;
 
 	if (max_g <= 2) {
 		_current_range = 2;
 		rangebits = OFFSET_LSB1_RANGE_2G;
+
 	} else if (max_g <= 3) {
 		_current_range = 3;
 		rangebits = OFFSET_LSB1_RANGE_3G;
+
 	} else if (max_g <= 4) {
 		_current_range = 4;
 		rangebits = OFFSET_LSB1_RANGE_4G;
+
 	} else if (max_g <= 8) {
 		_current_range = 8;
 		rangebits = OFFSET_LSB1_RANGE_8G;
+
 	} else if (max_g <= 16) {
 		_current_range = 16;
 		rangebits = OFFSET_LSB1_RANGE_16G;
+
 	} else {
 		return -EINVAL;
 	}
@@ -781,17 +792,21 @@ start()
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		goto fail;
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
 	exit(0);
 fail:
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
+
 	errx(1, "driver start failed");
 }
 
@@ -809,6 +824,7 @@ test()
 
 	/* get the driver */
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "%s open failed (try 'bma180 start' if the driver is not running)",
 		    ACCEL_DEVICE_PATH);
@@ -819,6 +835,7 @@ test()
 
 	/* do a simple demand read */
 	sz = read(fd, &a_report, sizeof(a_report));
+
 	if (sz != sizeof(a_report))
 		err(1, "immediate acc read failed");
 
@@ -846,10 +863,13 @@ void
 reset()
 {
 	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "failed ");
+
 	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
 		err(1, "driver reset failed");
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 

apps/drivers/boards/px4fmu/Makefile

@@ -32,11 +32,9 @@
 ############################################################################
 
 #
-# Basic example application
+# Board-specific startup code for the PX4FMU
 #
 
-APPNAME		 = ground_estimator
+INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
 
 include $(APPDIR)/mk/app.mk

apps/drivers/boards/px4fmu/px4fmu_adc.c

@@ -1,9 +1,6 @@
-/************************************************************************************
+/****************************************************************************
- * configs/stm3240g-eval/src/up_adc.c
- * arch/arm/src/board/up_adc.c
  *
- *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -15,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -32,7 +29,13 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  *
- ************************************************************************************/
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_adc.c
+ *
+ * Board-specific ADC functions.
+ */
 
 /************************************************************************************
  * Included Files
@@ -50,35 +53,8 @@
 #include "chip.h"
 #include "up_arch.h"
 
-//#include "stm32_pwm.h"
 #include "stm32_adc.h"
-#include "px4fmu-internal.h"
+#include "px4fmu_internal.h"
-
-#ifdef CONFIG_ADC
-
-/************************************************************************************
- * Definitions
- ************************************************************************************/
-
-/* Configuration ************************************************************/
-/* Up to 3 ADC interfaces are supported */
-
-#if STM32_NADC < 3
-#  undef CONFIG_STM32_ADC3
-#endif
-
-#if STM32_NADC < 2
-#  undef CONFIG_STM32_ADC2
-#endif
-
-#if STM32_NADC < 1
-#  undef CONFIG_STM32_ADC3
-#endif
-
-#if defined(CONFIG_STM32_ADC1) || defined(CONFIG_STM32_ADC2) || defined(CONFIG_STM32_ADC3)
-#ifndef CONFIG_STM32_ADC3
-#  warning "Channel information only available for ADC3"
-#endif
 
 #define ADC3_NCHANNELS 4
 
@@ -116,7 +92,6 @@ static const uint32_t g_pinlist[ADC3_NCHANNELS]  = {GPIO_ADC3_IN10, GPIO_ADC3_IN
 
 int adc_devinit(void)
 {
-#ifdef CONFIG_STM32_ADC3
 	static bool initialized = false;
 	struct adc_dev_s *adc[ADC3_NCHANNELS];
 	int ret;
@@ -124,25 +99,22 @@ int adc_devinit(void)
 
 	/* Check if we have already initialized */
 
-	if (!initialized)
+	if (!initialized) {
-	{
 		char name[11];
 
-		for (i = 0; i < ADC3_NCHANNELS; i++)
+		for (i = 0; i < ADC3_NCHANNELS; i++) {
-		{
 			stm32_configgpio(g_pinlist[i]);
 		}
 
-		for (i = 0; i < 1; i++)
+		for (i = 0; i < 1; i++) {
-		{
 			/* Configure the pins as analog inputs for the selected channels */
 			//stm32_configgpio(g_pinlist[i]);
 
 			/* Call stm32_adcinitialize() to get an instance of the ADC interface */
 			//multiple channels only supported with dma!
 			adc[i] = stm32_adcinitialize(3, (g_chanlist), 4);
-			if (adc == NULL)
+
-			{
+			if (adc == NULL) {
 				adbg("ERROR: Failed to get ADC interface\n");
 				return -ENODEV;
 			}
@@ -151,8 +123,8 @@ int adc_devinit(void)
 			/* Register the ADC driver at "/dev/adc0" */
 			sprintf(name, "/dev/adc%d", i);
 			ret = adc_register(name, adc[i]);
-			if (ret < 0)
+
-			{
+			if (ret < 0) {
 				adbg("adc_register failed for adc %s: %d\n", name, ret);
 				return ret;
 			}
@@ -164,10 +136,4 @@ int adc_devinit(void)
 	}
 
 	return OK;
-#else
-	return -ENOSYS;
-#endif
 }
-
-#endif /* CONFIG_STM32_ADC || CONFIG_STM32_ADC2 || CONFIG_STM32_ADC3 */
-#endif /* CONFIG_ADC */

apps/drivers/boards/px4fmu/px4fmu_can.c

@@ -1,9 +1,6 @@
-/************************************************************************************
+/****************************************************************************
- * configs/px4fmu/src/up_can.c
- * arch/arm/src/board/up_can.c
  *
- *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -15,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -32,7 +29,14 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  *
- ************************************************************************************/
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
 
 /************************************************************************************
  * Included Files
@@ -51,9 +55,7 @@
 
 #include "stm32.h"
 #include "stm32_can.h"
-#include "px4fmu-internal.h"
+#include "px4fmu_internal.h"
-
-#if defined(CONFIG_CAN) && (defined(CONFIG_STM32_CAN1) || defined(CONFIG_STM32_CAN2))
 
 /************************************************************************************
  * Pre-processor Definitions
@@ -111,13 +113,12 @@ int can_devinit(void)
 
 	/* Check if we have already initialized */
 
-  if (!initialized)
+	if (!initialized) {
-    {
 		/* Call stm32_caninitialize() to get an instance of the CAN interface */
 
 		can = stm32_caninitialize(CAN_PORT);
-      if (can == NULL)
+
-        {
+		if (can == NULL) {
 			candbg("ERROR:  Failed to get CAN interface\n");
 			return -ENODEV;
 		}
@@ -125,8 +126,8 @@ int can_devinit(void)
 		/* Register the CAN driver at "/dev/can0" */
 
 		ret = can_register("/dev/can0", can);
-      if (ret < 0)
+
-        {
+		if (ret < 0) {
 			candbg("ERROR: can_register failed: %d\n", ret);
 			return ret;
 		}
@@ -138,5 +139,3 @@ int can_devinit(void)
 
 	return OK;
 }
-
-#endif /* CONFIG_STM32_CAN || CONFIG_STM32_CAN2 || CONFIG_STM32_CAN3 */

apps/drivers/boards/px4fmu/px4fmu_init.c

@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/arch.h>
+
+#include "stm32_internal.h"
+#include "px4fmu_internal.h"
+#include "stm32_uart.h"
+
+#include <arch/board/board.h>
+#include <arch/board/drv_led.h>
+#include <arch/board/drv_eeprom.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/cpuload.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lib_lowprintf(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lib_lowprintf
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+extern int adc_devinit(void);
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi3;
+static struct i2c_dev_s *i2c1;
+static struct i2c_dev_s *i2c2;
+static struct i2c_dev_s *i2c3;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+	int result;
+
+	/* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
+
+	/* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
+
+	/* configure the high-resolution time/callout interface */
+#ifdef CONFIG_HRT_TIMER
+	hrt_init();
+#endif
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+#ifdef SERIAL_HAVE_DMA
+	{
+		static struct hrt_call serial_dma_call;
+		struct timespec ts;
+
+		/*
+		 * Poll at 1ms intervals for received bytes that have not triggered
+		 * a DMA event.
+		 */
+		ts.tv_sec = 0;
+		ts.tv_nsec = 1000000;
+
+		hrt_call_every(&serial_dma_call,
+			       ts_to_abstime(&ts),
+			       ts_to_abstime(&ts),
+			       (hrt_callout)stm32_serial_dma_poll,
+			       NULL);
+	}
+#endif
+
+	message("\r\n");
+
+	up_ledoff(LED_BLUE);
+	up_ledoff(LED_AMBER);
+
+	up_ledon(LED_BLUE);
+
+	/* Configure user-space led driver */
+	px4fmu_led_init();
+
+	/* Configure SPI-based devices */
+
+	spi1 = up_spiinitialize(1);
+
+	if (!spi1) {
+		message("[boot] FAILED to initialize SPI port 1\r\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	// Default SPI1 to 1MHz and de-assert the known chip selects.
+	SPI_SETFREQUENCY(spi1, 10000000);
+	SPI_SETBITS(spi1, 8);
+	SPI_SETMODE(spi1, SPIDEV_MODE3);
+	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
+	up_udelay(20);
+
+	message("[boot] Successfully initialized SPI port 1\r\n");
+
+	/* initialize I2C2 bus */
+
+	i2c2 = up_i2cinitialize(2);
+
+	if (!i2c2) {
+		message("[boot] FAILED to initialize I2C bus 2\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* set I2C2 speed */
+	I2C_SETFREQUENCY(i2c2, 400000);
+
+
+	i2c3 = up_i2cinitialize(3);
+
+	if (!i2c3) {
+		message("[boot] FAILED to initialize I2C bus 3\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* set I2C3 speed */
+	I2C_SETFREQUENCY(i2c3, 400000);
+
+	/* try to attach, don't fail if device is not responding */
+	(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
+			    FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
+			    FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
+			    FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
+
+#if defined(CONFIG_STM32_SPI3)
+	/* Get the SPI port */
+
+	message("[boot] Initializing SPI port 3\n");
+	spi3 = up_spiinitialize(3);
+
+	if (!spi3) {
+		message("[boot] FAILED to initialize SPI port 3\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	message("[boot] Successfully initialized SPI port 3\n");
+
+	/* Now bind the SPI interface to the MMCSD driver */
+	result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
+
+	if (result != OK) {
+		message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
+#endif /* SPI3 */
+
+	/* initialize I2C1 bus */
+
+	i2c1 = up_i2cinitialize(1);
+
+	if (!i2c1) {
+		message("[boot] FAILED to initialize I2C bus 1\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* set I2C1 speed */
+	I2C_SETFREQUENCY(i2c1, 400000);
+
+	/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
+
+	/* Get board information if available */
+
+	/* Initialize the user GPIOs */
+	px4fmu_gpio_init();
+
+#ifdef CONFIG_ADC
+	int adc_state = adc_devinit();
+
+	if (adc_state != OK) {
+		/* Try again */
+		adc_state = adc_devinit();
+
+		if (adc_state != OK) {
+			/* Give up */
+			message("[boot] FAILED adc_devinit: %d\n", adc_state);
+			return -ENODEV;
+		}
+	}
+
+#endif
+
+	return OK;
+}

apps/drivers/boards/px4fmu/px4fmu_internal.h

@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_internal.h
+ *
+ * PX4FMU internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+#ifdef CONFIG_STM32_SPI2
+#  error "SPI2 is not supported on this board"
+#endif
+
+#if defined(CONFIG_STM32_CAN1)
+#  warning "CAN1 is not supported on this board"
+#endif
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED2		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+
+/* External interrupts */
+#define GPIO_EXTI_COMPASS	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
+// XXX MPU6000 DRDY?
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SPI_CS_ACCEL	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
+
+/* User GPIOs
+ *
+ * GPIO0-1 are the buffered high-power GPIOs.
+ * GPIO2-5 are the USART2 pins.
+ * GPIO6-7 are the CAN2 pins.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2)
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* PWM
+ *
+ * The PX4FMU has five PWM outputs, of which only the output on
+ * pin PC8 is fixed assigned to this function. The other four possible
+ * pwm sources are the TX, RX, RTS and CTS pins of USART2
+ *
+ * Alternate function mapping:
+ * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
+ */
+
+#ifdef CONFIG_PWM
+#  if defined(CONFIG_STM32_TIM3_PWM)
+#    define BUZZER_PWMCHANNEL 3
+#	 define BUZZER_PWMTIMER 3
+#  elif defined(CONFIG_STM32_TIM8_PWM)
+#    define BUZZER_PWMCHANNEL 3
+#    define BUZZER_PWMTIMER 8
+#  endif
+#endif
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+/****************************************************************************************************
+ * Name: px4fmu_gpio_init
+ *
+ * Description:
+ *   Called to configure the PX4FMU user GPIOs
+ *
+ ****************************************************************************************************/
+
+extern void px4fmu_gpio_init(void);
+
+
+// XXX additional SPI chipselect functions required?
+
+#endif /* __ASSEMBLY__ */

apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c

@@ -31,46 +31,61 @@
  *
  ****************************************************************************/
 
-/************************************************************************************
+/*
- * Included Files
+ * @file px4fmu_pwm_servo.c
- ************************************************************************************/
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
 
-#include <nuttx/config.h>
+#include <stdint.h>
 
-#include <debug.h>
+#include <drivers/stm32/drv_pwm_servo.h>
 
 #include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
 
+#include "chip.h"
+#include "up_internal.h"
 #include "up_arch.h"
-#include "px4fmu-internal.h"
 
-/************************************************************************************
+#include "stm32_internal.h"
- * Definitions
+#include "stm32_gpio.h"
- ************************************************************************************/
+#include "stm32_tim.h"
 
-/************************************************************************************
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
- * Private Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: stm32_boardinitialize
- *
- * Description:
- *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
- *   and mapped but before any devices have been initialized.
- *
- ************************************************************************************/
-
-void stm32_boardinitialize(void)
 	{
-	/* configure SPI interfaces */
+		.base = STM32_TIM2_BASE,
-	stm32_spiinitialize();
+		.clock_register = STM32_RCC_APB1ENR,
-
+		.clock_bit = RCC_APB1ENR_TIM2EN,
-	/* configure LEDs */
+		.clock_freq = STM32_APB1_TIM2_CLKIN
-	up_ledinit();
 	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};

apps/drivers/boards/px4fmu/px4fmu_spi.c

@@ -1,9 +1,6 @@
-/************************************************************************************
+/****************************************************************************
- * configs/px4fmu/src/up_spi.c
- * arch/arm/src/board/up_spi.c
  *
- *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -15,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -32,7 +29,13 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  *
- ************************************************************************************/
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
 
 /************************************************************************************
  * Included Files
@@ -50,35 +53,7 @@
 #include "up_arch.h"
 #include "chip.h"
 #include "stm32_internal.h"
-#include "px4fmu-internal.h"
+#include "px4fmu_internal.h"
-
-#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || defined(CONFIG_STM32_SPI3)
-
-/************************************************************************************
- * Definitions
- ************************************************************************************/
-
-/* Enables debug output from this file (needs CONFIG_DEBUG too) */
-
-#undef SPI_DEBUG   /* Define to enable debug */
-#undef SPI_VERBOSE /* Define to enable verbose debug */
-
-#ifdef SPI_DEBUG
-#  define spidbg  lldbg
-#  ifdef SPI_VERBOSE
-#    define spivdbg lldbg
-#  else
-#    define spivdbg(x...)
-#  endif
-#else
-#  undef SPI_VERBOSE
-#  define spidbg(x...)
-#  define spivdbg(x...)
-#endif
-
-/************************************************************************************
- * Private Functions
- ************************************************************************************/
 
 /************************************************************************************
  * Public Functions
@@ -92,7 +67,7 @@
  *
  ************************************************************************************/
 
-void weak_function stm32_spiinitialize(void)
+__EXPORT void weak_function stm32_spiinitialize(void)
 {
 	stm32_configgpio(GPIO_SPI_CS_GYRO);
 	stm32_configgpio(GPIO_SPI_CS_ACCEL);
@@ -109,36 +84,8 @@ void weak_function stm32_spiinitialize(void)
 	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1);
 }
 
-/****************************************************************************
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
- * Name:  stm32_spi1/2/3select and stm32_spi1/2/3status
- *
- * Description:
- *   The external functions, stm32_spi1/2/3select and stm32_spi1/2/3status must be
- *   provided by board-specific logic.  They are implementations of the select
- *   and status methods of the SPI interface defined by struct spi_ops_s (see
- *   include/nuttx/spi.h). All other methods (including up_spiinitialize())
- *   are provided by common STM32 logic.  To use this common SPI logic on your
- *   board:
- *
- *   1. Provide logic in stm32_boardinitialize() to configure SPI chip select
- *      pins.
- *   2. Provide stm32_spi1/2/3select() and stm32_spi1/2/3status() functions in your
- *      board-specific logic.  These functions will perform chip selection and
- *      status operations using GPIOs in the way your board is configured.
- *   3. Add a calls to up_spiinitialize() in your low level application
- *      initialization logic
- *   4. The handle returned by up_spiinitialize() may then be used to bind the
- *      SPI driver to higher level logic (e.g., calling
- *      mmcsd_spislotinitialize(), for example, will bind the SPI driver to
- *      the SPI MMC/SD driver).
- *
- ****************************************************************************/
-
-#ifdef CONFIG_STM32_SPI1
-void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
 {
-	spidbg("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert");
-
 	/* SPI select is active low, so write !selected to select the device */
 
 	switch (devid) {
@@ -148,45 +95,42 @@ void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool sele
 		stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
 		break;
+
 	case PX4_SPIDEV_ACCEL:
 		/* Making sure the other peripherals are not selected */
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
 		stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
 		break;
+
 	case PX4_SPIDEV_MPU:
 		/* Making sure the other peripherals are not selected */
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
 		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
 		break;
+
 	default:
-		spidbg("devid: %d - unexpected\n", devid);
 		break;
 
 	}
 }
 
-uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
 {
 	return SPI_STATUS_PRESENT;
 }
-#endif
 
-#ifdef CONFIG_STM32_SPI3
+
-void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
 {
-	spidbg("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert");
-
 	/* there can only be one device on this bus, so always select it */
 	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 0);
 }
 
-uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
 {
 	/* this is actually bogus, but PX4 has no way to sense the presence of an SD card */
 	return SPI_STATUS_PRESENT;
 }
-#endif
 
-#endif /* CONFIG_STM32_SPI1 || CONFIG_STM32_SPI2 */

apps/drivers/boards/px4fmu/px4fmu_usb.c

@@ -1,9 +1,6 @@
-/************************************************************************************
+/****************************************************************************
- * configs/stm32f4discovery/src/up_usbdev.c
- * arch/arm/src/board/up_boot.c
  *
- *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -15,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -32,7 +29,13 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  *
- ************************************************************************************/
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
 
 /************************************************************************************
  * Included Files
@@ -50,7 +53,7 @@
 
 #include "up_arch.h"
 #include "stm32_internal.h"
-#include "px4fmu-internal.h"
+#include "px4fmu_internal.h"
 
 /************************************************************************************
  * Definitions
@@ -68,11 +71,11 @@
  * Name: stm32_usbinitialize
  *
  * Description:
- *   Called to setup USB-related GPIO pins for the STM3210E-EVAL board.
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
  *
  ************************************************************************************/
 
-void stm32_usbinitialize(void)
+__EXPORT void stm32_usbinitialize(void)
 {
 	/* The OTG FS has an internal soft pull-up */
 
@@ -98,7 +101,7 @@ void stm32_usbinitialize(void)
  *
  ************************************************************************************/
 
-void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
 {
 	ulldbg("resume: %d\n", resume);
 }

apps/drivers/device/spi.cpp

@@ -134,6 +134,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	/* do common setup */
 	if (!up_interrupt_context())
 		SPI_LOCK(_dev, true);
+
 	SPI_SETFREQUENCY(_dev, _frequency);
 	SPI_SETMODE(_dev, _mode);
 	SPI_SETBITS(_dev, 8);
@@ -144,6 +145,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 
 	/* and clean up */
 	SPI_SELECT(_dev, _device, false);
+
 	if (!up_interrupt_context())
 		SPI_LOCK(_dev, false);
 

apps/drivers/drv_hrt.h

@@ -31,12 +31,13 @@
  *
  ****************************************************************************/
 
-/*
+/**
- * High-resolution timer callouts and timekeeping.
+ * @file drv_hrt.h
+ *
+ * High-resolution timer with callouts and timekeeping.
  */
 
-#ifndef UP_HRT_H_
+#pragma once
-#define UP_HRT_H_
 
 #include <sys/types.h>
 #include <stdbool.h>
@@ -44,6 +45,8 @@
 #include <time.h>
 #include <queue.h>
 
+__BEGIN_DECLS
+
 /*
  * Absolute time, in microsecond units.
  *
@@ -76,17 +79,17 @@ typedef struct hrt_call {
 /*
  * Get absolute time.
  */
-extern hrt_abstime hrt_absolute_time(void);
+__EXPORT extern hrt_abstime hrt_absolute_time(void);
 
 /*
  * Convert a timespec to absolute time.
  */
-extern hrt_abstime ts_to_abstime(struct timespec *ts);
+__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
 
 /*
  * Convert absolute time to a timespec.
  */
-extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
+__EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
 
 /*
  * Call callout(arg) after delay has elapsed.
@@ -94,12 +97,12 @@ extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
  * If callout is NULL, this can be used to implement a timeout by testing the call
  * with hrt_called().
  */
-extern void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);
+__EXPORT extern void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);
 
 /*
  * Call callout(arg) at absolute time calltime.
  */
-extern void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg);
+__EXPORT extern void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg);
 
 /*
  * Call callout(arg) after delay, and then after every interval.
@@ -107,7 +110,7 @@ extern void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callou
  * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
  * jitter but should not drift.
  */
-extern void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
+__EXPORT extern void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
 
 /*
  * If this returns true, the entry has been invoked and removed from the callout list,
@@ -115,16 +118,16 @@ extern void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstim
  *
  * Always returns false for repeating callouts.
  */
-extern bool	hrt_called(struct hrt_call *entry);
+__EXPORT extern bool	hrt_called(struct hrt_call *entry);
 
 /*
  * Remove the entry from the callout list.
  */
-extern void	hrt_cancel(struct hrt_call *entry);
+__EXPORT extern void	hrt_cancel(struct hrt_call *entry);
 
 /*
  * Initialise the HRT.
  */
-extern void	hrt_init(void);
+__EXPORT extern void	hrt_init(void);
 
-#endif /* UP_HRT_H_ */
+__END_DECLS

apps/drivers/drv_pwm_output.h

@@ -41,14 +41,15 @@
  * channel.
  */
 
-#ifndef _DRV_PWM_OUTPUT_H
+#pragma once
-#define _DRV_PWM_OUTPUT_H
 
 #include <stdint.h>
 #include <sys/ioctl.h>
 
 #include "drv_orb_dev.h"
 
+__BEGIN_DECLS
+
 /**
  * Path for the default PWM output device.
  *
@@ -109,4 +110,63 @@ ORB_DECLARE(output_pwm);
 #define PWM_SERVO_GET(_servo)	_IOC(_PWM_SERVO_BASE, 0x40 + _servo)
 
 
-#endif /* _DRV_PWM_OUTPUT_H */
+/*
+ * Low-level PWM output interface.
+ *
+ * This is the low-level API to the platform-specific PWM driver.
+ */
+
+/**
+ * Intialise the PWM servo outputs using the specified configuration.
+ *
+ * @param channel_mask	Bitmask of channels (LSB = channel 0) to enable.
+ *			This allows some of the channels to remain configured
+ *			as GPIOs or as another function.
+ * @return		OK on success.
+ */
+__EXPORT extern int	up_pwm_servo_init(uint32_t channel_mask);
+
+/**
+ * De-initialise the PWM servo outputs.
+ */
+__EXPORT extern void	up_pwm_servo_deinit(void);
+
+/**
+ * Arm or disarm servo outputs.
+ *
+ * When disarmed, servos output no pulse.
+ *
+ * @bug This function should, but does not, guarantee that any pulse
+ *      currently in progress is cleanly completed.
+ *
+ * @param armed		If true, outputs are armed; if false they
+ *			are disarmed.
+ */
+__EXPORT extern void	up_pwm_servo_arm(bool armed);
+
+/**
+ * Set the servo update rate
+ *
+ * @param rate		The update rate in Hz to set.
+ * @return		OK on success, -ERANGE if an unsupported update rate is set.
+ */
+__EXPORT extern int	up_pwm_servo_set_rate(unsigned rate);
+
+/**
+ * Set the current output value for a channel.
+ *
+ * @param channel	The channel to set.
+ * @param value		The output pulse width in microseconds.
+ */
+__EXPORT extern int	up_pwm_servo_set(unsigned channel, servo_position_t value);
+
+/**
+ * Get the current output value for a channel.
+ *
+ * @param channel	The channel to read.
+ * @return		The output pulse width in microseconds, or zero if
+ *			outputs are not armed or not configured.
+ */
+__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
+
+__END_DECLS

apps/drivers/hmc5883/hmc5883.cpp

@@ -58,7 +58,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -115,6 +115,10 @@
 #endif
 static const int ERROR = -1;
 
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
 class HMC5883 : public device::I2C
 {
 public:
@@ -328,13 +332,16 @@ HMC5883::init()
 	/* allocate basic report buffers */
 	_num_reports = 2;
 	_reports = new struct mag_report[_num_reports];
+
 	if (_reports == nullptr)
 		goto out;
+
 	_oldest_report = _next_report = 0;
 
 	/* get a publish handle on the mag topic */
 	memset(&_reports[0], 0, sizeof(_reports[0]));
 	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
+
 	if (_mag_topic < 0)
 		debug("failed to create sensor_mag object");
 
@@ -354,30 +361,37 @@ int HMC5883::set_range(unsigned range)
 		range_bits = 0x00;
 		_range_scale = 1.0f / 1370.0f;
 		_range_ga = 0.88f;
+
 	} else if (range <= 1) {
 		range_bits = 0x01;
 		_range_scale = 1.0f / 1090.0f;
 		_range_ga = 1.3f;
+
 	} else if (range <= 2) {
 		range_bits = 0x02;
 		_range_scale = 1.0f / 820.0f;
 		_range_ga = 1.9f;
+
 	} else if (range <= 3) {
 		range_bits = 0x03;
 		_range_scale = 1.0f / 660.0f;
 		_range_ga = 2.5f;
+
 	} else if (range <= 4) {
 		range_bits = 0x04;
 		_range_scale = 1.0f / 440.0f;
 		_range_ga = 4.0f;
+
 	} else if (range <= 4.7f) {
 		range_bits = 0x05;
 		_range_scale = 1.0f / 390.0f;
 		_range_ga = 4.7f;
+
 	} else if (range <= 5.6f) {
 		range_bits = 0x06;
 		_range_scale = 1.0f / 330.0f;
 		_range_ga = 5.6f;
+
 	} else {
 		range_bits = 0x07;
 		_range_scale = 1.0f / 230.0f;
@@ -409,10 +423,12 @@ HMC5883::probe()
 	uint8_t data[3] = {0, 0, 0};
 
 	_retries = 10;
+
 	if (read_reg(ADDR_ID_A, data[0]) ||
 	    read_reg(ADDR_ID_B, data[1]) ||
 	    read_reg(ADDR_ID_C, data[2]))
 		debug("read_reg fail");
+
 	_retries = 1;
 
 	if ((data[0] != ID_A_WHO_AM_I) ||
@@ -548,6 +564,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGPOLLRATE:
 		if (_measure_ticks == 0)
 			return SENSOR_POLLRATE_MANUAL;
+
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
@@ -847,6 +864,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 
 	/* do a simple demand read */
 	sz = read(filp, (char *)&report, sizeof(report));
+
 	if (sz != sizeof(report)) {
 		warn("immediate read failed");
 		ret = 1;
@@ -916,6 +934,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		if (sz != sizeof(report)) {
 			warn("periodic read failed");
 			goto out;
+
 		} else {
 			avg_excited[0] += report.x;
 			avg_excited[1] += report.y;
@@ -968,11 +987,14 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	ret = OK;
 
 out:
+
 	if (ret == OK) {
 		warnx("mag scale calibration successfully finished.");
+
 	} else {
 		warnx("mag scale calibration failed.");
 	}
+
 	return ret;
 }
 
@@ -982,16 +1004,22 @@ int HMC5883::set_excitement(unsigned enable)
 	/* arm the excitement strap */
 	uint8_t conf_reg;
 	ret = read_reg(ADDR_CONF_A, conf_reg);
+
 	if (OK != ret)
 		perf_count(_comms_errors);
+
 	if (((int)enable) < 0) {
 		conf_reg |= 0x01;
+
 	} else if (enable > 0) {
 		conf_reg |= 0x02;
+
 	} else {
 		conf_reg &= ~0x03;
 	}
+
 	ret = write_reg(ADDR_CONF_A, conf_reg);
+
 	if (OK != ret)
 		perf_count(_comms_errors);
 
@@ -1083,17 +1111,22 @@ start()
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		goto fail;
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
+
 	exit(0);
 
 fail:
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
+
 	errx(1, "driver start failed");
 }
 
@@ -1110,11 +1143,13 @@ test()
 	int ret;
 
 	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
+
 	if (sz != sizeof(report))
 		err(1, "immediate read failed");
 
@@ -1203,6 +1238,7 @@ int calibrate()
 	int ret;
 
 	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
 
@@ -1214,6 +1250,7 @@ int calibrate()
 
 	if (ret == OK) {
 		errx(0, "PASS");
+
 	} else {
 		errx(1, "FAIL");
 	}
@@ -1226,10 +1263,13 @@ void
 reset()
 {
 	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "failed ");
+
 	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
 		err(1, "driver reset failed");
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 
@@ -1286,6 +1326,7 @@ hmc5883_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "calibrate")) {
 		if (hmc5883::calibrate() == 0) {
 			errx(0, "calibration successful");
+
 		} else {
 			errx(1, "calibration failed");
 		}

apps/drivers/l3gd20/l3gd20.cpp

@@ -57,7 +57,7 @@
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 
 #include <drivers/device/spi.h>
@@ -317,6 +317,7 @@ L3GD20::init()
 	_num_reports = 2;
 	_oldest_report = _next_report = 0;
 	_reports = new struct gyro_report[_num_reports];
+
 	if (_reports == nullptr)
 		goto out;
 
@@ -451,6 +452,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGPOLLRATE:
 		if (_call_interval == 0)
 			return SENSOR_POLLRATE_MANUAL;
+
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
@@ -562,12 +564,15 @@ L3GD20::set_range(unsigned max_dps)
 	if (max_dps <= 250) {
 		_current_range = 250;
 		bits |= RANGE_250DPS;
+
 	} else if (max_dps <= 500) {
 		_current_range = 500;
 		bits |= RANGE_500DPS;
+
 	} else if (max_dps <= 2000) {
 		_current_range = 2000;
 		bits |= RANGE_2000DPS;
+
 	} else {
 		return -EINVAL;
 	}
@@ -590,15 +595,19 @@ L3GD20::set_samplerate(unsigned frequency)
 	if (frequency <= 95) {
 		_current_rate = 95;
 		bits |= RATE_95HZ_LP_25HZ;
+
 	} else if (frequency <= 190) {
 		_current_rate = 190;
 		bits |= RATE_190HZ_LP_25HZ;
+
 	} else if (frequency <= 380) {
 		_current_rate = 380;
 		bits |= RATE_380HZ_LP_30HZ;
+
 	} else if (frequency <= 760) {
 		_current_rate = 760;
 		bits |= RATE_760HZ_LP_30HZ;
+
 	} else {
 		return -EINVAL;
 	}
@@ -746,17 +755,21 @@ start()
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		goto fail;
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
 	exit(0);
 fail:
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
+
 	errx(1, "driver start failed");
 }
 
@@ -774,6 +787,7 @@ test()
 
 	/* get the driver */
 	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+
 	if (fd_gyro < 0)
 		err(1, "%s open failed", GYRO_DEVICE_PATH);
 
@@ -783,6 +797,7 @@ test()
 
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
+
 	if (sz != sizeof(g_report))
 		err(1, "immediate gyro read failed");
 
@@ -808,10 +823,13 @@ void
 reset()
 {
 	int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "failed ");
+
 	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
 		err(1, "driver reset failed");
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 

apps/drivers/mpu6000/mpu6000.cpp

@@ -61,7 +61,7 @@
 #include <nuttx/clock.h>
 
 #include <arch/board/board.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
@@ -569,6 +569,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGPOLLRATE:
 		if (_call_interval == 0)
 			return SENSOR_POLLRATE_MANUAL;
+
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH:
@@ -976,17 +977,21 @@ start()
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		goto fail;
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
 	exit(0);
 fail:
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
+
 	errx(1, "driver start failed");
 }
 
@@ -1006,12 +1011,14 @@ test()
 
 	/* get the driver */
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
 		    ACCEL_DEVICE_PATH);
 
 	/* get the driver */
 	fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+
 	if (fd_gyro < 0)
 		err(1, "%s open failed", GYRO_DEVICE_PATH);
 
@@ -1021,6 +1028,7 @@ test()
 
 	/* do a simple demand read */
 	sz = read(fd, &a_report, sizeof(a_report));
+
 	if (sz != sizeof(a_report))
 		err(1, "immediate acc read failed");
 
@@ -1037,6 +1045,7 @@ test()
 
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
+
 	if (sz != sizeof(g_report))
 		err(1, "immediate gyro read failed");
 
@@ -1066,10 +1075,13 @@ void
 reset()
 {
 	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "failed ");
+
 	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
 		err(1, "driver reset failed");
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 

apps/drivers/ms5611/ms5611.cpp

@@ -57,7 +57,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -70,6 +70,10 @@
 #endif
 static const int ERROR = -1;
 
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
 /**
  * Calibration PROM as reported by the device.
  */
@@ -299,6 +303,7 @@ MS5611::init()
 	/* allocate basic report buffers */
 	_num_reports = 2;
 	_reports = new struct baro_report[_num_reports];
+
 	if (_reports == nullptr)
 		goto out;
 
@@ -307,6 +312,7 @@ MS5611::init()
 	/* get a publish handle on the baro topic */
 	memset(&_reports[0], 0, sizeof(_reports[0]));
 	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
+
 	if (_baro_topic < 0)
 		debug("failed to create sensor_baro object");
 
@@ -319,6 +325,7 @@ int
 MS5611::probe()
 {
 	_retries = 10;
+
 	if ((OK == probe_address(MS5611_ADDRESS_1)) ||
 	    (OK == probe_address(MS5611_ADDRESS_2))) {
 		_retries = 1;
@@ -480,6 +487,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGPOLLRATE:
 		if (_measure_ticks == 0)
 			return SENSOR_POLLRATE_MANUAL;
+
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
@@ -514,9 +522,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return -EINVAL;
 
 	case BAROIOCSMSLPRESSURE:
+
 		/* range-check for sanity */
 		if ((arg < 80000) || (arg > 120000))
 			return -EINVAL;
+
 		_msl_pressure = arg;
 		return OK;
 
@@ -693,6 +703,7 @@ MS5611::collect()
 			_OFF  -= OFF2;
 			_SENS -= SENS2;
 		}
+
 	} else {
 
 		/* pressure calculation, result in Pa */
@@ -937,17 +948,22 @@ start()
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		goto fail;
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
+
 	exit(0);
 
 fail:
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
+
 	errx(1, "driver start failed");
 }
 
@@ -964,11 +980,13 @@ test()
 	int ret;
 
 	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
+
 	if (sz != sizeof(report))
 		err(1, "immediate read failed");
 
@@ -1021,10 +1039,13 @@ void
 reset()
 {
 	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "failed ");
+
 	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
 		err(1, "driver reset failed");
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		err(1, "driver poll restart failed");
 
@@ -1057,6 +1078,7 @@ calibrate(unsigned altitude)
 	float	p1;
 
 	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
 	if (fd < 0)
 		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
 
@@ -1066,6 +1088,7 @@ calibrate(unsigned altitude)
 
 	/* average a few measurements */
 	pressure = 0.0f;
+
 	for (unsigned i = 0; i < 20; i++) {
 		struct pollfd fds;
 		int ret;
@@ -1087,6 +1110,7 @@ calibrate(unsigned altitude)
 
 		pressure += report.pressure;
 	}
+
 	pressure /= 20;		/* average */
 	pressure /= 10;		/* scale from millibar to kPa */
 
@@ -1104,8 +1128,10 @@ calibrate(unsigned altitude)
 
 	/* save as integer Pa */
 	p1 *= 1000.0f;
+
 	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
 		err(1, "BAROIOCSMSLPRESSURE");
+
 	exit(0);
 }
 

apps/drivers/px4io/px4io.cpp

@@ -190,6 +190,7 @@ PX4IO::~PX4IO()
 
 		/* spin waiting for the thread to stop */
 		unsigned i = 10;
+
 		do {
 			/* wait 50ms - it should wake every 100ms or so worst-case */
 			usleep(50000);
@@ -223,11 +224,13 @@ PX4IO::init()
 
 	/* do regular cdev init */
 	ret = CDev::init();
+
 	if (ret != OK)
 		return ret;
 
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
 	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+
 	if (ret == OK) {
 		log("default PWM output device");
 		_primary_pwm_device = true;
@@ -235,6 +238,7 @@ PX4IO::init()
 
 	/* start the IO interface task */
 	_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
 		return -errno;
@@ -256,6 +260,7 @@ PX4IO::task_main()
 
 	/* open the serial port */
 	_serial_fd = ::open("/dev/ttyS2", O_RDWR);
+
 	if (_serial_fd < 0) {
 		debug("failed to open serial port for IO: %d", errno);
 		_task = -1;
@@ -343,6 +348,7 @@ PX4IO::task_main()
 					_send_needed = true;
 				}
 			}
+
 			if (fds[2].revents & POLLIN) {
 
 				orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
@@ -458,13 +464,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
+
 		/* fake an update to the selected servo channel */
 		if ((arg >= 900) && (arg <= 2100)) {
 			_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
 			_send_needed = true;
+
 		} else {
 			ret = -EINVAL;
 		}
+
 		break;
 
 	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
@@ -481,6 +490,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			delete _mixers;
 			_mixers = nullptr;
 		}
+
 		break;
 
 	case MIXERIOCADDSIMPLE: {
@@ -519,6 +529,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 			/* allocate a new mixer group and load it from the file */
 			newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+
 			if (newmixers->load_from_file(path) != 0) {
 				delete newmixers;
 				ret = -EINVAL;
@@ -528,6 +539,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			if (_mixers != nullptr) {
 				delete _mixers;
 			}
+
 			_mixers = newmixers;
 
 		}
@@ -537,6 +549,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		/* not a recognised value */
 		ret = -ENOTTY;
 	}
+
 	unlock();
 
 	return ret;
@@ -576,6 +589,7 @@ px4io_main(int argc, char *argv[])
 		if (argc > 2) {
 			fn[0] = argv[2];
 			fn[1] = nullptr;
+
 		} else {
 			fn[0] = "/fs/microsd/px4io.bin";
 			fn[1] =	"/etc/px4io.bin";
@@ -589,18 +603,24 @@ px4io_main(int argc, char *argv[])
 		switch (ret) {
 		case OK:
 			break;
+
 		case -ENOENT:
 			errx(1, "PX4IO firmware file not found");
+
 		case -EEXIST:
 		case -EIO:
 			errx(1, "error updating PX4IO - check that bootloader mode is enabled");
+
 		case -EINVAL:
 			errx(1, "verify failed - retry the update");
+
 		case -ETIMEDOUT:
 			errx(1, "timed out waiting for bootloader - power-cycle and try again");
+
 		default:
 			errx(1, "unexpected error %d", ret);
 		}
+
 		return ret;
 	}
 

apps/drivers/px4io/uploader.cpp

@@ -67,6 +67,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 	int	ret;
 
 	_io_fd = open("/dev/ttyS2", O_RDWR);
+
 	if (_io_fd < 0) {
 		log("could not open interface");
 		return -errno;
@@ -74,6 +75,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 
 	/* look for the bootloader */
 	ret = sync();
+
 	if (ret != OK) {
 		/* this is immediately fatal */
 		log("bootloader not responding");
@@ -87,9 +89,11 @@ PX4IO_Uploader::upload(const char *filenames[])
 			log("failed to open %s", filenames[i]);
 			continue;
 		}
+
 		log("using firmware from %s", filenames[i]);
 		break;
 	}
+
 	if (_fw_fd == -1)
 		return -ENOENT;
 
@@ -98,6 +102,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 		if (retries > 0) {
 			log("retrying update...");
 			ret = sync();
+
 			if (ret != OK) {
 				/* this is immediately fatal */
 				log("bootloader not responding");
@@ -106,25 +111,33 @@ PX4IO_Uploader::upload(const char *filenames[])
 		}
 
 		ret = erase();
+
 		if (ret != OK) {
 			log("erase failed");
 			continue;
 		}
+
 		ret = program();
+
 		if (ret != OK) {
 			log("program failed");
 			continue;
 		}
+
 		ret = verify();
+
 		if (ret != OK) {
 			log("verify failed");
 			continue;
 		}
+
 		ret = reboot();
+
 		if (ret != OK) {
 			log("reboot failed");
 			return ret;
 		}
+
 		log("update complete");
 
 		ret = OK;
@@ -145,6 +158,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 
 	/* wait 100 ms for a character */
 	int ret = ::poll(&fds[0], 1, timeout);
+
 	if (ret < 1) {
 		//log("poll timeout %d", ret);
 		return -ETIMEDOUT;
@@ -160,9 +174,11 @@ PX4IO_Uploader::recv(uint8_t *p, unsigned count)
 {
 	while (count--) {
 		int ret = recv(*p++);
+
 		if (ret != OK)
 			return ret;
 	}
+
 	return OK;
 }
 
@@ -184,6 +200,7 @@ PX4IO_Uploader::send(uint8_t c)
 	//log("send 0x%02x", c);
 	if (write(_io_fd, &c, 1) != 1)
 		return -errno;
+
 	return OK;
 }
 
@@ -192,9 +209,11 @@ PX4IO_Uploader::send(uint8_t *p, unsigned count)
 {
 	while (count--) {
 		int ret = send(*p++);
+
 		if (ret != OK)
 			return ret;
 	}
+
 	return OK;
 }
 
@@ -205,15 +224,20 @@ PX4IO_Uploader::get_sync(unsigned timeout)
 	int ret;
 
 	ret = recv(c[0], timeout);
+
 	if (ret != OK)
 		return ret;
+
 	ret = recv(c[1], timeout);
+
 	if (ret != OK)
 		return ret;
+
 	if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) {
 		log("bad sync 0x%02x,0x%02x", c[0], c[1]);
 		return -EIO;
 	}
+
 	return OK;
 }
 
@@ -221,9 +245,11 @@ int
 PX4IO_Uploader::sync()
 {
 	drain();
+
 	/* complete any pending program operation */
 	for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++)
 		send(0);
+
 	send(PROTO_GET_SYNC);
 	send(PROTO_EOC);
 	return get_sync();
@@ -239,8 +265,10 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
 	send(PROTO_EOC);
 
 	ret = recv((uint8_t *)&val, sizeof(val));
+
 	if (ret != OK)
 		return ret;
+
 	return get_sync();
 }
 
@@ -267,10 +295,13 @@ PX4IO_Uploader::program()
 		/* get more bytes to program */
 		//log("  %d", (int)lseek(_fw_fd, 0, SEEK_CUR));
 		count = read(_fw_fd, file_buf, sizeof(file_buf));
+
 		if (count == 0)
 			return OK;
+
 		if (count < 0)
 			return -errno;
+
 		ASSERT((count % 4) == 0);
 
 		send(PROTO_PROG_MULTI);
@@ -279,6 +310,7 @@ PX4IO_Uploader::program()
 		send(PROTO_EOC);
 
 		ret = get_sync(1000);
+
 		if (ret != OK)
 			return ret;
 	}
@@ -297,6 +329,7 @@ PX4IO_Uploader::verify()
 	send(PROTO_CHIP_VERIFY);
 	send(PROTO_EOC);
 	ret = get_sync();
+
 	if (ret != OK)
 		return ret;
 
@@ -304,19 +337,24 @@ PX4IO_Uploader::verify()
 		/* get more bytes to verify */
 		int base = (int)lseek(_fw_fd, 0, SEEK_CUR);
 		count = read(_fw_fd, file_buf, sizeof(file_buf));
+
 		if (count == 0)
 			break;
+
 		if (count < 0)
 			return -errno;
+
 		ASSERT((count % 4) == 0);
 
 		send(PROTO_READ_MULTI);
 		send(count);
 		send(PROTO_EOC);
+
 		for (ssize_t i = 0; i < count; i++) {
 			uint8_t c;
 
 			ret = recv(c);
+
 			if (ret != OK) {
 				log("%d: got %d waiting for bytes", base + i, ret);
 				return ret;
@@ -327,12 +365,15 @@ PX4IO_Uploader::verify()
 				return -EINVAL;
 			}
 		}
+
 		ret = get_sync();
+
 		if (ret != OK) {
 			log("timeout waiting for post-verify sync");
 			return ret;
 		}
 	}
+
 	return OK;
 }
 
@@ -358,6 +399,7 @@ PX4IO_Uploader::compare(bool &identical)
 	send(PROTO_CHIP_VERIFY);
 	send(PROTO_EOC);
 	ret = get_sync();
+
 	if (ret != OK)
 		return ret;
 
@@ -365,6 +407,7 @@ PX4IO_Uploader::compare(bool &identical)
 	send(sizeof(fw_vectors));
 	send(PROTO_EOC);
 	ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors));
+
 	if (ret != OK)
 		return ret;
 

apps/drivers/stm32/Makefile

@@ -32,11 +32,11 @@
 ############################################################################
 
 #
-# Makefile to build the black magic attitude estimator
+# STM32 driver support code
+#
+# Modules in this directory are compiled for all STM32 targets.
 #
 
-APPNAME		 = attitude_estimator_bm
+INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-PRIORITY	= SCHED_PRIORITY_MAX - 10
-STACKSIZE	= 12000
 
 include $(APPDIR)/mk/app.mk

apps/drivers/stm32/drv_hrt.c

@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file High-resolution timer callouts and timekeeping.
+ * @file drv_hrt.c
+ *
+ * High-resolution timer callouts and timekeeping.
  *
  * This can use any general or advanced STM32 timer.
  *
@@ -58,7 +60,7 @@
 #include <string.h>
 
 #include <arch/board/board.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "chip.h"
 #include "up_internal.h"
@@ -320,16 +322,16 @@ static void		hrt_call_invoke(void);
 
 /* decoded PPM buffer */
 #define PPM_MAX_CHANNELS	12
-uint16_t ppm_buffer[PPM_MAX_CHANNELS];
+__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
-unsigned ppm_decoded_channels;
+__EXPORT unsigned ppm_decoded_channels;
-uint64_t ppm_last_valid_decode = 0;
+__EXPORT uint64_t ppm_last_valid_decode = 0;
 
 /* PPM edge history */
-uint16_t ppm_edge_history[32];
+__EXPORT uint16_t ppm_edge_history[32];
 unsigned ppm_edge_next;
 
 /* PPM pulse history */
-uint16_t ppm_pulse_history[32];
+__EXPORT uint16_t ppm_pulse_history[32];
 unsigned ppm_pulse_next;
 
 static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
@@ -426,6 +428,7 @@ hrt_ppm_decode(uint32_t status)
 	ppm.last_edge = count;
 
 	ppm_edge_history[ppm_edge_next++] = width;
+
 	if (ppm_edge_next >= 32)
 		ppm_edge_next = 0;
 
@@ -439,6 +442,7 @@ hrt_ppm_decode(uint32_t status)
 		if (ppm.next_channel > 4) {
 			for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
 				ppm_buffer[i] = ppm_temp_buffer[i];
+
 			ppm_decoded_channels = i;
 			ppm_last_valid_decode = hrt_absolute_time();
 
@@ -459,6 +463,7 @@ hrt_ppm_decode(uint32_t status)
 		return;
 
 	case ARM:
+
 		/* we expect a pulse giving us the first mark */
 		if (width > PPM_MAX_PULSE_WIDTH)
 			goto error;		/* pulse was too long */
@@ -479,6 +484,7 @@ hrt_ppm_decode(uint32_t status)
 		ppm.last_mark = count;
 
 		ppm_pulse_history[ppm_pulse_next++] = interval;
+
 		if (ppm_pulse_next >= 32)
 			ppm_pulse_next = 0;
 
@@ -524,9 +530,11 @@ hrt_tim_isr(int irq, void *context)
 	rSR = ~status;
 
 #ifdef CONFIG_HRT_PPM
+
 	/* was this a PPM edge? */
 	if (status & (SR_INT_PPM | SR_OVF_PPM))
 		hrt_ppm_decode(status);
+
 #endif
 
 	/* was this a timer tick? */
@@ -728,9 +736,11 @@ hrt_call_enter(struct hrt_call *entry)
 		//lldbg("call enter at head, reschedule\n");
 		/* we changed the next deadline, reschedule the timer event */
 		hrt_call_reschedule();
+
 	} else {
 		do {
 			next = (struct hrt_call *)sq_next(&call->link);
+
 			if ((next == NULL) || (entry->deadline < next->deadline)) {
 				//lldbg("call enter after head\n");
 				sq_addafter(&call->link, &entry->link, &callout_queue);
@@ -753,8 +763,10 @@ hrt_call_invoke(void)
 		hrt_abstime now = hrt_absolute_time();
 
 		call = (struct hrt_call *)sq_peek(&callout_queue);
+
 		if (call == NULL)
 			break;
+
 		if (call->deadline > now)
 			break;
 
@@ -811,11 +823,13 @@ hrt_call_reschedule()
 			//lldbg("pre-expired\n");
 			/* set a minimal deadline so that we call ASAP */
 			deadline = now + HRT_INTERVAL_MIN;
+
 		} else if (next->deadline < deadline) {
 			//lldbg("due soon\n");
 			deadline = next->deadline;
 		}
 	}
+
 	//lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
 
 	/* set the new compare value and remember it for latency tracking */
@@ -835,6 +849,7 @@ hrt_latency_update(void)
 			return;
 		}
 	}
+
 	/* catch-all at the end */
 	latency_counters[index]++;
 }

apps/drivers/stm32/drv_pwm_servo.c

@@ -32,6 +32,8 @@
  ****************************************************************************/
 
 /*
+ * @file drv_pwm_servo.c
+ *
  * Servo driver supporting PWM servos connected to STM32 timer blocks.
  *
  * Works with any of the 'generic' or 'advanced' STM32 timers that
@@ -54,7 +56,9 @@
 #include <stdio.h>
 
 #include <arch/board/board.h>
-#include <arch/board/up_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "drv_pwm_servo.h"
 
 #include "chip.h"
 #include "up_internal.h"
@@ -64,67 +68,10 @@
 #include "stm32_gpio.h"
 #include "stm32_tim.h"
 
-/* configuration limits */
-#define PWM_SERVO_MAX_TIMERS	2
-#define PWM_SERVO_MAX_CHANNELS	8
 
 /* default rate (in Hz) of PWM updates */
 static uint32_t	pwm_update_rate = 50;
 
-/*
- * Servo configuration for all of the pins that can be used as
- * PWM outputs on FMU.
- */
-
-/* array of timers dedicated to PWM servo use */
-static const struct pwm_servo_timer {
-	uint32_t	base;
-	uint32_t	clock_register;
-	uint32_t	clock_bit;
-	uint32_t	clock_freq;
-} pwm_timers[] = {
-	{
-		.base = STM32_TIM2_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM2EN,
-		.clock_freq = STM32_APB1_TIM2_CLKIN
-	}
-};
-
-/* array of channels in logical order */
-static const struct pwm_servo_channel {
-	uint32_t	gpio;
-	uint8_t		timer_index;
-	uint8_t		timer_channel;
-	servo_position_t default_value;
-} pwm_channels[] = {
-	{
-		.gpio = GPIO_TIM2_CH1OUT,
-		.timer_index = 0,
-		.timer_channel = 1,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH2OUT,
-		.timer_index = 0,
-		.timer_channel = 2,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH3OUT,
-		.timer_index = 0,
-		.timer_channel = 3,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH4OUT,
-		.timer_index = 0,
-		.timer_channel = 4,
-		.default_value = 1000,
-	}
-};
-
-
 #define REG(_tmr, _reg)	(*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
 
 #define rCR1(_tmr)    	REG(_tmr, STM32_GTIM_CR1_OFFSET)
@@ -200,16 +147,19 @@ pwm_channel_init(unsigned channel)
 		rCCR1(timer) = pwm_channels[channel].default_value;
 		rCCER(timer) |= (1 << 0);
 		break;
+
 	case 2:
 		rCCMR1(timer) |= (6 << 12);
 		rCCR2(timer) = pwm_channels[channel].default_value;
 		rCCER(timer) |= (1 << 4);
 		break;
+
 	case 3:
 		rCCMR2(timer) |= (6 << 4);
 		rCCR3(timer) = pwm_channels[channel].default_value;
 		rCCER(timer) |= (1 << 8);
 		break;
+
 	case 4:
 		rCCMR2(timer) |= (6 << 12);
 		rCCR4(timer) = pwm_channels[channel].default_value;
@@ -236,22 +186,28 @@ up_pwm_servo_set(unsigned channel, servo_position_t value)
 	/* configure the channel */
 	if (value > 0)
 		value--;
+
 	switch (pwm_channels[channel].timer_channel) {
 	case 1:
 		rCCR1(timer) = value;
 		break;
+
 	case 2:
 		rCCR2(timer) = value;
 		break;
+
 	case 3:
 		rCCR3(timer) = value;
 		break;
+
 	case 4:
 		rCCR4(timer) = value;
 		break;
+
 	default:
 		return -1;
 	}
+
 	return 0;
 }
 
@@ -276,16 +232,20 @@ up_pwm_servo_get(unsigned channel)
 	case 1:
 		value = rCCR1(timer);
 		break;
+
 	case 2:
 		value = rCCR2(timer);
 		break;
+
 	case 3:
 		value = rCCR3(timer);
 		break;
+
 	case 4:
 		value = rCCR4(timer);
 		break;
 	}
+
 	return value;
 }
 
@@ -304,6 +264,7 @@ up_pwm_servo_init(uint32_t channel_mask)
 		if (((1 << i) & channel_mask) && (pwm_channels[i].gpio != 0))
 			pwm_channel_init(i);
 	}
+
 	return OK;
 }
 
@@ -324,6 +285,7 @@ up_pwm_servo_set_rate(unsigned rate)
 		if (pwm_timers[i].base != 0)
 			pwm_timer_set_rate(i, rate);
 	}
+
 	return OK;
 }
 

apps/drivers/stm32/drv_pwm_servo.h

@@ -1,9 +1,6 @@
-/************************************************************************************
+/****************************************************************************
- * configs/stm3240g-eval/src/up_adc.c
- * arch/arm/src/board/up_adc.c
  *
- *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -15,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -32,29 +29,39 @@
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  *
- ************************************************************************************/
+ ****************************************************************************/
 
-/************************************************************************************
+/**
- * Included Files
+ * @file drv_pwm_servo.h
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#ifdef CONFIG_ADC
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: adc_devinit
  *
- * Description:
+ * stm32-specific PWM output data.
- *   All STM32 architectures must provide the following interface to work with
+ */
- *   examples/adc.
- *
- ************************************************************************************/
 
-int adc_devinit(void);
+#pragma once
 
-#endif /* CONFIG_ADC */
+#include <drivers/drv_pwm_output.h>
+
+/* configuration limits */
+#define PWM_SERVO_MAX_TIMERS	2
+#define PWM_SERVO_MAX_CHANNELS	8
+
+/* array of timers dedicated to PWM servo use */
+struct pwm_servo_timer {
+	uint32_t	base;
+	uint32_t	clock_register;
+	uint32_t	clock_bit;
+	uint32_t	clock_freq;
+};
+
+/* supplied by board-specific code */
+__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS];
+
+/* array of channels in logical order */
+struct pwm_servo_channel {
+	uint32_t	gpio;
+	uint8_t		timer_index;
+	uint8_t		timer_channel;
+	servo_position_t default_value;
+};
+
+__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS];

apps/drivers/stm32/tone_alarm/tone_alarm.cpp

@@ -71,7 +71,7 @@
 #include <unistd.h>
 
 #include <arch/board/board.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <arch/stm32/chip.h>
 #include <up_internal.h>
@@ -244,7 +244,8 @@ const tone_note ToneAlarm::_patterns[_max_pattern][_max_pattern_len] = {
 	{{TONE_NOTE_C7, 100}},
 	{{TONE_NOTE_D7, 100}},
 	{{TONE_NOTE_E7, 100}},
-	{	//This is tetris ;)
+	{
+		//This is tetris ;)
 		{TONE_NOTE_C6, 40},
 		{TONE_NOTE_G5, 20},
 		{TONE_NOTE_G5S, 20},
@@ -361,6 +362,7 @@ ToneAlarm::init()
 	int ret;
 
 	ret = CDev::init();
+
 	if (ret != OK)
 		return ret;
 
@@ -413,6 +415,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 	switch (cmd) {
 	case TONE_SET_ALARM:
 		debug("TONE_SET_ALARM %u", arg);
+
 		if (new_pattern == 0) {
 			/* cancel any current alarm */
 			_current_pattern = _pattern_none;
@@ -431,10 +434,13 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 
 			/* and start playing it */
 			next();
+
 		} else {
 			/* current pattern is higher priority than the new pattern, ignore */
 		}
+
 		break;
+
 	default:
 		result = -ENOTTY;
 		break;
@@ -457,8 +463,10 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len)
 	/* sanity-check the size of the write */
 	if (len > (_max_pattern_len * sizeof(tone_note)))
 		return -EFBIG;
+
 	if ((len % sizeof(tone_note)) || (len == 0))
 		return -EIO;
+
 	if (!check((tone_note *)buffer))
 		return -EIO;
 
@@ -479,6 +487,7 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len)
 		debug("starting user pattern");
 		next();
 	}
+
 	irqrestore(flags);
 
 	return len;
@@ -511,18 +520,22 @@ ToneAlarm::next(void)
 	/* find the note to play */
 	if (_current_pattern == _pattern_user) {
 		np = &_user_pattern[_next_note];
+
 	} else {
 		np = &_patterns[_current_pattern - 1][_next_note];
 	}
 
 	/* work out which note is next */
 	_next_note++;
+
 	if (_next_note >= _note_max) {
 		/* hit the end of the pattern, stop */
 		_current_pattern = _pattern_none;
+
 	} else if (np[1].duration == DURATION_END) {
 		/* hit the end of the pattern, stop */
 		_current_pattern = _pattern_none;
+
 	} else if (np[1].duration == DURATION_REPEAT) {
 		/* next note is a repeat, rewind in preparation */
 		_next_note = 0;
@@ -554,6 +567,7 @@ ToneAlarm::check(tone_note *pattern)
 		if ((i == 0) &&
 		    ((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT)))
 			return false;
+
 		if (pattern[i].duration == DURATION_END)
 			break;
 
@@ -561,6 +575,7 @@ ToneAlarm::check(tone_note *pattern)
 		if (pattern[i].pitch >= _note_max)
 			return false;
 	}
+
 	return true;
 }
 
@@ -592,13 +607,16 @@ play_pattern(unsigned pattern)
 	int	fd, ret;
 
 	fd = open("/dev/tone_alarm", 0);
+
 	if (fd < 0)
 		err(1, "/dev/tone_alarm");
 
 	warnx("playing pattern %u", pattern);
 	ret = ioctl(fd, TONE_SET_ALARM, pattern);
+
 	if (ret != 0)
 		err(1, "TONE_SET_ALARM");
+
 	exit(0);
 }
 
@@ -615,6 +633,7 @@ tone_alarm_main(int argc, char *argv[])
 
 		if (g_dev == nullptr)
 			errx(1, "couldn't allocate the ToneAlarm driver");
+
 		if (g_dev->init() != OK) {
 			delete g_dev;
 			errx(1, "ToneAlarm init failed");
@@ -623,8 +642,10 @@ tone_alarm_main(int argc, char *argv[])
 
 	if ((argc > 1) && !strcmp(argv[1], "start"))
 		play_pattern(1);
+
 	if ((argc > 1) && !strcmp(argv[1], "stop"))
 		play_pattern(0);
+
 	if ((pattern = strtol(argv[1], nullptr, 10)) != 0)
 		play_pattern(pattern);
 

apps/fixedwing_control/fixedwing_control.c

@@ -48,7 +48,7 @@
 #include <math.h>
 #include <poll.h>
 #include <time.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>

apps/gps/gps.c

@@ -564,10 +564,10 @@ void setup_port(char *device, int speed, int *fd)
 	*fd = open_port(device);
 
 	if (*fd != -1) {
-		if (gps_verbose) printf("[gps] Port opened: %s at %d speed\r\n", device, speed);
+		if (gps_verbose) printf("[gps] Port opened: %s at %d baud\n", device, speed);
 
 	} else {
-		fprintf(stderr, "[gps] Could not open port, exiting gps app!\r\n");
+		fprintf(stderr, "[gps] Could not open port, exiting gps app!\n");
 		fflush(stdout);
 	}
 
@@ -576,7 +576,7 @@ void setup_port(char *device, int speed, int *fd)
 	int termios_state;
 
 	if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) {
-		fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\r\n", device, termios_state);
+		fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\n", device, termios_state);
 		close(*fd);
 	}
 	if (gps_verbose) printf("[gps] Try to set baud rate %d now\n",speed);
@@ -584,7 +584,7 @@ void setup_port(char *device, int speed, int *fd)
 	cfsetispeed(&uart_config, speed);
 	cfsetospeed(&uart_config, speed);
 	if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) {
-		fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", device);
+		fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\n", device);
 		close(*fd);
 	}
 }

apps/gps/mtk.c

@@ -43,7 +43,7 @@
 #include <pthread.h>
 #include <poll.h>
 #include <fcntl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <mavlink/mavlink_log.h>

apps/gps/nmea_helper.c

@@ -44,7 +44,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <mavlink/mavlink_log.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2
 #define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2

apps/gps/ubx.c

@@ -40,7 +40,7 @@
 #include "gps.h"
 #include <sys/prctl.h>
 #include <poll.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <string.h>
 #include <stdbool.h>
@@ -50,6 +50,7 @@
 
 #define UBX_HEALTH_SUCCESS_COUNTER_LIMIT 2
 #define UBX_HEALTH_FAIL_COUNTER_LIMIT 2
+#define UBX_HEALTH_PROBE_COUNTER_LIMIT 4
 
 #define UBX_BUFFER_SIZE 1000
 
@@ -242,9 +243,9 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 						ubx_gps->timestamp = hrt_absolute_time();
 						ubx_gps->counter++;
 
-						pthread_mutex_lock(ubx_mutex);
+						//pthread_mutex_lock(ubx_mutex);
 						ubx_state->last_message_timestamps[NAV_POSLLH - 1] = hrt_absolute_time();
-						pthread_mutex_unlock(ubx_mutex);
+						//pthread_mutex_unlock(ubx_mutex);
 						ret = 1;
 
 					} else {
@@ -273,9 +274,9 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 						ubx_gps->counter++;
 
 
-						pthread_mutex_lock(ubx_mutex);
+						//pthread_mutex_lock(ubx_mutex);
 						ubx_state->last_message_timestamps[NAV_SOL - 1] = hrt_absolute_time();
-						pthread_mutex_unlock(ubx_mutex);
+						//pthread_mutex_unlock(ubx_mutex);
 						ret = 1;
 
 					} else {
@@ -305,9 +306,9 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 						ubx_gps->counter++;
 
 
-						pthread_mutex_lock(ubx_mutex);
+						//pthread_mutex_lock(ubx_mutex);
 						ubx_state->last_message_timestamps[NAV_DOP - 1] = hrt_absolute_time();
-						pthread_mutex_unlock(ubx_mutex);
+						//pthread_mutex_unlock(ubx_mutex);
 						ret = 1;
 
 					} else {
@@ -351,9 +352,9 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 						ubx_gps->counter++;
 
 
-						pthread_mutex_lock(ubx_mutex);
+						//pthread_mutex_lock(ubx_mutex);
 						ubx_state->last_message_timestamps[NAV_TIMEUTC - 1] = hrt_absolute_time();
-						pthread_mutex_unlock(ubx_mutex);
+						//pthread_mutex_unlock(ubx_mutex);
 						ret = 1;
 
 					} else {
@@ -452,9 +453,9 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 						ubx_gps->counter++;
 
 
-						pthread_mutex_lock(ubx_mutex);
+						//pthread_mutex_lock(ubx_mutex);
 						ubx_state->last_message_timestamps[NAV_SVINFO - 1] = hrt_absolute_time();
-						pthread_mutex_unlock(ubx_mutex);
+						//pthread_mutex_unlock(ubx_mutex);
 						ret = 1;
 
 					} else {
@@ -484,9 +485,9 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 						ubx_gps->counter++;
 
 
-						pthread_mutex_lock(ubx_mutex);
+						//pthread_mutex_lock(ubx_mutex);
 						ubx_state->last_message_timestamps[NAV_VELNED - 1] = hrt_absolute_time();
-						pthread_mutex_unlock(ubx_mutex);
+						//pthread_mutex_unlock(ubx_mutex);
 						ret = 1;
 
 					} else {
@@ -518,9 +519,9 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 						ubx_gps->counter++;
 
 
-						pthread_mutex_lock(ubx_mutex);
+						//pthread_mutex_lock(ubx_mutex);
 						ubx_state->last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
-						pthread_mutex_unlock(ubx_mutex);
+						//pthread_mutex_unlock(ubx_mutex);
 						ret = 1;
 
 					} else {
@@ -697,9 +698,10 @@ int write_config_message_ubx(uint8_t *message, size_t length, int fd)
 
 //	printf("[%x,%x]\n", ck_a, ck_b);
 
-	int result_write =  write(fd, message, length);
+	unsigned int result_write =  write(fd, message, length);
 	result_write +=  write(fd, &ck_a, 1);
 	result_write +=  write(fd, &ck_b, 1);
+	fsync(fd);
 
 	return (result_write != length + 2); //return 0 as success
 
@@ -759,15 +761,15 @@ void *ubx_watchdog_loop(void *args)
 
 
 			/* If we have too many failures and another mode or baud should be tried, exit... */
-			if ((gps_mode_try_all == true  || gps_baud_try_all == true) && (ubx_fail_count >= UBX_HEALTH_FAIL_COUNTER_LIMIT) && (ubx_healthy == false) && once_ok == false) {
+			if ((gps_mode_try_all == true  || gps_baud_try_all == true) && (ubx_fail_count >= UBX_HEALTH_PROBE_COUNTER_LIMIT) && (ubx_healthy == false) && once_ok == false) {
-				if (gps_verbose) printf("[gps] Connection attempt failed, no UBX module found\r\n");
+				if (gps_verbose) printf("[gps] Connection attempt failed, no UBX module found\n");
 
 				gps_mode_success = false;
 				break;
 			}
 
 			if (ubx_healthy && ubx_fail_count == UBX_HEALTH_FAIL_COUNTER_LIMIT) {
-				printf("[gps] ERROR: UBX GPS module stopped responding\r\n");
+				printf("[gps] ERROR: UBX GPS module stopped responding\n");
 				// global_data_send_subsystem_info(&ubx_present_enabled);
 				mavlink_log_critical(mavlink_fd, "[gps] UBX module stopped responding\n");
 				ubx_healthy = false;
@@ -817,7 +819,7 @@ void *ubx_loop(void *args)
 	char gps_rx_buffer[UBX_BUFFER_SIZE];
 
 
-	if (gps_verbose) printf("[gps] UBX protocol driver starting..\r\n");
+	if (gps_verbose) printf("[gps] UBX protocol driver starting..\n");
 
 	//set parameters for ubx_state
 
@@ -831,14 +833,14 @@ void *ubx_loop(void *args)
 	/* set parameters for ubx */
 
 	if (configure_gps_ubx(fd) != 0) {
-		printf("[gps] Configuration of gps module to ubx failed\r\n");
+		printf("[gps] Configuration of gps module to ubx failed\n");
 
 		/* Write shared variable sys_status */
 		// TODO enable this again
 		//global_data_send_subsystem_info(&ubx_present);
 
 	} else {
-		if (gps_verbose) printf("[gps] Attempting to configure GPS to UBX binary protocol\r\n");
+		if (gps_verbose) printf("[gps] Attempting to configure GPS to UBX binary protocol\n");
 
 		// XXX Shouldn't the system status only change if the module is known to work ok?
 

apps/gps/ubx.h

@@ -76,6 +76,7 @@
 
 // ************
 /** the structures of the binary packets */
+#pragma pack(push, 1)
 typedef struct {
 	uint32_t time_milliseconds; // GPS Millisecond Time of Week
 	int32_t lon;  // Longitude * 1e-7, deg
@@ -87,7 +88,7 @@ typedef struct {
 
 	uint8_t ck_a;
 	uint8_t ck_b;
-}  __attribute__((__packed__)) type_gps_bin_nav_posllh_packet;
+} type_gps_bin_nav_posllh_packet;
 
 typedef type_gps_bin_nav_posllh_packet gps_bin_nav_posllh_packet_t;
 
@@ -112,7 +113,7 @@ typedef struct {
 
 	uint8_t ck_a;
 	uint8_t ck_b;
-}  __attribute__((__packed__)) type_gps_bin_nav_sol_packet;
+} type_gps_bin_nav_sol_packet;
 
 typedef type_gps_bin_nav_sol_packet gps_bin_nav_sol_packet_t;
 
@@ -131,7 +132,7 @@ typedef struct {
 
 	uint8_t ck_a;
 	uint8_t ck_b;
-}  __attribute__((__packed__)) type_gps_bin_nav_timeutc_packet;
+} type_gps_bin_nav_timeutc_packet;
 
 typedef type_gps_bin_nav_timeutc_packet gps_bin_nav_timeutc_packet_t;
 
@@ -148,7 +149,7 @@ typedef struct {
 
 	uint8_t ck_a;
 	uint8_t ck_b;
-}  __attribute__((__packed__)) type_gps_bin_nav_dop_packet;
+} type_gps_bin_nav_dop_packet;
 
 typedef type_gps_bin_nav_dop_packet gps_bin_nav_dop_packet_t;
 
@@ -158,7 +159,7 @@ typedef struct {
 	uint8_t globalFlags;
 	uint16_t reserved2;
 
-}  __attribute__((__packed__)) type_gps_bin_nav_svinfo_part1_packet;
+} type_gps_bin_nav_svinfo_part1_packet;
 
 typedef type_gps_bin_nav_svinfo_part1_packet gps_bin_nav_svinfo_part1_packet_t;
 
@@ -172,7 +173,7 @@ typedef struct {
 	int16_t azim; //Azimuth in integer degrees
 	int32_t prRes; //Pseudo range residual in centimetres
 
-}  __attribute__((__packed__)) type_gps_bin_nav_svinfo_part2_packet;
+} type_gps_bin_nav_svinfo_part2_packet;
 
 typedef type_gps_bin_nav_svinfo_part2_packet gps_bin_nav_svinfo_part2_packet_t;
 
@@ -180,7 +181,7 @@ typedef struct {
 	uint8_t ck_a;
 	uint8_t ck_b;
 
-}  __attribute__((__packed__)) type_gps_bin_nav_svinfo_part3_packet;
+} type_gps_bin_nav_svinfo_part3_packet;
 
 typedef type_gps_bin_nav_svinfo_part3_packet gps_bin_nav_svinfo_part3_packet_t;
 
@@ -198,7 +199,7 @@ typedef struct {
 
 	uint8_t ck_a;
 	uint8_t ck_b;
-}  __attribute__((__packed__)) type_gps_bin_nav_velned_packet;
+} type_gps_bin_nav_velned_packet;
 
 typedef type_gps_bin_nav_velned_packet gps_bin_nav_velned_packet_t;
 
@@ -209,7 +210,7 @@ typedef struct {
 
 	//... rest of package is not used in this implementation
 
-}  __attribute__((__packed__)) type_gps_bin_rxm_svsi_packet;
+} type_gps_bin_rxm_svsi_packet;
 
 typedef type_gps_bin_rxm_svsi_packet gps_bin_rxm_svsi_packet_t;
 
@@ -219,7 +220,7 @@ typedef struct {
 
 	uint8_t ck_a;
 	uint8_t ck_b;
-}  __attribute__((__packed__)) type_gps_bin_ack_ack_packet;
+} type_gps_bin_ack_ack_packet;
 
 typedef type_gps_bin_ack_ack_packet gps_bin_ack_ack_packet_t;
 
@@ -229,7 +230,7 @@ typedef struct {
 
 	uint8_t ck_a;
 	uint8_t ck_b;
-}  __attribute__((__packed__)) type_gps_bin_ack_nak_packet;
+} type_gps_bin_ack_nak_packet;
 
 typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t;
 
@@ -283,9 +284,10 @@ typedef struct {
 	enum UBX_MESSAGE_IDS message_id;
 	uint64_t last_message_timestamps[UBX_NO_OF_MESSAGES];
 
-}  __attribute__((__packed__)) type_gps_bin_ubx_state;
+} type_gps_bin_ubx_state;
 
 typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
+#pragma pack(pop)
 
 extern pthread_mutex_t *ubx_mutex;
 extern gps_bin_ubx_state_t *ubx_state;

apps/mavlink/mavlink.c

@@ -50,7 +50,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -745,7 +745,10 @@ int mavlink_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "stop")) {
 		thread_should_exit = true;
-		/* XXX should wait for it to actually exit here */
+		while (thread_running) {
+			usleep(200000);
+		}
+		warnx("terminated.");
 		exit(0);
 	}
 

apps/mavlink/mavlink_bridge_header.h

@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +35,8 @@
 /**
  * @file mavlink_bridge_header
  * MAVLink bridge header for UART access.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 /* MAVLink adapter header */

apps/mavlink/mavlink_parameters.c

@@ -39,6 +39,8 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include "mavlink_bridge_header.h"
+#include <v1.0/common/mavlink.h>
 #include "mavlink_parameters.h"
 #include <uORB/uORB.h>
 #include "math.h" /* isinf / isnan checks */

apps/mavlink/mavlink_parameters.h

@@ -43,7 +43,7 @@
  or in the same folder as this source file */
 
 
-#include "v1.0/common/mavlink.h"
+#include <v1.0/mavlink_types.h>
 #include <stdbool.h>
 #include <systemlib/param/param.h>
 

apps/mavlink/mavlink_receiver.c

@@ -49,7 +49,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>

apps/mavlink/missionlib.c

@@ -49,7 +49,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>

apps/mavlink/orb_listener.c

@@ -46,7 +46,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>