From 01e427b17c161d8adaa38d6bdb91aecb434451f2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 16:14:32 -0700 Subject: Merge working changes into export-build branch. --- ROMFS/px4fmu_common/init.d/rc.FMU_quad_x | 67 ++++ ROMFS/px4fmu_common/init.d/rc.PX4IO | 80 ++++ ROMFS/px4fmu_common/init.d/rc.PX4IOAR | 98 +++++ ROMFS/px4fmu_common/init.d/rc.boarddetect | 66 ++++ ROMFS/px4fmu_common/init.d/rc.jig | 10 + ROMFS/px4fmu_common/init.d/rc.logging | 9 + ROMFS/px4fmu_common/init.d/rc.sensors | 34 ++ ROMFS/px4fmu_common/init.d/rc.standalone | 13 + ROMFS/px4fmu_common/init.d/rcS | 79 ++++ ROMFS/px4fmu_common/logging/logconv.m | 586 +++++++++++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_AERT.mix | 64 ++++ ROMFS/px4fmu_common/mixers/FMU_AET.mix | 60 +++ ROMFS/px4fmu_common/mixers/FMU_Q.mix | 52 +++ ROMFS/px4fmu_common/mixers/FMU_RET.mix | 53 +++ ROMFS/px4fmu_common/mixers/FMU_X5.mix | 50 +++ ROMFS/px4fmu_common/mixers/FMU_delta.mix | 50 +++ ROMFS/px4fmu_common/mixers/FMU_hex_+.mix | 7 + ROMFS/px4fmu_common/mixers/FMU_hex_x.mix | 7 + ROMFS/px4fmu_common/mixers/FMU_octo_+.mix | 7 + ROMFS/px4fmu_common/mixers/FMU_octo_x.mix | 7 + ROMFS/px4fmu_common/mixers/FMU_pass.mix | 23 ++ ROMFS/px4fmu_common/mixers/FMU_quad_+.mix | 7 + ROMFS/px4fmu_common/mixers/FMU_quad_x.mix | 7 + ROMFS/px4fmu_common/mixers/README | 154 ++++++++ ROMFS/px4fmu_default/logging/logconv.m | 586 ----------------------------- ROMFS/px4fmu_default/mixers/FMU_AERT.mix | 64 ---- ROMFS/px4fmu_default/mixers/FMU_AET.mix | 60 --- ROMFS/px4fmu_default/mixers/FMU_Q.mix | 52 --- ROMFS/px4fmu_default/mixers/FMU_RET.mix | 53 --- ROMFS/px4fmu_default/mixers/FMU_X5.mix | 50 --- ROMFS/px4fmu_default/mixers/FMU_delta.mix | 50 --- ROMFS/px4fmu_default/mixers/FMU_hex_+.mix | 7 - ROMFS/px4fmu_default/mixers/FMU_hex_x.mix | 7 - ROMFS/px4fmu_default/mixers/FMU_octo_+.mix | 7 - ROMFS/px4fmu_default/mixers/FMU_octo_x.mix | 7 - ROMFS/px4fmu_default/mixers/FMU_pass.mix | 23 -- ROMFS/px4fmu_default/mixers/FMU_quad_+.mix | 7 - ROMFS/px4fmu_default/mixers/FMU_quad_x.mix | 7 - ROMFS/px4fmu_default/mixers/README | 154 -------- 39 files changed, 1590 insertions(+), 1134 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.FMU_quad_x create mode 100644 ROMFS/px4fmu_common/init.d/rc.PX4IO create mode 100644 ROMFS/px4fmu_common/init.d/rc.PX4IOAR create mode 100644 ROMFS/px4fmu_common/init.d/rc.boarddetect create mode 100644 ROMFS/px4fmu_common/init.d/rc.jig create mode 100644 ROMFS/px4fmu_common/init.d/rc.logging create mode 100644 ROMFS/px4fmu_common/init.d/rc.sensors create mode 100644 ROMFS/px4fmu_common/init.d/rc.standalone create mode 100755 ROMFS/px4fmu_common/init.d/rcS create mode 100644 ROMFS/px4fmu_common/logging/logconv.m create mode 100644 ROMFS/px4fmu_common/mixers/FMU_AERT.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_AET.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_Q.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_RET.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_X5.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_delta.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_hex_+.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_hex_x.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_octo_+.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_octo_x.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_pass.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_quad_+.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_quad_x.mix create mode 100644 ROMFS/px4fmu_common/mixers/README delete mode 100644 ROMFS/px4fmu_default/logging/logconv.m delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_AERT.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_AET.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_Q.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_RET.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_X5.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_delta.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_hex_+.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_hex_x.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_octo_+.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_octo_x.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_pass.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_quad_+.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_quad_x.mix delete mode 100644 ROMFS/px4fmu_default/mixers/README (limited to 'ROMFS') diff --git a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x new file mode 100644 index 000000000..8787443ea --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x @@ -0,0 +1,67 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# Disable the USB interface +set USB no + +# Disable autostarting other apps +set MODE custom + +echo "[init] doing PX4FMU Quad startup..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] +then + param load /fs/microsd/parameters +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Start attitude control +# +multirotor_att_control start + +echo "[init] startup done, exiting" +exit \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/rc.PX4IO new file mode 100644 index 000000000..1e3963b9a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.PX4IO @@ -0,0 +1,80 @@ +#!nsh + +# Disable USB and autostart +set USB no +set MODE camflyer + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] +then + param load /fs/microsd/parameters +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Start the sensors. +# +sh /etc/init.d/rc.sensors + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start the commander. +# +commander start + +# +# Start GPS interface +# +gps start + +# +# Start the attitude estimator +# +kalman_demo start + +# +# Start PX4IO interface +# +px4io start + +# +# Load mixer and start controllers +# +mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +control_demo start + +# +# Start logging +# +sdlog start -s 10 + +# +# Start system state +# +if blinkm start +then + echo "using BlinkM for state indication" + blinkm systemstate +else + echo "no BlinkM found, OK." +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR new file mode 100644 index 000000000..72df68e35 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR @@ -0,0 +1,98 @@ +#!nsh +# +# Flight startup script for PX4FMU on PX4IOAR carrier board. +# + +# Disable the USB interface +set USB no + +# Disable autostarting other apps +set MODE ardrone + +echo "[init] doing PX4IOAR startup..." + +# +# Start the ORB +# +uorb start + +# +# Init the parameter storage +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] +then + param load /fs/microsd/parameters +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start the sensors. +# +sh /etc/init.d/rc.sensors + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the commander. +# +commander start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Configure PX4FMU for operation with PX4IOAR +# +fmu mode_gpio_serial + +# +# Fire up the multi rotor attitude controller +# +multirotor_att_control start + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 + +# +# Start GPS capture +# +gps start + +# +# Start logging +# +sdlog start -s 10 + +# +# Start system state +# +if blinkm start +then + echo "using BlinkM for state indication" + blinkm systemstate +else + echo "no BlinkM found, OK." +fi + +# +# startup is done; we don't want the shell because we +# use the same UART for telemetry +# +echo "[init] startup done" +exit \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.boarddetect b/ROMFS/px4fmu_common/init.d/rc.boarddetect new file mode 100644 index 000000000..f233e51df --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.boarddetect @@ -0,0 +1,66 @@ +#!nsh +# +# If we are still in flight mode, work out what airframe +# configuration we have and start up accordingly. +# +if [ $MODE != autostart ] +then + echo "[init] automatic startup cancelled by user script" +else + echo "[init] detecting attached hardware..." + + # + # Assume that we are PX4FMU in standalone mode + # + set BOARD PX4FMU + + # + # Are we attached to a PX4IOAR (AR.Drone carrier board)? + # + if boardinfo test name PX4IOAR + then + set BOARD PX4IOAR + if [ -f /etc/init.d/rc.PX4IOAR ] + then + echo "[init] reading /etc/init.d/rc.PX4IOAR" + usleep 500 + sh /etc/init.d/rc.PX4IOAR + fi + else + echo "[init] PX4IOAR not detected" + fi + + # + # Are we attached to a PX4IO? + # + if boardinfo test name PX4IO + then + set BOARD PX4IO + if [ -f /etc/init.d/rc.PX4IO ] + then + echo "[init] reading /etc/init.d/rc.PX4IO" + usleep 500 + sh /etc/init.d/rc.PX4IO + fi + else + echo "[init] PX4IO not detected" + fi + + # + # Looks like we are stand-alone + # + if [ $BOARD == PX4FMU ] + then + echo "[init] no expansion board detected" + if [ -f /etc/init.d/rc.standalone ] + then + echo "[init] reading /etc/init.d/rc.standalone" + sh /etc/init.d/rc.standalone + fi + fi + + # + # We may not reach here if the airframe-specific script exits the shell. + # + echo "[init] startup done." +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.jig b/ROMFS/px4fmu_common/init.d/rc.jig new file mode 100644 index 000000000..e2b5d8f30 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.jig @@ -0,0 +1,10 @@ +#!nsh +# +# Test jig startup script +# + +echo "[testing] doing production test.." + +tests jig + +echo "[testing] testing done" diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging new file mode 100644 index 000000000..09c2d00d1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -0,0 +1,9 @@ +#!nsh +# +# Initialise logging services. +# + +if [ -d /fs/microsd ] +then + sdlog start +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors new file mode 100644 index 000000000..42c2f52e9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -0,0 +1,34 @@ +#!nsh +# +# Standard startup script for PX4FMU onboard sensor drivers. +# + +# +# Start sensor drivers here. +# + +ms5611 start +adc start + +if mpu6000 start +then + echo "using MPU6000 and HMC5883L" + hmc5883 start +else + echo "using L3GD20 and LSM303D" + l3gd20 start + lsm303 start +fi + +# +# Start the sensor collection task. +# IMPORTANT: this also loads param offsets +# ALWAYS start this task before the +# preflight_check. +# +sensors start + +# +# Check sensors - run AFTER 'sensors start' +# +preflight_check \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone new file mode 100644 index 000000000..67e95215b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.standalone @@ -0,0 +1,13 @@ +#!nsh +# +# Flight startup script for PX4FMU standalone configuration. +# + +echo "[init] doing standalone PX4FMU startup..." + +# +# Start the ORB +# +uorb start + +echo "[init] startup done" diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS new file mode 100755 index 000000000..89a767879 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -0,0 +1,79 @@ +#!nsh +# +# PX4FMU startup script. +# +# This script is responsible for: +# +# - mounting the microSD card (if present) +# - running the user startup script from the microSD card (if present) +# - detecting the configuration of the system and picking a suitable +# startup script to continue with +# +# Note: DO NOT add configuration-specific commands to this script; +# add them to the per-configuration scripts instead. +# + +# +# Default to auto-start mode. An init script on the microSD card +# can change this to prevent automatic startup of the flight script. +# +set MODE autostart +set USB autoconnect + +# +# Start playing the startup tune +# +tone_alarm start + +# +# 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" +else + echo "[init] no microSD card found" +fi + +# +# Look for an init script on the microSD card. +# +# To prevent automatic startup in the current flight mode, +# the script should set MODE to some other value. +# +if [ -f /fs/microsd/etc/rc ] +then + echo "[init] reading /fs/microsd/etc/rc" + sh /fs/microsd/etc/rc +fi +# Also consider rc.txt files +if [ -f /fs/microsd/etc/rc.txt ] +then + echo "[init] reading /fs/microsd/etc/rc.txt" + sh /fs/microsd/etc/rc.txt +fi + +# +# Check for USB host +# +if [ $USB != autoconnect ] +then + echo "[init] not connecting USB" +else + if sercon + then + echo "[init] USB interface connected" + else + echo "[init] No USB connected" + fi +fi + +# if this is an APM build then there will be a rc.APM script +# from an EXTERNAL_SCRIPTS build option +if [ -f /etc/init.d/rc.APM ] +then + echo Running rc.APM + # if APM startup is successful then nsh will exit + sh /etc/init.d/rc.APM +fi diff --git a/ROMFS/px4fmu_common/logging/logconv.m b/ROMFS/px4fmu_common/logging/logconv.m new file mode 100644 index 000000000..3750ddae2 --- /dev/null +++ b/ROMFS/px4fmu_common/logging/logconv.m @@ -0,0 +1,586 @@ +% This Matlab Script can be used to import the binary logged values of the +% PX4FMU into data that can be plotted and analyzed. + +%% ************************************************************************ +% PX4LOG_PLOTSCRIPT: Main function +% ************************************************************************ +function PX4Log_Plotscript + +% Clear everything +clc +clear all +close all + +% ************************************************************************ +% SETTINGS +% ************************************************************************ + +% Set the path to your sysvector.bin file here +filePath = 'sysvector.bin'; + +% Set the minimum and maximum times to plot here [in seconds] +mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start] +maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end] + +%Determine which data to plot. Not completely implemented yet. +bDisplayGPS=true; + +%conversion factors +fconv_gpsalt=1E-3; %[mm] to [m] +fconv_gpslatlong=1E-7; %[gps_raw_position_unit] to [deg] +fconv_timestamp=1E-6; % [microseconds] to [seconds] + +% ************************************************************************ +% Import the PX4 logs +% ************************************************************************ +ImportPX4LogData(); + +%Translate min and max plot times to indices +time=double(sysvector.timestamp) .*fconv_timestamp; +mintime_log=time(1); %The minimum time/timestamp found in the log +maxtime_log=time(end); %The maximum time/timestamp found in the log +CurTime=mintime_log; %The current time at which to draw the aircraft position + +[imintime,imaxtime]=FindMinMaxTimeIndices(); + +% ************************************************************************ +% PLOT & GUI SETUP +% ************************************************************************ +NrFigures=5; +NrAxes=10; +h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively +h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively +h.pathpoints=[]; % Temporary initiliazation of path points + +% Setup the GUI to control the plots +InitControlGUI(); +% Setup the plotting-GUI (figures, axes) itself. +InitPlotGUI(); + +% ************************************************************************ +% DRAW EVERYTHING +% ************************************************************************ +DrawRawData(); +DrawCurrentAircraftState(); + +%% ************************************************************************ +% *** END OF MAIN SCRIPT *** +% NESTED FUNCTION DEFINTIONS FROM HERE ON +% ************************************************************************ + + +%% ************************************************************************ +% IMPORTPX4LOGDATA (nested function) +% ************************************************************************ +% Attention: This is the import routine for firmware from ca. 03/2013. +% Other firmware versions might require different import +% routines. + +function ImportPX4LogData() + % Work around a Matlab bug (not related to PX4) + % where timestamps from 1.1.1970 do not allow to + % read the file's size + if ismac + system('touch -t 201212121212.12 sysvector.bin'); + end + + % ************************************************************************ + % RETRIEVE SYSTEM VECTOR + % ************************************************************************* + % //All measurements in NED frame + % + % uint64_t timestamp; //[us] + % float gyro[3]; //[rad/s] + % float accel[3]; //[m/s^2] + % float mag[3]; //[gauss] + % float baro; //pressure [millibar] + % float baro_alt; //altitude above MSL [meter] + % float baro_temp; //[degree celcius] + % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] + % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) + % float vbat; //battery voltage in [volt] + % float bat_current - current drawn from battery at this time instant + % float bat_discharged - discharged energy in mAh + % float adc[4]; //remaining auxiliary ADC ports [volt] + % float local_position[3]; //tangent plane mapping into x,y,z [m] + % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] + % float attitude[3]; //pitch, roll, yaw [rad] + % float rotMatrix[9]; //unitvectors + % float actuator_control[4]; //unitvector + % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1] + % float diff_pressure; - pressure difference in millibar + % float ind_airspeed; + % float true_airspeed; + + % Definition of the logged values + logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); + logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); + logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + + % First get length of one line + columns = length(logFormat); + lineLength = 0; + + for i=1:columns + lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array; + end + + + if exist(filePath, 'file') + + fileInfo = dir(filePath); + fileSize = fileInfo.bytes; + + elements = int64(fileSize./(lineLength)); + + fid = fopen(filePath, 'r'); + offset = 0; + for i=1:columns + % using fread with a skip speeds up the import drastically, do not + % import the values one after the other + sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(... + fid, ... + [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ... + lineLength - logFormat{i}.bytes*logFormat{i}.array, ... + logFormat{i}.machineformat) ... + ); + offset = offset + logFormat{i}.bytes*logFormat{i}.array; + fseek(fid, offset,'bof'); + end + + % shot the flight time + time_us = sysvector.timestamp(end) - sysvector.timestamp(1); + time_s = time_us*1e-6; + time_m = time_s/60; + + % close the logfile + fclose(fid); + + disp(['end log2matlab conversion' char(10)]); + else + disp(['file: ' filePath ' does not exist' char(10)]); + end +end + +%% ************************************************************************ +% INITCONTROLGUI (nested function) +% ************************************************************************ +%Setup central control GUI components to control current time where data is shown +function InitControlGUI() + %********************************************************************** + % GUI size definitions + %********************************************************************** + dxy=5; %margins + %Panel: Plotctrl + dlabels=120; + dsliders=200; + dedits=80; + hslider=20; + + hpanel1=40; %panel1 + hpanel2=220;%panel2 + hpanel3=3*hslider+4*dxy+3*dxy;%panel3. + + width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width + height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height + + %********************************************************************** + % Create GUI + %********************************************************************** + h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI'); + h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1)); + h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1)); + h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1)); + + %%Control GUI-elements + %Slider: Current time + h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],... + 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel); + temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); + set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); + h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),... + 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel); + + %Slider: MaxTime + h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],... + 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),... + 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + + %Slider: MinTime + h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],... + 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),... + 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + + %%Current data/state GUI-elements (Multiline-edit-box) + h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',... + 'HorizontalAlignment','left','parent',h.aircraftstatepanel); + + h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',... + 'HorizontalAlignment','left','parent',h.guistatepanel); + +end + +%% ************************************************************************ +% INITPLOTGUI (nested function) +% ************************************************************************ +function InitPlotGUI() + + % Setup handles to lines and text + h.markertext=[]; + templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array + h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively + h.markerline(1:NrAxes)=0.0; + + % Setup all other figures and axes for plotting + % PLOT WINDOW 1: GPS POSITION + h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position'); + h.axes(1)=axes(); + set(h.axes(1),'Parent',h.figures(2)); + + % PLOT WINDOW 2: IMU, baro altitude + h.figures(3)=figure('Name', 'IMU / Baro Altitude'); + h.axes(2)=subplot(4,1,1); + h.axes(3)=subplot(4,1,2); + h.axes(4)=subplot(4,1,3); + h.axes(5)=subplot(4,1,4); + set(h.axes(2:5),'Parent',h.figures(3)); + + % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... + h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds'); + h.axes(6)=subplot(4,1,1); + h.axes(7)=subplot(4,1,2); + h.axes(8)=subplot(4,1,3); + h.axes(9)=subplot(4,1,4); + set(h.axes(6:9),'Parent',h.figures(4)); + + % PLOT WINDOW 4: LOG STATS + h.figures(5) = figure('Name', 'Log Statistics'); + h.axes(10)=subplot(1,1,1); + set(h.axes(10:10),'Parent',h.figures(5)); + +end + +%% ************************************************************************ +% DRAWRAWDATA (nested function) +% ************************************************************************ +%Draws the raw data from the sysvector, but does not add any +%marker-lines or so +function DrawRawData() + % ************************************************************************ + % PLOT WINDOW 1: GPS POSITION & GUI + % ************************************************************************ + figure(h.figures(2)); + % Only plot GPS data if available + if (sum(double(sysvector.gps_raw_position(imintime:imaxtime,1)))>0) && (bDisplayGPS) + %Draw data + plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:imaxtime,1))*fconv_gpslatlong, ... + double(sysvector.gps_raw_position(imintime:imaxtime,2))*fconv_gpslatlong, ... + double(sysvector.gps_raw_position(imintime:imaxtime,3))*fconv_gpsalt,'r.'); + title(h.axes(1),'GPS Position Data(if available)'); + xlabel(h.axes(1),'Latitude [deg]'); + ylabel(h.axes(1),'Longitude [deg]'); + zlabel(h.axes(1),'Altitude above MSL [m]'); + grid on + + %Reset path + h.pathpoints=0; + end + + % ************************************************************************ + % PLOT WINDOW 2: IMU, baro altitude + % ************************************************************************ + figure(h.figures(3)); + plot(h.axes(2),time(imintime:imaxtime),sysvector.mag(imintime:imaxtime,:)); + title(h.axes(2),'Magnetometers [Gauss]'); + legend(h.axes(2),'x','y','z'); + plot(h.axes(3),time(imintime:imaxtime),sysvector.accel(imintime:imaxtime,:)); + title(h.axes(3),'Accelerometers [m/s²]'); + legend(h.axes(3),'x','y','z'); + plot(h.axes(4),time(imintime:imaxtime),sysvector.gyro(imintime:imaxtime,:)); + title(h.axes(4),'Gyroscopes [rad/s]'); + legend(h.axes(4),'x','y','z'); + plot(h.axes(5),time(imintime:imaxtime),sysvector.baro_alt(imintime:imaxtime),'color','blue'); + if(bDisplayGPS) + hold on; + plot(h.axes(5),time(imintime:imaxtime),double(sysvector.gps_raw_position(imintime:imaxtime,3)).*fconv_gpsalt,'color','red'); + hold off + legend('Barometric Altitude [m]','GPS Altitude [m]'); + else + legend('Barometric Altitude [m]'); + end + title(h.axes(5),'Altitude above MSL [m]'); + + % ************************************************************************ + % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... + % ************************************************************************ + figure(h.figures(4)); + %Attitude Estimate + plot(h.axes(6),time(imintime:imaxtime), sysvector.attitude(imintime:imaxtime,:).*180./3.14159); + title(h.axes(6),'Estimated attitude [deg]'); + legend(h.axes(6),'roll','pitch','yaw'); + %Actuator Controls + plot(h.axes(7),time(imintime:imaxtime), sysvector.actuator_control(imintime:imaxtime,:)); + title(h.axes(7),'Actuator control [-]'); + legend(h.axes(7),'0','1','2','3'); + %Actuator Controls + plot(h.axes(8),time(imintime:imaxtime), sysvector.actuators(imintime:imaxtime,1:8)); + title(h.axes(8),'Actuator PWM (raw-)outputs [µs]'); + legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8'); + set(h.axes(8), 'YLim',[800 2200]); + %Airspeeds + plot(h.axes(9),time(imintime:imaxtime), sysvector.ind_airspeed(imintime:imaxtime)); + hold on + plot(h.axes(9),time(imintime:imaxtime), sysvector.true_airspeed(imintime:imaxtime)); + hold off + %add GPS total airspeed here + title(h.axes(9),'Airspeed [m/s]'); + legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed'); + %calculate time differences and plot them + intervals = zeros(0,imaxtime - imintime); + for k = imintime+1:imaxtime + intervals(k) = time(k) - time(k-1); + end + plot(h.axes(10), time(imintime:imaxtime), intervals); + + %Set same timescale for all plots + for i=2:NrAxes + set(h.axes(i),'XLim',[mintime maxtime]); + end + + set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); +end + +%% ************************************************************************ +% DRAWCURRENTAIRCRAFTSTATE(nested function) +% ************************************************************************ +function DrawCurrentAircraftState() + %find current data index + i=find(time>=CurTime,1,'first'); + + %********************************************************************** + % Current aircraft state label update + %********************************************************************** + acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong),'°, ',... + 'lon=',num2str(double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong),'°, ',... + 'alt=',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]']; + acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.mag(i,1)),... + ', y=',num2str(sysvector.mag(i,2)),... + ', z=',num2str(sysvector.mag(i,3)),']']; + acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.accel(i,1)),... + ', y=',num2str(sysvector.accel(i,2)),... + ', z=',num2str(sysvector.accel(i,3)),']']; + acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.gyro(i,1)),... + ', y=',num2str(sysvector.gyro(i,2)),... + ', z=',num2str(sysvector.gyro(i,3)),']']; + acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.baro_alt(i)),'m, GPS: ',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]']; + acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.attitude(i,1).*180./3.14159),... + ', Pitch=',num2str(sysvector.attitude(i,2).*180./3.14159),... + ', Yaw=',num2str(sysvector.attitude(i,3).*180./3.14159),']']; + acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:'); + for j=1:4 + acstate{7,:}=[acstate{7,:},num2str(sysvector.actuator_control(i,j)),',']; + end + acstate{7,:}=[acstate{7,:},']']; + acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:'); + for j=1:8 + acstate{8,:}=[acstate{8,:},num2str(sysvector.actuators(i,j)),',']; + end + acstate{8,:}=[acstate{8,:},']']; + acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.ind_airspeed(i)),', TAS: ',num2str(sysvector.true_airspeed(i)),']']; + + set(h.edits.AircraftState,'String',acstate); + + %********************************************************************** + % GPS Plot Update + %********************************************************************** + %Plot traveled path, and and time. + figure(h.figures(2)); + hold on; + if(CurTime>mintime+1) %the +1 is only a small bugfix + h.pathline=plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:i,1))*fconv_gpslatlong, ... + double(sysvector.gps_raw_position(imintime:i,2))*fconv_gpslatlong, ... + double(sysvector.gps_raw_position(imintime:i,3))*fconv_gpsalt,'b','LineWidth',2); + end; + hold off + %Plot current position + newpoint=[double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong double(sysvector.gps_raw_position(i,3))*fconv_gpsalt]; + if(numel(h.pathpoints)<=3) %empty path + h.pathpoints(1,1:3)=newpoint; + else %Not empty, append new point + h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint; + end + axes(h.axes(1)); + line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20); + + + % Plot current time (small label next to current gps position) + textdesc=strcat(' t=',num2str(time(i)),'s'); + if(isvalidhandle(h.markertext)) + delete(h.markertext); %delete old text + end + h.markertext=text(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong,double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong,... + double(sysvector.gps_raw_position(i,3))*fconv_gpsalt,textdesc); + set(h.edits.CurTime,'String',CurTime); + + %********************************************************************** + % Plot the lines showing the current time in the 2-d plots + %********************************************************************** + for i=2:NrAxes + if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end + ylims=get(h.axes(i),'YLim'); + h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black'); + set(h.markerline(i),'parent',h.axes(i)); + end + + set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); +end + +%% ************************************************************************ +% MINMAXTIME CALLBACK (nested function) +% ************************************************************************ +function minmaxtime_callback(hObj,event) %#ok + 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_maxtimeCurTime) + 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 + %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(tempmintime) + CurTime=temp; + else + %Error + set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red'); + end + else + %Error + set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red'); + end + + set(h.sliders.CurTime,'Value',CurTime); + set(h.edits.CurTime,'String',num2str(CurTime)); + + %Redraw time markers, but don't have to redraw the whole raw data + DrawCurrentAircraftState(); +end + +%% ************************************************************************ +% FINDMINMAXINDICES (nested function) +% ************************************************************************ +function [idxmin,idxmax] = FindMinMaxTimeIndices() + for i=1:size(sysvector.timestamp,1) + if time(i)>=mintime; idxmin=i; break; end + end + for i=1:size(sysvector.timestamp,1) + if maxtime==0; idxmax=size(sysvector.timestamp,1); break; end + if time(i)>=maxtime; idxmax=i; break; end + end + mintime=time(idxmin); + maxtime=time(idxmax); +end + +%% ************************************************************************ +% ISVALIDHANDLE (nested function) +% ************************************************************************ +function isvalid = isvalidhandle(handle) + if(exist(varname(handle))>0 && length(ishandle(handle))>0) + if(ishandle(handle)>0) + if(handle>0.0) + isvalid=true; + return; + end + end + end + isvalid=false; +end + +%% ************************************************************************ +% JUST SOME SMALL HELPER FUNCTIONS (nested function) +% ************************************************************************ +function out = varname(var) + out = inputname(1); +end + +%This is the end of the matlab file / the main function +end diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix new file mode 100644 index 000000000..75e82bb00 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -0,0 +1,64 @@ +Aileron/rudder/elevator/throttle mixer for PX4FMU +================================================== + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator and throttle controls using PX4FMU. The configuration +assumes the aileron servo(s) are connected to PX4FMU servo output 0, the +elevator to output 1, the rudder to output 2 and the throttle to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix new file mode 100644 index 000000000..20cb88b91 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_AET.mix @@ -0,0 +1,60 @@ +Aileron/elevator/throttle mixer for PX4FMU +================================================== + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, elevator and throttle controls using PX4FMU. The configuration assumes +the aileron servo(s) are connected to PX4FMU servo output 0, the elevator to +output 1 and the throttle to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +Alternatively, output 2 could be used as a second aileron servo output with +separate mixing. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix new file mode 100644 index 000000000..ebcb66b24 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix @@ -0,0 +1,52 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for Bormatec Camflyer Q + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -5000 -8000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -8000 -5000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix new file mode 100644 index 000000000..95beb8927 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_RET.mix @@ -0,0 +1,53 @@ +Rudder/elevator/throttle mixer for PX4FMU +========================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +rudder, elevator and throttle controls using PX4FMU. The configuration assumes +the rudder servo is connected to PX4FMU servo output 0, the elevator to output 1 +and the throttle to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Rudder mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix new file mode 100644 index 000000000..9f81e1dc3 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix @@ -0,0 +1,50 @@ +Delta-wing mixer for PX4FMU +=========================== + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +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 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -5000 -3000 0 -10000 10000 +S: 0 1 5000 5000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix new file mode 100644 index 000000000..981466704 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_delta.mix @@ -0,0 +1,50 @@ +Delta-wing mixer for PX4FMU +=========================== + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +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 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 5000 3000 0 -10000 10000 +S: 0 1 -5000 -5000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix new file mode 100644 index 000000000..b5e38ce9e --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix @@ -0,0 +1,7 @@ +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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix new file mode 100644 index 000000000..8e8d122ad --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix @@ -0,0 +1,7 @@ +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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix new file mode 100644 index 000000000..2cb70e814 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a octocopter in the + configuration. All controls +are mixed 100%. + +R: 8+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix new file mode 100644 index 000000000..edc71f013 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a octocopter in the X configuration. All controls +are mixed 100%. + +R: 8x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_pass.mix b/ROMFS/px4fmu_common/mixers/FMU_pass.mix new file mode 100644 index 000000000..e9a81f2bb --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_pass.mix @@ -0,0 +1,23 @@ +Passthrough mixer for PX4FMU +============================ + +This file defines passthrough mixers suitable for testing. + +Channel group 0, channels 0-3 are passed directly through to the outputs. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 10000 10000 0 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix new file mode 100644 index 000000000..dfdf1d58e --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the + configuration. All controls +are mixed 100%. + +R: 4+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix new file mode 100644 index 000000000..12a3bee20 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the X configuration. All controls +are mixed 100%. + +R: 4x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README new file mode 100644 index 000000000..6649c53c2 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/README @@ -0,0 +1,154 @@ +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 + + : + +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: + O: <-ve scale> <+ve scale> + +If is zero, the sum is effectively zero and the mixer will +output a fixed value that is constrained by and . + +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 entries describing the control +inputs and their scaling, in the form: + + S: <-ve scale> <+ve scale> + +The value identifies the control group from which the scaler will read, +and the 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: + +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_default/logging/logconv.m b/ROMFS/px4fmu_default/logging/logconv.m deleted file mode 100644 index 3750ddae2..000000000 --- a/ROMFS/px4fmu_default/logging/logconv.m +++ /dev/null @@ -1,586 +0,0 @@ -% This Matlab Script can be used to import the binary logged values of the -% PX4FMU into data that can be plotted and analyzed. - -%% ************************************************************************ -% PX4LOG_PLOTSCRIPT: Main function -% ************************************************************************ -function PX4Log_Plotscript - -% Clear everything -clc -clear all -close all - -% ************************************************************************ -% SETTINGS -% ************************************************************************ - -% Set the path to your sysvector.bin file here -filePath = 'sysvector.bin'; - -% Set the minimum and maximum times to plot here [in seconds] -mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start] -maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end] - -%Determine which data to plot. Not completely implemented yet. -bDisplayGPS=true; - -%conversion factors -fconv_gpsalt=1E-3; %[mm] to [m] -fconv_gpslatlong=1E-7; %[gps_raw_position_unit] to [deg] -fconv_timestamp=1E-6; % [microseconds] to [seconds] - -% ************************************************************************ -% Import the PX4 logs -% ************************************************************************ -ImportPX4LogData(); - -%Translate min and max plot times to indices -time=double(sysvector.timestamp) .*fconv_timestamp; -mintime_log=time(1); %The minimum time/timestamp found in the log -maxtime_log=time(end); %The maximum time/timestamp found in the log -CurTime=mintime_log; %The current time at which to draw the aircraft position - -[imintime,imaxtime]=FindMinMaxTimeIndices(); - -% ************************************************************************ -% PLOT & GUI SETUP -% ************************************************************************ -NrFigures=5; -NrAxes=10; -h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively -h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively -h.pathpoints=[]; % Temporary initiliazation of path points - -% Setup the GUI to control the plots -InitControlGUI(); -% Setup the plotting-GUI (figures, axes) itself. -InitPlotGUI(); - -% ************************************************************************ -% DRAW EVERYTHING -% ************************************************************************ -DrawRawData(); -DrawCurrentAircraftState(); - -%% ************************************************************************ -% *** END OF MAIN SCRIPT *** -% NESTED FUNCTION DEFINTIONS FROM HERE ON -% ************************************************************************ - - -%% ************************************************************************ -% IMPORTPX4LOGDATA (nested function) -% ************************************************************************ -% Attention: This is the import routine for firmware from ca. 03/2013. -% Other firmware versions might require different import -% routines. - -function ImportPX4LogData() - % Work around a Matlab bug (not related to PX4) - % where timestamps from 1.1.1970 do not allow to - % read the file's size - if ismac - system('touch -t 201212121212.12 sysvector.bin'); - end - - % ************************************************************************ - % RETRIEVE SYSTEM VECTOR - % ************************************************************************* - % //All measurements in NED frame - % - % uint64_t timestamp; //[us] - % float gyro[3]; //[rad/s] - % float accel[3]; //[m/s^2] - % float mag[3]; //[gauss] - % float baro; //pressure [millibar] - % float baro_alt; //altitude above MSL [meter] - % float baro_temp; //[degree celcius] - % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] - % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) - % float vbat; //battery voltage in [volt] - % float bat_current - current drawn from battery at this time instant - % float bat_discharged - discharged energy in mAh - % float adc[4]; //remaining auxiliary ADC ports [volt] - % float local_position[3]; //tangent plane mapping into x,y,z [m] - % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] - % float attitude[3]; //pitch, roll, yaw [rad] - % float rotMatrix[9]; //unitvectors - % float actuator_control[4]; //unitvector - % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1] - % float diff_pressure; - pressure difference in millibar - % float ind_airspeed; - % float true_airspeed; - - % Definition of the logged values - logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); - logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); - logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - - % First get length of one line - columns = length(logFormat); - lineLength = 0; - - for i=1:columns - lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array; - end - - - if exist(filePath, 'file') - - fileInfo = dir(filePath); - fileSize = fileInfo.bytes; - - elements = int64(fileSize./(lineLength)); - - fid = fopen(filePath, 'r'); - offset = 0; - for i=1:columns - % using fread with a skip speeds up the import drastically, do not - % import the values one after the other - sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(... - fid, ... - [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ... - lineLength - logFormat{i}.bytes*logFormat{i}.array, ... - logFormat{i}.machineformat) ... - ); - offset = offset + logFormat{i}.bytes*logFormat{i}.array; - fseek(fid, offset,'bof'); - end - - % shot the flight time - time_us = sysvector.timestamp(end) - sysvector.timestamp(1); - time_s = time_us*1e-6; - time_m = time_s/60; - - % close the logfile - fclose(fid); - - disp(['end log2matlab conversion' char(10)]); - else - disp(['file: ' filePath ' does not exist' char(10)]); - end -end - -%% ************************************************************************ -% INITCONTROLGUI (nested function) -% ************************************************************************ -%Setup central control GUI components to control current time where data is shown -function InitControlGUI() - %********************************************************************** - % GUI size definitions - %********************************************************************** - dxy=5; %margins - %Panel: Plotctrl - dlabels=120; - dsliders=200; - dedits=80; - hslider=20; - - hpanel1=40; %panel1 - hpanel2=220;%panel2 - hpanel3=3*hslider+4*dxy+3*dxy;%panel3. - - width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width - height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height - - %********************************************************************** - % Create GUI - %********************************************************************** - h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI'); - h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1)); - h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1)); - h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1)); - - %%Control GUI-elements - %Slider: Current time - h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],... - 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel); - temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); - set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); - h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),... - 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel); - - %Slider: MaxTime - h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],... - 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),... - 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - - %Slider: MinTime - h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],... - 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),... - 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - - %%Current data/state GUI-elements (Multiline-edit-box) - h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',... - 'HorizontalAlignment','left','parent',h.aircraftstatepanel); - - h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',... - 'HorizontalAlignment','left','parent',h.guistatepanel); - -end - -%% ************************************************************************ -% INITPLOTGUI (nested function) -% ************************************************************************ -function InitPlotGUI() - - % Setup handles to lines and text - h.markertext=[]; - templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array - h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively - h.markerline(1:NrAxes)=0.0; - - % Setup all other figures and axes for plotting - % PLOT WINDOW 1: GPS POSITION - h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position'); - h.axes(1)=axes(); - set(h.axes(1),'Parent',h.figures(2)); - - % PLOT WINDOW 2: IMU, baro altitude - h.figures(3)=figure('Name', 'IMU / Baro Altitude'); - h.axes(2)=subplot(4,1,1); - h.axes(3)=subplot(4,1,2); - h.axes(4)=subplot(4,1,3); - h.axes(5)=subplot(4,1,4); - set(h.axes(2:5),'Parent',h.figures(3)); - - % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... - h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds'); - h.axes(6)=subplot(4,1,1); - h.axes(7)=subplot(4,1,2); - h.axes(8)=subplot(4,1,3); - h.axes(9)=subplot(4,1,4); - set(h.axes(6:9),'Parent',h.figures(4)); - - % PLOT WINDOW 4: LOG STATS - h.figures(5) = figure('Name', 'Log Statistics'); - h.axes(10)=subplot(1,1,1); - set(h.axes(10:10),'Parent',h.figures(5)); - -end - -%% ************************************************************************ -% DRAWRAWDATA (nested function) -% ************************************************************************ -%Draws the raw data from the sysvector, but does not add any -%marker-lines or so -function DrawRawData() - % ************************************************************************ - % PLOT WINDOW 1: GPS POSITION & GUI - % ************************************************************************ - figure(h.figures(2)); - % Only plot GPS data if available - if (sum(double(sysvector.gps_raw_position(imintime:imaxtime,1)))>0) && (bDisplayGPS) - %Draw data - plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:imaxtime,1))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:imaxtime,2))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:imaxtime,3))*fconv_gpsalt,'r.'); - title(h.axes(1),'GPS Position Data(if available)'); - xlabel(h.axes(1),'Latitude [deg]'); - ylabel(h.axes(1),'Longitude [deg]'); - zlabel(h.axes(1),'Altitude above MSL [m]'); - grid on - - %Reset path - h.pathpoints=0; - end - - % ************************************************************************ - % PLOT WINDOW 2: IMU, baro altitude - % ************************************************************************ - figure(h.figures(3)); - plot(h.axes(2),time(imintime:imaxtime),sysvector.mag(imintime:imaxtime,:)); - title(h.axes(2),'Magnetometers [Gauss]'); - legend(h.axes(2),'x','y','z'); - plot(h.axes(3),time(imintime:imaxtime),sysvector.accel(imintime:imaxtime,:)); - title(h.axes(3),'Accelerometers [m/s²]'); - legend(h.axes(3),'x','y','z'); - plot(h.axes(4),time(imintime:imaxtime),sysvector.gyro(imintime:imaxtime,:)); - title(h.axes(4),'Gyroscopes [rad/s]'); - legend(h.axes(4),'x','y','z'); - plot(h.axes(5),time(imintime:imaxtime),sysvector.baro_alt(imintime:imaxtime),'color','blue'); - if(bDisplayGPS) - hold on; - plot(h.axes(5),time(imintime:imaxtime),double(sysvector.gps_raw_position(imintime:imaxtime,3)).*fconv_gpsalt,'color','red'); - hold off - legend('Barometric Altitude [m]','GPS Altitude [m]'); - else - legend('Barometric Altitude [m]'); - end - title(h.axes(5),'Altitude above MSL [m]'); - - % ************************************************************************ - % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... - % ************************************************************************ - figure(h.figures(4)); - %Attitude Estimate - plot(h.axes(6),time(imintime:imaxtime), sysvector.attitude(imintime:imaxtime,:).*180./3.14159); - title(h.axes(6),'Estimated attitude [deg]'); - legend(h.axes(6),'roll','pitch','yaw'); - %Actuator Controls - plot(h.axes(7),time(imintime:imaxtime), sysvector.actuator_control(imintime:imaxtime,:)); - title(h.axes(7),'Actuator control [-]'); - legend(h.axes(7),'0','1','2','3'); - %Actuator Controls - plot(h.axes(8),time(imintime:imaxtime), sysvector.actuators(imintime:imaxtime,1:8)); - title(h.axes(8),'Actuator PWM (raw-)outputs [µs]'); - legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8'); - set(h.axes(8), 'YLim',[800 2200]); - %Airspeeds - plot(h.axes(9),time(imintime:imaxtime), sysvector.ind_airspeed(imintime:imaxtime)); - hold on - plot(h.axes(9),time(imintime:imaxtime), sysvector.true_airspeed(imintime:imaxtime)); - hold off - %add GPS total airspeed here - title(h.axes(9),'Airspeed [m/s]'); - legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed'); - %calculate time differences and plot them - intervals = zeros(0,imaxtime - imintime); - for k = imintime+1:imaxtime - intervals(k) = time(k) - time(k-1); - end - plot(h.axes(10), time(imintime:imaxtime), intervals); - - %Set same timescale for all plots - for i=2:NrAxes - set(h.axes(i),'XLim',[mintime maxtime]); - end - - set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); -end - -%% ************************************************************************ -% DRAWCURRENTAIRCRAFTSTATE(nested function) -% ************************************************************************ -function DrawCurrentAircraftState() - %find current data index - i=find(time>=CurTime,1,'first'); - - %********************************************************************** - % Current aircraft state label update - %********************************************************************** - acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong),'°, ',... - 'lon=',num2str(double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong),'°, ',... - 'alt=',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]']; - acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.mag(i,1)),... - ', y=',num2str(sysvector.mag(i,2)),... - ', z=',num2str(sysvector.mag(i,3)),']']; - acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.accel(i,1)),... - ', y=',num2str(sysvector.accel(i,2)),... - ', z=',num2str(sysvector.accel(i,3)),']']; - acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.gyro(i,1)),... - ', y=',num2str(sysvector.gyro(i,2)),... - ', z=',num2str(sysvector.gyro(i,3)),']']; - acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.baro_alt(i)),'m, GPS: ',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]']; - acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.attitude(i,1).*180./3.14159),... - ', Pitch=',num2str(sysvector.attitude(i,2).*180./3.14159),... - ', Yaw=',num2str(sysvector.attitude(i,3).*180./3.14159),']']; - acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:'); - for j=1:4 - acstate{7,:}=[acstate{7,:},num2str(sysvector.actuator_control(i,j)),',']; - end - acstate{7,:}=[acstate{7,:},']']; - acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:'); - for j=1:8 - acstate{8,:}=[acstate{8,:},num2str(sysvector.actuators(i,j)),',']; - end - acstate{8,:}=[acstate{8,:},']']; - acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.ind_airspeed(i)),', TAS: ',num2str(sysvector.true_airspeed(i)),']']; - - set(h.edits.AircraftState,'String',acstate); - - %********************************************************************** - % GPS Plot Update - %********************************************************************** - %Plot traveled path, and and time. - figure(h.figures(2)); - hold on; - if(CurTime>mintime+1) %the +1 is only a small bugfix - h.pathline=plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:i,1))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:i,2))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:i,3))*fconv_gpsalt,'b','LineWidth',2); - end; - hold off - %Plot current position - newpoint=[double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong double(sysvector.gps_raw_position(i,3))*fconv_gpsalt]; - if(numel(h.pathpoints)<=3) %empty path - h.pathpoints(1,1:3)=newpoint; - else %Not empty, append new point - h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint; - end - axes(h.axes(1)); - line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20); - - - % Plot current time (small label next to current gps position) - textdesc=strcat(' t=',num2str(time(i)),'s'); - if(isvalidhandle(h.markertext)) - delete(h.markertext); %delete old text - end - h.markertext=text(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong,double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong,... - double(sysvector.gps_raw_position(i,3))*fconv_gpsalt,textdesc); - set(h.edits.CurTime,'String',CurTime); - - %********************************************************************** - % Plot the lines showing the current time in the 2-d plots - %********************************************************************** - for i=2:NrAxes - if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end - ylims=get(h.axes(i),'YLim'); - h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black'); - set(h.markerline(i),'parent',h.axes(i)); - end - - set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); -end - -%% ************************************************************************ -% MINMAXTIME CALLBACK (nested function) -% ************************************************************************ -function minmaxtime_callback(hObj,event) %#ok - 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_maxtimeCurTime) - 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 - %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(tempmintime) - CurTime=temp; - else - %Error - set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red'); - end - else - %Error - set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red'); - end - - set(h.sliders.CurTime,'Value',CurTime); - set(h.edits.CurTime,'String',num2str(CurTime)); - - %Redraw time markers, but don't have to redraw the whole raw data - DrawCurrentAircraftState(); -end - -%% ************************************************************************ -% FINDMINMAXINDICES (nested function) -% ************************************************************************ -function [idxmin,idxmax] = FindMinMaxTimeIndices() - for i=1:size(sysvector.timestamp,1) - if time(i)>=mintime; idxmin=i; break; end - end - for i=1:size(sysvector.timestamp,1) - if maxtime==0; idxmax=size(sysvector.timestamp,1); break; end - if time(i)>=maxtime; idxmax=i; break; end - end - mintime=time(idxmin); - maxtime=time(idxmax); -end - -%% ************************************************************************ -% ISVALIDHANDLE (nested function) -% ************************************************************************ -function isvalid = isvalidhandle(handle) - if(exist(varname(handle))>0 && length(ishandle(handle))>0) - if(ishandle(handle)>0) - if(handle>0.0) - isvalid=true; - return; - end - end - end - isvalid=false; -end - -%% ************************************************************************ -% JUST SOME SMALL HELPER FUNCTIONS (nested function) -% ************************************************************************ -function out = varname(var) - out = inputname(1); -end - -%This is the end of the matlab file / the main function -end diff --git a/ROMFS/px4fmu_default/mixers/FMU_AERT.mix b/ROMFS/px4fmu_default/mixers/FMU_AERT.mix deleted file mode 100644 index 75e82bb00..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_AERT.mix +++ /dev/null @@ -1,64 +0,0 @@ -Aileron/rudder/elevator/throttle mixer for PX4FMU -================================================== - -This file defines mixers suitable for controlling a fixed wing aircraft with -aileron, rudder, elevator and throttle controls using PX4FMU. The configuration -assumes the aileron servo(s) are connected to PX4FMU servo output 0, the -elevator to output 1, the rudder to output 2 and the throttle to output 3. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -Aileron mixer -------------- -Two scalers total (output, roll). - -This mixer assumes that the aileron servos are set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -As there is only one output, if using two servos adjustments to compensate for -differences between the servos must be made mechanically. To obtain the correct -motion using a Y cable, the servos can be positioned reversed from one another. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 0 10000 10000 0 -10000 10000 - -Elevator mixer ------------- -Two scalers total (output, roll). - -This mixer assumes that the elevator servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 1 -10000 -10000 0 -10000 10000 - -Rudder mixer ------------- -Two scalers total (output, yaw). - -This mixer assumes that the rudder servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 2 10000 10000 0 -10000 10000 - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_default/mixers/FMU_AET.mix b/ROMFS/px4fmu_default/mixers/FMU_AET.mix deleted file mode 100644 index 20cb88b91..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_AET.mix +++ /dev/null @@ -1,60 +0,0 @@ -Aileron/elevator/throttle mixer for PX4FMU -================================================== - -This file defines mixers suitable for controlling a fixed wing aircraft with -aileron, elevator and throttle controls using PX4FMU. The configuration assumes -the aileron servo(s) are connected to PX4FMU servo output 0, the elevator to -output 1 and the throttle to output 3. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -Aileron mixer -------------- -Two scalers total (output, roll). - -This mixer assumes that the aileron servos are set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -As there is only one output, if using two servos adjustments to compensate for -differences between the servos must be made mechanically. To obtain the correct -motion using a Y cable, the servos can be positioned reversed from one another. - -Alternatively, output 2 could be used as a second aileron servo output with -separate mixing. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 0 10000 10000 0 -10000 10000 - -Elevator mixer ------------- -Two scalers total (output, roll). - -This mixer assumes that the elevator servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 1 -10000 -10000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_default/mixers/FMU_Q.mix b/ROMFS/px4fmu_default/mixers/FMU_Q.mix deleted file mode 100644 index ebcb66b24..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_Q.mix +++ /dev/null @@ -1,52 +0,0 @@ -Delta-wing mixer for PX4FMU -=========================== - -Designed for Bormatec Camflyer Q - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -for the elevons. - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 -5000 -8000 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 -8000 -5000 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 - diff --git a/ROMFS/px4fmu_default/mixers/FMU_RET.mix b/ROMFS/px4fmu_default/mixers/FMU_RET.mix deleted file mode 100644 index 95beb8927..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_RET.mix +++ /dev/null @@ -1,53 +0,0 @@ -Rudder/elevator/throttle mixer for PX4FMU -========================================= - -This file defines mixers suitable for controlling a fixed wing aircraft with -rudder, elevator and throttle controls using PX4FMU. The configuration assumes -the rudder servo is connected to PX4FMU servo output 0, the elevator to output 1 -and the throttle to output 3. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -Rudder mixer ------------- -Two scalers total (output, roll). - -This mixer assumes that the rudder servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 0 10000 10000 0 -10000 10000 - -Elevator mixer ------------- -Two scalers total (output, roll). - -This mixer assumes that the elevator servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 1 -10000 -10000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_default/mixers/FMU_X5.mix b/ROMFS/px4fmu_default/mixers/FMU_X5.mix deleted file mode 100644 index 9f81e1dc3..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_X5.mix +++ /dev/null @@ -1,50 +0,0 @@ -Delta-wing mixer for PX4FMU -=========================== - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -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 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 -5000 -3000 0 -10000 10000 -S: 0 1 5000 5000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 - diff --git a/ROMFS/px4fmu_default/mixers/FMU_delta.mix b/ROMFS/px4fmu_default/mixers/FMU_delta.mix deleted file mode 100644 index 981466704..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_delta.mix +++ /dev/null @@ -1,50 +0,0 @@ -Delta-wing mixer for PX4FMU -=========================== - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -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 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 5000 3000 0 -10000 10000 -S: 0 1 -5000 -5000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 - diff --git a/ROMFS/px4fmu_default/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_default/mixers/FMU_hex_+.mix deleted file mode 100644 index b5e38ce9e..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_hex_+.mix +++ /dev/null @@ -1,7 +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 diff --git a/ROMFS/px4fmu_default/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_default/mixers/FMU_hex_x.mix deleted file mode 100644 index 8e8d122ad..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_hex_x.mix +++ /dev/null @@ -1,7 +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 diff --git a/ROMFS/px4fmu_default/mixers/FMU_octo_+.mix b/ROMFS/px4fmu_default/mixers/FMU_octo_+.mix deleted file mode 100644 index 2cb70e814..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_octo_+.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a octocopter in the + configuration. All controls -are mixed 100%. - -R: 8+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_default/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_default/mixers/FMU_octo_x.mix deleted file mode 100644 index edc71f013..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_octo_x.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a octocopter in the X configuration. All controls -are mixed 100%. - -R: 8x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_default/mixers/FMU_pass.mix b/ROMFS/px4fmu_default/mixers/FMU_pass.mix deleted file mode 100644 index e9a81f2bb..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_pass.mix +++ /dev/null @@ -1,23 +0,0 @@ -Passthrough mixer for PX4FMU -============================ - -This file defines passthrough mixers suitable for testing. - -Channel group 0, channels 0-3 are passed directly through to the outputs. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 0 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 1 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 2 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 10000 10000 0 -10000 10000 - diff --git a/ROMFS/px4fmu_default/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_default/mixers/FMU_quad_+.mix deleted file mode 100644 index dfdf1d58e..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_quad_+.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a quadrotor in the + configuration. All controls -are mixed 100%. - -R: 4+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_default/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_default/mixers/FMU_quad_x.mix deleted file mode 100644 index 12a3bee20..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_quad_x.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a quadrotor in the X configuration. All controls -are mixed 100%. - -R: 4x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_default/mixers/README b/ROMFS/px4fmu_default/mixers/README deleted file mode 100644 index 6649c53c2..000000000 --- a/ROMFS/px4fmu_default/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 - - : - -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: - O: <-ve scale> <+ve scale> - -If is zero, the sum is effectively zero and the mixer will -output a fixed value that is constrained by and . - -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 entries describing the control -inputs and their scaling, in the form: - - S: <-ve scale> <+ve scale> - -The value identifies the control group from which the scaler will read, -and the 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: - -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. -- cgit v1.2.3