fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 16:14:08 +08:00
Code Issues 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
commit 0669d2aee0
1516 changed files with 240775 additions and 39124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
.gitignore vendored
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

22
Debug/NuttX
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

54
Debug/PX4 Normal file
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

48
Makefile
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

8
ROMFS/Makefile
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.

275
ROMFS/logging/logconv.m
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

7
ROMFS/mixers/FMU_hex_+.mix Normal file
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

7
ROMFS/mixers/FMU_hex_x.mix Normal file
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

7
ROMFS/mixers/FMU_octo_+.mix Normal file
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

7
ROMFS/mixers/FMU_octo_x.mix Normal file
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

46
ROMFS/scripts/rc.PX4IO
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

34
ROMFS/scripts/rc.PX4IOAR
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
#

1
ROMFS/scripts/rc.sensors
View File

@ -8,6 +8,7 @@
#
ms5611 start
adc start
if mpu6000 start
then

18
ROMFS/scripts/rcS
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.

19
Tools/fix_code_style_ubuntu.sh Executable file
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 \

5268
Tools/mavlink_px4.py Normal file
View File

File diff suppressed because it is too large Load Diff

2
Tools/px_uploader.py
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")

128
apps/ChangeLog.txt
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.

4
apps/Kconfig
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"

15
apps/Make.defs
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

109
apps/Makefile
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

146
apps/README.txt
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
-----------------------------------------------------------------

32
apps/ardrone_interface/ardrone_motor_control.c
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) */

78
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
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!");
}

20
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
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;
}

6
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
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;
};
/**

17
apps/builtin/Kconfig Normal file
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

40
apps/builtin/Make.defs Normal file
View File

@ -0,0 +1,40 @@
############################################################################
# apps/builtin/Make.defs
# Adds selected applications to apps/ build
#
# 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.
#
############################################################################
ifeq ($(CONFIG_BUILTIN),y)
CONFIGURED_APPS += builtin
endif

130
apps/builtin/Makefile Normal file
View File

@ -0,0 +1,130 @@
############################################################################
# apps/builtin/Makefile
#
# 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
# 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
# Source and object files
ASRCS =
CSRCS = builtin.c builtin_list.c exec_builtin.c
AOBJS = $(ASRCS:.S=$(OBJEXT))
COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\libapps$(LIBEXT)
else
ifeq ($(WINTOOL),y)
BIN = ..\\libapps$(LIBEXT)
else
BIN = ../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
VPATH =
# Build Targets
all: .built
.PHONY: context depend clean distclean
$(AOBJS): %$(OBJEXT): %.S
$(call ASSEMBLE, $<, $@)
$(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
registry$(DELIM).updated:
$(V) $(MAKE) -C registry .updated TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
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
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
.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:
$(Q) $(MAKE) -C registry clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
$(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

80
apps/builtin/builtin.c Normal file
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;
}

79
apps/builtin/builtin_list.c Normal file
View File

@ -0,0 +1,79 @@
/****************************************************************************
* apps/builtin/builtin_list.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
****************************************************************************/
#include "builtin_proto.h"
const struct builtin_s g_builtins[] =
{
# include "builtin_list.h"
{ NULL, 0, 0, 0 }
};
const int g_builtin_count = sizeof(g_builtins) / sizeof(g_builtins[0]);
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/

467
apps/builtin/exec_builtin.c Normal file
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;
}

61
apps/builtin/registry/Makefile Normal file
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)

283
apps/commander/calibration_routines.c
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;
}

2
apps/commander/calibration_routines.h
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);

920
apps/commander/commander.c
View File

File diff suppressed because it is too large Load Diff

3
apps/commander/commander.h
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_ */

230
apps/commander/state_machine_helper.c
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;
}

9
apps/commander/state_machine_helper.h
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
*

46
apps/controllib/Makefile Normal file
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

210
apps/controllib/block/Block.cpp Normal file
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

131
apps/controllib/block/Block.hpp Normal file
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

77
apps/controllib/block/BlockParam.cpp Normal file
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

85
apps/controllib/block/BlockParam.hpp Normal file
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

71
apps/controllib/block/List.hpp Normal file
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;
};

39
apps/controllib/block/UOrbPublication.cpp Normal file
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"

118
apps/controllib/block/UOrbPublication.hpp Normal file
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

51
apps/controllib/block/UOrbSubscription.cpp Normal file
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

137
apps/controllib/block/UOrbSubscription.hpp Normal file
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

486
apps/controllib/blocks.cpp Normal file
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

494
apps/controllib/blocks.hpp Normal file
View File

@ -0,0 +1,494 @@
/****************************************************************************
*
* 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.h
*
* Controller library code
*/
#pragma once
#include <assert.h>
#include <time.h>
#include <stdlib.h>
#include <mathlib/math/test/test.hpp>
#include "block/Block.hpp"
#include "block/BlockParam.hpp"
namespace control
{
int __EXPORT basicBlocksTest();
/**
* A limiter/ saturation.
* The output of update is the input, bounded
* by min/max.
*/
class __EXPORT BlockLimit : public Block
{
public:
// methods
BlockLimit(SuperBlock *parent, const char *name) :
Block(parent, name),
_min(this, "MIN"),
_max(this, "MAX")
{};
virtual ~BlockLimit() {};
float update(float input);
// accessors
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
protected:
// attributes
BlockParam<float> _min;
BlockParam<float> _max;
};
int __EXPORT blockLimitTest();
/**
* A symmetric limiter/ saturation.
* Same as limiter but with only a max, is used for
* upper limit of +max, and lower limit of -max
*/
class __EXPORT BlockLimitSym : public Block
{
public:
// methods
BlockLimitSym(SuperBlock *parent, const char *name) :
Block(parent, name),
_max(this, "MAX")
{};
virtual ~BlockLimitSym() {};
float update(float input);
// accessors
float getMax() { return _max.get(); }
protected:
// attributes
BlockParam<float> _max;
};
int __EXPORT blockLimitSymTest();
/**
* A low pass filter as described here:
* http://en.wikipedia.org/wiki/Low-pass_filter.
*/
class __EXPORT BlockLowPass : public Block
{
public:
// methods
BlockLowPass(SuperBlock *parent, const char *name) :
Block(parent, name),
_state(0),
_fCut(this, "") // only one parameter, no need to name
{};
virtual ~BlockLowPass() {};
float update(float input);
// accessors
float getState() { return _state; }
float getFCut() { return _fCut.get(); }
void setState(float state) { _state = state; }
protected:
// attributes
float _state;
BlockParam<float> _fCut;
};
int __EXPORT blockLowPassTest();
/**
* A high pass filter as described here:
* http://en.wikipedia.org/wiki/High-pass_filter.
*/
class __EXPORT BlockHighPass : public Block
{
public:
// methods
BlockHighPass(SuperBlock *parent, const char *name) :
Block(parent, name),
_u(0),
_y(0),
_fCut(this, "") // only one parameter, no need to name
{};
virtual ~BlockHighPass() {};
float update(float input);
// accessors
float getU() {return _u;}
float getY() {return _y;}
float getFCut() {return _fCut.get();}
void setU(float u) {_u = u;}
void setY(float y) {_y = y;}
protected:
// attributes
float _u; /**< previous input */
float _y; /**< previous output */
BlockParam<float> _fCut; /**< cut-off frequency, Hz */
};
int __EXPORT blockHighPassTest();
/**
* A rectangular integrator.
* A limiter is built into the class to bound the
* integral's internal state. This is important
* for windup protection.
* @see Limit
*/
class __EXPORT BlockIntegral: public SuperBlock
{
public:
// methods
BlockIntegral(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_y(0),
_limit(this, "") {};
virtual ~BlockIntegral() {};
float update(float input);
// accessors
float getY() {return _y;}
float getMax() {return _limit.getMax();}
void setY(float y) {_y = y;}
protected:
// attributes
float _y; /**< previous output */
BlockLimitSym _limit; /**< limiter */
};
int __EXPORT blockIntegralTest();
/**
* A trapezoidal integrator.
* http://en.wikipedia.org/wiki/Trapezoidal_rule
* A limiter is built into the class to bound the
* integral's internal state. This is important
* for windup protection.
* @see Limit
*/
class __EXPORT BlockIntegralTrap : public SuperBlock
{
public:
// methods
BlockIntegralTrap(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_u(0),
_y(0),
_limit(this, "") {};
virtual ~BlockIntegralTrap() {};
float update(float input);
// accessors
float getU() {return _u;}
float getY() {return _y;}
float getMax() {return _limit.getMax();}
void setU(float u) {_u = u;}
void setY(float y) {_y = y;}
protected:
// attributes
float _u; /**< previous input */
float _y; /**< previous output */
BlockLimitSym _limit; /**< limiter */
};
int __EXPORT blockIntegralTrapTest();
/**
* A simple derivative approximation.
* This uses the previous and current input.
* This has a built in low pass filter.
* @see LowPass
*/
class __EXPORT BlockDerivative : public SuperBlock
{
public:
// methods
BlockDerivative(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_u(0),
_lowPass(this, "LP")
{};
virtual ~BlockDerivative() {};
float update(float input);
// accessors
void setU(float u) {_u = u;}
float getU() {return _u;}
float getLP() {return _lowPass.getFCut();}
protected:
// attributes
float _u; /**< previous input */
BlockLowPass _lowPass; /**< low pass filter */
};
int __EXPORT blockDerivativeTest();
/**
* A proportional controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockP: public Block
{
public:
// methods
BlockP(SuperBlock *parent, const char *name) :
Block(parent, name),
_kP(this, "") // only one param, no need to name
{};
virtual ~BlockP() {};
float update(float input) {
return getKP() * input;
}
// accessors
float getKP() { return _kP.get(); }
protected:
BlockParam<float> _kP;
};
int __EXPORT blockPTest();
/**
* A proportional-integral controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockPI: public SuperBlock
{
public:
// methods
BlockPI(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_integral(this, "I"),
_kP(this, "P"),
_kI(this, "I")
{};
virtual ~BlockPI() {};
float update(float input) {
return getKP() * input +
getKI() * getIntegral().update(input);
}
// accessors
float getKP() { return _kP.get(); }
float getKI() { return _kI.get(); }
BlockIntegral &getIntegral() { return _integral; }
private:
BlockIntegral _integral;
BlockParam<float> _kP;
BlockParam<float> _kI;
};
int __EXPORT blockPITest();
/**
* A proportional-derivative controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockPD: public SuperBlock
{
public:
// methods
BlockPD(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_derivative(this, "D"),
_kP(this, "P"),
_kD(this, "D")
{};
virtual ~BlockPD() {};
float update(float input) {
return getKP() * input +
getKD() * getDerivative().update(input);
}
// accessors
float getKP() { return _kP.get(); }
float getKD() { return _kD.get(); }
BlockDerivative &getDerivative() { return _derivative; }
private:
BlockDerivative _derivative;
BlockParam<float> _kP;
BlockParam<float> _kD;
};
int __EXPORT blockPDTest();
/**
* A proportional-integral-derivative controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockPID: public SuperBlock
{
public:
// methods
BlockPID(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_integral(this, "I"),
_derivative(this, "D"),
_kP(this, "P"),
_kI(this, "I"),
_kD(this, "D")
{};
virtual ~BlockPID() {};
float update(float input) {
return getKP() * input +
getKI() * getIntegral().update(input) +
getKD() * getDerivative().update(input);
}
// accessors
float getKP() { return _kP.get(); }
float getKI() { return _kI.get(); }
float getKD() { return _kD.get(); }
BlockIntegral &getIntegral() { return _integral; }
BlockDerivative &getDerivative() { return _derivative; }
private:
// attributes
BlockIntegral _integral;
BlockDerivative _derivative;
BlockParam<float> _kP;
BlockParam<float> _kI;
BlockParam<float> _kD;
};
int __EXPORT blockPIDTest();
/**
* An output trim/ saturation block
*/
class __EXPORT BlockOutput: public SuperBlock
{
public:
// methods
BlockOutput(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_trim(this, "TRIM"),
_limit(this, ""),
_val(0) {
update(0);
};
virtual ~BlockOutput() {};
void update(float input) {
_val = _limit.update(input + getTrim());
}
// accessors
float getMin() { return _limit.getMin(); }
float getMax() { return _limit.getMax(); }
float getTrim() { return _trim.get(); }
float get() { return _val; }
private:
// attributes
BlockParam<float> _trim;
BlockLimit _limit;
float _val;
};
int __EXPORT blockOutputTest();
/**
* A uniform random number generator
*/
class __EXPORT BlockRandUniform: public Block
{
public:
// methods
BlockRandUniform(SuperBlock *parent,
const char *name) :
Block(parent, name),
_min(this, "MIN"),
_max(this, "MAX") {
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandUniform() {};
float update() {
static float rand_max = MAX_RAND;
float rand_val = rand();
float bounds = getMax() - getMin();
return getMin() + (rand_val * bounds) / rand_max;
}
// accessors
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
private:
// attributes
BlockParam<float> _min;
BlockParam<float> _max;
};
int __EXPORT blockRandUniformTest();
class __EXPORT BlockRandGauss: public Block
{
public:
// methods
BlockRandGauss(SuperBlock *parent,
const char *name) :
Block(parent, name),
_mean(this, "MEAN"),
_stdDev(this, "DEV") {
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandGauss() {};
float update() {
static float V1, V2, S;
static int phase = 0;
float X;
if (phase == 0) {
do {
float U1 = (float)rand() / MAX_RAND;
float U2 = (float)rand() / MAX_RAND;
V1 = 2 * U1 - 1;
V2 = 2 * U2 - 1;
S = V1 * V1 + V2 * V2;
} while (S >= 1 || fabsf(S) < 1e-8f);
X = V1 * float(sqrt(-2 * float(log(S)) / S));
} else
X = V2 * float(sqrt(-2 * float(log(S)) / S));
phase = 1 - phase;
return X * getStdDev() + getMean();
}
// accessors
float getMean() { return _mean.get(); }
float getStdDev() { return _stdDev.get(); }
private:
// attributes
BlockParam<float> _mean;
BlockParam<float> _stdDev;
};
int __EXPORT blockRandGaussTest();
} // namespace control

367
apps/controllib/fixedwing.cpp Normal file
View File

@ -0,0 +1,367 @@
/****************************************************************************
*
* 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 fixedwing.cpp
*
* Controller library code
*/
#include "fixedwing.hpp"
namespace control
{
namespace fixedwing
{
BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_rLowPass(this, "R_LP"),
_rWashout(this, "R_HP"),
_r2Rdr(this, "R2RDR"),
_rudder(0)
{
}
BlockYawDamper::~BlockYawDamper() {};
void BlockYawDamper::update(float rCmd, float r)
{
_rudder = _r2Rdr.update(rCmd -
_rWashout.update(_rLowPass.update(r)));
}
BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_yawDamper(this, ""),
_pLowPass(this, "P_LP"),
_qLowPass(this, "Q_LP"),
_p2Ail(this, "P2AIL"),
_q2Elv(this, "Q2ELV"),
_aileron(0),
_elevator(0)
{
}
BlockStabilization::~BlockStabilization() {};
void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
float p, float q, float r)
{
_aileron = _p2Ail.update(
pCmd - _pLowPass.update(p));
_elevator = _q2Elv.update(
qCmd - _qLowPass.update(q));
_yawDamper.update(rCmd, r);
}
BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_psi2Phi(this, "PSI2PHI"),
_phi2P(this, "PHI2P"),
_phiLimit(this, "PHI_LIM")
{
}
BlockHeadingHold::~BlockHeadingHold() {};
float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
{
float psiError = _wrap_pi(psiCmd - psi);
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
return _phi2P.update(phiCmd - phi);
}
BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_v2Theta(this, "V2THE"),
_theta2Q(this, "THE2Q"),
_theLimit(this, "THE"),
_vLimit(this, "V")
{
}
BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
{
// negative sign because nose over to increase speed
float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
return _theta2Q.update(thetaCmd - theta);
}
BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_v2Thr(this, "V2THR")
{
}
BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
float BlockVelocityHoldFrontside::update(float vCmd, float v)
{
return _v2Thr.update(vCmd - v);
}
BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_h2Thr(this, "H2THR"),
_throttle(0)
{
}
BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
void BlockAltitudeHoldBackside::update(float hCmd, float h)
{
_throttle = _h2Thr.update(hCmd - h);
}
BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_h2Theta(this, "H2THE"),
_theta2Q(this, "THE2Q")
{
}
BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
{
float thetaCmd = _h2Theta.update(hCmd - h);
return _theta2Q.update(thetaCmd - theta);
}
BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
const char *name,
BlockStabilization *stabilization) :
SuperBlock(parent, name),
_stabilization(stabilization),
_headingHold(this, ""),
_velocityHold(this, ""),
_altitudeHold(this, ""),
_trimAil(this, "TRIM_AIL"),
_trimElv(this, "TRIM_ELV"),
_trimRdr(this, "TRIM_RDR"),
_trimThr(this, "TRIM_THR")
{
}
BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
float h, float v,
float phi, float theta, float psi,
float p, float q, float r)
{
_altitudeHold.update(hCmd, h);
_stabilization->update(
_headingHold.update(psiCmd, phi, psi, p),
_velocityHold.update(vCmd, v, theta, q),
rCmd,
p, q, r);
}
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_xtYawLimit(this, "XT2YAW"),
_xt2Yaw(this, "XT2YAW"),
_psiCmd(0)
{
}
BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd)
{
// heading to waypoint
float psiTrack = get_bearing_to_next_waypoint(
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
// cross track
struct crosstrack_error_s xtrackError;
get_distance_to_line(&xtrackError,
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)lastPosCmd.lat / (double)1e7d,
(double)lastPosCmd.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
_psiCmd = _wrap_2pi(psiTrack -
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
}
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_actuators(&getPublications(), ORB_ID(actuator_controls_0))
{
}
BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
BlockUorbEnabledAutopilot(parent, name),
_stabilization(this, ""), // no name needed, already unique
_backsideAutopilot(this, "", &_stabilization),
_guide(this, ""),
_vCmd(this, "V_CMD"),
_attPoll(),
_lastPosCmd(),
_timeStamp(0)
{
_attPoll.fd = _att.getHandle();
_attPoll.events = POLLIN;
}
void BlockMultiModeBacksideAutopilot::update()
{
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
// set dt for all child blocks
setDt(dt);
// store old position command before update if new command sent
if (_posCmd.updated()) {
_lastPosCmd = _posCmd.getData();
}
// check for new updates
if (_param_update.updated()) updateParams();
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
}
// XXX handle STABILIZED (loiter on spot) as well
// once the system switches from manual or auto to stabilized
// the setpoint should update to loitering around this position
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO ||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// commands
float rCmd = 0;
_backsideAutopilot.update(
_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
_pos.alt, v,
_att.roll, _att.pitch, _att.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed
);
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
_stabilization.update(
_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
}
}
// update all publications
updatePublications();
}
BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
{
// send one last publication when destroyed, setting
// all output to zero
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
updatePublications();
}
} // namespace fixedwing
} // namespace control

438
apps/controllib/fixedwing.hpp Normal file
View File

@ -0,0 +1,438 @@
/****************************************************************************
*
* 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 fixedwing.h
*
* Controller library code
*/
#pragma once
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <poll.h>
#include "blocks.hpp"
#include "block/UOrbSubscription.hpp"
#include "block/UOrbPublication.hpp"
extern "C" {
#include <systemlib/geo/geo.h>
}
namespace control
{
namespace fixedwing
{
/**
* BlockYawDamper
*
* This block has more explations to help new developers
* add their own blocks. It includes a limited explanation
* of some C++ basics.
*
* Block: The generic class describing a typical block as you
* would expect in Simulink or ScicosLab. A block can have
* parameters. It cannot have other blocks.
*
* SuperBlock: A superblock is a block that can have other
* blocks. It has methods that manage the blocks beneath it.
*
* BlockYawDamper inherits from SuperBlock publically, this
* means that any public function in SuperBlock are public within
* BlockYawDamper and may be called from outside the
* class methods. Any protected function within block
* are private to the class and may not be called from
* outside this class. Protected should be preferred
* where possible to public as it is important to
* limit access to the bare minimum to prevent
* accidental errors.
*/
class __EXPORT BlockYawDamper : public SuperBlock
{
private:
/**
* Declaring other blocks used by this block
*
* In this section we declare all child blocks that
* this block is composed of. They are private
* members so only this block has direct access to
* them.
*/
BlockLowPass _rLowPass;
BlockHighPass _rWashout;
BlockP _r2Rdr;
/**
* Declaring output values and accessors
*
* If we have any output values for the block we
* declare them here. Output can be directly returned
* through the update function, but outputs should be
* declared here if the information will likely be requested
* again, or if there are multiple outputs.
*
* You should only be able to set outputs from this block,
* so the outputs are declared in the private section.
* A get accessor is provided
* in the public section for other blocks to get the
* value of the output.
*/
float _rudder;
public:
/**
* BlockYawDamper Constructor
*
* The job of the constructor is to initialize all
* parameter in this block and initialize all child
* blocks. Note also, that the output values must be
* initialized here. The order of the members in the
* member initialization list should follow the
* order in which they are declared within the class.
* See the private member declarations above.
*
* Block Construction
*
* All blocks are constructed with their parent block
* and their name. This allows parameters within the
* block to construct a fully qualified name from
* concatenating the two. If the name provided to the
* block is "", then the block will use the parent
* name as it's name. This is useful in cases where
* you have a block that has parameters "MIN", "MAX",
* such as BlockLimit and you do not want an extra name
* to qualify them since the parent block has no "MIN",
* "MAX" parameters.
*
* Block Parameter Construction
*
* Block parameters are named constants, they are
* constructed using:
* BlockParam::BlockParam(Block * parent, const char * name)
* This funciton takes both a parent block and a name.
* The constructore then uses the parent name and the name of
* the paramter to ask the px4 param library if it has any
* parameters with this name. If it does, a handle to the
* parameter is retrieved.
*
* Block/ BlockParam Naming
*
* When desigining new blocks, the naming of the parameters and
* blocks determines the fully qualified name of the parameters
* within the ground station, so it is important to choose
* short, easily understandable names. Again, when a name of
* "" is passed, the parent block name is used as the value to
* prepend to paramters names.
*/
BlockYawDamper(SuperBlock *parent, const char *name);
/**
* Block deconstructor
*
* It is always a good idea to declare a virtual
* deconstructor so that upon calling delete from
* a class derived from this, all of the
* deconstructors all called, the derived class first, and
* then the base class
*/
virtual ~BlockYawDamper();
/**
* Block update function
*
* The job of the update function is to compute the output
* values for the block. In a simple block with one output,
* the output may be returned directly. If the output is
* required frequenly by other processses, it might be a
* good idea to declare a member to store the temporary
* variable.
*/
void update(float rCmd, float r);
/**
* Rudder output value accessor
*
* This is a public accessor function, which means that the
* private value _rudder is returned to anyone calling
* BlockYawDamper::getRudder(). Note thate a setRudder() is
* not provided, this is because the updateParams() call
* for a block provides the mechanism for updating the
* paramter.
*/
float getRudder() { return _rudder; }
};
/**
* Stability augmentation system.
* Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299
*/
class __EXPORT BlockStabilization : public SuperBlock
{
private:
BlockYawDamper _yawDamper;
BlockLowPass _pLowPass;
BlockLowPass _qLowPass;
BlockP _p2Ail;
BlockP _q2Elv;
float _aileron;
float _elevator;
public:
BlockStabilization(SuperBlock *parent, const char *name);
virtual ~BlockStabilization();
void update(float pCmd, float qCmd, float rCmd,
float p, float q, float r);
float getAileron() { return _aileron; }
float getElevator() { return _elevator; }
float getRudder() { return _yawDamper.getRudder(); }
};
/**
* Heading hold autopilot block.
* Aircraft Control and Simulation, Stevens and Lewis
* Heading hold, pg. 348
*/
class __EXPORT BlockHeadingHold : public SuperBlock
{
private:
BlockP _psi2Phi;
BlockP _phi2P;
BlockLimitSym _phiLimit;
public:
BlockHeadingHold(SuperBlock *parent, const char *name);
virtual ~BlockHeadingHold();
/**
* returns pCmd
*/
float update(float psiCmd, float phi, float psi, float p);
};
/**
* Frontside/ Backside Control Systems
*
* Frontside :
* velocity error -> throttle
* altitude error -> elevator
*
* Backside :
* velocity error -> elevator
* altitude error -> throttle
*
* Backside control systems are more resilient at
* slow speeds on the back-side of the power
* required curve/ landing etc. Less performance
* than frontside at high speeds.
*/
/**
* Backside velocity hold autopilot block.
* v -> theta -> q -> elevator
*/
class __EXPORT BlockVelocityHoldBackside : public SuperBlock
{
private:
BlockPID _v2Theta;
BlockPID _theta2Q;
BlockLimit _theLimit;
BlockLimit _vLimit;
public:
BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
virtual ~BlockVelocityHoldBackside();
/**
* returns qCmd
*/
float update(float vCmd, float v, float theta, float q);
};
/**
* Frontside velocity hold autopilot block.
* v -> throttle
*/
class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
{
private:
BlockPID _v2Thr;
public:
BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
virtual ~BlockVelocityHoldFrontside();
/**
* returns throttle
*/
float update(float vCmd, float v);
};
/**
* Backside altitude hold autopilot block.
* h -> throttle
*/
class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
{
private:
BlockPID _h2Thr;
float _throttle;
public:
BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
virtual ~BlockAltitudeHoldBackside();
void update(float hCmd, float h);
float getThrottle() { return _throttle; }
};
/**
* Frontside altitude hold autopilot block.
* h -> theta > q -> elevator
*/
class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
{
private:
BlockPID _h2Theta;
BlockPID _theta2Q;
public:
BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
virtual ~BlockAltitudeHoldFrontside();
/**
* return qCmd
*/
float update(float hCmd, float h, float theta, float q);
};
/**
* Backside autopilot
*/
class __EXPORT BlockBacksideAutopilot : public SuperBlock
{
private:
BlockStabilization *_stabilization;
BlockHeadingHold _headingHold;
BlockVelocityHoldBackside _velocityHold;
BlockAltitudeHoldBackside _altitudeHold;
BlockParam<float> _trimAil;
BlockParam<float> _trimElv;
BlockParam<float> _trimRdr;
BlockParam<float> _trimThr;
public:
BlockBacksideAutopilot(SuperBlock *parent,
const char *name,
BlockStabilization *stabilization);
virtual ~BlockBacksideAutopilot();
void update(float hCmd, float vCmd, float rCmd, float psiCmd,
float h, float v,
float phi, float theta, float psi,
float p, float q, float r);
float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
};
/**
* Waypoint Guidance block
*/
class __EXPORT BlockWaypointGuidance : public SuperBlock
{
private:
BlockLimitSym _xtYawLimit;
BlockP _xt2Yaw;
float _psiCmd;
public:
BlockWaypointGuidance(SuperBlock *parent, const char *name);
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd);
float getPsiCmd() { return _psiCmd; }
};
/**
* UorbEnabledAutopilot
*/
class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
{
protected:
// subscriptions
UOrbSubscription<vehicle_attitude_s> _att;
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
UOrbSubscription<vehicle_global_position_s> _pos;
UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd;
UOrbSubscription<manual_control_setpoint_s> _manual;
UOrbSubscription<vehicle_status_s> _status;
UOrbSubscription<parameter_update_s> _param_update;
// publications
UOrbPublication<actuator_controls_s> _actuators;
public:
BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
virtual ~BlockUorbEnabledAutopilot();
};
/**
* Multi-mode Autopilot
*/
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
{
private:
BlockStabilization _stabilization;
BlockBacksideAutopilot _backsideAutopilot;
BlockWaypointGuidance _guide;
BlockParam<float> _vCmd;
struct pollfd _attPoll;
vehicle_global_position_setpoint_s _lastPosCmd;
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
uint64_t _timeStamp;
public:
BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name);
void update();
virtual ~BlockMultiModeBacksideAutopilot();
};
} // namespace fixedwing
} // namespace control

22
apps/controllib/test_params.c Normal file
View File

@ -0,0 +1,22 @@
#include <systemlib/param/param.h>
// WARNING:
// do not changes these unless
// you want to recompute the
// answers for all of the unit tests
PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f);
PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f);
PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f);
PARAM_DEFINE_FLOAT(TEST_HP, 10.0f);
PARAM_DEFINE_FLOAT(TEST_LP, 10.0f);
PARAM_DEFINE_FLOAT(TEST_P, 0.2f);
PARAM_DEFINE_FLOAT(TEST_I, 0.1f);
PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f);
PARAM_DEFINE_FLOAT(TEST_D, 0.01f);
PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f);
PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f);
PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f);

42
apps/drivers/blinkm/Makefile Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# BlinkM I2C LED driver
#
APPNAME = blinkm
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk

886
apps/drivers/blinkm/blinkm.cpp Normal file
View File

@ -0,0 +1,886 @@
/****************************************************************************
*
* 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 blinkm.cpp
*
* Driver for the BlinkM LED controller connected via I2C.
*
* Connect the BlinkM to I2C3 and put the following line to the rc startup-script:
* blinkm start
*
* To start the system monitor put in the next line after the blinkm start:
* blinkm systemmonitor
*
*
* Description:
* After startup, the Application checked how many lipo cells are connected to the System.
* The recognized number off cells, will be blinked 5 times in purple color.
* 2 Cells = 2 blinks
* ...
* 5 Cells = 5 blinks
* Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
*
* System disarmed:
* The BlinkM should lit solid red.
*
* System armed:
* One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
*
* X-X-X-X-_-_-_-_-_-_-
* -------------------------
* G G G M
* P P P O
* S S S D
* E
*
* (X = on, _=off)
*
* The first 3 blinks indicates the status of the GPS-Signal (red):
* 0-4 satellites = X-X-X-X-_-_-_-_-_-_-
* 5 satellites = X-X-_-X-_-_-_-_-_-_-
* 6 satellites = X-_-_-X-_-_-_-_-_-_-
* >=7 satellites = _-_-_-X-_-_-_-_-_-_-
* If no GPS is found the first 3 blinks are white
*
* The fourth Blink indicates the Flightmode:
* MANUAL : amber
* STABILIZED : yellow
* HOLD : blue
* AUTO : green
*
* Battery Warning (low Battery Level):
* Continuously blinking in yellow X-X-X-X-X-X-X-X-X-X
*
* Battery Alert (critical Battery Level)
* Continuously blinking in red X-X-X-X-X-X-X-X-X-X
*
* General Error (no uOrb Data)
* Continuously blinking in white X-X-X-X-X-X-X-X-X-X
*
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <ctype.h>
#include <drivers/drv_blinkm.h>
#include <nuttx/wqueue.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_gps_position.h>
static const float MAX_CELL_VOLTAGE = 4.3f;
static const int LED_ONTIME = 120;
static const int LED_OFFTIME = 120;
static const int LED_BLINK = 1;
static const int LED_NOBLINK = 0;
class BlinkM : public device::I2C
{
public:
BlinkM(int bus);
~BlinkM();
virtual int init();
virtual int probe();
virtual int setMode(int mode);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
static const char *script_names[];
private:
enum ScriptID {
USER = 0,
RGB,
WHITE_FLASH,
RED_FLASH,
GREEN_FLASH,
BLUE_FLASH,
CYAN_FLASH,
MAGENTA_FLASH,
YELLOW_FLASH,
BLACK,
HUE_CYCLE,
MOOD_LIGHT,
VIRTUAL_CANDLE,
WATER_REFLECTIONS,
OLD_NEON,
THE_SEASONS,
THUNDERSTORM,
STOP_LIGHT,
MORSE_CODE
};
enum ledColors {
LED_OFF,
LED_RED,
LED_YELLOW,
LED_PURPLE,
LED_GREEN,
LED_BLUE,
LED_WHITE,
LED_AMBER
};
work_s _work;
int led_color_1;
int led_color_2;
int led_color_3;
int led_color_4;
int led_color_5;
int led_color_6;
int led_color_7;
int led_color_8;
int led_blink;
bool systemstate_run;
void setLEDColor(int ledcolor);
static void led_trampoline(void *arg);
void led();
int set_rgb(uint8_t r, uint8_t g, uint8_t b);
int fade_rgb(uint8_t r, uint8_t g, uint8_t b);
int fade_hsb(uint8_t h, uint8_t s, uint8_t b);
int fade_rgb_random(uint8_t r, uint8_t g, uint8_t b);
int fade_hsb_random(uint8_t h, uint8_t s, uint8_t b);
int set_fade_speed(uint8_t s);
int play_script(uint8_t script_id);
int play_script(const char *script_name);
int stop_script();
int write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3);
int read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]);
int set_script(uint8_t length, uint8_t repeats);
int get_rgb(uint8_t &r, uint8_t &g, uint8_t &b);
int get_firmware_version(uint8_t version[2]);
};
/* for now, we only support one BlinkM */
namespace
{
BlinkM *g_blinkm;
}
/* list of script names, must match script ID numbers */
const char *BlinkM::script_names[] = {
"USER",
"RGB",
"WHITE_FLASH",
"RED_FLASH",
"GREEN_FLASH",
"BLUE_FLASH",
"CYAN_FLASH",
"MAGENTA_FLASH",
"YELLOW_FLASH",
"BLACK",
"HUE_CYCLE",
"MOOD_LIGHT",
"VIRTUAL_CANDLE",
"WATER_REFLECTIONS",
"OLD_NEON",
"THE_SEASONS",
"THUNDERSTORM",
"STOP_LIGHT",
"MORSE_CODE",
nullptr
};
extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
BlinkM::BlinkM(int bus) :
I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000),
led_color_1(LED_OFF),
led_color_2(LED_OFF),
led_color_3(LED_OFF),
led_color_4(LED_OFF),
led_color_5(LED_OFF),
led_color_6(LED_OFF),
led_color_7(LED_OFF),
led_color_8(LED_OFF),
led_blink(LED_NOBLINK),
systemstate_run(false)
{
memset(&_work, 0, sizeof(_work));
}
BlinkM::~BlinkM()
{
}
int
BlinkM::init()
{
int ret;
ret = I2C::init();
if (ret != OK) {
warnx("I2C init failed");
return ret;
}
stop_script();
set_rgb(0,0,0);
return OK;
}
int
BlinkM::setMode(int mode)
{
if(mode == 1) {
if(systemstate_run == false) {
stop_script();
set_rgb(0,0,0);
systemstate_run = true;
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
}
} else {
systemstate_run = false;
}
return OK;
}
int
BlinkM::probe()
{
uint8_t version[2];
int ret;
ret = get_firmware_version(version);
if (ret == OK)
log("found BlinkM firmware version %c%c", version[1], version[0]);
return ret;
}
int
BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = ENOTTY;
switch (cmd) {
case BLINKM_PLAY_SCRIPT_NAMED:
if (arg == 0) {
ret = EINVAL;
break;
}
ret = play_script((const char *)arg);
break;
case BLINKM_PLAY_SCRIPT:
ret = play_script(arg);
break;
case BLINKM_SET_USER_SCRIPT: {
if (arg == 0) {
ret = EINVAL;
break;
}
unsigned lines = 0;
const uint8_t *script = (const uint8_t *)arg;
while ((lines < 50) && (script[1] != 0)) {
ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]);
if (ret != OK)
break;
script += 5;
}
if (ret == OK)
ret = set_script(lines, 0);
break;
}
default:
break;
}
return ret;
}
void
BlinkM::led_trampoline(void *arg)
{
BlinkM *bm = (BlinkM *)arg;
bm->led();
}
void
BlinkM::led()
{
static int vehicle_status_sub_fd;
static int vehicle_gps_position_sub_fd;
static int num_of_cells = 0;
static int detected_cells_runcount = 0;
static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
static int t_led_blink = 0;
static int led_thread_runcount=0;
static int led_interval = 1000;
static int no_data_vehicle_status = 0;
static int no_data_vehicle_gps_position = 0;
static bool topic_initialized = false;
static bool detected_cells_blinked = false;
static bool led_thread_ready = true;
int num_of_used_sats = 0;
if(!topic_initialized) {
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 1000);
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
topic_initialized = true;
}
if(led_thread_ready == true) {
if(!detected_cells_blinked) {
if(num_of_cells > 0) {
t_led_color[0] = LED_PURPLE;
}
if(num_of_cells > 1) {
t_led_color[1] = LED_PURPLE;
}
if(num_of_cells > 2) {
t_led_color[2] = LED_PURPLE;
}
if(num_of_cells > 3) {
t_led_color[3] = LED_PURPLE;
}
if(num_of_cells > 4) {
t_led_color[4] = LED_PURPLE;
}
t_led_color[5] = LED_OFF;
t_led_color[6] = LED_OFF;
t_led_color[7] = LED_OFF;
t_led_blink = LED_BLINK;
} else {
t_led_color[0] = led_color_1;
t_led_color[1] = led_color_2;
t_led_color[2] = led_color_3;
t_led_color[3] = led_color_4;
t_led_color[4] = led_color_5;
t_led_color[5] = led_color_6;
t_led_color[6] = led_color_7;
t_led_color[7] = led_color_8;
t_led_blink = led_blink;
}
led_thread_ready = false;
}
if (led_thread_runcount & 1) {
if (t_led_blink)
setLEDColor(LED_OFF);
led_interval = LED_OFFTIME;
} else {
setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
led_interval = LED_ONTIME;
}
if (led_thread_runcount == 15) {
/* obtained data for the first file descriptor */
struct vehicle_status_s vehicle_status_raw;
struct vehicle_gps_position_s vehicle_gps_position_raw;
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
bool new_data_vehicle_status;
bool new_data_vehicle_gps_position;
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
if (new_data_vehicle_status) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
no_data_vehicle_status = 0;
} else {
no_data_vehicle_status++;
if(no_data_vehicle_status >= 3)
no_data_vehicle_status = 3;
}
orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
if (new_data_vehicle_gps_position) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
no_data_vehicle_gps_position = 0;
} else {
no_data_vehicle_gps_position++;
if(no_data_vehicle_gps_position >= 3)
no_data_vehicle_gps_position = 3;
}
/* get number of used satellites in navigation */
num_of_used_sats = 0;
//for(int satloop=0; satloop<20; satloop++) {
for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
num_of_used_sats++;
}
}
if(new_data_vehicle_status || no_data_vehicle_status < 3){
if(num_of_cells == 0) {
/* looking for lipo cells that are connected */
printf("<blinkm> checking cells\n");
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
}
printf("<blinkm> cells found:%u\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
/* LED Pattern for battery low warning */
led_color_1 = LED_YELLOW;
led_color_2 = LED_YELLOW;
led_color_3 = LED_YELLOW;
led_color_4 = LED_YELLOW;
led_color_5 = LED_YELLOW;
led_color_6 = LED_YELLOW;
led_color_7 = LED_YELLOW;
led_color_8 = LED_YELLOW;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
/* LED Pattern for battery critical alerting */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
led_color_3 = LED_RED;
led_color_4 = LED_RED;
led_color_5 = LED_RED;
led_color_6 = LED_RED;
led_color_7 = LED_RED;
led_color_8 = LED_RED;
led_blink = LED_BLINK;
} else {
/* no battery warnings here */
if(vehicle_status_raw.flag_system_armed == false) {
/* system not armed */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
led_color_3 = LED_RED;
led_color_4 = LED_RED;
led_color_5 = LED_RED;
led_color_6 = LED_RED;
led_color_7 = LED_RED;
led_color_8 = LED_RED;
led_blink = LED_NOBLINK;
} else {
/* armed system - initial led pattern */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
led_color_3 = LED_RED;
led_color_4 = LED_OFF;
led_color_5 = LED_OFF;
led_color_6 = LED_OFF;
led_color_7 = LED_OFF;
led_color_8 = LED_OFF;
led_blink = LED_BLINK;
/* handle 4th led - flightmode indicator */
switch((int)vehicle_status_raw.flight_mode) {
case VEHICLE_FLIGHT_MODE_MANUAL:
led_color_4 = LED_AMBER;
break;
case VEHICLE_FLIGHT_MODE_STAB:
led_color_4 = LED_YELLOW;
break;
case VEHICLE_FLIGHT_MODE_HOLD:
led_color_4 = LED_BLUE;
break;
case VEHICLE_FLIGHT_MODE_AUTO:
led_color_4 = LED_GREEN;
break;
}
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
/* handling used sat´s */
if(num_of_used_sats >= 7) {
led_color_1 = LED_OFF;
led_color_2 = LED_OFF;
led_color_3 = LED_OFF;
} else if(num_of_used_sats == 6) {
led_color_2 = LED_OFF;
led_color_3 = LED_OFF;
} else if(num_of_used_sats == 5) {
led_color_3 = LED_OFF;
}
} else {
/* no vehicle_gps_position data */
led_color_1 = LED_WHITE;
led_color_2 = LED_WHITE;
led_color_3 = LED_WHITE;
}
}
}
}
} else {
/* LED Pattern for general Error - no vehicle_status can retrieved */
led_color_1 = LED_WHITE;
led_color_2 = LED_WHITE;
led_color_3 = LED_WHITE;
led_color_4 = LED_WHITE;
led_color_5 = LED_WHITE;
led_color_6 = LED_WHITE;
led_color_7 = LED_WHITE;
led_color_8 = LED_WHITE;
led_blink = LED_BLINK;
}
/*
printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
vehicle_status_raw.voltage_battery,
vehicle_status_raw.flag_system_armed,
vehicle_status_raw.flight_mode,
num_of_cells,
no_data_vehicle_status,
no_data_vehicle_gps_position,
num_of_used_sats,
vehicle_gps_position_raw.fix_type,
vehicle_gps_position_raw.satellites_visible);
*/
led_thread_runcount=0;
led_thread_ready = true;
led_interval = LED_OFFTIME;
if(detected_cells_runcount < 4){
detected_cells_runcount++;
} else {
detected_cells_blinked = true;
}
} else {
led_thread_runcount++;
}
if(systemstate_run == true) {
/* re-queue ourselves to run again later */
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
} else {
stop_script();
set_rgb(0,0,0);
}
}
void BlinkM::setLEDColor(int ledcolor) {
switch (ledcolor) {
case LED_OFF: // off
set_rgb(0,0,0);
break;
case LED_RED: // red
set_rgb(255,0,0);
break;
case LED_YELLOW: // yellow
set_rgb(255,70,0);
break;
case LED_PURPLE: // purple
set_rgb(255,0,255);
break;
case LED_GREEN: // green
set_rgb(0,255,0);
break;
case LED_BLUE: // blue
set_rgb(0,0,255);
break;
case LED_WHITE: // white
set_rgb(255,255,255);
break;
case LED_AMBER: // amber
set_rgb(255,20,0);
break;
}
}
int
BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b)
{
const uint8_t msg[4] = { 'n', r, g, b };
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b)
{
const uint8_t msg[4] = { 'c', r, g, b };
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b)
{
const uint8_t msg[4] = { 'h', h, s, b };
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b)
{
const uint8_t msg[4] = { 'C', r, g, b };
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b)
{
const uint8_t msg[4] = { 'H', h, s, b };
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
BlinkM::set_fade_speed(uint8_t s)
{
const uint8_t msg[2] = { 'f', s };
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
BlinkM::play_script(uint8_t script_id)
{
const uint8_t msg[4] = { 'p', script_id, 0, 0 };
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
BlinkM::play_script(const char *script_name)
{
/* handle HTML colour encoding */
if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) {
char code[3];
uint8_t r, g, b;
code[2] = '\0';
code[0] = script_name[1];
code[1] = script_name[2];
r = strtol(code, 0, 16);
code[0] = script_name[3];
code[1] = script_name[4];
g = strtol(code, 0, 16);
code[0] = script_name[5];
code[1] = script_name[6];
b = strtol(code, 0, 16);
stop_script();
return set_rgb(r, g, b);
}
for (unsigned i = 0; script_names[i] != nullptr; i++)
if (!strcasecmp(script_name, script_names[i]))
return play_script(i);
return -1;
}
int
BlinkM::stop_script()
{
const uint8_t msg[1] = { 'o' };
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3)
{
const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 };
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4])
{
const uint8_t msg[3] = { 'R', 0, line };
uint8_t result[5];
int ret = transfer(msg, sizeof(msg), result, sizeof(result));
if (ret == OK) {
ticks = result[0];
cmd[0] = result[1];
cmd[1] = result[2];
cmd[2] = result[3];
cmd[3] = result[4];
}
return ret;
}
int
BlinkM::set_script(uint8_t len, uint8_t repeats)
{
const uint8_t msg[4] = { 'L', 0, len, repeats };
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b)
{
const uint8_t msg = 'g';
uint8_t result[3];
int ret = transfer(&msg, sizeof(msg), result, sizeof(result));
if (ret == OK) {
r = result[0];
g = result[1];
b = result[2];
}
return ret;
}
int
BlinkM::get_firmware_version(uint8_t version[2])
{
const uint8_t msg = 'Z';
return transfer(&msg, sizeof(msg), version, sizeof(version));
}
int
blinkm_main(int argc, char *argv[])
{
if (!strcmp(argv[1], "start")) {
if (g_blinkm != nullptr)
errx(1, "already started");
g_blinkm = new BlinkM(3);
if (g_blinkm == nullptr)
errx(1, "new failed");
if (OK != g_blinkm->init()) {
delete g_blinkm;
g_blinkm = nullptr;
errx(1, "init failed");
}
exit(0);
}
if (g_blinkm == nullptr)
errx(1, "not started");
if (!strcmp(argv[1], "systemstate")) {
g_blinkm->setMode(1);
exit(0);
}
if (!strcmp(argv[1], "ledoff")) {
g_blinkm->setMode(0);
exit(0);
}
if (!strcmp(argv[1], "list")) {
for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++)
fprintf(stderr, " %s\n", BlinkM::script_names[i]);
fprintf(stderr, " <html color number>\n");
exit(0);
}
/* things that require access to the device */
int fd = open(BLINKM_DEVICE_PATH, 0);
if (fd < 0)
err(1, "can't open BlinkM device");
g_blinkm->setMode(0);
if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
exit(0);
errx(1, "missing command, try 'start', 'systemstate', 'ledoff', 'list' or a script name.");
}

139
apps/drivers/boards/px4fmu/px4fmu_adc.c
View File

@ -1,139 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file px4fmu_adc.c
*
* Board-specific ADC functions.
*/
/************************************************************************************
* Included Files
************************************************************************************/
#include <nuttx/config.h>
#include <errno.h>
#include <debug.h>
#include <stdio.h>
#include <nuttx/analog/adc.h>
#include <arch/board/board.h>
#include "chip.h"
#include "up_arch.h"
#include "stm32_adc.h"
#include "px4fmu_internal.h"
#define ADC3_NCHANNELS 4
/************************************************************************************
* Private Data
************************************************************************************/
/* The PX4FMU board has four ADC channels: ADC323 IN10-13
*/
/* Identifying number of each ADC channel: Variable Resistor. */
#ifdef CONFIG_STM32_ADC3
static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11};// , 12, 13}; ADC12 and 13 are used by MPU on v1.5 boards
/* Configurations of pins used byte each ADC channels */
static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, GPIO_ADC3_IN12, GPIO_ADC3_IN13};
#endif
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
/************************************************************************************
* Name: adc_devinit
*
* Description:
* All STM32 architectures must provide the following interface to work with
* examples/adc.
*
************************************************************************************/
int adc_devinit(void)
{
static bool initialized = false;
struct adc_dev_s *adc[ADC3_NCHANNELS];
int ret;
int i;
/* Check if we have already initialized */
if (!initialized) {
char name[11];
for (i = 0; i < ADC3_NCHANNELS; i++) {
stm32_configgpio(g_pinlist[i]);
}
for (i = 0; i < 1; i++) {
/* Configure the pins as analog inputs for the selected channels */
//stm32_configgpio(g_pinlist[i]);
/* Call stm32_adcinitialize() to get an instance of the ADC interface */
//multiple channels only supported with dma!
adc[i] = stm32_adcinitialize(3, (g_chanlist), 4);
if (adc == NULL) {
adbg("ERROR: Failed to get ADC interface\n");
return -ENODEV;
}
/* Register the ADC driver at "/dev/adc0" */
sprintf(name, "/dev/adc%d", i);
ret = adc_register(name, adc[i]);
if (ret < 0) {
adbg("adc_register failed for adc %s: %d\n", name, ret);
return ret;
}
}
/* Now we are initialized */
initialized = true;
}
return OK;
}

59
apps/drivers/boards/px4fmu/px4fmu_init.c
View File

@ -95,8 +95,6 @@
* Protected Functions
****************************************************************************/
extern int adc_devinit(void);
/****************************************************************************
* Public Functions
****************************************************************************/
@ -150,9 +148,7 @@ __EXPORT int nsh_archinitialize(void)
int result;
/* configure the high-resolution time/callout interface */
#ifdef CONFIG_HRT_TIMER
hrt_init();
#endif
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
@ -160,27 +156,21 @@ __EXPORT int nsh_archinitialize(void)
#endif
/* set up the serial DMA polling */
#ifdef SERIAL_HAVE_DMA
{
static struct hrt_call serial_dma_call;
struct timespec ts;
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
}
#endif
message("\r\n");
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
// initial LED state
drv_led_start();
@ -209,8 +199,7 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Successfully initialized SPI port 1\r\n");
#if defined(CONFIG_STM32_SPI3)
/* Get the SPI port */
/* Get the SPI port for the microSD slot */
message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
@ -233,23 +222,11 @@ __EXPORT int nsh_archinitialize(void)
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
#endif /* SPI3 */
#ifdef CONFIG_ADC
int adc_state = adc_devinit();
if (adc_state != OK) {
/* Try again */
adc_state = adc_devinit();
if (adc_state != OK) {
/* Give up */
message("[boot] FAILED adc_devinit: %d\n", adc_state);
return -ENODEV;
}
}
#endif
stm32_configgpio(GPIO_ADC1_IN10);
stm32_configgpio(GPIO_ADC1_IN11);
stm32_configgpio(GPIO_ADC1_IN12);
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
return OK;
}

3
apps/drivers/boards/px4io/px4io_init.c
View File

@ -92,4 +92,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_ACC_OC_DETECT);
stm32_configgpio(GPIO_SERVO_OC_DETECT);
stm32_configgpio(GPIO_BTN_SAFETY);
stm32_configgpio(GPIO_ADC_VBATT);
stm32_configgpio(GPIO_ADC_IN5);
}

27
apps/drivers/boards/px4io/px4io_internal.h
View File

@ -61,28 +61,6 @@
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
/* R/C in/out channels **************************************************************/
/* XXX just GPIOs for now - eventually timer pins */
#define GPIO_CH1_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0)
#define GPIO_CH2_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1)
#define GPIO_CH3_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8)
#define GPIO_CH4_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9)
#define GPIO_CH5_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6)
#define GPIO_CH6_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7)
#define GPIO_CH7_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0)
#define GPIO_CH8_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1)
#define GPIO_CH1_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
#define GPIO_CH2_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
#define GPIO_CH3_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
#define GPIO_CH4_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
#define GPIO_CH5_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
#define GPIO_CH6_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
#define GPIO_CH7_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
#define GPIO_CH8_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
/* Safety switch button *************************************************************/
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
@ -98,3 +76,8 @@
#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13)
#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
/* Analog inputs ********************************************************************/
#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)

4
apps/drivers/device/i2c.cpp
View File

@ -115,7 +115,7 @@ I2C::probe()
}
int
I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
{
struct i2c_msg_s msgv[2];
unsigned msgs;
@ -130,7 +130,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
if (send_len > 0) {
msgv[msgs].addr = _address;
msgv[msgs].flags = 0;
msgv[msgs].buffer = send;
msgv[msgs].buffer = const_cast<uint8_t *>(send);
msgv[msgs].length = send_len;
msgs++;
}

2
apps/drivers/device/i2c.h
View File

@ -97,7 +97,7 @@ protected:
* @return OK if the transfer was successful, -errno
* otherwise.
*/
int transfer(uint8_t *send, unsigned send_len,
int transfer(const uint8_t *send, unsigned send_len,
uint8_t *recv, unsigned recv_len);
/**

11
apps/drivers/device/spi.cpp
View File

@ -86,7 +86,7 @@ SPI::init()
{
int ret = OK;
// attach to the spi bus
/* attach to the spi bus */
if (_dev == nullptr)
_dev = up_spiinitialize(_bus);
@ -96,7 +96,10 @@ SPI::init()
goto out;
}
// call the probe function to check whether the device is present
/* deselect device to ensure high to low transition of pin select */
SPI_SELECT(_dev, _device, false);
/* call the probe function to check whether the device is present */
ret = probe();
if (ret != OK) {
@ -104,7 +107,7 @@ SPI::init()
goto out;
}
// do base class init, which will create the device node, etc.
/* do base class init, which will create the device node, etc. */
ret = CDev::init();
if (ret != OK) {
@ -112,7 +115,7 @@ SPI::init()
goto out;
}
// tell the workd where we are
/* tell the workd where we are */
log("on SPI bus %d at %d", _bus, _device);
out:

52
apps/drivers/drv_adc.h Normal file
View File

@ -0,0 +1,52 @@
/****************************************************************************
*
* 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 drv_adc.h
*
* ADC driver interface.
*
* This defines additional operations over and above the standard NuttX
* ADC API.
*/
#pragma once
#include <stdint.h>
#include <sys/ioctl.h>
#define ADC_DEVICE_PATH "/dev/adc0"
/*
* ioctl definitions
*/

69
apps/drivers/drv_blinkm.h Normal file
View File

@ -0,0 +1,69 @@
/****************************************************************************
*
* 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 drv_blinkm.h
*
* BlinkM driver API
*
* This could probably become a more generalised API for multi-colour LED
* driver systems, or be merged with the generic LED driver.
*/
#pragma once
#include <stdint.h>
#include <sys/ioctl.h>
#define BLINKM_DEVICE_PATH "/dev/blinkm"
/*
* ioctl() definitions
*/
#define _BLINKMIOCBASE (0x2900)
#define _BLINKMIOC(_n) (_IOC(_BLINKMIOCBASE, _n))
/** play the named script in *(char *)arg, repeating forever */
#define BLINKM_PLAY_SCRIPT_NAMED _BLINKMIOC(1)
/** play the numbered script in (arg), repeating forever */
#define BLINKM_PLAY_SCRIPT _BLINKMIOC(2)
/**
* Set the user script; (arg) is a pointer to an array of script lines,
* where each line is an array of four bytes giving <duration>, <command>, arg[0-2]
*
* The script is terminated by a zero command.
*/
#define BLINKM_SET_USER_SCRIPT _BLINKMIOC(3)

23
apps/drivers/drv_mixer.h
View File

@ -100,28 +100,13 @@ struct mixer_simple_s {
*/
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
/** multirotor output definition */
struct mixer_rotor_output_s {
float angle; /**< rotor angle clockwise from forward in radians */
float distance; /**< motor distance from centre in arbitrary units */
};
/** multirotor mixer */
struct mixer_multirotor_s {
uint8_t rotor_count;
struct mixer_control_s controls[4]; /**< controls are roll, pitch, yaw, thrust */
struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */
};
/* _MIXERIOC(3) was deprecated */
/* _MIXERIOC(4) was deprecated */
/**
* Add a multirotor mixer in (struct mixer_multirotor_s *)arg
* Add mixer(s) from the buffer in (const char *)arg
*/
#define MIXERIOCADDMULTIROTOR _MIXERIOC(3)
/**
* Add mixers(s) from a the file in (const char *)arg
*/
#define MIXERIOCLOADFILE _MIXERIOC(4)
#define MIXERIOCLOADBUF _MIXERIOC(5)
/*
* XXX Thoughts for additional operations:

5
apps/drivers/drv_pwm_output.h
View File

@ -95,7 +95,7 @@ ORB_DECLARE(output_pwm);
* Note that ioctls and ObjDev updates should not be mixed, as the
* behaviour of the system in this case is not defined.
*/
#define _PWM_SERVO_BASE 0x2700
#define _PWM_SERVO_BASE 0x2a00
/** arm all servo outputs handle by this driver */
#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0)
@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
/** set update rate in Hz */
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

58
apps/drivers/hil/hil.cpp
View File

@ -68,11 +68,11 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
// #include <drivers/boards/HIL/HIL_internal.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <systemlib/systemlib.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
@ -382,7 +382,6 @@ HIL::task_main()
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}
@ -396,16 +395,27 @@ HIL::task_main()
if (_mixers != nullptr) {
/* do mixing */
_mixers->mix(&outputs.output[0], num_outputs);
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
/* output to the servo */
// up_pwm_servo_set(i, outputs.output[i]);
/* last resort: catch NaN, INF and out-of-band errors */
if (i < (unsigned)outputs.noutputs &&
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = 900;
}
}
/* and publish for anyone that cares to see */
@ -419,9 +429,6 @@ HIL::task_main()
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
/* update PWM servo armed status */
// up_pwm_servo_arm(aa.armed);
}
}
@ -503,6 +510,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
// up_pwm_servo_arm(false);
break;
case PWM_SERVO_SET_UPDATE_RATE:
g_hil->set_pwm_rate(arg);
break;
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@ -577,26 +588,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
} else {
debug("loading mixers from %s", path);
ret = _mixers->load_from_file(path);
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
@ -605,10 +609,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
default:
ret = -ENOTTY;
break;

51
apps/drivers/hmc5883/hmc5883.cpp
View File

@ -288,6 +288,20 @@ private:
*/
int check_calibration();
/**
* Check the current scale calibration
*
* @return 0 if scale calibration is ok, 1 else
*/
int check_scale();
/**
* Check the current offset calibration
*
* @return 0 if offset calibration is ok, 1 else
*/
int check_offset();
};
/* helper macro for handling report buffer indices */
@ -1016,11 +1030,11 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
out:
if (ret == OK) {
if (!check_calibration()) {
if (!check_scale()) {
warnx("mag scale calibration successfully finished.");
} else {
warnx("mag scale calibration finished with invalid results.");
ret == ERROR;
ret = ERROR;
}
} else {
@ -1030,9 +1044,9 @@ out:
return ret;
}
int HMC5883::check_calibration()
int HMC5883::check_scale()
{
bool scale_valid, offset_valid;
bool scale_valid;
if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) &&
(-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) &&
@ -1043,6 +1057,14 @@ int HMC5883::check_calibration()
scale_valid = true;
}
/* return 0 if calibrated, 1 else */
return !scale_valid;
}
int HMC5883::check_offset()
{
bool offset_valid;
if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
@ -1052,17 +1074,36 @@ int HMC5883::check_calibration()
offset_valid = true;
}
/* return 0 if calibrated, 1 else */
return !offset_valid;
}
int HMC5883::check_calibration()
{
bool offset_valid = (check_offset() == OK);
bool scale_valid = (check_scale() == OK);
if (_calibrated != (offset_valid && scale_valid)) {
warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid");
_calibrated = (offset_valid && scale_valid);
// XXX Change advertisement
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
_calibrated,
SUBSYSTEM_TYPE_MAG};
orb_advert_t pub = orb_advertise(ORB_ID(subsystem_info), &info);
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
/* return 0 if calibrated, 1 else */

8
apps/drivers/l3gd20/l3gd20.cpp
View File

@ -151,7 +151,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI
{
public:
L3GD20(int bus, spi_dev_e device);
L3GD20(int bus, const char* path, spi_dev_e device);
~L3GD20();
virtual int init();
@ -265,8 +265,8 @@ private:
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
L3GD20::L3GD20(int bus, spi_dev_e device) :
SPI("L3GD20", GYRO_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
_call_interval(0),
_num_reports(0),
_next_report(0),
@ -745,7 +745,7 @@ start()
errx(1, "already started");
/* create the driver */
g_dev = new L3GD20(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_GYRO);
g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
if (g_dev == nullptr)
goto fail;

12
apps/drivers/mpu6000/mpu6000.cpp
View File

@ -446,8 +446,12 @@ MPU6000::init()
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000);
/* do CDev init for the gyro device node */
ret = _gyro->init();
/* do CDev init for the gyro device node, keep it optional */
int gyro_ret = _gyro->init();
if (gyro_ret != OK) {
_gyro_topic = -1;
}
return ret;
}
@ -938,7 +942,9 @@ MPU6000::measure()
/* and publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
if (_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
}
/* stop measuring */
perf_end(_sample_perf);

166
apps/drivers/px4fmu/fmu.cpp
View File

@ -58,12 +58,15 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/boards/px4fmu/px4fmu_internal.h>
#include <drivers/drv_hrt.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
@ -96,6 +99,7 @@ private:
int _t_actuators;
int _t_armed;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
bool _primary_pwm_device;
@ -110,9 +114,9 @@ private:
void task_main() __attribute__((noreturn));
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
uint8_t control_group,
uint8_t control_index,
float &input);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
@ -161,6 +165,7 @@ PX4FMU::PX4FMU() :
_t_actuators(-1),
_t_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
@ -177,6 +182,7 @@ PX4FMU::~PX4FMU()
_task_should_exit = true;
unsigned i = 10;
do {
/* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000);
@ -212,6 +218,7 @@ PX4FMU::init()
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
@ -245,7 +252,7 @@ PX4FMU::task_main_trampoline(int argc, char *argv[])
int
PX4FMU::set_mode(Mode mode)
{
/*
/*
* Configure for PWM output.
*
* Note that regardless of the configured mode, the task is always
@ -279,6 +286,7 @@ PX4FMU::set_mode(Mode mode)
default:
return -EINVAL;
}
_mode = mode;
return OK;
}
@ -315,6 +323,13 @@ PX4FMU::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
@ -331,8 +346,18 @@ PX4FMU::task_main()
/* handle update rate changes */
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
if (update_rate_in_ms < 2)
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
_update_rate = 500;
}
/* reject slower than 50 Hz updates */
if (update_rate_in_ms > 20) {
update_rate_in_ms = 20;
_update_rate = 50;
}
orb_set_interval(_t_actuators, update_rate_in_ms);
up_pwm_servo_set_rate(_update_rate);
_current_update_rate = _update_rate;
@ -358,20 +383,39 @@ PX4FMU::task_main()
if (_mixers != nullptr) {
/* do mixing */
_mixers->mix(&outputs.output[0], num_outputs);
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
// XXX output actual limited values
memcpy(&controls_effective, &_controls, sizeof(controls_effective));
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
/* last resort: catch NaN, INF and out-of-band errors */
if (i < outputs.noutputs &&
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = 900;
}
/* output to the servo */
up_pwm_servo_set(i, outputs.output[i]);
}
/* and publish for anyone that cares to see */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
}
}
@ -382,12 +426,13 @@ PX4FMU::task_main()
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
/* update PWM servo armed status */
up_pwm_servo_arm(aa.armed);
/* update PWM servo armed status if armed and not locked down */
up_pwm_servo_arm(aa.armed && !aa.lockdown);
}
}
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_armed);
/* make sure servos are off */
@ -404,9 +449,9 @@ PX4FMU::task_main()
int
PX4FMU::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
@ -424,15 +469,17 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
/* try it as a GPIO ioctl first */
ret = gpio_ioctl(filp, cmd, arg);
if (ret != -ENOTTY)
return ret;
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
switch(_mode) {
switch (_mode) {
case MODE_2PWM:
case MODE_4PWM:
ret = pwm_ioctl(filp, cmd, arg);
break;
default:
debug("not in a PWM mode");
break;
@ -462,6 +509,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(false);
break;
case PWM_SERVO_SET_UPDATE_RATE:
set_pwm_rate(arg);
break;
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@ -492,7 +543,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
channel = cmd - PWM_SERVO_SET(0);
channel = cmd - PWM_SERVO_GET(0);
*(servo_position_t *)arg = up_pwm_servo_get(channel);
break;
}
@ -536,26 +587,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
} else {
debug("loading mixers from %s", path);
ret = _mixers->load_from_file(path);
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
@ -564,7 +608,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
@ -582,7 +625,7 @@ void
PX4FMU::gpio_reset(void)
{
/*
* Setup default GPIO config - all pins as GPIOs, GPIO driver chip
* Setup default GPIO config - all pins as GPIOs, GPIO driver chip
* to input mode.
*/
for (unsigned i = 0; i < _ngpio; i++)
@ -609,17 +652,20 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
/* configure selected GPIOs as required */
for (unsigned i = 0; i < _ngpio; i++) {
if (gpios & (1<<i)) {
if (gpios & (1 << i)) {
switch (function) {
case GPIO_SET_INPUT:
stm32_configgpio(_gpio_tab[i].input);
break;
case GPIO_SET_OUTPUT:
stm32_configgpio(_gpio_tab[i].output);
break;
case GPIO_SET_ALT_1:
if (_gpio_tab[i].alt != 0)
stm32_configgpio(_gpio_tab[i].alt);
break;
}
}
@ -636,7 +682,7 @@ PX4FMU::gpio_write(uint32_t gpios, int function)
int value = (function == GPIO_SET) ? 1 : 0;
for (unsigned i = 0; i < _ngpio; i++)
if (gpios & (1<<i))
if (gpios & (1 << i))
stm32_gpiowrite(_gpio_tab[i].output, value);
}
@ -660,7 +706,7 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
lock();
switch (cmd) {
case GPIO_RESET:
gpio_reset();
break;
@ -762,6 +808,7 @@ fmu_new_mode(PortMode new_mode, int update_rate)
/* (re)set the PWM output mode */
g_fmu->set_mode(servo_mode);
if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
g_fmu->set_pwm_rate(update_rate);
@ -800,13 +847,18 @@ test(void)
fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
if (fd < 0) {
puts("open fail");
exit(1);
}
if (fd < 0)
errx(1, "open fail");
ioctl(fd, PWM_SERVO_ARM, 0);
ioctl(fd, PWM_SERVO_SET(0), 1000);
if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed");
if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
close(fd);
@ -816,10 +868,8 @@ test(void)
void
fake(int argc, char *argv[])
{
if (argc < 5) {
puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
exit(1);
}
if (argc < 5)
errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
actuator_controls_s ac;
@ -833,10 +883,18 @@ fake(int argc, char *argv[])
orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
if (handle < 0) {
puts("advertise failed");
exit(1);
}
if (handle < 0)
errx(1, "advertise failed");
actuator_armed_s aa;
aa.armed = true;
aa.lockdown = false;
handle = orb_advertise(ORB_ID(actuator_armed), &aa);
if (handle < 0)
errx(1, "advertise failed 2");
exit(0);
}
@ -890,15 +948,17 @@ fmu_main(int argc, char *argv[])
if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) {
if (argc > i + 1) {
pwm_update_rate_in_hz = atoi(argv[i + 1]);
} else {
fprintf(stderr, "missing argument for pwm update rate (-u)\n");
errx(1, "missing argument for pwm update rate (-u)");
return 1;
}
} else {
fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n");
errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio");
}
}
}
}
/* was a new mode set? */
if (new_mode != PORT_MODE_UNSET) {
@ -915,5 +975,5 @@ fmu_main(int argc, char *argv[])
fprintf(stderr, "FMU: unrecognised command, try:\n");
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
return -EINVAL;
exit(1);
}

572
apps/drivers/px4io/px4io.cpp
View File

@ -37,7 +37,6 @@
*
* PX4IO is connected via serial (or possibly some other interface at a later
* point).
*/
#include <nuttx/config.h>
@ -54,13 +53,16 @@
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <math.h>
#include <arch/board/board.h>
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
@ -69,10 +71,15 @@
#include <systemlib/hx_stream.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/param/param.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/battery_status.h>
#include <px4io/protocol.h>
#include "uploader.h"
@ -88,42 +95,59 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
void set_rx_mode(unsigned mode);
/**
* Set the PWM via serial update rate
* @warning this directly affects CPU load
*/
int set_pwm_rate(int hz);
bool dump_one;
private:
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
// XXX
static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS;
unsigned _update_rate; ///< serial send rate in Hz
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
int _task; ///< worker task
volatile int _task; ///< worker task
volatile bool _task_should_exit;
volatile bool _connected; ///< true once we have received a valid frame
int _t_actuators; ///< actuator output topic
actuator_controls_s _controls; ///< actuator outputs
orb_advert_t _t_actuators_effective; ///< effective actuator controls topic
actuator_controls_effective_s _controls_effective; ///< effective controls
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
int _t_vstatus; ///< system / vehicle status
vehicle_status_s _vstatus; ///< overall system state
orb_advert_t _to_input_rc; ///< rc inputs from io
rc_input_values _input_rc; ///< rc input values
orb_advert_t _to_battery; ///< battery status / voltage
battery_status_s _battery_status;///< battery status data
orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
MixerGroup *_mixers; ///< loaded mixers
const char *volatile _mix_buf; ///< mixer text buffer
volatile unsigned _mix_buf_len; ///< size of the mixer text buffer
bool _primary_pwm_device; ///< true if we are the default PWM output
uint32_t _relays; ///< state of the PX4IO relays, one bit per relay
volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work?
bool _send_needed; ///< If true, we need to send a packet to IO
bool _config_needed; ///< if true, we need to set a config update to IO
uint8_t _rx_mode; ///< the current RX mode on IO
/**
* Trampoline to the worker task
*/
@ -159,6 +183,11 @@ private:
*/
void config_send();
/**
* Send a buffer containing mixer text to PX4IO
*/
int mixer_send(const char *buf, unsigned buflen);
/**
* Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing.
@ -179,20 +208,25 @@ PX4IO *g_dev;
PX4IO::PX4IO() :
CDev("px4io", "/dev/px4io"),
dump_one(false),
_update_rate(50),
_serial_fd(-1),
_io_stream(nullptr),
_task(-1),
_task_should_exit(false),
_connected(false),
_t_actuators(-1),
_t_actuators_effective(-1),
_t_armed(-1),
_t_vstatus(-1),
_t_outputs(-1),
_mixers(nullptr),
_mix_buf(nullptr),
_mix_buf_len(0),
_primary_pwm_device(false),
_relays(0),
_switch_armed(false),
_send_needed(false),
_config_needed(false),
_rx_mode(RX_MODE_PPM_ONLY)
_config_needed(true)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@ -202,33 +236,18 @@ PX4IO::PX4IO() :
PX4IO::~PX4IO()
{
if (_task != -1) {
/* tell the task we want it to go away */
_task_should_exit = true;
/* tell the task we want it to go away */
_task_should_exit = true;
/* spin waiting for the thread to stop */
unsigned i = 10;
do {
/* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000);
/* if we have given up, kill it */
if (--i == 0) {
task_delete(_task);
break;
}
} while (_task != -1);
/* spin waiting for the task to stop */
for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
/* give it another 100ms */
usleep(100000);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
task_delete(_task);
g_dev = nullptr;
}
@ -255,7 +274,8 @@ PX4IO::init()
}
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@ -267,16 +287,30 @@ PX4IO::init()
debug("PX4IO connected");
break;
}
usleep(100000);
}
if (!_connected) {
/* error here will result in everything being torn down */
log("PX4IO not responding");
return -EIO;
}
return OK;
}
int
PX4IO::set_pwm_rate(int hz)
{
if (hz > 0 && hz <= 400) {
_update_rate = hz;
return OK;
} else {
return -EINVAL;
}
}
void
PX4IO::task_main_trampoline(int argc, char *argv[])
{
@ -287,6 +321,7 @@ void
PX4IO::task_main()
{
log("starting");
unsigned update_rate_in_ms;
/* open the serial port */
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
@ -296,12 +331,24 @@ PX4IO::task_main()
goto out;
}
/* 115200bps, no parity, one stop bit */
{
struct termios t;
tcgetattr(_serial_fd, &t);
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(_serial_fd, TCSANOW, &t);
}
/* protocol stream */
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
if (_io_stream == nullptr) {
log("failed to allocate HX protocol stream");
goto out;
}
hx_stream_set_counters(_io_stream,
perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
perf_alloc(PC_COUNT, "PX4IO frames received"),
@ -314,12 +361,20 @@ PX4IO::task_main()
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
//int update_rate_in_ms = int(1000 / _update_rate);
orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */
update_rate_in_ms = 1000 / _update_rate;
orb_set_interval(_t_actuators, update_rate_in_ms);
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
/* advertise the limited control inputs */
memset(&_controls_effective, 0, sizeof(_controls_effective));
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1),
&_controls_effective);
/* advertise the mixed control outputs */
memset(&_outputs, 0, sizeof(_outputs));
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
@ -329,27 +384,37 @@ PX4IO::task_main()
memset(&_input_rc, 0, sizeof(_input_rc));
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
/* do not advertise the battery status until its clear that a battery is connected */
memset(&_battery_status, 0, sizeof(_battery_status));
_to_battery = -1;
/* poll descriptor */
pollfd fds[3];
pollfd fds[4];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
fds[1].fd = _t_actuators;
fds[1].events = POLLIN;
fds[2].fd = _t_armed;
fds[2].events = POLLIN;
fds[3].fd = _t_vstatus;
fds[3].events = POLLIN;
log("ready");
debug("ready");
/* lock against the ioctl handler */
lock();
/* loop handling received serial bytes */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
unlock();
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
lock();
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}
@ -362,37 +427,32 @@ PX4IO::task_main()
if (fds[0].revents & POLLIN)
io_recv();
/* if we have new data from the ORB, go handle it */
/* if we have new control data from the ORB, handle it */
if (fds[1].revents & POLLIN) {
/* get controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* mix */
if (_mixers != nullptr) {
/* XXX is this the right count? */
_mixers->mix(&_outputs.output[0], _max_actuators);
/* scale controls to PWM (temporary measure) */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _controls.control[i]);
/* convert to PWM values */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
/* and flag for update */
_send_needed = true;
}
}
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
/* and flag for update */
_send_needed = true;
}
}
/* send an update to IO if required */
if (_send_needed) {
_send_needed = false;
io_send();
/* if we have an arming state update, handle it */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
_send_needed = true;
}
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus);
_send_needed = true;
}
}
/* send a config packet to IO if required */
@ -400,12 +460,38 @@ PX4IO::task_main()
_config_needed = false;
config_send();
}
/* send a mixer update if needed */
if (_mix_buf != nullptr) {
mixer_send(_mix_buf, _mix_buf_len);
/* clear the buffer record so the ioctl handler knows we're done */
_mix_buf = nullptr;
_mix_buf_len = 0;
}
/* send an update to IO if required */
if (_send_needed) {
_send_needed = false;
io_send();
}
}
unlock();
out:
debug("exiting");
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
::close(_serial_fd);
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
@ -452,36 +538,70 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{
const px4io_report *rep = (const px4io_report *)buffer;
lock();
// lock();
/* sanity-check the received frame size */
if (bytes_received != sizeof(px4io_report)) {
debug("got %u expected %u", bytes_received, sizeof(px4io_report));
goto out;
}
if (rep->i2f_magic != I2F_MAGIC) {
debug("bad magic");
goto out;
}
_connected = true;
/* publish raw rc channel values from IO */
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
for (int i = 0; i < rep->channel_count; i++)
{
_input_rc.values[i] = rep->rc_channel[i];
}
/* publish raw rc channel values from IO if valid channels are present */
if (rep->channel_count > 0) {
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
for (int i = 0; i < rep->channel_count; i++) {
_input_rc.values[i] = rep->rc_channel[i];
}
orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
}
/* remember the latched arming switch state */
_switch_armed = rep->armed;
/* publish battery information */
/* only publish if battery has a valid minimum voltage */
if (rep->battery_mv > 3300) {
_battery_status.timestamp = hrt_absolute_time();
_battery_status.voltage_v = rep->battery_mv / 1000.0f;
/* current and discharge are unknown */
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* announce the battery voltage if needed, just publish else */
if (_to_battery > 0) {
orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status);
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
}
_send_needed = true;
/* if monitoring, dump the received info */
if (dump_one) {
dump_one = false;
printf("IO: %s armed ", rep->armed ? "" : "not");
for (unsigned i = 0; i < rep->channel_count; i++)
printf("%d: %d ", i, rep->rc_channel[i]);
printf("\n");
}
out:
unlock();
// unlock();
return;
}
void
@ -493,17 +613,29 @@ PX4IO::io_send()
cmd.f2i_magic = F2I_MAGIC;
/* set outputs */
for (unsigned i = 0; i < _max_actuators; i++)
cmd.servo_command[i] = _outputs.output[i];
for (unsigned i = 0; i < _max_actuators; i++) {
cmd.output_control[i] = _outputs.output[i];
}
/* publish as we send */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
_outputs.timestamp = hrt_absolute_time();
/* XXX needs to be based off post-mix values from the IO side */
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
// XXX relays
cmd.arm_ok = _armed.armed;
/* update relays */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
/* armed and not locked down -> arming ok */
cmd.arm_ok = (_armed.armed && !_armed.lockdown);
/* indicate that full autonomous position control / vector flight mode is available */
cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok;
/* allow manual override on IO (not allowed for multirotors or other systems with SAS) */
cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok;
/* set desired PWM output rate */
cmd.servo_rate = _update_rate;
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret)
debug("send error %d", ret);
}
@ -515,13 +647,89 @@ PX4IO::config_send()
int ret;
cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
cfg.serial_rx_mode = _rx_mode;
int val;
/* maintaing the standard order of Roll, Pitch, Yaw, Throttle */
param_get(param_find("RC_MAP_ROLL"), &val);
cfg.rc_map[0] = val;
param_get(param_find("RC_MAP_PITCH"), &val);
cfg.rc_map[1] = val;
param_get(param_find("RC_MAP_YAW"), &val);
cfg.rc_map[2] = val;
param_get(param_find("RC_MAP_THROTTLE"), &val);
cfg.rc_map[3] = val;
/* set the individual channel properties */
char nbuf[16];
float float_val;
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_MIN", i + 1);
param_get(param_find(nbuf), &float_val);
cfg.rc_min[i] = float_val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_TRIM", i + 1);
param_get(param_find(nbuf), &float_val);
cfg.rc_trim[i] = float_val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_MAX", i + 1);
param_get(param_find(nbuf), &float_val);
cfg.rc_max[i] = float_val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_REV", i + 1);
param_get(param_find(nbuf), &float_val);
cfg.rc_rev[i] = float_val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_DZ", i + 1);
param_get(param_find(nbuf), &float_val);
cfg.rc_dz[i] = float_val;
}
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
if (ret)
debug("config error %d", ret);
}
int
PX4IO::mixer_send(const char *buf, unsigned buflen)
{
uint8_t frame[HX_STREAM_MAX_FRAME];
px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
msg->action = F2I_MIXER_ACTION_RESET;
do {
unsigned count = buflen;
if (count > F2I_MIXER_MAX_TEXT)
count = F2I_MIXER_MAX_TEXT;
if (count > 0) {
memcpy(&msg->text[0], buf, count);
buf += count;
buflen -= count;
}
int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count);
if (ret) {
log("mixer send error %d", ret);
return ret;
}
msg->action = F2I_MIXER_ACTION_APPEND;
} while (buflen > 0);
return 0;
}
int
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
@ -539,13 +747,18 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case PWM_SERVO_DISARM:
/* fake a disarmed transition */
_armed.armed = true;
_armed.armed = false;
_send_needed = true;
break;
case PWM_SERVO_SET_UPDATE_RATE:
// not supported yet
ret = -EINVAL;
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
/* fake an update to the selected servo channel */
/* fake an update to the selected 'servo' channel */
if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true;
@ -561,68 +774,53 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break;
case GPIO_RESET:
_relays = 0;
_send_needed = true;
break;
case GPIO_SET:
case GPIO_CLEAR:
/* make sure only valid bits are being set */
if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) {
ret = EINVAL;
break;
}
if (cmd == GPIO_SET) {
_relays |= arg;
} else {
_relays &= ~arg;
}
_send_needed = true;
break;
case GPIO_GET:
*(uint32_t *)arg = _relays;
break;
case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _max_actuators;
*(unsigned *)arg = PX4IO_CONTROL_CHANNELS;
break;
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
ret = 0; /* load always resets */
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
case MIXERIOCLOADBUF:
/* build the new mixer from the supplied argument */
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
/* set the buffer up for transfer */
_mix_buf = (const char *)arg;
_mix_buf_len = strnlen(_mix_buf, 1024);
/* validate the new mixer */
if (mixer->check()) {
delete mixer;
ret = -EINVAL;
/* drop the lock and wait for the thread to clear the transmit */
unlock();
} else {
/* if we don't have a group yet, allocate one */
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
while (_mix_buf != nullptr)
usleep(1000);
/* add the new mixer to the group */
_mixers->add_mixer(mixer);
}
lock();
}
break;
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADFILE: {
MixerGroup *newmixers;
const char *path = (const char *)arg;
/* allocate a new mixer group and load it from the file */
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (newmixers->load_from_file(path) != 0) {
delete newmixers;
ret = -EINVAL;
}
/* swap the new mixers in for the old */
if (_mixers != nullptr) {
delete _mixers;
}
_mixers = newmixers;
}
ret = 0;
break;
default:
@ -635,15 +833,6 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
return ret;
}
void
PX4IO::set_rx_mode(unsigned mode)
{
if (mode != _rx_mode) {
_rx_mode = mode;
_config_needed = true;
}
}
extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
namespace
@ -673,9 +862,42 @@ test(void)
close(fd);
actuator_armed_s aa;
aa.armed = true;
aa.lockdown = false;
orb_advertise(ORB_ID(actuator_armed), &aa);
exit(0);
}
void
monitor(void)
{
unsigned cancels = 3;
printf("Hit <enter> three times to exit monitor mode\n");
for (;;) {
pollfd fds[1];
fds[0].fd = 0;
fds[0].events = POLLIN;
poll(fds, 1, 500);
if (fds[0].revents == POLLIN) {
int c;
read(0, &c, 1);
if (cancels-- == 0)
exit(0);
}
if (g_dev != nullptr)
g_dev->dump_one = true;
}
}
}
int
@ -697,12 +919,51 @@ px4io_main(int argc, char *argv[])
errx(1, "driver init failed");
}
/* look for the optional pwm update rate for the supported modes */
if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
if (argc > 2 + 1) {
g_dev->set_pwm_rate(atoi(argv[2 + 1]));
} else {
fprintf(stderr, "missing argument for pwm update rate (-u)\n");
return 1;
}
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (g_dev != nullptr) {
/* stop the driver */
delete g_dev;
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (g_dev != nullptr)
printf("[px4io] loaded\n");
else
printf("[px4io] not loaded\n");
exit(0);
}
/* note, stop not currently implemented */
if (!strcmp(argv[1], "update")) {
if (g_dev != nullptr) {
printf("[px4io] loaded, detaching first\n");
/* stop the driver */
delete g_dev;
}
PX4IO_Uploader *up;
const char *fn[3];
@ -745,25 +1006,18 @@ px4io_main(int argc, char *argv[])
return ret;
}
if (!strcmp(argv[1], "rx_dsm_10bit")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_DSM_10BIT);
}
if (!strcmp(argv[1], "rx_dsm_11bit")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_DSM_11BIT);
}
if (!strcmp(argv[1], "rx_sbus")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS);
}
if (!strcmp(argv[1], "rx_dsm") ||
!strcmp(argv[1], "rx_dsm_10bit") ||
!strcmp(argv[1], "rx_dsm_11bit") ||
!strcmp(argv[1], "rx_sbus") ||
!strcmp(argv[1], "rx_ppm"))
errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
if (!strcmp(argv[1], "test"))
test();
if (!strcmp(argv[1], "monitor"))
monitor();
errx(1, "need a command, try 'start', 'test', 'rx_dsm_10bit', 'rx_dsm_11bit', 'rx_sbus' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor' or 'update'");
}

138
apps/drivers/px4io/uploader.cpp
View File

@ -51,6 +51,50 @@
#include "uploader.h"
static const uint32_t crctab[] =
{
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};
static uint32_t
crc32(const uint8_t *src, unsigned len, unsigned state)
{
for (unsigned i = 0; i < len; i++)
state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
return state;
}
PX4IO_Uploader::PX4IO_Uploader() :
_io_fd(-1),
_fw_fd(-1)
@ -110,6 +154,17 @@ PX4IO_Uploader::upload(const char *filenames[])
}
}
ret = get_info(INFO_BL_REV, bl_rev);
if (ret == OK) {
if (bl_rev <= BL_REV) {
log("found bootloader revision: %d", bl_rev);
} else {
log("found unsupported bootloader revision %d, exiting", bl_rev);
return OK;
}
}
ret = erase();
if (ret != OK) {
@ -124,7 +179,11 @@ PX4IO_Uploader::upload(const char *filenames[])
continue;
}
ret = verify();
if (bl_rev <= 2)
ret = verify_rev2();
else if(bl_rev == 3) {
ret = verify_rev3();
}
if (ret != OK) {
log("verify failed");
@ -189,8 +248,11 @@ PX4IO_Uploader::drain()
int ret;
do {
ret = recv(c, 10);
//log("discard 0x%02x", c);
ret = recv(c, 250);
if (ret == OK) {
//log("discard 0x%02x", c);
}
} while (ret == OK);
}
@ -317,7 +379,7 @@ PX4IO_Uploader::program()
}
int
PX4IO_Uploader::verify()
PX4IO_Uploader::verify_rev2()
{
uint8_t file_buf[PROG_MULTI_MAX];
ssize_t count;
@ -377,6 +439,74 @@ PX4IO_Uploader::verify()
return OK;
}
int
PX4IO_Uploader::verify_rev3()
{
int ret;
uint8_t file_buf[4];
ssize_t count;
uint32_t sum = 0;
uint32_t bytes_read = 0;
uint32_t fw_size = 0;
uint32_t crc = 0;
uint8_t fill_blank = 0xff;
log("verify...");
lseek(_fw_fd, 0, SEEK_SET);
ret = get_info(INFO_FLASH_SIZE, fw_size);
send(PROTO_EOC);
if (ret != OK) {
log("could not read firmware size");
return ret;
}
/* read through the firmware file again and calculate the checksum*/
while (true) {
lseek(_fw_fd, 0, SEEK_CUR);
count = read(_fw_fd, file_buf, sizeof(file_buf));
/* set the rest to ff */
if (count == 0) {
break;
}
/* stop if the file cannot be read */
if (count < 0)
return -errno;
/* calculate crc32 sum */
sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum);
bytes_read += count;
}
/* fill the rest with 0xff */
while (bytes_read < fw_size) {
sum = crc32(&fill_blank, sizeof(fill_blank), sum);
bytes_read += sizeof(fill_blank);
}
/* request CRC from IO */
send(PROTO_GET_CRC);
send(PROTO_EOC);
ret = recv((uint8_t*)(&crc), sizeof(crc));
if (ret != OK) {
log("did not receive CRC checksum");
return ret;
}
/* compare the CRC sum from the IO with the one calculated */
if (sum != crc) {
log("CRC wrong: received: %d, expected: %d", crc, sum);
return -EINVAL;
}
return OK;
}
int
PX4IO_Uploader::reboot()
{

9
apps/drivers/px4io/uploader.h
View File

@ -64,21 +64,25 @@ private:
PROTO_CHIP_VERIFY = 0x24,
PROTO_PROG_MULTI = 0x27,
PROTO_READ_MULTI = 0x28,
PROTO_GET_CRC = 0x29,
PROTO_REBOOT = 0x30,
INFO_BL_REV = 1, /**< bootloader protocol revision */
BL_REV = 2, /**< supported bootloader protocol */
BL_REV = 3, /**< supported bootloader protocol */
INFO_BOARD_ID = 2, /**< board type */
INFO_BOARD_REV = 3, /**< board revision */
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */
READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */
};
int _io_fd;
int _fw_fd;
uint32_t bl_rev; /**< bootloader revision */
void log(const char *fmt, ...);
int recv(uint8_t &c, unsigned timeout = 1000);
@ -91,7 +95,8 @@ private:
int get_info(int param, uint32_t &val);
int erase();
int program();
int verify();
int verify_rev2();
int verify_rev3();
int reboot();
int compare(bool &identical);
};

43
apps/drivers/stm32/adc/Makefile Normal file
View File

@ -0,0 +1,43 @@
############################################################################
#
# 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.
#
############################################################################
#
# STM32 ADC driver
#
APPNAME = adc
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
include $(APPDIR)/mk/app.mk

387
apps/drivers/stm32/adc/adc.cpp Normal file
View File

@ -0,0 +1,387 @@
/****************************************************************************
*
* 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 adc.cpp
*
* Driver for the STM32 ADC.
*
* This is a low-rate driver, designed for sampling things like voltages
* and so forth. It avoids the gross complexity of the NuttX ADC driver.
*/
#include <nuttx/config.h>
#include <drivers/device/device.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <stdio.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_adc.h>
#include <arch/stm32/chip.h>
#include <stm32_internal.h>
#include <stm32_gpio.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
/*
* Register accessors.
* For now, no reason not to just use ADC1.
*/
#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
#define rSR REG(STM32_ADC_SR_OFFSET)
#define rCR1 REG(STM32_ADC_CR1_OFFSET)
#define rCR2 REG(STM32_ADC_CR2_OFFSET)
#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET)
#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET)
#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET)
#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET)
#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET)
#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET)
#define rHTR REG(STM32_ADC_HTR_OFFSET)
#define rLTR REG(STM32_ADC_LTR_OFFSET)
#define rSQR1 REG(STM32_ADC_SQR1_OFFSET)
#define rSQR2 REG(STM32_ADC_SQR2_OFFSET)
#define rSQR3 REG(STM32_ADC_SQR3_OFFSET)
#define rJSQR REG(STM32_ADC_JSQR_OFFSET)
#define rJDR1 REG(STM32_ADC_JDR1_OFFSET)
#define rJDR2 REG(STM32_ADC_JDR2_OFFSET)
#define rJDR3 REG(STM32_ADC_JDR3_OFFSET)
#define rJDR4 REG(STM32_ADC_JDR4_OFFSET)
#define rDR REG(STM32_ADC_DR_OFFSET)
#ifdef STM32_ADC_CCR
# define rCCR REG(STM32_ADC_CCR_OFFSET)
#endif
class ADC : public device::CDev
{
public:
ADC(uint32_t channels);
~ADC();
virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t read(file *filp, char *buffer, size_t len);
protected:
virtual int open_first(struct file *filp);
virtual int close_last(struct file *filp);
private:
static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */
hrt_call _call;
perf_counter_t _sample_perf;
unsigned _channel_count;
adc_msg_s *_samples; /**< sample buffer */
/** work trampoline */
static void _tick_trampoline(void *arg);
/** worker function */
void _tick();
/**
* Sample a single channel and return the measured value.
*
* @param channel The channel to sample.
* @return The sampled value, or 0xffff if
* sampling failed.
*/
uint16_t _sample(unsigned channel);
};
ADC::ADC(uint32_t channels) :
CDev("adc", ADC_DEVICE_PATH),
_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
_channel_count(0),
_samples(nullptr)
{
_debug_enabled = true;
/* always enable the temperature sensor */
channels |= 1 << 16;
/* allocate the sample array */
for (unsigned i = 0; i < 32; i++) {
if (channels & (1 << i)) {
_channel_count++;
}
}
_samples = new adc_msg_s[_channel_count];
/* prefill the channel numbers in the sample array */
if (_samples != nullptr) {
unsigned index = 0;
for (unsigned i = 0; i < 32; i++) {
if (channels & (1 << i)) {
_samples[index].am_channel = i;
_samples[index].am_data = 0;
index++;
}
}
}
}
ADC::~ADC()
{
if (_samples != nullptr)
delete _samples;
}
int
ADC::init()
{
/* do calibration if supported */
#ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_CAL;
usleep(100);
if (rCR2 & ADC_CR2_CAL)
return -1;
#endif
/* arbitrarily configure all channels for 55 cycle sample time */
rSMPR1 = 0b00000011011011011011011011011011;
rSMPR2 = 0b00011011011011011011011011011011;
/* XXX for F2/4, might want to select 12-bit mode? */
rCR1 = 0;
/* enable the temperature sensor / Vrefint channel if supported*/
rCR2 =
#ifdef ADC_CR2_TSVREFE
/* enable the temperature sensor in CR2 */
ADC_CR2_TSVREFE |
#endif
0;
#ifdef ADC_CCR_TSVREFE
/* enable temperature sensor in CCR */
rCCR = ADC_CCR_TSVREFE;
#endif
/* configure for a single-channel sequence */
rSQR1 = 0;
rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel each tick */
/* power-cycle the ADC and turn it on */
rCR2 &= ~ADC_CR2_ADON;
usleep(10);
rCR2 |= ADC_CR2_ADON;
usleep(10);
rCR2 |= ADC_CR2_ADON;
usleep(10);
/* kick off a sample and wait for it to complete */
hrt_abstime now = hrt_absolute_time();
rCR2 |= ADC_CR2_SWSTART;
while (!(rSR & ADC_SR_EOC)) {
/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
if ((hrt_absolute_time() - now) > 500) {
log("sample timeout");
return -1;
return 0xffff;
}
}
debug("init done");
/* create the device node */
return CDev::init();
}
int
ADC::ioctl(file *filp, int cmd, unsigned long arg)
{
return -ENOTTY;
}
ssize_t
ADC::read(file *filp, char *buffer, size_t len)
{
const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
if (len > maxsize)
len = maxsize;
/* block interrupts while copying samples to avoid racing with an update */
irqstate_t flags = irqsave();
memcpy(buffer, _samples, len);
irqrestore(flags);
return len;
}
int
ADC::open_first(struct file *filp)
{
/* get fresh data */
_tick();
/* and schedule regular updates */
hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this);
return 0;
}
int
ADC::close_last(struct file *filp)
{
hrt_cancel(&_call);
return 0;
}
void
ADC::_tick_trampoline(void *arg)
{
((ADC *)arg)->_tick();
}
void
ADC::_tick()
{
/* scan the channel set and sample each */
for (unsigned i = 0; i < _channel_count; i++)
_samples[i].am_data = _sample(_samples[i].am_channel);
}
uint16_t
ADC::_sample(unsigned channel)
{
perf_begin(_sample_perf);
/* clear any previous EOC */
if (rSR & ADC_SR_EOC)
rSR &= ~ADC_SR_EOC;
/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
rSQR3 = channel;
rCR2 |= ADC_CR2_SWSTART;
/* wait for the conversion to complete */
hrt_abstime now = hrt_absolute_time();
while (!(rSR & ADC_SR_EOC)) {
/* don't wait for more than 50us, since that means something broke - should reset here if we see this */
if ((hrt_absolute_time() - now) > 50) {
log("sample timeout");
return 0xffff;
}
}
/* read the result and clear EOC */
uint16_t result = rDR;
perf_end(_sample_perf);
return result;
}
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int adc_main(int argc, char *argv[]);
namespace
{
ADC *g_adc;
void
test(void)
{
int fd = open(ADC_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "can't open ADC device");
for (unsigned i = 0; i < 50; i++) {
adc_msg_s data[8];
ssize_t count = read(fd, data, sizeof(data));
if (count < 0)
errx(1, "read error");
unsigned channels = count / sizeof(data[0]);
for (unsigned j = 0; j < channels; j++) {
printf ("%d: %u ", data[j].am_channel, data[j].am_data);
}
printf("\n");
usleep(500000);
}
exit(0);
}
}
int
adc_main(int argc, char *argv[])
{
if (g_adc == nullptr) {
/* XXX this hardcodes the default channel set for PX4FMU - should be configurable */
g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
if (g_adc == nullptr)
errx(1, "couldn't allocate the ADC driver");
if (g_adc->init() != OK) {
delete g_adc;
errx(1, "ADC init failed");
}
}
if (argc > 1) {
if (!strcmp(argv[1], "test"))
test();
}
exit(0);
}

51
apps/drivers/stm32/drv_pwm_servo.c
View File

@ -60,13 +60,13 @@
#include "drv_pwm_servo.h"
#include "chip.h"
#include "up_internal.h"
#include "up_arch.h"
#include <chip.h>
#include <up_internal.h>
#include <up_arch.h>
#include "stm32_internal.h"
#include "stm32_gpio.h"
#include "stm32_tim.h"
#include <stm32_internal.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
/* default rate (in Hz) of PWM updates */
@ -143,27 +143,27 @@ pwm_channel_init(unsigned channel)
/* configure the channel */
switch (pwm_channels[channel].timer_channel) {
case 1:
rCCMR1(timer) |= (6 << 4);
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
rCCR1(timer) = pwm_channels[channel].default_value;
rCCER(timer) |= (1 << 0);
rCCER(timer) |= GTIM_CCER_CC1E;
break;
case 2:
rCCMR1(timer) |= (6 << 12);
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
rCCR2(timer) = pwm_channels[channel].default_value;
rCCER(timer) |= (1 << 4);
rCCER(timer) |= GTIM_CCER_CC2E;
break;
case 3:
rCCMR2(timer) |= (6 << 4);
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
rCCR3(timer) = pwm_channels[channel].default_value;
rCCER(timer) |= (1 << 8);
rCCER(timer) |= GTIM_CCER_CC3E;
break;
case 4:
rCCMR2(timer) |= (6 << 12);
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
rCCR4(timer) = pwm_channels[channel].default_value;
rCCER(timer) |= (1 << 12);
rCCER(timer) |= GTIM_CCER_CC4E;
break;
}
}
@ -288,17 +288,20 @@ up_pwm_servo_set_rate(unsigned rate)
void
up_pwm_servo_arm(bool armed)
{
/*
* XXX this is inelgant and in particular will either jam outputs at whatever level
* they happen to be at at the time the timers stop or generate runts.
* The right thing is almost certainly to kill auto-reload on the timers so that
* they just stop at the end of their count for disable, and to reset/restart them
* for enable.
*/
/* iterate timers and arm/disarm appropriately */
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
if (pwm_timers[i].base != 0)
rCR1(i) = armed ? GTIM_CR1_CEN : 0;
if (pwm_timers[i].base != 0) {
if (armed) {
/* force an update to preload all registers */
rEGR(i) = GTIM_EGR_UG;
/* arm requires the timer be enabled */
rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
} else {
/* on disarm, just stop auto-reload so we don't generate runts */
rCR1(i) &= ~GTIM_CR1_ARPE;
}
}
}
}

160
apps/examples/Kconfig
View File

@ -3,206 +3,60 @@
# see misc/tools/kconfig-language.txt.
#
menu "ADC Example"
source "$APPSDIR/examples/adc/Kconfig"
endmenu
menu "Buttons Example"
source "$APPSDIR/examples/buttons/Kconfig"
endmenu
menu "CAN Example"
source "$APPSDIR/examples/can/Kconfig"
endmenu
menu "USB CDC/ACM Class Driver Example"
source "$APPSDIR/examples/cdcacm/Kconfig"
endmenu
menu "USB composite Class Driver Example"
source "$APPSDIR/examples/composite/Kconfig"
endmenu
menu "DHCP Server Example"
source "$APPSDIR/examples/cxxtest/Kconfig"
source "$APPSDIR/examples/dhcpd/Kconfig"
endmenu
menu "FTP Client Example"
source "$APPSDIR/examples/elf/Kconfig"
source "$APPSDIR/examples/ftpc/Kconfig"
endmenu
menu "FTP Server Example"
source "$APPSDIR/examples/ftpd/Kconfig"
endmenu
menu "\"Hello, World!\" Example"
source "$APPSDIR/examples/hello/Kconfig"
endmenu
menu "\"Hello, World!\" C++ Example"
source "$APPSDIR/examples/helloxx/Kconfig"
endmenu
menu "USB HID Keyboard Example"
source "$APPSDIR/examples/json/Kconfig"
source "$APPSDIR/examples/hidkbd/Kconfig"
endmenu
menu "IGMP Example"
source "$APPSDIR/examples/keypadtest/Kconfig"
source "$APPSDIR/examples/igmp/Kconfig"
endmenu
menu "LCD Read/Write Example"
source "$APPSDIR/examples/lcdrw/Kconfig"
endmenu
menu "Memory Management Example"
source "$APPSDIR/examples/mm/Kconfig"
endmenu
menu "File System Mount Example"
source "$APPSDIR/examples/mount/Kconfig"
endmenu
menu "FreeModBus Example"
source "$APPSDIR/examples/modbus/Kconfig"
endmenu
menu "Network Test Example"
source "$APPSDIR/examples/nettest/Kconfig"
endmenu
menu "NuttShell (NSH) Example"
source "$APPSDIR/examples/nsh/Kconfig"
endmenu
menu "NULL Example"
source "$APPSDIR/examples/null/Kconfig"
endmenu
menu "NX Graphics Example"
source "$APPSDIR/examples/nx/Kconfig"
endmenu
menu "NxConsole Example"
source "$APPSDIR/examples/nxconsole/Kconfig"
endmenu
menu "NXFFS File System Example"
source "$APPSDIR/examples/nxffs/Kconfig"
endmenu
menu "NXFLAT Example"
source "$APPSDIR/examples/nxflat/Kconfig"
endmenu
menu "NX Graphics \"Hello, World!\" Example"
source "$APPSDIR/examples/nxhello/Kconfig"
endmenu
menu "NX Graphics image Example"
source "$APPSDIR/examples/nximage/Kconfig"
endmenu
menu "NX Graphics lines Example"
source "$APPSDIR/examples/nxlines/Kconfig"
endmenu
menu "NX Graphics Text Example"
source "$APPSDIR/examples/nxtext/Kconfig"
endmenu
menu "OS Test Example"
source "$APPSDIR/examples/ostest/Kconfig"
endmenu
menu "Pascal \"Hello, World!\"example"
source "$APPSDIR/examples/pashello/Kconfig"
endmenu
menu "Pipe Example"
source "$APPSDIR/examples/pipe/Kconfig"
endmenu
menu "Poll Example"
source "$APPSDIR/examples/poll/Kconfig"
endmenu
menu "Pulse Width Modulation (PWM) Example"
source "$APPSDIR/examples/pwm/Kconfig"
endmenu
menu "Quadrature Encoder Example"
source "$APPSDIR/examples/posix_spawn/Kconfig"
source "$APPSDIR/examples/qencoder/Kconfig"
endmenu
menu "RGMP Example"
source "$APPSDIR/examples/relays/Kconfig"
source "$APPSDIR/examples/rgmp/Kconfig"
endmenu
menu "ROMFS Example"
source "$APPSDIR/examples/romfs/Kconfig"
endmenu
menu "sendmail Example"
source "$APPSDIR/examples/sendmail/Kconfig"
endmenu
menu "Serial Loopback Example"
source "$APPSDIR/examples/serloop/Kconfig"
endmenu
menu "Telnet Daemon Example"
source "$APPSDIR/examples/telnetd/Kconfig"
endmenu
menu "THTTPD Web Server Example"
source "$APPSDIR/examples/thttpd/Kconfig"
endmenu
menu "TIFF Generation Example"
source "$APPSDIR/examples/tiff/Kconfig"
endmenu
menu "Touchscreen Example"
source "$APPSDIR/examples/touchscreen/Kconfig"
endmenu
menu "UDP Example"
source "$APPSDIR/examples/udp/Kconfig"
endmenu
menu "UDP Discovery Daemon Example"
source "$APPSDIR/examples/discover/Kconfig"
endmenu
menu "uIP Web Server Example"
source "$APPSDIR/examples/uip/Kconfig"
endmenu
menu "USB Serial Test Example"
source "$APPSDIR/examples/usbserial/Kconfig"
endmenu
menu "USB Mass Storage Class Example"
source "$APPSDIR/examples/usbstorage/Kconfig"
endmenu
menu "USB Serial Terminal Example"
source "$APPSDIR/examples/usbterm/Kconfig"
endmenu
menu "Watchdog timer Example"
source "$APPSDIR/examples/watchdog/Kconfig"
endmenu
menu "wget Example"
source "$APPSDIR/examples/wget/Kconfig"
endmenu
menu "WLAN Example"
source "$APPSDIR/examples/wlan/Kconfig"
endmenu
menu "XML RPC Example"
source "$APPSDIR/examples/wgetjson/Kconfig"
source "$APPSDIR/examples/xmlrpc/Kconfig"
endmenu

32
apps/examples/Make.defs
View File

@ -54,6 +54,10 @@ ifeq ($(CONFIG_EXAMPLES_COMPOSITE),y)
CONFIGURED_APPS += examples/composite
endif
ifeq ($(CONFIG_EXAMPLES_CXXTEST),y)
CONFIGURED_APPS += examples/cxxtest
endif
ifeq ($(CONFIG_EXAMPLES_DHCPD),y)
CONFIGURED_APPS += examples/dhcpd
endif
@ -62,6 +66,10 @@ ifeq ($(CONFIG_EXAMPLES_DISCOVER),y)
CONFIGURED_APPS += examples/discover
endif
ifeq ($(CONFIG_EXAMPLES_ELF),y)
CONFIGURED_APPS += examples/elf
endif
ifeq ($(CONFIG_EXAMPLES_FTPC),y)
CONFIGURED_APPS += examples/ftpc
endif
@ -86,6 +94,14 @@ ifeq ($(CONFIG_EXAMPLES_IGMP),y)
CONFIGURED_APPS += examples/igmp
endif
ifeq ($(CONFIG_EXAMPLES_JSON),y)
CONFIGURED_APPS += examples/json
endif
ifeq ($(CONFIG_EXAMPLES_KEYPADTEST),y)
CONFIGURED_APPS += examples/keypadtest
endif
ifeq ($(CONFIG_EXAMPLES_LCDRW),y)
CONFIGURED_APPS += examples/lcdrw
endif
@ -94,6 +110,10 @@ ifeq ($(CONFIG_EXAMPLES_MM),y)
CONFIGURED_APPS += examples/mm
endif
ifeq ($(CONFIG_EXAMPLES_MODBUS),y)
CONFIGURED_APPS += examples/modbus
endif
ifeq ($(CONFIG_EXAMPLES_MOUNT),y)
CONFIGURED_APPS += examples/mount
endif
@ -162,10 +182,18 @@ ifeq ($(CONFIG_EXAMPLES_PWM),y)
CONFIGURED_APPS += examples/pwm
endif
ifeq ($(CONFIG_EXAMPLES_POSIXSPAWN),y)
CONFIGURED_APPS += examples/posix_spawn
endif
ifeq ($(CONFIG_EXAMPLES_QENCODER),y)
CONFIGURED_APPS += examples/qencoder
endif
ifeq ($(CONFIG_EXAMPLES_RELAYS),y)
CONFIGURED_APPS += examples/relays
endif
ifeq ($(CONFIG_EXAMPLES_RGMP),y)
CONFIGURED_APPS += examples/rgmp
endif
@ -226,8 +254,8 @@ ifeq ($(CONFIG_EXAMPLES_WGET),y)
CONFIGURED_APPS += examples/wget
endif
ifeq ($(CONFIG_EXAMPLES_WLAN),y)
CONFIGURED_APPS += examples/wlan
ifeq ($(CONFIG_EXAMPLES_WGETJSON),y)
CONFIGURED_APPS += examples/wgetjson
endif
ifeq ($(CONFIG_EXAMPLES_XMLRPC),y)

59
apps/examples/Makefile
View File

@ -37,12 +37,15 @@
# Sub-directories
SUBDIRS = adc buttons can cdcacm composite dhcpd discover ftpc ftpd hello
SUBDIRS += helloxx hidkbd igmp lcdrw mm modbus mount nettest nsh null nx
SUBDIRS += nxconsole nxffs nxflat nxhello nximage nxlines nxtext ostest
SUBDIRS += pashello pipe poll pwm qencoder rgmp romfs serloop telnetd
SUBDIRS += thttpd tiff touchscreen udp uip usbserial sendmail usbstorage
SUBDIRS += usbterm watchdog wget wlan
SUBDIRS = adc can cdcacm nsh
SUBDIRS += math_demo control_demo kalman_demo px4_deamon_app
#SUBDIRS = adc buttons can cdcacm composite cxxtest dhcpd discover elf ftpc
#SUBDIRS += ftpd hello helloxx hidkbd igmp json keypadtest lcdrw mm modbus mount
#SUBDIRS += nettest nsh null nx nxconsole nxffs nxflat nxhello nximage
#SUBDIRS += nxlines nxtext ostest pashello pipe poll pwm posix_spawn qencoder
#SUBDIRS += relays rgmp romfs serloop telnetd thttpd tiff touchscreen udp uip
#SUBDIRS += usbserial sendmail usbstorage usbterm watchdog wget wgetjson
# Sub-directories that might need context setup. Directories may need
# context setup for a variety of reasons, but the most common is because
@ -57,13 +60,14 @@ SUBDIRS += usbterm watchdog wget wlan
CNTXTDIRS = pwm
ifeq ($(CONFIG_NSH_BUILTIN_APPS),y)
CNTXTDIRS += adc can cdcacm composite discover ftpd dhcpd modbus nettest
CNTXTDIRS += qencoder telnetd watchdog
CNTXTDIRS += adc can cdcacm
#CNTXTDIRS += adc can cdcacm composite cxxtest dhcpd discover ftpd hello json
#CNTXTDIRS += keypadtestmodbus nettest nxlines relays qencoder telnetd watchdog
#CNTXTDIRS += wgetjson
endif
ifeq ($(CONFIG_EXAMPLES_HELLO_BUILTIN),y)
CNTXTDIRS += hello
endif
ifeq ($(CONFIG_EXAMPLES_HELLOXX_BUILTIN),y)
CNTXTDIRS += helloxx
endif
@ -79,9 +83,6 @@ endif
ifeq ($(CONFIG_EXAMPLES_NXIMAGE_BUILTIN),y)
CNTXTDIRS += nximage
endif
ifeq ($(CONFIG_EXAMPLES_LINES_BUILTIN),y)
CNTXTDIRS += nxlines
endif
ifeq ($(CONFIG_EXAMPLES_NXTEXT_BUILTIN),y)
CNTXTDIRS += nxtext
endif
@ -105,27 +106,25 @@ all: nothing
.PHONY: nothing context depend clean distclean
define SDIR_template
$(1)_$(2):
$(Q) $(MAKE) -C $(1) $(2) TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
endef
$(foreach SDIR, $(CNTXTDIRS), $(eval $(call SDIR_template,$(SDIR),context)))
$(foreach SDIR, $(SUBDIRS), $(eval $(call SDIR_template,$(SDIR),depend)))
$(foreach SDIR, $(SUBDIRS), $(eval $(call SDIR_template,$(SDIR),clean)))
$(foreach SDIR, $(SUBDIRS), $(eval $(call SDIR_template,$(SDIR),distclean)))
nothing:
context:
@for dir in $(CNTXTDIRS) ; do \
$(MAKE) -C $$dir context TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
context: $(foreach SDIR, $(CNTXTDIRS), $(SDIR)_context)
depend:
@for dir in $(SUBDIRS) ; do \
$(MAKE) -C $$dir depend TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
depend: $(foreach SDIR, $(SUBDIRS), $(SDIR)_depend)
clean:
@for dir in $(SUBDIRS) ; do \
$(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
clean: $(foreach SDIR, $(SUBDIRS), $(SDIR)_clean)
distclean: clean
@for dir in $(SUBDIRS) ; do \
$(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
distclean: $(foreach SDIR, $(SUBDIRS), $(SDIR)_distclean)
-include Make.dep

266
apps/examples/README.txt
View File

@ -239,6 +239,29 @@ examples/composite
CONFIG_EXAMPLES_COMPOSITE_TRACEINTERRUPTS
Show interrupt-related events.
examples/cxxtest
^^^^^^^^^^^^^^^^
This is a test of the C++ standard library. At present a port of the uClibc++
C++ library is available. Due to licensinging issues, the uClibc++ C++ library
is not included in the NuttX source tree by default, but must be installed
(see misc/uClibc++/README.txt for installation).
The NuttX setting that are required include:
CONFIG_HAVE_CXX=y
CONFIG_HAVE_CXXINITIALIZE=y
CONFIG_UCLIBCXX=y
Additional uClibc++ settings may be required in your build environment.
The uClibc++ test includes simple test of:
- iostreams,
- STL,
- RTTI, and
- Exceptions
examples/dhcpd
^^^^^^^^^^^^^^
@ -297,6 +320,68 @@ examples/discover
CONFIG_EXAMPLES_DISCOVER_DRIPADDR - Router IP address
CONFIG_EXAMPLES_DISCOVER_NETMASK - Network Mask
examples/elf
^^^^^^^^^^^^
This example builds a small ELF loader test case. This includes several
test programs under examples/elf tests. These tests are build using
the relocatable ELF format and installed in a ROMFS file system. At run time,
each program in the ROMFS file system is executed. Requires CONFIG_ELF.
Other configuration options:
CONFIG_EXAMPLES_ELF_DEVMINOR - The minor device number of the ROMFS block
driver. For example, the N in /dev/ramN. Used for registering the RAM
block driver that will hold the ROMFS file system containing the ELF
executables to be tested. Default: 0
CONFIG_EXAMPLES_ELF_DEVPATH - The path to the ROMFS block driver device. This
must match EXAMPLES_ELF_DEVMINOR. Used for registering the RAM block driver
that will hold the ROMFS file system containing the ELF executables to be
tested. Default: "/dev/ram0"
NOTES:
1. CFLAGS should be provided in CELFFLAGS. RAM and FLASH memory regions
may require long allcs. For ARM, this might be:
CELFFLAGS = $(CFLAGS) -mlong-calls
Similarly for C++ flags which must be provided in CXXELFFLAGS.
2. Your top-level nuttx/Make.defs file must also include an approproate definition,
LDELFFLAGS, to generate a relocatable ELF object. With GNU LD, this should
include '-r' and '-e main' (or _main on some platforms).
LDELFFLAGS = -r -e main
If you use GCC to link, you make also need to include '-nostdlib' or
'-nostartfiles' and '-nodefaultlibs'.
3. This example also requires genromfs. genromfs can be build as part of the
nuttx toolchain. Or can built from the genromfs sources that can be found
at misc/tools/genromfs-0.5.2.tar.gz. In any event, the PATH variable must
include the path to the genromfs executable.
4. ELF size: The ELF files in this example are, be default, quite large
because they include a lot of "build garbage". You can greatly reduce the
size of the ELF binaries are using the 'objcopy --strip-unneeded' command to
remove un-necessary information from the ELF files.
5. Simulator. You cannot use this example with the the NuttX simulator on
Cygwin. That is because the Cygwin GCC does not generate ELF file but
rather some Windows-native binary format.
If you really want to do this, you can create a NuttX x86 buildroot toolchain
and use that be build the ELF executables for the ROMFS file system.
6. Linker scripts. You might also want to use a linker scripts to combine
sections better. An example linker script is at nuttx/binfmt/libelf/gnu-elf.ld.
That example might have to be tuned for your particular linker output to
position additional sections correctly. The GNU LD LDELFFLAGS then might
be:
LDELFFLAGS = -r -e main -T$(TOPDIR)/binfmt/libelf/gnu-elf.ld
examples/ftpc
^^^^^^^^^^^^^
@ -406,7 +491,7 @@ examples/hello
than examples/null with a single printf statement. Really useful only
for bringing up new NuttX architectures.
* CONFIG_EXAMPLES_HELLO_BUILTIN
* CONFIG_NSH_BUILTIN_APPS
Build the "Hello, World" example as an NSH built-in application.
examples/helloxx
@ -455,9 +540,21 @@ examples/hidkbd
This is a simple test to debug/verify the USB host HID keyboard class
driver.
CONFIG_EXAMPLES_HIDKBD_DEFPRIO - Priority of "waiter" thread.
CONFIG_EXAMPLES_HIDKBD_STACKSIZE - Stacksize of "waiter" thread.
CONFIG_EXAMPLES_HIDKBD_DEFPRIO - Priority of "waiter" thread. Default:
50
CONFIG_EXAMPLES_HIDKBD_STACKSIZE - Stacksize of "waiter" thread. Default
1024
CONFIG_EXAMPLES_HIDKBD_DEVNAME - Name of keyboard device to be used.
Default: "/dev/kbda"
CONFIG_EXAMPLES_HIDKBD_ENCODED - Decode special key press events in the
user buffer. In this case, the example coded will use the interfaces
defined in include/nuttx/input/kbd_codec.h to decode the returned
keyboard data. These special keys include such things as up/down
arrows, home and end keys, etc. If this not defined, only 7-bit print-
able and control ASCII characters will be provided to the user.
Requires CONFIG_HIDKBD_ENCODED && CONFIG_LIB_KBDCODEC
endif
examples/igmp
^^^^^^^^^^^^^
@ -482,6 +579,32 @@ examples/igmp
CONFIGURED_APPS += uiplib
examples/json
^^^^^^^^^^^^^
This example exercises the cJSON implementation at apps/netutils/json.
This example contains logic taken from the cJSON project:
http://sourceforge.net/projects/cjson/
The example corresponds to SVN revision r42 (with lots of changes for
NuttX coding standards). As of r42, the SVN repository was last updated
on 2011-10-10 so I presume that the code is stable and there is no risk
of maintaining duplicate logic in the NuttX repository.
examples/keypadtest
^^^^^^^^^^^^^^^^^^^
This is a generic keypad test example. It is similar to the USB hidkbd
example, but makes no assumptions about the underlying keyboard interface.
It uses the interfaces of include/nuttx/input/keypad.h.
CONFIG_EXAMPLES_KEYPADTEST - Selects the keypadtest example (only need
if the mconf/Kconfig tool is used.
CONFIG_EXAMPLES_KEYPAD_DEVNAME - The name of the keypad device that will
be opened in order to perform the keypad test. Default: "/dev/keypad"
examples/lcdrw
^^^^^^^^^^^^^^
@ -496,6 +619,11 @@ examples/lcdrw
* CONFIG_EXAMPLES_LDCRW_YRES
LCD Y resolution. Default: 320
NOTE: This test exercises internal lcd driver interfaces. As such, it
relies on internal OS interfaces that are not normally available to a
user-space program. As a result, this example cannot be used if a
NuttX is built as a protected, supervisor kernel (CONFIG_NUTTX_KERNEL).
examples/mm
^^^^^^^^^^^
@ -838,8 +966,6 @@ examplex/nxlines
The following configuration options can be selected:
CONFIG_EXAMPLES_NXLINES_BUILTIN -- Build the NXLINES example as a "built-in"
that can be executed from the NSH command line
CONFIG_EXAMPLES_NXLINES_VPLANE -- The plane to select from the frame-
buffer driver for use in the test. Default: 0
CONFIG_EXAMPLES_NXLINES_DEVNO - The LCD device to select from the LCD
@ -877,6 +1003,9 @@ examplex/nxlines
FAR struct fb_vtable_s *up_nxdrvinit(unsigned int devno);
#endif
CONFIG_NSH_BUILTIN_APPS - Build the NX lines examples as an NSH built-in
function.
examples/nxtext
^^^^^^^^^^^^^^^
@ -984,6 +1113,17 @@ examples/ostest
Specifies the number of threads to create in the barrier
test. The default is 8 but a smaller number may be needed on
systems without sufficient memory to start so many threads.
* CONFIG_EXAMPLES_OSTEST_RR_RANGE
During round-robin scheduling test two threads are created. Each of the threads
searches for prime numbers in the configurable range, doing that configurable
number of times.
This value specifies the end of search range and together with number of runs
allows to configure the length of this test - it should last at least a few
tens of seconds. Allowed values [1; 32767], default 10000
* CONFIG_EXAMPLES_OSTEST_RR_RUNS
During round-robin scheduling test two threads are created. Each of the threads
searches for prime numbers in the configurable range, doing that configurable
number of times.
examples/pashello
^^^^^^^^^^^^^^^^^
@ -1062,6 +1202,80 @@ examples/poll
CONFIGURED_APPS += uiplib
examples/posix_spawn
^^^^^^^^^^^^^^^^^^^^
This is a simple test of the posix_spawn() API. The example derives from
examples/elf. As a result, these tests are built using the relocatable
ELF format installed in a ROMFS file system. At run time, the test program
in the ROMFS file system is spawned using posix_spawn().
Requires:
CONFIG_BINFMT_DISABLE=n - Don't disable the binary loader
CONFIG_ELF=y - Enable ELF binary loader
CONFIG_LIBC_EXECFUNCS=y - Enable support for posix_spawn
CONFIG_EXECFUNCS_SYMTAB="exports" - The name of the symbol table
created by the test.
CONFIG_EXECFUNCS_NSYMBOLS=10 - Value does not matter, it will be
corrected at runtime.
CONFIG_POSIX_SPAWN_STACKSIZE=768 - This default setting.
Test-specific configuration options:
CONFIG_EXAMPLES_POSIXSPAWN_DEVMINOR - The minor device number of the ROMFS
block. driver. For example, the N in /dev/ramN. Used for registering the
RAM block driver that will hold the ROMFS file system containing the ELF
executables to be tested. Default: 0
CONFIG_EXAMPLES_POSIXSPAWN_DEVPATH - The path to the ROMFS block driver
device. This must match EXAMPLES_POSIXSPAWN_DEVMINOR. Used for
registering the RAM block driver that will hold the ROMFS file system
containing the ELF executables to be tested. Default: "/dev/ram0"
NOTES:
1. CFLAGS should be provided in CELFFLAGS. RAM and FLASH memory regions
may require long allcs. For ARM, this might be:
CELFFLAGS = $(CFLAGS) -mlong-calls
Similarly for C++ flags which must be provided in CXXELFFLAGS.
2. Your top-level nuttx/Make.defs file must also include an approproate
definition, LDELFFLAGS, to generate a relocatable ELF object. With GNU
LD, this should include '-r' and '-e main' (or _main on some platforms).
LDELFFLAGS = -r -e main
If you use GCC to link, you make also need to include '-nostdlib' or
'-nostartfiles' and '-nodefaultlibs'.
3. This example also requires genromfs. genromfs can be build as part of the
nuttx toolchain. Or can built from the genromfs sources that can be found
at misc/tools/genromfs-0.5.2.tar.gz. In any event, the PATH variable must
include the path to the genromfs executable.
4. ELF size: The ELF files in this example are, be default, quite large
because they include a lot of "build garbage". You can greatly reduce the
size of the ELF binaries are using the 'objcopy --strip-unneeded' command to
remove un-necessary information from the ELF files.
5. Simulator. You cannot use this example with the the NuttX simulator on
Cygwin. That is because the Cygwin GCC does not generate ELF file but
rather some Windows-native binary format.
If you really want to do this, you can create a NuttX x86 buildroot toolchain
and use that be build the ELF executables for the ROMFS file system.
6. Linker scripts. You might also want to use a linker scripts to combine
sections better. An example linker script is at nuttx/binfmt/libelf/gnu-elf.ld.
That example might have to be tuned for your particular linker output to
position additional sections correctly. The GNU LD LDELFFLAGS then might
be:
LDELFFLAGS = -r -e main -T$(TOPDIR)/binfmt/libelf/gnu-elf.ld
examples/pwm
^^^^^^^^^^^^
@ -1110,17 +1324,28 @@ examples/qencoder
Specific configuration options for this example include:
CONFIG_EXAMPLES_QENCODER_DEVPATH - The path to the QE device. Default:
/dev/qe0
CONFIG_EXAMPLES_QENCODER_NSAMPLES - If CONFIG_NSH_BUILTIN_APPS
is defined, then the number of samples is provided on the command line
and this value is ignored. Otherwise, this number of samples is
collected and the program terminates. Default: Samples are collected
indefinitely.
CONFIG_EXAMPLES_QENCODER_DELAY - This value provides the delay (in
milliseonds) between each sample. If CONFIG_NSH_BUILTIN_APPS
is defined, then this value is the default delay if no other delay is
provided on the command line. Default: 100 milliseconds
CONFIG_EXAMPLES_QENCODER_DEVPATH - The path to the QE device. Default:
/dev/qe0
CONFIG_EXAMPLES_QENCODER_NSAMPLES - If CONFIG_NSH_BUILTIN_APPS
is defined, then the number of samples is provided on the command line
and this value is ignored. Otherwise, this number of samples is
collected and the program terminates. Default: Samples are collected
indefinitely.
CONFIG_EXAMPLES_QENCODER_DELAY - This value provides the delay (in
milliseonds) between each sample. If CONFIG_NSH_BUILTIN_APPS
is defined, then this value is the default delay if no other delay is
provided on the command line. Default: 100 milliseconds
examples/relays
^^^^^^^^^^^^^^^
Requires CONFIG_ARCH_RELAYS.
Contributed by Darcy Gong.
NOTE: This test exercises internal relay driver interfaces. As such, it
relies on internal OS interfaces that are not normally available to a
user-space program. As a result, this example cannot be used if a
NuttX is built as a protected, supervisor kernel (CONFIG_NUTTX_KERNEL).
examples/rgmp
^^^^^^^^^^^^^
@ -1672,7 +1897,16 @@ examples/wget
CONFIGURED_APPS += resolv
CONFIGURED_APPS += webclient
examples/wget
^^^^^^^^^^^^^
Uses wget to get a JSON encoded file, then decodes the file.
CONFIG_EXAMPLES_WDGETJSON_MAXSIZE - Max. JSON Buffer Size
CONFIG_EXAMPLES_EXAMPLES_WGETJSON_URL - wget URL
examples/xmlrpc
^^^^^^^^^^^^^^^
This example exercises the "Embeddable Lightweight XML-RPC Server" which
is discussed at:

32
apps/examples/adc/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/adc/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
@ -48,10 +48,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 .
@ -76,30 +80,30 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
ifeq ($(CONFIG_NSH_BUILTIN_APPS),y)
$(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat: $(DEPCONFIG) Makefile
$(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main)
@touch $@
context: $(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat
else
context:
endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

7
apps/examples/adc/adc_main.c
View File

@ -289,7 +289,7 @@ int adc_main(int argc, char *argv[])
{
message("adc_main: open %s failed: %d\n", g_adcstate.devpath, errno);
errval = 2;
goto errout_with_dev;
goto errout;
}
/* Now loop the appropriate number of times, displaying the collected
@ -357,6 +357,11 @@ int adc_main(int argc, char *argv[])
}
}
close(fd);
return OK;
/* Error exits */
errout_with_dev:
close(fd);

32
apps/examples/buttons/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/buttons/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
@ -48,10 +48,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 .
@ -76,30 +80,30 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
ifeq ($(CONFIG_NSH_BUILTIN_APPS),y)
$(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat: $(DEPCONFIG) Makefile
$(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main)
@touch $@
context: $(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat
else
context:
endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

32
apps/examples/can/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/can/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
@ -48,10 +48,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 .
@ -76,30 +80,30 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
ifeq ($(CONFIG_NSH_BUILTIN_APPS),y)
$(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat: $(DEPCONFIG) Makefile
$(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main)
@touch $@
context: $(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat
else
context:
endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

34
apps/examples/cdcacm/Makefile
View File

@ -48,10 +48,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 .
@ -80,30 +84,34 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
ifeq ($(CONFIG_NSH_BUILTIN_APPS),y)
$(BUILTIN_REGISTRY)$(DELIM)$(APPNAME1)_main.bdat: $(DEPCONFIG) Makefile
$(call REGISTER,$(APPNAME1),$(PRIORITY1),$(STACKSIZE1),$(APPNAME1)_main)
$(call REGISTER,$(APPNAME2),$(PRIORITY2),$(STACKSIZE2),$(APPNAME2)_main)
@touch $@
context: .context
$(BUILTIN_REGISTRY)$(DELIM)$(APPNAME2)_main.bdat: $(DEPCONFIG) Makefile
$(call REGISTER,$(APPNAME2),$(PRIORITY2),$(STACKSIZE2),$(APPNAME2)_main)
context: $(BUILTIN_REGISTRY)$(DELIM)$(APPNAME1)_main.bdat $(BUILTIN_REGISTRY)$(DELIM)$(APPNAME2)_main.bdat
else
context:
endif
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

42
apps/examples/control_demo/Makefile Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# Basic example application
#
APPNAME = control_demo
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk

168
apps/examples/control_demo/control_demo.cpp Normal file
View File

@ -0,0 +1,168 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file control_demo.cpp
* Demonstration of control library
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <controllib/fixedwing.hpp>
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
#include <math.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int control_demo_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int control_demo_thread_main(int argc, char *argv[]);
/**
* Test function
*/
void test();
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int control_demo_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("control_demo already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("control_demo",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
5120,
control_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "test")) {
test();
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tcontrol_demo app is running\n");
} else {
printf("\tcontrol_demo app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int control_demo_thread_main(int argc, char *argv[])
{
printf("[control_Demo] starting\n");
using namespace control;
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
thread_running = true;
while (!thread_should_exit) {
autopilot.update();
}
printf("[control_demo] exiting.\n");
thread_running = false;
return 0;
}
void test()
{
printf("beginning control lib test\n");
control::basicBlocksTest();
}

63
apps/examples/control_demo/params.c Normal file
View File

@ -0,0 +1,63 @@
#include <systemlib/param/param.h>
// currently tuned for easystar from arkhangar in HIL
//https://github.com/arktools/arkhangar
// 16 is max name length
// gyro low pass filter
PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq
PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq
PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
// yaw washout
PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
// stabilization mode
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
// psi -> phi -> p
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll
PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg
// velocity -> theta
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle
// theta -> q
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID
PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
// h -> thr
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
// crosstrack
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain
// speed command
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); // commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity
// trim
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)

32
apps/examples/hello/Makefile
View File

@ -54,10 +54,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 .
@ -76,30 +80,30 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
ifeq ($(CONFIG_EXAMPLES_HELLO_BUILTIN),y)
ifeq ($(CONFIG_NSH_BUILTIN_APPS),y)
$(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat: $(DEPCONFIG) Makefile
$(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main)
@touch $@
context: $(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat
else
context:
endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

34
apps/examples/helloxx/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/helloxx/Makefile
#
# Copyright (C) 2009-2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2009-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -50,10 +50,14 @@ CXXOBJS = $(CXXSRCS:.cxx=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS) $(CXXSRCS)
OBJS = $(AOBJS) $(COBJS) $(CXXOBJS)
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 .
@ -69,7 +73,7 @@ STACKSIZE = 2048
VPATH =
all: .built
.PHONY: clean depend disclean chkcxx
.PHONY: clean depend distclean chkcxx
chkcxx:
ifneq ($(CONFIG_HAVE_CXX),y)
@ -93,30 +97,30 @@ $(CXXOBJS): %$(OBJEXT): %.cxx
$(call COMPILEXX, $<, $@)
.built: chkcxx $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
ifeq ($(CONFIG_EXAMPLES_HELLOXX_BUILTIN),y)
$(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat: $(DEPCONFIG) Makefile
$(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main)
@touch $@
context: $(BUILTIN_REGISTRY)$(DELIM)$(APPNAME)_main.bdat
else
context:
endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

761
apps/examples/kalman_demo/KalmanNav.cpp Normal file
View File

@ -0,0 +1,761 @@
/****************************************************************************
*
* 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 KalmanNav.cpp
*
* kalman filter navigation code
*/
#include <poll.h>
#include "KalmanNav.hpp"
// constants
// Titterton pg. 52
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
static const float R0 = 6378137.0f; // earth radius, m
static const float g0 = 9.806f; // standard gravitational accel. m/s^2
static const int8_t ret_ok = 0; // no error in function
static const int8_t ret_error = -1; // error occurred
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// ekf matrices
F(9, 9),
G(9, 6),
P(9, 9),
P0(9, 9),
V(6, 6),
// attitude measurement ekf matrices
HAtt(6, 9),
RAtt(6, 6),
// position measurement ekf matrices
HPos(5, 9),
RPos(5, 5),
// attitude representations
C_nb(),
q(),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
_predictTimeStamp(hrt_absolute_time()),
_attTimeStamp(hrt_absolute_time()),
_outTimeStamp(hrt_absolute_time()),
// frame count
_navFrames(0),
// miss counts
_miss(0),
// accelerations
fN(0), fE(0), fD(0),
// state
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
// parameters for ground station
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
_rMag(this, "R_MAG"),
_rGpsVel(this, "R_GPS_VEL"),
_rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"),
_rAccel(this, "R_ACCEL"),
_magDip(this, "ENV_MAG_DIP"),
_magDec(this, "ENV_MAG_DEC"),
_g(this, "ENV_G"),
_faultPos(this, "FAULT_POS"),
_faultAtt(this, "FAULT_ATT"),
_attitudeInitialized(false),
_positionInitialized(false),
_attitudeInitCounter(0)
{
using namespace math;
// initial state covariance matrix
P0 = Matrix::identity(9) * 0.01f;
P = P0;
// initial state
phi = 0.0f;
theta = 0.0f;
psi = 0.0f;
vN = 0.0f;
vE = 0.0f;
vD = 0.0f;
lat = 0.0f;
lon = 0.0f;
alt = 0.0f;
// initialize quaternions
q = Quaternion(EulerAngles(phi, theta, psi));
// initialize dcm
C_nb = Dcm(q);
// HPos is constant
HPos(0, 3) = 1.0f;
HPos(1, 4) = 1.0f;
HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(4, 8) = 1.0f;
// initialize all parameters
updateParams();
}
void KalmanNav::update()
{
using namespace math;
struct pollfd fds[1];
fds[0].fd = _sensors.getHandle();
fds[0].events = POLLIN;
// poll for new data
int ret = poll(fds, 1, 1000);
if (ret < 0) {
// XXX this is seriously bad - should be an emergency
return;
} else if (ret == 0) { // timeout
return;
}
// get new timestamp
uint64_t newTimeStamp = hrt_absolute_time();
// check updated subscriptions
if (_param_update.updated()) updateParams();
bool gpsUpdate = _gps.updated();
bool sensorsUpdate = _sensors.updated();
// get new information from subscriptions
// this clears update flag
updateSubscriptions();
// initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate &&
_sensors.accelerometer_counter > 10 &&
_sensors.gyro_counter > 10 &&
_sensors.magnetometer_counter > 10) {
if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) {
printf("[kalman_demo] initialized EKF attitude\n");
printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
double(phi), double(theta), double(psi));
_attitudeInitialized = true;
}
}
// initialize position when gps received
if (!_positionInitialized &&
_attitudeInitialized && // wait for attitude first
gpsUpdate &&
_gps.fix_type > 2 &&
_gps.counter_pos_valid > 10) {
vN = _gps.vel_n;
vE = _gps.vel_e;
vD = _gps.vel_d;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
_positionInitialized = true;
printf("[kalman_demo] initialized EKF state with GPS\n");
printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(vN), double(vE), double(vD),
lat, lon, alt);
}
// prediciton step
// using sensors timestamp so we can account for packet lag
float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
//printf("dt: %15.10f\n", double(dt));
_predictTimeStamp = _sensors.timestamp;
// don't predict if time greater than a second
if (dt < 1.0f) {
predictState(dt);
predictStateCovariance(dt);
// count fast frames
_navFrames += 1;
}
// count times 100 Hz rate isn't met
if (dt > 0.01f) _miss++;
// gps correction step
if (_positionInitialized && gpsUpdate) {
correctPos();
}
// attitude correction step
if (_attitudeInitialized // initialized
&& sensorsUpdate // new data
&& _sensors.timestamp - _attTimeStamp > 1e6 / 20 // 20 Hz
) {
_attTimeStamp = _sensors.timestamp;
correctAtt();
}
// publication
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
_pubTimeStamp = newTimeStamp;
updatePublications();
}
// output
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp;
printf("nav: %4d Hz, miss #: %4d\n",
_navFrames / 10, _miss / 10);
_navFrames = 0;
_miss = 0;
}
}
void KalmanNav::updatePublications()
{
using namespace math;
// position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp;
_pos.valid = true;
_pos.lat = getLatDegE7();
_pos.lon = getLonDegE7();
_pos.alt = float(alt);
_pos.relative_alt = float(alt); // TODO, make relative
_pos.vx = vN;
_pos.vy = vE;
_pos.vz = vD;
_pos.hdg = psi;
// attitude publication
_att.timestamp = _pubTimeStamp;
_att.roll = phi;
_att.pitch = theta;
_att.yaw = psi;
_att.rollspeed = _sensors.gyro_rad_s[0];
_att.pitchspeed = _sensors.gyro_rad_s[1];
_att.yawspeed = _sensors.gyro_rad_s[2];
// TODO, add gyro offsets to filter
_att.rate_offsets[0] = 0.0f;
_att.rate_offsets[1] = 0.0f;
_att.rate_offsets[2] = 0.0f;
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = C_nb(i, j);
for (int i = 0; i < 4; i++) _att.q[i] = q(i);
_att.R_valid = true;
_att.q_valid = true;
_att.counter = _navFrames;
// selectively update publications,
// do NOT call superblock do-all method
if (_positionInitialized)
_pos.update();
if (_attitudeInitialized)
_att.update();
}
int KalmanNav::predictState(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
// attitude prediction
if (_attitudeInitialized) {
Vector3 w(_sensors.gyro_rad_s);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
q = q.unit();
}
// C_nb update
C_nb = Dcm(q);
// euler update
EulerAngles euler(C_nb);
phi = euler.getPhi();
theta = euler.getTheta();
psi = euler.getPsi();
// specific acceleration in nav frame
Vector3 accelB(_sensors.accelerometer_m_s2);
Vector3 accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
}
// position prediction
if (_positionInitialized) {
// neglects angular deflections in local gravity
// see Titerton pg. 70
float R = R0 + float(alt);
float LDot = vN / R;
float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot;
float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
float vDDot = fD - vE * rotRate * cosL -
vN * LDot + _g.get();
float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL;
// rectangular integration
vN += vNDot * dt;
vE += vEDot * dt;
vD += vDDot * dt;
lat += double(LDot * dt);
lon += double(lDot * dt);
alt += double(-vD * dt);
}
return ret_ok;
}
int KalmanNav::predictStateCovariance(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSq = cosL * cosL;
float tanL = tanf(lat);
// prepare for matrix
float R = R0 + float(alt);
float RSq = R * R;
// F Matrix
// Titterton pg. 291
F(0, 1) = -(omega * sinL + vE * tanL / R);
F(0, 2) = vN / R;
F(0, 4) = 1.0f / R;
F(0, 6) = -omega * sinL;
F(0, 8) = -vE / RSq;
F(1, 0) = omega * sinL + vE * tanL / R;
F(1, 2) = omega * cosL + vE / R;
F(1, 3) = -1.0f / R;
F(1, 8) = vN / RSq;
F(2, 0) = -vN / R;
F(2, 1) = -omega * cosL - vE / R;
F(2, 4) = -tanL / R;
F(2, 6) = -omega * cosL - vE / (R * cosLSq);
F(2, 8) = vE * tanL / RSq;
F(3, 1) = -fD;
F(3, 2) = fE;
F(3, 3) = vD / R;
F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
F(3, 5) = vN / R;
F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
F(4, 0) = fD;
F(4, 2) = -fN;
F(4, 3) = 2 * omega * sinL + vE * tanL / R;
F(4, 4) = (vN * tanL + vD) / R;
F(4, 5) = 2 * omega * cosL + vE / R;
F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
vN * vE / (R * cosLSq);
F(4, 8) = -vE * (vN * tanL + vD) / RSq;
F(5, 0) = -fE;
F(5, 1) = fN;
F(5, 3) = -2 * vN / R;
F(5, 4) = -2 * (omega * cosL + vE / R);
F(5, 6) = 2 * omega * vE * sinL;
F(5, 8) = (vN * vN + vE * vE) / RSq;
F(6, 3) = 1 / R;
F(6, 8) = -vN / RSq;
F(7, 4) = 1 / (R * cosL);
F(7, 6) = vE * tanL / (R * cosL);
F(7, 8) = -vE / (cosL * RSq);
F(8, 5) = -1;
// G Matrix
// Titterton pg. 291
G(0, 0) = -C_nb(0, 0);
G(0, 1) = -C_nb(0, 1);
G(0, 2) = -C_nb(0, 2);
G(1, 0) = -C_nb(1, 0);
G(1, 1) = -C_nb(1, 1);
G(1, 2) = -C_nb(1, 2);
G(2, 0) = -C_nb(2, 0);
G(2, 1) = -C_nb(2, 1);
G(2, 2) = -C_nb(2, 2);
G(3, 3) = C_nb(0, 0);
G(3, 4) = C_nb(0, 1);
G(3, 5) = C_nb(0, 2);
G(4, 3) = C_nb(1, 0);
G(4, 4) = C_nb(1, 1);
G(4, 5) = C_nb(1, 2);
G(5, 3) = C_nb(2, 0);
G(5, 4) = C_nb(2, 1);
G(5, 5) = C_nb(2, 2);
// continuous predictioon equations
// for discrte time EKF
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
return ret_ok;
}
int KalmanNav::correctAtt()
{
using namespace math;
// trig
float cosPhi = cosf(phi);
float cosTheta = cosf(theta);
float cosPsi = cosf(psi);
float sinPhi = sinf(phi);
float sinTheta = sinf(theta);
float sinPsi = sinf(psi);
// mag measurement
Vector3 zMag(_sensors.magnetometer_ga);
//float magNorm = zMag.norm();
zMag = zMag.unit();
// mag predicted measurement
// choosing some typical magnetic field properties,
// TODO dip/dec depend on lat/ lon/ time
float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
float bN = cosf(dip) * cosf(dec);
float bE = cosf(dip) * sinf(dec);
float bD = sinf(dip);
Vector3 bNav(bN, bE, bD);
Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
// accel measurement
Vector3 zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.norm();
zAccel = zAccel.unit();
// ignore accel correction when accel mag not close to g
Matrix RAttAdjust = RAtt;
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
if (ignoreAccel) {
RAttAdjust(3, 3) = 1.0e10;
RAttAdjust(4, 4) = 1.0e10;
RAttAdjust(5, 5) = 1.0e10;
} else {
//printf("correcting attitude with accel\n");
}
// accel predicted measurement
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
// combined measurement
Vector zAtt(6);
Vector zAttHat(6);
for (int i = 0; i < 3; i++) {
zAtt(i) = zMag(i);
zAtt(i + 3) = zAccel(i);
zAttHat(i) = zMagHat(i);
zAttHat(i + 3) = zAccelHat(i);
}
// HMag , HAtt (0-2,:)
float tmp1 =
cosPsi * cosTheta * bN +
sinPsi * cosTheta * bE -
sinTheta * bD;
HAtt(0, 1) = -(
cosPsi * sinTheta * bN +
sinPsi * sinTheta * bE +
cosTheta * bD
);
HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
HAtt(1, 0) =
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
cosPhi * cosTheta * bD;
HAtt(1, 1) = sinPhi * tmp1;
HAtt(1, 2) = -(
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
);
HAtt(2, 0) = -(
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
(sinPhi * cosTheta) * bD
);
HAtt(2, 1) = cosPhi * tmp1;
HAtt(2, 2) = -(
(cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
);
// HAccel , HAtt (3-5,:)
HAtt(3, 1) = cosTheta;
HAtt(4, 0) = -cosPhi * cosTheta;
HAtt(4, 1) = sinPhi * sinTheta;
HAtt(5, 0) = sinPhi * cosTheta;
HAtt(5, 1) = cosPhi * sinTheta;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Vector y = zAtt - zAttHat; // residual
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
Matrix K = P * HAtt.transpose() * S.inverse();
Vector xCorrect = K * y;
// check correciton is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
// abort correction and return
printf("[kalman_demo] numerical failure in att correction\n");
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
if (!ignoreAccel) {
phi += xCorrect(PHI);
theta += xCorrect(THETA);
}
psi += xCorrect(PSI);
// attitude also affects nav velocities
if (_positionInitialized) {
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
}
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HAtt * P;
// fault detection
float beta = y.dot(S.inverse() * y);
if (beta > _faultAtt.get()) {
printf("fault in attitude: beta = %8.4f\n", (double)beta);
printf("y:\n"); y.print();
printf("zMagHat:\n"); zMagHat.print();
printf("zMag:\n"); zMag.print();
printf("bNav:\n"); bNav.print();
}
// update quaternions from euler
// angle correction
q = Quaternion(EulerAngles(phi, theta, psi));
return ret_ok;
}
int KalmanNav::correctPos()
{
using namespace math;
// residual
Vector y(5);
y(0) = _gps.vel_n - vN;
y(1) = _gps.vel_e - vE;
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
y(4) = double(_gps.alt) / 1.0e3 - alt;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
Matrix K = P * HPos.transpose() * S.inverse();
Vector xCorrect = K * y;
// check correction is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
// abort correction and return
printf("[kalman_demo] numerical failure in gps correction\n");
// fallback to GPS
vN = _gps.vel_n;
vE = _gps.vel_e;
vD = _gps.vel_d;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
lat += double(xCorrect(LAT));
lon += double(xCorrect(LON));
alt += double(xCorrect(ALT));
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HPos * P;
// fault detetcion
float beta = y.dot(S.inverse() * y);
if (beta > _faultPos.get()) {
printf("fault in gps: beta = %8.4f\n", (double)beta);
printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),
double(y(3) / sqrtf(RPos(3, 3))),
double(y(4) / sqrtf(RPos(4, 4))));
}
return ret_ok;
}
void KalmanNav::updateParams()
{
using namespace math;
using namespace control;
SuperBlock::updateParams();
// gyro noise
V(0, 0) = _vGyro.get(); // gyro x, rad/s
V(1, 1) = _vGyro.get(); // gyro y
V(2, 2) = _vGyro.get(); // gyro z
// accel noise
V(3, 3) = _vAccel.get(); // accel x, m/s^2
V(4, 4) = _vAccel.get(); // accel y
V(5, 5) = _vAccel.get(); // accel z
// magnetometer noise
float noiseMin = 1e-6f;
float noiseMagSq = _rMag.get() * _rMag.get();
if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
RAtt(0, 0) = noiseMagSq; // normalized direction
RAtt(1, 1) = noiseMagSq;
RAtt(2, 2) = noiseMagSq;
// accelerometer noise
float noiseAccelSq = _rAccel.get() * _rAccel.get();
// bound noise to prevent singularities
if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
RAtt(3, 3) = noiseAccelSq; // normalized direction
RAtt(4, 4) = noiseAccelSq;
RAtt(5, 5) = noiseAccelSq;
// gps noise
float R = R0 + float(alt);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
float noiseVel = _rGpsVel.get();
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
float noiseAlt = _rGpsAlt.get();
// bound noise to prevent singularities
if (noiseVel < noiseMin) noiseVel = noiseMin;
if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
if (noiseAlt < noiseMin) noiseAlt = noiseMin;
RPos(0, 0) = noiseVel * noiseVel; // vn
RPos(1, 1) = noiseVel * noiseVel; // ve
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
RPos(4, 4) = noiseAlt * noiseAlt; // h
// XXX, note that RPos depends on lat, so updateParams should
// be called if lat changes significantly
}

179
apps/examples/kalman_demo/KalmanNav.hpp Normal file
View File

@ -0,0 +1,179 @@
/****************************************************************************
*
* 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 KalmanNav.hpp
*
* kalman filter navigation code
*/
#pragma once
//#define MATRIX_ASSERT
//#define VECTOR_ASSERT
#include <nuttx/config.h>
#include <mathlib/mathlib.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <controllib/block/UOrbSubscription.hpp>
#include <controllib/block/UOrbPublication.hpp>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <poll.h>
#include <unistd.h>
/**
* Kalman filter navigation class
* http://en.wikipedia.org/wiki/Extended_Kalman_filter
* Discrete-time extended Kalman filter
*/
class KalmanNav : public control::SuperBlock
{
public:
/**
* Constructor
*/
KalmanNav(SuperBlock *parent, const char *name);
/**
* Deconstuctor
*/
virtual ~KalmanNav() {};
/**
* The main callback function for the class
*/
void update();
/**
* Publication update
*/
virtual void updatePublications();
/**
* State prediction
* Continuous, non-linear
*/
int predictState(float dt);
/**
* State covariance prediction
* Continuous, linear
*/
int predictStateCovariance(float dt);
/**
* Attitude correction
*/
int correctAtt();
/**
* Position correction
*/
int correctPos();
/**
* Overloaded update parameters
*/
virtual void updateParams();
protected:
// kalman filter
math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix G; /**< noise shaping matrix for gyro/accel */
math::Matrix P; /**< state covariance matrix */
math::Matrix P0; /**< initial state covariance matrix */
math::Matrix V; /**< gyro/ accel noise matrix */
math::Matrix HAtt; /**< attitude measurement matrix */
math::Matrix RAtt; /**< attitude measurement noise matrix */
math::Matrix HPos; /**< position measurement jacobian matrix */
math::Matrix RPos; /**< position measurement noise matrix */
// attitude
math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */
math::Quaternion q; /**< quaternion from body to nav frame */
// subscriptions
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
uint64_t _attTimeStamp; /**< attitude correction time stamp */
uint64_t _outTimeStamp; /**< output time stamp */
// frame count
uint16_t _navFrames; /**< navigation frames completed in output cycle */
// miss counts
uint16_t _miss; /**< number of times fast prediction loop missed */
// accelerations
float fN, fE, fD; /**< navigation frame acceleration */
// states
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon, alt; /**< lat, lon, alt, radians */
// parameters
control::BlockParam<float> _vGyro; /**< gyro process noise */
control::BlockParam<float> _vAccel; /**< accelerometer process noise */
control::BlockParam<float> _rMag; /**< magnetometer measurement noise */
control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */
control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */
control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */
control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
control::BlockParam<float> _magDip; /**< magnetic inclination with level */
control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
control::BlockParam<float> _g; /**< gravitational constant */
control::BlockParam<float> _faultPos; /**< fault detection threshold for position */
control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */
// status
bool _attitudeInitialized;
bool _positionInitialized;
uint16_t _attitudeInitCounter;
// accessors
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getAltE3() { return int32_t(alt * 1.0e3); }
void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
};

42
apps/examples/kalman_demo/Makefile Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# Basic example application
#
APPNAME = kalman_demo
PRIORITY = SCHED_PRIORITY_MAX - 30
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk

152
apps/examples/kalman_demo/kalman_demo.cpp Normal file
View File

@ -0,0 +1,152 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file kalman_demo.cpp
* Demonstration of control library
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "KalmanNav.hpp"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int kalman_demo_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int kalman_demo_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int kalman_demo_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("kalman_demo already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("kalman_demo",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
4096,
kalman_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tkalman_demo app is running\n");
} else {
printf("\tkalman_demo app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int kalman_demo_thread_main(int argc, char *argv[])
{
printf("[kalman_demo] starting\n");
using namespace math;
thread_running = true;
KalmanNav nav(NULL, "KF");
while (!thread_should_exit) {
nav.update();
}
printf("[kalman_demo] exiting.\n");
thread_running = false;
return 0;
}

16
apps/examples/kalman_demo/params.c Normal file
View File

@ -0,0 +1,16 @@
#include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);

42
apps/examples/math_demo/Makefile Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# Basic example application
#
APPNAME = math_demo
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 8192
include $(APPDIR)/mk/app.mk

105
apps/examples/math_demo/math_demo.cpp Normal file
View File

@ -0,0 +1,105 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file math_demo.cpp
* Demonstration of math library
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
/**
* Management function.
*/
extern "C" __EXPORT int math_demo_main(int argc, char *argv[]);
/**
* Test function
*/
void test();
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: math_demo {test}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int math_demo_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "test")) {
test();
exit(0);
}
usage("unrecognized command");
exit(1);
}
void test()
{
printf("beginning math lib test\n");
using namespace math;
vectorTest();
matrixTest();
vector3Test();
eulerAnglesTest();
quaternionTest();
dcmTest();
}

23
apps/examples/mm/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/mm/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
@ -48,10 +48,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 .
@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
context:
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

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