fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 07:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Thomas Gubler
2012-10-26 21:21:07 +02:00
parent b9d03c7c27 241a0d8653
commit f321e7f419
133 changed files with 2423 additions and 4388 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+1 -1
View File
@@ -117,7 +117,7 @@ upload: $(FIRMWARE_BUNDLE) $(UPLOADER)
upload-jtag-px4fmu:
@echo Attempting to flash PX4FMU board via JTAG
@openocd -f interface/olimex-jtag-tiny.cfg -f Tools/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
@openocd -f interface/olimex-jtag-tiny.cfg -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
#
# Hacks and fixups
ROMFS/logging/logconv.m
+69 -2
View File
@@ -1,5 +1,72 @@
clear all
clc
close all
%%%%%%%%%%%%%%%%%%%%%%%
% SYSTEM VECTOR
%
% All measurements in NED frame
%
% uint64_t timestamp;
% float gyro[3]; in rad/s
% float accel[3]; in m/s^2
% float mag[3]; in Gauss
% float baro; pressure in millibar
% float baro_alt; altitude above MSL in meters
% float baro_temp; in degrees celcius
% float control[4]; roll, pitch, yaw [-1..1], thrust [0..1]
% float actuators[8]; motor 1-8, in motor units (PWM: 1000-2000,
% AR.Drone: 0-512
% float vbat; battery voltage in volt
% float adc[3]; remaining auxiliary ADC ports in volt
% float local_position
% int32 gps_raw_position
if exist('sysvector.bin', 'file')
% Read actuators file
myFile = java.io.File('sysvector.bin')
fileSize = length(myFile)
fid = fopen('sysvector.bin', 'r');
elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
for i=1:elements
% timestamp
sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% actuators 1-16
% quadrotor: motor 1-4 on the first four positions
sysvector(i, 2:32) = fread(fid, 28+3, 'float', 'ieee-le');
sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
end
sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
sysvector_minutes = sysvector_interval_seconds / 60
% Normalize time
sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
% Create some basic plots
% Remove zero rows from GPS
gps = sysvector(:,33:35);
gps(~any(gps,2), :) = [];
all_data = figure('Name', 'GPS RAW');
gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
all_data = figure('Name', 'Complete Log Data (exc. GPS)');
plot(sysvector(:,1), sysvector(:,2:32));
actuator_inputs = figure('Name', 'Attitude controller outputs');
plot(sysvector(:,1), sysvector(:,14:17));
legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
actuator_outputs = figure('Name', 'Actuator outputs');
plot(sysvector(:,1), sysvector(:,18:25));
legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
end
if exist('actuator_outputs0.bin', 'file')
% Read actuators file
@@ -9,7 +76,7 @@ if exist('actuator_outputs0.bin', 'file')
fid = fopen('actuator_outputs0.bin', 'r');
elements = int64(fileSize./(16*4+8))
for i=1:(elements-2)
for i=1:elements
% timestamp
actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% actuators 1-16
apps/ardrone_interface/ardrone_interface.c
+1 -1
View File
@@ -51,7 +51,7 @@
#include <time.h>
#include <systemlib/err.h>
#include <sys/prctl.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
apps/ardrone_interface/ardrone_motor_control.c
+1 -1
View File
@@ -42,7 +42,7 @@
#include <fcntl.h>
#include <unistd.h>
#include <drivers/drv_gpio.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
apps/attitude_estimator_ekf/attitudeKalmanfilter.m
-108
View File
@@ -1,108 +0,0 @@
function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
%#codegen
%Extended Attitude Kalmanfilter
%
%state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]'
%measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]'
%knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW]
%
%[x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
%
%Example....
%
% $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $
%%define the matrices
acc_ProcessNoise=knownConst(1);
mag_ProcessNoise=knownConst(2);
ratesOffset_ProcessNoise=knownConst(3);
rates_ProcessNoise=knownConst(4);
acc_MeasurementNoise=knownConst(5);
mag_MeasurementNoise=knownConst(6);
rates_MeasurementNoise=knownConst(7);
%process noise covariance matrix
Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3);
zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3);
zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3);
zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise];
%measurement noise covariance matrix
R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3);
zeros(3), eye(3)*mag_MeasurementNoise, zeros(3);
zeros(3), zeros(3), eye(3)*rates_MeasurementNoise];
%observation matrix
H_k=[ eye(3), zeros(3), zeros(3), zeros(3);
zeros(3), eye(3), zeros(3), zeros(3);
zeros(3), zeros(3), eye(3), eye(3)];
%compute A(t,w)
%x_aposteriori_k[10,11,12] should be [p,q,r]
%R_temp=[1,-r, q
% r, 1, -p
% -q, p, 1]
R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11);
dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10);
-dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1];
%strange, should not be transposed
A_pred=[R_temp', zeros(3), zeros(3), zeros(3);
zeros(3), R_temp', zeros(3), zeros(3);
zeros(3), zeros(3), eye(3), zeros(3);
zeros(3), zeros(3), zeros(3), eye(3)];
%%prediction step
x_apriori=A_pred*x_aposteriori_k;
%linearization
acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2);
-dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1);
dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0];
mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5);
-dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4);
dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0];
A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat';
zeros(3), R_temp', zeros(3), mag_temp_mat';
zeros(3), zeros(3), eye(3), zeros(3);
zeros(3), zeros(3), zeros(3), eye(3)];
P_apriori=A_lin*P_aposteriori_k*A_lin'+Q;
%%update step
y_k=z_k-H_k*x_apriori;
S_k=H_k*P_apriori*H_k'+R;
K_k=(P_apriori*H_k'/(S_k));
x_aposteriori=x_apriori+K_k*y_k;
P_aposteriori=(eye(12)-K_k*H_k)*P_apriori;
%%Rotation matrix generation
earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3));
earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6)));
earth_y=cross(earth_x,earth_z);
Rot_matrix=[earth_x,earth_y,earth_z];
apps/attitude_estimator_ekf/attitudeKalmanfilter.prj
-270
View File
@@ -1,270 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<deployment-project>
<configuration target="target.matlab.ecoder" target-name="MATLAB Embedded Coder Target" name="attitudeKalmanfilter" location="F:\codegenerationMatlabAttFilter" file="F:\codegenerationMatlabAttFilter\attitudeKalmanfilter.prj" build-checksum="2344418414">
<param.mex.general.TargetLang>option.general.TargetLang.C</param.mex.general.TargetLang>
<param.mex.general.IntegrityChecks>true</param.mex.general.IntegrityChecks>
<param.mex.general.ResponsivenessChecks>true</param.mex.general.ResponsivenessChecks>
<param.mex.general.EnableBLAS>true</param.mex.general.EnableBLAS>
<param.mex.general.ExtrinsicCalls>true</param.mex.general.ExtrinsicCalls>
<param.mex.general.EchoExpressions>true</param.mex.general.EchoExpressions>
<param.mex.general.EnableDebugging>true</param.mex.general.EnableDebugging>
<param.mex.general.SaturateOnIntegerOverflow>true</param.mex.general.SaturateOnIntegerOverflow>
<param.mex.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.mex.general.FilePartitionMethod>
<param.mex.general.GlobalDataSyncMethod>option.general.GlobalDataSyncMethod.SyncAlways</param.mex.general.GlobalDataSyncMethod>
<param.mex.general.EnableVariableSizing>true</param.mex.general.EnableVariableSizing>
<param.mex.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.mex.general.DynamicMemoryAllocation>
<param.mex.paths.working>option.paths.working.project</param.mex.paths.working>
<param.mex.paths.working.specified />
<param.mex.paths.build>option.paths.build.project</param.mex.paths.build>
<param.mex.paths.build.specified />
<param.mex.paths.search />
<param.mex.report.GenerateReport>true</param.mex.report.GenerateReport>
<param.mex.report.LaunchReport>false</param.mex.report.LaunchReport>
<param.mex.comments.GenerateComments>true</param.mex.comments.GenerateComments>
<param.mex.comments.MATLABSourceComments>true</param.mex.comments.MATLABSourceComments>
<param.mex.symbols.ReservedNameArray />
<param.mex.customcode.CustomSourceCode />
<param.mex.customcode.CustomHeaderCode />
<param.mex.customcode.CustomInitializer />
<param.mex.customcode.CustomTerminator />
<param.mex.customcode.CustomInclude />
<param.mex.customcode.CustomSource />
<param.mex.customcode.CustomLibrary />
<param.mex.PostCodeGenCommand />
<param.mex.EnableMemcpy>true</param.mex.EnableMemcpy>
<param.mex.MemcpyThreshold>64</param.mex.MemcpyThreshold>
<param.mex.InitFltsAndDblsToZero>true</param.mex.InitFltsAndDblsToZero>
<param.mex.InlineThreshold>10</param.mex.InlineThreshold>
<param.mex.InlineThresholdMax>200</param.mex.InlineThresholdMax>
<param.mex.InlineStackLimit>4000</param.mex.InlineStackLimit>
<param.mex.StackUsageMax>200000</param.mex.StackUsageMax>
<param.mex.ConstantFoldingTimeout>10000</param.mex.ConstantFoldingTimeout>
<param.grt.general.TargetLang>option.general.TargetLang.C</param.grt.general.TargetLang>
<param.general.target.description>MATLAB Embedded Coder Target</param.general.target.description>
<param.grt.CCompilerOptimization>option.CCompilerOptimization.On</param.grt.CCompilerOptimization>
<param.grt.CCompilerCustomOptimizations />
<param.grt.general.GenerateMakefile>true</param.grt.general.GenerateMakefile>
<param.grt.general.MakeCommand>make_rtw</param.grt.general.MakeCommand>
<param.grt.general.TemplateMakefile>default_tmf</param.grt.general.TemplateMakefile>
<param.grt.general.SaturateOnIntegerOverflow>true</param.grt.general.SaturateOnIntegerOverflow>
<param.grt.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.grt.general.FilePartitionMethod>
<param.grt.general.EnableVariableSizing>true</param.grt.general.EnableVariableSizing>
<param.grt.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.grt.general.DynamicMemoryAllocation>
<param.grt.paths.working>option.paths.working.project</param.grt.paths.working>
<param.grt.paths.working.specified />
<param.grt.paths.build>option.paths.build.project</param.grt.paths.build>
<param.grt.paths.build.specified />
<param.grt.paths.search />
<param.grt.report.GenerateReport>true</param.grt.report.GenerateReport>
<param.grt.report.LaunchReport>false</param.grt.report.LaunchReport>
<param.grt.GenerateComments>true</param.grt.GenerateComments>
<param.grt.MATLABSourceComments>true</param.grt.MATLABSourceComments>
<param.ert.MATLABFcnDesc>false</param.ert.MATLABFcnDesc>
<param.ert.CustomSymbolStrGlobalVar>$M$N</param.ert.CustomSymbolStrGlobalVar>
<param.ert.CustomSymbolStrType>$M$N</param.ert.CustomSymbolStrType>
<param.ert.CustomSymbolStrField>$M$N</param.ert.CustomSymbolStrField>
<param.ert.CustomSymbolStrFcn>$M$N</param.ert.CustomSymbolStrFcn>
<param.ert.CustomSymbolStrTmpVar>$M$N</param.ert.CustomSymbolStrTmpVar>
<param.ert.CustomSymbolStrMacro>$M$N</param.ert.CustomSymbolStrMacro>
<param.ert.CustomSymbolStrEMXArray>emxArray_$M$N</param.ert.CustomSymbolStrEMXArray>
<param.grt.MaxIdLength>32</param.grt.MaxIdLength>
<param.grt.ReservedNameArray />
<param.grt.customcode.CustomSourceCode />
<param.grt.customcode.CustomHeaderCode />
<param.grt.customcode.CustomInitializer />
<param.grt.customcode.CustomTerminator />
<param.grt.customcode.CustomInclude />
<param.grt.customcode.CustomSource />
<param.grt.customcode.CustomLibrary />
<param.grt.PostCodeGenCommand />
<param.grt.Verbose>false</param.grt.Verbose>
<param.grt.TargetFunctionLibrary>C89/C90 (ANSI)</param.grt.TargetFunctionLibrary>
<param.grt.SupportNonFinite>true</param.grt.SupportNonFinite>
<param.ert.TargetFunctionLibrary>C99 (ISO)</param.ert.TargetFunctionLibrary>
<param.ert.PurelyIntegerCode>false</param.ert.PurelyIntegerCode>
<param.ert.SupportNonFinite>true</param.ert.SupportNonFinite>
<param.ert.IncludeTerminateFcn>true</param.ert.IncludeTerminateFcn>
<param.ert.MultiInstanceCode>false</param.ert.MultiInstanceCode>
<param.ert.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ert.ParenthesesLevel>
<param.ert.ConvertIfToSwitch>false</param.ert.ConvertIfToSwitch>
<param.ert.PreserveExternInFcnDecls>true</param.ert.PreserveExternInFcnDecls>
<param.grt.EnableMemcpy>true</param.grt.EnableMemcpy>
<param.grt.MemcpyThreshold>64</param.grt.MemcpyThreshold>
<param.grt.InitFltsAndDblsToZero>true</param.grt.InitFltsAndDblsToZero>
<param.grt.InlineThreshold>10</param.grt.InlineThreshold>
<param.grt.InlineThresholdMax>200</param.grt.InlineThresholdMax>
<param.grt.InlineStackLimit>4000</param.grt.InlineStackLimit>
<param.grt.StackUsageMax>200000</param.grt.StackUsageMax>
<param.grt.ConstantFoldingTimeout>10000</param.grt.ConstantFoldingTimeout>
<param.UseECoderFeatures>true</param.UseECoderFeatures>
<param.mex.outputfile>attitudeKalmanfilter_mex</param.mex.outputfile>
<param.grt.outputfile>attitudeKalmanfilter</param.grt.outputfile>
<param.artifact>option.target.artifact.lib</param.artifact>
<param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
<param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
<param.version>R2011a</param.version>
<param.HasECoderFeatures>true</param.HasECoderFeatures>
<param.mex.mainhtml />
<param.grt.mainhtml>F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter\html\index.html</param.grt.mainhtml>
<unset>
<param.mex.general.TargetLang />
<param.mex.general.IntegrityChecks />
<param.mex.general.ResponsivenessChecks />
<param.mex.general.EnableBLAS />
<param.mex.general.ExtrinsicCalls />
<param.mex.general.EchoExpressions />
<param.mex.general.EnableDebugging />
<param.mex.general.SaturateOnIntegerOverflow />
<param.mex.general.FilePartitionMethod />
<param.mex.general.GlobalDataSyncMethod />
<param.mex.general.EnableVariableSizing />
<param.mex.general.DynamicMemoryAllocation />
<param.mex.paths.working />
<param.mex.paths.working.specified />
<param.mex.paths.build />
<param.mex.paths.build.specified />
<param.mex.paths.search />
<param.mex.report.GenerateReport />
<param.mex.report.LaunchReport />
<param.mex.comments.GenerateComments />
<param.mex.comments.MATLABSourceComments />
<param.mex.symbols.ReservedNameArray />
<param.mex.customcode.CustomSourceCode />
<param.mex.customcode.CustomHeaderCode />
<param.mex.customcode.CustomInitializer />
<param.mex.customcode.CustomTerminator />
<param.mex.customcode.CustomInclude />
<param.mex.customcode.CustomSource />
<param.mex.customcode.CustomLibrary />
<param.mex.PostCodeGenCommand />
<param.mex.EnableMemcpy />
<param.mex.MemcpyThreshold />
<param.mex.InitFltsAndDblsToZero />
<param.mex.InlineThreshold />
<param.mex.InlineThresholdMax />
<param.mex.InlineStackLimit />
<param.mex.StackUsageMax />
<param.mex.ConstantFoldingTimeout />
<param.grt.general.TargetLang />
<param.grt.CCompilerCustomOptimizations />
<param.grt.general.GenerateMakefile />
<param.grt.general.MakeCommand />
<param.grt.general.TemplateMakefile />
<param.grt.general.SaturateOnIntegerOverflow />
<param.grt.general.FilePartitionMethod />
<param.grt.general.EnableVariableSizing />
<param.grt.general.DynamicMemoryAllocation />
<param.grt.paths.working />
<param.grt.paths.working.specified />
<param.grt.paths.build />
<param.grt.paths.build.specified />
<param.grt.paths.search />
<param.grt.report.GenerateReport />
<param.grt.report.LaunchReport />
<param.grt.GenerateComments />
<param.ert.MATLABFcnDesc />
<param.ert.CustomSymbolStrGlobalVar />
<param.ert.CustomSymbolStrType />
<param.ert.CustomSymbolStrField />
<param.ert.CustomSymbolStrFcn />
<param.ert.CustomSymbolStrTmpVar />
<param.ert.CustomSymbolStrMacro />
<param.ert.CustomSymbolStrEMXArray />
<param.grt.MaxIdLength />
<param.grt.ReservedNameArray />
<param.grt.customcode.CustomHeaderCode />
<param.grt.customcode.CustomInitializer />
<param.grt.customcode.CustomTerminator />
<param.grt.customcode.CustomInclude />
<param.grt.customcode.CustomSource />
<param.grt.customcode.CustomLibrary />
<param.grt.PostCodeGenCommand />
<param.grt.Verbose />
<param.grt.SupportNonFinite />
<param.ert.PurelyIntegerCode />
<param.ert.SupportNonFinite />
<param.ert.IncludeTerminateFcn />
<param.ert.MultiInstanceCode />
<param.ert.ParenthesesLevel />
<param.ert.ConvertIfToSwitch />
<param.ert.PreserveExternInFcnDecls />
<param.grt.EnableMemcpy />
<param.grt.MemcpyThreshold />
<param.grt.InitFltsAndDblsToZero />
<param.grt.InlineThreshold />
<param.grt.InlineThresholdMax />
<param.grt.InlineStackLimit />
<param.grt.StackUsageMax />
<param.grt.ConstantFoldingTimeout />
<param.UseECoderFeatures />
<param.mex.outputfile />
<param.grt.outputfile />
<param.mex.GenCodeOnly />
<param.version />
<param.HasECoderFeatures />
<param.mex.mainhtml />
</unset>
<fileset.entrypoints>
<file value="${PROJECT_ROOT}\attitudeKalmanfilter.m" custom-data-expanded="true">
<Inputs fileName="attitudeKalmanfilter.m" functionName="attitudeKalmanfilter">
<Input Name="dt">
<Class>single</Class>
<Size>1 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="z_k">
<Class>single</Class>
<Size>9 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="x_aposteriori_k">
<Class>single</Class>
<Size>12 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="P_aposteriori_k">
<Class>single</Class>
<Size>12 x 12</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
<Input Name="knownConst">
<Class>single</Class>
<Size>7 x 1</Size>
<Value />
<InitialValue />
<Complex>false</Complex>
</Input>
</Inputs>
</file>
</fileset.entrypoints>
<fileset.package />
<build-deliverables />
<matlab>
<root>C:\Program Files\MATLAB\R2011a</root>
</matlab>
<platform>
<unix>false</unix>
<mac>false</mac>
<windows>true</windows>
<win2k>false</win2k>
<winxp>false</winxp>
<vista>false</vista>
<linux>false</linux>
<solaris>false</solaris>
<osver>6.1</osver>
<os32>false</os32>
<os64>true</os64>
<arch>win64</arch>
<matlab>true</matlab>
</platform>
</configuration>
</deployment-project>
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+11 -4
View File
@@ -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!");
}
}
}
}
apps/commander/calibration_routines.c
+41
View File
@@ -1,3 +1,44 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file calibration_routines.c
* Calibration routines implementations.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <math.h>
#include "calibration_routines.h"
apps/commander/calibration_routines.h
+40
View File
@@ -1,3 +1,43 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file calibration_routines.h
* Calibration routines definitions.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
/**
* Least-squares fit of a sphere to a set of points.
apps/commander/commander.c
+57 -28
View File
@@ -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, &current_status);
/* Start monitoring loop */
uint16_t counter = 0;
@@ -1218,7 +1247,7 @@ int commander_thread_main(int argc, char *argv[])
* valid and system has been running for two and a half seconds
*/
if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) {
bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
bat_remain = battery_remaining_estimate_voltage(battery_voltage);
}
/* Slow but important 8 Hz checks */
apps/commander/commander.h
+13 -4
View File
@@ -1,10 +1,11 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
@@ -34,12 +35,20 @@
*
****************************************************************************/
/* @file Main system state machine definition */
/**
* @file commander.h
* Main system state machine definition.
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*
*/
#ifndef COMMANDER_H_
#define COMMANDER_H_
#define VOLTAGE_BATTERY_CRITICAL_VOLTS 10.0f
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
apps/commander/state_machine_helper.c
+1 -1
View File
@@ -45,7 +45,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/systemlib.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
apps/drivers/bma180/bma180.cpp
+33 -13
View File
@@ -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");
apps/px4/ground_estimator/Makefile → apps/drivers/boards/px4fmu/Makefile
+2 -4
View File
@@ -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
nuttx/configs/px4fmu/src/up_adc.c → apps/drivers/boards/px4fmu/px4fmu_adc.c
+19 -53
View File
@@ -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 */
nuttx/configs/px4fmu/src/up_can.c → apps/drivers/boards/px4fmu/px4fmu_can.c
+35 -36
View File
@@ -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 */
initialized = true;
}
return OK;
}
#endif /* CONFIG_STM32_CAN || CONFIG_STM32_CAN2 || CONFIG_STM32_CAN3 */
apps/drivers/boards/px4fmu/px4fmu_init.c
+317
View File
@@ -0,0 +1,317 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file px4fmu_init.c
*
* PX4FMU-specific early startup code. This file implements the
* nsh_archinitialize() function that is called early by nsh during startup.
*
* Code here is run before the rcS script is invoked; it should start required
* subsystems and perform board-specific initialisation.
*/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdbool.h>
#include <stdio.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/arch.h>
#include <nuttx/spi.h>
#include <nuttx/i2c.h>
#include <nuttx/mmcsd.h>
#include <nuttx/analog/adc.h>
#include <nuttx/arch.h>
#include "stm32_internal.h"
#include "px4fmu_internal.h"
#include "stm32_uart.h"
#include <arch/board/board.h>
#include <arch/board/drv_led.h>
#include <arch/board/drv_eeprom.h>
#include <drivers/drv_hrt.h>
#include <systemlib/cpuload.h>
/****************************************************************************
* Pre-Processor Definitions
****************************************************************************/
/* Configuration ************************************************************/
/* Debug ********************************************************************/
#ifdef CONFIG_CPP_HAVE_VARARGS
# ifdef CONFIG_DEBUG
# define message(...) lib_lowprintf(__VA_ARGS__)
# else
# define message(...) printf(__VA_ARGS__)
# endif
#else
# ifdef CONFIG_DEBUG
# define message lib_lowprintf
# else
# define message printf
# endif
#endif
/****************************************************************************
* Protected Functions
****************************************************************************/
extern int adc_devinit(void);
/****************************************************************************
* Public Functions
****************************************************************************/
/************************************************************************************
* Name: stm32_boardinitialize
*
* Description:
* All STM32 architectures must provide the following entry point. This entry point
* is called early in the intitialization -- after all memory has been configured
* and mapped but before any devices have been initialized.
*
************************************************************************************/
__EXPORT void stm32_boardinitialize(void)
{
/* configure SPI interfaces */
stm32_spiinitialize();
/* configure LEDs */
up_ledinit();
}
/****************************************************************************
* Name: nsh_archinitialize
*
* Description:
* Perform architecture specific initialization
*
****************************************************************************/
static struct spi_dev_s *spi1;
static struct spi_dev_s *spi3;
static struct i2c_dev_s *i2c1;
static struct i2c_dev_s *i2c2;
static struct i2c_dev_s *i2c3;
#include <math.h>
#ifdef __cplusplus
__EXPORT int matherr(struct __exception *e)
{
return 1;
}
#else
__EXPORT int matherr(struct exception *e)
{
return 1;
}
#endif
__EXPORT int nsh_archinitialize(void)
{
int result;
/* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
/* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
/* configure the high-resolution time/callout interface */
#ifdef CONFIG_HRT_TIMER
hrt_init();
#endif
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
#ifdef SERIAL_HAVE_DMA
{
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
}
#endif
message("\r\n");
up_ledoff(LED_BLUE);
up_ledoff(LED_AMBER);
up_ledon(LED_BLUE);
/* Configure user-space led driver */
px4fmu_led_init();
/* Configure SPI-based devices */
spi1 = up_spiinitialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
// Default SPI1 to 1MHz and de-assert the known chip selects.
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\r\n");
/* initialize I2C2 bus */
i2c2 = up_i2cinitialize(2);
if (!i2c2) {
message("[boot] FAILED to initialize I2C bus 2\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C2 speed */
I2C_SETFREQUENCY(i2c2, 400000);
i2c3 = up_i2cinitialize(3);
if (!i2c3) {
message("[boot] FAILED to initialize I2C bus 3\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C3 speed */
I2C_SETFREQUENCY(i2c3, 400000);
/* try to attach, don't fail if device is not responding */
(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
#if defined(CONFIG_STM32_SPI3)
/* Get the SPI port */
message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
if (!spi3) {
message("[boot] FAILED to initialize SPI port 3\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully initialized SPI port 3\n");
/* Now bind the SPI interface to the MMCSD driver */
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
if (result != OK) {
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
#endif /* SPI3 */
/* initialize I2C1 bus */
i2c1 = up_i2cinitialize(1);
if (!i2c1) {
message("[boot] FAILED to initialize I2C bus 1\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C1 speed */
I2C_SETFREQUENCY(i2c1, 400000);
/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
/* Get board information if available */
/* Initialize the user GPIOs */
px4fmu_gpio_init();
#ifdef CONFIG_ADC
int adc_state = adc_devinit();
if (adc_state != OK) {
/* Try again */
adc_state = adc_devinit();
if (adc_state != OK) {
/* Give up */
message("[boot] FAILED adc_devinit: %d\n", adc_state);
return -ENODEV;
}
}
#endif
return OK;
}
apps/drivers/boards/px4fmu/px4fmu_internal.h
+166
View File
@@ -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__ */
nuttx/configs/px4fmu/src/up_boot.c → apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+49 -34
View File
@@ -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
************************************************************************************/
#include "stm32_internal.h"
#include "stm32_gpio.h"
#include "stm32_tim.h"
/************************************************************************************
* Private Functions
************************************************************************************/
__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
}
};
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: stm32_boardinitialize
*
* Description:
* All STM32 architectures must provide the following entry point. This entry point
* is called early in the intitialization -- after all memory has been configured
* and mapped but before any devices have been initialized.
*
************************************************************************************/
void stm32_boardinitialize(void)
{
/* configure SPI interfaces */
stm32_spiinitialize();
/* 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,
}
};
nuttx/configs/px4fmu/src/up_spi.c → apps/drivers/boards/px4fmu/px4fmu_spi.c
+21 -77
View File
@@ -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)
__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
spidbg("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert");
/* there can only be one device on this bus, so always select it */
stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 0);
}
uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
{
/* this is actually bogus, but PX4 has no way to sense the presence of an SD card */
return SPI_STATUS_PRESENT;
}
#endif
#endif /* CONFIG_STM32_SPI1 || CONFIG_STM32_SPI2 */
nuttx/configs/px4fmu/src/up_usbdev.c → apps/drivers/boards/px4fmu/px4fmu_usb.c
+22 -19
View File
@@ -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);
}
apps/drivers/device/device.h
+9 -9
View File
@@ -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:
/**
apps/drivers/device/i2c.cpp
+3 -3
View File
@@ -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);
apps/drivers/device/i2c.h
+1 -1
View File
@@ -42,7 +42,7 @@
#include <nuttx/i2c.h>
namespace device __EXPORT
namespace device __EXPORT
{
/**
apps/drivers/device/spi.cpp
+2
View File
@@ -134,6 +134,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
/* do common setup */
if (!up_interrupt_context())
SPI_LOCK(_dev, true);
SPI_SETFREQUENCY(_dev, _frequency);
SPI_SETMODE(_dev, _mode);
SPI_SETBITS(_dev, 8);
@@ -144,6 +145,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
/* and clean up */
SPI_SELECT(_dev, _device, false);
if (!up_interrupt_context())
SPI_LOCK(_dev, false);
apps/drivers/device/spi.h
+1 -1
View File
@@ -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.
apps/drivers/drv_accel.h
+1 -1
View File
@@ -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
*/
apps/drivers/drv_gyro.h
+1 -1
View File
@@ -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;
nuttx/configs/px4fmu/include/up_hrt.h → apps/drivers/drv_hrt.h
+19 -16
View File
@@ -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
apps/drivers/drv_mag.h
+1 -1
View File
@@ -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;
apps/drivers/drv_pwm_output.h
+63 -3
View File
@@ -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
apps/drivers/drv_sensor.h
+3 -3
View File
@@ -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.
apps/drivers/drv_tone_alarm.h
+2 -2
View File
@@ -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.
*
apps/drivers/hmc5883/hmc5883.cpp
+53 -12
View File
@@ -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");
}
apps/drivers/l3gd20/l3gd20.cpp
+25 -7
View File
@@ -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");
apps/drivers/mpu6000/mpu6000.cpp
+22 -10
View File
@@ -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");
apps/drivers/ms5611/ms5611.cpp
+34 -8
View File
@@ -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);
}
apps/px4/px4io/driver/Makefile → apps/drivers/px4io/Makefile
View File
apps/px4/px4io/driver/px4io.cpp → apps/drivers/px4io/px4io.cpp
+28 -8
View File
@@ -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;
}
apps/px4/px4io/driver/uploader.cpp → apps/drivers/px4io/uploader.cpp
+45 -2
View File
@@ -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;
apps/px4/px4io/driver/uploader.h → apps/drivers/px4io/uploader.h
View File
apps/px4/attitude_estimator_bm/Makefile → apps/drivers/stm32/Makefile
+4 -4
View File
@@ -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
nuttx/configs/px4fmu/src/up_hrt.c → apps/drivers/stm32/drv_hrt.c
+60 -45
View File
@@ -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]++;
}
nuttx/configs/px4fmu/src/up_pwm_servo.c → apps/drivers/stm32/drv_pwm_servo.c
+70 -108
View File
@@ -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.
*/
nuttx/configs/px4fmu/include/up_adc.h → apps/drivers/stm32/drv_pwm_servo.h
+35 -28
View File
@@ -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
************************************************************************************/
#include <nuttx/config.h>
#ifdef CONFIG_ADC
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: adc_devinit
/**
* @file drv_pwm_servo.h
*
* Description:
* All STM32 architectures must provide the following interface to work with
* examples/adc.
*
************************************************************************************/
* stm32-specific PWM output data.
*/
int adc_devinit(void);
#pragma once
#endif /* CONFIG_ADC */
#include <drivers/drv_pwm_output.h>
/* configuration limits */
#define PWM_SERVO_MAX_TIMERS 2
#define PWM_SERVO_MAX_CHANNELS 8
/* array of timers dedicated to PWM servo use */
struct pwm_servo_timer {
uint32_t base;
uint32_t clock_register;
uint32_t clock_bit;
uint32_t clock_freq;
};
/* supplied by board-specific code */
__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS];
/* array of channels in logical order */
struct pwm_servo_channel {
uint32_t gpio;
uint8_t timer_index;
uint8_t timer_channel;
servo_position_t default_value;
};
__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS];
apps/drivers/stm32/tone_alarm/tone_alarm.cpp
+51 -30
View File
@@ -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;
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;
/* 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;
/*
* 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;
/* make sure the timer is running */
rCR1 = GTIM_CR1_CEN;
debug("ready");
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);
apps/fixedwing_control/fixedwing_control.c
+1 -1
View File
@@ -48,7 +48,7 @@
#include <math.h>
#include <poll.h>
#include <time.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
apps/gps/gps.c
+4 -4
View File
@@ -564,10 +564,10 @@ void setup_port(char *device, int speed, int *fd)
*fd = open_port(device);
if (*fd != -1) {
if (gps_verbose) printf("[gps] Port opened: %s at %d speed\r\n", device, speed);
if (gps_verbose) printf("[gps] Port opened: %s at %d baud\n", device, speed);
} else {
fprintf(stderr, "[gps] Could not open port, exiting gps app!\r\n");
fprintf(stderr, "[gps] Could not open port, exiting gps app!\n");
fflush(stdout);
}
@@ -576,7 +576,7 @@ void setup_port(char *device, int speed, int *fd)
int termios_state;
if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) {
fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\r\n", device, termios_state);
fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\n", device, termios_state);
close(*fd);
}
if (gps_verbose) printf("[gps] Try to set baud rate %d now\n",speed);
@@ -584,7 +584,7 @@ void setup_port(char *device, int speed, int *fd)
cfsetispeed(&uart_config, speed);
cfsetospeed(&uart_config, speed);
if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) {
fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", device);
fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\n", device);
close(*fd);
}
}
apps/gps/mtk.c
+1 -1
View File
@@ -43,7 +43,7 @@
#include <pthread.h>
#include <poll.h>
#include <fcntl.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
apps/gps/nmea_helper.c
+1 -1
View File
@@ -44,7 +44,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
#define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2
#define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2
apps/gps/ubx.c
+24 -22
View File
@@ -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?
apps/gps/ubx.h
+14 -12
View File
@@ -76,6 +76,7 @@
// ************
/** the structures of the binary packets */
#pragma pack(push, 1)
typedef struct {
uint32_t time_milliseconds; // GPS Millisecond Time of Week
int32_t lon; // Longitude * 1e-7, deg
@@ -87,7 +88,7 @@ typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} __attribute__((__packed__)) type_gps_bin_nav_posllh_packet;
} type_gps_bin_nav_posllh_packet;
typedef type_gps_bin_nav_posllh_packet gps_bin_nav_posllh_packet_t;
@@ -112,7 +113,7 @@ typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} __attribute__((__packed__)) type_gps_bin_nav_sol_packet;
} type_gps_bin_nav_sol_packet;
typedef type_gps_bin_nav_sol_packet gps_bin_nav_sol_packet_t;
@@ -131,7 +132,7 @@ typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} __attribute__((__packed__)) type_gps_bin_nav_timeutc_packet;
} type_gps_bin_nav_timeutc_packet;
typedef type_gps_bin_nav_timeutc_packet gps_bin_nav_timeutc_packet_t;
@@ -148,7 +149,7 @@ typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} __attribute__((__packed__)) type_gps_bin_nav_dop_packet;
} type_gps_bin_nav_dop_packet;
typedef type_gps_bin_nav_dop_packet gps_bin_nav_dop_packet_t;
@@ -158,7 +159,7 @@ typedef struct {
uint8_t globalFlags;
uint16_t reserved2;
} __attribute__((__packed__)) type_gps_bin_nav_svinfo_part1_packet;
} type_gps_bin_nav_svinfo_part1_packet;
typedef type_gps_bin_nav_svinfo_part1_packet gps_bin_nav_svinfo_part1_packet_t;
@@ -172,7 +173,7 @@ typedef struct {
int16_t azim; //Azimuth in integer degrees
int32_t prRes; //Pseudo range residual in centimetres
} __attribute__((__packed__)) type_gps_bin_nav_svinfo_part2_packet;
} type_gps_bin_nav_svinfo_part2_packet;
typedef type_gps_bin_nav_svinfo_part2_packet gps_bin_nav_svinfo_part2_packet_t;
@@ -180,7 +181,7 @@ typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} __attribute__((__packed__)) type_gps_bin_nav_svinfo_part3_packet;
} type_gps_bin_nav_svinfo_part3_packet;
typedef type_gps_bin_nav_svinfo_part3_packet gps_bin_nav_svinfo_part3_packet_t;
@@ -198,7 +199,7 @@ typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} __attribute__((__packed__)) type_gps_bin_nav_velned_packet;
} type_gps_bin_nav_velned_packet;
typedef type_gps_bin_nav_velned_packet gps_bin_nav_velned_packet_t;
@@ -209,7 +210,7 @@ typedef struct {
//... rest of package is not used in this implementation
} __attribute__((__packed__)) type_gps_bin_rxm_svsi_packet;
} type_gps_bin_rxm_svsi_packet;
typedef type_gps_bin_rxm_svsi_packet gps_bin_rxm_svsi_packet_t;
@@ -219,7 +220,7 @@ typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} __attribute__((__packed__)) type_gps_bin_ack_ack_packet;
} type_gps_bin_ack_ack_packet;
typedef type_gps_bin_ack_ack_packet gps_bin_ack_ack_packet_t;
@@ -229,7 +230,7 @@ typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} __attribute__((__packed__)) type_gps_bin_ack_nak_packet;
} type_gps_bin_ack_nak_packet;
typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t;
@@ -283,9 +284,10 @@ typedef struct {
enum UBX_MESSAGE_IDS message_id;
uint64_t last_message_timestamps[UBX_NO_OF_MESSAGES];
} __attribute__((__packed__)) type_gps_bin_ubx_state;
} type_gps_bin_ubx_state;
typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
#pragma pack(pop)
extern pthread_mutex_t *ubx_mutex;
extern gps_bin_ubx_state_t *ubx_state;
apps/mavlink/mavlink.c
+5 -2
View File
@@ -50,7 +50,7 @@
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
@@ -745,7 +745,10 @@ int mavlink_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
/* XXX should wait for it to actually exit here */
while (thread_running) {
usleep(200000);
}
warnx("terminated.");
exit(0);
}
apps/mavlink/mavlink_bridge_header.h
+4 -2
View File
@@ -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 */
apps/mavlink/mavlink_parameters.c
+2
View File
@@ -39,6 +39,8 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include "mavlink_parameters.h"
#include <uORB/uORB.h>
#include "math.h" /* isinf / isnan checks */
apps/mavlink/mavlink_parameters.h
+1 -1
View File
@@ -43,7 +43,7 @@
or in the same folder as this source file */
#include "v1.0/common/mavlink.h"
#include <v1.0/mavlink_types.h>
#include <stdbool.h>
#include <systemlib/param/param.h>
apps/mavlink/mavlink_receiver.c
+1 -1
View File
@@ -49,7 +49,7 @@
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
apps/mavlink/missionlib.c
+1 -1
View File
@@ -49,7 +49,7 @@
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
apps/mavlink/orb_listener.c
+1 -1
View File
@@ -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>
apps/multirotor_att_control/multirotor_att_control_main.c
+1 -1
View File
@@ -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>
apps/multirotor_att_control/multirotor_attitude_control.c
+1 -1
View File
@@ -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);
apps/multirotor_att_control/multirotor_rate_control.c
+1 -1
View File
@@ -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);
apps/multirotor_pos_control/multirotor_pos_control.c
+1 -1
View File
@@ -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>
apps/position_estimator/Makefile
+1 -7
View File
@@ -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
apps/position_estimator/codegen/position_estimator.c
-261
View File
@@ -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) */
apps/position_estimator/codegen/position_estimator.h
-32
View File
@@ -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) */
apps/position_estimator/codegen/position_estimator_initialize.c
-31
View File
@@ -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) */
apps/position_estimator/codegen/position_estimator_initialize.h
-32
View File
@@ -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) */
apps/position_estimator/codegen/position_estimator_terminate.c
-31
View File
@@ -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) */
apps/position_estimator/codegen/position_estimator_terminate.h
-32
View File
@@ -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) */
apps/position_estimator/codegen/position_estimator_types.h
-16
View File
@@ -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) */
apps/position_estimator/codegen/rtGetInf.c
-139
View File
@@ -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) */
apps/position_estimator/codegen/rtGetInf.h
-23
View File
@@ -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) */
apps/position_estimator/codegen/rtGetNaN.c
-96
View File
@@ -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) */
apps/position_estimator/codegen/rtGetNaN.h
-21
View File
@@ -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) */
apps/position_estimator/codegen/rt_nonfinite.c
-87
View File
@@ -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) */
apps/position_estimator/codegen/rt_nonfinite.h
-53
View File
@@ -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) */
apps/position_estimator/codegen/rtwtypes.h
-175
View File
@@ -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) */
apps/position_estimator/position_estimator.m
-62
View File
@@ -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
apps/position_estimator/position_estimator.prj
-269
View File
@@ -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>
apps/position_estimator/position_estimator_main.c
-2
View File
@@ -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
apps/px4/attitude_estimator_bm/.context
View File
apps/px4/attitude_estimator_bm/attitude_bm.c
-322
View File
@@ -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);
}
apps/px4/attitude_estimator_bm/attitude_bm.h
-24
View File
@@ -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_ */
apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
-283
View File
@@ -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;
}
apps/px4/attitude_estimator_bm/kalman.c
-115
View File
@@ -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);
}
apps/px4/attitude_estimator_bm/kalman.h
-35
View File
@@ -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_ */
apps/px4/attitude_estimator_bm/matrix.h
-156
View File
@@ -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_ */
apps/px4/fmu/fmu.cpp
-2
View File
@@ -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>
apps/px4/ground_estimator/ground_estimator.c
-180
View File
@@ -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);
}
apps/px4/px4io/driver/.context
View File
apps/px4/tests/test_hrt.c
+1 -1
View File
@@ -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>
apps/px4/tests/test_time.c
+1 -1
View File
@@ -53,7 +53,7 @@
#include <math.h>
#include <float.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
/****************************************************************************
apps/px4/tests/test_uart_console.c
+1 -1
View File
@@ -56,7 +56,7 @@
#include <math.h>
#include <float.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
/****************************************************************************
apps/px4/tests/test_uart_send.c
+1 -1
View File
@@ -56,7 +56,7 @@
#include <math.h>
#include <float.h>
#include <arch/board/up_hrt.h>
#include <drivers/drv_hrt.h>
/****************************************************************************
apps/px4/tests/tests_file.c
+1 -1
View File
@@ -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"
apps/px4io/comms.c
+1 -1
View File
@@ -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>
apps/px4io/mixer.c
+1 -1
View File
@@ -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"
apps/px4io/px4io.c
+1 -1
View File
@@ -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"
apps/px4io/safety.c
+1 -1
View File
@@ -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"

Some files were not shown because too many files have changed in this diff Show More