Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 17:35:22 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into hott

Browse Source
This commit is contained in:
Simon Wilks
2013-01-24 23:27:27 +01:00
parent 9924c4f425 beb4522298
commit 0669d2aee0
1298 changed files with 213453 additions and 11802 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+16 -2
View File
@@ -1,16 +1,23 @@
.built
*.context
*.bdat
*.pdat
.depend
.updated
.config
.config-e
.version
.project
.cproject
apps/namedapp/namedapp_list.h
apps/namedapp/namedapp_proto.h
apps/builtin/builtin_list.h
apps/builtin/builtin_proto.h
Make.dep
*.pyc
*.o
*.a
*.d
*~
*.dSYM
Images/*.bin
Images/*.px4
nuttx/Make.defs
@@ -40,3 +47,10 @@ nsh_romfsimg.h
cscope.out
.configX-e
nuttx-export.zip
dot.gdbinit
mavlink/include/mavlink/v0.9/
.*.swp
.swp
core
.gdbinit
mkdeps
Debug/NuttX
+17 -5
View File
@@ -168,17 +168,29 @@ define _showsemaphore
printf "\n"
end
#
# Print information about a task's stack usage
#
define showtaskstack
set $task = (struct _TCB *)$arg0
set $stack_free = 0
while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free)
set $stack_free = $stack_free + 1
end
printf" stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free
end
#
# Print details of a task
#
define showtask
set $task = (struct _TCB *)$arg0
printf "%p %.2d ", $task, $task->pid
_showtaskstate $task
printf " %s\n", $task->name
set $stack_free = 0
while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free)
set $stack_free = $stack_free + 1
end
printf" stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free
showtaskstack $task
if $task->task_state == TSTATE_WAIT_SEM
printf " waiting on %p ", $task->waitsem
Debug/PX4
+54
View File
@@ -0,0 +1,54 @@
#
# Various PX4-specific macros
#
source Debug/NuttX
echo Loading PX4 GDB macros. Use 'help px4' for more information.\n
define px4
echo Use 'help px4' for more information.\n
end
document px4
. Various macros for working with the PX4 firmware.
.
. perf
. Prints the state of all performance counters.
.
. Use 'help <macro>' for more specific help.
end
define _perf_print
set $hdr = (struct perf_ctr_header *)$arg0
printf "%p\n", $hdr
printf "%s: ", $hdr->name
# PC_COUNT
if $hdr->type == 0
set $count = (struct perf_ctr_count *)$hdr
printf "%llu events,\n", $count->event_count;
end
# PC_ELPASED
if $hdr->type == 1
set $elapsed = (struct perf_ctr_elapsed *)$hdr
printf "%llu events, %lluus elapsed, min %lluus, max %lluus\n", $elapsed->event_count, $elapsed->time_total, $elapsed->time_least, $elapsed->time_most
end
# PC_INTERVAL
if $hdr->type == 2
set $interval = (struct perf_ctr_interval *)$hdr
printf "%llu events, %llu avg, min %lluus max %lluus\n", $interval->event_count, ($interval->time_last - $interval->time_first) / $interval->event_count, $interval->time_least, $interval->time_most
end
end
define perf
set $ctr = (sq_entry_t *)(perf_counters.head)
while $ctr != 0
_perf_print $ctr
set $ctr = $ctr->flink
end
end
document perf
. perf
. Prints performance counters.
end
Makefile
+33 -15
View File
@@ -28,12 +28,8 @@ UPLOADER = $(PX4BASE)/Tools/px_uploader.py
# What are we currently configured for?
#
CONFIGURED = $(PX4BASE)/.configured
ifeq ($(wildcard $(CONFIGURED)),)
# the $(CONFIGURED) target will make this a reality before building
export TARGET = px4fmu
$(shell echo $(TARGET) > $(CONFIGURED))
else
export TARGET = $(shell cat $(CONFIGURED))
ifneq ($(wildcard $(CONFIGURED)),)
export TARGET := $(shell cat $(CONFIGURED))
endif
#
@@ -59,12 +55,13 @@ $(FIRMWARE_BUNDLE): $(FIRMWARE_BINARY) $(MKFW) $(FIRMWARE_PROTOTYPE)
@$(MKFW) --prototype $(FIRMWARE_PROTOTYPE) \
--git_identity $(PX4BASE) \
--image $(FIRMWARE_BINARY) > $@
#
# Build the firmware binary.
#
.PHONY: $(FIRMWARE_BINARY)
$(FIRMWARE_BINARY): configure_$(TARGET) setup_$(TARGET)
@echo Building $@
$(FIRMWARE_BINARY): setup_$(TARGET) configure-check
@echo Building $@ for $(TARGET)
@make -C $(NUTTX_SRC) -r $(MQUIET) all
@cp $(NUTTX_SRC)/nuttx.bin $@
@@ -73,19 +70,26 @@ $(FIRMWARE_BINARY): configure_$(TARGET) setup_$(TARGET)
# and makes it current.
#
configure_px4fmu:
ifneq ($(TARGET),px4fmu)
@echo Configuring for px4fmu
@make -C $(PX4BASE) distclean
endif
@cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4fmu/nsh
@echo px4fmu > $(CONFIGURED)
configure_px4io:
ifneq ($(TARGET),px4io)
@echo Configuring for px4io
@make -C $(PX4BASE) distclean
endif
@cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4io/io
@echo px4io > $(CONFIGURED)
configure-check:
ifeq ($(wildcard $(CONFIGURED)),)
@echo
@echo "Not configured - use 'make configure_px4fmu' or 'make configure_px4io' first"
@echo
@exit 1
endif
#
# Per-configuration additional targets
#
@@ -96,6 +100,9 @@ setup_px4fmu:
setup_px4io:
# fake target to make configure-check happy if TARGET is not set
setup_:
#
# Firmware uploading.
#
@@ -109,15 +116,26 @@ ifeq ($(SYSTYPE),Linux)
SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0"
endif
ifeq ($(SERIAL_PORTS),)
SERIAL_PORTS = "\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0"
SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0"
endif
upload: $(FIRMWARE_BUNDLE) $(UPLOADER)
@python -u $(UPLOADER) --port $(SERIAL_PORTS) $(FIRMWARE_BUNDLE)
#
# JTAG firmware uploading with OpenOCD
#
ifeq ($(JTAGCONFIG),)
JTAGCONFIG=interface/olimex-jtag-tiny.cfg
endif
upload-jtag-px4fmu:
@echo Attempting to flash PX4FMU board via JTAG
@openocd -f interface/olimex-jtag-tiny.cfg -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
@openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
upload-jtag-px4io: all
@echo Attempting to flash PX4IO board via JTAG
@openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f1x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4io_bl.elf" -c "reset run" -c shutdown
#
# Hacks and fixups
ROMFS/Makefile
+8
View File
@@ -29,8 +29,16 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \
$(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.
ROMFS/logging/logconv.m
+77 -198
View File
@@ -1,6 +1,21 @@
% 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
%
@@ -16,220 +31,84 @@ close all
% 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
%myPath = '..\LOG30102012\session0002\'; %set relative path here
myPath = '.\';
myFile = 'sysvector.bin';
filePath = strcat(myPath,myFile);
if exist(filePath, 'file')
fileInfo = dir(filePath);
fileSize = fileInfo.bytes;
fid = fopen(filePath, 'r');
elements = int64(fileSize./(16*4+8))/4
elements = int64(fileSize./(lineLength))
for i=1:elements
% timestamp
sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% gyro (3 channels)
sensors(i,2:4) = fread(fid, 3, 'float', 0, 'ieee-le');
% accelerometer (3 channels)
sensors(i,5:7) = fread(fid, 3, 'float', 0, 'ieee-le');
% mag (3 channels)
sensors(i,8:10) = fread(fid, 3, 'float', 0, 'ieee-le');
% baro pressure
sensors(i,11) = fread(fid, 1, 'float', 0, 'ieee-le');
% baro alt
sensors(i,12) = fread(fid, 1, 'float', 0, 'ieee-le');
% baro temp
sensors(i,13) = fread(fid, 1, 'float', 0, 'ieee-le');
% actuator control (4 channels)
sensors(i,14:17) = fread(fid, 4, 'float', 0, 'ieee-le');
% actuator outputs (8 channels)
sensors(i,18:25) = fread(fid, 8, 'float', 0, 'ieee-le');
% vbat
sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
% adc voltage (3 channels)
sensors(i,27:29) = fread(fid, 3, 'float', 0, 'ieee-le');
% local position (3 channels)
sensors(i,30:32) = fread(fid, 3, 'float', 0, 'ieee-le');
% gps_raw_position (3 channels)
sensors(i,33:35) = fread(fid, 3, 'uint32', 0, 'ieee-le');
% attitude (3 channels)
sensors(i,36:38) = fread(fid, 3, 'float', 0, 'ieee-le');
% RotMatrix (9 channels)
sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le');
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
time_us = sensors(elements,1) - sensors(1,1);
% 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
%% old version of reading in different files from sdlog.c
% if exist('sysvector.bin', 'file')
% % Read actuators file
% myFile = java.io.File('sysvector.bin')
% fileSize = length(myFile)
%
% fid = fopen('sysvector.bin', 'r');
% elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
%
% for i=1:elements
% % timestamp
% sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% % actuators 1-16
% % quadrotor: motor 1-4 on the first four positions
% sysvector(i, 2:32) = fread(fid, 28+3, 'float', 'ieee-le');
% sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
% end
%
% sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
% sysvector_minutes = sysvector_interval_seconds / 60
%
% % Normalize time
% sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
%
% % Create some basic plots
%
% % Remove zero rows from GPS
% gps = sysvector(:,33:35);
% gps(~any(gps,2), :) = [];
%
% all_data = figure('Name', 'GPS RAW');
% gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
%
%
% all_data = figure('Name', 'Complete Log Data (exc. GPS)');
% plot(sysvector(:,1), sysvector(:,2:32));
%
% actuator_inputs = figure('Name', 'Attitude controller outputs');
% plot(sysvector(:,1), sysvector(:,14:17));
% legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
%
% actuator_outputs = figure('Name', 'Actuator outputs');
% plot(sysvector(:,1), sysvector(:,18:25));
% legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
%
% end
%
% if exist('actuator_outputs0.bin', 'file')
% % Read actuators file
% myFile = java.io.File('actuator_outputs0.bin')
% fileSize = length(myFile)
%
% fid = fopen('actuator_outputs0.bin', 'r');
% elements = int64(fileSize./(16*4+8))
%
% for i=1:elements
% % timestamp
% actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% % actuators 1-16
% % quadrotor: motor 1-4 on the first four positions
% actuators(i, 2:17) = fread(fid, 16, 'float', 'ieee-le');
% end
% end
%
% if exist('actuator_controls0.bin', 'file')
% % Read actuators file
% myFile = java.io.File('actuator_controls0.bin')
% fileSize = length(myFile)
%
% fid = fopen('actuator_controls0.bin', 'r');
% elements = int64(fileSize./(8*4+8))
%
% for i=1:elements
% % timestamp
% actuator_controls(i,1) = fread(fid, 1, 'uint64', 0, 'ieee-le.l64');
% % actuators 1-16
% % quadrotor: motor 1-4 on the first four positions
% actuator_controls(i, 2:9) = fread(fid, 8, 'float', 'ieee-le');
% end
% end
%
%
% if exist('sensor_combined.bin', 'file')
% % Read sensor combined file
% % Type definition: Firmware/apps/uORB/topics/sensor_combined.h
% % Struct: sensor_combined_s
% fileInfo = dir('sensor_combined.bin');
% fileSize = fileInfo.bytes;
%
% fid = fopen('sensor_combined.bin', 'r');
%
% for i=1:elements
% % timestamp
% sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% % gyro raw
% sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le');
% % gyro counter
% sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le');
% % gyro in rad/s
% sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le');
%
% % accelerometer raw
% sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
% % padding bytes
% fread(fid, 1, 'int16', 0, 'ieee-le');
% % accelerometer counter
% sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le');
% % accel in m/s2
% sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le');
% % accel mode
% sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le');
% % accel range
% sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le');
%
% % mag raw
% sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
% % padding bytes
% fread(fid, 1, 'int16', 0, 'ieee-le');
% % mag in Gauss
% sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
% % mag mode
% sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
% % mag range
% sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
% % mag cuttoff freq
% sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
% % mag counter
% sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le');
%
% % baro pressure millibar
% % baro alt meter
% % baro temp celcius
% % battery voltage
% % adc voltage (3 channels)
% sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
% % baro counter and battery counter
% sensors(i,35:36) = fread(fid, 2, 'uint32', 0, 'ieee-le');
% % battery voltage valid flag
% sensors(i,37) = fread(fid, 1, 'uint32', 0, 'ieee-le');
%
% end
% end
ROMFS/mixers/FMU_hex_+.mix
+7
View File
@@ -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
ROMFS/mixers/FMU_hex_x.mix
+7
View File
@@ -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
ROMFS/mixers/FMU_octo_+.mix
+7
View File
@@ -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
ROMFS/mixers/FMU_octo_x.mix
+7
View File
@@ -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
ROMFS/scripts/rc.PX4IO
+22 -24
View File
@@ -1,12 +1,9 @@
#!nsh
#
# Flight startup script for PX4FMU with PX4IO carrier board.
#
echo "[init] doing PX4IO startup..."
set USB no
#
# Start the ORB
# Start the object request broker
#
uorb start
@@ -20,15 +17,27 @@ then
param load
fi
#
# Enable / connect to PX4IO
#
px4io start
#
# Load an appropriate mixer. FMU_pass.mix is a passthru mixer
# which is good for testing. See ROMFS/mixers for a full list of mixers.
#
mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix
#
# Start the sensors.
#
sh /etc/init.d/rc.sensors
#
# Start MAVLink
# Start MAVLink on UART1 (dev/ttyS0) at 57600 baud (CLI is now unusable)
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
# Start the commander.
@@ -41,35 +50,24 @@ commander start
attitude_estimator_ekf start
#
# Configure PX4FMU for operation with PX4IO
# Start the attitude and position controller
#
# XXX arguments?
#
px4fmu start
fixedwing_att_control start
fixedwing_pos_control start
#
# Start the fixed-wing controller
#
fixedwing_control start
#
# Fire up the PX4IO interface.
#
px4io start
#
# Start looking for a GPS.
# Start GPS capture. Comment this out if you do not have a GPS.
#
gps start
#
# Start logging to microSD if we can
#
#
sh /etc/init.d/rc.logging
#
# startup is done; we don't want the shell because we
# use the same UART for telemetry (dumb).
# use the same UART for telemetry
#
echo "[init] startup done, exiting."
echo "[init] startup done"
exit
ROMFS/scripts/rc.PX4IOAR
+19 -15
View File
@@ -3,8 +3,12 @@
# 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..."
#
@@ -13,26 +17,26 @@ echo "[init] doing PX4IOAR startup..."
uorb start
#
# Init the EEPROM
# Load microSD params
#
echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
then
param load
param load /fs/microsd/parameters
fi
#
# Start the sensors.
#
sh /etc/init.d/rc.sensors
#
# 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.
#
@@ -56,13 +60,13 @@ multirotor_att_control start
#
# Fire up the AR.Drone interface.
#
ardrone_interface start
ardrone_interface start -d /dev/ttyS1
#
# Start logging to microSD if we can
# Start logging
#
#sh /etc/init.d/rc.logging
#sdlog start
#
# Start GPS capture
#
ROMFS/scripts/rc.sensors
+1
View File
@@ -8,6 +8,7 @@
#
ms5611 start
adc start
if mpu6000 start
then
ROMFS/scripts/rcS
+16 -2
View File
@@ -46,8 +46,12 @@ if [ -f /fs/microsd/etc/rc ]
then
echo "[init] reading /fs/microsd/etc/rc"
sh /fs/microsd/etc/rc
else
echo "[init] script /fs/microsd/etc/rc not present"
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
#
@@ -65,6 +69,16 @@ else
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
#
# If we are still in flight mode, work out what airframe
# configuration we have and start up accordingly.
Tools/fix_code_style_ubuntu.sh
Executable
+19
View File
@@ -0,0 +1,19 @@
#!/bin/sh
astyle \
--style=linux \
--indent=force-tab=8 \
--indent-cases \
--indent-preprocessor \
--break-blocks=all \
--pad-oper \
--pad-header \
--unpad-paren \
--keep-one-line-blocks \
--keep-one-line-statements \
--align-pointer=name \
--suffix=none \
--lineend=linux \
$*
#--ignore-exclude-errors-x \
#--exclude=EASTL \
#--align-reference=name \
Tools/mavlink_px4.py
+5268
View File
File diff suppressed because it is too large Load Diff
Tools/px_uploader.py
+1 -1
View File
@@ -330,7 +330,7 @@ class uploader(object):
def upload(self, fw):
# Make sure we are doing the right thing
if self.board_type != fw.property('board_id'):
raise RuntimeError("Firmware not suitable for this board")
raise RuntimeError("Firmware not suitable for this board (run 'make configure_px4fmu && make clean' or 'make configure_px4io && make clean' to reconfigure).")
if self.fw_maxsize < fw.property('image_size'):
raise RuntimeError("Firmware image is too large for this board")
apps/ChangeLog.txt
+126 -2
View File
@@ -33,7 +33,7 @@
6.3 2011-05-15 Gregory Nutt <gnutt@nuttx.org>
* apps/interpreter: Add a directory to hold interpreters. The Pascal add-
* apps/interpreter: Add a directory to hold interpreters. The Pascal add-
on module now installs and builds under this directory.
* apps/interpreter/ficl: Added logic to build Ficl (the "Forth Inspired
Command Language"). See http://ficl.sourceforge.net/.
@@ -349,7 +349,7 @@
* apps/NxWidgets/Kconfig: Add option to turn on the memory monitor
feature of the NxWidgets/NxWM unit tests.
6.23 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
6.23 2012-11-05 Gregory Nutt <gnutt@nuttx.org>
* vsn: Moved all NSH commands from vsn/ to system/. Deleted the vsn/
directory.
@@ -368,3 +368,127 @@
recent check-ins (Darcy Gong).
* apps/netutils/webclient/webclient.c: Fix another but that I introduced
when I was trying to add correct handling for loss of connection (Darcy Gong)
* apps/nshlib/nsh_telnetd.c: Add support for login to Telnet session via
username and password (Darcy Gong).
* apps/netutils/resolv/resolv.c (and files using the DNS resolver): Various
DNS address resolution improvements from Darcy Gong.
* apps/nshlib/nsh_netcmds.c: The ping command now passes a maximum round
trip time to uip_icmpping(). This allows pinging of hosts on complex
networks where the ICMP ECHO round trip time may exceed the ping interval.
* apps/examples/nxtext/nxtext_main.c: Fix bad conditional compilation
when CONFIG_NX_KBD is not defined. Submitted by Petteri Aimonen.
* apps/examples/nximage/nximage_main.c: Add a 5 second delay after the
NX logo is presented so that there is time for the image to be verified.
Suggested by Petteri Aimonen.
* apps/Makefile: Small change that reduces the number of shell invocations
by one (Mike Smith).
* apps/examples/elf: Test example for the ELF loader.
* apps/examples/elf: The ELF module test example appears fully functional.
* apps/netutils/json: Add a snapshot of the cJSON project. Contributed by
Darcy Gong.
* apps/examples/json: Test example for cJSON from Darcy Gong
* apps/nshlib/nsh_netinit.c: Fix static IP DNS problem (Darcy Gong)
* apps/netutils/resolv/resolv.c: DNS fixes from Darcy Gong.
* COPYING: Licensing information added.
* apps/netutils/codec and include/netutils/urldecode.h, base64.h, and md5.h:
A port of the BASE46, MD5 and URL CODEC library from Darcy Gong.
* nsnlib/nsh_codeccmd.c: NSH commands to use the CODEC library.
Contributed by Darcy Gong.
* apps/examples/wgetjson: Test example contributed by Darcy Gong
* apps/examples/cxxtest: A test for the uClibc++ library provided by
Qiang Yu and the RGMP team.
* apps/netutils/webclient, apps/netutils.codes, and apps/examples/wgetjson:
Add support for wget POST interface. Contributed by Darcy Gong.
* apps/examples/relays: A relay example contributed by Darcy Gong.
* apps/nshlib/nsh_netcmds: Add ifup and ifdown commands (from Darcy
Gong).
* apps/nshlib/nsh_netcmds: Extend the ifconfig command so that it
supports setting IP addresses, network masks, name server addresses,
and hardware address (from Darcy Gong).
6.24 2012-12-20 Gregory Nutt <gnutt@nuttx.org>
* apps/examples/ostest/roundrobin.c: Replace large tables with
algorithmic prime number generation. This allows the roundrobin
test to run on platforms with minimal SRAM (Freddie Chopin).
* apps/nshlib/nsh_dbgcmds.c: Add hexdump command to dump the contents
of a file (or character device) to the console Contributed by Petteri
Aimonen.
* apps/examples/modbus: Fixes from Freddie Chopin
* apps/examples/modbus/Kconfig: Kconfig logic for FreeModBus contributed
by Freddie Chopin.
* Makefile, */Makefile: Various fixes for Windows native build. Now uses
make foreach loops instead of shell loops.
* apps/examples/elf/test/*/Makefile: OSX doesn't support install -D, use
mkdir -p then install without the -D. From Mike Smith.
* apps/examples/relays/Makefile: Reduced stack requirement (Darcy Gong).
* apps/nshlib and apps/netutils/dhcpc: Extend the NSH ifconfig command plus
various DHCPC improvements(Darcy Gong).
* apps/nshlib/nsh_apps.c: Fix compilation errors when CONFIG_NSH_DISABLEBG=y.
From Freddie Chopin.
* Rename CONFIG_PCODE and CONFIG_FICL as CONFIG_INTERPRETERS_PCODE and
CONFIG_INTERPRETERS_FICL for consistency with other configuration naming.
* apps/examples/keypadtest: A keypad test example contributed by Denis
Carikli.
* apps/examples/elf and nxflat: If CONFIG_BINFMT_EXEPATH is defined, these
tests will now use a relative path to the program and expect the binfmt/
logic to find the absolute path to the program using the PATH variable.
6.25 2013-xx-xx Gregory Nutt <gnutt@nuttx.org>
* Makefiles: Removed dependency of distclean on clean in most top-level
files. It makes sense for 'leaf' Makefiles to have this dependency,
but it does not make sense for upper-level Makefiles.
* apps/namedapp/: Renamed to builtins in preparation for another change.
* .context: Removed the .context kludge. This caused lots of problems
when changing configurations because there is no easy way to get the
system to rebuild the context. Now, the context will be rebuilt
whenever there is a change in either .config or the Makefile.
* apps/builtin/registry: Updated new built-in registration logic to handle
cases where (1) old apps/.config is used, and (2) applications ared
removed, not just added.
* apps/examples/nettest/Makefile: Fix an error that crept in during
some of the recent, massive build system changes.
* apps/builtin/Makefile: Need to have auto-generated header files
in place early in the dependency generation phase to avoid warnings.
It is not important if they are only stubbed out header files at
this build phase.
* apps/examples/hidbkd: Now supports decoding of encoded special keys
if CONFIG_EXAMPLES_HIDKBD_ENCODED is defined.
* apps/examples/hidbkd: Add support for decoding key release events
as well. However, the USB HID keyboard drier has not yet been
updated to detect key release events. That is kind of tricky in
the USB HID keyboard report data.
* apps/examples/wlan: Remove non-functional example.
* apps/examples/ostest/vfork.c: Added a test of vfork().
* apps/exampes/posix_spawn: Added a test of poxis_spawn().
* apps/examples/ostest: Extend signal handler test to catch
death-of-child signals (SIGCHLD).
* apps/examples/ostest/waitpid.c: Add a test for waitpid(), waitid(),
and wait().
* builtin/binfs.c: Add hooks for dup() method (not implemented).
* builtin/exec_builtin.c, nshlib/nsh_parse.c, and nshlib/nsh_builtin.c:
NSH now supports re-direction of I/O to files (but still not from).
* builtin/binfs.c: Greatly simplified (it is going to need to be
very lightweight). Now supports open, close, and a new ioctl to recover
the builtin filename. The latter will be needed to support a binfs
binfmt.
* builtin/binfs.c: Move apps/builtin/binfs.c to fs/binfs/fs_binfs.c
CONFIG_APPS_BINDIR rename CONFIG_FS_BINFS
* apps/include/builtin.h: Some of the content of
apps/include/apps.h moved to include/nuttx/binfmt/builtin.h.
apps/include/apps.h renamed builtin.h
* apps/builtin/exec_builtins.c: Move utility builtin
utility functions from apps/builtin/exec_builtins.c to
binfmt/libbuiltin/libbuiltin_utils.c
* apps/nshlib/nsh_mountcmds.c: The block driver/source
argument is now optional. Many files systems do not need
a source and it is really stupid to have to enter a bogus
source parameter.
* apps/nshlib/nsh_fileapp.c: Add the ability to execute a file
from a file system using posix_spawn().
* apps/builtin/: Extensions from Mike Smith.
* apps/examples/ftpd/Makefile: Name ftpd_start is not the name of
the entrypoint. Should be ftpd_main (from Yan T.)
* apps/netutils/telnetd/telnetd_driver: Was stuck in a loop if
recv[from]() ever returned a value <= 0.
apps/Kconfig
+2 -2
View File
@@ -3,8 +3,8 @@
# see misc/tools/kconfig-language.txt.
#
menu "Named Applications"
source "$APPSDIR/namedapp/Kconfig"
menu "Built-In Applications"
source "$APPSDIR/builtin/Kconfig"
endmenu
menu "Examples"
apps/Make.defs
+12 -3
View File
@@ -34,8 +34,17 @@
#
############################################################################
BUILTIN_REGISTRY = $(APPDIR)$(DELIM)builtin$(DELIM)registry
ifeq ($(CONFIG_NUTTX_NEWCONFIG),y)
DEPCONFIG = $(TOPDIR)$(DELIM).config
else
DEPCONFIG = $(TOPDIR)$(DELIM).config $(APPDIR)$(DELIM).config
endif
define REGISTER
@echo "Register: $1"
@echo "{ \"$1\", $2, $3, $4 }," >> "$(APPDIR)/namedapp/namedapp_list.h"
@echo "EXTERN int $4(int argc, char *argv[]);" >> "$(APPDIR)/namedapp/namedapp_proto.h"
$(Q) echo "Register: $1"
$(Q) echo "{ \"$1\", $2, $3, $4 }," > "$(BUILTIN_REGISTRY)$(DELIM)$4.bdat"
$(Q) echo "int $4(int argc, char *argv[]);" > "$(BUILTIN_REGISTRY)$(DELIM)$4.pdat"
$(Q) touch "$(BUILTIN_REGISTRY)$(DELIM).updated"
endef
apps/Makefile
+70 -39
View File
@@ -45,14 +45,14 @@ APPDIR = ${shell pwd}
# action. It is created by the configured appconfig file (a copy of which
# appears in this directory as .config)
# SUBDIRS is the list of all directories containing Makefiles. It is used
# only for cleaning. namedapp must always be the first in the list. This
# only for cleaning. builtin must always be the first in the list. This
# list can be extended by the .config file as well.
CONFIGURED_APPS =
#SUBDIRS = examples graphics interpreters modbus namedapp nshlib netutils system
ALL_SUBDIRS = $(dir $(shell /usr/bin/find . -name Makefile))
SUBDIRS = namedapp/ $(filter-out ./ ./namedapp/ ./examples/,$(ALL_SUBDIRS))
SUBDIRS = examples interpreters builtin nshlib system
#SUBDIRS = examples graphics interpreters modbus builtin nshlib netutils system
# There are two different mechanisms for obtaining the list of configured
# directories:
@@ -73,20 +73,20 @@ SUBDIRS = namedapp/ $(filter-out ./ ./namedapp/ ./examples/,$(ALL_SUBDIRS))
ifeq ($(CONFIG_NUTTX_NEWCONFIG),y)
# namedapp/Make.defs must be included first
# builtin/Make.defs must be included first
-include namedapp/Make.defs
-include examples/Make.defs
-include graphics/Make.defs
-include interpreters/Make.defs
-include modbus/Make.defs
-include netutils/Make.defs
-include nshlib/Make.defs
-include system/Make.defs
include builtin/Make.defs
include examples/Make.defs
include graphics/Make.defs
include interpreters/Make.defs
include modbus/Make.defs
include netutils/Make.defs
include nshlib/Make.defs
include system/Make.defs
# INSTALLED_APPS is the list of currently available application directories. It
# is the same as CONFIGURED_APPS, but filtered to exclude any non-existent
# application directory. namedapp is always in the list of applications to be
# application directory. builtin is always in the list of applications to be
# built.
INSTALLED_APPS =
@@ -98,27 +98,32 @@ else
# INSTALLED_APPS is the list of currently available application directories. It
# is the same as CONFIGURED_APPS, but filtered to exclude any non-existent
# application directory. namedapp is always in the list of applications to be
# application directory. builtin is always in the list of applications to be
# built.
INSTALLED_APPS = namedapp
INSTALLED_APPS = builtin
endif
# Create the list of available applications (INSTALLED_APPS)
define ADD_BUILTIN
INSTALLED_APPS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi}
INSTALLED_APPS += $(if $(wildcard $1$(DELIM)Makefile),$1,)
endef
$(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN))))
# EXTERNAL_APPS is used to add out of tree apps to the build
INSTALLED_APPS += $(EXTERNAL_APPS)
# The external/ directory may also be added to the INSTALLED_APPS. But there
# is no external/ directory in the repository. Rather, this directory may be
# provided by the user (possibly as a symbolic link) to add libraries and
# applications to the standard build from the repository.
INSTALLED_APPS += ${shell if [ -r external/Makefile ]; then echo "external"; fi}
SUBDIRS += ${shell if [ -r external/Makefile ]; then echo "external"; fi}
EXTERNAL_DIR := $(dir $(wildcard external$(DELIM)Makefile))
INSTALLED_APPS += $(EXTERNAL_DIR)
SUBDIRS += $(EXTERNAL_DIR)
# The final build target
@@ -130,48 +135,74 @@ all: $(BIN)
.PHONY: $(INSTALLED_APPS) context depend clean distclean
$(INSTALLED_APPS):
@$(MAKE) -C $@ TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)";
$(Q) $(MAKE) -C $@ TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
$(BIN): $(INSTALLED_APPS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $@, $${obj}); \
done ; )
.context:
@for dir in $(INSTALLED_APPS) ; do \
rm -f $$dir/.context ; \
$(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context ; \
context:
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
$(Q) for %%G in ($(INSTALLED_APPS)) do ( \
$(MAKE) -C %%G TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context \
)
else
$(Q) for dir in $(INSTALLED_APPS) ; do \
$(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context ; \
done
@touch $@
context: .context
endif
.depend: context Makefile $(SRCS)
@for dir in $(INSTALLED_APPS) ; do \
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
$(Q) for %%G in ($(INSTALLED_APPS)) do ( \
if exist %%G\.depend del /f /q %%G\.depend \
$(MAKE) -C %%G TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend \
)
else
$(Q) for dir in $(INSTALLED_APPS) ; do \
rm -f $$dir/.depend ; \
$(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend ; \
$(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend ; \
done
@touch $@
endif
$(Q) touch $@
depend: .depend
clean:
@for dir in $(SUBDIRS) ; do \
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
$(Q) for %%G in ($(SUBDIRS)) do ( \
$(MAKE) -C %%G clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" \
)
else
$(Q) for dir in $(SUBDIRS) ; do \
$(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
@rm -f $(BIN) *~ .*.swp *.o
endif
$(call DELFILE, $(BIN))
$(call CLEAN)
distclean: # clean
@for dir in $(SUBDIRS) ; do \
distclean:
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
$(Q) for %%G in ($(SUBDIRS)) do ( \
$(MAKE) -C %%G distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" \
)
$(call DELFILE, .config)
$(call DELFILE, .depend)
$(Q) ( if exist external ( \
echo ********************************************************" \
echo * The external directory/link must be removed manually *" \
echo ********************************************************" \
)
else
$(Q) for dir in $(SUBDIRS) ; do \
$(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
@rm -f .config .context .depend
@( if [ -e external ]; then \
$(call DELFILE, .config)
$(call DELFILE, .depend)
$(Q) ( if [ -e external ]; then \
echo "********************************************************"; \
echo "* The external directory/link must be removed manually *"; \
echo "********************************************************"; \
fi; \
)
endif
apps/README.txt
+104 -42
View File
@@ -6,26 +6,25 @@ Contents
General
Directory Location
Named Applications
Named Startup main() function
Built-In Applications
NuttShell (NSH) Built-In Commands
Synchronous Built-In Commands
Application Configuration File
Example Named Application
Example Built-In Application
Building NuttX with Board-Specific Pieces Outside the Source Tree
General
-------
This folder provides various applications found in sub-directories. These
applications are not inherently a part of NuttX but are provided you help
applications are not inherently a part of NuttX but are provided to help
you develop your own applications. The apps/ directory is a "break away"
part of the configuration that you may chose to use or not.
part of the configuration that you may choose to use or not.
Directory Location
------------------
The default application directory used by the NuttX build should be named
apps/ (or apps-x.y/ where x.y is the NuttX version number). This apps/
directoy should appear in the directory tree at the same level as the
directory should appear in the directory tree at the same level as the
NuttX directory. Like:
.
@@ -47,14 +46,14 @@ ways to do that:
path to the application directory on the configuration command line
like: ./configure.sh -a <app-dir> <board-name>/<config-name>
Named Applications
------------------
Built-In Applications
---------------------
NuttX also supports applications that can be started using a name string.
In this case, application entry points with their requirements are gathered
together in two files:
- namedapp/namedapp_proto.h Entry points, prototype function
- namedapp/namedapp_list.h Application specific information and requirements
- builtin/builtin_proto.h Entry points, prototype function
- builtin/builtin_list.h Application specific information and requirements
The build occurs in several phases as different build targets are executed:
(1) context, (2) depend, and (3) default (all). Application information is
@@ -62,18 +61,18 @@ collected during the make context build phase.
To execute an application function:
exec_namedapp() is defined in the nuttx/include/apps/apps.h
exec_builtin() is defined in the nuttx/include/apps/builtin.h
NuttShell (NSH) Built-In Commands
---------------------------------
One use of named applications is to provide a way of invoking your custom
One use of builtin applications is to provide a way of invoking your custom
application through the NuttShell (NSH) command line. NSH will support
a seamless method invoking the applications, when the following option is
enabled in the NuttX configuration file:
CONFIG_NSH_BUILTIN_APPS=y
Applications registered in the apps/namedapp/namedapp_list.h file will then
Applications registered in the apps/builtin/builtin_list.h file will then
be accessible from the NSH command line. If you type 'help' at the NSH
prompt, you will see a list of the registered commands.
@@ -96,11 +95,11 @@ after the NSH command.
Application Configuration File
------------------------------
A special configuration file is used to configure which applications
are to be included in the build. The source for this file is
configs/<board>/<configuration>/appconfig. The existence of the appconfig
file in the board configuration directory is sufficient to enable building
of applications.
The old-style NuttX configuration uses a special configuration file is
used to configure which applications are to be included in the build.
The source for this file is configs/<board>/<configuration>/appconfig.
The existence of the appconfig file in the board configuration directory\
is sufficient to enable building of applications.
The appconfig file is copied into the apps/ directory as .config when
NuttX is configured. .config is included in the toplevel apps/Makefile.
@@ -109,38 +108,101 @@ CONFIGURED_APPS list like:
CONFIGURED_APPS += examples/hello system/poweroff
Named Start-Up main() function
------------------------------
A named application can even be used as the main, start-up entry point
into your embedded software. When the user defines this option in
the NuttX configuration file:
The new NuttX configuration uses kconfig-frontends tools and only the
NuttX .config file. The new configuration is indicated by the existence
of the definition CONFIG_NUTTX_NEWCONFIG=y in the NuttX .config file.
If CONFIG_NUTTX_NEWCONFIG is defined, then the Makefile will:
CONFIG_BUILTIN_APP_START=<application name>
that application shall be invoked immediately after system starts
*instead* of the default "user_start" entry point.
Note that <application name> must be provided as: "hello",
will call:
- Assume that there is no apps/.config file and will instead
- Include Make.defs files from each of the subdirectories.
int hello_main(int argc, char *argv[])
When an application is enabled using the kconfig-frontends tool, then
a new definition is added to the NuttX .config file. For example, if
you want to enable apps/examples/hello then the old apps/.config would
have had:
Example Named Application
-------------------------
CONFIGURED_APPS += examples/hello
But in the new configuration there will be no apps/.config file and,
instead, the NuttX .config will have:
CONFIG_EXAMPLES_HELLO=y
This will select the apps/examples/hello in the following way:
- The top-level make will include examples/Make.defs
- examples/Make.defs will set CONFIGURED_APPS += examples/hello
like this:
ifeq ($(CONFIG_EXAMPLES_HELLO),y)
CONFIGURED_APPS += examples/hello
endif
Thus accomplishing the same thing with no apps/.config file.
Example Built-In Application
----------------------------
An example application skeleton can be found under the examples/hello
sub-directory. This example shows how a named application can be added
sub-directory. This example shows how a builtin application can be added
to the project. One must define:
1. create sub-directory as: appname
2. provide entry point: appname_main()
3. set the requirements in the file: Makefile, specially the lines:
Old configuration method:
APPNAME = appname
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 768
ASRCS = asm source file list as a.asm b.asm ...
CSRCS = C source file list as foo1.c foo2.c ..
1. Create sub-directory as: appname
4. add application in the apps/.config
2. In this directory there should be:
- A Makefile, and
- The application source code.
3. The application source code should provide the entry point:
appname_main()
4. Set the requirements in the file: Makefile, specially the lines:
APPNAME = appname
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 768
ASRCS = asm source file list as a.asm b.asm ...
CSRCS = C source file list as foo1.c foo2.c ..
Look at some of the other Makefiles for examples. Note the
special registration logic needed for the context: target
5. Add the to the application to the CONFIGIURED_APPS in the
apps/.config file:
CONFIGURED_APPS += appname
New Configuration Method:
1. Create sub-directory as: appname
2. In this directory there should be:
- A Make.defs file that would be included by the apps/Makefile
- A Kconfig file that would be used by the configuration tool (see
misc/tools/kconfig-language.txt). This Kconfig file should be
included by the apps/Kconfig file
- A Makefile, and
- The application source code.
3. The application source code should provide the entry point:
appname_main()
4. Set the requirements in the file: Makefile, specially the lines:
APPNAME = appname
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 768
ASRCS = asm source file list as a.asm b.asm ...
CSRCS = C source file list as foo1.c foo2.c ..
4b. The Make.defs file should include a line like:
ifeq ($(CONFIG_APPNAME),y)
CONFIGURED_APPS += appname
endif
Building NuttX with Board-Specific Pieces Outside the Source Tree
-----------------------------------------------------------------
apps/ardrone_interface/ardrone_motor_control.c
+28 -4
View File
@@ -45,6 +45,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <systemlib/err.h>
#include "ardrone_motor_control.h"
@@ -385,6 +386,11 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
float yaw_factor = 1.0f;
static bool initialized = false;
/* publish effective outputs */
static struct actuator_controls_effective_s actuator_controls_effective;
static orb_advert_t actuator_controls_effective_pub;
if (motor_thrust <= min_thrust) {
motor_thrust = min_thrust;
output_band = 0.0f;
@@ -417,17 +423,18 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
&& motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
yaw_factor = 0.5f;
yaw_control *= yaw_factor;
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor);
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor);
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor);
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor);
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
}
for (int i = 0; i < 4; i++) {
@@ -441,6 +448,23 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
}
}
/* publish effective outputs */
actuator_controls_effective.control_effective[0] = roll_control;
actuator_controls_effective.control_effective[1] = pitch_control;
/* yaw output after limiting */
actuator_controls_effective.control_effective[2] = yaw_control;
/* possible motor thrust limiting */
actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
if (!initialized) {
/* advertise and publish */
actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
initialized = true;
} else {
/* already initialized, just publishing */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
}
/* set the motor values */
/* scale up from 0..1 to 10..512) */
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+45 -33
View File
@@ -35,7 +35,7 @@
/*
* @file attitude_estimator_ekf_main.c
*
*
* Extended Kalman Filter for Attitude Estimation.
*/
@@ -79,18 +79,18 @@ static float dt = 0.005f;
static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
static float x_aposteriori_k[12]; /**< states */
static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
static float x_aposteriori[12];
static float P_aposteriori[144];
@@ -99,9 +99,9 @@ static float P_aposteriori[144];
static float euler[3] = {0.0f, 0.0f, 0.0f};
static float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
static bool thread_should_exit = false; /**< Deamon exit flag */
@@ -123,6 +123,7 @@ usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
@@ -131,7 +132,7 @@ usage(const char *reason)
* The attitude_estimator_ekf app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
*
* The actual stack size should be set in the call
* to task_create().
*/
@@ -150,11 +151,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
thread_should_exit = false;
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12000,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12000,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
@@ -166,9 +167,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tattitude_estimator_ekf app is running\n");
} else {
printf("\tattitude_estimator_ekf app not started\n");
}
exit(0);
}
@@ -235,7 +238,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* advertise debug value */
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
@@ -263,8 +266,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
while (!thread_should_exit) {
struct pollfd fds[2] = {
{ .fd = sub_raw, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN }
{ .fd = sub_raw, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN }
};
int ret = poll(fds, 2, 1000);
@@ -273,10 +276,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
if (!state.flag_hil_enabled) {
fprintf(stderr,
fprintf(stderr,
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
}
} else {
/* only update parameters if they changed */
@@ -308,6 +313,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
}
} else {
perf_begin(ekf_loop_perf);
@@ -336,6 +342,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
}
z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_m_s2[2];
@@ -347,6 +354,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
}
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
@@ -368,8 +376,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.005f)
{
if (!const_initialized && dt < 0.05f && dt > 0.005f) {
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
@@ -395,13 +402,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
}
uint64_t timing_start = hrt_absolute_time();
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
} else {
/* due to inputs or numerical failure the output is invalid, skip it */
continue;
@@ -413,9 +422,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* send out */
att.timestamp = raw.timestamp;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2];
// XXX Apply the same transformation to the rotation matrix
att.roll = euler[0] - ekf_params.roll_off;
att.pitch = euler[1] - ekf_params.pitch_off;
att.yaw = euler[2] - ekf_params.yaw_off;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
@@ -431,6 +442,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
} else {
warnx("NaN in roll/pitch/yaw estimate!");
}
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+16 -4
View File
@@ -35,7 +35,7 @@
/*
* @file attitude_estimator_ekf_params.c
*
*
* Parameters for EKF filter
*/
@@ -65,13 +65,17 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f);
PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f);
PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f);
/* accelerometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f);
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f);
PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f);
/* magnetometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f);
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
{
@@ -99,6 +103,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->r7 = param_find("EKF_ATT_R7");
h->r8 = param_find("EKF_ATT_R8");
h->roll_off = param_find("ATT_ROLL_OFFS");
h->pitch_off = param_find("ATT_PITCH_OFFS");
h->yaw_off = param_find("ATT_YAW_OFFS");
return OK;
}
@@ -127,5 +135,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->r7, &(p->r[7]));
param_get(h->r8, &(p->r[8]));
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+5 -1
View File
@@ -35,7 +35,7 @@
/*
* @file attitude_estimator_ekf_params.h
*
*
* Parameters for EKF filter
*/
@@ -44,11 +44,15 @@
struct attitude_estimator_ekf_params {
float r[9];
float q[12];
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3, r4, r5, r6, r7, r8;
param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11;
param_t roll_off, pitch_off, yaw_off;
};
/**
apps/builtin/Kconfig
+17
View File
@@ -0,0 +1,17 @@
#
# For a description of the syntax of this configuration file,
# see misc/tools/kconfig-language.txt.
#
if BUILTIN
config BUILTIN_PROXY_STACKSIZE
int "Builtin Proxy Stack Size"
default 1024
---help---
If exec_builtin uses I/O redirection options, then it will require
an intermediary/proxy task to muck with the file descriptors. This
configuration item specifies the stack size used for the proxy. Default:
1024 bytes.
endif
apps/namedapp/Make.defs → apps/builtin/Make.defs
+3 -3
View File
@@ -1,5 +1,5 @@
############################################################################
# apps/namedapps/Make.defs
# apps/builtin/Make.defs
# Adds selected applications to apps/ build
#
# Copyright (C) 2012 Gregory Nutt. All rights reserved.
@@ -34,7 +34,7 @@
#
############################################################################
ifeq ($(CONFIG_NAMEDAPP),y)
CONFIGURED_APPS += namedapp
ifeq ($(CONFIG_BUILTIN),y)
CONFIGURED_APPS += builtin
endif
apps/namedapp/Makefile → apps/builtin/Makefile
+56 -32
View File
@@ -1,7 +1,7 @@
############################################################################
# apps/nshlib/Makefile
# apps/builtin/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@@ -33,20 +33,13 @@
#
############################################################################
-include $(TOPDIR)/.config
-include $(TOPDIR)/Make.defs
include $(APPDIR)/Make.defs
# NSH Library
# Source and object files
ASRCS =
CSRCS = namedapp.c exec_namedapp.c
ifeq ($(CONFIG_APPS_BINDIR),y)
CSRCS += binfs.c
endif
CSRCS = builtin.c builtin_list.c exec_builtin.c
AOBJS = $(ASRCS:.S=$(OBJEXT))
COBJS = $(CSRCS:.c=$(OBJEXT))
@@ -54,10 +47,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\libapps$(LIBEXT)
else
BIN = ../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@@ -66,7 +63,7 @@ VPATH =
# Build Targets
all: .built
.PHONY: .context context depend clean distclean
.PHONY: context depend clean distclean
$(AOBJS): %$(OBJEXT): %.S
$(call ASSEMBLE, $<, $@)
@@ -74,33 +71,60 @@ $(AOBJS): %$(OBJEXT): %.S
$(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
@touch .built
registry$(DELIM).updated:
$(V) $(MAKE) -C registry .updated TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
.context:
@echo "/* List of application requirements, generated during make context. */" > namedapp_list.h
@echo "/* List of application entry points, generated during make context. */" > namedapp_proto.h
@touch $@
builtin_list.h: registry$(DELIM).updated
$(call DELFILE, builtin_list.h)
$(Q) touch builtin_list.h
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
$(Q) for /f %%G in ('dir /b registry\*.bdat`) do ( type registry\%%G >> builtin_list.h )
else
$(Q) ( \
filelist=`ls registry/*.bdat 2>/dev/null || echo ""`; \
for file in $$filelist; \
do cat $$file >> builtin_list.h; \
done; \
)
endif
context: .context
builtin_proto.h: registry$(DELIM).updated
$(call DELFILE, builtin_proto.h)
$(Q) touch builtin_proto.h
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
$(Q) for /f %%G in ('dir /b registry\*.pdat`) do ( type registry\%%G >> builtin_proto.h )
else
$(Q) ( \
filelist=`ls registry/*.pdat 2>/dev/null || echo ""`; \
for file in $$filelist; \
do cat $$file >> builtin_proto.h; \
done; \
)
endif
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
.built: builtin_list.h builtin_proto.h $(OBJS)
$(call ARCHIVE, $(BIN), $(OBJS))
$(Q) touch .built
context:
$(Q) $(MAKE) -C registry context TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
.depend: Makefile $(SRCS) builtin_list.h builtin_proto.h
$(Q) $(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
$(Q) touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(Q) $(MAKE) -C registry clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f .context Make.dep .depend
@rm -f namedapp_list.h
@rm -f namedapp_proto.h
$(Q) $(MAKE) -C registry distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
$(call DELFILE, builtin_list.h)
$(call DELFILE, builtin_proto.h)
-include Make.dep
apps/builtin/builtin.c
+80
View File
@@ -0,0 +1,80 @@
/****************************************************************************
* apps/builtin/builtin.c
*
* Copyright (C) 2011 Uros Platise. All rights reserved.
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
* Authors: Uros Platise <uros.platise@isotel.eu>
* Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/binfmt/builtin.h>
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
extern const struct builtin_s g_builtins[];
extern const int g_builtin_count;
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
FAR const struct builtin_s *builtin_for_index(int index)
{
if (index < g_builtin_count)
{
return &g_builtins[index];
}
return NULL;
}
apps/namedapp/namedapp.c → apps/builtin/builtin_list.c
+7 -24
View File
@@ -1,5 +1,5 @@
/****************************************************************************
* apps/namedaps/namedapp.c
* apps/builtin/builtin_list.c
*
* Copyright (C) 2011 Uros Platise. All rights reserved.
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
@@ -40,7 +40,8 @@
****************************************************************************/
#include <nuttx/config.h>
#include <apps/apps.h>
#include <nuttx/binfmt/builtin.h>
/****************************************************************************
* Private Types
@@ -54,27 +55,15 @@
* Public Data
****************************************************************************/
#undef EXTERN
#if defined(__cplusplus)
#define EXTERN extern "C"
extern "C" {
#else
#define EXTERN extern
#endif
#include "builtin_proto.h"
#include "namedapp_proto.h"
const struct namedapp_s namedapps[] =
const struct builtin_s g_builtins[] =
{
# include "namedapp_list.h"
# include "builtin_list.h"
{ NULL, 0, 0, 0 }
};
#undef EXTERN
#if defined(__cplusplus)
}
#endif
const int g_builtin_count = sizeof(g_builtins) / sizeof(g_builtins[0]);
/****************************************************************************
* Private Data
@@ -88,9 +77,3 @@ const struct namedapp_s namedapps[] =
* Public Functions
****************************************************************************/
int number_namedapps(void)
{
return sizeof(namedapps)/sizeof(struct namedapp_s) - 1;
}
apps/builtin/exec_builtin.c
+467
View File
@@ -0,0 +1,467 @@
/****************************************************************************
* apps/builtin/exec_builtin.c
*
* Originally by:
*
* Copyright (C) 2011 Uros Platise. All rights reserved.
* Author: Uros Platise <uros.platise@isotel.eu>
*
* With subsequent updates, modifications, and general maintenance by:
*
* Copyright (C) 2012-2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/wait.h>
#include <sched.h>
#include <string.h>
#include <fcntl.h>
#include <semaphore.h>
#include <errno.h>
#include <debug.h>
#include <nuttx/binfmt/builtin.h>
#include <apps/builtin.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#ifndef CONFIG_BUILTIN_PROXY_STACKSIZE
# define CONFIG_BUILTIN_PROXY_STACKSIZE 1024
#endif
/****************************************************************************
* Private Types
****************************************************************************/
struct builtin_parms_s
{
/* Input values */
FAR const char *redirfile;
FAR const char **argv;
int oflags;
int index;
/* Returned values */
pid_t result;
int errcode;
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
static sem_t g_builtin_parmsem = SEM_INITIALIZER(1);
#ifndef CONFIG_SCHED_WAITPID
static sem_t g_builtin_execsem = SEM_INITIALIZER(0);
#endif
static struct builtin_parms_s g_builtin_parms;
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: bultin_semtake and builtin_semgive
*
* Description:
* Give and take semaphores
*
* Input Parameters:
*
* sem - The semaphore to act on.
*
* Returned Value:
* None
*
****************************************************************************/
static void bultin_semtake(FAR sem_t *sem)
{
int ret;
do
{
ret = sem_wait(sem);
ASSERT(ret == 0 || get_errno() == EINTR);
}
while (ret != 0);
}
#define builtin_semgive(sem) sem_post(sem)
/****************************************************************************
* Name: builtin_taskcreate
*
* Description:
* Execute the builtin task
*
* Returned Value:
* On success, the task ID of the builtin task is returned; On failure, -1
* (ERROR) is returned and the errno is set appropriately.
*
****************************************************************************/
static int builtin_taskcreate(int index, FAR const char **argv)
{
FAR const struct builtin_s *b;
int ret;
b = builtin_for_index(index);
if (b == NULL)
{
set_errno(ENOENT);
return ERROR;
}
/* Disable pre-emption. This means that although we start the builtin
* application here, it will not actually run until pre-emption is
* re-enabled below.
*/
sched_lock();
/* Start the builtin application task */
ret = TASK_CREATE(b->name, b->priority, b->stacksize, b->main,
(argv) ? &argv[1] : (FAR const char **)NULL);
/* If robin robin scheduling is enabled, then set the scheduling policy
* of the new task to SCHED_RR before it has a chance to run.
*/
#if CONFIG_RR_INTERVAL > 0
if (ret > 0)
{
struct sched_param param;
/* Pre-emption is disabled so the task creation and the
* following operation will be atomic. The priority of the
* new task cannot yet have changed from its initial value.
*/
param.sched_priority = b->priority;
(void)sched_setscheduler(ret, SCHED_RR, &param);
}
#endif
/* Now let the builtin application run */
sched_unlock();
/* Return the task ID of the new task if the task was sucessfully
* started. Otherwise, ret will be ERROR (and the errno value will
* be set appropriately).
*/
return ret;
}
/****************************************************************************
* Name: builtin_proxy
*
* Description:
* Perform output redirection, then execute the builtin task.
*
* Input Parameters:
* Standard task start-up parameters
*
* Returned Value:
* Standard task return value.
*
****************************************************************************/
static int builtin_proxy(int argc, char *argv[])
{
int fd;
int ret = ERROR;
/* Open the output file for redirection */
svdbg("Open'ing redirfile=%s oflags=%04x mode=0644\n",
g_builtin_parms.redirfile, g_builtin_parms.oflags);
fd = open(g_builtin_parms.redirfile, g_builtin_parms.oflags, 0644);
if (fd < 0)
{
/* Remember the errno value. ret is already set to ERROR */
g_builtin_parms.errcode = get_errno();
sdbg("ERROR: open of %s failed: %d\n",
g_builtin_parms.redirfile, g_builtin_parms.errcode);
}
/* Does the return file descriptor happen to match the required file
* desciptor number?
*/
else if (fd != 1)
{
/* No.. dup2 to get the correct file number */
svdbg("Dup'ing %d->1\n", fd);
ret = dup2(fd, 1);
if (ret < 0)
{
g_builtin_parms.errcode = get_errno();
sdbg("ERROR: dup2 failed: %d\n", g_builtin_parms.errcode);
}
svdbg("Closing fd=%d\n", fd);
close(fd);
}
/* Was the setup successful? */
if (ret == OK)
{
/* Yes.. Start the task. On success, the task ID of the builtin task
* is returned; On failure, -1 (ERROR) is returned and the errno
* is set appropriately.
*/
ret = builtin_taskcreate(g_builtin_parms.index, g_builtin_parms.argv);
if (ret < 0)
{
g_builtin_parms.errcode = get_errno();
sdbg("ERROR: builtin_taskcreate failed: %d\n",
g_builtin_parms.errcode);
}
}
/* NOTE: There is a logical error here if CONFIG_SCHED_HAVE_PARENT is
* defined: The new task is the child of this proxy task, not the
* original caller. As a consequence, operations like waitpid() will
* fail on the caller's thread.
*/
/* Post the semaphore to inform the parent task that we have completed
* what we need to do.
*/
g_builtin_parms.result = ret;
#ifndef CONFIG_SCHED_WAITPID
builtin_semgive(&g_builtin_execsem);
#endif
return 0;
}
/****************************************************************************
* Name: builtin_startproxy
*
* Description:
* Perform output redirection, then execute the builtin task.
*
* Input Parameters:
* Standard task start-up parameters
*
* Returned Value:
* On success, the task ID of the builtin task is returned; On failure, -1
* (ERROR) is returned and the errno is set appropriately.
*
****************************************************************************/
static inline int builtin_startproxy(int index, FAR const char **argv,
FAR const char *redirfile, int oflags)
{
struct sched_param param;
pid_t proxy;
int errcode;
#ifdef CONFIG_SCHED_WAITPID
int status;
#endif
int ret;
svdbg("index=%d argv=%p redirfile=%s oflags=%04x\n",
index, argv, redirfile, oflags);
/* We will have to go through an intermediary/proxy task in order to
* perform the I/O redirection. This would be a natural place to fork().
* However, true fork() behavior requires an MMU and most implementations
* of vfork() are not capable of these operations.
*
* Even without fork(), we can still do the job, but parameter passing is
* messier. Unfortunately, there is no (clean) way to pass binary values
* as a task parameter, so we will use a semaphore-protected global
* structure.
*/
/* Get exclusive access to the global parameter structure */
bultin_semtake(&g_builtin_parmsem);
/* Populate the parameter structure */
g_builtin_parms.redirfile = redirfile;
g_builtin_parms.argv = argv;
g_builtin_parms.result = ERROR;
g_builtin_parms.oflags = oflags;
g_builtin_parms.index = index;
/* Get the priority of this (parent) task */
ret = sched_getparam(0, &param);
if (ret < 0)
{
errcode = get_errno();
sdbg("ERROR: sched_getparam failed: %d\n", errcode);
goto errout_with_sem;
}
/* Disable pre-emption so that the proxy does not run until we waitpid
* is called. This is probably unnecessary since the builtin_proxy has
* the same priority as this thread; it should be schedule behind this
* task in the ready-to-run list.
*/
#ifdef CONFIG_SCHED_WAITPID
sched_lock();
#endif
/* Start the intermediary/proxy task at the same priority as the parent task. */
proxy = TASK_CREATE("builtin_proxy", param.sched_priority,
CONFIG_BUILTIN_PROXY_STACKSIZE, (main_t)builtin_proxy,
(FAR const char **)NULL);
if (proxy < 0)
{
errcode = get_errno();
sdbg("ERROR: Failed to start builtin_proxy: %d\n", errcode);
goto errout_with_lock;
}
/* Wait for the proxy to complete its job. We could use waitpid()
* for this.
*/
#ifdef CONFIG_SCHED_WAITPID
ret = waitpid(proxy, &status, 0);
if (ret < 0)
{
sdbg("ERROR: waitpid() failed: %d\n", get_errno());
goto errout_with_lock;
}
#else
bultin_semtake(&g_builtin_execsem);
#endif
/* Get the result and relinquish our access to the parameter structure */
set_errno(g_builtin_parms.errcode);
builtin_semgive(&g_builtin_parmsem);
return g_builtin_parms.result;
errout_with_lock:
#ifdef CONFIG_SCHED_WAITPID
sched_unlock();
#endif
errout_with_sem:
set_errno(errcode);
builtin_semgive(&g_builtin_parmsem);
return ERROR;
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: exec_builtin
*
* Description:
* Executes builtin applications registered during 'make context' time.
* New application is run in a separate task context (and thread).
*
* Input Parameter:
* filename - Name of the linked-in binary to be started.
* argv - Argument list
* redirfile - If output if redirected, this parameter will be non-NULL
* and will provide the full path to the file.
* oflags - If output is redirected, this parameter will provide the
* open flags to use. This will support file replacement
* of appending to an existing file.
*
* Returned Value:
* This is an end-user function, so it follows the normal convention:
* Returns the PID of the exec'ed module. On failure, it.returns
* -1 (ERROR) and sets errno appropriately.
*
****************************************************************************/
int exec_builtin(FAR const char *appname, FAR const char **argv,
FAR const char *redirfile, int oflags)
{
int index;
int ret = ERROR;
/* Verify that an application with this name exists */
index = builtin_isavail(appname);
if (index >= 0)
{
/* Is output being redirected? */
if (redirfile)
{
ret = builtin_startproxy(index, argv, redirfile, oflags);
}
else
{
/* Start the builtin application task */
ret = builtin_taskcreate(index, argv);
}
}
/* Return the task ID of the new task if the task was sucessfully
* started. Otherwise, ret will be ERROR (and the errno value will
* be set appropriately).
*/
return ret;
}
apps/builtin/registry/Makefile
+61
View File
@@ -0,0 +1,61 @@
############################################################################
# apps/builtin/registry/Makefile
#
# Copyright (C) 2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name NuttX nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
-include $(TOPDIR)/Make.defs
include $(APPDIR)/Make.defs
# NSH Library
all:
.PHONY: context depend clean distclean
.updated: $(DEPCONFIG)
$(call DELFILE, *.bdat)
$(call DELFILE, *.pdat)
$(Q) touch .updated
# This must run before any other context target
context: .updated
depend:
clean:
$(call CLEAN)
distclean: clean
$(call DELFILE, *.bdat)
$(call DELFILE, *.pdat)
$(call DELFILE, .updated)
apps/commander/calibration_routines.c
+143 -140
View File
@@ -45,172 +45,175 @@
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) {
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius)
{
float x_sumplain = 0.0f;
float x_sumsq = 0.0f;
float x_sumcube = 0.0f;
float x_sumplain = 0.0f;
float x_sumsq = 0.0f;
float x_sumcube = 0.0f;
float y_sumplain = 0.0f;
float y_sumsq = 0.0f;
float y_sumcube = 0.0f;
float y_sumplain = 0.0f;
float y_sumsq = 0.0f;
float y_sumcube = 0.0f;
float z_sumplain = 0.0f;
float z_sumsq = 0.0f;
float z_sumcube = 0.0f;
float z_sumplain = 0.0f;
float z_sumsq = 0.0f;
float z_sumcube = 0.0f;
float xy_sum = 0.0f;
float xz_sum = 0.0f;
float yz_sum = 0.0f;
float xy_sum = 0.0f;
float xz_sum = 0.0f;
float yz_sum = 0.0f;
float x2y_sum = 0.0f;
float x2z_sum = 0.0f;
float y2x_sum = 0.0f;
float y2z_sum = 0.0f;
float z2x_sum = 0.0f;
float z2y_sum = 0.0f;
float x2y_sum = 0.0f;
float x2z_sum = 0.0f;
float y2x_sum = 0.0f;
float y2z_sum = 0.0f;
float z2x_sum = 0.0f;
float z2y_sum = 0.0f;
for (unsigned int i = 0; i < size; i++) {
for (unsigned int i = 0; i < size; i++) {
float x2 = x[i] * x[i];
float y2 = y[i] * y[i];
float z2 = z[i] * z[i];
float x2 = x[i] * x[i];
float y2 = y[i] * y[i];
float z2 = z[i] * z[i];
x_sumplain += x[i];
x_sumsq += x2;
x_sumcube += x2 * x[i];
x_sumplain += x[i];
x_sumsq += x2;
x_sumcube += x2 * x[i];
y_sumplain += y[i];
y_sumsq += y2;
y_sumcube += y2 * y[i];
y_sumplain += y[i];
y_sumsq += y2;
y_sumcube += y2 * y[i];
z_sumplain += z[i];
z_sumsq += z2;
z_sumcube += z2 * z[i];
z_sumplain += z[i];
z_sumsq += z2;
z_sumcube += z2 * z[i];
xy_sum += x[i] * y[i];
xz_sum += x[i] * z[i];
yz_sum += y[i] * z[i];
xy_sum += x[i] * y[i];
xz_sum += x[i] * z[i];
yz_sum += y[i] * z[i];
x2y_sum += x2 * y[i];
x2z_sum += x2 * z[i];
x2y_sum += x2 * y[i];
x2z_sum += x2 * z[i];
y2x_sum += y2 * x[i];
y2z_sum += y2 * z[i];
y2x_sum += y2 * x[i];
y2z_sum += y2 * z[i];
z2x_sum += z2 * x[i];
z2y_sum += z2 * y[i];
}
z2x_sum += z2 * x[i];
z2y_sum += z2 * y[i];
}
//
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
//
// P is a structure that has been computed with the data earlier.
// P.npoints is the number of elements; the length of X,Y,Z are identical.
// P's members are logically named.
//
// X[n] is the x component of point n
// Y[n] is the y component of point n
// Z[n] is the z component of point n
//
// A is the x coordiante of the sphere
// B is the y coordiante of the sphere
// C is the z coordiante of the sphere
// Rsq is the radius squared of the sphere.
//
//This method should converge; maybe 5-100 iterations or more.
//
float x_sum = x_sumplain / size; //sum( X[n] )
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
float y_sum = y_sumplain / size; //sum( Y[n] )
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
float z_sum = z_sumplain / size; //sum( Z[n] )
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
//
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
//
// P is a structure that has been computed with the data earlier.
// P.npoints is the number of elements; the length of X,Y,Z are identical.
// P's members are logically named.
//
// X[n] is the x component of point n
// Y[n] is the y component of point n
// Z[n] is the z component of point n
//
// A is the x coordiante of the sphere
// B is the y coordiante of the sphere
// C is the z coordiante of the sphere
// Rsq is the radius squared of the sphere.
//
//This method should converge; maybe 5-100 iterations or more.
//
float x_sum = x_sumplain / size; //sum( X[n] )
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
float y_sum = y_sumplain / size; //sum( Y[n] )
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
float z_sum = z_sumplain / size; //sum( Z[n] )
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
float XY = xy_sum / size; //sum( X[n] * Y[n] )
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
float XY = xy_sum / size; //sum( X[n] * Y[n] )
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
//Reduction of multiplications
float F0 = x_sum2 + y_sum2 + z_sum2;
float F1 = 0.5f * F0;
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
//Reduction of multiplications
float F0 = x_sum2 + y_sum2 + z_sum2;
float F1 = 0.5f * F0;
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
//Set initial conditions:
float A = x_sum;
float B = y_sum;
float C = z_sum;
//Set initial conditions:
float A = x_sum;
float B = y_sum;
float C = z_sum;
//First iteration computation:
float A2 = A*A;
float B2 = B*B;
float C2 = C*C;
float QS = A2 + B2 + C2;
float QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
//First iteration computation:
float A2 = A * A;
float B2 = B * B;
float C2 = C * C;
float QS = A2 + B2 + C2;
float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
//Set initial conditions:
float Rsq = F0 + QB + QS;
//Set initial conditions:
float Rsq = F0 + QB + QS;
//First iteration computation:
float Q0 = 0.5f * (QS - Rsq);
float Q1 = F1 + Q0;
float Q2 = 8.0f * ( QS - Rsq + QB + F0 );
float aA,aB,aC,nA,nB,nC,dA,dB,dC;
//First iteration computation:
float Q0 = 0.5f * (QS - Rsq);
float Q1 = F1 + Q0;
float Q2 = 8.0f * (QS - Rsq + QB + F0);
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
//Iterate N times, ignore stop condition.
int n = 0;
while( n < max_iterations ){
n++;
//Iterate N times, ignore stop condition.
int n = 0;
//Compute denominator:
aA = Q2 + 16.0f * (A2 - 2.0f * A*x_sum + x_sum2);
aB = Q2 + 16.0f *(B2 - 2.0f * B*y_sum + y_sum2);
aC = Q2 + 16.0f *(C2 - 2.0f * C*z_sum + z_sum2);
aA = (aA == 0.0f) ? 1.0f : aA;
aB = (aB == 0.0f) ? 1.0f : aB;
aC = (aC == 0.0f) ? 1.0f : aC;
while (n < max_iterations) {
n++;
//Compute next iteration
nA = A - ((F2 + 16.0f * ( B*XY + C*XZ + x_sum*(-A2 - Q0) + A*(x_sum2 + Q1 - C*z_sum - B*y_sum) ) )/aA);
nB = B - ((F3 + 16.0f * ( A*XY + C*YZ + y_sum*(-B2 - Q0) + B*(y_sum2 + Q1 - A*x_sum - C*z_sum) ) )/aB);
nC = C - ((F4 + 16.0f * ( A*XZ + B*YZ + z_sum*(-C2 - Q0) + C*(z_sum2 + Q1 - A*x_sum - B*y_sum) ) )/aC);
//Compute denominator:
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
aA = (aA == 0.0f) ? 1.0f : aA;
aB = (aB == 0.0f) ? 1.0f : aB;
aC = (aC == 0.0f) ? 1.0f : aC;
//Check for stop condition
dA = (nA - A);
dB = (nB - B);
dC = (nC - C);
if( (dA*dA + dB*dB + dC*dC) <= delta ){ break; }
//Compute next iteration
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
//Compute next iteration's values
A = nA;
B = nB;
C = nC;
A2 = A*A;
B2 = B*B;
C2 = C*C;
QS = A2 + B2 + C2;
QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
Rsq = F0 + QB + QS;
Q0 = 0.5f * (QS - Rsq);
Q1 = F1 + Q0;
Q2 = 8.0f * ( QS - Rsq + QB + F0 );
}
//Check for stop condition
dA = (nA - A);
dB = (nB - B);
dC = (nC - C);
*sphere_x = A;
*sphere_y = B;
*sphere_z = C;
*sphere_radius = sqrtf(Rsq);
if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
return 0;
//Compute next iteration's values
A = nA;
B = nB;
C = nC;
A2 = A * A;
B2 = B * B;
C2 = C * C;
QS = A2 + B2 + C2;
QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
Rsq = F0 + QB + QS;
Q0 = 0.5f * (QS - Rsq);
Q1 = F1 + Q0;
Q2 = 8.0f * (QS - Rsq + QB + F0);
}
*sphere_x = A;
*sphere_y = B;
*sphere_z = C;
*sphere_radius = sqrtf(Rsq);
return 0;
}
apps/commander/calibration_routines.h
+1 -1
View File
@@ -58,4 +58,4 @@
* @return 0 on success, 1 on failure
*/
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
apps/commander/commander.c
+643 -277
View File
File diff suppressed because it is too large Load Diff
apps/commander/commander.h
+3
View File
@@ -52,4 +52,7 @@
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
void tune_confirm(void);
void tune_error(void);
#endif /* COMMANDER_H_ */
apps/commander/state_machine_helper.c
+158 -72
View File
@@ -50,7 +50,7 @@
#include "state_machine_helper.h"
static const char* system_state_txt[] = {
static const char *system_state_txt[] = {
"SYSTEM_STATE_PREFLIGHT",
"SYSTEM_STATE_STANDBY",
"SYSTEM_STATE_GROUND_READY",
@@ -79,7 +79,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
case SYSTEM_STATE_MISSION_ABORT: {
/* Indoor or outdoor */
// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
// } else {
// ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
@@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = true;
fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
warnx("EMERGENCY LANDING!\n");
mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
break;
case SYSTEM_STATE_EMCY_CUTOFF:
@@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = false;
fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
warnx("EMERGENCY MOTOR CUTOFF!\n");
mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
break;
case SYSTEM_STATE_GROUND_ERROR:
@@ -114,36 +114,41 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* prevent actuators from arming */
current_status->flag_system_armed = false;
fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
warnx("GROUND ERROR, locking down propulsion system\n");
mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
break;
case SYSTEM_STATE_PREFLIGHT:
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
/* set system flags according to state */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
}
break;
case SYSTEM_STATE_REBOOT:
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
|| current_status->flag_hil_enabled) {
invalid_state = false;
/* set system flags according to state */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
usleep(500000);
up_systemreset();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT");
mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
}
break;
case SYSTEM_STATE_STANDBY:
@@ -152,7 +157,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* standby enforces disarmed */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
break;
case SYSTEM_STATE_GROUND_READY:
@@ -162,7 +167,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* ground ready has motors / actuators armed */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
break;
case SYSTEM_STATE_AUTO:
@@ -172,7 +177,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* auto is airborne and in auto mode, motors armed */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
break;
case SYSTEM_STATE_STABILIZED:
@@ -180,7 +185,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
break;
case SYSTEM_STATE_MANUAL:
@@ -188,7 +193,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
break;
default:
@@ -202,14 +207,17 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
publish_armed_status(current_status);
ret = OK;
}
if (invalid_state) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
ret = ERROR;
}
return ret;
}
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
/* publish the new state */
current_status->counter++;
current_status->timestamp = hrt_absolute_time();
@@ -217,9 +225,11 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
/* assemble state vector based on flag values */
if (current_status->flag_control_rates_enabled) {
current_status->onboard_control_sensors_present |= 0x400;
} else {
current_status->onboard_control_sensors_present &= ~0x400;
}
current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
@@ -232,17 +242,15 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
}
void publish_armed_status(const struct vehicle_status_s *current_status) {
void publish_armed_status(const struct vehicle_status_s *current_status)
{
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
/* lock down actuators if required */
// XXX FIXME Currently any loss of RC will completely disable all actuators
// needs proper failsafe
armed.lockdown = ((current_status->rc_signal_lost && current_status->offboard_control_signal_lost)
|| current_status->flag_hil_enabled) ? true : false;
/* lock down actuators if required, only in HIL */
armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@@ -253,17 +261,19 @@ void publish_armed_status(const struct vehicle_status_s *current_status) {
*/
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
fprintf(stderr, "[commander] EMERGENCY HANDLER\n");
warnx("EMERGENCY HANDLER\n");
/* Depending on the current state go to one of the error states */
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
// DO NOT abort mission
//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
} else {
fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
warnx("Unknown system state: #%d\n", current_status->state_machine);
}
}
@@ -273,7 +283,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
} else {
//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
}
}
@@ -498,7 +508,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
printf("[commander] arming\n");
printf("[cmd] arming\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
}
}
@@ -506,11 +516,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
printf("[commander] going standby\n");
printf("[cmd] going standby\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] MISSION ABORT!\n");
printf("[cmd] MISSION ABORT!\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
}
}
@@ -519,54 +529,139 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->flag_control_manual_enabled = true;
/* enable attitude control per default */
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
/* set behaviour based on airframe */
if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
/* assuming a rotary wing, set to SAS */
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
} else {
/* assuming a fixed wing, set to direct pass-through */
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
current_status->flag_control_attitude_enabled = false;
current_status->flag_control_rates_enabled = false;
}
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] manual mode\n");
printf("[cmd] manual mode\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
}
}
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
int old_mode = current_status->flight_mode;
int old_manual_control_mode = current_status->manual_control_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
current_status->flag_control_manual_enabled = true;
if (old_mode != current_status->flight_mode ||
old_manual_control_mode != current_status->manual_control_mode) {
printf("[cmd] att stabilized mode\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
}
void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
if (!current_status->flag_vector_flight_mode_ok) {
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
tune_error();
return;
}
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] stabilized mode\n");
printf("[cmd] position guided mode\n");
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
current_status->flag_control_manual_enabled = false;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (!current_status->flag_vector_flight_mode_ok) {
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
return;
}
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
printf("[commander] auto mode\n");
printf("[cmd] auto mode\n");
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->flag_control_manual_enabled = false;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
{
printf("[commander] Requested new mode: %d\n", (int)mode);
uint8_t ret = 1;
/* Switch on HIL if in standby and not already in HIL mode */
if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
&& !current_status->flag_hil_enabled) {
if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
/* Enable HIL on request */
current_status->flag_hil_enabled = true;
ret = OK;
state_machine_publish(status_pub, current_status, mavlink_fd);
publish_armed_status(current_status);
printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
current_status->flag_system_armed) {
mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
} else {
mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
}
}
/* switch manual / auto */
if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
}
/* vehicle is disarmed, mode requests arming */
if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
/* only arm in standby state */
@@ -574,7 +669,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
ret = OK;
printf("[commander] arming due to command request\n");
printf("[cmd] arming due to command request\n");
}
}
@@ -584,26 +679,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
ret = OK;
printf("[commander] disarming due to command request\n");
printf("[cmd] disarming due to command request\n");
}
}
/* Switch on HIL if in standby and not already in HIL mode */
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
&& !current_status->flag_hil_enabled) {
/* Enable HIL on request */
current_status->flag_hil_enabled = true;
ret = OK;
state_machine_publish(status_pub, current_status, mavlink_fd);
publish_armed_status(current_status);
printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
} else if (current_status->state_machine != SYSTEM_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.")
}
/* NEVER actually switch off HIL without reboot */
if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
ret = ERROR;
}
@@ -626,9 +709,12 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
case SYSTEM_STATE_REBOOT:
printf("try to reboot\n");
if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
if (current_system_state == SYSTEM_STATE_STANDBY
|| current_system_state == SYSTEM_STATE_PREFLIGHT
|| current_status->flag_hil_enabled) {
printf("system will reboot\n");
//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
mavlink_log_critical(mavlink_fd, "Rebooting..");
usleep(200000);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
ret = 0;
}
apps/commander/state_machine_helper.h
+9
View File
@@ -127,6 +127,15 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
*/
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if mode switch is guided
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if mode switch is auto
*
apps/controllib/Makefile
+46
View File
@@ -0,0 +1,46 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Control library
#
CSRCS = test_params.c
CXXSRCS = block/Block.cpp \
block/BlockParam.cpp \
block/UOrbPublication.cpp \
block/UOrbSubscription.cpp \
blocks.cpp \
fixedwing.cpp
include $(APPDIR)/mk/app.mk
apps/controllib/block/Block.cpp
+210
View File
@@ -0,0 +1,210 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Block.cpp
*
* Controller library code
*/
#include <math.h>
#include <string.h>
#include <stdio.h>
#include "Block.hpp"
#include "BlockParam.hpp"
#include "UOrbSubscription.hpp"
#include "UOrbPublication.hpp"
namespace control
{
Block::Block(SuperBlock *parent, const char *name) :
_name(name),
_parent(parent),
_dt(0),
_subscriptions(),
_params()
{
if (getParent() != NULL) {
getParent()->getChildren().add(this);
}
}
void Block::getName(char *buf, size_t n)
{
if (getParent() == NULL) {
strncpy(buf, _name, n);
} else {
char parentName[blockNameLengthMax];
getParent()->getName(parentName, n);
if (!strcmp(_name, "")) {
strncpy(buf, parentName, blockNameLengthMax);
} else {
snprintf(buf, blockNameLengthMax, "%s_%s", parentName, _name);
}
}
}
void Block::updateParams()
{
BlockParamBase *param = getParams().getHead();
int count = 0;
while (param != NULL) {
if (count++ > maxParamsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max params for block: %s\n", name);
break;
}
//printf("updating param: %s\n", param->getName());
param->update();
param = param->getSibling();
}
}
void Block::updateSubscriptions()
{
UOrbSubscriptionBase *sub = getSubscriptions().getHead();
int count = 0;
while (sub != NULL) {
if (count++ > maxSubscriptionsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max subscriptions for block: %s\n", name);
break;
}
sub->update();
sub = sub->getSibling();
}
}
void Block::updatePublications()
{
UOrbPublicationBase *pub = getPublications().getHead();
int count = 0;
while (pub != NULL) {
if (count++ > maxPublicationsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max publications for block: %s\n", name);
break;
}
pub->update();
pub = pub->getSibling();
}
}
void SuperBlock::setDt(float dt)
{
Block::setDt(dt);
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->setDt(dt);
child = child->getSibling();
}
}
void SuperBlock::updateChildParams()
{
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->updateParams();
child = child->getSibling();
}
}
void SuperBlock::updateChildSubscriptions()
{
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->updateSubscriptions();
child = child->getSibling();
}
}
void SuperBlock::updateChildPublications()
{
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->updatePublications();
child = child->getSibling();
}
}
} // namespace control
apps/controllib/block/Block.hpp
+131
View File
@@ -0,0 +1,131 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Block.h
*
* Controller library code
*/
#pragma once
#include <stdint.h>
#include <inttypes.h>
#include "List.hpp"
namespace control
{
static const uint16_t maxChildrenPerBlock = 100;
static const uint16_t maxParamsPerBlock = 100;
static const uint16_t maxSubscriptionsPerBlock = 100;
static const uint16_t maxPublicationsPerBlock = 100;
static const uint8_t blockNameLengthMax = 80;
// forward declaration
class BlockParamBase;
class UOrbSubscriptionBase;
class UOrbPublicationBase;
class SuperBlock;
/**
*/
class __EXPORT Block :
public ListNode<Block *>
{
public:
friend class BlockParamBase;
// methods
Block(SuperBlock *parent, const char *name);
void getName(char *name, size_t n);
virtual ~Block() {};
virtual void updateParams();
virtual void updateSubscriptions();
virtual void updatePublications();
virtual void setDt(float dt) { _dt = dt; }
// accessors
float getDt() { return _dt; }
protected:
// accessors
SuperBlock *getParent() { return _parent; }
List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
List<UOrbPublicationBase *> & getPublications() { return _publications; }
List<BlockParamBase *> & getParams() { return _params; }
// attributes
const char *_name;
SuperBlock *_parent;
float _dt;
List<UOrbSubscriptionBase *> _subscriptions;
List<UOrbPublicationBase *> _publications;
List<BlockParamBase *> _params;
};
class __EXPORT SuperBlock :
public Block
{
public:
friend class Block;
// methods
SuperBlock(SuperBlock *parent, const char *name) :
Block(parent, name),
_children() {
}
virtual ~SuperBlock() {};
virtual void setDt(float dt);
virtual void updateParams() {
Block::updateParams();
if (getChildren().getHead() != NULL) updateChildParams();
}
virtual void updateSubscriptions() {
Block::updateSubscriptions();
if (getChildren().getHead() != NULL) updateChildSubscriptions();
}
virtual void updatePublications() {
Block::updatePublications();
if (getChildren().getHead() != NULL) updateChildPublications();
}
protected:
// methods
List<Block *> & getChildren() { return _children; }
void updateChildParams();
void updateChildSubscriptions();
void updateChildPublications();
// attributes
List<Block *> _children;
};
} // namespace control
apps/controllib/block/BlockParam.cpp
+77
View File
@@ -0,0 +1,77 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Blockparam.cpp
*
* Controller library code
*/
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "BlockParam.hpp"
namespace control
{
BlockParamBase::BlockParamBase(Block *parent, const char *name) :
_handle(PARAM_INVALID)
{
char fullname[blockNameLengthMax];
if (parent == NULL) {
strncpy(fullname, name, blockNameLengthMax);
} else {
char parentName[blockNameLengthMax];
parent->getName(parentName, blockNameLengthMax);
if (!strcmp(name, "")) {
strncpy(fullname, parentName, blockNameLengthMax);
} else {
snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
}
parent->getParams().add(this);
}
_handle = param_find(fullname);
if (_handle == PARAM_INVALID)
printf("error finding param: %s\n", fullname);
};
} // namespace control
apps/controllib/block/BlockParam.hpp
+85
View File
@@ -0,0 +1,85 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file BlockParam.h
*
* Controller library code
*/
#pragma once
#include <systemlib/param/param.h>
#include "Block.hpp"
#include "List.hpp"
namespace control
{
/**
* A base class for block params that enables traversing linked list.
*/
class __EXPORT BlockParamBase : public ListNode<BlockParamBase *>
{
public:
BlockParamBase(Block *parent, const char *name);
virtual ~BlockParamBase() {};
virtual void update() = 0;
const char *getName() { return param_name(_handle); }
protected:
param_t _handle;
};
/**
* Parameters that are tied to blocks for updating and nameing.
*/
template<class T>
class __EXPORT BlockParam : public BlockParamBase
{
public:
BlockParam(Block *block, const char *name) :
BlockParamBase(block, name),
_val() {
update();
}
T get() { return _val; }
void set(T val) { _val = val; }
void update() {
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
}
protected:
T _val;
};
} // namespace control
apps/controllib/block/List.hpp
+71
View File
@@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Node.h
*
* A node of a linked list.
*/
#pragma once
template<class T>
class __EXPORT ListNode
{
public:
ListNode() : _sibling(NULL) {
}
void setSibling(T sibling) { _sibling = sibling; }
T getSibling() { return _sibling; }
T get() {
return _sibling;
}
protected:
T _sibling;
};
template<class T>
class __EXPORT List
{
public:
List() : _head() {
}
void add(T newNode) {
newNode->setSibling(getHead());
setHead(newNode);
}
T getHead() { return _head; }
private:
void setHead(T &head) { _head = head; }
T _head;
};
apps/controllib/block/UOrbPublication.cpp
+39
View File
@@ -0,0 +1,39 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file UOrbPublication.cpp
*
*/
#include "UOrbPublication.hpp"
apps/controllib/block/UOrbPublication.hpp
+118
View File
@@ -0,0 +1,118 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file UOrbPublication.h
*
*/
#pragma once
#include <uORB/uORB.h>
#include "Block.hpp"
#include "List.hpp"
namespace control
{
class Block;
/**
* Base publication warapper class, used in list traversal
* of various publications.
*/
class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
{
public:
UOrbPublicationBase(
List<UOrbPublicationBase *> * list,
const struct orb_metadata *meta) :
_meta(meta),
_handle() {
if (list != NULL) list->add(this);
}
void update() {
orb_publish(getMeta(), getHandle(), getDataVoidPtr());
}
virtual void *getDataVoidPtr() = 0;
virtual ~UOrbPublicationBase() {
orb_unsubscribe(getHandle());
}
const struct orb_metadata *getMeta() { return _meta; }
int getHandle() { return _handle; }
protected:
void setHandle(orb_advert_t handle) { _handle = handle; }
const struct orb_metadata *_meta;
orb_advert_t _handle;
};
/**
* UOrb Publication wrapper class
*/
template<class T>
class UOrbPublication :
public T, // this must be first!
public UOrbPublicationBase
{
public:
/**
* Constructor
*
* @param list A list interface for adding to list during construction
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
*/
UOrbPublication(
List<UOrbPublicationBase *> * list,
const struct orb_metadata *meta) :
T(), // initialize data structure to zero
UOrbPublicationBase(list, meta) {
// It is important that we call T()
// before we publish the data, so we
// call this here instead of the base class
setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
}
virtual ~UOrbPublication() {}
/*
* XXX
* This function gets the T struct, assuming
* the struct is the first base class, this
* should use dynamic cast, but doesn't
* seem to be available
*/
void *getDataVoidPtr() { return (void *)(T *)(this); }
};
} // namespace control
apps/controllib/block/UOrbSubscription.cpp
+51
View File
@@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file UOrbSubscription.cpp
*
*/
#include "UOrbSubscription.hpp"
namespace control
{
bool __EXPORT UOrbSubscriptionBase::updated()
{
bool isUpdated = false;
orb_check(_handle, &isUpdated);
return isUpdated;
}
} // namespace control
apps/controllib/block/UOrbSubscription.hpp
+137
View File
@@ -0,0 +1,137 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file UOrbSubscription.h
*
*/
#pragma once
#include <uORB/uORB.h>
#include "Block.hpp"
#include "List.hpp"
namespace control
{
class Block;
/**
* Base subscription warapper class, used in list traversal
* of various subscriptions.
*/
class __EXPORT UOrbSubscriptionBase :
public ListNode<control::UOrbSubscriptionBase *>
{
public:
// methods
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
*/
UOrbSubscriptionBase(
List<UOrbSubscriptionBase *> * list,
const struct orb_metadata *meta) :
_meta(meta),
_handle() {
if (list != NULL) list->add(this);
}
bool updated();
void update() {
if (updated()) {
orb_copy(_meta, _handle, getDataVoidPtr());
}
}
virtual void *getDataVoidPtr() = 0;
virtual ~UOrbSubscriptionBase() {
orb_unsubscribe(_handle);
}
// accessors
const struct orb_metadata *getMeta() { return _meta; }
int getHandle() { return _handle; }
protected:
// accessors
void setHandle(int handle) { _handle = handle; }
// attributes
const struct orb_metadata *_meta;
int _handle;
};
/**
* UOrb Subscription wrapper class
*/
template<class T>
class __EXPORT UOrbSubscription :
public T, // this must be first!
public UOrbSubscriptionBase
{
public:
/**
* Constructor
*
* @param list A list interface for adding to list during construction
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @param interval The minimum interval in milliseconds between updates
*/
UOrbSubscription(
List<UOrbSubscriptionBase *> * list,
const struct orb_metadata *meta, unsigned interval) :
T(), // initialize data structure to zero
UOrbSubscriptionBase(list, meta) {
setHandle(orb_subscribe(getMeta()));
orb_set_interval(getHandle(), interval);
}
/**
* Deconstructor
*/
virtual ~UOrbSubscription() {}
/*
* XXX
* This function gets the T struct, assuming
* the struct is the first base class, this
* should use dynamic cast, but doesn't
* seem to be available
*/
void *getDataVoidPtr() { return (void *)(T *)(this); }
T getData() { return T(*this); }
};
} // namespace control
apps/controllib/blocks.cpp
+486
View File
@@ -0,0 +1,486 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file blocks.cpp
*
* Controller library code
*/
#include <math.h>
#include <stdio.h>
#include "blocks.hpp"
namespace control
{
int basicBlocksTest()
{
blockLimitTest();
blockLimitSymTest();
blockLowPassTest();
blockHighPassTest();
blockIntegralTest();
blockIntegralTrapTest();
blockDerivativeTest();
blockPTest();
blockPITest();
blockPDTest();
blockPIDTest();
blockOutputTest();
blockRandUniformTest();
blockRandGaussTest();
return 0;
}
float BlockLimit::update(float input)
{
if (input > getMax()) {
input = _max.get();
} else if (input < getMin()) {
input = getMin();
}
return input;
}
int blockLimitTest()
{
printf("Test BlockLimit\t\t\t: ");
BlockLimit limit(NULL, "TEST");
// initial state
ASSERT(equal(1.0f, limit.getMax()));
ASSERT(equal(-1.0f, limit.getMin()));
ASSERT(equal(0.0f, limit.getDt()));
// update
ASSERT(equal(-1.0f, limit.update(-2.0f)));
ASSERT(equal(1.0f, limit.update(2.0f)));
ASSERT(equal(0.0f, limit.update(0.0f)));
printf("PASS\n");
return 0;
}
float BlockLimitSym::update(float input)
{
if (input > getMax()) {
input = _max.get();
} else if (input < -getMax()) {
input = -getMax();
}
return input;
}
int blockLimitSymTest()
{
printf("Test BlockLimitSym\t\t: ");
BlockLimitSym limit(NULL, "TEST");
// initial state
ASSERT(equal(1.0f, limit.getMax()));
ASSERT(equal(0.0f, limit.getDt()));
// update
ASSERT(equal(-1.0f, limit.update(-2.0f)));
ASSERT(equal(1.0f, limit.update(2.0f)));
ASSERT(equal(0.0f, limit.update(0.0f)));
printf("PASS\n");
return 0;
}
float BlockLowPass::update(float input)
{
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = b / (1 + b);
setState(a * input + (1 - a)*getState());
return getState();
}
int blockLowPassTest()
{
printf("Test BlockLowPass\t\t: ");
BlockLowPass lowPass(NULL, "TEST_LP");
// test initial state
ASSERT(equal(10.0f, lowPass.getFCut()));
ASSERT(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt()));
// set dt
lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt()));
// set state
lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState()));
// test update
ASSERT(equal(1.8626974f, lowPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
lowPass.update(2.0f);
}
ASSERT(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n");
return 0;
};
float BlockHighPass::update(float input)
{
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = 1 / (1 + b);
setY(a * (getY() + input - getU()));
setU(input);
return getY();
}
int blockHighPassTest()
{
printf("Test BlockHighPass\t\t: ");
BlockHighPass highPass(NULL, "TEST_HP");
// test initial state
ASSERT(equal(10.0f, highPass.getFCut()));
ASSERT(equal(0.0f, highPass.getU()));
ASSERT(equal(0.0f, highPass.getY()));
ASSERT(equal(0.0f, highPass.getDt()));
// set dt
highPass.setDt(0.1f);
ASSERT(equal(0.1f, highPass.getDt()));
// set state
highPass.setU(1.0f);
ASSERT(equal(1.0f, highPass.getU()));
highPass.setY(1.0f);
ASSERT(equal(1.0f, highPass.getY()));
// test update
ASSERT(equal(0.2746051f, highPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
highPass.update(2.0f);
}
ASSERT(equal(0.0f, highPass.getY()));
ASSERT(equal(0.0f, highPass.update(2.0f)));
printf("PASS\n");
return 0;
}
float BlockIntegral::update(float input)
{
// trapezoidal integration
setY(_limit.update(getY() + input * getDt()));
return getY();
}
int blockIntegralTest()
{
printf("Test BlockIntegral\t\t: ");
BlockIntegral integral(NULL, "TEST_I");
// test initial state
ASSERT(equal(1.0f, integral.getMax()));
ASSERT(equal(0.0f, integral.getDt()));
// set dt
integral.setDt(0.1f);
ASSERT(equal(0.1f, integral.getDt()));
// set Y
integral.setY(0.9f);
ASSERT(equal(0.9f, integral.getY()));
// test exceed max
for (int i = 0; i < 100; i++) {
integral.update(1.0f);
}
ASSERT(equal(1.0f, integral.update(1.0f)));
// test exceed min
integral.setY(-0.9f);
ASSERT(equal(-0.9f, integral.getY()));
for (int i = 0; i < 100; i++) {
integral.update(-1.0f);
}
ASSERT(equal(-1.0f, integral.update(-1.0f)));
// test update
integral.setY(0.1f);
ASSERT(equal(0.2f, integral.update(1.0)));
ASSERT(equal(0.2f, integral.getY()));
printf("PASS\n");
return 0;
}
float BlockIntegralTrap::update(float input)
{
// trapezoidal integration
setY(_limit.update(getY() +
(getU() + input) / 2.0f * getDt()));
setU(input);
return getY();
}
int blockIntegralTrapTest()
{
printf("Test BlockIntegralTrap\t\t: ");
BlockIntegralTrap integral(NULL, "TEST_I");
// test initial state
ASSERT(equal(1.0f, integral.getMax()));
ASSERT(equal(0.0f, integral.getDt()));
// set dt
integral.setDt(0.1f);
ASSERT(equal(0.1f, integral.getDt()));
// set U
integral.setU(1.0f);
ASSERT(equal(1.0f, integral.getU()));
// set Y
integral.setY(0.9f);
ASSERT(equal(0.9f, integral.getY()));
// test exceed max
for (int i = 0; i < 100; i++) {
integral.update(1.0f);
}
ASSERT(equal(1.0f, integral.update(1.0f)));
// test exceed min
integral.setU(-1.0f);
integral.setY(-0.9f);
ASSERT(equal(-0.9f, integral.getY()));
for (int i = 0; i < 100; i++) {
integral.update(-1.0f);
}
ASSERT(equal(-1.0f, integral.update(-1.0f)));
// test update
integral.setU(2.0f);
integral.setY(0.1f);
ASSERT(equal(0.25f, integral.update(1.0)));
ASSERT(equal(0.25f, integral.getY()));
ASSERT(equal(1.0f, integral.getU()));
printf("PASS\n");
return 0;
}
float BlockDerivative::update(float input)
{
float output = _lowPass.update((input - getU()) / getDt());
setU(input);
return output;
}
int blockDerivativeTest()
{
printf("Test BlockDerivative\t\t: ");
BlockDerivative derivative(NULL, "TEST_D");
// test initial state
ASSERT(equal(0.0f, derivative.getU()));
ASSERT(equal(10.0f, derivative.getLP()));
// set dt
derivative.setDt(0.1f);
ASSERT(equal(0.1f, derivative.getDt()));
// set U
derivative.setU(1.0f);
ASSERT(equal(1.0f, derivative.getU()));
// test update
ASSERT(equal(8.6269744f, derivative.update(2.0f)));
ASSERT(equal(2.0f, derivative.getU()));
printf("PASS\n");
return 0;
}
int blockPTest()
{
printf("Test BlockP\t\t\t: ");
BlockP blockP(NULL, "TEST_P");
// test initial state
ASSERT(equal(0.2f, blockP.getKP()));
ASSERT(equal(0.0f, blockP.getDt()));
// set dt
blockP.setDt(0.1f);
ASSERT(equal(0.1f, blockP.getDt()));
// test update
ASSERT(equal(0.4f, blockP.update(2.0f)));
printf("PASS\n");
return 0;
}
int blockPITest()
{
printf("Test BlockPI\t\t\t: ");
BlockPI blockPI(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPI.getKP()));
ASSERT(equal(0.1f, blockPI.getKI()));
ASSERT(equal(0.0f, blockPI.getDt()));
ASSERT(equal(1.0f, blockPI.getIntegral().getMax()));
// set dt
blockPI.setDt(0.1f);
ASSERT(equal(0.1f, blockPI.getDt()));
// set integral state
blockPI.getIntegral().setY(0.1f);
ASSERT(equal(0.1f, blockPI.getIntegral().getY()));
// test update
// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
ASSERT(equal(0.43f, blockPI.update(2.0f)));
printf("PASS\n");
return 0;
}
int blockPDTest()
{
printf("Test BlockPD\t\t\t: ");
BlockPD blockPD(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPD.getKP()));
ASSERT(equal(0.01f, blockPD.getKD()));
ASSERT(equal(0.0f, blockPD.getDt()));
ASSERT(equal(10.0f, blockPD.getDerivative().getLP()));
// set dt
blockPD.setDt(0.1f);
ASSERT(equal(0.1f, blockPD.getDt()));
// set derivative state
blockPD.getDerivative().setU(1.0f);
ASSERT(equal(1.0f, blockPD.getDerivative().getU()));
// test update
// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
ASSERT(equal(0.486269744f, blockPD.update(2.0f)));
printf("PASS\n");
return 0;
}
int blockPIDTest()
{
printf("Test BlockPID\t\t\t: ");
BlockPID blockPID(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPID.getKP()));
ASSERT(equal(0.1f, blockPID.getKI()));
ASSERT(equal(0.01f, blockPID.getKD()));
ASSERT(equal(0.0f, blockPID.getDt()));
ASSERT(equal(10.0f, blockPID.getDerivative().getLP()));
ASSERT(equal(1.0f, blockPID.getIntegral().getMax()));
// set dt
blockPID.setDt(0.1f);
ASSERT(equal(0.1f, blockPID.getDt()));
// set derivative state
blockPID.getDerivative().setU(1.0f);
ASSERT(equal(1.0f, blockPID.getDerivative().getU()));
// set integral state
blockPID.getIntegral().setY(0.1f);
ASSERT(equal(0.1f, blockPID.getIntegral().getY()));
// test update
// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
ASSERT(equal(0.5162697f, blockPID.update(2.0f)));
printf("PASS\n");
return 0;
}
int blockOutputTest()
{
printf("Test BlockOutput\t\t: ");
BlockOutput blockOutput(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockOutput.getDt()));
ASSERT(equal(0.5f, blockOutput.get()));
ASSERT(equal(-1.0f, blockOutput.getMin()));
ASSERT(equal(1.0f, blockOutput.getMax()));
// test update below min
blockOutput.update(-2.0f);
ASSERT(equal(-1.0f, blockOutput.get()));
// test update above max
blockOutput.update(2.0f);
ASSERT(equal(1.0f, blockOutput.get()));
// test trim
blockOutput.update(0.0f);
ASSERT(equal(0.5f, blockOutput.get()));
printf("PASS\n");
return 0;
}
int blockRandUniformTest()
{
srand(1234);
printf("Test BlockRandUniform\t\t: ");
BlockRandUniform blockRandUniform(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockRandUniform.getDt()));
ASSERT(equal(-1.0f, blockRandUniform.getMin()));
ASSERT(equal(1.0f, blockRandUniform.getMax()));
// test update
int n = 10000;
float mean = blockRandUniform.update();
// recursive mean algorithm from Knuth
for (int i = 2; i < n + 1; i++) {
float val = blockRandUniform.update();
mean += (val - mean) / i;
ASSERT(val <= blockRandUniform.getMax());
ASSERT(val >= blockRandUniform.getMin());
}
ASSERT(equal(mean, (blockRandUniform.getMin() +
blockRandUniform.getMax()) / 2, 1e-1));
printf("PASS\n");
return 0;
}
int blockRandGaussTest()
{
srand(1234);
printf("Test BlockRandGauss\t\t: ");
BlockRandGauss blockRandGauss(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockRandGauss.getDt()));
ASSERT(equal(1.0f, blockRandGauss.getMean()));
ASSERT(equal(2.0f, blockRandGauss.getStdDev()));
// test update
int n = 10000;
float mean = blockRandGauss.update();
float sum = 0;
// recursive mean, stdev algorithm from Knuth
for (int i = 2; i < n + 1; i++) {
float val = blockRandGauss.update();
float newMean = mean + (val - mean) / i;
sum += (val - mean) * (val - newMean);
mean = newMean;
}
float stdDev = sqrt(sum / (n - 1));
ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
printf("PASS\n");
return 0;
}
} // namespace control

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