diff options
51 files changed, 1075 insertions, 999 deletions
diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox new file mode 100644 index 000000000..2dc83a517 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -0,0 +1,39 @@ +#!nsh +# +# UNTESTED UNTESTED! +# +# Generic 10” Hexa coaxial geometry +# +# Lorenz Meier <lm@inf.ethz.ch> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER hexa_cox + +set PWM_OUTPUTS 12345678 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 5f3cec4e0..a55fc5a30 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -2,7 +2,7 @@ # # Generic 10” Octo coaxial geometry # -# Maintainers: Lorenz Meier <lm@inf.ethz.ch> +# Lorenz Meier <lm@inf.ethz.ch> # if [ $DO_AUTOCONFIG == yes ] @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE mc set MIXER FMU_octo_cox -set PWM_OUTPUTS 1234 +set PWM_OUTPUTS 12345678 set PWM_RATE 400 # DJI ESC range set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm index ddec8f36e..447796709 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE mc set MIXER FMU_hexa_x -set PWM_OUTPUTS 1234 +set PWM_OUTPUTS 12345678 set PWM_RATE 400 # DJI ESC range set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm index 106e0fb54..c4e9560d1 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE mc set MIXER FMU_hexa_+ -set PWM_OUTPUTS 1234 +set PWM_OUTPUTS 12345678 set PWM_RATE 400 # DJI ESC range set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm index f0eea339b..ea56195d4 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE mc set MIXER FMU_octo_x -set PWM_OUTPUTS 1234 +set PWM_OUTPUTS 12345678 set PWM_RATE 400 # DJI ESC range set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm index 992a7aeba..f7693875c 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE mc set MIXER FMU_octo_+ -set PWM_OUTPUTS 1234 +set PWM_OUTPUTS 12345678 set PWM_RATE 400 # DJI ESC range set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 00baed646..38b1cb57e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -186,10 +186,19 @@ then fi # +# Hexa Coaxial +# + +if param compare SYS_AUTOSTART 11001 +then + sh /etc/init.d/11001_hexa_cox +fi + +# # Octo Coaxial # if param compare SYS_AUTOSTART 12001 then - sh /etc/init.d/12001_octo_cox_pwm + sh /etc/init.d/12001_octo_cox fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6f4e1f3b5..d12785368 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -465,21 +465,25 @@ then set MAV_TYPE 2 # Use mixer to detect vehicle type - if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] + if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ] then - param set MAV_TYPE 13 + set MAV_TYPE 13 + fi + if [ $MIXER == hexa_cox ] + then + set MAV_TYPE 13 fi if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] then - param set MAV_TYPE 14 + set MAV_TYPE 14 fi if [ $MIXER == FMU_octo_cox ] then - param set MAV_TYPE 14 + set MAV_TYPE 14 fi fi - param set MAV_TYPE $MAV_TYPE + param set MAV_TYPE $MAV_TYPE # Load mixer and configure outputs sh /etc/init.d/rc.interface diff --git a/ROMFS/px4fmu_common/logging/conv.zip b/ROMFS/px4fmu_common/logging/conv.zip Binary files differindex 7cb837e56..7f485184c 100644 --- a/ROMFS/px4fmu_common/logging/conv.zip +++ b/ROMFS/px4fmu_common/logging/conv.zip diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix index 610868354..80e3bac09 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_X5.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix @@ -23,13 +23,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -3000 -5000 0 -10000 10000 -S: 0 1 -5000 -5000 0 -10000 10000 +S: 0 0 -8000 -8000 0 -10000 10000 +S: 0 1 6000 6000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -5000 -3000 0 -10000 10000 -S: 0 1 5000 5000 0 -10000 10000 +S: 0 0 -8000 -8000 0 -10000 10000 +S: 0 1 -6000 -6000 0 -10000 10000 Output 2 -------- @@ -48,7 +48,7 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / payload mixer for last four channels +Gimbal / flaps / payload mixer for last four channels ----------------------------------------------------- M: 1 @@ -66,4 +66,3 @@ S: 0 6 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 S: 0 7 10000 10000 0 -10000 10000 - diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix deleted file mode 100644 index f8f9f0e4d..000000000 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix +++ /dev/null @@ -1,18 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a hexacopter in the + configuration. All controls -are mixed 100%. - -R: 6+ 10000 10000 10000 0 - -Gimbal / payload mixer for last two channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix deleted file mode 100644 index 26b40b9e9..000000000 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix +++ /dev/null @@ -1,18 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a hexacopter in the X configuration. All controls -are mixed 100%. - -R: 6x 10000 10000 10000 0 - -Gimbal / payload mixer for last two channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix new file mode 100644 index 000000000..e608b459f --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix @@ -0,0 +1,3 @@ +# Hexa + + +R: 6+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix new file mode 100644 index 000000000..16e6e22f9 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix @@ -0,0 +1,3 @@ +# Hexa X + +R: 6x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix index 51cebb785..f7845450d 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix @@ -1,7 +1,3 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a Coaxial Octocopter in the X configuration. All controls -are mixed 100%. +# Octo coaxial R: 8c 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix index edc71f013..c9a348aa4 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix @@ -1,7 +1,3 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a octocopter in the X configuration. All controls -are mixed 100%. +# Octo X R: 8x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README deleted file mode 100644 index 6649c53c2..000000000 --- a/ROMFS/px4fmu_common/mixers/README +++ /dev/null @@ -1,154 +0,0 @@ -PX4 mixer definitions -===================== - -Files in this directory implement example mixers that can be used as a basis -for customisation, or for general testing purposes. - -Mixer basics ------------- - -Mixers combine control values from various sources (control tasks, user inputs, -etc.) and produce output values suitable for controlling actuators; servos, -motors, switches and so on. - -An actuator derives its value from the combination of one or more control -values. Each of the control values is scaled according to the actuator's -configuration and then combined to produce the actuator value, which may then be -further scaled to suit the specific output type. - -Internally, all scaling is performed using floating point values. Inputs and -outputs are clamped to the range -1.0 to 1.0. - -control control control - | | | - v v v - scale scale scale - | | | - | v | - +-------> mix <------+ - | - scale - | - v - out - -Scaling -------- - -Basic scalers provide linear scaling of the input to the output. - -Each scaler allows the input value to be scaled independently for inputs -greater/less than zero. An offset can be applied to the output, and lower and -upper boundary constraints can be applied. Negative scaling factors cause the -output to be inverted (negative input produces positive output). - -Scaler pseudocode: - -if (input < 0) - output = (input * NEGATIVE_SCALE) + OFFSET -else - output = (input * POSITIVE_SCALE) + OFFSET - -if (output < LOWER_LIMIT) - output = LOWER_LIMIT -if (output > UPPER_LIMIT) - output = UPPER_LIMIT - -Syntax ------- - -Mixer definitions are text files; lines beginning with a single capital letter -followed by a colon are significant. All other lines are ignored, meaning that -explanatory text can be freely mixed with the definitions. - -Each file may define more than one mixer; the allocation of mixers to actuators -is specific to the device reading the mixer definition, and the number of -actuator outputs generated by a mixer is specific to the mixer. - -A mixer begins with a line of the form - - <tag>: <mixer arguments> - -The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a -multirotor mixer, etc. - -Null Mixer -.......... - -A null mixer consumes no controls and generates a single actuator output whose -value is always zero. Typically a null mixer is used as a placeholder in a -collection of mixers in order to achieve a specific pattern of actuator outputs. - -The null mixer definition has the form: - - Z: - -Simple Mixer -............ - -A simple mixer combines zero or more control inputs into a single actuator -output. Inputs are scaled, and the mixing function sums the result before -applying an output scaler. - -A simple mixer definition begins with: - - M: <control count> - O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit> - -If <control count> is zero, the sum is effectively zero and the mixer will -output a fixed value that is <offset> constrained by <lower limit> and <upper -limit>. - -The second line defines the output scaler with scaler parameters as discussed -above. Whilst the calculations are performed as floating-point operations, the -values stored in the definition file are scaled by a factor of 10000; i.e. an -offset of -0.5 is encoded as -5000. - -The definition continues with <control count> entries describing the control -inputs and their scaling, in the form: - - S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit> - -The <group> value identifies the control group from which the scaler will read, -and the <index> value an offset within that group. These values are specific to -the device reading the mixer definition. - -When used to mix vehicle controls, mixer group zero is the vehicle attitude -control group, and index values zero through three are normally roll, pitch, -yaw and thrust respectively. - -The remaining fields on the line configure the control scaler with parameters as -discussed above. Whilst the calculations are performed as floating-point -operations, the values stored in the definition file are scaled by a factor of -10000; i.e. an offset of -0.5 is encoded as -5000. - -Multirotor Mixer -................ - -The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust) -into a set of actuator outputs intended to drive motor speed controllers. - -The mixer definition is a single line of the form: - -R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband> - -The supported geometries include: - - 4x - quadrotor in X configuration - 4+ - quadrotor in + configuration - 6x - hexcopter in X configuration - 6+ - hexcopter in + configuration - 8x - octocopter in X configuration - 8+ - octocopter in + configuration - -Each of the roll, pitch and yaw scale values determine scaling of the roll, -pitch and yaw controls relative to the thrust control. Whilst the calculations -are performed as floating-point operations, the values stored in the definition -file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000. - -Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the -thrust input ranges from 0.0 to 1.0. Output for each actuator is in the -range -1.0 to 1.0. - -In the case where an actuator saturates, all actuator values are rescaled so that -the saturating actuator is limited to 1.0. diff --git a/ROMFS/px4fmu_common/mixers/hexa_cox.mix b/ROMFS/px4fmu_common/mixers/hexa_cox.mix new file mode 100644 index 000000000..497786feb --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/hexa_cox.mix @@ -0,0 +1,3 @@ +# Hexa coaxial + +R: 6c 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_logging/init.d/rcS b/ROMFS/px4fmu_logging/init.d/rcS deleted file mode 100644 index 7b8856719..000000000 --- a/ROMFS/px4fmu_logging/init.d/rcS +++ /dev/null @@ -1,88 +0,0 @@ -#!nsh -# -# PX4FMU startup script for logging purposes -# - -# -# Try to mount the microSD card. -# -echo "[init] looking for microSD..." -if mount -t vfat /dev/mmcsd0 /fs/microsd -then - echo "[init] card mounted at /fs/microsd" - # Start playing the startup tune - tone_alarm start -else - echo "[init] no microSD card found" - # Play SOS - tone_alarm error -fi - -uorb start - -# -# Start sensor drivers here. -# - -ms5611 start -adc start - -# mag might be external -if hmc5883 start -then - echo "using HMC5883" -fi - -if mpu6000 start -then - echo "using MPU6000" -fi - -if l3gd20 start -then - echo "using L3GD20(H)" -fi - -if lsm303d start -then - set BOARD fmuv2 -else - set BOARD fmuv1 -fi - -# Start airspeed sensors -if meas_airspeed start -then - echo "using MEAS airspeed sensor" -else - if ets_airspeed start - then - echo "using ETS airspeed sensor (bus 3)" - else - if ets_airspeed start -b 1 - then - echo "Using ETS airspeed sensor (bus 1)" - fi - fi -fi - -# -# Start the sensor collection task. -# IMPORTANT: this also loads param offsets -# ALWAYS start this task before the -# preflight_check. -# -if sensors start -then - echo "SENSORS STARTED" -fi - -sdlog2 start -r 250 -e -b 16 - -if sercon -then - echo "[init] USB interface connected" - - # Try to get an USB console - nshterm /dev/ttyACM0 & -fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_logging/logging/conv.zip b/ROMFS/px4fmu_logging/logging/conv.zip Binary files differdeleted file mode 100644 index 7cb837e56..000000000 --- a/ROMFS/px4fmu_logging/logging/conv.zip +++ /dev/null diff --git a/Tools/README.txt b/Tools/sdlog2/README.txt index abeb9a4c7..abeb9a4c7 100644 --- a/Tools/README.txt +++ b/Tools/sdlog2/README.txt diff --git a/Tools/logconv.m b/Tools/sdlog2/logconv.m index c416b8095..e19c97fa3 100644 --- a/Tools/logconv.m +++ b/Tools/sdlog2/logconv.m @@ -1,535 +1,535 @@ -% This Matlab Script can be used to import the binary logged values of the
-% PX4FMU into data that can be plotted and analyzed.
-
-%% ************************************************************************
-% PX4LOG_PLOTSCRIPT: Main function
-% ************************************************************************
-function PX4Log_Plotscript
-
-% Clear everything
-clc
-clear all
-close all
-
-% ************************************************************************
-% SETTINGS
-% ************************************************************************
-
-% Set the path to your sysvector.bin file here
-filePath = 'log001.bin';
-
-% Set the minimum and maximum times to plot here [in seconds]
-mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start]
-maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end]
-
-%Determine which data to plot. Not completely implemented yet.
-bDisplayGPS=true;
-
-%conversion factors
-fconv_gpsalt=1; %[mm] to [m]
-fconv_gpslatlong=1; %[gps_raw_position_unit] to [deg]
-fconv_timestamp=1E-6; % [microseconds] to [seconds]
-
-% ************************************************************************
-% Import the PX4 logs
-% ************************************************************************
-ImportPX4LogData();
-
-%Translate min and max plot times to indices
-time=double(sysvector.TIME_StartTime) .*fconv_timestamp;
-mintime_log=time(1); %The minimum time/timestamp found in the log
-maxtime_log=time(end); %The maximum time/timestamp found in the log
-CurTime=mintime_log; %The current time at which to draw the aircraft position
-
-[imintime,imaxtime]=FindMinMaxTimeIndices();
-
-% ************************************************************************
-% PLOT & GUI SETUP
-% ************************************************************************
-NrFigures=5;
-NrAxes=10;
-h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively
-h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively
-h.pathpoints=[]; % Temporary initiliazation of path points
-
-% Setup the GUI to control the plots
-InitControlGUI();
-% Setup the plotting-GUI (figures, axes) itself.
-InitPlotGUI();
-
-% ************************************************************************
-% DRAW EVERYTHING
-% ************************************************************************
-DrawRawData();
-DrawCurrentAircraftState();
-
-%% ************************************************************************
-% *** END OF MAIN SCRIPT ***
-% NESTED FUNCTION DEFINTIONS FROM HERE ON
-% ************************************************************************
-
-
-%% ************************************************************************
-% IMPORTPX4LOGDATA (nested function)
-% ************************************************************************
-% Attention: This is the import routine for firmware from ca. 03/2013.
-% Other firmware versions might require different import
-% routines.
-
-%% ************************************************************************
-% IMPORTPX4LOGDATA (nested function)
-% ************************************************************************
-% Attention: This is the import routine for firmware from ca. 03/2013.
-% Other firmware versions might require different import
-% routines.
-
-function ImportPX4LogData()
-
- % ************************************************************************
- % RETRIEVE SYSTEM VECTOR
- % *************************************************************************
- % //All measurements in NED frame
-
- % Convert to CSV
- %arg1 = 'log-fx61-20130721-2.bin';
- arg1 = filePath;
- delim = ',';
- time_field = 'TIME';
- data_file = 'data.csv';
- csv_null = '';
-
- if not(exist(data_file, 'file'))
- s = system( sprintf('python sdlog2_dump.py "%s" -f "%s" -t"%s" -d"%s" -n"%s"', arg1, data_file, time_field, delim, csv_null) );
- end
-
- if exist(data_file, 'file')
-
- %data = csvread(data_file);
- sysvector = tdfread(data_file, ',');
-
- % shot the flight time
- time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1);
- time_s = uint64(time_us*1e-6);
- time_m = uint64(time_s/60);
- time_s = time_s - time_m * 60;
-
- disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]);
-
- disp(['logfile conversion finished.' char(10)]);
- else
- disp(['file: ' data_file ' does not exist' char(10)]);
- end
-end
-
-%% ************************************************************************
-% INITCONTROLGUI (nested function)
-% ************************************************************************
-%Setup central control GUI components to control current time where data is shown
-function InitControlGUI()
- %**********************************************************************
- % GUI size definitions
- %**********************************************************************
- dxy=5; %margins
- %Panel: Plotctrl
- dlabels=120;
- dsliders=200;
- dedits=80;
- hslider=20;
-
- hpanel1=40; %panel1
- hpanel2=220;%panel2
- hpanel3=3*hslider+4*dxy+3*dxy;%panel3.
-
- width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width
- height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height
-
- %**********************************************************************
- % Create GUI
- %**********************************************************************
- h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI');
- h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1));
- h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1));
- h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1));
-
- %%Control GUI-elements
- %Slider: Current time
- h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left');
- h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],...
- 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel);
- temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min');
- set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);
- h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),...
- 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel);
-
- %Slider: MaxTime
- h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left');
- h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],...
- 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
- h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),...
- 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
-
- %Slider: MinTime
- h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left');
- h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],...
- 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
- h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),...
- 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
-
- %%Current data/state GUI-elements (Multiline-edit-box)
- h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',...
- 'HorizontalAlignment','left','parent',h.aircraftstatepanel);
-
- h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',...
- 'HorizontalAlignment','left','parent',h.guistatepanel);
-
-end
-
-%% ************************************************************************
-% INITPLOTGUI (nested function)
-% ************************************************************************
-function InitPlotGUI()
-
- % Setup handles to lines and text
- h.markertext=[];
- templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array
- h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively
- h.markerline(1:NrAxes)=0.0;
-
- % Setup all other figures and axes for plotting
- % PLOT WINDOW 1: GPS POSITION
- h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position');
- h.axes(1)=axes();
- set(h.axes(1),'Parent',h.figures(2));
-
- % PLOT WINDOW 2: IMU, baro altitude
- h.figures(3)=figure('Name', 'IMU / Baro Altitude');
- h.axes(2)=subplot(4,1,1);
- h.axes(3)=subplot(4,1,2);
- h.axes(4)=subplot(4,1,3);
- h.axes(5)=subplot(4,1,4);
- set(h.axes(2:5),'Parent',h.figures(3));
-
- % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...
- h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds');
- h.axes(6)=subplot(4,1,1);
- h.axes(7)=subplot(4,1,2);
- h.axes(8)=subplot(4,1,3);
- h.axes(9)=subplot(4,1,4);
- set(h.axes(6:9),'Parent',h.figures(4));
-
- % PLOT WINDOW 4: LOG STATS
- h.figures(5) = figure('Name', 'Log Statistics');
- h.axes(10)=subplot(1,1,1);
- set(h.axes(10:10),'Parent',h.figures(5));
-
-end
-
-%% ************************************************************************
-% DRAWRAWDATA (nested function)
-% ************************************************************************
-%Draws the raw data from the sysvector, but does not add any
-%marker-lines or so
-function DrawRawData()
- % ************************************************************************
- % PLOT WINDOW 1: GPS POSITION & GUI
- % ************************************************************************
- figure(h.figures(2));
- % Only plot GPS data if available
- if (sum(double(sysvector.GPS_Lat(imintime:imaxtime)))>0) && (bDisplayGPS)
- %Draw data
- plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:imaxtime))*fconv_gpslatlong, ...
- double(sysvector.GPS_Lon(imintime:imaxtime))*fconv_gpslatlong, ...
- double(sysvector.GPS_Alt(imintime:imaxtime))*fconv_gpsalt,'r.');
- title(h.axes(1),'GPS Position Data(if available)');
- xlabel(h.axes(1),'Latitude [deg]');
- ylabel(h.axes(1),'Longitude [deg]');
- zlabel(h.axes(1),'Altitude above MSL [m]');
- grid on
-
- %Reset path
- h.pathpoints=0;
- end
-
- % ************************************************************************
- % PLOT WINDOW 2: IMU, baro altitude
- % ************************************************************************
- figure(h.figures(3));
- plot(h.axes(2),time(imintime:imaxtime),[sysvector.IMU_MagX(imintime:imaxtime), sysvector.IMU_MagY(imintime:imaxtime), sysvector.IMU_MagZ(imintime:imaxtime)]);
- title(h.axes(2),'Magnetometers [Gauss]');
- legend(h.axes(2),'x','y','z');
- plot(h.axes(3),time(imintime:imaxtime),[sysvector.IMU_AccX(imintime:imaxtime), sysvector.IMU_AccY(imintime:imaxtime), sysvector.IMU_AccZ(imintime:imaxtime)]);
- title(h.axes(3),'Accelerometers [m/s]');
- legend(h.axes(3),'x','y','z');
- plot(h.axes(4),time(imintime:imaxtime),[sysvector.IMU_GyroX(imintime:imaxtime), sysvector.IMU_GyroY(imintime:imaxtime), sysvector.IMU_GyroZ(imintime:imaxtime)]);
- title(h.axes(4),'Gyroscopes [rad/s]');
- legend(h.axes(4),'x','y','z');
- plot(h.axes(5),time(imintime:imaxtime),sysvector.SENS_BaroAlt(imintime:imaxtime),'color','blue');
- if(bDisplayGPS)
- hold on;
- plot(h.axes(5),time(imintime:imaxtime),double(sysvector.GPS_Alt(imintime:imaxtime)).*fconv_gpsalt,'color','red');
- hold off
- legend('Barometric Altitude [m]','GPS Altitude [m]');
- else
- legend('Barometric Altitude [m]');
- end
- title(h.axes(5),'Altitude above MSL [m]');
-
- % ************************************************************************
- % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...
- % ************************************************************************
- figure(h.figures(4));
- %Attitude Estimate
- plot(h.axes(6),time(imintime:imaxtime), [sysvector.ATT_Roll(imintime:imaxtime), sysvector.ATT_Pitch(imintime:imaxtime), sysvector.ATT_Yaw(imintime:imaxtime)] .*180./3.14159);
- title(h.axes(6),'Estimated attitude [deg]');
- legend(h.axes(6),'roll','pitch','yaw');
- %Actuator Controls
- plot(h.axes(7),time(imintime:imaxtime), [sysvector.ATTC_Roll(imintime:imaxtime), sysvector.ATTC_Pitch(imintime:imaxtime), sysvector.ATTC_Yaw(imintime:imaxtime), sysvector.ATTC_Thrust(imintime:imaxtime)]);
- title(h.axes(7),'Actuator control [-]');
- legend(h.axes(7),'ATT CTRL Roll [-1..+1]','ATT CTRL Pitch [-1..+1]','ATT CTRL Yaw [-1..+1]','ATT CTRL Thrust [0..+1]');
- %Actuator Controls
- plot(h.axes(8),time(imintime:imaxtime), [sysvector.OUT0_Out0(imintime:imaxtime), sysvector.OUT0_Out1(imintime:imaxtime), sysvector.OUT0_Out2(imintime:imaxtime), sysvector.OUT0_Out3(imintime:imaxtime), sysvector.OUT0_Out4(imintime:imaxtime), sysvector.OUT0_Out5(imintime:imaxtime), sysvector.OUT0_Out6(imintime:imaxtime), sysvector.OUT0_Out7(imintime:imaxtime)]);
- title(h.axes(8),'Actuator PWM (raw-)outputs [s]');
- legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8');
- set(h.axes(8), 'YLim',[800 2200]);
- %Airspeeds
- plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_IndSpeed(imintime:imaxtime));
- hold on
- plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_TrueSpeed(imintime:imaxtime));
- hold off
- %add GPS total airspeed here
- title(h.axes(9),'Airspeed [m/s]');
- legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed');
- %calculate time differences and plot them
- intervals = zeros(0,imaxtime - imintime);
- for k = imintime+1:imaxtime
- intervals(k) = time(k) - time(k-1);
- end
- plot(h.axes(10), time(imintime:imaxtime), intervals);
-
- %Set same timescale for all plots
- for i=2:NrAxes
- set(h.axes(i),'XLim',[mintime maxtime]);
- end
-
- set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);
-end
-
-%% ************************************************************************
-% DRAWCURRENTAIRCRAFTSTATE(nested function)
-% ************************************************************************
-function DrawCurrentAircraftState()
- %find current data index
- i=find(time>=CurTime,1,'first');
-
- %**********************************************************************
- % Current aircraft state label update
- %**********************************************************************
- acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.GPS_Lat(i))*fconv_gpslatlong),', ',...
- 'lon=',num2str(double(sysvector.GPS_Lon(i))*fconv_gpslatlong),', ',...
- 'alt=',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]'];
- acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.IMU_MagX(i)),...
- ', y=',num2str(sysvector.IMU_MagY(i)),...
- ', z=',num2str(sysvector.IMU_MagZ(i)),']'];
- acstate{3,:}=[sprintf('%s \t\t','Accels[m/s]'),'[x=',num2str(sysvector.IMU_AccX(i)),...
- ', y=',num2str(sysvector.IMU_AccY(i)),...
- ', z=',num2str(sysvector.IMU_AccZ(i)),']'];
- acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.IMU_GyroX(i)),...
- ', y=',num2str(sysvector.IMU_GyroY(i)),...
- ', z=',num2str(sysvector.IMU_GyroZ(i)),']'];
- acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.SENS_BaroAlt(i)),'m, GPS: ',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]'];
- acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.ATT_Roll(i).*180./3.14159),...
- ', Pitch=',num2str(sysvector.ATT_Pitch(i).*180./3.14159),...
- ', Yaw=',num2str(sysvector.ATT_Yaw(i).*180./3.14159),']'];
- acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:');
- %for j=1:4
- acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Roll(i)),','];
- acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Pitch(i)),','];
- acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Yaw(i)),','];
- acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Thrust(i)),','];
- %end
- acstate{7,:}=[acstate{7,:},']'];
- acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/s]:');
- %for j=1:8
- acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out0(i)),','];
- acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out1(i)),','];
- acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out2(i)),','];
- acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out3(i)),','];
- acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out4(i)),','];
- acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out5(i)),','];
- acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out6(i)),','];
- acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out7(i)),','];
- %end
- acstate{8,:}=[acstate{8,:},']'];
- acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.AIRS_IndSpeed(i)),', TAS: ',num2str(sysvector.AIRS_TrueSpeed(i)),']'];
-
- set(h.edits.AircraftState,'String',acstate);
-
- %**********************************************************************
- % GPS Plot Update
- %**********************************************************************
- %Plot traveled path, and and time.
- figure(h.figures(2));
- hold on;
- if(CurTime>mintime+1) %the +1 is only a small bugfix
- h.pathline=plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:i))*fconv_gpslatlong, ...
- double(sysvector.GPS_Lon(imintime:i))*fconv_gpslatlong, ...
- double(sysvector.GPS_Alt(imintime:i))*fconv_gpsalt,'b','LineWidth',2);
- end;
- hold off
- %Plot current position
- newpoint=[double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Alt(i))*fconv_gpsalt];
- if(numel(h.pathpoints)<=3) %empty path
- h.pathpoints(1,1:3)=newpoint;
- else %Not empty, append new point
- h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint;
- end
- axes(h.axes(1));
- line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20);
-
-
- % Plot current time (small label next to current gps position)
- textdesc=strcat(' t=',num2str(time(i)),'s');
- if(isvalidhandle(h.markertext))
- delete(h.markertext); %delete old text
- end
- h.markertext=text(double(sysvector.GPS_Lat(i))*fconv_gpslatlong,double(sysvector.GPS_Lon(i))*fconv_gpslatlong,...
- double(sysvector.GPS_Alt(i))*fconv_gpsalt,textdesc);
- set(h.edits.CurTime,'String',CurTime);
-
- %**********************************************************************
- % Plot the lines showing the current time in the 2-d plots
- %**********************************************************************
- for i=2:NrAxes
- if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end
- ylims=get(h.axes(i),'YLim');
- h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black');
- set(h.markerline(i),'parent',h.axes(i));
- end
-
- set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);
-end
-
-%% ************************************************************************
-% MINMAXTIME CALLBACK (nested function)
-% ************************************************************************
-function minmaxtime_callback(hObj,event) %#ok<INUSL>
- new_mintime=get(h.sliders.MinTime,'Value');
- new_maxtime=get(h.sliders.MaxTime,'Value');
-
- %Safety checks:
- bErr=false;
- %1: mintime must be < maxtime
- if((new_mintime>maxtime) || (new_maxtime<mintime))
- set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than maxtime! Values were not changed.','BackgroundColor','red');
- bErr=true;
- else
- %2: MinTime must be <=CurTime
- if(new_mintime>CurTime)
- set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red');
- mintime=new_mintime;
- CurTime=mintime;
- bErr=true;
- end
- %3: MaxTime must be >CurTime
- if(new_maxtime<CurTime)
- set(h.labels.GUIState,'String','Error: Maxtime cannot be smaller than CurTime! CurTime set to new maxtime.','BackgroundColor','red');
- maxtime=new_maxtime;
- CurTime=maxtime;
- bErr=true;
- end
- end
-
- if(bErr==false)
- maxtime=new_maxtime;
- mintime=new_mintime;
- end
-
- %Needs to be done in case values were reset above
- set(h.sliders.MinTime,'Value',mintime);
- set(h.sliders.MaxTime,'Value',maxtime);
-
- %Update curtime-slider
- set(h.sliders.CurTime,'Value',CurTime);
- set(h.sliders.CurTime,'Max',maxtime);
- set(h.sliders.CurTime,'Min',mintime);
- temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min');
- set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); %Set Stepsize to constant [in seconds]
-
- %update edit fields
- set(h.edits.CurTime,'String',get(h.sliders.CurTime,'Value'));
- set(h.edits.MinTime,'String',get(h.sliders.MinTime,'Value'));
- set(h.edits.MaxTime,'String',get(h.sliders.MaxTime,'Value'));
-
- %Finally, we have to redraw. Update time indices first.
- [imintime,imaxtime]=FindMinMaxTimeIndices();
- DrawRawData(); %Rawdata only
- DrawCurrentAircraftState(); %path info & markers
-end
-
-
-%% ************************************************************************
-% CURTIME CALLBACK (nested function)
-% ************************************************************************
-function curtime_callback(hObj,event) %#ok<INUSL>
- %find current time
- if(hObj==h.sliders.CurTime)
- CurTime=get(h.sliders.CurTime,'Value');
- elseif (hObj==h.edits.CurTime)
- temp=str2num(get(h.edits.CurTime,'String'));
- if(temp<maxtime && temp>mintime)
- CurTime=temp;
- else
- %Error
- set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red');
- end
- else
- %Error
- set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red');
- end
-
- set(h.sliders.CurTime,'Value',CurTime);
- set(h.edits.CurTime,'String',num2str(CurTime));
-
- %Redraw time markers, but don't have to redraw the whole raw data
- DrawCurrentAircraftState();
-end
-
-%% ************************************************************************
-% FINDMINMAXINDICES (nested function)
-% ************************************************************************
-function [idxmin,idxmax] = FindMinMaxTimeIndices()
- for i=1:size(sysvector.TIME_StartTime,1)
- if time(i)>=mintime; idxmin=i; break; end
- end
- for i=1:size(sysvector.TIME_StartTime,1)
- if maxtime==0; idxmax=size(sysvector.TIME_StartTime,1); break; end
- if time(i)>=maxtime; idxmax=i; break; end
- end
- mintime=time(idxmin);
- maxtime=time(idxmax);
-end
-
-%% ************************************************************************
-% ISVALIDHANDLE (nested function)
-% ************************************************************************
-function isvalid = isvalidhandle(handle)
- if(exist(varname(handle))>0 && length(ishandle(handle))>0)
- if(ishandle(handle)>0)
- if(handle>0.0)
- isvalid=true;
- return;
- end
- end
- end
- isvalid=false;
-end
-
-%% ************************************************************************
-% JUST SOME SMALL HELPER FUNCTIONS (nested function)
-% ************************************************************************
-function out = varname(var)
- out = inputname(1);
-end
-
-%This is the end of the matlab file / the main function
-end
+% This Matlab Script can be used to import the binary logged values of the +% PX4FMU into data that can be plotted and analyzed. + +%% ************************************************************************ +% PX4LOG_PLOTSCRIPT: Main function +% ************************************************************************ +function PX4Log_Plotscript + +% Clear everything +clc +clear all +close all + +% ************************************************************************ +% SETTINGS +% ************************************************************************ + +% Set the path to your sysvector.bin file here +filePath = 'log001.bin'; + +% Set the minimum and maximum times to plot here [in seconds] +mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start] +maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end] + +%Determine which data to plot. Not completely implemented yet. +bDisplayGPS=true; + +%conversion factors +fconv_gpsalt=1; %[mm] to [m] +fconv_gpslatlong=1; %[gps_raw_position_unit] to [deg] +fconv_timestamp=1E-6; % [microseconds] to [seconds] + +% ************************************************************************ +% Import the PX4 logs +% ************************************************************************ +ImportPX4LogData(); + +%Translate min and max plot times to indices +time=double(sysvector.TIME_StartTime) .*fconv_timestamp; +mintime_log=time(1); %The minimum time/timestamp found in the log +maxtime_log=time(end); %The maximum time/timestamp found in the log +CurTime=mintime_log; %The current time at which to draw the aircraft position + +[imintime,imaxtime]=FindMinMaxTimeIndices(); + +% ************************************************************************ +% PLOT & GUI SETUP +% ************************************************************************ +NrFigures=5; +NrAxes=10; +h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively +h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively +h.pathpoints=[]; % Temporary initiliazation of path points + +% Setup the GUI to control the plots +InitControlGUI(); +% Setup the plotting-GUI (figures, axes) itself. +InitPlotGUI(); + +% ************************************************************************ +% DRAW EVERYTHING +% ************************************************************************ +DrawRawData(); +DrawCurrentAircraftState(); + +%% ************************************************************************ +% *** END OF MAIN SCRIPT *** +% NESTED FUNCTION DEFINTIONS FROM HERE ON +% ************************************************************************ + + +%% ************************************************************************ +% IMPORTPX4LOGDATA (nested function) +% ************************************************************************ +% Attention: This is the import routine for firmware from ca. 03/2013. +% Other firmware versions might require different import +% routines. + +%% ************************************************************************ +% IMPORTPX4LOGDATA (nested function) +% ************************************************************************ +% Attention: This is the import routine for firmware from ca. 03/2013. +% Other firmware versions might require different import +% routines. + +function ImportPX4LogData() + + % ************************************************************************ + % RETRIEVE SYSTEM VECTOR + % ************************************************************************* + % //All measurements in NED frame + + % Convert to CSV + %arg1 = 'log-fx61-20130721-2.bin'; + arg1 = filePath; + delim = ','; + time_field = 'TIME'; + data_file = 'data.csv'; + csv_null = ''; + + if not(exist(data_file, 'file')) + s = system( sprintf('python sdlog2_dump.py "%s" -f "%s" -t"%s" -d"%s" -n"%s"', arg1, data_file, time_field, delim, csv_null) ); + end + + if exist(data_file, 'file') + + %data = csvread(data_file); + sysvector = tdfread(data_file, ','); + + % shot the flight time + time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1); + time_s = uint64(time_us*1e-6); + time_m = uint64(time_s/60); + time_s = time_s - time_m * 60; + + disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]); + + disp(['logfile conversion finished.' char(10)]); + else + disp(['file: ' data_file ' does not exist' char(10)]); + end +end + +%% ************************************************************************ +% INITCONTROLGUI (nested function) +% ************************************************************************ +%Setup central control GUI components to control current time where data is shown +function InitControlGUI() + %********************************************************************** + % GUI size definitions + %********************************************************************** + dxy=5; %margins + %Panel: Plotctrl + dlabels=120; + dsliders=200; + dedits=80; + hslider=20; + + hpanel1=40; %panel1 + hpanel2=220;%panel2 + hpanel3=3*hslider+4*dxy+3*dxy;%panel3. + + width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width + height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height + + %********************************************************************** + % Create GUI + %********************************************************************** + h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI'); + h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1)); + h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1)); + h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1)); + + %%Control GUI-elements + %Slider: Current time + h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],... + 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel); + temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); + set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); + h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),... + 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel); + + %Slider: MaxTime + h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],... + 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),... + 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + + %Slider: MinTime + h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],... + 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),... + 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + + %%Current data/state GUI-elements (Multiline-edit-box) + h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',... + 'HorizontalAlignment','left','parent',h.aircraftstatepanel); + + h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',... + 'HorizontalAlignment','left','parent',h.guistatepanel); + +end + +%% ************************************************************************ +% INITPLOTGUI (nested function) +% ************************************************************************ +function InitPlotGUI() + + % Setup handles to lines and text + h.markertext=[]; + templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array + h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively + h.markerline(1:NrAxes)=0.0; + + % Setup all other figures and axes for plotting + % PLOT WINDOW 1: GPS POSITION + h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position'); + h.axes(1)=axes(); + set(h.axes(1),'Parent',h.figures(2)); + + % PLOT WINDOW 2: IMU, baro altitude + h.figures(3)=figure('Name', 'IMU / Baro Altitude'); + h.axes(2)=subplot(4,1,1); + h.axes(3)=subplot(4,1,2); + h.axes(4)=subplot(4,1,3); + h.axes(5)=subplot(4,1,4); + set(h.axes(2:5),'Parent',h.figures(3)); + + % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... + h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds'); + h.axes(6)=subplot(4,1,1); + h.axes(7)=subplot(4,1,2); + h.axes(8)=subplot(4,1,3); + h.axes(9)=subplot(4,1,4); + set(h.axes(6:9),'Parent',h.figures(4)); + + % PLOT WINDOW 4: LOG STATS + h.figures(5) = figure('Name', 'Log Statistics'); + h.axes(10)=subplot(1,1,1); + set(h.axes(10:10),'Parent',h.figures(5)); + +end + +%% ************************************************************************ +% DRAWRAWDATA (nested function) +% ************************************************************************ +%Draws the raw data from the sysvector, but does not add any +%marker-lines or so +function DrawRawData() + % ************************************************************************ + % PLOT WINDOW 1: GPS POSITION & GUI + % ************************************************************************ + figure(h.figures(2)); + % Only plot GPS data if available + if (sum(double(sysvector.GPS_Lat(imintime:imaxtime)))>0) && (bDisplayGPS) + %Draw data + plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:imaxtime))*fconv_gpslatlong, ... + double(sysvector.GPS_Lon(imintime:imaxtime))*fconv_gpslatlong, ... + double(sysvector.GPS_Alt(imintime:imaxtime))*fconv_gpsalt,'r.'); + title(h.axes(1),'GPS Position Data(if available)'); + xlabel(h.axes(1),'Latitude [deg]'); + ylabel(h.axes(1),'Longitude [deg]'); + zlabel(h.axes(1),'Altitude above MSL [m]'); + grid on + + %Reset path + h.pathpoints=0; + end + + % ************************************************************************ + % PLOT WINDOW 2: IMU, baro altitude + % ************************************************************************ + figure(h.figures(3)); + plot(h.axes(2),time(imintime:imaxtime),[sysvector.IMU_MagX(imintime:imaxtime), sysvector.IMU_MagY(imintime:imaxtime), sysvector.IMU_MagZ(imintime:imaxtime)]); + title(h.axes(2),'Magnetometers [Gauss]'); + legend(h.axes(2),'x','y','z'); + plot(h.axes(3),time(imintime:imaxtime),[sysvector.IMU_AccX(imintime:imaxtime), sysvector.IMU_AccY(imintime:imaxtime), sysvector.IMU_AccZ(imintime:imaxtime)]); + title(h.axes(3),'Accelerometers [m/s]'); + legend(h.axes(3),'x','y','z'); + plot(h.axes(4),time(imintime:imaxtime),[sysvector.IMU_GyroX(imintime:imaxtime), sysvector.IMU_GyroY(imintime:imaxtime), sysvector.IMU_GyroZ(imintime:imaxtime)]); + title(h.axes(4),'Gyroscopes [rad/s]'); + legend(h.axes(4),'x','y','z'); + plot(h.axes(5),time(imintime:imaxtime),sysvector.SENS_BaroAlt(imintime:imaxtime),'color','blue'); + if(bDisplayGPS) + hold on; + plot(h.axes(5),time(imintime:imaxtime),double(sysvector.GPS_Alt(imintime:imaxtime)).*fconv_gpsalt,'color','red'); + hold off + legend('Barometric Altitude [m]','GPS Altitude [m]'); + else + legend('Barometric Altitude [m]'); + end + title(h.axes(5),'Altitude above MSL [m]'); + + % ************************************************************************ + % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... + % ************************************************************************ + figure(h.figures(4)); + %Attitude Estimate + plot(h.axes(6),time(imintime:imaxtime), [sysvector.ATT_Roll(imintime:imaxtime), sysvector.ATT_Pitch(imintime:imaxtime), sysvector.ATT_Yaw(imintime:imaxtime)] .*180./3.14159); + title(h.axes(6),'Estimated attitude [deg]'); + legend(h.axes(6),'roll','pitch','yaw'); + %Actuator Controls + plot(h.axes(7),time(imintime:imaxtime), [sysvector.ATTC_Roll(imintime:imaxtime), sysvector.ATTC_Pitch(imintime:imaxtime), sysvector.ATTC_Yaw(imintime:imaxtime), sysvector.ATTC_Thrust(imintime:imaxtime)]); + title(h.axes(7),'Actuator control [-]'); + legend(h.axes(7),'ATT CTRL Roll [-1..+1]','ATT CTRL Pitch [-1..+1]','ATT CTRL Yaw [-1..+1]','ATT CTRL Thrust [0..+1]'); + %Actuator Controls + plot(h.axes(8),time(imintime:imaxtime), [sysvector.OUT0_Out0(imintime:imaxtime), sysvector.OUT0_Out1(imintime:imaxtime), sysvector.OUT0_Out2(imintime:imaxtime), sysvector.OUT0_Out3(imintime:imaxtime), sysvector.OUT0_Out4(imintime:imaxtime), sysvector.OUT0_Out5(imintime:imaxtime), sysvector.OUT0_Out6(imintime:imaxtime), sysvector.OUT0_Out7(imintime:imaxtime)]); + title(h.axes(8),'Actuator PWM (raw-)outputs [s]'); + legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8'); + set(h.axes(8), 'YLim',[800 2200]); + %Airspeeds + plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_IndSpeed(imintime:imaxtime)); + hold on + plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_TrueSpeed(imintime:imaxtime)); + hold off + %add GPS total airspeed here + title(h.axes(9),'Airspeed [m/s]'); + legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed'); + %calculate time differences and plot them + intervals = zeros(0,imaxtime - imintime); + for k = imintime+1:imaxtime + intervals(k) = time(k) - time(k-1); + end + plot(h.axes(10), time(imintime:imaxtime), intervals); + + %Set same timescale for all plots + for i=2:NrAxes + set(h.axes(i),'XLim',[mintime maxtime]); + end + + set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); +end + +%% ************************************************************************ +% DRAWCURRENTAIRCRAFTSTATE(nested function) +% ************************************************************************ +function DrawCurrentAircraftState() + %find current data index + i=find(time>=CurTime,1,'first'); + + %********************************************************************** + % Current aircraft state label update + %********************************************************************** + acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.GPS_Lat(i))*fconv_gpslatlong),', ',... + 'lon=',num2str(double(sysvector.GPS_Lon(i))*fconv_gpslatlong),', ',... + 'alt=',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]']; + acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.IMU_MagX(i)),... + ', y=',num2str(sysvector.IMU_MagY(i)),... + ', z=',num2str(sysvector.IMU_MagZ(i)),']']; + acstate{3,:}=[sprintf('%s \t\t','Accels[m/s]'),'[x=',num2str(sysvector.IMU_AccX(i)),... + ', y=',num2str(sysvector.IMU_AccY(i)),... + ', z=',num2str(sysvector.IMU_AccZ(i)),']']; + acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.IMU_GyroX(i)),... + ', y=',num2str(sysvector.IMU_GyroY(i)),... + ', z=',num2str(sysvector.IMU_GyroZ(i)),']']; + acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.SENS_BaroAlt(i)),'m, GPS: ',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]']; + acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.ATT_Roll(i).*180./3.14159),... + ', Pitch=',num2str(sysvector.ATT_Pitch(i).*180./3.14159),... + ', Yaw=',num2str(sysvector.ATT_Yaw(i).*180./3.14159),']']; + acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:'); + %for j=1:4 + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Roll(i)),',']; + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Pitch(i)),',']; + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Yaw(i)),',']; + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Thrust(i)),',']; + %end + acstate{7,:}=[acstate{7,:},']']; + acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/s]:'); + %for j=1:8 + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out0(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out1(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out2(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out3(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out4(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out5(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out6(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out7(i)),',']; + %end + acstate{8,:}=[acstate{8,:},']']; + acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.AIRS_IndSpeed(i)),', TAS: ',num2str(sysvector.AIRS_TrueSpeed(i)),']']; + + set(h.edits.AircraftState,'String',acstate); + + %********************************************************************** + % GPS Plot Update + %********************************************************************** + %Plot traveled path, and and time. + figure(h.figures(2)); + hold on; + if(CurTime>mintime+1) %the +1 is only a small bugfix + h.pathline=plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:i))*fconv_gpslatlong, ... + double(sysvector.GPS_Lon(imintime:i))*fconv_gpslatlong, ... + double(sysvector.GPS_Alt(imintime:i))*fconv_gpsalt,'b','LineWidth',2); + end; + hold off + %Plot current position + newpoint=[double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Alt(i))*fconv_gpsalt]; + if(numel(h.pathpoints)<=3) %empty path + h.pathpoints(1,1:3)=newpoint; + else %Not empty, append new point + h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint; + end + axes(h.axes(1)); + line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20); + + + % Plot current time (small label next to current gps position) + textdesc=strcat(' t=',num2str(time(i)),'s'); + if(isvalidhandle(h.markertext)) + delete(h.markertext); %delete old text + end + h.markertext=text(double(sysvector.GPS_Lat(i))*fconv_gpslatlong,double(sysvector.GPS_Lon(i))*fconv_gpslatlong,... + double(sysvector.GPS_Alt(i))*fconv_gpsalt,textdesc); + set(h.edits.CurTime,'String',CurTime); + + %********************************************************************** + % Plot the lines showing the current time in the 2-d plots + %********************************************************************** + for i=2:NrAxes + if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end + ylims=get(h.axes(i),'YLim'); + h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black'); + set(h.markerline(i),'parent',h.axes(i)); + end + + set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); +end + +%% ************************************************************************ +% MINMAXTIME CALLBACK (nested function) +% ************************************************************************ +function minmaxtime_callback(hObj,event) %#ok<INUSL> + new_mintime=get(h.sliders.MinTime,'Value'); + new_maxtime=get(h.sliders.MaxTime,'Value'); + + %Safety checks: + bErr=false; + %1: mintime must be < maxtime + if((new_mintime>maxtime) || (new_maxtime<mintime)) + set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than maxtime! Values were not changed.','BackgroundColor','red'); + bErr=true; + else + %2: MinTime must be <=CurTime + if(new_mintime>CurTime) + set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red'); + mintime=new_mintime; + CurTime=mintime; + bErr=true; + end + %3: MaxTime must be >CurTime + if(new_maxtime<CurTime) + set(h.labels.GUIState,'String','Error: Maxtime cannot be smaller than CurTime! CurTime set to new maxtime.','BackgroundColor','red'); + maxtime=new_maxtime; + CurTime=maxtime; + bErr=true; + end + end + + if(bErr==false) + maxtime=new_maxtime; + mintime=new_mintime; + end + + %Needs to be done in case values were reset above + set(h.sliders.MinTime,'Value',mintime); + set(h.sliders.MaxTime,'Value',maxtime); + + %Update curtime-slider + set(h.sliders.CurTime,'Value',CurTime); + set(h.sliders.CurTime,'Max',maxtime); + set(h.sliders.CurTime,'Min',mintime); + temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); + set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); %Set Stepsize to constant [in seconds] + + %update edit fields + set(h.edits.CurTime,'String',get(h.sliders.CurTime,'Value')); + set(h.edits.MinTime,'String',get(h.sliders.MinTime,'Value')); + set(h.edits.MaxTime,'String',get(h.sliders.MaxTime,'Value')); + + %Finally, we have to redraw. Update time indices first. + [imintime,imaxtime]=FindMinMaxTimeIndices(); + DrawRawData(); %Rawdata only + DrawCurrentAircraftState(); %path info & markers +end + + +%% ************************************************************************ +% CURTIME CALLBACK (nested function) +% ************************************************************************ +function curtime_callback(hObj,event) %#ok<INUSL> + %find current time + if(hObj==h.sliders.CurTime) + CurTime=get(h.sliders.CurTime,'Value'); + elseif (hObj==h.edits.CurTime) + temp=str2num(get(h.edits.CurTime,'String')); + if(temp<maxtime && temp>mintime) + CurTime=temp; + else + %Error + set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red'); + end + else + %Error + set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red'); + end + + set(h.sliders.CurTime,'Value',CurTime); + set(h.edits.CurTime,'String',num2str(CurTime)); + + %Redraw time markers, but don't have to redraw the whole raw data + DrawCurrentAircraftState(); +end + +%% ************************************************************************ +% FINDMINMAXINDICES (nested function) +% ************************************************************************ +function [idxmin,idxmax] = FindMinMaxTimeIndices() + for i=1:size(sysvector.TIME_StartTime,1) + if time(i)>=mintime; idxmin=i; break; end + end + for i=1:size(sysvector.TIME_StartTime,1) + if maxtime==0; idxmax=size(sysvector.TIME_StartTime,1); break; end + if time(i)>=maxtime; idxmax=i; break; end + end + mintime=time(idxmin); + maxtime=time(idxmax); +end + +%% ************************************************************************ +% ISVALIDHANDLE (nested function) +% ************************************************************************ +function isvalid = isvalidhandle(handle) + if(exist(varname(handle))>0 && length(ishandle(handle))>0) + if(ishandle(handle)>0) + if(handle>0.0) + isvalid=true; + return; + end + end + end + isvalid=false; +end + +%% ************************************************************************ +% JUST SOME SMALL HELPER FUNCTIONS (nested function) +% ************************************************************************ +function out = varname(var) + out = inputname(1); +end + +%This is the end of the matlab file / the main function +end diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py index 5b1e55e22..5b1e55e22 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2/sdlog2_dump.py diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index d5a50cfae..4f997e96b 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -18,6 +18,12 @@ MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/boards/px4fmu-v2 MODULES += drivers/px4io +MODULES += drivers/rgbled +MODULES += drivers/mpu6000 +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests @@ -31,6 +37,9 @@ MODULES += systemcmds/hw_ver MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter # # Transitional support - add commands from the NuttX export archive. diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index e56b14ba4..3bede8a1f 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -141,9 +141,9 @@ #define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) #define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx * otherwise frequency is 2xAPBx. @@ -209,8 +209,8 @@ #define GPIO_USART3_RX GPIO_USART3_RX_3 #define GPIO_USART3_TX GPIO_USART3_TX_3 -#define GPIO_USART2_RTS GPIO_USART2_RTS_2 -#define GPIO_USART2_CTS GPIO_USART2_CTS_2 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 #define GPIO_UART4_RX GPIO_UART4_RX_1 #define GPIO_UART4_TX GPIO_UART4_TX_1 diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index f73a3ef01..771f2128f 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -86,6 +86,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(-1), + _class_instance(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) @@ -102,6 +103,9 @@ Airspeed::~Airspeed() /* make sure we are truly inactive */ stop(); + if (_class_instance != -1) + unregister_class_devname(AIRSPEED_DEVICE_PATH, _class_instance); + /* free any existing reports */ if (_reports != nullptr) delete _reports; @@ -126,13 +130,23 @@ Airspeed::init() if (_reports == nullptr) goto out; - /* get a publish handle on the airspeed topic */ - differential_pressure_s zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report); + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(AIRSPEED_DEVICE_PATH); + + /* publication init */ + if (_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct differential_pressure_s arp; + measure(); + _reports->get(&arp); - if (_airspeed_pub < 0) - warnx("failed to create airspeed sensor object. Did you start uOrb?"); + /* measurement will have generated a report, publish */ + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); + + if (_airspeed_pub < 0) + warnx("failed to create airspeed sensor object. uORB started?"); + } ret = OK; /* sensor is ok, but we don't really know if it is within range */ diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index c341aa2c6..c27b1bcd8 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -127,6 +127,8 @@ protected: orb_advert_t _airspeed_pub; + int _class_instance; + unsigned _conversion_interval; perf_counter_t _sample_perf; diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 1590cc182..e43a34805 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -153,6 +153,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + int _class_instance; unsigned _current_lowpass; unsigned _current_range; @@ -238,6 +239,7 @@ BMA180::BMA180(int bus, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _class_instance(-1), _current_lowpass(0), _current_range(0), _sample_perf(perf_alloc(PC_ELAPSED, "bma180_read")) @@ -282,11 +284,6 @@ BMA180::init() if (_reports == nullptr) goto out; - /* advertise sensor topic */ - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -322,6 +319,19 @@ BMA180::init() ret = ERROR; } + _class_instance = register_class_devname(ACCEL_DEVICE_PATH); + + /* advertise sensor topic, measure manually to initialize valid report */ + measure(); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + struct accel_report arp; + _reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + } + out: return ret; } @@ -723,7 +733,8 @@ BMA180::measure() poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); + if (_accel_topic > 0 && !(_pub_blocked)) + orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); /* stop the perf counter */ perf_end(_sample_perf); diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 7954ce5ab..b157b3f18 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -38,6 +38,7 @@ */ #include "device.h" +#include "drivers/drv_device.h" #include <sys/ioctl.h> #include <arch/irq.h> @@ -93,6 +94,7 @@ CDev::CDev(const char *name, Device(name, irq), // public // protected + _pub_blocked(false), // private _devname(devname), _registered(false), @@ -256,6 +258,14 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg) case DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; return OK; + break; + case DEVIOCSPUBBLOCK: + _pub_blocked = (arg != 0); + return OK; + break; + case DEVIOCGPUBBLOCK: + return _pub_blocked; + break; } return -ENOTTY; diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index c3ee77b1c..683455149 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 0235f6284..d99f22922 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -415,6 +415,8 @@ protected: */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + bool _pub_blocked; /**< true if publishing should be blocked */ + private: static const unsigned _max_pollwaiters = 8; diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h new file mode 100644 index 000000000..b310beb74 --- /dev/null +++ b/src/drivers/drv_device.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 drv_device.h + * + * Generic device / sensor interface. + */ + +#ifndef _DRV_DEVICE_H +#define _DRV_DEVICE_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +/* + * ioctl() definitions + */ + +#define _DEVICEIOCBASE (0x100) +#define _DEVICEIOC(_n) (_IOC(_DEVICEIOCBASE, _n)) + +/** ask device to stop publishing */ +#define DEVIOCSPUBBLOCK _DEVICEIOC(0) + +/** check publication block status */ +#define DEVIOCGPUBBLOCK _DEVICEIOC(1) + +#endif /* _DRV_DEVICE_H */ diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index de371bf32..8bbef5cfa 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -184,8 +184,10 @@ ETSAirspeed::collect() report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + if (_airspeed_pub > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } new_report(report); diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6b72d560f..a736cbdf6 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -289,11 +289,13 @@ GPS::task_main() //no time and satellite information simulated - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (!(_pub_blocked)) { + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } } usleep(2e5); @@ -330,11 +332,14 @@ GPS::task_main() while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + if (!(_pub_blocked)) { + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } } last_rate_count++; diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 2e2cbc8dd..2360ff39b 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * Copyright (c) 2012-2014 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 @@ -41,6 +39,9 @@ /** * @file gps_helper.cpp + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> */ float diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 73d4b889c..cfb9e0d43 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * Copyright (c) 2012-2014 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 @@ -35,6 +33,8 @@ /** * @file gps_helper.h + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef GPS_HELPER_H diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index 097db2abf..82c67d40a 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2014 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 diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 56b702ea6..c90ecbe28 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,7 +31,12 @@ * ****************************************************************************/ -/* @file mkt.cpp */ +/** + * @file mtk.cpp + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + */ #include <unistd.h> #include <stdio.h> diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h index b5cfbf0a6..a2d5e27bb 100644 --- a/src/drivers/gps/mtk.h +++ b/src/drivers/gps/mtk.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,7 +31,12 @@ * ****************************************************************************/ -/* @file mtk.h */ +/** + * @file mtk.cpp + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + */ #ifndef MTK_H_ #define MTK_H_ diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 86291901c..8a2afecb7 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Anton Babushkin <anton.babushkin@me.com> + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,8 +37,13 @@ * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description * including Prototol Specification. * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf */ + #include <assert.h> #include <math.h> #include <poll.h> diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 76ef873a3..79a904f4a 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Anton Babushkin <anton.babushkin@me.com> + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,7 +31,17 @@ * ****************************************************************************/ -/* @file U-Blox protocol definitions */ +/** + * @file ubx.h + * + * U-Blox protocol definition. Following u-blox 6/7 Receiver Description + * including Prototol Specification. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * + */ #ifndef UBX_H_ #define UBX_H_ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 9b9c11af2..4c85c0cda 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -381,16 +381,6 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the mag topic if we are - * the primary mag */ - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); - } ret = OK; /* sensor is ok, but not calibrated */ @@ -867,9 +857,17 @@ HMC5883::collect() /* z remains z */ new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; - if (_mag_topic != -1) { - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_mag_topic != -1) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } else { + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report); + + if (_mag_topic < 0) + debug("failed to create sensor_mag publication"); + } } /* post a report to the ring */ @@ -1140,10 +1138,12 @@ int HMC5883::check_calibration() SUBSYSTEM_TYPE_MAG}; static orb_advert_t pub = -1; - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); + if (!(_pub_blocked)) { + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } } } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 670e51b97..90c3db9ae 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -379,15 +379,24 @@ L3GD20::init() goto out; _class_instance = register_class_devname(GYRO_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* advertise sensor topic */ - struct gyro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); - } reset(); + measure(); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _reports->get(&grp); + + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + + if (_gyro_topic < 0) + debug("failed to create sensor_gyro publication"); + + } + ret = OK; out: return ret; @@ -894,8 +903,10 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - if (_gyro_topic > 0) + if (_gyro_topic > 0 && !(_pub_blocked)) { + /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + } _read++; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 969b5e25f..4dee7649b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -277,15 +277,15 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - int _class_instance; + int _accel_class_instance; unsigned _accel_read; unsigned _mag_read; perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; + perf_counter_t _reg7_resets; perf_counter_t _extreme_values; perf_counter_t _accel_reschedules; @@ -295,8 +295,8 @@ private: // expceted values of reg1 and reg7 to catch in-flight // brownouts of the sensor - uint8_t _reg7_expected; uint8_t _reg1_expected; + uint8_t _reg7_expected; // accel logging int _accel_log_fd; @@ -500,7 +500,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _class_instance(-1), + _accel_class_instance(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), @@ -551,8 +551,8 @@ LSM303D::~LSM303D() if (_mag_reports != nullptr) delete _mag_reports; - if (_class_instance != -1) - unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance); + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance); delete _mag; @@ -562,13 +562,13 @@ LSM303D::~LSM303D() perf_free(_reg1_resets); perf_free(_reg7_resets); perf_free(_extreme_values); + perf_free(_accel_reschedules); } int LSM303D::init() { int ret = ERROR; - int mag_ret; /* do SPI init (and probe) first */ if (SPI::init() != OK) { @@ -597,13 +597,37 @@ LSM303D::init() goto out; } - _class_instance = register_class_devname(ACCEL_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - // we are the primary accel device, so advertise to - // the ORB - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); + /* fill report structures */ + measure(); + + if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct mag_report mrp; + _mag_reports->get(&mrp); + + /* measurement will have generated a report, publish */ + _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp); + + if (_mag->_mag_topic < 0) + debug("failed to create sensor_mag publication"); + + } + + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + + if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + + if (_accel_topic < 0) + debug("failed to create sensor_accel publication"); + } out: @@ -727,7 +751,7 @@ LSM303D::check_extremes(const accel_report *arb) _last_log_us = now; ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n", (unsigned long long)arb->timestamp, - arb->x, arb->y, arb->z, + (double)arb->x, (double)arb->y, (double)arb->z, (int)arb->x_raw, (int)arb->y_raw, (int)arb->z_raw, @@ -1517,8 +1541,8 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_accel_topic != -1) { - /* publish for subscribers */ + if (_accel_topic > 0 && !(_pub_blocked)) { + /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } @@ -1591,8 +1615,8 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_mag->_mag_topic != -1) { - /* publish for subscribers */ + if (_mag->_mag_topic > 0 && !(_pub_blocked)) { + /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } @@ -1707,13 +1731,6 @@ LSM303D_mag::init() goto out; _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); - if (_mag_class_instance == CLASS_DEVICE_PRIMARY) { - // we are the primary mag device, so advertise to - // the ORB - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - } out: return ret; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 9251cff7b..05ae21c1f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -208,8 +208,10 @@ MEASAirspeed::collect() report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + if (_airspeed_pub > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } new_report(report); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index ec5f77d74..705e98eea 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -705,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit; foundMotorCount++; - if (Motor[i].MaxPWM == 250) { + if ((Motor[i].MaxPWM & 252) == 248) { Motor[i].Version = BLCTRL_NEW; } else { diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bbc595af4..ac75682c4 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -443,7 +443,6 @@ int MPU6000::init() { int ret; - int gyro_ret; /* do SPI init (and probe) first */ ret = SPI::init(); @@ -488,16 +487,36 @@ MPU6000::init() return ret; } - /* fetch an initial set of measurements for advertisement */ + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + measure(); - _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(&ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); - } + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + + if (_accel_topic < 0) + debug("failed to create sensor_accel publication"); + + } + + if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); + + _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + + if (_gyro->_gyro_topic < 0) + debug("failed to create sensor_gyro publication"); + + } out: return ret; @@ -1307,10 +1326,13 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - if (_accel_topic != -1) { + if (_accel_topic > 0 && !(_pub_blocked)) { + /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } - if (_gyro->_gyro_topic != -1) { + + if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) { + /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } @@ -1331,6 +1353,7 @@ MPU6000::print_info() MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent), + _gyro_topic(-1), _gyro_class_instance(-1) { } @@ -1356,11 +1379,6 @@ MPU6000_gyro::init() } _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); - if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) { - gyro_report gr; - memset(&gr, 0, sizeof(gr)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } out: return ret; diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 6326cf7fc..0ef056273 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -90,6 +90,7 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +#define MS5611_BARO_DEVICE_PATH "/dev/ms5611" class MS5611 : public device::CDev { @@ -132,6 +133,8 @@ protected: orb_advert_t _baro_topic; + int _class_instance; + perf_counter_t _sample_perf; perf_counter_t _measure_perf; perf_counter_t _comms_errors; @@ -192,7 +195,7 @@ protected: extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : - CDev("MS5611", BARO_DEVICE_PATH), + CDev("MS5611", MS5611_BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), @@ -204,6 +207,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _SENS(0), _msl_pressure(101325), _baro_topic(-1), + _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), @@ -218,6 +222,9 @@ MS5611::~MS5611() /* make sure we are truly inactive */ stop_cycle(); + if (_class_instance != -1) + unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance); + /* free any existing reports */ if (_reports != nullptr) delete _reports; @@ -251,18 +258,57 @@ MS5611::init() goto out; } - /* get a publish handle on the baro topic */ - struct baro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report); + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(BARO_DEVICE_PATH); - if (_baro_topic < 0) { - debug("failed to create sensor_baro object"); - ret = -ENOSPC; - goto out; - } + struct baro_report brp; + /* do a first measurement cycle to populate reports with valid data */ + _measure_phase = 0; + _reports->flush(); + + /* this do..while is goto without goto */ + do { + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + _reports->get(&brp); + + ret = OK; + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp); + + if (_baro_topic < 0) + debug("failed to create sensor_baro publication"); + } + + } while (0); - ret = OK; out: return ret; } @@ -670,7 +716,10 @@ MS5611::collect() report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + if (_baro_topic > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } if (_reports->force(&report)) { perf_count(_buffer_overflows); @@ -812,7 +861,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(BARO_DEVICE_PATH, O_RDONLY); + fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); if (fd < 0) { warnx("can't open baro device"); goto fail; @@ -846,10 +895,10 @@ test() ssize_t sz; int ret; - int fd = open(BARO_DEVICE_PATH, O_RDONLY); + int fd = open(MS5611_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); + err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -905,7 +954,7 @@ test() void reset() { - int fd = open(BARO_DEVICE_PATH, O_RDONLY); + int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -944,10 +993,10 @@ calibrate(unsigned altitude) float pressure; float p1; - int fd = open(BARO_DEVICE_PATH, O_RDONLY); + int fd = open(MS5611_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); + err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); /* start the sensor polling at max */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 490fc8fc6..7ae61d9ef 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -42,6 +42,8 @@ #include <unistd.h> #include <stdint.h> #include <stdbool.h> +#include <dirent.h> +#include <fcntl.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -51,6 +53,7 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_device.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" @@ -491,6 +494,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; + + // Disable publication of all attached sensors + + /* list directory */ + DIR *d; + struct dirent *direntry; + d = opendir("/dev"); + if (d) { + + while ((direntry = readdir(d)) != NULL) { + + int sensfd = ::open(direntry->d_name, 0); + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + close(sensfd); + + printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); + return 1; + } } break; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f39fcf7ec..6a4429461 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -179,7 +179,7 @@ mixer_tick(void) ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) ) ); diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 80689f20c..476015f3e 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -56,6 +56,7 @@ #include <drivers/drv_gyro.h> #include <drivers/drv_accel.h> #include <drivers/drv_mag.h> +#include <drivers/drv_device.h> #include "systemlib/systemlib.h" #include "systemlib/err.h" @@ -65,6 +66,7 @@ __EXPORT int config_main(int argc, char *argv[]); static void do_gyro(int argc, char *argv[]); static void do_accel(int argc, char *argv[]); static void do_mag(int argc, char *argv[]); +static void do_device(int argc, char *argv[]); int config_main(int argc, char *argv[]) @@ -72,14 +74,12 @@ config_main(int argc, char *argv[]) if (argc >= 2) { if (!strcmp(argv[1], "gyro")) { do_gyro(argc - 2, argv + 2); - } - - if (!strcmp(argv[1], "accel")) { + } else if (!strcmp(argv[1], "accel")) { do_accel(argc - 2, argv + 2); - } - - if (!strcmp(argv[1], "mag")) { + } else if (!strcmp(argv[1], "mag")) { do_mag(argc - 2, argv + 2); + } else { + do_device(argc - 1, argv + 1); } } @@ -87,6 +87,48 @@ config_main(int argc, char *argv[]) } static void +do_device(int argc, char *argv[]) +{ + if (argc < 2) { + errx(1, "no device path provided and command provided."); + } + + int fd; + int ret; + + fd = open(argv[0], 0); + + if (fd < 0) { + warn("%s", argv[0]); + errx(1, "FATAL: no device found"); + + } else { + + if (argc == 2 && !strcmp(argv[1], "block")) { + + /* disable the device publications */ + ret = ioctl(fd, DEVIOCSPUBBLOCK, 1); + + if (ret) + errx(ret,"uORB publications could not be blocked"); + + } else if (argc == 2 && !strcmp(argv[1], "unblock")) { + + /* enable the device publications */ + ret = ioctl(fd, DEVIOCSPUBBLOCK, 0); + + if (ret) + errx(ret,"uORB publications could not be unblocked"); + + } else { + errx("no valid command: %s", argv[1]); + } + } + + exit(0); +} + +static void do_gyro(int argc, char *argv[]) { int fd; @@ -124,7 +166,7 @@ do_gyro(int argc, char *argv[]) if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if (argc == 1 && !strcmp(argv[0], "check")) { ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { |