From a7a1cc4625ea4097761a9aef88f9f0b4608944a4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 16 Jan 2013 23:40:40 -0800 Subject: Add support for per-config ROMFS generation. --- Makefile | 2 - ROMFS/.gitignore | 1 - ROMFS/Makefile | 120 ---------------- ROMFS/logging/logconv.m | 224 ----------------------------- ROMFS/mixers/FMU_AERT.mix | 64 --------- ROMFS/mixers/FMU_AET.mix | 60 -------- ROMFS/mixers/FMU_Q.mix | 52 ------- ROMFS/mixers/FMU_RET.mix | 53 ------- ROMFS/mixers/FMU_X5.mix | 50 ------- ROMFS/mixers/FMU_hex_+.mix | 7 - ROMFS/mixers/FMU_hex_x.mix | 7 - ROMFS/mixers/FMU_octo_+.mix | 7 - ROMFS/mixers/FMU_octo_x.mix | 7 - ROMFS/mixers/FMU_pass.mix | 23 --- ROMFS/mixers/FMU_quad_+.mix | 7 - ROMFS/mixers/FMU_quad_x.mix | 7 - ROMFS/mixers/README | 154 -------------------- ROMFS/px4fmu_default/logging/logconv.m | 224 +++++++++++++++++++++++++++++ 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 ++++++++++++++++++++ ROMFS/scripts/rc.FMU_quad_x | 67 --------- ROMFS/scripts/rc.PX4IO | 80 ----------- ROMFS/scripts/rc.PX4IOAR | 98 ------------- ROMFS/scripts/rc.boarddetect | 66 --------- ROMFS/scripts/rc.jig | 10 -- ROMFS/scripts/rc.logging | 9 -- ROMFS/scripts/rc.sensors | 34 ----- ROMFS/scripts/rc.standalone | 13 -- ROMFS/scripts/rcS | 79 ---------- makefiles/config_px4fmu_default.mk | 1 + makefiles/firmware.mk | 58 +++++++- makefiles/upload.mk | 2 +- 44 files changed, 831 insertions(+), 1303 deletions(-) delete mode 100644 ROMFS/.gitignore delete mode 100644 ROMFS/Makefile delete mode 100755 ROMFS/logging/logconv.m delete mode 100644 ROMFS/mixers/FMU_AERT.mix delete mode 100644 ROMFS/mixers/FMU_AET.mix delete mode 100644 ROMFS/mixers/FMU_Q.mix delete mode 100644 ROMFS/mixers/FMU_RET.mix delete mode 100644 ROMFS/mixers/FMU_X5.mix delete mode 100644 ROMFS/mixers/FMU_hex_+.mix delete mode 100644 ROMFS/mixers/FMU_hex_x.mix delete mode 100644 ROMFS/mixers/FMU_octo_+.mix delete mode 100644 ROMFS/mixers/FMU_octo_x.mix delete mode 100644 ROMFS/mixers/FMU_pass.mix delete mode 100644 ROMFS/mixers/FMU_quad_+.mix delete mode 100644 ROMFS/mixers/FMU_quad_x.mix delete mode 100644 ROMFS/mixers/README create mode 100755 ROMFS/px4fmu_default/logging/logconv.m create mode 100644 ROMFS/px4fmu_default/mixers/FMU_AERT.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_AET.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_Q.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_RET.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_X5.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_delta.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_hex_+.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_hex_x.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_octo_+.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_octo_x.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_pass.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_quad_+.mix create mode 100644 ROMFS/px4fmu_default/mixers/FMU_quad_x.mix create mode 100644 ROMFS/px4fmu_default/mixers/README delete mode 100644 ROMFS/scripts/rc.FMU_quad_x delete mode 100644 ROMFS/scripts/rc.PX4IO delete mode 100644 ROMFS/scripts/rc.PX4IOAR delete mode 100644 ROMFS/scripts/rc.boarddetect delete mode 100644 ROMFS/scripts/rc.jig delete mode 100644 ROMFS/scripts/rc.logging delete mode 100644 ROMFS/scripts/rc.sensors delete mode 100644 ROMFS/scripts/rc.standalone delete mode 100755 ROMFS/scripts/rcS diff --git a/Makefile b/Makefile index 4d42831f4..b5a66e59a 100644 --- a/Makefile +++ b/Makefile @@ -103,8 +103,6 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)/%.export: $(NUTTX_SRC) $(NUTTX_APPS) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/tools && ./configure.sh $(platform)/$(configuration)) - @echo Generating ROMFS for $(platform) XXX move this! - $(Q) make -C $(ROMFS_SRC) all @echo %% Exporting NuttX for $(platform) $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) export $(Q) mkdir -p $(dir $@) diff --git a/ROMFS/.gitignore b/ROMFS/.gitignore deleted file mode 100644 index 30d3d7fe5..000000000 --- a/ROMFS/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/img diff --git a/ROMFS/Makefile b/ROMFS/Makefile deleted file mode 100644 index d827fa491..000000000 --- a/ROMFS/Makefile +++ /dev/null @@ -1,120 +0,0 @@ -# -# Makefile to generate a PX4FMU ROMFS image. -# -# In normal use, 'make install' will generate a new ROMFS header and place it -# into the px4fmu configuration in the appropriate location. -# - -# -# Directories of interest -# -SRCROOT ?= $(dir $(lastword $(MAKEFILE_LIST))) -BUILDROOT ?= $(SRCROOT)/img -ROMFS_HEADER ?= $(SRCROOT)/../nuttx/configs/px4fmu/include/nsh_romfsimg.h - -# -# List of files to install in the ROMFS, specified as ~ -# -ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \ - $(SRCROOT)/scripts/rc.sensors~init.d/rc.sensors \ - $(SRCROOT)/scripts/rc.logging~init.d/rc.logging \ - $(SRCROOT)/scripts/rc.standalone~init.d/rc.standalone \ - $(SRCROOT)/scripts/rc.PX4IO~init.d/rc.PX4IO \ - $(SRCROOT)/scripts/rc.PX4IOAR~init.d/rc.PX4IOAR \ - $(SRCROOT)/scripts/rc.FMU_quad_x~init.d/rc.FMU_quad_x \ - $(SRCROOT)/mixers/FMU_pass.mix~mixers/FMU_pass.mix \ - $(SRCROOT)/mixers/FMU_Q.mix~mixers/FMU_Q.mix \ - $(SRCROOT)/mixers/FMU_X5.mix~mixers/FMU_X5.mix \ - $(SRCROOT)/mixers/FMU_AERT.mix~mixers/FMU_AERT.mix \ - $(SRCROOT)/mixers/FMU_AET.mix~mixers/FMU_AET.mix \ - $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \ - $(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \ - $(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \ - $(SRCROOT)/mixers/FMU_hex_x.mix~mixers/FMU_hex_x.mix \ - $(SRCROOT)/mixers/FMU_hex_+.mix~mixers/FMU_hex_+.mix \ - $(SRCROOT)/mixers/FMU_octo_x.mix~mixers/FMU_octo_x.mix \ - $(SRCROOT)/mixers/FMU_octo_+.mix~mixers/FMU_octo_+.mix \ - $(SRCROOT)/logging/logconv.m~logging/logconv.m - -# the EXTERNAL_SCRIPTS variable is used to add out of tree scripts -# to ROMFS. -ROMFS_FSSPEC += $(EXTERNAL_SCRIPTS) - -# -# Add the PX4IO firmware to the spec if someone has dropped it into the -# source directory, or otherwise specified its location. -# -# Normally this is only something you'd do when working on PX4IO; most -# users will upgrade with firmware off the microSD card. -# -PX4IO_FIRMWARE ?= $(SRCROOT)/px4io.bin -ifneq ($(wildcard $(PX4IO_FIRMWARE)),) -ROMFS_FSSPEC += $(PX4IO_FIRMWARE)~px4io.bin -endif - -################################################################################ -# No user-serviceable parts below -################################################################################ - -# -# Just the source files from the ROMFS spec, so that we can fail cleanly if they don't -# exist -# -ROMFS_SRCFILES = $(foreach spec,$(ROMFS_FSSPEC),$(firstword $(subst ~, ,$(spec)))) - -# -# Just the destination directories from the ROMFS spec -# -ROMFS_DIRS = $(sort $(dir $(foreach spec,$(ROMFS_FSSPEC),$(lastword $(subst ~, ,$(spec)))))) - - -# -# Intermediate products -# -ROMFS_IMG = $(BUILDROOT)/romfs.img -ROMFS_WORKDIR = $(BUILDROOT)/romfs - -# -# Convenience target for rebuilding the ROMFS header -# -all: $(ROMFS_HEADER) - -$(ROMFS_HEADER): $(ROMFS_IMG) $(dir $(ROMFS_HEADER)) - @echo Generating the ROMFS header... - @(cd $(dir $(ROMFS_IMG)) && xxd -i $(notdir $(ROMFS_IMG))) > $@ - -$(ROMFS_IMG): $(ROMFS_WORKDIR) - @echo Generating the ROMFS image... - @genromfs -f $@ -d $(ROMFS_WORKDIR) -V "NSHInitVol" - -$(ROMFS_WORKDIR): $(ROMFS_SRCFILES) - @echo Rebuilding the ROMFS work area... - @rm -rf $(ROMFS_WORKDIR) - @mkdir -p $(ROMFS_WORKDIR) - @for dir in $(ROMFS_DIRS) ; do mkdir -p $(ROMFS_WORKDIR)/$$dir; done - @for spec in $(ROMFS_FSSPEC) ; do \ - echo $$spec | sed -e 's%^.*~% %' ;\ - `echo "cp $$spec" | sed -e 's%~% $(ROMFS_WORKDIR)/%'` ;\ - done - -$(BUILDROOT): - @mkdir -p $(BUILDROOT) - -clean: - @rm -rf $(BUILDROOT) - -distclean: clean - @rm -f $(PX4IO_FIRMWARE) $(ROMFS_HEADER) - -.PHONY: all install clean distclean - -# -# Hacks and fixups -# -SYSTYPE = $(shell uname) - -ifeq ($(SYSTYPE),Darwin) -# PATH inherited by Eclipse may not include toolchain install location -export PATH := $(PATH):/usr/local/bin -endif - diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m deleted file mode 100755 index 5ea2aeb95..000000000 --- a/ROMFS/logging/logconv.m +++ /dev/null @@ -1,224 +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. - -% Clear everything -clc -clear all -close all - -% Set the path to your sysvector.bin file here -filePath = 'sysvector.bin'; - -% 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 - -%%%%%%%%%%%%%%%%%%%%%%% -% 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[3]; //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', 3, '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 - -%% Plot GPS RAW measurements - -% Only plot GPS data if available -if cumsum(double(sysvector.gps_raw_position(200:end,1))) > 0 - figure('units','normalized','outerposition',[0 0 1 1]) - plot3(sysvector.gps_raw_position(200:end,1), sysvector.gps_raw_position(200:end,2), sysvector.gps_raw_position(200:end,3)); -end - - -%% Plot optical flow trajectory - -flow_sz = size(sysvector.timestamp); -flow_elements = flow_sz(1); - -xt(1:flow_elements,1) = sysvector.timestamp(:,1); % time column [ms] - - -%calc dt -dt = zeros(flow_elements,1); -for i = 1:flow_elements-1 - dt(i+1,1) = double(xt(i+1,1)-xt(i,1)) * 10^(-6); % timestep [s] -end -dt(1,1) = mean(dt); - - -global_speed = zeros(flow_elements,3); - -%calc global speed (with rot matrix) -for i = 1:flow_elements - rotM = [sysvector.rot_matrix(i,1:3);sysvector.rot_matrix(i,4:6);sysvector.rot_matrix(i,7:9)]'; - speedX = sysvector.optical_flow(i,3); - speedY = sysvector.optical_flow(i,4); - - relSpeed = [-speedY,speedX,0]; - global_speed(i,:) = relSpeed * rotM; -end - - - -px = zeros(flow_elements,1); -py = zeros(flow_elements,1); -distance = 0; - -last_vx = 0; -last_vy = 0; -elem_cnt = 0; - -% Very basic accumulation, stops on bad flow quality -for i = 1:flow_elements - if sysvector.optical_flow(i,6) > 5 - px(i,1) = global_speed(i,1)*dt(i,1); - py(i,1) = global_speed(i,2)*dt(i,1); - distance = distance + norm([px(i,1) py(i,1)]); - last_vx = px(i,1); - last_vy = py(i,1); - else - px(i,1) = last_vx; - py(i,1) = last_vy; - last_vx = last_vx*0.95; - last_vy = last_vy*0.95; - end -end - -px_sum = cumsum(px); -py_sum = cumsum(py); -time = cumsum(dt); - -figure() -set(gca, 'Units','normal'); - -plot(py_sum, px_sum, '-blue', 'LineWidth',2); -axis equal; -% set title and axis captions -xlabel('X position (meters)','fontsize',14) -ylabel('Y position (meters)','fontsize',14) -% mark begin and end -hold on -plot(py_sum(1,1),px_sum(1,1),'ks','LineWidth',2,... -'MarkerEdgeColor','k',... -'MarkerFaceColor','g',... -'MarkerSize',10) -hold on -plot(py_sum(end,1),px_sum(end,1),'kv','LineWidth',2,... -'MarkerEdgeColor','k',... -'MarkerFaceColor','b',... -'MarkerSize',10) -% add total length as annotation -set(gca,'fontsize',13); -legend('Trajectory', 'START', sprintf('END\n(%.2f m, %.0f:%.0f s)', distance, time_m, time_s - time_m*60)); -title('Optical Flow Position Integration', 'fontsize', 15); - -figure() -plot(time, sysvector.optical_flow(:,5), 'blue'); -axis([time(1,1) time(end,1) 0 (max(sysvector.optical_flow(i,5))+0.2)]); -xlabel('seconds','fontsize',14); -ylabel('m','fontsize',14); -set(gca,'fontsize',13); -title('Ultrasound Altitude', 'fontsize', 15); - - -figure() -plot(time, global_speed(:,2), 'red'); -hold on; -plot(time, global_speed(:,1), 'blue'); -legend('y velocity (m/s)', 'x velocity (m/s)'); -xlabel('seconds','fontsize',14); -ylabel('m/s','fontsize',14); -set(gca,'fontsize',13); -title('Optical Flow Velocity', 'fontsize', 15); diff --git a/ROMFS/mixers/FMU_AERT.mix b/ROMFS/mixers/FMU_AERT.mix deleted file mode 100644 index 75e82bb00..000000000 --- a/ROMFS/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/mixers/FMU_AET.mix b/ROMFS/mixers/FMU_AET.mix deleted file mode 100644 index 20cb88b91..000000000 --- a/ROMFS/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/mixers/FMU_Q.mix b/ROMFS/mixers/FMU_Q.mix deleted file mode 100644 index a35d299fd..000000000 --- a/ROMFS/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/mixers/FMU_RET.mix b/ROMFS/mixers/FMU_RET.mix deleted file mode 100644 index 95beb8927..000000000 --- a/ROMFS/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/mixers/FMU_X5.mix b/ROMFS/mixers/FMU_X5.mix deleted file mode 100644 index 981466704..000000000 --- a/ROMFS/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/mixers/FMU_hex_+.mix b/ROMFS/mixers/FMU_hex_+.mix deleted file mode 100644 index b5e38ce9e..000000000 --- a/ROMFS/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/mixers/FMU_hex_x.mix b/ROMFS/mixers/FMU_hex_x.mix deleted file mode 100644 index 8e8d122ad..000000000 --- a/ROMFS/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/mixers/FMU_octo_+.mix b/ROMFS/mixers/FMU_octo_+.mix deleted file mode 100644 index 2cb70e814..000000000 --- a/ROMFS/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/mixers/FMU_octo_x.mix b/ROMFS/mixers/FMU_octo_x.mix deleted file mode 100644 index edc71f013..000000000 --- a/ROMFS/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/mixers/FMU_pass.mix b/ROMFS/mixers/FMU_pass.mix deleted file mode 100644 index e9a81f2bb..000000000 --- a/ROMFS/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/mixers/FMU_quad_+.mix b/ROMFS/mixers/FMU_quad_+.mix deleted file mode 100644 index dfdf1d58e..000000000 --- a/ROMFS/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/mixers/FMU_quad_x.mix b/ROMFS/mixers/FMU_quad_x.mix deleted file mode 100644 index 12a3bee20..000000000 --- a/ROMFS/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/mixers/README b/ROMFS/mixers/README deleted file mode 100644 index 6649c53c2..000000000 --- a/ROMFS/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. diff --git a/ROMFS/px4fmu_default/logging/logconv.m b/ROMFS/px4fmu_default/logging/logconv.m new file mode 100755 index 000000000..5ea2aeb95 --- /dev/null +++ b/ROMFS/px4fmu_default/logging/logconv.m @@ -0,0 +1,224 @@ +% This Matlab Script can be used to import the binary logged values of the +% PX4FMU into data that can be plotted and analyzed. + +% Clear everything +clc +clear all +close all + +% Set the path to your sysvector.bin file here +filePath = 'sysvector.bin'; + +% 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 + +%%%%%%%%%%%%%%%%%%%%%%% +% 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[3]; //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', 3, '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 + +%% Plot GPS RAW measurements + +% Only plot GPS data if available +if cumsum(double(sysvector.gps_raw_position(200:end,1))) > 0 + figure('units','normalized','outerposition',[0 0 1 1]) + plot3(sysvector.gps_raw_position(200:end,1), sysvector.gps_raw_position(200:end,2), sysvector.gps_raw_position(200:end,3)); +end + + +%% Plot optical flow trajectory + +flow_sz = size(sysvector.timestamp); +flow_elements = flow_sz(1); + +xt(1:flow_elements,1) = sysvector.timestamp(:,1); % time column [ms] + + +%calc dt +dt = zeros(flow_elements,1); +for i = 1:flow_elements-1 + dt(i+1,1) = double(xt(i+1,1)-xt(i,1)) * 10^(-6); % timestep [s] +end +dt(1,1) = mean(dt); + + +global_speed = zeros(flow_elements,3); + +%calc global speed (with rot matrix) +for i = 1:flow_elements + rotM = [sysvector.rot_matrix(i,1:3);sysvector.rot_matrix(i,4:6);sysvector.rot_matrix(i,7:9)]'; + speedX = sysvector.optical_flow(i,3); + speedY = sysvector.optical_flow(i,4); + + relSpeed = [-speedY,speedX,0]; + global_speed(i,:) = relSpeed * rotM; +end + + + +px = zeros(flow_elements,1); +py = zeros(flow_elements,1); +distance = 0; + +last_vx = 0; +last_vy = 0; +elem_cnt = 0; + +% Very basic accumulation, stops on bad flow quality +for i = 1:flow_elements + if sysvector.optical_flow(i,6) > 5 + px(i,1) = global_speed(i,1)*dt(i,1); + py(i,1) = global_speed(i,2)*dt(i,1); + distance = distance + norm([px(i,1) py(i,1)]); + last_vx = px(i,1); + last_vy = py(i,1); + else + px(i,1) = last_vx; + py(i,1) = last_vy; + last_vx = last_vx*0.95; + last_vy = last_vy*0.95; + end +end + +px_sum = cumsum(px); +py_sum = cumsum(py); +time = cumsum(dt); + +figure() +set(gca, 'Units','normal'); + +plot(py_sum, px_sum, '-blue', 'LineWidth',2); +axis equal; +% set title and axis captions +xlabel('X position (meters)','fontsize',14) +ylabel('Y position (meters)','fontsize',14) +% mark begin and end +hold on +plot(py_sum(1,1),px_sum(1,1),'ks','LineWidth',2,... +'MarkerEdgeColor','k',... +'MarkerFaceColor','g',... +'MarkerSize',10) +hold on +plot(py_sum(end,1),px_sum(end,1),'kv','LineWidth',2,... +'MarkerEdgeColor','k',... +'MarkerFaceColor','b',... +'MarkerSize',10) +% add total length as annotation +set(gca,'fontsize',13); +legend('Trajectory', 'START', sprintf('END\n(%.2f m, %.0f:%.0f s)', distance, time_m, time_s - time_m*60)); +title('Optical Flow Position Integration', 'fontsize', 15); + +figure() +plot(time, sysvector.optical_flow(:,5), 'blue'); +axis([time(1,1) time(end,1) 0 (max(sysvector.optical_flow(i,5))+0.2)]); +xlabel('seconds','fontsize',14); +ylabel('m','fontsize',14); +set(gca,'fontsize',13); +title('Ultrasound Altitude', 'fontsize', 15); + + +figure() +plot(time, global_speed(:,2), 'red'); +hold on; +plot(time, global_speed(:,1), 'blue'); +legend('y velocity (m/s)', 'x velocity (m/s)'); +xlabel('seconds','fontsize',14); +ylabel('m/s','fontsize',14); +set(gca,'fontsize',13); +title('Optical Flow Velocity', 'fontsize', 15); diff --git a/ROMFS/px4fmu_default/mixers/FMU_AERT.mix b/ROMFS/px4fmu_default/mixers/FMU_AERT.mix new file mode 100644 index 000000000..75e82bb00 --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_AET.mix b/ROMFS/px4fmu_default/mixers/FMU_AET.mix new file mode 100644 index 000000000..20cb88b91 --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_Q.mix b/ROMFS/px4fmu_default/mixers/FMU_Q.mix new file mode 100644 index 000000000..a35d299fd --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_RET.mix b/ROMFS/px4fmu_default/mixers/FMU_RET.mix new file mode 100644 index 000000000..95beb8927 --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_X5.mix b/ROMFS/px4fmu_default/mixers/FMU_X5.mix new file mode 100644 index 000000000..981466704 --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_delta.mix b/ROMFS/px4fmu_default/mixers/FMU_delta.mix new file mode 100644 index 000000000..981466704 --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_default/mixers/FMU_hex_+.mix new file mode 100644 index 000000000..b5e38ce9e --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_default/mixers/FMU_hex_x.mix new file mode 100644 index 000000000..8e8d122ad --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_octo_+.mix b/ROMFS/px4fmu_default/mixers/FMU_octo_+.mix new file mode 100644 index 000000000..2cb70e814 --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_default/mixers/FMU_octo_x.mix new file mode 100644 index 000000000..edc71f013 --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_pass.mix b/ROMFS/px4fmu_default/mixers/FMU_pass.mix new file mode 100644 index 000000000..e9a81f2bb --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_default/mixers/FMU_quad_+.mix new file mode 100644 index 000000000..dfdf1d58e --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_default/mixers/FMU_quad_x.mix new file mode 100644 index 000000000..12a3bee20 --- /dev/null +++ b/ROMFS/px4fmu_default/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_default/mixers/README b/ROMFS/px4fmu_default/mixers/README new file mode 100644 index 000000000..6649c53c2 --- /dev/null +++ b/ROMFS/px4fmu_default/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/scripts/rc.FMU_quad_x b/ROMFS/scripts/rc.FMU_quad_x deleted file mode 100644 index 8787443ea..000000000 --- a/ROMFS/scripts/rc.FMU_quad_x +++ /dev/null @@ -1,67 +0,0 @@ -#!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/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO deleted file mode 100644 index 1e3963b9a..000000000 --- a/ROMFS/scripts/rc.PX4IO +++ /dev/null @@ -1,80 +0,0 @@ -#!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/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR deleted file mode 100644 index 72df68e35..000000000 --- a/ROMFS/scripts/rc.PX4IOAR +++ /dev/null @@ -1,98 +0,0 @@ -#!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/scripts/rc.boarddetect b/ROMFS/scripts/rc.boarddetect deleted file mode 100644 index f233e51df..000000000 --- a/ROMFS/scripts/rc.boarddetect +++ /dev/null @@ -1,66 +0,0 @@ -#!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/scripts/rc.jig b/ROMFS/scripts/rc.jig deleted file mode 100644 index e2b5d8f30..000000000 --- a/ROMFS/scripts/rc.jig +++ /dev/null @@ -1,10 +0,0 @@ -#!nsh -# -# Test jig startup script -# - -echo "[testing] doing production test.." - -tests jig - -echo "[testing] testing done" diff --git a/ROMFS/scripts/rc.logging b/ROMFS/scripts/rc.logging deleted file mode 100644 index 09c2d00d1..000000000 --- a/ROMFS/scripts/rc.logging +++ /dev/null @@ -1,9 +0,0 @@ -#!nsh -# -# Initialise logging services. -# - -if [ -d /fs/microsd ] -then - sdlog start -fi diff --git a/ROMFS/scripts/rc.sensors b/ROMFS/scripts/rc.sensors deleted file mode 100644 index 42c2f52e9..000000000 --- a/ROMFS/scripts/rc.sensors +++ /dev/null @@ -1,34 +0,0 @@ -#!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/scripts/rc.standalone b/ROMFS/scripts/rc.standalone deleted file mode 100644 index 67e95215b..000000000 --- a/ROMFS/scripts/rc.standalone +++ /dev/null @@ -1,13 +0,0 @@ -#!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/scripts/rcS b/ROMFS/scripts/rcS deleted file mode 100755 index 89a767879..000000000 --- a/ROMFS/scripts/rcS +++ /dev/null @@ -1,79 +0,0 @@ -#!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/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 45b1170ff..e0c1aff9d 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -4,5 +4,6 @@ CONFIG = px4fmu_default SRCS = $(PX4_BASE)/platforms/empty.c +ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) include $(PX4_BASE)/makefiles/firmware.mk diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 338bda8d3..d64d39ea4 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -23,6 +23,11 @@ # to the directory 'build' under the directory containing the # parent Makefile. # +# ROMFS_ROOT: +# If set to the path to a directory, a ROMFS image will be generated +# containing the files under the directory and linked into the final +# image. +# ################################################################################ # Paths and configuration @@ -71,6 +76,7 @@ MKFW = $(PX4_BASE)/Tools/px_mkfw.py COPY = cp REMOVE = rm -f RMDIR = rm -rf +GENROMFS = genromfs # # Sanity-check the PLATFORM variable and then get the platform config. @@ -134,6 +140,29 @@ LIB_DIRS += $(NUTTX_EXPORT_DIR)/libs LIBS += -lapps -lnuttx LINK_DEPS += $(wildcard $(addsuffix /*.a,$(LIB_DIRS))) +################################################################################ +# ROMFS generation +################################################################################ + +# +# Note that we can't just put romfs.c in SRCS, as it's depended on by the +# NuttX export library. Instead, we have to treat it like a library. +# +ifneq ($(ROMFS_ROOT),) +ROMFS_DEPS += $(wildcard \ + (ROMFS_ROOT)/* \ + (ROMFS_ROOT)/*/* \ + (ROMFS_ROOT)/*/*/* \ + (ROMFS_ROOT)/*/*/*/* \ + (ROMFS_ROOT)/*/*/*/*/* \ + (ROMFS_ROOT)/*/*/*/*/*/*) +ROMFS_IMG = $(WORK_DIR)/romfs.img +ROMFS_CSRC = $(ROMFS_IMG:.img=.c) +ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) +LIBS += $(ROMFS_OBJ) +LINK_DEPS += $(ROMFS_OBJ) +endif + ################################################################################ # Build rules ################################################################################ @@ -159,7 +188,7 @@ all: $(PRODUCT_BUNDLE) OBJS = $(foreach src,$(SRCS),$(WORK_DIR)/$(src).o) # -# Rules +# SRCS -> OBJS rules # $(filter %.c.o,$(OBJS)): $(WORK_DIR)/%.c.o: %.c $(GLOBAL_DEPS) @@ -174,11 +203,19 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)/%.S.o: %.S $(GLOBAL_DEPS) @mkdir -p $(dir $@) $(call ASSEMBLE,$<,$@) +# +# Build directory setup rules +# + $(NUTTX_CONFIG_HEADER): $(NUTTX_ARCHIVE) @echo %% Unpacking $(NUTTX_ARCHIVE) $(Q) unzip -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE) $(Q) touch $@ +# +# Built product rules +# + $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @echo %% Generating $@ $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(PLATFORM).prototype \ @@ -191,6 +228,25 @@ $(PRODUCT_BIN): $(PRODUCT_SYM) $(PRODUCT_SYM): $(OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(call LINK,$@,$(OBJS)) +# +# ROMFS rules +# + +$(ROMFS_OBJ): $(ROMFS_CSRC) + $(call COMPILE,$<,$@) + +$(ROMFS_CSRC): $(ROMFS_IMG) + @echo %% generating $@ + $(Q) (cd $(dir $<) && xxd -i $(notdir $<)) > $@ + +$(ROMFS_IMG): $(ROMFS_DEPS) + @echo %% generating $@ + $(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol" + +# +# Utility rules +# + upload: $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(Q) make -f $(PX4_MK_INCLUDE)/upload.mk \ METHOD=serial \ diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 15e0e240a..5308aaa3e 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -1,5 +1,5 @@ # -# Rules and tools for uploading firmware. +# Rules and tools for uploading firmware to various PX4 boards. # UPLOADER = $(PX4_BASE)/Tools/px_uploader.py -- cgit v1.2.3