mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
Added matrix based actuator mixing. Note the makefile is set to use the original actuator code.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1698 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
les
les
parent
24840b461d
commit
875437f483
@ -1,24 +1,24 @@
|
||||
#####
|
||||
# Project: OpenPilot
|
||||
#
|
||||
#
|
||||
#
|
||||
#
|
||||
# Makefile for OpenPilot project build PiOS and the AP.
|
||||
#
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
|
||||
#
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
#
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
# for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
#####
|
||||
|
||||
@ -30,7 +30,7 @@ DEBUG ?= YES
|
||||
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
|
||||
ENABLE_DEBUG_PINS ?= NO
|
||||
|
||||
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
|
||||
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
|
||||
ENABLE_AUX_UART ?= NO
|
||||
|
||||
#
|
||||
@ -38,7 +38,7 @@ USE_BOOTLOADER ?= NO
|
||||
|
||||
|
||||
# Set to YES when using Code Sourcery toolchain
|
||||
CODE_SOURCERY ?= NO
|
||||
CODE_SOURCERY ?= YES
|
||||
|
||||
# Toolchain prefix (i.e arm-elf- -> arm-elf-gcc.exe)
|
||||
TCHAIN_PREFIX ?= arm-none-eabi-
|
||||
@ -52,15 +52,21 @@ endif
|
||||
|
||||
FLASH_TOOL = OPENOCD
|
||||
|
||||
# YES enables -mthumb option to flags for source-files listed
|
||||
# YES enables -mthumb option to flags for source-files listed
|
||||
# in SRC and CPPSRC
|
||||
USE_THUMB_MODE = YES
|
||||
|
||||
# List of modules to include
|
||||
MODULES = Telemetry GPS ManualControl Actuator Altitude AHRSComms Stabilization/simple/Stabilization Watchdog
|
||||
MODULES = Telemetry GPS ManualControl Altitude AHRSComms Stabilization/simple/Stabilization Watchdog
|
||||
#matrix based actuator mixer
|
||||
#MODULES += Actuator/matrix/Actuator
|
||||
#original actuator with separate mixers for fixed wing, heli and VTOL
|
||||
MODULES += Actuator
|
||||
|
||||
|
||||
#MODULES = Telemetry Example
|
||||
#MODULES = Telemetry MK/MKSerial
|
||||
#MODULES = Telemetry
|
||||
#MODULES = Telemetry
|
||||
#MODULES += Osd/OsdEtStd
|
||||
|
||||
|
||||
@ -120,7 +126,7 @@ DOXYGENDIR = ../Doc/Doxygen
|
||||
# List C source files here. (C dependencies are automatically generated.)
|
||||
# use file-extension c for "c-only"-files
|
||||
|
||||
MODNAMES = $(notdir ${MODULES})
|
||||
MODNAMES = $(notdir ${MODULES})
|
||||
|
||||
ifndef TESTAPP
|
||||
## MODULES
|
||||
@ -180,6 +186,9 @@ SRC += $(OPUAVOBJ)/homelocation.c
|
||||
SRC += $(OPUAVOBJ)/attitudesettings.c
|
||||
SRC += $(OPUAVOBJ)/vtolsettings.c
|
||||
SRC += $(OPUAVOBJ)/vtolstatus.c
|
||||
SRC += $(OPUAVOBJ)/mixersettings.c
|
||||
SRC += $(OPUAVOBJ)/mixerstatus.c
|
||||
#SRC += $(OPUAVOBJ)/lesstabilizationsettings.c
|
||||
endif
|
||||
|
||||
## PIOS Hardware (STM32F10x)
|
||||
@ -254,11 +263,11 @@ SRC += $(STMUSBSRCDIR)/usb_regs.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_sil.c
|
||||
|
||||
## RTOS
|
||||
SRC += $(RTOSSRCDIR)/list.c
|
||||
SRC += $(RTOSSRCDIR)/queue.c
|
||||
SRC += $(RTOSSRCDIR)/list.c
|
||||
SRC += $(RTOSSRCDIR)/queue.c
|
||||
SRC += $(RTOSSRCDIR)/tasks.c
|
||||
|
||||
## RTOS Portable
|
||||
## RTOS Portable
|
||||
SRC += $(RTOSSRCDIR)/portable/GCC/ARM_CM3/port.c
|
||||
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_2.c
|
||||
|
||||
@ -277,16 +286,16 @@ SRC += $(DOSFSDIR)/dfs_sdcard.c
|
||||
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
|
||||
# use file-extension c for "c-only"-files
|
||||
## just for testing, timer.c could be compiled in thumb-mode too
|
||||
SRCARM =
|
||||
SRCARM =
|
||||
|
||||
# List C++ source files here.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
CPPSRC =
|
||||
CPPSRC =
|
||||
|
||||
# List C++ source files here which must be compiled in ARM-Mode.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
CPPSRCARM =
|
||||
|
||||
# List Assembler source files here.
|
||||
# Make them always end in a capital .S. Files ending in a lowercase .s
|
||||
@ -298,7 +307,7 @@ CPPSRCARM =
|
||||
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL)_OP.S
|
||||
|
||||
# List Assembler source files here which must be assembled in ARM-Mode..
|
||||
ASRCARM =
|
||||
ASRCARM =
|
||||
|
||||
# List any extra directories to look for include files here.
|
||||
# Each directory must be seperated by a space.
|
||||
@ -330,11 +339,11 @@ EXTRAINCDIRS += ${foreach MOD, ${MODULES}, Modules/${MOD}/inc} ${OPMODULEDIR}/Sy
|
||||
# Also add directories where the linker should search for
|
||||
# includes from linker-script to the list
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRA_LIBDIRS =
|
||||
EXTRA_LIBDIRS =
|
||||
|
||||
# Extra Libraries
|
||||
# Each library-name must be seperated by a space.
|
||||
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
|
||||
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
|
||||
# EXTRA_LIBS = xyz abc efsl
|
||||
# for newlib-lpc (file: libnewlibc-lpc.a):
|
||||
# EXTRA_LIBS = newlib-lpc
|
||||
@ -343,7 +352,7 @@ EXTRA_LIBS =
|
||||
# Path to Linker-Scripts
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# 0 = turn off optimization. s = optimize for size.
|
||||
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
|
||||
|
||||
@ -354,7 +363,7 @@ OPT = s
|
||||
endif
|
||||
|
||||
# Output format. (can be ihex or binary or both)
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# ihex to create a load-image in Intel hex format
|
||||
#LOADFORMAT = ihex
|
||||
#LOADFORMAT = binary
|
||||
@ -363,7 +372,7 @@ LOADFORMAT = both
|
||||
# Debugging format.
|
||||
DEBUGF = dwarf-2
|
||||
|
||||
# Place project-specific -D (define) and/or
|
||||
# Place project-specific -D (define) and/or
|
||||
# -U options for C here.
|
||||
CDEFS = -DSTM32F10X_$(MODEL)
|
||||
CDEFS += -DUSE_STDPERIPH_DRIVER
|
||||
@ -379,7 +388,7 @@ CDEFS += -DUSE_BOOTLOADER
|
||||
endif
|
||||
|
||||
|
||||
# Place project-specific -D and/or -U options for
|
||||
# Place project-specific -D and/or -U options for
|
||||
# Assembler with preprocessor here.
|
||||
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
|
||||
ADEFS = -D__ASSEMBLY__
|
||||
@ -413,18 +422,20 @@ CFLAGS += -mcpu=$(MCU) -mthumb
|
||||
CFLAGS += $(CDEFS)
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
|
||||
CFLAGS += -mapcs-frame
|
||||
CFLAGS += -mapcs-frame
|
||||
CFLAGS += -fomit-frame-pointer
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Werror
|
||||
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
|
||||
# flags only for C
|
||||
#CONLYFLAGS += -Wnested-externs
|
||||
#CONLYFLAGS += -Wnested-externs
|
||||
CONLYFLAGS += $(CSTANDARD)
|
||||
|
||||
# Assembler flags.
|
||||
@ -446,7 +457,7 @@ LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
|
||||
LDFLAGS += -lc
|
||||
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lc -lgcc
|
||||
LDFLAGS += -lc -lgcc
|
||||
|
||||
# Set linker-script name depending on selected submodel name
|
||||
LDFLAGS +=-T$(LINKERSCRIPTPATH)/link_stm32f10x_$(BOOT_MODEL).ld
|
||||
@ -465,7 +476,7 @@ UNAME := $(shell uname)
|
||||
ifeq ($(UNAME), Darwin)
|
||||
OOCD_CL+=-f ../Project/OpenOCD/floss-jtag.openpilot.osx.cfg -f ../Project/OpenOCD/stm32.cfg
|
||||
else
|
||||
OOCD_CL+=-f ../Project/OpenOCD/floss-jtag.openpilot.cfg -f ../Project/OpenOCD/stm32.cfg
|
||||
OOCD_CL+=-f ../Project/OpenOCD/floss-jtag-revb.cfg -f ../Project/OpenOCD/stm32.cfg
|
||||
endif
|
||||
# initialize
|
||||
OOCD_CL+=-c init
|
||||
@ -473,10 +484,10 @@ OOCD_CL+=-c init
|
||||
OOCD_CL+=-c targets
|
||||
# commands to prepare flash-write
|
||||
OOCD_CL+= -c "reset halt"
|
||||
# flash erase
|
||||
# flash erase
|
||||
OOCD_CL+=-c "stm32x mass_erase 0"
|
||||
# flash-write
|
||||
OOCD_CL+=-c "flash write_image $(OOCD_LOADFILE)"
|
||||
OOCD_CL+=-c "flash write_image $(OOCD_LOADFILE)"
|
||||
# Verify
|
||||
OOCD_CL+=-c "verify_image $(OOCD_LOADFILE)"
|
||||
# reset target
|
||||
@ -506,7 +517,7 @@ MSG_ERRORS_NONE = Errors: none
|
||||
MSG_BEGIN = ${quote}-------- begin (mode: $(RUN_MODE)) --------${quote}
|
||||
MSG_END = ${quote}-------- end --------${quote}
|
||||
MSG_MODINIT = ${quote}**** Generating ModInit.c${quote}
|
||||
MSG_SIZE_BEFORE = ${quote}Size before:${quote}
|
||||
MSG_SIZE_BEFORE = ${quote}Size before:${quote}
|
||||
MSG_SIZE_AFTER = ${quote}Size after build:${quote}
|
||||
MSG_LOAD_FILE = ${quote}Creating load file:${quote}
|
||||
MSG_EXTENDED_LISTING = ${quote}Creating Extended Listing/Disassembly:${quote}
|
||||
@ -536,7 +547,7 @@ LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
|
||||
# Define all depedency-files (used for make clean).
|
||||
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
|
||||
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
lss: $(OUTDIR)/$(TARGET).lss
|
||||
sym: $(OUTDIR)/$(TARGET).sym
|
||||
hex: $(OUTDIR)/$(TARGET).hex
|
||||
@ -548,13 +559,13 @@ all: begin gccversion build sizeafter finished end
|
||||
|
||||
ifeq ($(LOADFORMAT),ihex)
|
||||
build: elf hex lss sym
|
||||
else
|
||||
else
|
||||
ifeq ($(LOADFORMAT),binary)
|
||||
build: elf bin lss sym
|
||||
else
|
||||
else
|
||||
ifeq ($(LOADFORMAT),both)
|
||||
build: elf hex bin lss sym
|
||||
else
|
||||
else
|
||||
$(error "$(MSG_FORMATERROR) $(FORMAT)")
|
||||
endif
|
||||
endif
|
||||
@ -565,17 +576,17 @@ result = ${shell echo "test"}
|
||||
ifeq (${result}, test)
|
||||
quote = '
|
||||
else
|
||||
quote =
|
||||
quote =
|
||||
endif
|
||||
|
||||
${OUTDIR}/InitMods.c: Makefile
|
||||
@echo ${MSG_MODINIT}
|
||||
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
|
||||
|
||||
|
||||
# Eye candy.
|
||||
begin:
|
||||
## @echo
|
||||
@ -598,9 +609,9 @@ sizeafter:
|
||||
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
|
||||
@echo $(MSG_SIZE_AFTER)
|
||||
$(ELFSIZE)
|
||||
|
||||
|
||||
# Display compiler version information.
|
||||
gccversion :
|
||||
gccversion :
|
||||
@$(CC) --version
|
||||
# @echo $(ALLOBJ)
|
||||
|
||||
@ -624,7 +635,7 @@ endif
|
||||
## @echo
|
||||
@echo $(MSG_LOAD_FILE) $@
|
||||
$(OBJCOPY) -O ihex $< $@
|
||||
|
||||
|
||||
# Create final output file (.bin) from ELF output file.
|
||||
%.bin: %.elf
|
||||
## @echo
|
||||
@ -660,18 +671,18 @@ define ASSEMBLE_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_ASSEMBLING) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
|
||||
$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
|
||||
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
|
||||
|
||||
# Assemble: create object files from assembler source files. ARM-only
|
||||
define ASSEMBLE_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_ASSEMBLING_ARM) $$< to $$@
|
||||
$(CC) -c $$(ASFLAGS) $$< -o $$@
|
||||
$(CC) -c $$(ASFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
|
||||
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create object files from C source files.
|
||||
@ -679,18 +690,18 @@ define COMPILE_C_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILING) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
|
||||
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files. ARM-only
|
||||
define COMPILE_C_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILING_ARM) $$< to $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
|
||||
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create object files from C++ source files.
|
||||
@ -698,18 +709,18 @@ define COMPILE_CPP_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILINGCPP) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
|
||||
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files. ARM-only
|
||||
define COMPILE_CPP_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILINGCPP_ARM) $$< to $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
|
||||
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create assembler files from C source files. ARM/Thumb
|
||||
@ -725,7 +736,7 @@ $(SRCARM:.c=.s) : %.s : %.c
|
||||
# Generate Doxygen documents
|
||||
docs:
|
||||
doxygen $(DOXYGENDIR)/doxygen.cfg
|
||||
|
||||
|
||||
# Target: clean project.
|
||||
clean: begin clean_list finished end
|
||||
|
||||
|
||||
@ -1,11 +1,11 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @{
|
||||
* @addtogroup ActuatorModule Actuator Module
|
||||
* @brief Compute servo/motor settings based on @ref ActuatorDesired "desired actuator positions" and aircraft type.
|
||||
* This is where all the mixing of channels is computed.
|
||||
* @{
|
||||
* @{
|
||||
*
|
||||
* @file actuator.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
@ -195,15 +195,15 @@ static void actuatorTask(void* parameters)
|
||||
|
||||
// Update output object
|
||||
ActuatorCommandSet(&cmd);
|
||||
// Update in case read only (eg. during servo configuration)
|
||||
// Update in case read only (eg. during servo configuration)
|
||||
ActuatorCommandGet(&cmd);
|
||||
|
||||
|
||||
// Update servo outputs
|
||||
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
|
||||
{
|
||||
PIOS_Servo_Set( n, cmd.Channel[n] );
|
||||
}
|
||||
|
||||
|
||||
// Wait until next update
|
||||
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS );
|
||||
|
||||
@ -216,7 +216,7 @@ static void actuatorTask(void* parameters)
|
||||
*/
|
||||
static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd)
|
||||
{
|
||||
|
||||
|
||||
// Check settings
|
||||
if ( settings->FixedWingPitch1 == ACTUATORSETTINGS_FIXEDWINGPITCH1_NONE ||
|
||||
settings->FixedWingRoll1 == ACTUATORSETTINGS_FIXEDWINGROLL1_NONE ||
|
||||
@ -227,7 +227,7 @@ static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const Actuat
|
||||
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
|
||||
|
||||
// Set pitch servo command
|
||||
cmd->Channel[ settings->FixedWingPitch1 ] = scaleChannel(desired->Pitch, settings->ChannelMax[ settings->FixedWingPitch1 ],
|
||||
settings->ChannelMin[ settings->FixedWingPitch1 ],
|
||||
@ -263,7 +263,7 @@ static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const Actuat
|
||||
cmd->Channel[ settings->FixedWingThrottle ] = scaleChannel(desired->Throttle, settings->ChannelMax[ settings->FixedWingThrottle ],
|
||||
settings->ChannelMin[ settings->FixedWingThrottle ],
|
||||
settings->ChannelNeutral[ settings->FixedWingThrottle ]);
|
||||
else
|
||||
else
|
||||
cmd->Channel[ settings->FixedWingThrottle ] = -1;
|
||||
|
||||
// Done
|
||||
@ -283,10 +283,10 @@ static int32_t mixerFixedWingElevon(const ActuatorSettingsData* settings, const
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
|
||||
|
||||
// Set first elevon servo command
|
||||
cmd->Channel[ settings->FixedWingRoll1 ] = scaleChannel(desired->Pitch + desired->Roll, settings->ChannelMax[ settings->FixedWingRoll1 ],
|
||||
settings->ChannelMin[ settings->FixedWingRoll1 ],
|
||||
@ -296,24 +296,24 @@ static int32_t mixerFixedWingElevon(const ActuatorSettingsData* settings, const
|
||||
cmd->Channel[ settings->FixedWingRoll2 ] = scaleChannel(desired->Pitch - desired->Roll, settings->ChannelMax[ settings->FixedWingRoll2 ],
|
||||
settings->ChannelMin[ settings->FixedWingRoll2 ],
|
||||
settings->ChannelNeutral[ settings->FixedWingRoll2 ]);
|
||||
|
||||
|
||||
// Set throttle servo command
|
||||
if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE)
|
||||
cmd->Channel[ settings->FixedWingThrottle ] = scaleChannel(desired->Throttle, settings->ChannelMax[ settings->FixedWingThrottle ],
|
||||
settings->ChannelMin[ settings->FixedWingThrottle ],
|
||||
settings->ChannelNeutral[ settings->FixedWingThrottle ]);
|
||||
else
|
||||
else
|
||||
cmd->Channel[ settings->FixedWingThrottle ] = -1;
|
||||
|
||||
|
||||
// Done
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Mixer for VTOL (quads and octo copters). Converts desired roll,pitch,yaw and throttle to servo outputs.
|
||||
*
|
||||
* I will probably add settings to change the weighting for each control to each motor. This will allow more
|
||||
* flexible support for various motor topologies. For now hard coding in simple versions and lettings the
|
||||
*
|
||||
* I will probably add settings to change the weighting for each control to each motor. This will allow more
|
||||
* flexible support for various motor topologies. For now hard coding in simple versions and lettings the
|
||||
* scaling capabilities fix the subtlties.
|
||||
*
|
||||
* Also, because of how the Throttle ranges from 0 to 1, the motors should too!
|
||||
@ -326,32 +326,32 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
|
||||
VTOLStatusData vtolStatus;
|
||||
VTOLSettingsGet(&vtolSettings);
|
||||
VTOLStatusGet(&vtolStatus);
|
||||
|
||||
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
|
||||
|
||||
const int vtolMin = -1;
|
||||
int vtolMax = -1;
|
||||
|
||||
|
||||
if((manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE) &&
|
||||
(desired->Throttle >= 0))
|
||||
vtolMax = 1;
|
||||
else
|
||||
vtolMax = -1;
|
||||
|
||||
if(((settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) +
|
||||
(settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORS_NONE) +
|
||||
(settings->VTOLMotorE != ACTUATORSETTINGS_VTOLMOTORE_NONE) +
|
||||
(settings->VTOLMotorSE != ACTUATORSETTINGS_VTOLMOTORSE_NONE) +
|
||||
(settings->VTOLMotorS != ACTUATORSETTINGS_VTOLMOTORS_NONE) +
|
||||
(settings->VTOLMotorSW != ACTUATORSETTINGS_VTOLMOTORSW_NONE) +
|
||||
(settings->VTOLMotorW != ACTUATORSETTINGS_VTOLMOTORW_NONE) +
|
||||
else
|
||||
vtolMax = -1;
|
||||
|
||||
if(((settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) +
|
||||
(settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORS_NONE) +
|
||||
(settings->VTOLMotorE != ACTUATORSETTINGS_VTOLMOTORE_NONE) +
|
||||
(settings->VTOLMotorSE != ACTUATORSETTINGS_VTOLMOTORSE_NONE) +
|
||||
(settings->VTOLMotorS != ACTUATORSETTINGS_VTOLMOTORS_NONE) +
|
||||
(settings->VTOLMotorSW != ACTUATORSETTINGS_VTOLMOTORSW_NONE) +
|
||||
(settings->VTOLMotorW != ACTUATORSETTINGS_VTOLMOTORW_NONE) +
|
||||
(settings->VTOLMotorNW != ACTUATORSETTINGS_VTOLMOTORNW_NONE)) <= 2) {
|
||||
return -1; // can't fly with 2 or less engines (i believe)
|
||||
}
|
||||
|
||||
|
||||
if(settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) {
|
||||
vtolStatus.MotorN = desired->Throttle * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_THROTTLE] +
|
||||
vtolStatus.MotorN = desired->Throttle * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_THROTTLE] +
|
||||
desired->Pitch * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_PITCH] +
|
||||
desired->Roll * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_ROLL] +
|
||||
desired->Yaw * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_YAW];
|
||||
@ -361,7 +361,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
|
||||
settings->ChannelNeutral[settings->VTOLMotorN]);
|
||||
}
|
||||
if(settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORNE_NONE) {
|
||||
vtolStatus.MotorNE = desired->Throttle * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_THROTTLE] +
|
||||
vtolStatus.MotorNE = desired->Throttle * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_THROTTLE] +
|
||||
desired->Pitch * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_PITCH] +
|
||||
desired->Roll * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_ROLL] +
|
||||
desired->Yaw * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_YAW];
|
||||
@ -371,7 +371,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
|
||||
settings->ChannelNeutral[settings->VTOLMotorNE]);
|
||||
}
|
||||
if(settings->VTOLMotorE != ACTUATORSETTINGS_VTOLMOTORE_NONE) {
|
||||
vtolStatus.MotorE = desired->Throttle * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_THROTTLE] +
|
||||
vtolStatus.MotorE = desired->Throttle * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_THROTTLE] +
|
||||
desired->Pitch * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_PITCH] +
|
||||
desired->Roll * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_ROLL] +
|
||||
desired->Yaw * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_YAW];
|
||||
@ -381,7 +381,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
|
||||
settings->ChannelNeutral[settings->VTOLMotorE]);
|
||||
}
|
||||
if(settings->VTOLMotorSE != ACTUATORSETTINGS_VTOLMOTORSE_NONE) {
|
||||
vtolStatus.MotorSE = desired->Throttle * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_THROTTLE] +
|
||||
vtolStatus.MotorSE = desired->Throttle * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_THROTTLE] +
|
||||
desired->Pitch * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_PITCH] +
|
||||
desired->Roll * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_ROLL] +
|
||||
desired->Yaw * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_YAW];
|
||||
@ -391,7 +391,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
|
||||
settings->ChannelNeutral[settings->VTOLMotorSE]);
|
||||
}
|
||||
if(settings->VTOLMotorS != ACTUATORSETTINGS_VTOLMOTORS_NONE) {
|
||||
vtolStatus.MotorS = desired->Throttle * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_THROTTLE] +
|
||||
vtolStatus.MotorS = desired->Throttle * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_THROTTLE] +
|
||||
desired->Pitch * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_PITCH] +
|
||||
desired->Roll * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_ROLL] +
|
||||
desired->Yaw * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_YAW];
|
||||
@ -401,17 +401,17 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
|
||||
settings->ChannelNeutral[settings->VTOLMotorS]);
|
||||
}
|
||||
if(settings->VTOLMotorSW != ACTUATORSETTINGS_VTOLMOTORSW_NONE) {
|
||||
vtolStatus.MotorSW = bound(desired->Throttle * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_THROTTLE] +
|
||||
vtolStatus.MotorSW = bound(desired->Throttle * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_THROTTLE] +
|
||||
desired->Pitch * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_PITCH] +
|
||||
desired->Roll * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_ROLL] +
|
||||
desired->Yaw * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_YAW],vtolMin,vtolMax);
|
||||
cmd->Channel[settings->VTOLMotorSW] = scaleChannel(vtolStatus.MotorSW,
|
||||
cmd->Channel[settings->VTOLMotorSW] = scaleChannel(vtolStatus.MotorSW,
|
||||
settings->ChannelMax[settings->VTOLMotorSW],
|
||||
settings->ChannelMin[settings->VTOLMotorSW],
|
||||
settings->ChannelNeutral[settings->VTOLMotorSW]);
|
||||
}
|
||||
if(settings->VTOLMotorW != ACTUATORSETTINGS_VTOLMOTORW_NONE) {
|
||||
vtolStatus.MotorW = desired->Throttle * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_THROTTLE] +
|
||||
vtolStatus.MotorW = desired->Throttle * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_THROTTLE] +
|
||||
desired->Pitch * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_PITCH] +
|
||||
desired->Roll * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_ROLL] +
|
||||
desired->Yaw * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_YAW];
|
||||
@ -421,7 +421,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
|
||||
settings->ChannelNeutral[settings->VTOLMotorW]);
|
||||
}
|
||||
if(settings->VTOLMotorNW != ACTUATORSETTINGS_VTOLMOTORNW_NONE) {
|
||||
vtolStatus.MotorNW = desired->Throttle * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_THROTTLE] +
|
||||
vtolStatus.MotorNW = desired->Throttle * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_THROTTLE] +
|
||||
desired->Pitch * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_PITCH] +
|
||||
desired->Roll * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_ROLL] +
|
||||
desired->Yaw * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_YAW];
|
||||
@ -431,7 +431,7 @@ static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDes
|
||||
settings->ChannelNeutral[settings->VTOLMotorNW]);
|
||||
}
|
||||
VTOLStatusSet(&vtolStatus);
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -645,7 +645,7 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
|
||||
{
|
||||
valueScaled = (int16_t)(value*((float)(neutral-min))) + neutral;
|
||||
}
|
||||
|
||||
|
||||
if (max>min)
|
||||
{
|
||||
if( valueScaled > max ) valueScaled = max;
|
||||
@ -706,7 +706,7 @@ static float bound(float val, float min, float max)
|
||||
return val;
|
||||
}
|
||||
|
||||
/**
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
||||
@ -0,0 +1,373 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup ActuatorModule Actuator Module
|
||||
* @brief Compute servo/motor settings based on @ref ActuatorDesired "desired actuator positions" and aircraft type.
|
||||
* This is where all the mixing of channels is computed.
|
||||
* @{
|
||||
*
|
||||
* @file actuator.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Actuator module. Drives the actuators (servos, motors etc).
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "actuator.h"
|
||||
#include "actuatorsettings.h"
|
||||
//#include "vtolsettings.h"
|
||||
#include "systemsettings.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "actuatorcommand.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "mixersettings.h"
|
||||
#include "mixerstatus.h"
|
||||
|
||||
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 2
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
||||
#define FAILSAFE_TIMEOUT_MS 100
|
||||
|
||||
// Private types
|
||||
|
||||
|
||||
// Private variables
|
||||
static xQueueHandle queue;
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void actuatorTask(void* parameters);
|
||||
static int32_t RunMixers(ActuatorCommandData * command, ActuatorSettingsData* settings);
|
||||
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
|
||||
static void setFailsafe();
|
||||
static float MixerCurve(const float throttle, const float* curve);
|
||||
float ProcessMixer(const int index, const float curve1, const float curve2,
|
||||
MixerSettingsData* mixerSettings, ActuatorDesiredData* desired,
|
||||
const float period);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Module initialization
|
||||
* @return 0
|
||||
*/
|
||||
int32_t ActuatorInitialize()
|
||||
{
|
||||
// Create object queue
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
|
||||
// Listen for ExampleObject1 updates
|
||||
ActuatorDesiredConnectQueue(queue);
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Main module task
|
||||
*/
|
||||
static void actuatorTask(void* parameters)
|
||||
{
|
||||
// UAVObjEvent ev;
|
||||
portTickType lastSysTime;
|
||||
ActuatorCommandData command;
|
||||
ActuatorSettingsData settings;
|
||||
|
||||
// Set servo update frequency (done only on start-up)
|
||||
ActuatorSettingsGet(&settings);
|
||||
PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]);
|
||||
|
||||
// Go to the neutral (failsafe) values until an ActuatorDesired update is received
|
||||
setFailsafe();
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
while (1)
|
||||
{
|
||||
|
||||
ActuatorCommandGet(&command);
|
||||
ActuatorSettingsGet(&settings);
|
||||
if ( RunMixers(&command, &settings) == -1 )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
else
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
||||
}
|
||||
|
||||
// Update output object
|
||||
ActuatorCommandSet(&command);
|
||||
// Update in case read only (eg. during servo configuration)
|
||||
ActuatorCommandGet(&command);
|
||||
|
||||
// Update servo outputs
|
||||
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
|
||||
{
|
||||
PIOS_Servo_Set( n, command.Channel[n] );
|
||||
}
|
||||
// Wait until next update
|
||||
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS );
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Universal matrix based mixer for VTOL, helis and fixed wing.
|
||||
* Converts desired roll,pitch,yaw and throttle to servo/ESC outputs.
|
||||
*
|
||||
* Because of how the Throttle ranges from 0 to 1, the motors should too!
|
||||
*
|
||||
* Note this code depends on the UAVObjects for the mixers being all being the same
|
||||
* and in sequence. If you change the object definition, make sure you check the code!
|
||||
*
|
||||
* @return -1 if error, 0 if success
|
||||
*/
|
||||
|
||||
//this structure is equivalent to the UAVObjects for one mixer.
|
||||
typedef struct {
|
||||
uint8_t type;
|
||||
float matrix[5];
|
||||
} __attribute__((packed)) Mixer_t;
|
||||
|
||||
|
||||
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
|
||||
|
||||
static int32_t RunMixers(ActuatorCommandData* command, ActuatorSettingsData* settings)
|
||||
{
|
||||
SystemSettingsData sysSettings;
|
||||
MixerSettingsData mixerSettings;
|
||||
ActuatorDesiredData desired;
|
||||
MixerStatusData mixerStatus;
|
||||
|
||||
SystemSettingsGet(&sysSettings);
|
||||
MixerStatusGet(&mixerStatus);
|
||||
MixerSettingsGet (&mixerSettings);
|
||||
ActuatorDesiredGet(&desired);
|
||||
|
||||
float * status = (float *)&mixerStatus; //access status objects as an array of floats
|
||||
|
||||
|
||||
int nMixers = 0;
|
||||
Mixer_t * mixers = (Mixer_t *)&mixerSettings.Mixer0Type;
|
||||
for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
|
||||
{
|
||||
if(mixers[ct].type != MIXERSETTINGS_MIXER0TYPE_DISABLED)
|
||||
{
|
||||
nMixers ++;
|
||||
}
|
||||
}
|
||||
if(nMixers < 2) //Nothing can fly with less than two mixers.
|
||||
{
|
||||
return(-1);
|
||||
}
|
||||
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
|
||||
|
||||
float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
|
||||
float curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2);
|
||||
for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
|
||||
{
|
||||
if(mixers[ct].type != MIXERSETTINGS_MIXER0TYPE_DISABLED)
|
||||
{
|
||||
status[ct] = scaleChannel(ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, settings->UpdatePeriod),
|
||||
settings->ChannelMax[ct],
|
||||
settings->ChannelMin[ct],
|
||||
settings->ChannelNeutral[ct]);
|
||||
if(manualControl.Armed != MANUALCONTROLCOMMAND_ARMED_TRUE &&
|
||||
mixers[ct].type == MIXERSETTINGS_MIXER0TYPE_MOTOR)
|
||||
{
|
||||
command->Channel[ct] = -1; //force zero throttle
|
||||
}else
|
||||
{
|
||||
command->Channel[ct] = status[ct]; //servos always follow command
|
||||
}
|
||||
}
|
||||
}
|
||||
MixerStatusSet(&mixerStatus);
|
||||
return(0);
|
||||
}
|
||||
|
||||
/**
|
||||
*Process mixing for one actuator
|
||||
*/
|
||||
|
||||
float ProcessMixer(const int index, const float curve1, const float curve2,
|
||||
MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, const float period)
|
||||
{
|
||||
static float lastResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0};
|
||||
static float lastFilteredResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0};
|
||||
static float filterAccumulator[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0};
|
||||
Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer0Type; //pointer to array of mixers in UAVObjects
|
||||
Mixer_t * mixer = &mixers[index];
|
||||
float result = (mixer->matrix[MIXERSETTINGS_MIXER0MATRIX_THROTTLECURVE1] * curve1) +
|
||||
(mixer->matrix[MIXERSETTINGS_MIXER0MATRIX_THROTTLECURVE2] * curve2) +
|
||||
(mixer->matrix[MIXERSETTINGS_MIXER0MATRIX_ROLL] * desired->Roll) +
|
||||
(mixer->matrix[MIXERSETTINGS_MIXER0MATRIX_PITCH] * desired->Pitch) +
|
||||
(mixer->matrix[MIXERSETTINGS_MIXER0MATRIX_YAW] * desired->Yaw);
|
||||
if(mixer->type == MIXERSETTINGS_MIXER0TYPE_MOTOR)
|
||||
{
|
||||
if(result < 0) //idle throttle
|
||||
{
|
||||
result = 0;
|
||||
}
|
||||
|
||||
//feed forward
|
||||
float accumulator = filterAccumulator[index];
|
||||
accumulator += (result - lastResult[index]) * mixerSettings->FeedForward;
|
||||
lastResult[index] = result;
|
||||
result += accumulator;
|
||||
if(accumulator > 0)
|
||||
{
|
||||
float filter = mixerSettings->AccelTime / period;
|
||||
if(filter <1)
|
||||
{
|
||||
filter = 1;
|
||||
}
|
||||
accumulator -= accumulator / filter;
|
||||
}else
|
||||
{
|
||||
float filter = mixerSettings->DecelTime / period;
|
||||
if(filter <1)
|
||||
{
|
||||
filter = 1;
|
||||
}
|
||||
accumulator -= accumulator / filter;
|
||||
}
|
||||
filterAccumulator[index] = accumulator;
|
||||
result += accumulator;
|
||||
|
||||
//acceleration limit
|
||||
float dt = result - lastFilteredResult[index];
|
||||
float maxDt = mixerSettings->MaxAccel * (period / 1000);
|
||||
if(dt > maxDt) //we are accelerating too hard
|
||||
{
|
||||
result = lastFilteredResult[index] + maxDt;
|
||||
}
|
||||
lastFilteredResult[index] = result;
|
||||
}
|
||||
return(result);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
*Interpolate a throttle curve. Throttle input should be in the range 0 to 1.
|
||||
*Output is in the range 0 to 1.
|
||||
*/
|
||||
|
||||
#define MIXER_CURVE_ENTRIES 5
|
||||
|
||||
static float MixerCurve(const float throttle, const float* curve)
|
||||
{
|
||||
float scale = throttle * MIXER_CURVE_ENTRIES;
|
||||
int idx1 = scale;
|
||||
scale -= (float)idx1; //remainder
|
||||
if(curve[0] < -1)
|
||||
{
|
||||
return(throttle);
|
||||
}
|
||||
if (idx1 < 0)
|
||||
{
|
||||
idx1 = 0; //clamp to lowest entry in table
|
||||
scale = 0;
|
||||
}
|
||||
int idx2 = idx1 + 1;
|
||||
if(idx2 >= MIXER_CURVE_ENTRIES)
|
||||
{
|
||||
idx2 = MIXER_CURVE_ENTRIES -1; //clamp to highest entry in table
|
||||
if(idx1 >= MIXER_CURVE_ENTRIES)
|
||||
{
|
||||
idx1 = MIXER_CURVE_ENTRIES -1;
|
||||
}
|
||||
}
|
||||
return((curve[idx1] * (1 - scale)) + (curve[idx2] * scale));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Convert channel from -1/+1 to servo pulse duration in microseconds
|
||||
*/
|
||||
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral)
|
||||
{
|
||||
int16_t valueScaled;
|
||||
// Scale
|
||||
if ( value >= 0.0)
|
||||
{
|
||||
valueScaled = (int16_t)(value*((float)(max-neutral))) + neutral;
|
||||
}
|
||||
else
|
||||
{
|
||||
valueScaled = (int16_t)(value*((float)(neutral-min))) + neutral;
|
||||
}
|
||||
|
||||
if (max>min)
|
||||
{
|
||||
if( valueScaled > max ) valueScaled = max;
|
||||
if( valueScaled < min ) valueScaled = min;
|
||||
}
|
||||
else
|
||||
{
|
||||
if( valueScaled < max ) valueScaled = max;
|
||||
if( valueScaled > min ) valueScaled = min;
|
||||
}
|
||||
|
||||
return valueScaled;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set actuator output to the neutral values (failsafe)
|
||||
*/
|
||||
static void setFailsafe()
|
||||
{
|
||||
ActuatorCommandData command;
|
||||
ActuatorSettingsData settings;
|
||||
|
||||
ActuatorCommandGet(&command);
|
||||
ActuatorSettingsGet(&settings);
|
||||
// Reset ActuatorCommand to neutral values
|
||||
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
|
||||
{
|
||||
command.Channel[n] = settings.ChannelNeutral[n];
|
||||
}
|
||||
|
||||
// Set alarm
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
|
||||
// Update servo outputs
|
||||
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
|
||||
{
|
||||
PIOS_Servo_Set( n, command.Channel[n] );
|
||||
}
|
||||
|
||||
// Update output object
|
||||
ActuatorCommandSet(&command);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -0,0 +1,42 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup ActuatorModule Actuator Module
|
||||
* @brief Compute servo/motor settings based on @ref ActuatorDesired "desired actuator positions" and aircraft type.
|
||||
* This is where all the mixing of channels is computed.
|
||||
* @{
|
||||
*
|
||||
* @file actuator.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Actuator module. Drives the actuators (servos, motors etc).
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef ACTUATOR_H
|
||||
#define ACTUATOR_H
|
||||
|
||||
int32_t ActuatorInitialize();
|
||||
|
||||
#endif // ACTUATOR_H
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -52,6 +52,8 @@
|
||||
#include "homelocation.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
#include "mixersettings.h"
|
||||
#include "mixerstatus.h"
|
||||
#include "navigationdesired.h"
|
||||
#include "navigationsettings.h"
|
||||
#include "objectpersistence.h"
|
||||
@ -95,6 +97,8 @@ void UAVObjectsInitializeAll()
|
||||
HomeLocationInitialize();
|
||||
ManualControlCommandInitialize();
|
||||
ManualControlSettingsInitialize();
|
||||
MixerSettingsInitialize();
|
||||
MixerStatusInitialize();
|
||||
NavigationDesiredInitialize();
|
||||
NavigationSettingsInitialize();
|
||||
ObjectPersistenceInitialize();
|
||||
|
||||
@ -1,91 +1,95 @@
|
||||
TEMPLATE = lib
|
||||
TARGET = UAVObjects
|
||||
DEFINES += UAVOBJECTS_LIBRARY
|
||||
include(../../openpilotgcsplugin.pri)
|
||||
include(uavobjects_dependencies.pri)
|
||||
HEADERS += uavobjects_global.h \
|
||||
uavobject.h \
|
||||
uavmetaobject.h \
|
||||
uavobjectmanager.h \
|
||||
uavdataobject.h \
|
||||
uavobjectfield.h \
|
||||
uavobjectsinit.h \
|
||||
uavobjectsplugin.h \
|
||||
examplesettings.h \
|
||||
ahrsstatus.h \
|
||||
ahrscalibration.h \
|
||||
baroaltitude.h \
|
||||
attitudeactual.h \
|
||||
ahrssettings.h \
|
||||
exampleobject2.h \
|
||||
exampleobject1.h \
|
||||
gcstelemetrystats.h \
|
||||
attituderaw.h \
|
||||
flighttelemetrystats.h \
|
||||
systemstats.h \
|
||||
systemalarms.h \
|
||||
objectpersistence.h \
|
||||
telemetrysettings.h \
|
||||
systemsettings.h \
|
||||
stabilizationsettings.h \
|
||||
flightsituationactual.h \
|
||||
manualcontrolsettings.h \
|
||||
manualcontrolcommand.h \
|
||||
attitudedesired.h \
|
||||
actuatorsettings.h \
|
||||
actuatordesired.h \
|
||||
actuatorcommand.h \
|
||||
navigationsettings.h \
|
||||
navigationdesired.h \
|
||||
gpsposition.h \
|
||||
gpstime.h \
|
||||
gpssatellites.h \
|
||||
positionactual.h \
|
||||
flightbatterystate.h \
|
||||
homelocation.h \
|
||||
vtolsettings.h \
|
||||
vtolstatus.h \
|
||||
attitudesettings.h
|
||||
SOURCES += uavobject.cpp \
|
||||
uavmetaobject.cpp \
|
||||
uavobjectmanager.cpp \
|
||||
uavdataobject.cpp \
|
||||
uavobjectfield.cpp \
|
||||
uavobjectsinit.cpp \
|
||||
uavobjectsplugin.cpp \
|
||||
ahrsstatus.cpp \
|
||||
ahrscalibration.cpp \
|
||||
baroaltitude.cpp \
|
||||
attitudeactual.cpp \
|
||||
ahrssettings.cpp \
|
||||
examplesettings.cpp \
|
||||
exampleobject2.cpp \
|
||||
exampleobject1.cpp \
|
||||
gcstelemetrystats.cpp \
|
||||
attituderaw.cpp \
|
||||
flighttelemetrystats.cpp \
|
||||
systemstats.cpp \
|
||||
systemalarms.cpp \
|
||||
objectpersistence.cpp \
|
||||
telemetrysettings.cpp \
|
||||
systemsettings.cpp \
|
||||
stabilizationsettings.cpp \
|
||||
flightsituationactual.cpp \
|
||||
manualcontrolsettings.cpp \
|
||||
manualcontrolcommand.cpp \
|
||||
attitudedesired.cpp \
|
||||
actuatorsettings.cpp \
|
||||
actuatordesired.cpp \
|
||||
actuatorcommand.cpp \
|
||||
navigationsettings.cpp \
|
||||
navigationdesired.cpp \
|
||||
gpsposition.cpp \
|
||||
gpstime.cpp \
|
||||
gpssatellites.cpp \
|
||||
positionactual.cpp \
|
||||
flightbatterystate.cpp \
|
||||
homelocation.cpp \
|
||||
vtolsettings.cpp \
|
||||
vtolstatus.cpp \
|
||||
attitudesettings.cpp
|
||||
OTHER_FILES += UAVObjects.pluginspec
|
||||
TEMPLATE = lib
|
||||
TARGET = UAVObjects
|
||||
DEFINES += UAVOBJECTS_LIBRARY
|
||||
include(../../openpilotgcsplugin.pri)
|
||||
include(uavobjects_dependencies.pri)
|
||||
HEADERS += uavobjects_global.h \
|
||||
uavobject.h \
|
||||
uavmetaobject.h \
|
||||
uavobjectmanager.h \
|
||||
uavdataobject.h \
|
||||
uavobjectfield.h \
|
||||
uavobjectsinit.h \
|
||||
uavobjectsplugin.h \
|
||||
examplesettings.h \
|
||||
ahrsstatus.h \
|
||||
ahrscalibration.h \
|
||||
baroaltitude.h \
|
||||
attitudeactual.h \
|
||||
ahrssettings.h \
|
||||
exampleobject2.h \
|
||||
exampleobject1.h \
|
||||
gcstelemetrystats.h \
|
||||
attituderaw.h \
|
||||
flighttelemetrystats.h \
|
||||
systemstats.h \
|
||||
systemalarms.h \
|
||||
objectpersistence.h \
|
||||
telemetrysettings.h \
|
||||
systemsettings.h \
|
||||
stabilizationsettings.h \
|
||||
flightsituationactual.h \
|
||||
manualcontrolsettings.h \
|
||||
manualcontrolcommand.h \
|
||||
attitudedesired.h \
|
||||
actuatorsettings.h \
|
||||
actuatordesired.h \
|
||||
actuatorcommand.h \
|
||||
navigationsettings.h \
|
||||
navigationdesired.h \
|
||||
gpsposition.h \
|
||||
gpstime.h \
|
||||
gpssatellites.h \
|
||||
positionactual.h \
|
||||
flightbatterystate.h \
|
||||
homelocation.h \
|
||||
vtolsettings.h \
|
||||
vtolstatus.h \
|
||||
attitudesettings.h \
|
||||
mixersettings.h \
|
||||
mixerstatus.h
|
||||
SOURCES += uavobject.cpp \
|
||||
uavmetaobject.cpp \
|
||||
uavobjectmanager.cpp \
|
||||
uavdataobject.cpp \
|
||||
uavobjectfield.cpp \
|
||||
uavobjectsinit.cpp \
|
||||
uavobjectsplugin.cpp \
|
||||
ahrsstatus.cpp \
|
||||
ahrscalibration.cpp \
|
||||
baroaltitude.cpp \
|
||||
attitudeactual.cpp \
|
||||
ahrssettings.cpp \
|
||||
examplesettings.cpp \
|
||||
exampleobject2.cpp \
|
||||
exampleobject1.cpp \
|
||||
gcstelemetrystats.cpp \
|
||||
attituderaw.cpp \
|
||||
flighttelemetrystats.cpp \
|
||||
systemstats.cpp \
|
||||
systemalarms.cpp \
|
||||
objectpersistence.cpp \
|
||||
telemetrysettings.cpp \
|
||||
systemsettings.cpp \
|
||||
stabilizationsettings.cpp \
|
||||
flightsituationactual.cpp \
|
||||
manualcontrolsettings.cpp \
|
||||
manualcontrolcommand.cpp \
|
||||
attitudedesired.cpp \
|
||||
actuatorsettings.cpp \
|
||||
actuatordesired.cpp \
|
||||
actuatorcommand.cpp \
|
||||
navigationsettings.cpp \
|
||||
navigationdesired.cpp \
|
||||
gpsposition.cpp \
|
||||
gpstime.cpp \
|
||||
gpssatellites.cpp \
|
||||
positionactual.cpp \
|
||||
flightbatterystate.cpp \
|
||||
homelocation.cpp \
|
||||
vtolsettings.cpp \
|
||||
vtolstatus.cpp \
|
||||
attitudesettings.cpp \
|
||||
mixersettings.cpp \
|
||||
mixerstatus.cpp
|
||||
OTHER_FILES += UAVObjects.pluginspec
|
||||
|
||||
@ -54,6 +54,8 @@
|
||||
#include "homelocation.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
#include "mixersettings.h"
|
||||
#include "mixerstatus.h"
|
||||
#include "navigationdesired.h"
|
||||
#include "navigationsettings.h"
|
||||
#include "objectpersistence.h"
|
||||
@ -97,6 +99,8 @@ void UAVObjectsInitialize(UAVObjectManager* objMngr)
|
||||
objMngr->registerObject( new HomeLocation() );
|
||||
objMngr->registerObject( new ManualControlCommand() );
|
||||
objMngr->registerObject( new ManualControlSettings() );
|
||||
objMngr->registerObject( new MixerSettings() );
|
||||
objMngr->registerObject( new MixerStatus() );
|
||||
objMngr->registerObject( new NavigationDesired() );
|
||||
objMngr->registerObject( new NavigationSettings() );
|
||||
objMngr->registerObject( new ObjectPersistence() );
|
||||
|
||||
31
ground/src/shared/uavobjectdefinition/mixersettings.xml
Normal file
31
ground/src/shared/uavobjectdefinition/mixersettings.xml
Normal file
@ -0,0 +1,31 @@
|
||||
<xml>
|
||||
<object name="MixerSettings" singleinstance="true" settings="true">
|
||||
<description>Settings for the @ref ActuatorModule that controls the channel assignments for the mixer based on AircraftType</description>
|
||||
<field name="MaxAccel" units="units/sec" type="float" elements="1" defaultvalue="1000"/>
|
||||
<field name="FeedForward" units="" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="AccelTime" units="ms" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="DecelTime" units="ms" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="ThrottleCurve1" units="percent" type="float" elements="5" elementnames="0,25,50,75,100" defaultvalue="-10"/>
|
||||
<field name="ThrottleCurve2" units="percent" type="float" elements="5" elementnames="0,25,50,75,100" defaultvalue="-10"/>
|
||||
<field name="Mixer0Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo" defaultvalue="Disabled"/>
|
||||
<field name="Mixer0Matrix" units="" type="float" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Mixer1Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo" defaultvalue="Disabled"/>
|
||||
<field name="Mixer1Matrix" units="" type="float" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Mixer2Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo" defaultvalue="Disabled"/>
|
||||
<field name="Mixer2Matrix" units="" type="float" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Mixer3Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo" defaultvalue="Disabled"/>
|
||||
<field name="Mixer3Matrix" units="" type="float" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Mixer4Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo" defaultvalue="Disabled"/>
|
||||
<field name="Mixer4Matrix" units="" type="float" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Mixer5Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo" defaultvalue="Disabled"/>
|
||||
<field name="Mixer5Matrix" units="" type="float" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Mixer6Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo" defaultvalue="Disabled"/>
|
||||
<field name="Mixer6Matrix" units="" type="float" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Mixer7Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo" defaultvalue="Disabled"/>
|
||||
<field name="Mixer7Matrix" units="" type="float" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
<logging updatemode="never" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
17
ground/src/shared/uavobjectdefinition/mixerstatus.xml
Normal file
17
ground/src/shared/uavobjectdefinition/mixerstatus.xml
Normal file
@ -0,0 +1,17 @@
|
||||
<xml>
|
||||
<object name="MixerStatus" singleinstance="true" settings="false">
|
||||
<description>Status for the matrix mixer showing the output of each mixer after all scaling</description>
|
||||
<field name="Mixer0" units="us" type="float" elements="1"/>
|
||||
<field name="Mixer1" units="us" type="float" elements="1"/>
|
||||
<field name="Mixer2" units="us" type="float" elements="1"/>
|
||||
<field name="Mixer3" units="us" type="float" elements="1"/>
|
||||
<field name="Mixer4" units="us" type="float" elements="1"/>
|
||||
<field name="Mixer5" units="us" type="float" elements="1"/>
|
||||
<field name="Mixer6" units="us" type="float" elements="1"/>
|
||||
<field name="Mixer7" units="us" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
<logging updatemode="periodic" period="1000"/>
|
||||
</object>
|
||||
</xml>
|
||||
Loading…
x
Reference in New Issue
Block a user