diff options
133 files changed, 2453 insertions, 4418 deletions
@@ -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 diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m index 5a615228d..579a582d3 100644 --- a/ROMFS/logging/logconv.m +++ b/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 diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 833425aa6..a8ce3ea77 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/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> diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index c68a26df9..70b8ad6de 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/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> diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.m b/apps/attitude_estimator_ekf/attitudeKalmanfilter.m deleted file mode 100644 index 5fb4aa94f..000000000 --- a/apps/attitude_estimator_ekf/attitudeKalmanfilter.m +++ /dev/null @@ -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];
-
-
-
-
-
-
-
diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj b/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj deleted file mode 100644 index 431ddb71e..000000000 --- a/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj +++ /dev/null @@ -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>
-
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 7bb8490e5..4130b1c06 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/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; - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + 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!"); + } } } } diff --git a/apps/commander/calibration_routines.c b/apps/commander/calibration_routines.c index 88b64a06a..dbf6cacaf 100644 --- a/apps/commander/calibration_routines.c +++ b/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" diff --git a/apps/commander/calibration_routines.h b/apps/commander/calibration_routines.h index c5a184341..063823109 100644 --- a/apps/commander/calibration_routines.h +++ b/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. diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a3dccfd73..a7550b54b 100644 --- a/apps/commander/commander.c +++ b/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 Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@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 @@ -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 <arch/board/up_hrt.h> +#include <drivers/drv_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_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +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 }; - float chemistry_voltage_full[1] = { 4.05f }; + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; if (counter % 100 == 0) { - param_get(bat_volt_empty, &(chemistry_voltage_empty[0])); - param_get(bat_volt_full, &(chemistry_voltage_full[0])); + param_get(bat_volt_empty, &chemistry_voltage_empty); + 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, ¤t_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 */ diff --git a/apps/commander/commander.h b/apps/commander/commander.h index e9e3b24ca..bea67bead 100644 --- a/apps/commander/commander.h +++ b/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 diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index b21f3858f..794fb679c 100644 --- a/apps/commander/state_machine_helper.c +++ b/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" diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp index c554df9ae..bc4d4b3bf 100644 --- a/apps/drivers/bma180/bma180.cpp +++ b/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: { @@ -468,7 +473,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGQUEUEDEPTH: - return _num_reports -1; + return _num_reports - 1; case SENSORIOCRESET: /* XXX implement */ @@ -488,12 +493,12 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: /* copy scale in */ - memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale)); + memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); return OK; case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -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; } @@ -586,7 +597,7 @@ BMA180::set_range(unsigned max_g) /* check if wanted value is now in register */ return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) == - (OFFSET_LSB1_RANGE_MASK & rangebits)); + (OFFSET_LSB1_RANGE_MASK & rangebits)); } int @@ -633,7 +644,7 @@ BMA180::set_lowpass(unsigned frequency) /* check if wanted value is now in register */ return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) == - (BW_TCS_BW_MASK & bwbits)); + (BW_TCS_BW_MASK & bwbits)); } void @@ -703,9 +714,9 @@ BMA180::measure() * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+1)) << 8) | (read_reg(ADDR_ACC_X_LSB));// XXX PX4DEV raw_report.x; - report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+3)) << 8) | (read_reg(ADDR_ACC_X_LSB+2));// XXX PX4DEV raw_report.y; - report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+5)) << 8) | (read_reg(ADDR_ACC_X_LSB+4));// XXX PX4DEV raw_report.z; + report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 1)) << 8) | (read_reg(ADDR_ACC_X_LSB)); // XXX PX4DEV raw_report.x; + report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 3)) << 8) | (read_reg(ADDR_ACC_X_LSB + 2)); // XXX PX4DEV raw_report.y; + report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 5)) << 8) | (read_reg(ADDR_ACC_X_LSB + 4)); // XXX PX4DEV raw_report.z; /* discard two non-value bits in the 16 bit measurement */ report->x_raw = (report->x_raw >> 2); @@ -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,16 +824,18 @@ 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); + err(1, "%s open failed (try 'bma180 start' if the driver is not running)", + ACCEL_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) err(1, "reset to manual polling"); - + /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); + if (sz != sizeof(a_report)) err(1, "immediate acc read failed"); @@ -831,7 +848,7 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); + (double)(a_report.range_m_s2 / 9.81f)); /* XXX add poll-rate tests here too */ @@ -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"); diff --git a/apps/px4/ground_estimator/Makefile b/apps/drivers/boards/px4fmu/Makefile index b44d871c6..393e96e32 100644 --- a/apps/px4/ground_estimator/Makefile +++ b/apps/drivers/boards/px4fmu/Makefile @@ -32,11 +32,9 @@ ############################################################################ # -# Basic example application +# Board-specific startup code for the PX4FMU # -APPNAME = ground_estimator -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common include $(APPDIR)/mk/app.mk diff --git a/nuttx/configs/px4fmu/src/up_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c index 2d74e6f00..987894163 100644 --- a/nuttx/configs/px4fmu/src/up_adc.c +++ b/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. - * Author: Gregory Nutt <gnutt@nuttx.org> + * 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 @@ -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" - -#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 +#include "px4fmu_internal.h" #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++) - { - stm32_configgpio(g_pinlist[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 */ diff --git a/nuttx/configs/px4fmu/src/up_can.c b/apps/drivers/boards/px4fmu/px4fmu_can.c index daf6484f2..0d0b5fcd3 100644 --- a/nuttx/configs/px4fmu/src/up_can.c +++ b/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. - * Author: Gregory Nutt <gnutt@nuttx.org> + * 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 @@ -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" - -#if defined(CONFIG_CAN) && (defined(CONFIG_STM32_CAN1) || defined(CONFIG_STM32_CAN2)) +#include "px4fmu_internal.h" /************************************************************************************ * Pre-processor Definitions @@ -105,38 +107,35 @@ int can_devinit(void) { - static bool initialized = false; - struct can_dev_s *can; - int ret; + static bool initialized = false; + struct can_dev_s *can; + int ret; - /* Check if we have already initialized */ + /* Check if we have already initialized */ - if (!initialized) - { - /* Call stm32_caninitialize() to get an instance of the CAN interface */ + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ - can = stm32_caninitialize(CAN_PORT); - if (can == NULL) - { - candbg("ERROR: Failed to get CAN interface\n"); - return -ENODEV; - } + can = stm32_caninitialize(CAN_PORT); - /* Register the CAN driver at "/dev/can0" */ + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } - ret = can_register("/dev/can0", can); - if (ret < 0) - { - candbg("ERROR: can_register failed: %d\n", ret); - return ret; - } + /* Register the CAN driver at "/dev/can0" */ - /* Now we are initialized */ + ret = can_register("/dev/can0", can); - initialized = true; - } + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } - return OK; -} + /* Now we are initialized */ -#endif /* CONFIG_STM32_CAN || CONFIG_STM32_CAN2 || CONFIG_STM32_CAN3 */ + initialized = true; + } + + return OK; +} diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c new file mode 100644 index 000000000..e5ded7328 --- /dev/null +++ b/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; +} diff --git a/apps/drivers/boards/px4fmu/px4fmu_internal.h b/apps/drivers/boards/px4fmu/px4fmu_internal.h new file mode 100644 index 000000000..c58a8a5c4 --- /dev/null +++ b/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__ */ diff --git a/nuttx/configs/px4fmu/src/up_boot.c b/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c index da396cdd6..d1ff8c112 100644 --- a/nuttx/configs/px4fmu/src/up_boot.c +++ b/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" - -/************************************************************************************ - * Definitions - ************************************************************************************/ - -/************************************************************************************ - * 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. - * - ************************************************************************************/ +#include "stm32_internal.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" -void stm32_boardinitialize(void) -{ - /* configure SPI interfaces */ - stm32_spiinitialize(); +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + } +}; - /* configure LEDs */ - 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, + } +}; diff --git a/nuttx/configs/px4fmu/src/up_spi.c b/apps/drivers/boards/px4fmu/px4fmu_spi.c index ea34c30ce..70245a3ec 100644 --- a/nuttx/configs/px4fmu/src/up_spi.c +++ b/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. - * Author: Gregory Nutt <gnutt@nuttx.org> + * 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 @@ -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" - -#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 - ************************************************************************************/ +#include "px4fmu_internal.h" /************************************************************************************ * 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); } -/**************************************************************************** - * 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) +__EXPORT 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) -{ - spidbg("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert"); +__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ /* 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 */ diff --git a/nuttx/configs/px4fmu/src/up_usbdev.c b/apps/drivers/boards/px4fmu/px4fmu_usb.c index 4ef105e91..b0b669fbe 100644 --- a/nuttx/configs/px4fmu/src/up_usbdev.c +++ b/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. - * Author: Gregory Nutt <gnutt@nuttx.org> + * 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 @@ -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,22 +71,22 @@ * 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 */ + /* The OTG FS has an internal soft pull-up */ - /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ #ifdef CONFIG_STM32_OTGFS - stm32_configgpio(GPIO_OTGFS_VBUS); - /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); - */ + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ #endif } @@ -98,8 +101,8 @@ 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); + ulldbg("resume: %d\n", resume); } diff --git a/apps/drivers/device/device.h b/apps/drivers/device/device.h index 9af678465..01692c315 100644 --- a/apps/drivers/device/device.h +++ b/apps/drivers/device/device.h @@ -54,7 +54,7 @@ /** * Namespace encapsulating all device framework classes, functions and data. */ -namespace device __EXPORT +namespace device __EXPORT { /** @@ -276,14 +276,14 @@ public: */ virtual int poll(struct file *filp, struct pollfd *fds, bool setup); - /** - * Test whether the device is currently open. - * - * This can be used to avoid tearing down a device that is still active. - * - * @return True if the device is currently open. - */ - bool is_open() { return _open_count > 0; } + /** + * Test whether the device is currently open. + * + * This can be used to avoid tearing down a device that is still active. + * + * @return True if the device is currently open. + */ + bool is_open() { return _open_count > 0; } protected: /** diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index c4b2aa944..4b832b548 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -121,7 +121,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len unsigned tries = 0; do { - // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); msgs = 0; @@ -144,7 +144,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len if (msgs == 0) return -EINVAL; - /* + /* * I2C architecture means there is an unavoidable race here * if there are any devices on the bus with a different frequency * preference. Really, this is pointless. @@ -154,7 +154,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len if (ret == OK) break; - + // reset the I2C bus to unwedge on error up_i2creset(_dev); diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 7c5a14d6b..eb1b6cb05 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -42,7 +42,7 @@ #include <nuttx/i2c.h> -namespace device __EXPORT +namespace device __EXPORT { /** diff --git a/apps/drivers/device/spi.cpp b/apps/drivers/device/spi.cpp index a1761b769..528333e04 100644 --- a/apps/drivers/device/spi.cpp +++ b/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); diff --git a/apps/drivers/device/spi.h b/apps/drivers/device/spi.h index b2a111562..e8c8e2c5e 100644 --- a/apps/drivers/device/spi.h +++ b/apps/drivers/device/spi.h @@ -84,7 +84,7 @@ protected: * If called from interrupt context, this interface does not lock * the bus and may interfere with non-interrupt-context callers. * - * Clients in a mixed interrupt/non-interrupt configuration must + * Clients in a mixed interrupt/non-interrupt configuration must * ensure appropriate interlocking. * * At least one of send or recv must be non-null. diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h index 6d0c8c545..3834795e0 100644 --- a/apps/drivers/drv_accel.h +++ b/apps/drivers/drv_accel.h @@ -83,7 +83,7 @@ ORB_DECLARE(sensor_accel); /* * ioctl() definitions * - * Accelerometer drivers also implement the generic sensor driver + * Accelerometer drivers also implement the generic sensor driver * interfaces from drv_sensor.h */ diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h index 5c646f243..0dbf05c5b 100644 --- a/apps/drivers/drv_gyro.h +++ b/apps/drivers/drv_gyro.h @@ -58,7 +58,7 @@ struct gyro_report { float temperature; /**< temperature in degrees celcius */ float range_rad_s; float scaling; - + int16_t x_raw; int16_t y_raw; int16_t z_raw; diff --git a/nuttx/configs/px4fmu/include/up_hrt.h b/apps/drivers/drv_hrt.h index 6fd66ad74..3b493a81a 100644 --- a/nuttx/configs/px4fmu/include/up_hrt.h +++ b/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_ -#define UP_HRT_H_ +#pragma once #include <sys/types.h> #include <stdbool.h> @@ -44,6 +45,8 @@ #include <time.h> #include <queue.h> +__BEGIN_DECLS + /* * Absolute time, in microsecond units. * @@ -59,7 +62,7 @@ typedef uint64_t hrt_abstime; * they are serialised with respect to each other, and must not * block. */ -typedef void (* hrt_callout)(void *arg); +typedef void (* hrt_callout)(void *arg); /* * Callout record. @@ -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,24 +110,24 @@ 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, + * If this returns true, the entry has been invoked and removed from the callout list, * or it has never been entered. * * 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 diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 7ba9dd695..114bcb646 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -59,7 +59,7 @@ struct mag_report { float z; float range_ga; float scaling; - + int16_t x_raw; int16_t y_raw; int16_t z_raw; diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 340175a4b..97033f2cd 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -41,14 +41,15 @@ * channel. */ -#ifndef _DRV_PWM_OUTPUT_H -#define _DRV_PWM_OUTPUT_H +#pragma once #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 diff --git a/apps/drivers/drv_sensor.h b/apps/drivers/drv_sensor.h index 325bd42bf..3a89cab9d 100644 --- a/apps/drivers/drv_sensor.h +++ b/apps/drivers/drv_sensor.h @@ -52,7 +52,7 @@ #define _SENSORIOC(_n) (_IOC(_SENSORIOCBASE, _n)) /** - * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE + * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE * constants */ #define SENSORIOCSPOLLRATE _SENSORIOC(0) @@ -68,8 +68,8 @@ #define SENSOR_POLLRATE_MAX 1000002 /**< poll at device maximum rate */ #define SENSOR_POLLRATE_DEFAULT 1000003 /**< poll at driver normal rate */ -/** - * Set the internal queue depth to (arg) entries, must be at least 1 +/** + * Set the internal queue depth to (arg) entries, must be at least 1 * * This sets the upper bound on the number of readings that can be * read from the driver. diff --git a/apps/drivers/drv_tone_alarm.h b/apps/drivers/drv_tone_alarm.h index 27b146de9..0c6afc64c 100644 --- a/apps/drivers/drv_tone_alarm.h +++ b/apps/drivers/drv_tone_alarm.h @@ -34,8 +34,8 @@ /* * Driver for the PX4 audio alarm port, /dev/tone_alarm. * - * The tone_alarm driver supports a set of predefined "alarm" - * patterns and one user-supplied pattern. Patterns are ordered by + * The tone_alarm driver supports a set of predefined "alarm" + * patterns and one user-supplied pattern. Patterns are ordered by * priority, with a higher-priority pattern interrupting any * lower-priority pattern that might be playing. * diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 8e78825c3..7943012cc 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/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: { @@ -662,7 +679,7 @@ HMC5883::cycle() if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) { /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, + work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, @@ -680,7 +697,7 @@ HMC5883::cycle() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, + work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, @@ -846,7 +863,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) warnx("starting mag scale calibration"); /* do a simple demand read */ - sz = read(filp, (char*)&report, sizeof(report)); + 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; @@ -942,7 +961,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) scaling[2] = fabsf(1.08f / avg_excited[2]); warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - + /* set back to normal mode */ /* Set to 1.1 Gauss */ if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { @@ -967,12 +986,15 @@ 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."); - } +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"); @@ -1163,7 +1198,7 @@ test() * Basic idea: * * output = (ext field +- 1.1 Ga self-test) * scale factor - * + * * and consequently: * * 1.1 Ga = (excited - normal) * scale factor @@ -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"); } diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index e9b60b01c..4915b81e3 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/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; @@ -330,7 +331,7 @@ L3GD20::init() write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); - + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ @@ -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: { @@ -478,7 +480,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGQUEUEDEPTH: - return _num_reports -1; + return _num_reports - 1; case SENSORIOCRESET: /* XXX implement */ @@ -497,12 +499,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -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,15 +787,17 @@ test() /* get the driver */ fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + if (fd_gyro < 0) err(1, "%s open failed", GYRO_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) err(1, "reset to manual polling"); - + /* 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"); @@ -793,7 +808,7 @@ test() warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f)); + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); /* XXX add poll-rate tests here too */ @@ -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"); diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 7b8a84f7e..90786886a 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/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: @@ -592,12 +593,12 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: /* copy scale in */ - memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale)); + memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); return OK; case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -639,12 +640,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -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,21 +1011,24 @@ 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); + 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); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) err(1, "reset to manual polling"); - + /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); + if (sz != sizeof(a_report)) err(1, "immediate acc read failed"); @@ -1033,10 +1041,11 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); + (double)(a_report.range_m_s2 / 9.81f)); /* 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"); @@ -1047,7 +1056,7 @@ test() warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f)); + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); @@ -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"); diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 4df9de94c..699cd36d2 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/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,9 +325,10 @@ int MS5611::probe() { _retries = 10; - if((OK == probe_address(MS5611_ADDRESS_1)) || - (OK == probe_address(MS5611_ADDRESS_2))) { - _retries = 1; + + if ((OK == probe_address(MS5611_ADDRESS_1)) || + (OK == probe_address(MS5611_ADDRESS_2))) { + _retries = 1; return OK; } @@ -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; @@ -684,7 +694,7 @@ MS5611::collect() int64_t SENS2 = 5 * f >> 2; if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); + int64_t f2 = POW2(_TEMP + 1500); OFF2 += 7 * f2; SENS2 += 11 * f2 >> 1; } @@ -693,6 +703,7 @@ MS5611::collect() _OFF -= OFF2; _SENS -= SENS2; } + } else { /* pressure calculation, result in Pa */ @@ -810,8 +821,8 @@ MS5611::read_prom() uint16_t w; } cvt; - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are * called immediately after reset. */ usleep(3000); @@ -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); } @@ -1146,7 +1172,7 @@ ms5611_main(int argc, char *argv[]) errx(1, "missing altitude"); long altitude = strtol(argv[2], nullptr, 10); - + ms5611::calibrate(altitude); } diff --git a/apps/px4/px4io/driver/Makefile b/apps/drivers/px4io/Makefile index cbd942546..cbd942546 100644 --- a/apps/px4/px4io/driver/Makefile +++ b/apps/drivers/px4io/Makefile diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 66db1c360..995c9393f 100644 --- a/apps/px4/px4io/driver/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -110,7 +110,7 @@ private: bool _primary_pwm_device; ///< true if we are the default PWM output volatile bool _switch_armed; ///< PX4IO switch armed state - // XXX how should this work? + // XXX how should this work? bool _send_needed; ///< If true, we need to send a packet to IO @@ -149,13 +149,13 @@ private: * group/index during mixing. */ static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); }; -namespace +namespace { PX4IO *g_dev; @@ -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); @@ -364,9 +370,9 @@ PX4IO::task_main() int PX4IO::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -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; } diff --git a/apps/px4/px4io/driver/uploader.cpp b/apps/drivers/px4io/uploader.cpp index 0fbbac839..5669aeb01 100644 --- a/apps/px4/px4io/driver/uploader.cpp +++ b/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,17 +89,20 @@ 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; /* do the usual program thing - allow for failure */ for (unsigned retries = 0; retries < 1; retries++) { if (retries > 0) { - log("retrying update..."); + 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; } @@ -175,7 +191,7 @@ PX4IO_Uploader::drain() do { ret = recv(c, 10); //log("discard 0x%02x", c); - } while(ret == OK); + } while (ret == OK); } int @@ -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; diff --git a/apps/px4/px4io/driver/uploader.h b/apps/drivers/px4io/uploader.h index 8d41992f8..8d41992f8 100644 --- a/apps/px4/px4io/driver/uploader.h +++ b/apps/drivers/px4io/uploader.h diff --git a/apps/px4/attitude_estimator_bm/Makefile b/apps/drivers/stm32/Makefile index 2c1cfc510..4ad57f413 100644 --- a/apps/px4/attitude_estimator_bm/Makefile +++ b/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 -PRIORITY = SCHED_PRIORITY_MAX - 10 -STACKSIZE = 12000 +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common include $(APPDIR)/mk/app.mk diff --git a/nuttx/configs/px4fmu/src/up_hrt.c b/apps/drivers/stm32/drv_hrt.c index c8ff1ce38..1ac90b16d 100644 --- a/nuttx/configs/px4fmu/src/up_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -30,16 +30,18 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - + /** - * @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. * * Note that really, this could use systick too, but that's * monopolised by NuttX and stealing it would just be awkward. * - * We don't use the NuttX STM32 driver per se; rather, we + * We don't use the NuttX STM32 driver per se; rather, we * claim the timer and then drive it directly. */ @@ -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" @@ -262,10 +264,10 @@ static void hrt_latency_update(void); /* callout list manipulation */ static void hrt_call_internal(struct hrt_call *entry, - hrt_abstime deadline, - hrt_abstime interval, - hrt_callout callout, - void *arg); + hrt_abstime deadline, + hrt_abstime interval, + hrt_callout callout, + void *arg); static void hrt_call_enter(struct hrt_call *entry); static void hrt_call_reschedule(void); static void hrt_call_invoke(void); @@ -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]; -unsigned ppm_decoded_channels; -uint64_t ppm_last_valid_decode = 0; +__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT unsigned ppm_decoded_channels; +__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]; @@ -370,39 +372,39 @@ static void hrt_tim_init(void) { /* clock/power on our timer */ - modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); + modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); - /* claim our interrupt vector */ - irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); + /* claim our interrupt vector */ + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); - /* disable and configure the timer */ - rCR1 = 0; - rCR2 = 0; - rSMCR = 0; - rDIER = DIER_HRT | DIER_PPM; - rCCER = 0; /* unlock CCMR* registers */ - rCCMR1 = CCMR1_PPM; - rCCMR2 = CCMR2_PPM; - rCCER = CCER_PPM; - rDCR = 0; + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_HRT | DIER_PPM; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PPM; + rCCMR2 = CCMR2_PPM; + rCCER = CCER_PPM; + rDCR = 0; - /* configure the timer to free-run at 1MHz */ - rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ + /* configure the timer to free-run at 1MHz */ + rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ - /* run the full span of the counter */ - rARR = 0xffff; + /* run the full span of the counter */ + rARR = 0xffff; - /* set an initial capture a little ways off */ - rCCR_HRT = 1000; + /* set an initial capture a little ways off */ + rCCR_HRT = 1000; - /* generate an update event; reloads the counter, all registers */ - rEGR = GTIM_EGR_UG; + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; - /* enable the timer */ - rCR1 = GTIM_CR1_CEN; + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; - /* enable interrupts */ - up_enable_irq(HRT_TIMER_VECTOR); + /* enable interrupts */ + up_enable_irq(HRT_TIMER_VECTOR); } #ifdef CONFIG_HRT_PPM @@ -410,7 +412,7 @@ hrt_tim_init(void) * Handle the PPM decoder state machine. */ static void -hrt_ppm_decode(uint32_t status) +hrt_ppm_decode(uint32_t status) { uint16_t count = rCCR_PPM; uint16_t width; @@ -426,10 +428,11 @@ 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; - /* + /* * if this looks like a start pulse, then push the last set of values * and reset the state machine */ @@ -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,10 +463,11 @@ 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 */ - + /* record the mark timing, expect an inactive edge */ ppm.last_mark = count; ppm.phase = INACTIVE; @@ -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; @@ -491,7 +497,7 @@ hrt_ppm_decode(uint32_t status) ppm_temp_buffer[ppm.next_channel++] = interval; ppm.phase = INACTIVE; - return; + return; } @@ -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? */ @@ -638,7 +646,7 @@ hrt_init(void) void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) { - hrt_call_internal(entry, + hrt_call_internal(entry, hrt_absolute_time() + delay, 0, callout, @@ -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; @@ -762,7 +774,7 @@ hrt_call_invoke(void) //lldbg("call pop\n"); /* save the intended deadline for periodic calls */ - deadline = call->deadline; + deadline = call->deadline; /* zero the deadline, as the call has occurred */ call->deadline = 0; @@ -802,7 +814,7 @@ hrt_call_reschedule() * we want. * * It is important for accurate timekeeping that the compare - * interrupt fires sufficiently often that the base_time update in + * interrupt fires sufficiently often that the base_time update in * hrt_absolute_time runs at least once per timer period. */ if (next != NULL) { @@ -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]++; } diff --git a/nuttx/configs/px4fmu/src/up_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index d278da55c..e3801a417 100644 --- a/nuttx/configs/px4fmu/src/up_pwm_servo.c +++ b/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) @@ -195,26 +142,29 @@ pwm_channel_init(unsigned channel) /* configure the channel */ switch (pwm_channels[channel].timer_channel) { - case 1: - rCCMR1(timer) |= (6 << 4); - 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; - rCCER(timer) |= (1 << 12); - break; + case 1: + rCCMR1(timer) |= (6 << 4); + 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; + rCCER(timer) |= (1 << 12); + break; } } @@ -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; + 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; } @@ -273,19 +229,23 @@ up_pwm_servo_get(unsigned channel) /* configure the channel */ switch (pwm_channels[channel].timer_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; + 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; } @@ -301,9 +261,10 @@ up_pwm_servo_init(uint32_t channel_mask) /* now init channels */ for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { /* don't do init for disabled channels; this leaves the pin configs alone */ - if (((1<<i) & channel_mask) && (pwm_channels[i].gpio != 0)) + if (((1 << i) & channel_mask) && (pwm_channels[i].gpio != 0)) pwm_channel_init(i); } + return OK; } @@ -324,17 +285,18 @@ up_pwm_servo_set_rate(unsigned rate) if (pwm_timers[i].base != 0) pwm_timer_set_rate(i, rate); } + return OK; } void up_pwm_servo_arm(bool armed) { - /* + /* * XXX this is inelgant and in particular will either jam outputs at whatever level * they happen to be at at the time the timers stop or generate runts. - * The right thing is almost certainly to kill auto-reload on the timers so that - * they just stop at the end of their count for disable, and to reset/restart them + * The right thing is almost certainly to kill auto-reload on the timers so that + * they just stop at the end of their count for disable, and to reset/restart them * for enable. */ diff --git a/nuttx/configs/px4fmu/include/up_adc.h b/apps/drivers/stm32/drv_pwm_servo.h index 699c6a59a..667283424 100644 --- a/nuttx/configs/px4fmu/include/up_adc.h +++ b/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. - * Author: Gregory Nutt <gnutt@nuttx.org> + * 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 @@ -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 + * + * stm32-specific PWM output data. + */ -#include <nuttx/config.h> +#pragma once -#ifdef CONFIG_ADC +#include <drivers/drv_pwm_output.h> -/************************************************************************************ - * Public Functions - ************************************************************************************/ +/* configuration limits */ +#define PWM_SERVO_MAX_TIMERS 2 +#define PWM_SERVO_MAX_CHANNELS 8 -/************************************************************************************ - * Name: adc_devinit - * - * Description: - * All STM32 architectures must provide the following interface to work with - * examples/adc. - * - ************************************************************************************/ +/* 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]; -int adc_devinit(void); +/* 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; +}; -#endif /* CONFIG_ADC */ +__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS]; diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index bfde83cde..03cf3ee5d 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -34,8 +34,8 @@ /** * Driver for the PX4 audio alarm port, /dev/tone_alarm. * - * The tone_alarm driver supports a set of predefined "alarm" - * patterns and one user-supplied pattern. Patterns are ordered by + * The tone_alarm driver supports a set of predefined "alarm" + * patterns and one user-supplied pattern. Patterns are ordered by * priority, with a higher-priority pattern interrupting any * lower-priority pattern that might be playing. * @@ -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; @@ -368,34 +370,34 @@ ToneAlarm::init() stm32_configgpio(GPIO_TONE_ALARM); /* clock/power on our timer */ - modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); + modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); /* initialise the timer */ - rCR1 = 0; - rCR2 = 0; - rSMCR = 0; - rDIER = 0; - rCCER &= TONE_CCER; /* unlock CCMR* registers */ - rCCMR1 = TONE_CCMR1; - rCCMR2 = TONE_CCMR2; - rCCER = TONE_CCER; - rDCR = 0; - - /* toggle the CC output each time the count passes 1 */ - TONE_rCCR = 1; - - /* - * Configure the timebase to free-run at half max frequency. - * XXX this should be more flexible in order to get a better - * frequency range, but for the F4 with the APB1 timers based - * at 42MHz, this gets us down to ~320Hz or so. - */ - rPSC = 1; - - /* make sure the timer is running */ - rCR1 = GTIM_CR1_CEN; - - debug("ready"); + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = 0; + rCCER &= TONE_CCER; /* unlock CCMR* registers */ + rCCMR1 = TONE_CCMR1; + rCCMR2 = TONE_CCMR2; + rCCER = TONE_CCER; + rDCR = 0; + + /* toggle the CC output each time the count passes 1 */ + TONE_rCCR = 1; + + /* + * Configure the timebase to free-run at half max frequency. + * XXX this should be more flexible in order to get a better + * frequency range, but for the F4 with the APB1 timers based + * at 42MHz, this gets us down to ~320Hz or so. + */ + rPSC = 1; + + /* make sure the timer is running */ + rCR1 = GTIM_CR1_CEN; + + debug("ready"); return OK; } @@ -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; @@ -534,11 +547,11 @@ ToneAlarm::next(void) /* set reload based on the pitch */ rARR = _notes[np->pitch]; - /* force an update, reloads the counter and all registers */ - rEGR = GTIM_EGR_UG; + /* force an update, reloads the counter and all registers */ + rEGR = GTIM_EGR_UG; - /* enable the output */ - rCCER |= TONE_CCER; + /* enable the output */ + rCCER |= TONE_CCER; } /* arrange a callback when the note/rest is done */ @@ -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); diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 8629c2f11..e04033481 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/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> diff --git a/apps/gps/gps.c b/apps/gps/gps.c index b26b09f95..00f6ee9f8 100644 --- a/apps/gps/gps.c +++ b/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); } } diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c index 2e90f50b1..374280d28 100644 --- a/apps/gps/mtk.c +++ b/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> diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c index d1c9d364b..577a3a01c 100644 --- a/apps/gps/nmea_helper.c +++ b/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 diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index b1655afd7..f676ffdfb 100644 --- a/apps/gps/ubx.c +++ b/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_verbose) printf("[gps] Connection attempt failed, no UBX module found\r\n"); + 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\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? diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h index 8e98cca5f..f4f01df9a 100644 --- a/apps/gps/ubx.h +++ b/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; diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 698e43f96..7d8232b3a 100644 --- a/apps/mavlink/mavlink.c +++ b/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); } diff --git a/apps/mavlink/mavlink_bridge_header.h b/apps/mavlink/mavlink_bridge_header.h index de8bc4ae7..bf7c5354c 100644 --- a/apps/mavlink/mavlink_bridge_header.h +++ b/apps/mavlink/mavlink_bridge_header.h @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * 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 @@ -35,6 +35,8 @@ /** * @file mavlink_bridge_header * MAVLink bridge header for UART access. + * + * @author Lorenz Meier <lm@inf.ethz.ch> */ /* MAVLink adapter header */ diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index 8874fe528..f41889535 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/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 */ diff --git a/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h index 37fd44286..146e9e15c 100644 --- a/apps/mavlink/mavlink_parameters.h +++ b/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> diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 550746794..cfff0f469 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/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> diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c index d2be9a88d..50282a9e3 100644 --- a/apps/mavlink/missionlib.c +++ b/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> diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 0b073cc65..90b0073cf 100644 --- a/apps/mavlink/orb_listener.c +++ b/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> diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index dfb778fd0..0cdb53554 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -54,7 +54,7 @@ #include <math.h> #include <poll.h> #include <sys/prctl.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <drivers/drv_gyro.h> #include <uORB/topics/vehicle_status.h> diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 8c0e10fc3..eb94cca8a 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -52,7 +52,7 @@ #include <math.h> #include <systemlib/pid/pid.h> #include <systemlib/param/param.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 42aa0ac63..a5bff97e6 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -53,7 +53,7 @@ #include <systemlib/pid/pid.h> #include <systemlib/param/param.h> #include <systemlib/err.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 7cf687306..474ced731 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -48,7 +48,7 @@ #include <termios.h> #include <time.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/vehicle_attitude.h> diff --git a/apps/position_estimator/Makefile b/apps/position_estimator/Makefile index 3502cc402..cc5072152 100644 --- a/apps/position_estimator/Makefile +++ b/apps/position_estimator/Makefile @@ -39,12 +39,6 @@ APPNAME = position_estimator PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 -CSRCS = position_estimator_main.c \ - codegen/position_estimator.c \ - codegen/position_estimator_initialize.c \ - codegen/position_estimator_terminate.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c +CSRCS = position_estimator_main.c include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator/codegen/position_estimator.c b/apps/position_estimator/codegen/position_estimator.c deleted file mode 100644 index 731ae03e3..000000000 --- a/apps/position_estimator/codegen/position_estimator.c +++ /dev/null @@ -1,261 +0,0 @@ -/* - * position_estimator.c - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "position_estimator.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T - xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T - predict_only, real32_T xapo1[6], real32_T Papo1[36]) -{ - real32_T fv0[6]; - real32_T fv1[6]; - real32_T I[36]; - real32_T xapri[6]; - int32_T i; - int32_T r1; - static const real32_T fv2[36] = { 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.004F, - 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.004F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.004F, 1.0F }; - - static const real32_T fv3[12] = { 0.0F, 0.0F, 0.1744F, 87.2F, 0.0F, 0.0F, - -0.1744F, -87.2F, 0.0F, 0.0F, 0.0F, 0.0F }; - - int32_T r2; - real32_T Papri[36]; - real32_T maxval; - static const real32_T fv4[36] = { 1.0F, 0.004F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F }; - - static const real32_T fv5[36] = { 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F }; - - real32_T K[18]; - static const int8_T iv0[18] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0 }; - - real32_T fv6[9]; - static const int8_T iv1[18] = { 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 1, 0 }; - - real32_T b_gps_covariance[9]; - real32_T A[9]; - real32_T B[18]; - int32_T r3; - real32_T a21; - real32_T Y[18]; - real32_T b_z[3]; - int8_T b_I[36]; - - /* if predit_onli == 1: no update step: use this when no new gps data is available */ - /* %initialization */ - /* use model F=m*a x''=F/m */ - /* 250Hz---> dT = 0.004s */ - /* u=[phi;theta] */ - /* x=[px;vx;py;vy]; */ - /* %------------------------------------------ */ - /* %------------------------------------------------ */ - /* R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1]; */ - /* process Covariance Matrix */ - /* measurement Covariance Matrix */ - /* %prediction */ - for (i = 0; i < 6; i++) { - fv0[i] = 0.0F; - for (r1 = 0; r1 < 6; r1++) { - fv0[i] += fv2[i + 6 * r1] * xapo[r1]; - } - - fv1[i] = 0.0F; - for (r1 = 0; r1 < 2; r1++) { - fv1[i] += fv3[i + 6 * r1] * u[r1]; - } - - xapri[i] = fv0[i] + fv1[i]; - for (r1 = 0; r1 < 6; r1++) { - I[i + 6 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - I[i + 6 * r1] += fv2[i + 6 * r2] * Papo[r2 + 6 * r1]; - } - } - } - - for (i = 0; i < 6; i++) { - for (r1 = 0; r1 < 6; r1++) { - maxval = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - maxval += I[i + 6 * r2] * fv4[r2 + 6 * r1]; - } - - Papri[i + 6 * r1] = maxval + fv5[i + 6 * r1]; - } - } - - if (1 != predict_only) { - /* update */ - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 6; r1++) { - K[i + 3 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - K[i + 3 * r1] += (real32_T)iv0[i + 3 * r2] * Papri[r2 + 6 * r1]; - } - } - } - - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 3; r1++) { - fv6[i + 3 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - fv6[i + 3 * r1] += K[r1 + 3 * r2] * (real32_T)iv1[r2 + 6 * i]; - } - } - } - - b_gps_covariance[0] = gps_covariance[0]; - b_gps_covariance[1] = 0.0F; - b_gps_covariance[2] = 0.0F; - b_gps_covariance[3] = 0.0F; - b_gps_covariance[4] = gps_covariance[1]; - b_gps_covariance[5] = 0.0F; - b_gps_covariance[6] = 0.0F; - b_gps_covariance[7] = 0.0F; - b_gps_covariance[8] = gps_covariance[2]; - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 3; r1++) { - A[r1 + 3 * i] = fv6[r1 + 3 * i] + b_gps_covariance[r1 + 3 * i]; - } - - for (r1 = 0; r1 < 6; r1++) { - B[i + 3 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - B[i + 3 * r1] += Papri[r1 + 6 * r2] * (real32_T)iv1[r2 + 6 * i]; - } - } - } - - r1 = 0; - r2 = 1; - r3 = 2; - maxval = (real32_T)fabs(A[0]); - a21 = (real32_T)fabs(A[1]); - if (a21 > maxval) { - maxval = a21; - r1 = 1; - r2 = 0; - } - - if ((real32_T)fabs(A[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } - - A[r2] /= A[r1]; - A[r3] /= A[r1]; - A[3 + r2] -= A[r2] * A[3 + r1]; - A[3 + r3] -= A[r3] * A[3 + r1]; - A[6 + r2] -= A[r2] * A[6 + r1]; - A[6 + r3] -= A[r3] * A[6 + r1]; - if ((real32_T)fabs(A[3 + r3]) > (real32_T)fabs(A[3 + r2])) { - i = r2; - r2 = r3; - r3 = i; - } - - A[3 + r3] /= A[3 + r2]; - A[6 + r3] -= A[3 + r3] * A[6 + r2]; - for (i = 0; i < 6; i++) { - Y[3 * i] = B[r1 + 3 * i]; - Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * A[r2]; - Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * A[r3]) - Y[1 + 3 * i] * A[3 + - r3]; - Y[2 + 3 * i] /= A[6 + r3]; - Y[3 * i] -= Y[2 + 3 * i] * A[6 + r1]; - Y[1 + 3 * i] -= Y[2 + 3 * i] * A[6 + r2]; - Y[1 + 3 * i] /= A[3 + r2]; - Y[3 * i] -= Y[1 + 3 * i] * A[3 + r1]; - Y[3 * i] /= A[r1]; - } - - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 6; r1++) { - K[r1 + 6 * i] = Y[i + 3 * r1]; - } - } - - for (i = 0; i < 3; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 6; r1++) { - maxval += (real32_T)iv0[i + 3 * r1] * xapri[r1]; - } - - b_z[i] = z[i] - maxval; - } - - for (i = 0; i < 6; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 3; r1++) { - maxval += K[i + 6 * r1] * b_z[r1]; - } - - xapo1[i] = xapri[i] + maxval; - } - - for (i = 0; i < 36; i++) { - b_I[i] = 0; - } - - for (i = 0; i < 6; i++) { - b_I[i + 6 * i] = 1; - } - - for (i = 0; i < 6; i++) { - for (r1 = 0; r1 < 6; r1++) { - maxval = 0.0F; - for (r2 = 0; r2 < 3; r2++) { - maxval += K[i + 6 * r2] * (real32_T)iv0[r2 + 3 * r1]; - } - - I[i + 6 * r1] = (real32_T)b_I[i + 6 * r1] - maxval; - } - } - - for (i = 0; i < 6; i++) { - for (r1 = 0; r1 < 6; r1++) { - Papo1[i + 6 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - Papo1[i + 6 * r1] += I[i + 6 * r2] * Papri[r2 + 6 * r1]; - } - } - } - } else { - memcpy((void *)&Papo1[0], (void *)&Papri[0], 36U * sizeof(real32_T)); - for (i = 0; i < 6; i++) { - xapo1[i] = xapri[i]; - } - } -} - -/* End of code generation (position_estimator.c) */ diff --git a/apps/position_estimator/codegen/position_estimator.h b/apps/position_estimator/codegen/position_estimator.h deleted file mode 100644 index 5e2c9acd8..000000000 --- a/apps/position_estimator/codegen/position_estimator.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * position_estimator.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __POSITION_ESTIMATOR_H__ -#define __POSITION_ESTIMATOR_H__ -/* Include files */ -#include <math.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> - -#include "rtwtypes.h" -#include "position_estimator_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T predict_only, real32_T xapo1[6], real32_T Papo1[36]); -#endif -/* End of code generation (position_estimator.h) */ diff --git a/apps/position_estimator/codegen/position_estimator_initialize.c b/apps/position_estimator/codegen/position_estimator_initialize.c deleted file mode 100644 index 862381d3b..000000000 --- a/apps/position_estimator/codegen/position_estimator_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * position_estimator_initialize.c - * - * Code generation for function 'position_estimator_initialize' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "position_estimator.h" -#include "position_estimator_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void position_estimator_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (position_estimator_initialize.c) */ diff --git a/apps/position_estimator/codegen/position_estimator_initialize.h b/apps/position_estimator/codegen/position_estimator_initialize.h deleted file mode 100644 index 71013b390..000000000 --- a/apps/position_estimator/codegen/position_estimator_initialize.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * position_estimator_initialize.h - * - * Code generation for function 'position_estimator_initialize' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __POSITION_ESTIMATOR_INITIALIZE_H__ -#define __POSITION_ESTIMATOR_INITIALIZE_H__ -/* Include files */ -#include <math.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> - -#include "rtwtypes.h" -#include "position_estimator_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void position_estimator_initialize(void); -#endif -/* End of code generation (position_estimator_initialize.h) */ diff --git a/apps/position_estimator/codegen/position_estimator_terminate.c b/apps/position_estimator/codegen/position_estimator_terminate.c deleted file mode 100644 index b25aabf40..000000000 --- a/apps/position_estimator/codegen/position_estimator_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * position_estimator_terminate.c - * - * Code generation for function 'position_estimator_terminate' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "position_estimator.h" -#include "position_estimator_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void position_estimator_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (position_estimator_terminate.c) */ diff --git a/apps/position_estimator/codegen/position_estimator_terminate.h b/apps/position_estimator/codegen/position_estimator_terminate.h deleted file mode 100644 index d9fcf838b..000000000 --- a/apps/position_estimator/codegen/position_estimator_terminate.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * position_estimator_terminate.h - * - * Code generation for function 'position_estimator_terminate' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __POSITION_ESTIMATOR_TERMINATE_H__ -#define __POSITION_ESTIMATOR_TERMINATE_H__ -/* Include files */ -#include <math.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> - -#include "rtwtypes.h" -#include "position_estimator_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void position_estimator_terminate(void); -#endif -/* End of code generation (position_estimator_terminate.h) */ diff --git a/apps/position_estimator/codegen/position_estimator_types.h b/apps/position_estimator/codegen/position_estimator_types.h deleted file mode 100644 index cb01c7426..000000000 --- a/apps/position_estimator/codegen/position_estimator_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * position_estimator_types.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __POSITION_ESTIMATOR_TYPES_H__ -#define __POSITION_ESTIMATOR_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (position_estimator_types.h) */ diff --git a/apps/position_estimator/codegen/rtGetInf.c b/apps/position_estimator/codegen/rtGetInf.c deleted file mode 100644 index 20a64117c..000000000 --- a/apps/position_estimator/codegen/rtGetInf.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * rtGetInf.c - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, Inf and MinusInf - */ -#include "rtGetInf.h" -#define NumBitsPerChar 8U - -/* Function: rtGetInf ================================================== - * Abstract: - * Initialize rtInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - } - } - - return inf; -} - -/* Function: rtGetInfF ================================================== - * Abstract: - * Initialize rtInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetInfF(void) -{ - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; -} - -/* Function: rtGetMinusInf ================================================== - * Abstract: - * Initialize rtMinusInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetMinusInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - } - } - - return minf; -} - -/* Function: rtGetMinusInfF ================================================== - * Abstract: - * Initialize rtMinusInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetMinusInfF(void) -{ - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; -} - -/* End of code generation (rtGetInf.c) */ diff --git a/apps/position_estimator/codegen/rtGetInf.h b/apps/position_estimator/codegen/rtGetInf.h deleted file mode 100644 index c74f1fc28..000000000 --- a/apps/position_estimator/codegen/rtGetInf.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * rtGetInf.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __RTGETINF_H__ -#define __RTGETINF_H__ - -#include <stddef.h> -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetInf(void); -extern real32_T rtGetInfF(void); -extern real_T rtGetMinusInf(void); -extern real32_T rtGetMinusInfF(void); - -#endif -/* End of code generation (rtGetInf.h) */ diff --git a/apps/position_estimator/codegen/rtGetNaN.c b/apps/position_estimator/codegen/rtGetNaN.c deleted file mode 100644 index 6aa2f4639..000000000 --- a/apps/position_estimator/codegen/rtGetNaN.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * rtGetNaN.c - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, NaN - */ -#include "rtGetNaN.h" -#define NumBitsPerChar 8U - -/* Function: rtGetNaN ================================================== - * Abstract: - * Initialize rtNaN needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetNaN(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; - tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; - nan = tmpVal.fltVal; - break; - } - } - } - - return nan; -} - -/* Function: rtGetNaNF ================================================== - * Abstract: - * Initialize rtNaNF needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetNaNF(void) -{ - IEEESingle nanF = { { 0 } }; - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - nanF.wordL.wordLuint = 0xFFC00000U; - break; - } - - case BigEndian: - { - nanF.wordL.wordLuint = 0x7FFFFFFFU; - break; - } - } - - return nanF.wordL.wordLreal; -} - -/* End of code generation (rtGetNaN.c) */ diff --git a/apps/position_estimator/codegen/rtGetNaN.h b/apps/position_estimator/codegen/rtGetNaN.h deleted file mode 100644 index d3ec61c9e..000000000 --- a/apps/position_estimator/codegen/rtGetNaN.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * rtGetNaN.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __RTGETNAN_H__ -#define __RTGETNAN_H__ - -#include <stddef.h> -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetNaN(void); -extern real32_T rtGetNaNF(void); - -#endif -/* End of code generation (rtGetNaN.h) */ diff --git a/apps/position_estimator/codegen/rt_nonfinite.c b/apps/position_estimator/codegen/rt_nonfinite.c deleted file mode 100644 index 40e15fd22..000000000 --- a/apps/position_estimator/codegen/rt_nonfinite.c +++ /dev/null @@ -1,87 +0,0 @@ -/* - * rt_nonfinite.c - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finites, - * (Inf, NaN and -Inf). - */ -#include "rt_nonfinite.h" -#include "rtGetNaN.h" -#include "rtGetInf.h" - -real_T rtInf; -real_T rtMinusInf; -real_T rtNaN; -real32_T rtInfF; -real32_T rtMinusInfF; -real32_T rtNaNF; - -/* Function: rt_InitInfAndNaN ================================================== - * Abstract: - * Initialize the rtInf, rtMinusInf, and rtNaN needed by the - * generated code. NaN is initialized as non-signaling. Assumes IEEE. - */ -void rt_InitInfAndNaN(size_t realSize) -{ - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); -} - -/* Function: rtIsInf ================================================== - * Abstract: - * Test if value is infinite - */ -boolean_T rtIsInf(real_T value) -{ - return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); -} - -/* Function: rtIsInfF ================================================= - * Abstract: - * Test if single-precision value is infinite - */ -boolean_T rtIsInfF(real32_T value) -{ - return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); -} - -/* Function: rtIsNaN ================================================== - * Abstract: - * Test if value is not a number - */ -boolean_T rtIsNaN(real_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan(value)? TRUE:FALSE; -#else - return (value!=value)? 1U:0U; -#endif -} - -/* Function: rtIsNaNF ================================================= - * Abstract: - * Test if single-precision value is not a number - */ -boolean_T rtIsNaNF(real32_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan((real_T)value)? true:false; -#else - return (value!=value)? 1U:0U; -#endif -} - - -/* End of code generation (rt_nonfinite.c) */ diff --git a/apps/position_estimator/codegen/rt_nonfinite.h b/apps/position_estimator/codegen/rt_nonfinite.h deleted file mode 100644 index ac9660124..000000000 --- a/apps/position_estimator/codegen/rt_nonfinite.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * rt_nonfinite.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __RT_NONFINITE_H__ -#define __RT_NONFINITE_H__ - -#if defined(_MSC_VER) && (_MSC_VER <= 1200) -#include <float.h> -#endif -#include <stddef.h> -#include "rtwtypes.h" - -extern real_T rtInf; -extern real_T rtMinusInf; -extern real_T rtNaN; -extern real32_T rtInfF; -extern real32_T rtMinusInfF; -extern real32_T rtNaNF; -extern void rt_InitInfAndNaN(size_t realSize); -extern boolean_T rtIsInf(real_T value); -extern boolean_T rtIsInfF(real32_T value); -extern boolean_T rtIsNaN(real_T value); -extern boolean_T rtIsNaNF(real32_T value); - -typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; -} BigEndianIEEEDouble; - -typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; -} LittleEndianIEEEDouble; - -typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; -} IEEESingle; - -#endif -/* End of code generation (rt_nonfinite.h) */ diff --git a/apps/position_estimator/codegen/rtwtypes.h b/apps/position_estimator/codegen/rtwtypes.h deleted file mode 100644 index e87ae1fdc..000000000 --- a/apps/position_estimator/codegen/rtwtypes.h +++ /dev/null @@ -1,175 +0,0 @@ -/* - * rtwtypes.h - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -#ifndef __RTWTYPES_H__ -#define __RTWTYPES_H__ -#ifndef TRUE -# define TRUE (1U) -#endif -#ifndef FALSE -# define FALSE (0U) -#endif -#ifndef __TMWTYPES__ -#define __TMWTYPES__ - -#include <limits.h> - -/*=======================================================================* - * Target hardware information - * Device type: Generic->MATLAB Host Computer - * Number of bits: char: 8 short: 16 int: 32 - * long: 64 native word size: 64 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ - -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef long int64_T; -typedef unsigned long uint64_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * - * ulong_T, char_T and byte_T. * - *===========================================================================*/ - -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef char_T byte_T; - -/*===========================================================================* - * Complex number type definitions * - *===========================================================================*/ -#define CREAL_T - typedef struct { - real32_T re; - real32_T im; - } creal32_T; - - typedef struct { - real64_T re; - real64_T im; - } creal64_T; - - typedef struct { - real_T re; - real_T im; - } creal_T; - - typedef struct { - int8_T re; - int8_T im; - } cint8_T; - - typedef struct { - uint8_T re; - uint8_T im; - } cuint8_T; - - typedef struct { - int16_T re; - int16_T im; - } cint16_T; - - typedef struct { - uint16_T re; - uint16_T im; - } cuint16_T; - - typedef struct { - int32_T re; - int32_T im; - } cint32_T; - - typedef struct { - uint32_T re; - uint32_T im; - } cuint32_T; - - typedef struct { - int64_T re; - int64_T im; - } cint64_T; - - typedef struct { - uint64_T re; - uint64_T im; - } cuint64_T; - - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ - -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255)) -#define MIN_uint8_T ((uint8_T)(0)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535)) -#define MIN_uint16_T ((uint16_T)(0)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MIN_uint32_T ((uint32_T)(0)) -#define MAX_int64_T ((int64_T)(9223372036854775807L)) -#define MIN_int64_T ((int64_T)(-9223372036854775807L-1L)) -#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFUL)) -#define MIN_uint64_T ((uint64_T)(0UL)) - -/* Logical type definitions */ -#if !defined(__cplusplus) && !defined(__true_false_are_keywords) -# ifndef false -# define false (0U) -# endif -# ifndef true -# define true (1U) -# endif -#endif - -/* - * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation - * for signed integer values. - */ -#if ((SCHAR_MIN + 1) != -SCHAR_MAX) -#error "This code must be compiled using a 2's complement representation for signed integer values" -#endif - -/* - * Maximum length of a MATLAB identifier (function/variable) - * including the null-termination character. Referenced by - * rt_logging.c and rt_matrx.c. - */ -#define TMW_NAME_LENGTH_MAX 64 - -#endif -#endif -/* End of code generation (rtwtypes.h) */ diff --git a/apps/position_estimator/position_estimator.m b/apps/position_estimator/position_estimator.m deleted file mode 100644 index 2ef4d8b06..000000000 --- a/apps/position_estimator/position_estimator.m +++ /dev/null @@ -1,62 +0,0 @@ -function [xapo1,Papo1] = position_estimator(u,z,xapo,Papo,gps_covariance,predict_only) %if predit_onli == 1: no update step: use this when no new gps data is available -%#codegen -%%initialization -%use model F=m*a x''=F/m -% 250Hz---> dT = 0.004s -%u=[phi;theta] -%x=[px;vx;py;vy]; -%%------------------------------------------ -dT=0.004; -%%------------------------------------------------ - -%R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1]; - - -F=[ 1, 0.004, 0, 0, 0, 0; - 0, 1, 0, 0, 0, 0; - 0, 0, 1, 0.004, 0, 0; - 0, 0, 0, 1, 0, 0; - 0, 0, 0, 0, 1, 0.004; - 0, 0, 0, 0, 0, 1]; - -B=[ 0, -0.1744; - 0, -87.2; - 0.1744, 0; - 87.2, 0; - 0, 0; - 0, 0]; - -H=[1,0,0,0,0,0; - 0,0,1,0,0,0; - 0,0,0,0,1,0]; - - - - Q=[1e-007 ,0 ,0 ,0 ,0 ,0; - 0 ,1 ,0 ,0 ,0 ,0; - 0 ,0 ,1e-007 ,0 ,0 ,0; - 0 ,0 ,0 ,1 ,0 ,0 - 0 ,0 ,0 ,0 ,1e-007 ,0; - 0 ,0 ,0 ,0 ,0 ,1]; %process Covariance Matrix - - -R=[gps_covariance(1), 0, 0; - 0, gps_covariance(2), 0; - 0, 0, gps_covariance(3)]; %measurement Covariance Matrix - -%%prediction - -xapri=F*xapo+B*u; -Papri=F*Papo*F'+Q; - -if 1 ~= predict_only - %update - yr=z-H*xapri; - S=H*Papri*H'+R; - K=(Papri*H')/S; - xapo1=xapri+K*yr; - Papo1=(eye(6)-K*H)*Papri; -else - Papo1=Papri; - xapo1=xapri; -end
\ No newline at end of file diff --git a/apps/position_estimator/position_estimator.prj b/apps/position_estimator/position_estimator.prj deleted file mode 100644 index d754f36cc..000000000 --- a/apps/position_estimator/position_estimator.prj +++ /dev/null @@ -1,269 +0,0 @@ -<?xml version="1.0" encoding="UTF-8"?>
-<deployment-project>
- <configuration target="target.matlab.ecoder" target-name="MATLAB Embedded Coder Target" name="position_estimator" location="/home/thomasgubler/src/px4_nxbuilder/px4fmu/apps/position_estimator" file="/home/thomasgubler/src/px4_nxbuilder/px4fmu/apps/position_estimator/position_estimator.prj" build-checksum="2425236060">
- <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>false</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.Off</param.grt.CCompilerOptimization>
- <param.grt.CCompilerCustomOptimizations />
- <param.grt.general.GenerateMakefile>false</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.specified</param.grt.paths.build>
- <param.grt.paths.build.specified>./codegen</param.grt.paths.build.specified>
- <param.grt.paths.search />
- <param.grt.report.GenerateReport>false</param.grt.report.GenerateReport>
- <param.grt.report.LaunchReport>false</param.grt.report.LaunchReport>
- <param.grt.GenerateComments>true</param.grt.GenerateComments>
- <param.grt.MATLABSourceComments>false</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>C89/C90 (ANSI)</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>position_estimator_mex</param.mex.outputfile>
- <param.grt.outputfile>position_estimator</param.grt.outputfile>
- <param.artifact>option.target.artifact.lib</param.artifact>
- <param.mex.GenCodeOnly>true</param.mex.GenCodeOnly>
- <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
- <param.version>R2011a</param.version>
- <param.HasECoderFeatures>true</param.HasECoderFeatures>
- <param.mex.mainhtml>/home/thomasgubler/Dropbox/Semester Project Autonomous Landing PX4/position_estimator/codegen/mex/position_estimator/html/index.html</param.mex.mainhtml>
- <param.grt.mainhtml />
- <unset>
- <param.mex.general.TargetLang />
- <param.mex.general.IntegrityChecks />
- <param.mex.general.ResponsivenessChecks />
- <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.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.CCompilerOptimization />
- <param.grt.CCompilerCustomOptimizations />
- <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.search />
- <param.grt.report.LaunchReport />
- <param.grt.GenerateComments />
- <param.grt.MATLABSourceComments />
- <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.version />
- <param.HasECoderFeatures />
- </unset>
- <fileset.entrypoints>
- <file value="${PROJECT_ROOT}/position_estimator.m" custom-data-expanded="true">
- <Inputs fileName="position_estimator.m" functionName="position_estimator">
- <Input Name="u">
- <Class>single</Class>
- <Size>2 x 1</Size>
- <Value />
- <InitialValue />
- <Complex>false</Complex>
- </Input>
- <Input Name="z">
- <Class>single</Class>
- <Size>3 x 1</Size>
- <Value />
- <InitialValue />
- <Complex>false</Complex>
- </Input>
- <Input Name="xapo">
- <Class>single</Class>
- <Size>6 x 1</Size>
- <Value />
- <InitialValue />
- <Complex>false</Complex>
- </Input>
- <Input Name="Papo">
- <Class>single</Class>
- <Size>6 x 6</Size>
- <Value />
- <InitialValue />
- <Complex>false</Complex>
- </Input>
- <Input Name="gps_covariance">
- <Class>single</Class>
- <Size>3 x 1</Size>
- <Value />
- <InitialValue />
- <Complex>false</Complex>
- </Input>
- <Input Name="predict_only">
- <Class>uint8</Class>
- <Size>1 x 1</Size>
- <Value />
- <InitialValue />
- <Complex>false</Complex>
- </Input>
- </Inputs>
- </file>
- </fileset.entrypoints>
- <fileset.package />
- <build-deliverables />
- <workflow />
- <matlab>
- <root>/home/thomasgubler/tools/matlab</root>
- </matlab>
- <platform>
- <unix>true</unix>
- <mac>false</mac>
- <windows>false</windows>
- <win2k>false</win2k>
- <winxp>false</winxp>
- <vista>false</vista>
- <linux>true</linux>
- <solaris>false</solaris>
- <osver>3.2.0-25-generic</osver>
- <os32>false</os32>
- <os64>true</os64>
- <arch>glnxa64</arch>
- <matlab>true</matlab>
- </platform>
- </configuration>
-</deployment-project>
-
diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c index f8390f100..e84945299 100644 --- a/apps/position_estimator/position_estimator_main.c +++ b/apps/position_estimator/position_estimator_main.c @@ -61,8 +61,6 @@ #include <uORB/topics/vehicle_local_position.h> #include <poll.h> -#include "codegen/position_estimator.h" - #define N_STATES 6 #define ERROR_COVARIANCE_INIT 3 #define R_EARTH 6371000.0 diff --git a/apps/px4/attitude_estimator_bm/.context b/apps/px4/attitude_estimator_bm/.context deleted file mode 100644 index e69de29bb..000000000 --- a/apps/px4/attitude_estimator_bm/.context +++ /dev/null diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c deleted file mode 100644 index 6921db375..000000000 --- a/apps/px4/attitude_estimator_bm/attitude_bm.c +++ /dev/null @@ -1,322 +0,0 @@ - -/* - * attitude_bm.c - * - * Created on: 21.12.2010 - * Author: Laurens Mackay, Tobias Naegeli - */ - -#include <math.h> -#include "attitude_bm.h" -#include "kalman.h" - - -#define TIME_STEP (1.0f / 500.0f) - -static kalman_t attitude_blackmagic_kal; - -void vect_norm(float_vect3 *vect) -{ - float length = sqrtf( - vect->x * vect->x + vect->y * vect->y + vect->z * vect->z); - - if (length != 0) { - vect->x /= length; - vect->y /= length; - vect->z /= length; - } -} - - -void vect_cross_product(const float_vect3 *a, const float_vect3 *b, - float_vect3 *c) -{ - c->x = a->y * b->z - a->z * b->y; - c->y = a->z * b->x - a->x * b->z; - c->z = a->x * b->y - a->y * b->x; -} - -void attitude_blackmagic_update_a(void) -{ - // for acc - // Idendity matrix already in A. - M(attitude_blackmagic_kal.a, 0, 1) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 0, 2) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - - M(attitude_blackmagic_kal.a, 1, 0) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 1, 2) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - - M(attitude_blackmagic_kal.a, 2, 0) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - M(attitude_blackmagic_kal.a, 2, 1) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - - // for mag - // Idendity matrix already in A. - M(attitude_blackmagic_kal.a, 3, 4) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 3, 5) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - - M(attitude_blackmagic_kal.a, 4, 3) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 4, 5) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - - M(attitude_blackmagic_kal.a, 5, 3) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - M(attitude_blackmagic_kal.a, 5, 4) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - -} - -void attitude_blackmagic_init(void) -{ - //X Kalmanfilter - //initalize matrices - - static m_elem kal_a[12 * 12] = { - 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f - }; - - static m_elem kal_c[9 * 12] = { - 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f - }; - - - -#define FACTOR 0.5 -#define FACTORstart 1 - - -// static m_elem kal_gain[12 * 9] = -// { 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, -// 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0, -// 0 , 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0, -// 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0 , 0, -// 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0, -// 0 , 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0, -// 0.0000 , +0.00002,0 , 0 , 0, 0, 0, 0 , 0, -// -0.00002,0 , 0 , 0 , 0, 0, 0, 0, 0, -// 0, 0 , 0 , 0, 0, 0, 0, 0, 0, -// 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0 , 0, -// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0, -// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 -// }; - - static m_elem kal_gain[12 * 9] = { - 0.0007f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0.0007f , 0 , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0.0007f , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0, - 0.0000f , +0.00002f, 0 , 0 , 0, 0, 0, 0 , 0, - -0.00002f, 0 , 0 , 0 , 0, 0, 0, 0, 0, - 0, 0 , 0 , 0, 0, 0, 0, 0, 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0.7f , 0 , 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.7f , 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.7f - }; - //offset update only correct if not upside down. - -#define K (10.0f*TIME_STEP) - - static m_elem kal_gain_start[12 * 9] = { - K, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, K, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, K, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, K, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, K, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, K, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, K, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, K, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, K, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0 - }; - - - - static m_elem kal_x_apriori[12 * 1] = - { }; - - - //---> initial states sind aposteriori!? ---> fehler - static m_elem kal_x_aposteriori[12 * 1] = - { 0.0f, 0.0f, -1.0f, 0.6f, 0.0f, 0.8f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; - - kalman_init(&attitude_blackmagic_kal, 12, 9, kal_a, kal_c, - kal_gain_start, kal_gain, kal_x_apriori, kal_x_aposteriori, 1000); - -} - -void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro) -{ - // Kalman Filter - - //Calculate new linearized A matrix - attitude_blackmagic_update_a(); - - kalman_predict(&attitude_blackmagic_kal); - - //correction update - - m_elem measurement[9] = - { }; - m_elem mask[9] = - { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f }; - - // XXX Hack - stop updating accel if upside down - - if (accel->z > 0) { - mask[0] = 0.0f; - mask[1] = 0.0f; - mask[2] = 0.0f; - } else { - mask[0] = 1.0f; - mask[1] = 1.0f; - mask[2] = 1.0f; - } - - measurement[0] = accel->x; - measurement[1] = accel->y; - measurement[2] = accel->z; - - measurement[3] = mag->x; - measurement[4] = mag->y; - measurement[5] = mag->z; - - measurement[6] = gyro->x; - measurement[7] = gyro->y; - measurement[8] = gyro->z; - - //Put measurements into filter - - -// static int j = 0; -// if (j >= 3) -// { -// j = 0; -// -// mask[3]=1; -// mask[4]=1; -// mask[5]=1; -// j=0; -// -// }else{ -// j++;} - - kalman_correct(&attitude_blackmagic_kal, measurement, mask); - -} -void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) -{ - //debug - - // save outputs - float_vect3 kal_acc; - float_vect3 kal_mag; -// float_vect3 kal_w0, kal_w; - - kal_acc.x = kalman_get_state(&attitude_blackmagic_kal, 0); - kal_acc.y = kalman_get_state(&attitude_blackmagic_kal, 1); - kal_acc.z = kalman_get_state(&attitude_blackmagic_kal, 2); - - kal_mag.x = kalman_get_state(&attitude_blackmagic_kal, 3); - kal_mag.y = kalman_get_state(&attitude_blackmagic_kal, 4); - kal_mag.z = kalman_get_state(&attitude_blackmagic_kal, 5); - -// kal_w0.x = kalman_get_state(&attitude_blackmagic_kal, 6); -// kal_w0.y = kalman_get_state(&attitude_blackmagic_kal, 7); -// kal_w0.z = kalman_get_state(&attitude_blackmagic_kal, 8); -// -// kal_w.x = kalman_get_state(&attitude_blackmagic_kal, 9); -// kal_w.y = kalman_get_state(&attitude_blackmagic_kal, 10); -// kal_w.z = kalman_get_state(&attitude_blackmagic_kal, 11); - - rates->x = kalman_get_state(&attitude_blackmagic_kal, 9); - rates->y = kalman_get_state(&attitude_blackmagic_kal, 10); - rates->z = kalman_get_state(&attitude_blackmagic_kal, 11); - - - -// kal_w = kal_w; // XXX hack to silence compiler warning -// kal_w0 = kal_w0; // XXX hack to silence compiler warning - - - - //debug_vect("magn", mag); - - //float_vect3 x_n_b, y_n_b, z_n_b; - z_n_b->x = -kal_acc.x; - z_n_b->y = -kal_acc.y; - z_n_b->z = -kal_acc.z; - vect_norm(z_n_b); - vect_cross_product(z_n_b, &kal_mag, y_n_b); - vect_norm(y_n_b); - - vect_cross_product(y_n_b, z_n_b, x_n_b); - - - - //save euler angles - euler->x = atan2f(z_n_b->y, z_n_b->z); - euler->y = -asinf(z_n_b->x); - euler->z = atan2f(y_n_b->x, x_n_b->x); - -} diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.h b/apps/px4/attitude_estimator_bm/attitude_bm.h deleted file mode 100644 index c21b3d6f1..000000000 --- a/apps/px4/attitude_estimator_bm/attitude_bm.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * attitude_blackmagic.h - * - * Created on: May 31, 2011 - * Author: pixhawk - */ - -#ifndef attitude_blackmagic_H_ -#define attitude_blackmagic_H_ - -#include "matrix.h" - -void vect_norm(float_vect3 *vect); - -void vect_cross_product(const float_vect3 *a, const float_vect3 *b, float_vect3 *c); - -void attitude_blackmagic_update_a(void); - -void attitude_blackmagic_init(void); - -void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro); - -void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); -#endif /* attitude_blackmagic_H_ */ diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c deleted file mode 100644 index 45267f315..000000000 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ /dev/null @@ -1,283 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Laurens Mackay <mackayl@student.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 attitude_estimator_bm.c - * Black Magic Attitude Estimator - */ - - - -#include <nuttx/config.h> -#include <stdio.h> -#include <unistd.h> -#include <sys/time.h> -#include <stdbool.h> -#include <fcntl.h> -#include <arch/board/up_hrt.h> -#include <string.h> -#include <poll.h> -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> -#include <math.h> -#include <errno.h> - -#include "attitude_bm.h" - -static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds - -__EXPORT int attitude_estimator_bm_main(int argc, char *argv[]); - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * user_start - ****************************************************************************/ -int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); - -int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) -{ - float_vect3 gyro_values; - gyro_values.x = raw->gyro_rad_s[0]; - gyro_values.y = raw->gyro_rad_s[1]; - gyro_values.z = raw->gyro_rad_s[2]; - - float_vect3 accel_values; - accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100; - accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100; - accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100; - - float_vect3 mag_values; - mag_values.x = raw->magnetometer_ga[0]*1500.0f; - mag_values.y = raw->magnetometer_ga[1]*1500.0f; - mag_values.z = raw->magnetometer_ga[2]*1500.0f; - - // static int i = 0; - - // if (i == 500) { - // printf("[att estim bm] gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n", - // gyro_values.x, gyro_values.y, gyro_values.z, - // accel_values.x, accel_values.y, accel_values.z, - // mag_values.x, mag_values.y, mag_values.z); - // i = 0; - // } - // i++; - - attitude_blackmagic(&accel_values, &mag_values, &gyro_values); - - /* read out values */ - attitude_blackmagic_get_all(euler, rates, x_n_b, y_n_b, z_n_b); - - return OK; -} - - -int attitude_estimator_bm_main(int argc, char *argv[]) -{ - // print text - printf("Black Magic Attitude Estimator initialized..\n\n"); - fflush(stdout); - - /* data structures to read euler angles and rotation matrix back */ - float_vect3 euler = {.x = 0, .y = 0, .z = 0}; - float_vect3 rates; - float_vect3 x_n_b; - float_vect3 y_n_b; - float_vect3 z_n_b; - - int overloadcounter = 19; - - /* initialize */ - attitude_blackmagic_init(); - - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); - - struct sensor_combined_s sensor_combined_s_local = { .gyro_raw = {0}}; - struct vehicle_attitude_s att = {.roll = 0.0f, .pitch = 0.0f, .yaw = 0.0f, - .rollspeed = 0.0f, .pitchspeed = 0.0f, .yawspeed = 0.0f, - .R = {0}, .timestamp = 0}; - - uint64_t last_data = 0; - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - - /* rate-limit raw data updates to 200Hz */ - //orb_set_interval(sub_raw, 5); - - bool hil_enabled = false; - bool publishing = false; - - /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - publishing = true; - - struct pollfd fds[] = { - { .fd = sub_raw, .events = POLLIN }, - }; - - /* subscribe to system status */ - struct vehicle_status_s vstatus = {0}; - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - - unsigned int loopcounter = 0; - - uint64_t last_checkstate_stamp = 0; - - /* Main loop*/ - while (true) { - - - /* wait for sensor update */ - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); - } else { - orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local); - - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - //#if 0 - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - if (overloadcounter == 20) { - printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - overloadcounter = 0; - } - - overloadcounter++; - } - - //#endif - // now = hrt_absolute_time(); - /* filter values */ - attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b); - - // time_elapsed = hrt_absolute_time() - now; - // if (blubb == 20) - // { - // printf("Estimator: %lu\n", time_elapsed); - // blubb = 0; - // } - // blubb++; - - // if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data); - - // printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data); - // last_data = sensor_combined_s_local.timestamp; - - /*correct yaw */ - // euler.z = euler.z + M_PI; - - /* send out */ - - att.timestamp = sensor_combined_s_local.timestamp; - att.roll = euler.x; - att.pitch = euler.y; - att.yaw = euler.z; - - if (att.yaw > M_PI_F) { - att.yaw -= 2.0f * M_PI_F; - } - - if (att.yaw < -M_PI_F) { - att.yaw += 2.0f * M_PI_F; - } - - att.rollspeed = rates.x; - att.pitchspeed = rates.y; - att.yawspeed = rates.z; - - att.R[0][0] = x_n_b.x; - att.R[0][1] = x_n_b.y; - att.R[0][2] = x_n_b.z; - - // Broadcast - if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - } - - - // XXX add remaining entries - - if (hrt_absolute_time() - last_checkstate_stamp > 500000) { - /* Check HIL state */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - /* switching from non-HIL to HIL mode */ - //printf("[attitude_estimator_bm] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !hil_enabled) { - hil_enabled = true; - publishing = false; - int ret = close(pub_att); - printf("Closing attitude: %i \n", ret); - - /* switching from HIL to non-HIL mode */ - - } else if (!publishing && !hil_enabled) { - /* advertise the topic and make the initial publication */ - pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - hil_enabled = false; - publishing = true; - } - last_checkstate_stamp = hrt_absolute_time(); - } - - loopcounter++; - } - - /* Should never reach here */ - return 0; -} - - diff --git a/apps/px4/attitude_estimator_bm/kalman.c b/apps/px4/attitude_estimator_bm/kalman.c deleted file mode 100644 index e4ea7a417..000000000 --- a/apps/px4/attitude_estimator_bm/kalman.c +++ /dev/null @@ -1,115 +0,0 @@ -/* - * kalman.c - * - * Created on: 01.12.2010 - * Author: Laurens Mackay - */ -#include "kalman.h" -//#include "mavlink_debug.h" - -void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[], - m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[], - m_elem x_aposteriori[], int gainfactorsteps) -{ - kalman->states = states; - kalman->measurements = measurements; - kalman->gainfactorsteps = gainfactorsteps; - kalman->gainfactor = 0; - - //Create all matrices that are persistent - kalman->a = matrix_create(states, states, a); - kalman->c = matrix_create(measurements, states, c); - kalman->gain_start = matrix_create(states, measurements, gain_start); - kalman->gain = matrix_create(states, measurements, gain); - kalman->x_apriori = matrix_create(states, 1, x_apriori); - kalman->x_aposteriori = matrix_create(states, 1, x_aposteriori); -} - -void kalman_predict(kalman_t *kalman) -{ - matrix_mult(kalman->a, kalman->x_aposteriori, kalman->x_apriori); -} - -void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]) -{ - //create matrices from inputs - matrix_t measurement = - matrix_create(kalman->measurements, 1, measurement_a); - matrix_t mask = matrix_create(kalman->measurements, 1, mask_a); - - //create temporary matrices - m_elem gain_start_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = - { }; - matrix_t gain_start_part = matrix_create(kalman->states, - kalman->measurements, gain_start_part_a); - - m_elem gain_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = - { }; - matrix_t gain_part = matrix_create(kalman->states, kalman->measurements, - gain_part_a); - - m_elem gain_sum_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = - { }; - matrix_t gain_sum = matrix_create(kalman->states, kalman->measurements, - gain_sum_a); - - m_elem error_a[KALMAN_MAX_MEASUREMENTS * 1] = - { }; - matrix_t error = matrix_create(kalman->measurements, 1, error_a); - - m_elem measurement_estimate_a[KALMAN_MAX_MEASUREMENTS * 1] = - { }; - matrix_t measurement_estimate = matrix_create(kalman->measurements, 1, - measurement_estimate_a); - - m_elem x_update_a[KALMAN_MAX_STATES * 1] = - { }; - matrix_t x_update = matrix_create(kalman->states, 1, x_update_a); - - - - //x(:,i+1)=xapriori+(gainfactor*[M_50(:,1) M(:,2)]+(1-gainfactor)*M_start)*(z-C*xapriori); - - - //est=C*xapriori; - matrix_mult(kalman->c, kalman->x_apriori, measurement_estimate); - //error=(z-C*xapriori) = measurement-estimate - matrix_sub(measurement, measurement_estimate, error); - matrix_mult_element(error, mask, error); - - kalman->gainfactor = kalman->gainfactor * (1.0f - 1.0f - / kalman->gainfactorsteps) + 1.0f * 1.0f / kalman->gainfactorsteps; - - - - matrix_mult_scalar(kalman->gainfactor, kalman->gain, gain_part); - - matrix_mult_scalar(1.0f - kalman->gainfactor, kalman->gain_start, - gain_start_part); - - matrix_add(gain_start_part, gain_part, gain_sum); - - //gain*(z-C*xapriori) - matrix_mult(gain_sum, error, x_update); - - //xaposteriori = xapriori + update - - matrix_add(kalman->x_apriori, x_update, kalman->x_aposteriori); - - -// static int i=0; -// if(i++==4){ -// i=0; -// float_vect3 out_kal; -// out_kal.x = M(gain_sum,0,1); -//// out_kal_z.x = z_measurement[1]; -// out_kal.y = M(gain_sum,1,1); -// out_kal.z = M(gain_sum,2,1); -// debug_vect("out_kal", out_kal); -// } -} - -m_elem kalman_get_state(kalman_t *kalman, int state) -{ - return M(kalman->x_aposteriori, state, 0); -} diff --git a/apps/px4/attitude_estimator_bm/kalman.h b/apps/px4/attitude_estimator_bm/kalman.h deleted file mode 100644 index 0a6a18505..000000000 --- a/apps/px4/attitude_estimator_bm/kalman.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * kalman.h - * - * Created on: 01.12.2010 - * Author: Laurens Mackay - */ - -#ifndef KALMAN_H_ -#define KALMAN_H_ - -#include "matrix.h" - -#define KALMAN_MAX_STATES 12 -#define KALMAN_MAX_MEASUREMENTS 9 -typedef struct { - int states; - int measurements; - matrix_t a; - matrix_t c; - matrix_t gain_start; - matrix_t gain; - matrix_t x_apriori; - matrix_t x_aposteriori; - float gainfactor; - int gainfactorsteps; -} kalman_t; - -void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[], - m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[], - m_elem x_aposteriori[], int gainfactorsteps); -void kalman_predict(kalman_t *kalman); -void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]); -m_elem kalman_get_state(kalman_t *kalman, int state); - -#endif /* KALMAN_H_ */ diff --git a/apps/px4/attitude_estimator_bm/matrix.h b/apps/px4/attitude_estimator_bm/matrix.h deleted file mode 100644 index 613a2d081..000000000 --- a/apps/px4/attitude_estimator_bm/matrix.h +++ /dev/null @@ -1,156 +0,0 @@ -/* - * matrix.h - * - * Created on: 18.11.2010 - * Author: Laurens Mackay - */ - -#ifndef MATRIX_H_ -#define MATRIX_H_ - -typedef float m_elem; - -typedef struct { - int rows; - int cols; - m_elem *a; -} matrix_t; - -typedef struct { - float x; - float y; - float z; -} float_vect3; - -#define M(m,i,j) m.a[m.cols*i+j] - -///* This is the datatype used for the math and non-type specific ops. */ -// -//matrix_t matrix_create(const int rows, const int cols, m_elem * a); -///* matrix C = matrix A + matrix B , both of size m x n */ -//void matrix_add(const matrix_t a, const matrix_t b, matrix_t c); -// -///* matrix C = matrix A - matrix B , all of size m x n */ -//void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c); -// -///* matrix C = matrix A x matrix B , A(a_rows x a_cols), B(a_cols x b_cols) */ -//void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c); -// -//void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c); -// -//void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c); -// -///* matrix C = A*B'*/ -//void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c); - - -static inline matrix_t matrix_create(const int rows, const int cols, m_elem *a) -{ - matrix_t ret; - ret.rows = rows; - ret.cols = cols; - ret.a = a; - return ret; -} - -static inline void matrix_add(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols - != c.cols) { - //debug_message_buffer("matrix_add: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = M(a, i, j) + M(b, i, j); - } - - } -} - -static inline void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols - != c.cols) { - //debug_message_buffer("matrix_sub: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = M(a, i, j) - M(b, i, j); - } - - } -} - -static inline void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || b.cols != c.cols || a.cols != b.rows) { - //debug_message_buffer("matrix_mult: Dimension mismatch"); - } - - for (int i = 0; i < a.rows; i++) { - for (int j = 0; j < b.cols; j++) { - M(c, i, j) = 0; - - for (int k = 0; k < a.cols; k++) { - M(c, i, j) += M(a, i, k) * M(b, k, j); - } - } - - } -} - -static inline void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c) -{ - - if (a.rows != c.rows || b.rows != c.cols || a.cols != b.cols) { - //debug_message_buffer("matrix_mult: Dimension mismatch"); - } - - for (int i = 0; i < a.rows; i++) { - for (int j = 0; j < b.cols; j++) { - M(c, i, j) = 0; - - for (int k = 0; k < a.cols; k++) { - M(c, i, j) += M(a, i, k) * M(b, j, k); - } - } - - } - -} - -static inline void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols) { - //debug_message_buffer("matrix_mult_scalar: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = f * M(a, i, j); - } - - } -} - - -static inline void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols - != c.cols) { - //debug_message_buffer("matrix_mult_element: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = M(a, i, j) * M(b, i, j); - } - - } -} - - - -#endif /* MATRIX_H_ */ diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 6d990c46b..eccdeb78e 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -61,8 +61,6 @@ #include <systemlib/mixer/mixer.h> #include <drivers/drv_mixer.h> -#include <arch/board/up_pwm_servo.h> - #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c deleted file mode 100644 index ccf9ee3ec..000000000 --- a/apps/px4/ground_estimator/ground_estimator.c +++ /dev/null @@ -1,180 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User <mail@example.com> - * - * 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 ground_estimator.c - * ground_estimator application example for PX4 autopilot - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdio.h> -#include <poll.h> -#include <string.h> -#include <stdlib.h> - -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/debug_key_value.h> - - -static bool thread_should_exit = false; /**< ground_estimator exit flag */ -static bool thread_running = false; /**< ground_estimator status flag */ -static int ground_estimator_task; /**< Handle of ground_estimator task / thread */ - -/** - * ground_estimator management function. - */ -__EXPORT int ground_estimator_main(int argc, char *argv[]); - -/** - * Mainloop of ground_estimator. - */ -int ground_estimator_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -int ground_estimator_thread_main(int argc, char *argv[]) { - - printf("[ground_estimator] starting\n"); - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - - /* advertise debug value */ - struct debug_key_value_s dbg = { .key = "posx", .value = 0.0f }; - orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); - - float position[3] = {}; - float velocity[3] = {}; - - uint64_t last_time = 0; - - struct pollfd fds[] = { - { .fd = sub_raw, .events = POLLIN }, - }; - - while (!thread_should_exit) { - - /* wait for sensor update */ - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[ground estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); - } else { - struct sensor_combined_s s; - orb_copy(ORB_ID(sensor_combined), sub_raw, &s); - - float dt = ((float)(s.timestamp - last_time)) / 1000000.0f; - - /* Integration */ - position[0] += velocity[0] * dt; - position[1] += velocity[1] * dt; - position[2] += velocity[2] * dt; - - velocity[0] += velocity[0] * 0.01f + 0.99f * s.accelerometer_m_s2[0] * dt; - velocity[1] += velocity[1] * 0.01f + 0.99f * s.accelerometer_m_s2[1] * dt; - velocity[2] += velocity[2] * 0.01f + 0.99f * s.accelerometer_m_s2[2] * dt; - - dbg.value = position[0]; - - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - } - - } - - printf("[ground_estimator] exiting.\n"); - - return 0; -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: ground_estimator {start|stop|status} [-p <additional params>]\n\n"); - exit(1); -} - -/** - * The ground_estimator app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int ground_estimator_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("ground_estimator already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - ground_estimator_task = task_create("ground_estimator", SCHED_PRIORITY_DEFAULT, 4096, ground_estimator_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tground_estimator is running\n"); - } else { - printf("\tground_estimator not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} diff --git a/apps/px4/px4io/driver/.context b/apps/px4/px4io/driver/.context deleted file mode 100644 index e69de29bb..000000000 --- a/apps/px4/px4io/driver/.context +++ /dev/null diff --git a/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c index 8c6951b63..f364ea080 100644 --- a/apps/px4/tests/test_hrt.c +++ b/apps/px4/tests/test_hrt.c @@ -50,7 +50,7 @@ #include <unistd.h> #include <arch/board/board.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_tone_alarm.h> #include <nuttx/spi.h> diff --git a/apps/px4/tests/test_time.c b/apps/px4/tests/test_time.c index c128c73a3..25bf02c31 100644 --- a/apps/px4/tests/test_time.c +++ b/apps/px4/tests/test_time.c @@ -53,7 +53,7 @@ #include <math.h> #include <float.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> /**************************************************************************** diff --git a/apps/px4/tests/test_uart_console.c b/apps/px4/tests/test_uart_console.c index de1249b8c..fc5b03036 100644 --- a/apps/px4/tests/test_uart_console.c +++ b/apps/px4/tests/test_uart_console.c @@ -56,7 +56,7 @@ #include <math.h> #include <float.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> /**************************************************************************** diff --git a/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c index 83d205440..a88e617d9 100644 --- a/apps/px4/tests/test_uart_send.c +++ b/apps/px4/tests/test_uart_send.c @@ -56,7 +56,7 @@ #include <math.h> #include <float.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> /**************************************************************************** diff --git a/apps/px4/tests/tests_file.c b/apps/px4/tests/tests_file.c index 2cff622f7..697410cee 100644 --- a/apps/px4/tests/tests_file.c +++ b/apps/px4/tests/tests_file.c @@ -44,7 +44,7 @@ #include <systemlib/perf_counter.h> #include <string.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "tests.h" diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 19802bf4f..f9106d830 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -48,7 +48,7 @@ #include <nuttx/clock.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 9dc1fdcba..a91aad3ca 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -47,7 +47,7 @@ #include <arch/board/drv_ppm_input.h> #include <arch/board/drv_pwm_servo.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "px4io.h" diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 30a62fa65..a67db9a7e 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -50,7 +50,7 @@ #include <arch/board/up_boardinitialize.h> #include <arch/board/drv_gpio.h> #include <arch/board/drv_ppm_input.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "px4io.h" diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 895c33806..6cb85798f 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -49,7 +49,7 @@ #include <arch/board/up_boardinitialize.h> #include <arch/board/drv_gpio.h> #include <arch/board/drv_ppm_input.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "px4io.h" diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 36225386c..7d2f6afba 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -51,7 +51,7 @@ #include <string.h> #include <systemlib/err.h> #include <unistd.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> @@ -72,6 +72,7 @@ static int deamon_task; /**< Handle of deamon task / thread */ static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ static const char *mountpoint = "/fs/microsd"; +static const char *mfile_in = "/etc/logging/logconv.m"; /** * SD log management function. @@ -90,10 +91,12 @@ static void usage(const char *reason); static int file_exist(const char *filename); +static int file_copy(const char* file_old, const char* file_new); + /** * Print the current status. */ -static void print_sdlog_status(); +static void print_sdlog_status(void); /** * Create folder for current logging session. @@ -181,7 +184,17 @@ int create_logfolder(char* folder_path) { /* the result is -1 if the folder exists */ if (mkdir_ret == 0) { - /* folder does not exist */ + /* folder does not exist, success */ + + /* now copy the Matlab/Octave file */ + char mfile_out[100]; + sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber); + int ret = file_copy(mfile_in, mfile_out); + if (!ret) { + warnx("copied m file to %s", mfile_out); + } else { + warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out); + } break; } else if (mkdir_ret == -1) { @@ -379,7 +392,7 @@ int sdlog_thread_main(int argc, char *argv[]) { * set up poll to block for new data, * wait for a maximum of 1000 ms (1 second) */ - const int timeout = 1000; + // const int timeout = 1000; thread_running = true; @@ -550,3 +563,42 @@ int file_exist(const char *filename) return stat(filename, &buffer); } +int file_copy(const char* file_old, const char* file_new) +{ + FILE *source, *target; + source = fopen(file_old, "r"); + int ret = 0; + + if( source == NULL ) + { + warnx("failed opening input file to copy"); + return 1; + } + + target = fopen(file_new, "w"); + + if( target == NULL ) + { + fclose(source); + warnx("failed to open output file to copy"); + return 1; + } + + char buf[128]; + int nread; + while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) { + int ret = fwrite(buf, 1, nread, target); + if (ret <= 0) { + warnx("error writing file"); + ret = 1; + break; + } + } + fsync(fileno(target)); + + fclose(source); + fclose(target); + + return ret; +} + diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 3e204662a..54d2f6a0b 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -52,15 +52,13 @@ #include <errno.h> #include <math.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_accel.h> #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> #include <drivers/drv_baro.h> -#include <arch/board/up_adc.h> - #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> diff --git a/apps/systemcmds/bl_update/bl_update.c b/apps/systemcmds/bl_update/bl_update.c index 752c01986..745f76852 100644 --- a/apps/systemcmds/bl_update/bl_update.c +++ b/apps/systemcmds/bl_update/bl_update.c @@ -67,10 +67,12 @@ bl_update_main(int argc, char *argv[]) setopt(); int fd = open(argv[1], O_RDONLY); + if (fd < 0) err(1, "open %s", argv[1]); struct stat s; + if (stat(argv[1], &s) < 0) err(1, "stat %s", argv[1]); @@ -79,14 +81,17 @@ bl_update_main(int argc, char *argv[]) errx(1, "%s: file too large", argv[1]); uint8_t *buf = malloc(s.st_size); + if (buf == NULL) errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size); if (read(fd, buf, s.st_size) != s.st_size) err(1, "firmware read error"); + close(fd); uint32_t *hdr = (uint32_t *)buf; + if ((hdr[0] < 0x20000000) || /* stack not below RAM */ (hdr[0] > (0x20000000 + (128 * 1024))) || /* stack not above RAM */ (hdr[1] < 0x08000000) || /* entrypoint not below flash */ @@ -123,6 +128,7 @@ bl_update_main(int argc, char *argv[]) /* wait for the operation to complete */ while (*sr & 0x1000) { } + if (*sr & 0xf2) { warnx("WARNING: erase error 0x%02x", *sr); goto flash_end; @@ -148,6 +154,7 @@ bl_update_main(int argc, char *argv[]) /* wait for the operation to complete */ while (*sr & 0x1000) { } + if (*sr & 0xf2) { warnx("WARNING: program error 0x%02x", *sr); goto flash_end; @@ -203,6 +210,7 @@ setopt(void) if ((*optcr & opt_mask) == opt_bits) errx(0, "option bits set"); + errx(1, "option bits setting failed; readback 0x%04x", *optcr); }
\ No newline at end of file diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c index 3cded52fa..79149caa0 100644 --- a/apps/systemcmds/eeprom/24xxxx_mtd.c +++ b/apps/systemcmds/eeprom/24xxxx_mtd.c @@ -135,20 +135,19 @@ * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. */ -struct at24c_dev_s -{ - struct mtd_dev_s mtd; /* MTD interface */ - FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ - uint8_t addr; /* I2C address */ - uint16_t pagesize; /* 32, 63 */ - uint16_t npages; /* 128, 256, 512, 1024 */ - - perf_counter_t perf_reads; - perf_counter_t perf_writes; - perf_counter_t perf_resets; - perf_counter_t perf_read_retries; - perf_counter_t perf_read_errors; - perf_counter_t perf_write_errors; +struct at24c_dev_s { + struct mtd_dev_s mtd; /* MTD interface */ + FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ + uint8_t addr; /* I2C address */ + uint16_t pagesize; /* 32, 63 */ + uint16_t npages; /* 128, 256, 512, 1024 */ + + perf_counter_t perf_reads; + perf_counter_t perf_writes; + perf_counter_t perf_resets; + perf_counter_t perf_read_retries; + perf_counter_t perf_read_errors; + perf_counter_t perf_write_errors; }; /************************************************************************************ @@ -159,9 +158,9 @@ struct at24c_dev_s static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buf); + size_t nblocks, FAR uint8_t *buf); static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR const uint8_t *buf); + size_t nblocks, FAR const uint8_t *buf); static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); /************************************************************************************ @@ -180,35 +179,32 @@ static struct at24c_dev_s g_at24c; static int at24c_eraseall(FAR struct at24c_dev_s *priv) { - int startblock = 0; - uint8_t buf[AT24XX_PAGESIZE + 2]; - - struct i2c_msg_s msgv[1] = - { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - memset(&buf[2],0xff,priv->pagesize); - - for (startblock = 0; startblock < priv->npages; startblock++) - { - uint16_t offset = startblock * priv->pagesize; - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - - while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) - { - fvdbg("erase stall\n"); - usleep(10000); - } - } - - return OK; + int startblock = 0; + uint8_t buf[AT24XX_PAGESIZE + 2]; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + memset(&buf[2], 0xff, priv->pagesize); + + for (startblock = 0; startblock < priv->npages; startblock++) { + uint16_t offset = startblock * priv->pagesize; + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + + while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { + fvdbg("erase stall\n"); + usleep(10000); + } + } + + return OK; } /************************************************************************************ @@ -217,9 +213,9 @@ static int at24c_eraseall(FAR struct at24c_dev_s *priv) static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) { - /* EEprom need not erase */ + /* EEprom need not erase */ - return (int)nblocks; + return (int)nblocks; } /************************************************************************************ @@ -227,90 +223,86 @@ static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nbloc ************************************************************************************/ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buffer) + size_t nblocks, FAR uint8_t *buffer) { - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t addr[2]; - int ret; - - struct i2c_msg_s msgv[2] = - { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addr[0], - .length = sizeof(addr), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = 0, - .length = priv->pagesize, - } - }; + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t addr[2]; + int ret; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addr[0], + .length = sizeof(addr), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = 0, + .length = priv->pagesize, + } + }; #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #endif - blocksleft = nblocks; - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - if (startblock >= priv->npages) - { - return 0; - } - - if (startblock + nblocks > priv->npages) - { - nblocks = priv->npages - startblock; - } - - while (blocksleft-- > 0) - { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - addr[1] = offset & 0xff; - addr[0] = (offset >> 8) & 0xff; - msgv[1].buffer = buffer; - - for (;;) - { - - perf_begin(priv->perf_reads); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - if (ret >= 0) - break; - - fvdbg("read stall"); - usleep(1000); - - /* We should normally only be here on the first read after - * a write. - * - * XXX maybe do special first-read handling with optional - * bus reset as well? - */ - perf_count(priv->perf_read_retries); - if (--tries == 0) - { - perf_count(priv->perf_read_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } + blocksleft = nblocks; + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + addr[1] = offset & 0xff; + addr[0] = (offset >> 8) & 0xff; + msgv[1].buffer = buffer; + + for (;;) { + + perf_begin(priv->perf_reads); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret >= 0) + break; + + fvdbg("read stall"); + usleep(1000); + + /* We should normally only be here on the first read after + * a write. + * + * XXX maybe do special first-read handling with optional + * bus reset as well? + */ + perf_count(priv->perf_read_retries); + + if (--tries == 0) { + perf_count(priv->perf_read_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #else - return nblocks; + return nblocks; #endif } @@ -322,81 +314,75 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, ************************************************************************************/ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, - FAR const uint8_t *buffer) + FAR const uint8_t *buffer) { - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t buf[AT24XX_PAGESIZE+2]; - int ret; - - struct i2c_msg_s msgv[1] = - { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t buf[AT24XX_PAGESIZE + 2]; + int ret; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #endif - blocksleft = nblocks; + blocksleft = nblocks; - if (startblock >= priv->npages) - { - return 0; - } + if (startblock >= priv->npages) { + return 0; + } - if (startblock + nblocks > priv->npages) - { - nblocks = priv->npages - startblock; - } + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - while (blocksleft-- > 0) - { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - memcpy(&buf[2], buffer, priv->pagesize); + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + memcpy(&buf[2], buffer, priv->pagesize); - for (;;) - { + for (;;) { - perf_begin(priv->perf_writes); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); - perf_end(priv->perf_writes); + perf_begin(priv->perf_writes); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); + perf_end(priv->perf_writes); - if (ret >= 0) - break; + if (ret >= 0) + break; - fvdbg("write stall"); - usleep(1000); + fvdbg("write stall"); + usleep(1000); - /* We expect to see a number of retries per write cycle as we - * poll for write completion. - */ - if (--tries == 0) - { - perf_count(priv->perf_write_errors); - return ERROR; - } - } + /* We expect to see a number of retries per write cycle as we + * poll for write completion. + */ + if (--tries == 0) { + perf_count(priv->perf_write_errors); + return ERROR; + } + } - startblock++; - buffer += priv->pagesize; - } + startblock++; + buffer += priv->pagesize; + } #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); #else - return nblocks; + return nblocks; #endif } @@ -406,67 +392,65 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) { - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - int ret = -EINVAL; /* Assume good command with bad parameters */ - - fvdbg("cmd: %d \n", cmd); - - switch (cmd) - { - case MTDIOC_GEOMETRY: - { - FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); - if (geo) - { - /* Populate the geometry structure with information need to know - * the capacity and how to access the device. - * - * NOTE: that the device is treated as though it where just an array - * of fixed size blocks. That is most likely not true, but the client - * will expect the device logic to do whatever is necessary to make it - * appear so. - * - * blocksize: - * May be user defined. The block size for the at24XX devices may be - * larger than the page size in order to better support file systems. - * The read and write functions translate BLOCKS to pages for the - * small flash devices - * erasesize: - * It has to be at least as big as the blocksize, bigger serves no - * purpose. - * neraseblocks - * Note that the device size is in kilobits and must be scaled by - * 1024 / 8 - */ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + fvdbg("cmd: %d \n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to know + * the capacity and how to access the device. + * + * NOTE: that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the client + * will expect the device logic to do whatever is necessary to make it + * appear so. + * + * blocksize: + * May be user defined. The block size for the at24XX devices may be + * larger than the page size in order to better support file systems. + * The read and write functions translate BLOCKS to pages for the + * small flash devices + * erasesize: + * It has to be at least as big as the blocksize, bigger serves no + * purpose. + * neraseblocks + * Note that the device size is in kilobits and must be scaled by + * 1024 / 8 + */ #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; #else - geo->blocksize = priv->pagesize; - geo->erasesize = priv->pagesize; - geo->neraseblocks = priv->npages; + geo->blocksize = priv->pagesize; + geo->erasesize = priv->pagesize; + geo->neraseblocks = priv->npages; #endif - ret = OK; + ret = OK; - fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", - geo->blocksize, geo->erasesize, geo->neraseblocks); - } - } - break; + fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; - case MTDIOC_BULKERASE: - ret=at24c_eraseall(priv); - break; + case MTDIOC_BULKERASE: + ret = at24c_eraseall(priv); + break; - case MTDIOC_XIPBASE: - default: - ret = -ENOTTY; /* Bad command */ - break; - } + case MTDIOC_XIPBASE: + default: + ret = -ENOTTY; /* Bad command */ + break; + } - return ret; + return ret; } /************************************************************************************ @@ -483,46 +467,45 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) * ************************************************************************************/ -FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) -{ - FAR struct at24c_dev_s *priv; - - fvdbg("dev: %p\n", dev); - - /* Allocate a state structure (we allocate the structure instead of using - * a fixed, static allocation so that we can handle multiple FLASH devices. - * The current implementation would handle only one FLASH part per I2C - * device (only because of the SPIDEV_FLASH definition) and so would have - * to be extended to handle multiple FLASH parts on the same I2C bus. - */ - - priv = &g_at24c; - if (priv) - { - /* Initialize the allocated structure */ - - priv->addr = CONFIG_AT24XX_ADDR; - priv->pagesize = AT24XX_PAGESIZE; - priv->npages = AT24XX_NPAGES; - - priv->mtd.erase = at24c_erase; - priv->mtd.bread = at24c_bread; - priv->mtd.bwrite = at24c_bwrite; - priv->mtd.ioctl = at24c_ioctl; - priv->dev = dev; - - priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); - priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); - priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); - priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); - priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); - priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); - } - - /* Return the implementation-specific state structure as the MTD device */ - - fvdbg("Return %p\n", priv); - return (FAR struct mtd_dev_s *)priv; +FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { + FAR struct at24c_dev_s *priv; + + fvdbg("dev: %p\n", dev); + + /* Allocate a state structure (we allocate the structure instead of using + * a fixed, static allocation so that we can handle multiple FLASH devices. + * The current implementation would handle only one FLASH part per I2C + * device (only because of the SPIDEV_FLASH definition) and so would have + * to be extended to handle multiple FLASH parts on the same I2C bus. + */ + + priv = &g_at24c; + + if (priv) { + /* Initialize the allocated structure */ + + priv->addr = CONFIG_AT24XX_ADDR; + priv->pagesize = AT24XX_PAGESIZE; + priv->npages = AT24XX_NPAGES; + + priv->mtd.erase = at24c_erase; + priv->mtd.bread = at24c_bread; + priv->mtd.bwrite = at24c_bwrite; + priv->mtd.ioctl = at24c_ioctl; + priv->dev = dev; + + priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); + priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); + priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); + priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); + priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); + priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); + } + + /* Return the implementation-specific state structure as the MTD device */ + + fvdbg("Return %p\n", priv); + return (FAR struct mtd_dev_s *)priv; } /* @@ -530,5 +513,5 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) */ int at24c_nuke(void) { - return at24c_eraseall(&g_at24c); + return at24c_eraseall(&g_at24c); } diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c index fa88fa09e..19a14aa02 100644 --- a/apps/systemcmds/eeprom/eeprom.c +++ b/apps/systemcmds/eeprom/eeprom.c @@ -143,13 +143,13 @@ eeprom_start(void) if (ret < 0) errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); - + /* mount the EEPROM */ ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); if (ret < 0) errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); - + started = true; warnx("mounted EEPROM at /eeprom"); exit(0); @@ -165,6 +165,7 @@ eeprom_erase(void) if (at24c_nuke()) errx(1, "erase failed"); + errx(0, "erase done, reboot now"); } @@ -190,7 +191,7 @@ eeprom_save(const char *name) if (!started) errx(1, "must be started first"); - if (!name) + if (!name) err(1, "missing argument for device name, try '/eeprom/parameters'"); warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); @@ -221,9 +222,9 @@ eeprom_load(const char *name) if (!started) errx(1, "must be started first"); - if (!name) + if (!name) err(1, "missing argument for device name, try '/eeprom/parameters'"); - + warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); int fd = open(name, O_RDONLY); diff --git a/apps/systemcmds/led/led.c b/apps/systemcmds/led/led.c index 02c1bb133..15d448118 100644 --- a/apps/systemcmds/led/led.c +++ b/apps/systemcmds/led/led.c @@ -50,7 +50,7 @@ #include <termios.h> #include <time.h> #include <sys/prctl.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <arch/board/drv_led.h> #include <systemlib/err.h> @@ -121,6 +121,7 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: led {start|stop|status} [-d <UART>]\n\n"); exit(1); } @@ -129,7 +130,7 @@ usage(const char *reason) * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_create(). */ @@ -165,9 +166,11 @@ int led_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tled is running\n"); + } else { printf("\tled not started\n"); } + exit(0); } @@ -187,7 +190,7 @@ int led_thread_main(int argc, char *argv[]) while (!thread_should_exit) { /* swell blue led */ - + /* 200 Hz base loop */ usleep(1000000 / rate); diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index 9d52557e7..3f52bdbf1 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -97,6 +97,7 @@ load(const char *devname, const char *fname) /* tell it to load the file */ ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname); + if (ret != 0) { fprintf(stderr, "failed loading %s\n", fname); } diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index d70d15da4..68dbd822e 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -70,10 +70,13 @@ param_main(int argc, char *argv[]) if (argc >= 2) { if (!strcmp(argv[1], "save")) do_save(); + if (!strcmp(argv[1], "load")) do_load(); + if (!strcmp(argv[1], "import")) do_import(); + if (!strcmp(argv[1], "show")) do_show(); } @@ -154,8 +157,8 @@ do_show_print(void *arg, param_t param) float f; printf("%c %s: ", - param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), - param_name(param)); + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); /* * This case can be expanded to handle printing common structure types. @@ -167,19 +170,25 @@ do_show_print(void *arg, param_t param) printf("%d\n", i); return; } + break; + case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { printf("%4.4f\n", (double)f); return; } + break; + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param)); return; + default: printf("<unknown type %d>\n", 0 + param_type(param)); return; } + printf("<error fetching parameter %d>\n", param); } diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c index f4e260211..59d2bc8f1 100644 --- a/apps/systemcmds/top/top.c +++ b/apps/systemcmds/top/top.c @@ -46,8 +46,8 @@ #include <string.h> #include <poll.h> -#include <arch/board/up_cpuload.h> -#include <arch/board/up_hrt.h> +#include <systemlib/cpuload.h> +#include <drivers/drv_hrt.h> /** * Start the top application. @@ -190,13 +190,15 @@ int top_main(int argc, char *argv[]) runtime_spaces = ""; } - unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; unsigned stack_free = 0; uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + while (stack_free < stack_size) { if (*stack_sweeper++ != 0xff) break; + stack_free++; } diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index b49a3c54a..4bc7033e7 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -41,8 +41,13 @@ CSRCS = err.c \ param/param.c \ bson/tinybson.c \ conversions.c \ + cpuload.c \ getopt_long.c +# ppm_decode.c \ + + + # # XXX this really should be a CONFIG_* test # diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c index 10b736fd6..75578d2ec 100644 --- a/apps/systemlib/bson/tinybson.c +++ b/apps/systemlib/bson/tinybson.c @@ -56,7 +56,7 @@ static int read_int8(bson_decoder_t decoder, int8_t *b) { - return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1; + return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1; } static int @@ -119,12 +119,14 @@ bson_decoder_next(bson_decoder_t decoder) while (decoder->pending > 0) { if (read_int8(decoder, &tbyte)) CODER_KILL(decoder, "read error discarding pending bytes"); + decoder->pending--; } /* get the type byte */ if (read_int8(decoder, &tbyte)) CODER_KILL(decoder, "read error on type byte"); + decoder->node.type = tbyte; decoder->pending = 0; @@ -135,13 +137,17 @@ bson_decoder_next(bson_decoder_t decoder) /* get the node name */ nlen = 0; + for (;;) { if (nlen >= BSON_MAXNAME) CODER_KILL(decoder, "node name overflow"); + if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) CODER_KILL(decoder, "read error on node name"); + if (decoder->node.name[nlen] == '\0') break; + nlen++; } @@ -151,20 +157,28 @@ bson_decoder_next(bson_decoder_t decoder) case BSON_INT: if (read_int32(decoder, &decoder->node.i)) CODER_KILL(decoder, "read error on BSON_INT"); + break; + case BSON_DOUBLE: if (read_double(decoder, &decoder->node.d)) CODER_KILL(decoder, "read error on BSON_DOUBLE"); + break; + case BSON_STRING: if (read_int32(decoder, &decoder->pending)) CODER_KILL(decoder, "read error on BSON_STRING length"); + break; + case BSON_BINDATA: if (read_int32(decoder, &decoder->pending)) CODER_KILL(decoder, "read error on BSON_BINDATA size"); + if (read_int8(decoder, &tbyte)) CODER_KILL(decoder, "read error on BSON_BINDATA subtype"); + decoder->node.subtype = tbyte; break; @@ -186,11 +200,12 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf) CODER_CHECK(decoder); /* if data already copied, return zero bytes */ - if (decoder->pending == 0) + if (decoder->pending == 0) return 0; /* copy bytes per the node size */ result = read(decoder->fd, buf, decoder->pending); + if (result != decoder->pending) CODER_KILL(decoder, "read error on copy_data"); @@ -209,7 +224,7 @@ static int write_int8(bson_encoder_t encoder, int8_t b) { debug("write_int8 %d", b); - return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1; + return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1; } static int @@ -233,6 +248,7 @@ write_name(bson_encoder_t encoder, const char *name) if (len > BSON_MAXNAME) return -1; + debug("write name '%s' len %d", name, len); return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -1; } @@ -300,6 +316,7 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char write_int32(encoder, len) || write(encoder->fd, name, len + 1) != (int)(len + 1)) CODER_KILL(encoder, "write error on BSON_STRING"); + return 0; } @@ -314,5 +331,6 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary write_int8(encoder, subtype) || write(encoder->fd, data, size) != (int)(size)) CODER_KILL(encoder, "write error on BSON_BINDATA"); + return 0; } diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h index 1b9de5cd3..b6229dc50 100644 --- a/apps/systemlib/bson/tinybson.h +++ b/apps/systemlib/bson/tinybson.h @@ -31,14 +31,14 @@ * ****************************************************************************/ - /** - * @file tinybson.h - * - * A simple subset SAX-style BSON parser and generator. See http://bsonspec.org - * - * Some types and defines taken from the standalone BSON parser/generator - * in the Mongo C connector. - */ +/** +* @file tinybson.h +* +* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org +* +* Some types and defines taken from the standalone BSON parser/generator +* in the Mongo C connector. +*/ #ifndef _TINYBSON_H #define _TINYBSON_H @@ -77,8 +77,7 @@ typedef enum bson_binary_subtype { /** * Node structure passed to the callback. */ -typedef struct bson_node_s -{ +typedef struct bson_node_s { char name[BSON_MAXNAME]; bson_type_t type; bson_binary_subtype_t subtype; @@ -96,8 +95,7 @@ typedef struct bson_decoder_s *bson_decoder_t; */ typedef int (* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node); -struct bson_decoder_s -{ +struct bson_decoder_s { int fd; bson_decoder_callback callback; void *private; @@ -143,8 +141,7 @@ __EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder); /** * Encoder state structure. */ -typedef struct bson_encoder_s -{ +typedef struct bson_encoder_s { int fd; } *bson_encoder_t; @@ -169,7 +166,7 @@ __EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, i */ __EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value); -/** +/** * Append a string to the encoded stream. */ __EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string); diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 99ee41508..9105d83cb 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -45,13 +45,13 @@ int16_t int16_t_from_bytes(uint8_t bytes[]) { - union { - uint8_t b[2]; - int16_t w; - } u; + union { + uint8_t b[2]; + int16_t w; + } u; - u.b[1] = bytes[0]; - u.b[0] = bytes[1]; + u.b[1] = bytes[0]; + u.b[0] = bytes[1]; - return u.w; + return u.w; } diff --git a/nuttx/configs/px4fmu/src/up_cpuload.c b/apps/systemlib/cpuload.c index bf867ae8b..20b711fa6 100644 --- a/nuttx/configs/px4fmu/src/up_cpuload.c +++ b/apps/systemlib/cpuload.c @@ -48,14 +48,11 @@ #include <sched.h> #include <arch/board/board.h> -#include <arch/board/up_hrt.h> -#include <arch/board/up_cpuload.h> +#include <drivers/drv_hrt.h> -#include "chip.h" -#include "up_arch.h" -#include "up_internal.h" -#include "stm32_internal.h" -#include "px4fmu-internal.h" +#include "cpuload.h" + +#ifdef CONFIG_SCHED_INSTRUMENTATION /**************************************************************************** * Definitions @@ -66,59 +63,59 @@ * Public Functions ****************************************************************************/ +__EXPORT void sched_note_start(FAR _TCB *tcb); +__EXPORT void sched_note_stop(FAR _TCB *tcb); +__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb); + /**************************************************************************** * Name: ****************************************************************************/ -#ifdef CONFIG_SCHED_INSTRUMENTATION - -struct system_load_s system_load; +__EXPORT struct system_load_s system_load; extern FAR _TCB *sched_gettcb(pid_t pid); -void cpuload_initialize_once(void); - void cpuload_initialize_once() { // if (!system_load.initialized) // { - system_load.start_time = hrt_absolute_time(); - int i; - for (i = 0; i < CONFIG_MAX_TASKS; i++) - { - system_load.tasks[i].valid = false; - } - system_load.total_count = 0; - - uint64_t now = hrt_absolute_time(); - - /* initialize idle thread statically */ - system_load.tasks[0].start_time = now; - system_load.tasks[0].total_runtime = 0; - system_load.tasks[0].curr_start_time = 0; - system_load.tasks[0].tcb = sched_gettcb(0); - system_load.tasks[0].valid = true; - system_load.total_count++; - - /* initialize init thread statically */ - system_load.tasks[1].start_time = now; - system_load.tasks[1].total_runtime = 0; - system_load.tasks[1].curr_start_time = 0; - system_load.tasks[1].tcb = sched_gettcb(1); - system_load.tasks[1].valid = true; - /* count init thread */ - system_load.total_count++; - // } + system_load.start_time = hrt_absolute_time(); + int i; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + system_load.tasks[i].valid = false; + } + + system_load.total_count = 0; + + uint64_t now = hrt_absolute_time(); + + /* initialize idle thread statically */ + system_load.tasks[0].start_time = now; + system_load.tasks[0].total_runtime = 0; + system_load.tasks[0].curr_start_time = 0; + system_load.tasks[0].tcb = sched_gettcb(0); + system_load.tasks[0].valid = true; + system_load.total_count++; + + /* initialize init thread statically */ + system_load.tasks[1].start_time = now; + system_load.tasks[1].total_runtime = 0; + system_load.tasks[1].curr_start_time = 0; + system_load.tasks[1].tcb = sched_gettcb(1); + system_load.tasks[1].valid = true; + /* count init thread */ + system_load.total_count++; + // } } -void sched_note_start(FAR _TCB *tcb ) +void sched_note_start(FAR _TCB *tcb) { /* search first free slot */ int i; - for (i = 1; i < CONFIG_MAX_TASKS; i++) - { - if (!system_load.tasks[i].valid) - { + + for (i = 1; i < CONFIG_MAX_TASKS; i++) { + if (!system_load.tasks[i].valid) { /* slot is available */ system_load.tasks[i].start_time = hrt_absolute_time(); system_load.tasks[i].total_runtime = 0; @@ -131,13 +128,12 @@ void sched_note_start(FAR _TCB *tcb ) } } -void sched_note_stop(FAR _TCB *tcb ) +void sched_note_stop(FAR _TCB *tcb) { int i; - for (i = 1; i < CONFIG_MAX_TASKS; i++) - { - if (system_load.tasks[i].tcb->pid == tcb->pid) - { + + for (i = 1; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].tcb->pid == tcb->pid) { /* mark slot as fee */ system_load.tasks[i].valid = false; system_load.tasks[i].total_runtime = 0; @@ -155,26 +151,23 @@ void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb) /* Kind of inefficient: find both tasks and update times */ uint8_t both_found = 0; - for (int i = 0; i < CONFIG_MAX_TASKS; i++) - { + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { /* Task ending its current scheduling run */ - if (system_load.tasks[i].tcb->pid == pFromTcb->pid) - { + if (system_load.tasks[i].tcb->pid == pFromTcb->pid) { //if (system_load.tasks[i].curr_start_time != 0) { system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time; } both_found++; - } - else if (system_load.tasks[i].tcb->pid == pToTcb->pid) - { + + } else if (system_load.tasks[i].tcb->pid == pToTcb->pid) { system_load.tasks[i].curr_start_time = new_time; both_found++; } /* Do only iterate as long as needed */ - if (both_found == 2) - { + if (both_found == 2) { break; } } diff --git a/nuttx/configs/px4fmu/include/up_cpuload.h b/apps/systemlib/cpuload.h index b61c5c550..a97047ea8 100644 --- a/nuttx/configs/px4fmu/include/up_cpuload.h +++ b/apps/systemlib/cpuload.h @@ -31,11 +31,12 @@ * ****************************************************************************/ -#ifndef UP_CPULOAD_H_ -#define UP_CPULOAD_H_ +#pragma once #ifdef CONFIG_SCHED_INSTRUMENTATION +__BEGIN_DECLS + #include <nuttx/sched.h> struct system_load_taskinfo_s { @@ -55,8 +56,8 @@ struct system_load_s { int sleeping_count; }; -void cpuload_initialize_once(void); +__EXPORT extern struct system_load_s system_load; -#endif +__EXPORT void cpuload_initialize_once(void); #endif diff --git a/apps/systemlib/err.c b/apps/systemlib/err.c index f22c5ed0d..3011743a1 100644 --- a/apps/systemlib/err.c +++ b/apps/systemlib/err.c @@ -79,8 +79,10 @@ warnerr_core(int errcode, const char *fmt, va_list args) /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) errcode = -errcode; + if (errcode < NOCODE) fprintf(stderr, ": %s", strerror(errcode)); + fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC lib_lowprintf("%s: ", getprogname()); @@ -89,8 +91,10 @@ warnerr_core(int errcode, const char *fmt, va_list args) /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) errcode = -errcode; + if (errcode < NOCODE) lib_lowprintf(": %s", strerror(errcode)); + lib_lowprintf("\n"); #endif } @@ -144,7 +148,7 @@ verrx(int exitcode, const char *fmt, va_list args) } void -warn(const char *fmt, ...) +warn(const char *fmt, ...) { va_list args; @@ -153,13 +157,13 @@ warn(const char *fmt, ...) } void -vwarn(const char *fmt, va_list args) +vwarn(const char *fmt, va_list args) { warnerr_core(errno, fmt, args); } void -warnc(int errcode, const char *fmt, ...) +warnc(int errcode, const char *fmt, ...) { va_list args; @@ -168,13 +172,13 @@ warnc(int errcode, const char *fmt, ...) } void -vwarnc(int errcode, const char *fmt, va_list args) +vwarnc(int errcode, const char *fmt, va_list args) { warnerr_core(errcode, fmt, args); } void -warnx(const char *fmt, ...) +warnx(const char *fmt, ...) { va_list args; @@ -183,7 +187,7 @@ warnx(const char *fmt, ...) } void -vwarnx(const char *fmt, va_list args) +vwarnx(const char *fmt, va_list args) { warnerr_core(NOCODE, fmt, args); } diff --git a/apps/systemlib/err.h b/apps/systemlib/err.h index f798b97e7..ca13d6265 100644 --- a/apps/systemlib/err.h +++ b/apps/systemlib/err.h @@ -36,30 +36,30 @@ * * Simple error/warning functions, heavily inspired by the BSD functions of * the same names. - * + * * The err() and warn() family of functions display a formatted error * message on the standard error output. In all cases, the last * component of the program name, a colon character, and a space are * output. If the fmt argument is not NULL, the printf(3)-like formatted * error message is output. The output is terminated by a newline * character. - * + * * The err(), errc(), verr(), verrc(), warn(), warnc(), vwarn(), and * vwarnc() functions append an error message obtained from strerror(3) * based on a supplied error code value or the global variable errno, * preceded by another colon and space unless the fmt argument is NULL. - * + * * In the case of the errc(), verrc(), warnc(), and vwarnc() functions, * the code argument is used to look up the error message. - * + * * The err(), verr(), warn(), and vwarn() functions use the global * variable errno to look up the error message. - * + * * The errx() and warnx() functions do not append an error message. - * + * * The err(), verr(), errc(), verrc(), errx(), and verrx() functions do * not return, but exit with the value of the argument eval. - * + * */ #ifndef _SYSTEMLIB_ERR_H @@ -71,18 +71,18 @@ __BEGIN_DECLS __EXPORT const char *getprogname(void); -__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn,format(printf,2, 3))); -__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn,format(printf,2, 0))); -__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn,format(printf,3, 4))); -__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn,format(printf,3, 0))); -__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn,format(printf,2, 3))); -__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn,format(printf,2, 0))); -__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf,1, 2))); -__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf,1, 0))); -__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf,2, 3))); -__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf,2, 0))); -__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf,1, 2))); -__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf,1, 0))); +__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); +__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); +__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn, format(printf, 3, 4))); +__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn, format(printf, 3, 0))); +__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); +__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); +__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf, 1, 2))); +__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); +__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf, 2, 3))); +__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf, 2, 0))); +__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf, 1, 2))); +__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); __END_DECLS diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index bc467bfa3..789368fbd 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -89,7 +89,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) { // This function returns the distance to the nearest point on the track line. Distance is positive if current -// position is right of the track and negative if left of the track as seen from a point on the track line +// position is right of the track and negative if left of the track as seen from a point on the track line // headed towards the end point. crosstrack_error_s return_var; @@ -97,43 +97,46 @@ __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, float bearing_end; float bearing_track; float bearing_diff; - + return_var.error = true; // Set error flag, cleared when valid result calculated. return_var.past_end = false; return_var.distance = 0.0f; return_var.bearing = 0.0f; - + // Return error if arguments are bad - if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var; - + if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var; + bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); bearing_diff = bearing_track - bearing_end; bearing_diff = _wrapPI(bearing_diff); - + // Return past_end = true if past end point of line - if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { + if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { return_var.past_end = true; return_var.error = false; return return_var; } - + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - return_var.distance = (dist_to_end)*sin(bearing_diff); - if(sin(bearing_diff) >=0 ) { + return_var.distance = (dist_to_end) * sin(bearing_diff); + + if (sin(bearing_diff) >= 0) { return_var.bearing = _wrapPI(bearing_track - M_PI_2_F); + } else { return_var.bearing = _wrapPI(bearing_track + M_PI_2_F); } + return_var.error = false; - + return return_var; } -__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep) +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and @@ -146,68 +149,76 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, float bearing_sector_end; float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); bool in_sector; - + return_var.error = true; // Set error flag, cleared when valid result calculated. return_var.past_end = false; return_var.distance = 0.0f; return_var.bearing = 0.0f; - + // Return error if arguments are bad - if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var; - - - if(arc_sweep >= 0) { + if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var; + + + if (arc_sweep >= 0) { bearing_sector_start = arc_start_bearing; bearing_sector_end = arc_start_bearing + arc_sweep; - if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F; + + if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; + } else { bearing_sector_end = arc_start_bearing; bearing_sector_start = arc_start_bearing - arc_sweep; - if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F; + + if (bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F; } + in_sector = false; - + // Case where sector does not span zero - if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; - + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + // Case where sector does span zero - if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true; - + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; + // If in the sector then calculate distance and bearing to closest point - if(in_sector) { + if (in_sector) { return_var.past_end = false; float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); - - if(dist_to_center <= radius) { + + if (dist_to_center <= radius) { return_var.distance = radius - dist_to_center; return_var.bearing = bearing_now + M_PI_F; + } else { return_var.distance = dist_to_center - radius; return_var.bearing = bearing_now; } - - // If out of the sector then calculate dist and bearing to start or end point + + // If out of the sector then calculate dist and bearing to start or end point + } else { - - // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) - // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to - // calculate the position of the start and end points. We should not be doing this often - // as this function generally will not be called repeatedly when we are out of the sector. - - // TO DO - this is messed up and won't compile + + // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) + // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to + // calculate the position of the start and end points. We should not be doing this often + // as this function generally will not be called repeatedly when we are out of the sector. + + // TO DO - this is messed up and won't compile float start_disp_x = radius * sin(arc_start_bearing); float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep)); - float lon_start = lon_now + start_disp_x/111111.0d; - float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d; - float lon_end = lon_now + end_disp_x/111111.0d; - float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d; + float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); + float lon_start = lon_now + start_disp_x / 111111.0d; + float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; + float lon_end = lon_now + end_disp_x / 111111.0d; + float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - if(dist_to_start < dist_to_end) { + + if (dist_to_start < dist_to_end) { return_var.distance = dist_to_start; return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + } else { return_var.past_end = true; return_var.distance = dist_to_end; @@ -215,10 +226,10 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, } } - + return_var.bearing = _wrapPI(return_var.bearing); return_var.error = false; - return return_var; + return return_var; } float _wrapPI(float bearing) @@ -227,21 +238,25 @@ float _wrapPI(float bearing) while (bearing > M_PI_F) { bearing = bearing - M_TWOPI_F; } + while (bearing <= -M_PI_F) { bearing = bearing + M_TWOPI_F; } + return bearing; } - + float _wrap2PI(float bearing) { while (bearing >= M_TWOPI_F) { bearing = bearing - M_TWOPI_F; } + while (bearing < 0.0f) { bearing = bearing + M_TWOPI_F; } + return bearing; } @@ -251,23 +266,26 @@ float _wrap180(float bearing) while (bearing > 180.0f) { bearing = bearing - 360.0f; } + while (bearing <= -180.0f) { bearing = bearing + 360.0f; } + return bearing; } - + float _wrap360(float bearing) { while (bearing >= 360.0f) { bearing = bearing - 360.0f; } + while (bearing < 0.0f) { bearing = bearing + 360.0f; } + return bearing; } - -
\ No newline at end of file + diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h index 205afc79e..c98b4c85d 100644 --- a/apps/systemlib/geo/geo.h +++ b/apps/systemlib/geo/geo.h @@ -44,8 +44,8 @@ * @author Lorenz Meier <lm@inf.ethz.ch> * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> */ - - + + #include <stdbool.h> typedef struct { @@ -59,14 +59,14 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -// +// __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); -__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep); - +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); + float _wrap180(float bearing); -float _wrap360(float bearing); +float _wrap360(float bearing); float _wrapPI(float bearing); float _wrap2PI(float bearing);
\ No newline at end of file diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index b56226c03..6c1bbe469 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -63,7 +63,7 @@ float Mixer::get_control(uint8_t group, uint8_t index) { float value; - + _control_cb(_cb_handle, group, index, value); return value; diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 44a33bf8b..004151235 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -224,29 +224,35 @@ mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, co if (!strcmp(geomname, "4+")) { geometry = MultirotorMixer::QUAD_PLUS; + } else if (!strcmp(geomname, "4x")) { geometry = MultirotorMixer::QUAD_X; + } else if (!strcmp(geomname, "6+")) { geometry = MultirotorMixer::HEX_PLUS; + } else if (!strcmp(geomname, "6x")) { geometry = MultirotorMixer::HEX_X; + } else if (!strcmp(geomname, "8+")) { geometry = MultirotorMixer::OCTA_PLUS; + } else if (!strcmp(geomname, "8x")) { geometry = MultirotorMixer::OCTA_X; + } else { debug("unrecognised geometry '%s'", geomname); return nullptr; } return new MultirotorMixer( - control_cb, - cb_handle, - geometry, - s[0] / 10000.0f, - s[1] / 10000.0f, - s[2] / 10000.0f, - s[3] / 10000.0f); + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f); } int diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index e2576a848..0e714ed50 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -57,7 +57,7 @@ #define CW (-1.0f) #define CCW (1.0f) -namespace +namespace { /* @@ -167,17 +167,21 @@ MultirotorMixer::mix(float *outputs, unsigned space) pitch * _rotors[i].pitch_scale + yaw * _rotors[i].yaw_scale + thrust; + if (tmp > max) max = tmp; + outputs[i] = tmp; } /* scale values into the -1.0 - 1.0 range */ if (max > 1.0f) { fixup_scale = 2.0f / max; + } else { fixup_scale = 2.0f; } + for (unsigned i = 0; i < _rotor_count; i++) outputs[i] = -1.0f + (outputs[i] * fixup_scale); diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 9e886ea65..eeb867f11 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -50,7 +50,7 @@ #include <sys/stat.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "systemlib/param/param.h" #include "systemlib/uthash/utarray.h" @@ -189,6 +189,7 @@ param_notify_changes(void) */ if (param_topic == -1) { param_topic = orb_advertise(ORB_ID(parameter_update), &pup); + } else { orb_publish(ORB_ID(parameter_update), param_topic, &pup); } @@ -455,6 +456,7 @@ param_reset(param_t param) utarray_erase(param_values, pos, 1); } } + param_unlock(); if (s != NULL) @@ -560,8 +562,7 @@ out: return result; } -struct param_import_state -{ +struct param_import_state { bool mark_saved; }; @@ -689,9 +690,10 @@ param_import_internal(int fd, bool mark_saved) do { result = bson_decoder_next(&decoder); - } while(result > 0); + } while (result > 0); out: + if (result < 0) debug("BSON error decoding parameters"); diff --git a/apps/systemlib/perf_counter.c b/apps/systemlib/perf_counter.c index e25e548f0..e6c49d432 100644 --- a/apps/systemlib/perf_counter.c +++ b/apps/systemlib/perf_counter.c @@ -40,7 +40,7 @@ #include <stdlib.h> #include <stdio.h> #include <sys/queue.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include "perf_counter.h" diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index f9b50d030..7e277cdc7 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -43,7 +43,7 @@ #include <math.h> __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode) + float limit, uint8_t mode) { pid->kp = kp; pid->ki = ki; @@ -65,30 +65,35 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float if (isfinite(kp)) { pid->kp = kp; + } else { ret = 1; } if (isfinite(ki)) { pid->ki = ki; + } else { ret = 1; } if (isfinite(kd)) { pid->kd = kd; + } else { ret = 1; } if (isfinite(intmax)) { pid->intmax = intmax; + } else { ret = 1; } - + if (isfinite(limit)) { pid->limit = limit; + } else { ret = 1; } @@ -121,17 +126,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo goto start */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) - { + if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { return pid->last_output; } float i, d; pid->sp = sp; - + // Calculated current error value float error = pid->sp - val; - + if (isfinite(error)) { // Why is this necessary? DEW pid->error_previous = error; } @@ -140,30 +144,36 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo if (pid->mode == PID_MODE_DERIVATIV_CALC) { d = (error - pid->error_previous) / dt; + } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; + } else { d = 0.0f; } // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if( fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabs(i) > pid->intmax ) - { + + if (fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || + fabs(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; + } else { if (!isfinite(i)) { i = 0; } + pid->integral = i; pid->saturated = 0; } // Calculate the output. Limit output magnitude to pid->limit float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); + if (output > pid->limit) output = pid->limit; + if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { diff --git a/apps/systemlib/ppm_decode.c b/apps/systemlib/ppm_decode.c new file mode 100644 index 000000000..a5d2f738d --- /dev/null +++ b/apps/systemlib/ppm_decode.c @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * 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 ppm_decode.c + * + * PPM input decoder. + */ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <string.h> + +#include <drivers/drv_hrt.h> + +#include "ppm_decode.h" + +/* + * PPM decoder tuning parameters. + * + * The PPM decoder works as follows. + * + * Initially, the decoder waits in the UNSYNCH state for two edges + * separated by PPM_MIN_START. Once the second edge is detected, + * the decoder moves to the ARM state. + * + * The ARM state expects an edge within PPM_MAX_PULSE_WIDTH, being the + * timing mark for the first channel. If this is detected, it moves to + * the INACTIVE state. + * + * The INACTIVE phase waits for and discards the next edge, as it is not + * significant. Once the edge is detected, it moves to the ACTIVE stae. + * + * The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when + * received calculates the time from the previous mark and records + * this time as the value for the next channel. + * + * If at any time waiting for an edge, the delay from the previous edge + * exceeds PPM_MIN_START the frame is deemed to have ended and the recorded + * values are advertised to clients. + */ +#define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */ +#define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ +#define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ +#define PPM_MIN_START 2500 /* shortest valid start gap */ + +/* Input timeout - after this interval we assume signal is lost */ +#define PPM_INPUT_TIMEOUT 100 * 1000 /* 100ms */ + +/* Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 3 /* should be less than the input timeout */ + +/* decoded PPM buffer */ +#define PPM_MIN_CHANNELS 4 +#define PPM_MAX_CHANNELS 12 + +/* + * Public decoder state + */ +uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +unsigned ppm_decoded_channels; +hrt_abstime ppm_last_valid_decode; + +static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/* PPM decoder state machine */ +static struct { + uint16_t last_edge; /* last capture time */ + uint16_t last_mark; /* last significant edge */ + unsigned next_channel; + unsigned count_max; + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} ppm; + + +void +ppm_input_init(unsigned count_max) +{ + ppm_decoded_channels = 0; + ppm_last_valid_decode = 0; + + memset(&ppm, 0, sizeof(ppm)); + ppm.count_max = count_max; +} + +void +ppm_input_decode(bool reset, unsigned count) +{ + uint16_t width; + uint16_t interval; + unsigned i; + + /* if we missed an edge, we have to give up */ + if (reset) + goto error; + + /* how long since the last edge? */ + width = count - ppm.last_edge; + + if (count < ppm.last_edge) + width += ppm.count_max; /* handle wrapped count */ + + ppm.last_edge = count; + + /* + * If this looks like a start pulse, then push the last set of values + * and reset the state machine. + * + * Note that this is not a "high performance" design; it implies a whole + * frame of latency between the pulses being received and their being + * considered valid. + */ + if (width >= PPM_MIN_START) { + + /* + * If the number of channels changes unexpectedly, we don't want + * to just immediately jump on the new count as it may be a result + * of noise or dropped edges. Instead, take a few frames to settle. + */ + if (ppm.next_channel != ppm_decoded_channels) { + static unsigned new_channel_count; + static unsigned new_channel_holdoff; + + if (new_channel_count != ppm.next_channel) { + /* start the lock counter for the new channel count */ + new_channel_count = ppm.next_channel; + new_channel_holdoff = PPM_CHANNEL_LOCK; + + } else if (new_channel_holdoff > 0) { + /* this frame matched the last one, decrement the lock counter */ + new_channel_holdoff--; + + } else { + /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */ + ppm_decoded_channels = new_channel_count; + new_channel_count = 0; + } + + } else { + /* frame channel count matches expected, let's use it */ + if (ppm.next_channel > PPM_MIN_CHANNELS) { + for (i = 0; i < ppm.next_channel; i++) + ppm_buffer[i] = ppm_temp_buffer[i]; + + ppm_last_valid_decode = hrt_absolute_time(); + } + } + + /* reset for the next frame */ + ppm.next_channel = 0; + + /* next edge is the reference for the first channel */ + ppm.phase = ARM; + + return; + } + + switch (ppm.phase) { + case UNSYNCH: + /* we are waiting for a start pulse - nothing useful to do here */ + return; + + case ARM: + + /* we expect a pulse giving us the first mark */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + + /* record the mark timing, expect an inactive edge */ + ppm.last_mark = count; + ppm.phase = INACTIVE; + return; + + case INACTIVE: + /* this edge is not interesting, but now we are ready for the next mark */ + ppm.phase = ACTIVE; + + /* note that we don't bother looking at the timing of this edge */ + + return; + + case ACTIVE: + + /* we expect a well-formed pulse */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + + /* determine the interval from the last mark */ + interval = count - ppm.last_mark; + ppm.last_mark = count; + + /* if the mark-mark timing is out of bounds, abandon the frame */ + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) + goto error; + + /* if we have room to store the value, do so */ + if (ppm.next_channel < PPM_MAX_CHANNELS) + ppm_temp_buffer[ppm.next_channel++] = interval; + + ppm.phase = INACTIVE; + return; + + } + + /* the state machine is corrupted; reset it */ + +error: + /* we don't like the state of the decoder, reset it and try again */ + ppm.phase = UNSYNCH; + ppm_decoded_channels = 0; +} + diff --git a/apps/systemlib/ppm_decode.h b/apps/systemlib/ppm_decode.h new file mode 100644 index 000000000..c2b24321a --- /dev/null +++ b/apps/systemlib/ppm_decode.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * 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 ppm_decode.h + * + * PPM input decoder. + */ + +#pragma once + +#include <drivers/drv_hrt.h> + +/** + * Maximum number of channels that we will decode. + */ +#define PPM_MAX_CHANNELS 12 + +__BEGIN_DECLS + +/* + * PPM decoder state + */ +__EXPORT extern uint16_t ppm_buffer[]; /**< decoded PPM channel values */ +__EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */ +__EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */ + +/** + * Initialise the PPM input decoder. + * + * @param count_max The maximum value of the counter passed to + * ppm_input_decode, used to determine how to + * handle counter wrap. + */ +__EXPORT void ppm_input_init(unsigned count_max); + +/** + * Inform the decoder of an edge on the PPM input. + * + * This function can be registered with the HRT as the PPM edge handler. + * + * @param reset If set, the edge detector has missed one or + * more edges and the decoder needs to be reset. + * @param count A microsecond timestamp corresponding to the + * edge, in the range 0-count_max. This value + * is expected to wrap. + */ +__EXPORT void ppm_input_decode(bool reset, unsigned count); + +__END_DECLS
\ No newline at end of file diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c index 94a5283f0..50ac62464 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -131,7 +131,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma pid = task_create(name, priority, stack_size, entry, argv); if (pid > 0) { - + /* configure the scheduler */ struct sched_param param; @@ -140,6 +140,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma /* XXX do any other private task accounting here before the task starts */ } + sched_unlock(); return pid; @@ -157,15 +158,18 @@ int fmu_get_board_info(struct fmu_board_info_s *info) statres = stat("/dev/bma180", &sb); if (statres == OK) { - /* BMA180 indicates a v1.5-v1.6 board */ - strcpy(info->board_name, "FMU v1.6"); - info->board_version = 16; + /* BMA180 indicates a v1.5-v1.6 board */ + strcpy(info->board_name, "FMU v1.6"); + info->board_version = 16; + } else { statres = stat("/dev/accel", &sb); + if (statres == OK) { /* MPU-6000 indicates a v1.7+ board */ strcpy(info->board_name, "FMU v1.7"); info->board_version = 17; + } else { /* If no BMA and no MPU is present, it is a v1.3 board */ strcpy(info->board_name, "FMU v1.3"); diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 72a60f871..532e54b8e 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -56,7 +56,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/up_hrt.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_orb_dev.h> diff --git a/nuttx/configs/px4fmu/include/up_pwm_servo.h b/nuttx/configs/px4fmu/include/up_pwm_servo.h deleted file mode 100644 index 0b35035d5..000000000 --- a/nuttx/configs/px4fmu/include/up_pwm_servo.h +++ /dev/null @@ -1,117 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/* - * Low-level PWM servo control. - * - * The pwm_servo module supports servos connected to STM32 timer - * blocks. - * - * On PX4FMU, the outputs are: - * - * 0 : USART2/multi CTS - * 1 : USART2/multi RTS - * 2 : USART2/multi TX - * 3 : USART2/multi RX - * 4 : CAN2 TX - * 5 : CAN2 RX - */ - -#ifndef UP_PWM_SERVO_H -#define UP_PWM_SERVO_H - -typedef uint16_t servo_position_t; - - -#if defined(__cplusplus) -extern "C" { -#endif - -/** - * 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. - */ -extern int up_pwm_servo_init(uint32_t channel_mask); - -/** - * De-initialise the PWM servo outputs. - */ -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. - */ -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. - */ -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. - */ -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. - */ -extern servo_position_t up_pwm_servo_get(unsigned channel); - -#if defined(__cplusplus) -} -#endif - -#endif /* UP_PWM_SERVO_H */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 6c9a98ae7..1b5574681 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -76,27 +76,27 @@ CONFIGURED_APPS += sensors CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_pos_control -CONFIGURED_APPS += px4/attitude_estimator_bm CONFIGURED_APPS += fixedwing_control CONFIGURED_APPS += fixedwing_att_control CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator CONFIGURED_APPS += attitude_estimator_ekf -CONFIGURED_APPS += px4/ground_estimator # Hacking tools #CONFIGURED_APPS += system/i2c #CONFIGURED_APPS += tools/i2c_dev # Communication and Drivers +CONFIGURED_APPS += drivers/boards/px4fmu CONFIGURED_APPS += drivers/device CONFIGURED_APPS += drivers/ms5611 CONFIGURED_APPS += drivers/hmc5883 CONFIGURED_APPS += drivers/mpu6000 CONFIGURED_APPS += drivers/bma180 CONFIGURED_APPS += drivers/l3gd20 +CONFIGURED_APPS += drivers/px4io +CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += drivers/stm32/tone_alarm -CONFIGURED_APPS += px4/px4io/driver CONFIGURED_APPS += px4/fmu # Testing stuff diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index 338f364fc..d88040013 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -40,23 +40,9 @@ CFLAGS += -I$(TOPDIR)/sched ASRCS = AOBJS = $(ASRCS:.S=$(OBJEXT)) -CSRCS = up_boot.c up_leds.c up_spi.c up_hrt.c \ +CSRCS = up_leds.c \ drv_gpio.c \ - drv_led.c drv_eeprom.c \ - up_pwm_servo.c up_usbdev.c \ - up_cpuload.c - -ifeq ($(CONFIG_NSH_ARCHINIT),y) -CSRCS += up_nsh.c -endif - -ifeq ($(CONFIG_ADC),y) -CSRCS += up_adc.c -endif - -ifeq ($(CONFIG_CAN),y) -CSRCS += up_can.c -endif + drv_led.c drv_eeprom.c COBJS = $(CSRCS:.c=$(OBJEXT)) diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c deleted file mode 100644 index 763207d3a..000000000 --- a/nuttx/configs/px4fmu/src/up_nsh.c +++ /dev/null @@ -1,280 +0,0 @@ -/**************************************************************************** - * config/px4fmu/src/up_nsh.c - * arch/arm/src/board/up_nsh.c - * - * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt <spudmonkey@racsa.co.cr> - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * 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/up_hrt.h> -#include <arch/board/up_cpuload.h> -#include <arch/board/up_adc.h> -#include <arch/board/board.h> -#include <arch/board/drv_led.h> -#include <arch/board/drv_eeprom.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 - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * 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 -int matherr(struct __exception *e) { - return 1; -} -#else -int matherr(struct exception *e) { - return 1; -} -#endif - -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; -} |