Merge branch 'master' into ahrs_refactor

* master: (175 commits)
  [tests] fix math tests
  [fixedwing] use min/max_cruise_throttle variables instead of defines
  [conf] update some telemetry files
  [ext] update luftboot
  [ext] update libopencm3
  [mavlink] add sending of origin and waypoints
  [mavlink] rename SETTINGS to SETTINGS_NAMES_SHORT
  [mavlink] send quaternion
  [mavlink] parse PARAM_SET
  [conf] add AUTOPILOT_VERSION to some default telemetry files
  [conf] add python real time plotter to control panel
  [python] messagesapp: growable value column width
  [auto1-flaps] Please remove this as soon a better solution exits. For now it is critical to fly. closing #878
  [conf] vivify fix digital_cam_uart missing settings
  [conf] obc fix nav_airspeed names, and enable AMSL
  fourth release candidate for next stable release v5.4
  [server] close file even if file empty
  [build] improve build version stuff
  Add some more flexibility to conf and control_panel naming
  [paparazzicenter] fix GLib-CRITICAL warning when running clean/build/upload
  ...

Conflicts:
	sw/airborne/boards/ardrone/actuators_at.c
	sw/airborne/firmwares/fixedwing/main_ap.c
	sw/airborne/firmwares/rotorcraft/main.c
	sw/airborne/modules/geo_mag/geo_mag.c
	sw/airborne/modules/ins/ahrs_chimu_spi.c
	sw/airborne/modules/ins/ahrs_chimu_uart.c
	sw/airborne/subsystems/ins/ins_ardrone2.c
	sw/airborne/test/subsystems/test_ahrs.c
This commit is contained in:
Felix Ruess
2015-01-02 12:56:05 +01:00
1271 changed files with 85121 additions and 22857 deletions
+135 -8
View File
@@ -1,7 +1,70 @@
Paparazzi 5.3_devel
===================
Paparazzi 5.3.3_testing
=======================
currently ongoing development, changes so far (no particular order, nor complete)
Fourth release candidate for v5.4 stable release.
- tmtc: fix rx_lost_time in link
- paparazzicenter: fix GLib-CRITICAL warning when running clean/build/upload
- server: write paparzzi_version as comment at beginning of log file
[#1049] (https://github.com/paparazzi/paparazzi/pull/1049)
- replace BOOT message with AUTOPILOT_VERSION, show in GCS and add build version
[#1050] (https://github.com/paparazzi/paparazzi/pull/1050)
- GCS: fix green color after colormap changes on some new systems
[#1051] (https://github.com/paparazzi/paparazzi/pull/1051)
[#575] (https://github.com/paparazzi/paparazzi/issues/575)
- settings: display ? for current value on undo
[#1054] (https://github.com/paparazzi/paparazzi/issues/1054)
- Add some more flexibility to conf and control_panel naming
[#1055] (https://github.com/paparazzi/paparazzi/pull/1055)
Paparazzi 5.3.2_testing
=======================
Third release candidate for v5.4 stable release.
- code style: fix in (nearly) all airborne code
- support Piksi GPS modules
[#1043] (https://github.com/paparazzi/paparazzi/pull/1043)
[#957] (https://github.com/paparazzi/paparazzi/issues/957)
- fixedwing: airspeed tuning
[#877] (https://github.com/paparazzi/paparazzi/pull/877)
- linux: handle SIGINT for console debug
[#1008] (https://github.com/paparazzi/paparazzi/pull/
- rotorcraft: always allow to switch to MODE_MANUAL via RC
[#1036] (https://github.com/paparazzi/paparazzi/pull/1036)
- flight plan: use flight plan position in nps for most example files
- airframes: OBC example airframes
[#1044] (https://github.com/paparazzi/paparazzi/pull/1044)
- transitioning: fix transition offset handling
[#1045] (https://github.com/paparazzi/paparazzi/pull/1045)
Paparazzi 5.3.1_testing
=======================
Second release candidate for v5.4 stable release.
- modules: nav_survey_poly_osam improvements
[#938] (https://github.com/paparazzi/paparazzi/pull/938)
- ARDrone2: mag freeze detection
[#1025] (https://github.com/paparazzi/paparazzi/pull/1025)
[#1026] (https://github.com/paparazzi/paparazzi/pull/1026)
[#1030] (https://github.com/paparazzi/paparazzi/pull/1030)
- ARDrone2: remove unneeded last_checksum_wrong
[#1032] (https://github.com/paparazzi/paparazzi/pull/1032)
- rotorcraft: fix USE_KILL_SWITCH_FOR_MOTOR_ARMING
[#1038] (https://github.com/paparazzi/paparazzi/pull/1038)
- server: log LINK_REPORT message
[#1031] (https://github.com/paparazzi/paparazzi/issues/1031)
- stm32: partial Lisa MX luftboot support
[#1017] (https://github.com/paparazzi/paparazzi/pull/1017)
Paparazzi 5.3.0_testing
=======================
First release candidate for v5.4 stable release.
General
-------
@@ -25,6 +88,27 @@ General
[#945] (https://github.com/paparazzi/paparazzi/pull/945)
- GCS: save size in layout
[#968] (https://github.com/paparazzi/paparazzi/issues/968)
- link/GCS: improve datalink/telemetry report and display link page in GCS for single link
[#999] (https://github.com/paparazzi/paparazzi/pull/999)
- build: default to parallel make for aircrafts
[#1002] (https://github.com/paparazzi/paparazzi/pull/1002)
- select_conf.py fixes and also selects control_panel.xml
[#1001] (https://github.com/paparazzi/paparazzi/pull/1001)
- DFU flashing: CRC support and longer timeout for Krooz
[#997] (https://github.com/paparazzi/paparazzi/pull/997)
[#691] (https://github.com/paparazzi/paparazzi/pull/691)
- messages: possibility to add descriptions
[#987] (https://github.com/paparazzi/paparazzi/pull/987)
- messages: mission modules use LLA waypoints in 1e7deg instead of float
[#986] (https://github.com/paparazzi/paparazzi/pull/986)
- GCS: alert/console window: always insert messages at the end
[#996] (https://github.com/paparazzi/paparazzi/issues/996)
- improve dependency handling for modules
[#1007] (https://github.com/paparazzi/paparazzi/pull/1007)
- Settings: display unconfirmed settins with "?" as current value
[#1013] (https://github.com/paparazzi/paparazzi/pull/1023)
- messages/plotter: improve drag-and-drop of fields from messages to realtime plotter
[#1020] (https://github.com/paparazzi/paparazzi/pull/1020)
Simulation
----------
@@ -36,30 +120,73 @@ Simulation
- NPS: add commandline option to set time_factor
- radio_conrol spektrum for sim target
[#851] (https://github.com/paparazzi/paparazzi/pull/851)
- OCaml sim: sliders in simulated RC always sensitive
[#821] (https://github.com/paparazzi/paparazzi/issues/821)
Airborne
--------
- pass dt to ahrs/ins propagation
[#818] (https://github.com/paparazzi/paparazzi/pull/818)
- cleanup math lib and convert lots of macros to functions
[#819] (https://github.com/paparazzi/paparazzi/pull/819)
- radio_control spektrum also usable for intermcu
[#847] (https://github.com/paparazzi/paparazzi/pull/847)
- Replace telemetry macros with functions
[#931] (https://github.com/paparazzi/paparazzi/pull/931)
[#1027] (https://github.com/paparazzi/paparazzi/pull/1027)
- arch: rename arch/omap to arch/linux
[#982] (https://github.com/paparazzi/paparazzi/pull/982)
- radio_control: cleanup channel defines and possibility to send less than available via intermcu
[#975] (https://github.com/paparazzi/paparazzi/pull/975)
- state interface: change computation order in stateCalcPositionLla_i
[#1013] (https://github.com/paparazzi/paparazzi/pull/1013)
- ARDrone2: Handle memory full FTP upload error
[#967] (https://github.com/paparazzi/paparazzi/issues/967)
- rotorcraft: force MODE_STARTUP instead of KILL until ahrs is aligned
[#983] (https://github.com/paparazzi/paparazzi/pull/983)
- rotorcraft: fix NavCircleCount()
- rotorcraft: datalink: check ac_id of RC_4CH message
- rotorcraft: allow to turn off motors in failsafe mode
[#989] (https://github.com/paparazzi/paparazzi/pull/989)
Modules
-------
- Convert air_data subsystem to module with QNH and true airspeed support
[#853] (https://github.com/paparazzi/paparazzi/pull/853)
- add airspeed_ms45xx_i2c module
[#852] (https://github.com/paparazzi/paparazzi/pull/852)
- Replace telemetry macros with functions
[#931] (https://github.com/paparazzi/paparazzi/pull/931)
- airspeed_ets, retry after failed transaction
- add temperature adc module
[#857] (https://github.com/paparazzi/paparazzi/pull/857)
- clean up digital_cam, usable for rotorcrafts, show real photo coordinates in GCS
[#936] (https://github.com/paparazzi/paparazzi/pull/936)
- modules: add basic mavlink module
[#1028] (https://github.com/paparazzi/paparazzi/pull/1028)
- modules: improved video/images sending for ARDrone2
[#1021]: (https://github.com/paparazzi/paparazzi/pull/1021)
Drivers/HW support
------------------
- stm32: usb_serial (CDC) impelmentation for transparent_usb telemetry
[#998] (https://github.com/paparazzi/paparazzi/pull/998)
- stm32: add usb_tunnel
[#1014] (https://github.com/paparazzi/paparazzi/pull/1014)
- Add Furuno NMEA based GPS
[#959] (https://github.com/paparazzi/paparazzi/pull/959)
- Driver for MPU9250
[#953] (https://github.com/paparazzi/paparazzi/pull/953)
- Driver for AKM8963 magnetometer
[#947] (https://github.com/paparazzi/paparazzi/pull/947)
- Basic linux I2C driver
- linux: add basic I2C and SPI drivers
[#961] (https://github.com/paparazzi/paparazzi/pull/961)
- ARDrone2: Handle memory full FTP upload error
[#967] (https://github.com/paparazzi/paparazzi/issues/967)
[#979] (https://github.com/paparazzi/paparazzi/pull/979)
- actuators: basic esc32 motor controller implementation via CAN for STM32F1
[#1004] (https://github.com/paparazzi/paparazzi/pull/1004)
- basic support for new Parrot Bebop
[#1003] (https://github.com/paparazzi/paparazzi/pull/1003)
Paparazzi 5.2.1_stable
-1
View File
@@ -1528,7 +1528,6 @@ PREDEFINED = USE_GPS \
USE_MAGNETOMETER \
USE_BAROMETER \
USE_LED \
USE_SYS_TIME \
SPI_MASTER \
SPI_SLAVE \
USE_SPI0 \
+13 -4
View File
@@ -104,11 +104,20 @@ GEN_HEADERS = $(MESSAGES_H) $(UBX_PROTOCOL_H) $(MTK_PROTOCOL_H) $(XSENS_PROTOCOL
all: ground_segment ext lpctools
print_build_version:
_print_building:
@echo "------------------------------------------------------------"
@echo "Building Paparazzi version" $(shell ./paparazzi_version)
@echo "------------------------------------------------------------"
print_build_version:
@echo "------------------------------------------------------------"
@echo "Last build Paparazzi version" $(shell cat $(PAPARAZZI_HOME)/var/build_version.txt 2> /dev/null || echo UNKNOWN)
@echo "------------------------------------------------------------"
_save_build_version:
$(Q)test -d $(PAPARAZZI_HOME)/var || mkdir -p $(PAPARAZZI_HOME)/var
$(Q)./paparazzi_version > $(PAPARAZZI_HOME)/var/build_version.txt
update_google_version:
-$(MAKE) -C data/maps
@@ -118,12 +127,12 @@ conf/%.xml :conf/%_example.xml
[ -L $@ ] || [ -f $@ ] || cp $< $@
ground_segment: print_build_version update_google_version conf libpprz subdirs commands static
ground_segment: _print_building update_google_version conf libpprz subdirs commands static
ground_segment.opt: ground_segment cockpit.opt tmtc.opt
static: cockpit tmtc generators sim_static joystick static_h
libpprz:
libpprz: _save_build_version
$(MAKE) -C $(LIB)/ocaml
multimon:
@@ -309,7 +318,7 @@ run_tests:
test: all replace_current_conf_xml run_tests restore_conf_xml
.PHONY: all print_build_version update_google_version dox ground_segment ground_segment.opt \
.PHONY: all print_build_version _print_building _save_build_version update_google_version dox ground_segment ground_segment.opt \
subdirs $(SUBDIRS) conf ext libpprz multimon cockpit cockpit.opt tmtc tmtc.opt generators\
static sim_static lpctools commands \
clean cleanspaces ab_clean dist_clean distclean dist_clean_irreversible \
+34 -6
View File
@@ -60,12 +60,12 @@ else
MKTEMP = mktemp
endif
# detection of number of processors for parallel compilation
# by passing J=AUTO from toplevel make.
# By default, detect number of processors for parallel compilation
# same as passing J=AUTO from toplevel make.
# Number of processes can also be explicitly set with e.g. J=4
# Calling make without a J variable is the same as J=1 (serial compilation)
# For serial compilation, call make with J=1
NPROCS := 1
ifdef J
J ?= AUTO
ifeq ($J,AUTO)
ifeq ($(UNAME),Linux)
NPROCS := $(shell grep -c ^processor /proc/cpuinfo)
@@ -74,9 +74,37 @@ else ifeq ($(UNAME),Darwin)
endif # $(UNAME)
else # not auto, explicitly specified
NPROCS := $J
endif # $J
endif
#
# export paparazzi version
#
GIT_SHA1 := $(shell git log -1 --pretty=format:%H 2> /dev/null)
ifneq ($(words $(GIT_SHA1)),1)
GIT_SHA1 := "UNKNOWN"
endif
GIT_DESC := $(shell ./paparazzi_version)
BUILD_DESC := $(shell cat $(PAPARAZZI_HOME)/var/build_version.txt 2> /dev/null || echo UNKNOWN)
# extract only version number in format <major>.<minor>.<patch>
PPRZ_VER := $(shell echo $(GIT_DESC) | sed 's/[^0-9.]*\([0-9.]*\).*/\1/')
PPRZ_VER_MAJOR := $(shell echo $(GIT_DESC) | sed 's/v\([0-9]*\).*/\1/')
PPRZ_VER_MINOR := $(shell echo $(GIT_DESC) | sed 's/v[0-9]*.\([0-9]*\).*/\1/')
PPRZ_VER_PATCH := $(shell echo $(GIT_DESC) | sed 's/v[0-9]*.[0-9]*.\([0-9]*\).*/\1/')
ifneq ($(words $(PPRZ_VER_PATCH)),1)
PPRZ_VER_PATCH := 0
endif
export GIT_DESC
export BUILD_DESC
export GIT_SHA1
export PPRZ_VER_MAJOR
export PPRZ_VER_MINOR
export PPRZ_VER_PATCH
# show warning with print_version if PPRZ_VER (e.g. 5.3.2) is not part of the BUILD_DESC string
VERSION_MATCH = ""
ifeq (,$(findstring $(PPRZ_VER),$(BUILD_DESC)))
VERSION_MATCH = "\nWarning: version differs from build version ($(BUILD_DESC)), you might want to run the toplevel make."
endif
# "make Q=''" to get full echo
Q=@
@@ -106,7 +134,7 @@ init:
print_version:
@echo "-----------------------------------------------------------------------"
@echo "Paparazzi version" $(shell ./paparazzi_version)
@echo "Paparazzi version" $(GIT_DESC)$(VERSION_MATCH)
@echo "-----------------------------------------------------------------------"
+1 -1
View File
@@ -6,7 +6,7 @@ Paparazzi UAS
[![Build Status](https://travis-ci.org/paparazzi/paparazzi.png?branch=master)](https://travis-ci.org/paparazzi/paparazzi) [![Gitter chat](https://badges.gitter.im/paparazzi/discuss.svg)](https://gitter.im/paparazzi/discuss)
Paparazzi is an attempt to develop a free software Unmanned (Air) Vehicle System.
As of today the system is being used successfuly by a number of hobyists, universities and companies all over the world, on vehicle of various size ( 100g to 25Kg ) and of various nature ( fixed wing, rotorcrafts, boats and surface vehicles).
As of today the system is being used successfuly by a number of hobbyists, universities and companies all over the world, on vehicle of various size ( 100g to 25Kg ) and of various nature ( fixed wing, rotorcrafts, boats and surface vehicles).
Up to date information is available in the wiki http://wiki.paparazziuav.org
+5 -54
View File
@@ -20,66 +20,17 @@
#
#
# This Makefile uses the generic Makefile.omap and adds upload rules for the ARDrone2
# This Makefile uses the generic Makefile.linux and adds upload rules for the ARDrone2
#
include $(PAPARAZZI_SRC)/conf/Makefile.omap
DRONE = $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/ardrone2.py
# Allow modules or other raw makefiles to add actions to the upload
upload_extra:
include $(PAPARAZZI_SRC)/conf/Makefile.linux
DRONE = $(PAPARAZZI_SRC)/sw/tools/parrot/ardrone2.py
# Program the device and start it.
upload program: upload_extra $(OBJDIR)/$(TARGET).elf
$(Q)$(DRONE) --host=$(HOST) insmod $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko
upload program: $(OBJDIR)/$(TARGET).elf
$(Q)$(DRONE) --host=$(HOST) upload_file_and_run $(OBJDIR)/$(TARGET).elf $(SUB_DIR)
$(Q)$(DRONE) --host=$(HOST) status
# Program the device and start it.
upload2 program2: $(OBJDIR)/$(TARGET).elf
# Kill the application
-echo "killall -9 $(TARGET).elf" | telnet $(HOST)
# Make the target dir and edit the config
-{ \
echo "mkdir -p $(TARGET_DIR)"; \
echo "if grep -q \"start_paparazzi *= \" /data/config.ini; then sed -i 's/\(start_paparazzi *= *\).*/\\\1$(ARDRONE2_START_PAPARAZZI)/g' /data/config.ini; else echo \"start_paparazzi = $(ARDRONE2_START_PAPARAZZI)\" >> /data/config.ini; fi"; \
echo "if grep -q \"wifi_mode *= \" /data/config.ini; then sed -i 's/\(wifi_mode *= *\).*/\\\1$(ARDRONE2_WIFI_MODE)/g' /data/config.ini; else echo \"wifi_mode = $(ARDRONE2_WIFI_MODE)\" >> /data/config.ini; fi"; \
echo "if grep -q \"ssid_single_player *= \" /data/config.ini; then sed -i 's/\(ssid_single_player *= *\).*/\\\1$(ARDRONE2_SSID)/g' /data/config.ini; else echo \"ssid_single_player = $(ARDRONE2_SSID)\" >> /data/config.ini; fi"; \
echo "if grep -q \"static_ip_address_base *= \" /data/config.ini; then sed -i 's/\(static_ip_address_base *= *\).*/\\\1$(ARDRONE2_IP_ADDRESS_BASE)/g' /data/config.ini; else echo \"static_ip_address_base = $(ARDRONE2_IP_ADDRESS_BASE)\" >> /data/config.ini; fi"; \
echo "if grep -q \"static_ip_address_probe *= \" /data/config.ini; then sed -i 's/\(static_ip_address_probe *= *\).*/\\\1$(ARDRONE2_IP_ADDRESS_PROBE)/g' /data/config.ini; else echo \"static_ip_address_probe = $(ARDRONE2_IP_ADDRESS_PROBE)\" >> /data/config.ini; fi"; \
} | telnet $(HOST)
# Upload the drivers and new application
{ \
echo "binary"; \
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko /$(SUB_DIR)/cdc-acm.ko"; \
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/check_update.sh check_update.sh"; \
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/wifi_setup.sh wifi_setup.sh"; \
echo "put $(OBJDIR)/$(TARGET).elf /$(SUB_DIR)/$(TARGET).elf"; \
echo "quit"; \
} | ftp -n $(HOST)
# Upload the modules and start the application
-{ \
echo "mv /data/video/check_update.sh /bin/"; \
echo "mv /data/video/wifi_setup.sh /bin/"; \
echo "chmod 777 /bin/check_update.sh" \
echo "chmod 777 /bin/wifi_setup.sh" \
echo "insmod $(TARGET_DIR)/cdc-acm.ko"; \
echo "chmod 777 $(TARGET_DIR)/$(TARGET).elf"; \
echo "$(TARGET_DIR)/$(TARGET).elf > /dev/null 2>&1 &"; \
} | telnet $(HOST)
ifeq ($(ARDRONE2_REBOOT),1)
-{ \
echo "reboot"; \
} | telnet $(HOST)
endif
# Listing of phony targets.
.PHONY : upload_extra upload program upload2 program2
.PHONY : upload program
+35
View File
@@ -0,0 +1,35 @@
# Hey Emacs, this is a -*- makefile -*-
#
# Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
#
# This file is part of paparazzi.
#
# paparazzi 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 2, or (at your option)
# any later version.
#
# paparazzi 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 paparazzi; see the file COPYING. If not, see
# <http://www.gnu.org/licenses/>.
#
#
# This Makefile uses the generic Makefile.linux and adds upload rules for the ARDrone2
#
include $(PAPARAZZI_SRC)/conf/Makefile.linux
DRONE = $(PAPARAZZI_SRC)/sw/tools/parrot/bebop.py
# Program the device and start it.
upload program: $(OBJDIR)/$(TARGET).elf
$(Q)$(DRONE) --host=$(HOST) upload_file_and_run $(OBJDIR)/$(TARGET).elf $(SUB_DIR)
# Listing of phony targets.
.PHONY : upload program
+11 -8
View File
@@ -20,6 +20,9 @@
# Boston, MA 02111-1307, USA.
#
#
# Makefile for linux targets (e.g. OMAP, BBB, ARDrone)
#
#
# find compiler toolchain
@@ -65,11 +68,11 @@ CXXFLAGS += -Wall -Wextra
CXXFLAGS += $($(TARGET).CXXFLAGS)
CXXFLAGS += $(USER_CFLAGS)
SRC_C_OMAP = $($(TARGET).srcs)
OBJ_C_OMAP = $(SRC_C_OMAP:%.c=$(OBJDIR)/%.o)
SRC_C_LINUX = $($(TARGET).srcs)
OBJ_C_LINUX = $(SRC_C_LINUX:%.c=$(OBJDIR)/%.o)
SRC_CPP_OMAP = $($(TARGET).cpp_srcs)
OBJ_CPP_OMAP = $(SRC_CPP_OMAP:%.cpp=$(OBJDIR)/%.o)
SRC_CPP_LINUX = $($(TARGET).cpp_srcs)
OBJ_CPP_LINUX = $(SRC_CPP_LINUX:%.cpp=$(OBJDIR)/%.o)
printcommands:
@echo ""
@@ -94,12 +97,12 @@ elf: $(OBJDIR)/$(TARGET).elf
# Link: create ELF output file from object files.
.SECONDARY : $(OBJDIR)/$(TARGET).elf
.PRECIOUS : $(OBJ_C_OMAP) $(OBJ_CPP_OMAP)
%.elf: $(OBJ_C_OMAP) $(OBJ_CPP_OMAP)
.PRECIOUS : $(OBJ_C_LINUX) $(OBJ_CPP_LINUX)
%.elf: $(OBJ_C_LINUX) $(OBJ_CPP_LINUX)
@echo LD $@
$(Q)if (expr "$($(TARGET).cpp_srcs)"); \
then $(CXX) $(CXXFLAGS) $(OBJ_CPP_OMAP) $(OBJ_C_OMAP) --output $@ $(LDFLAGS) $($(TARGET).LDFLAGS); \
else $(CC) $(CFLAGS) $(OBJ_C_OMAP) --output $@ $(LDFLAGS) $($(TARGET).LDFLAGS); fi
then $(CXX) $(CXXFLAGS) $(OBJ_CPP_LINUX) $(OBJ_C_LINUX) --output $@ $(LDFLAGS) $($(TARGET).LDFLAGS); \
else $(CC) $(CFLAGS) $(OBJ_C_LINUX) --output $@ $(LDFLAGS) $($(TARGET).LDFLAGS); fi
# Compile: create object files from C source files
$(OBJDIR)/%.o : %.c $(OBJDIR)/../Makefile.ac
+255
View File
@@ -0,0 +1,255 @@
<!-- this is a LadyBird quadrotor frame equiped with Lisa/S 1.0 -->
<!-- The LadyBird frame comes with four brushed motors in an X configuration. -->
<!--
The motor and rotor configuration is the following:
Front
^
|
Motor3(NW) Motor0(NE)
CW CCW
\ /
,___,
| |
| |
|___|
/ \
CCW CW
Motor2(SW) Motor1(SE)
The Lisa/S is rotated by 13deg CCW against the frame.
-->
<!--
Applicable configuration:
airframe="airframes/examples/ladybird_lisa_s.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
-->
<airframe name="quadrotor_lisa_s">
<servos driver="Pwm">
<servo name="NE" no="0" min="0" neutral="50" max="1000"/>
<servo name="SE" no="1" min="0" neutral="50" max="1000"/>
<servo name="SW" no="2" min="0" neutral="50" max="1000"/>
<servo name="NW" no="3" min="0" neutral="50" max="1000"/>
</servos>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="NE" value="motor_mixing.commands[0]"/>
<set servo="SE" value="motor_mixing.commands[1]"/>
<set servo="SW" value="motor_mixing.commands[2]"/>
<set servo="NW" value="motor_mixing.commands[3]"/>
</command_laws>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="256"/>
<define name="ROLL_COEF" value="{ -256, -256, 256, 256 }"/>
<define name="PITCH_COEF" value="{ 256, -256, -256, 256 }"/>
<define name="YAW_COEF" value="{ 256, -256, 256, -256 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
</section>
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="-13." unit="deg"/>
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<!-- MAGNETO CALIBRATION ANTWERP -->
<define name="MAG_X_NEUTRAL" value="-272"/>
<define name="MAG_Y_NEUTRAL" value="-1048"/>
<define name="MAG_Z_NEUTRAL" value="-196"/>
<define name="MAG_X_SENS" value="3.71896242867" integer="16"/>
<define name="MAG_Y_SENS" value="4.08168121098" integer="16"/>
<define name="MAG_Z_SENS" value="3.93086401526" integer="16"/>
<!-- MAGNETO CURRENT CALIBRATION -->
<define name= "MAG_X_CURRENT_COEF" value="-0.0176704713698"/>
<define name= "MAG_Y_CURRENT_COEF" value="0.0174222594634"/>
<define name= "MAG_Z_CURRENT_COEF" value="0.0187570316492"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="14000"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="3.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="3.4" unit="V"/>
<define name="MAX_BAT_LEVEL" value="4.1" unit="V"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="15000"/>
<define name="SP_MAX_Q" value="15000"/>
<define name="SP_MAX_R" value="15000"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="418"/>
<define name="IGAIN_P" value="301"/>
<define name="IGAIN_Q" value="302"/>
<define name="IGAIN_R" value="303"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_PSI" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="SP_MAX_P" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="500"/>
<define name="PSI_DGAIN" value="300"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PHI_AGAIN" value="0"/>
<define name="THETA_AGAIN" value="0"/>
<define name="PSI_AGAIN" value="0"/>
</section>
<section name="INS" prefix="INS_">
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="276"/>
<define name="HOVER_KD" value="455"/>
<define name="HOVER_KI" value="100"/>
<define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value="0.5"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- Delft magnetic field -->
<define name="H_X" value="0.39049610"/>
<define name="H_Y" value="0.00278894"/>
<define name="H_Z" value="0.92060036"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="90"/>
<define name="DGAIN" value="50"/>
<define name="IGAIN" value="24"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;, &quot;nw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_x_quad_ccw&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
<modules main_freq="512">
<load name="gps_ubx_ucenter.xml"/>
<load name="send_imu_mag_current.xml"/>
</modules>
<firmware name="rotorcraft">
<target name="ap" board="lisa_s_1.0">
<subsystem name="radio_control" type="superbitrf_rc">
<define name="RADIO_MODE" value="RADIO_GEAR"/>
<!--<define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>-->
<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
</subsystem>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
</target>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<!-- This airframe is using the servo PWM outputs directly to drive mosfets, which in turn directly drive brushed motors.
With the following settings we will be driving the motors with a 36KHz signal that should be well above anyone's hearing range.
If we went with the 500Hz that woul be the default we would be hearing a very annoying high pitched sound coming from the motors.
-->
<!-- Setting the PWM timer base frequency to 36MHz -->
<define name="PWM_BASE_FREQ" value="36000000"/>
<!-- Setting the PWM interval to 36KHz -->
<define name="SERVO_HZ" value="36000"/>
<define name="USE_SERVOS_1AND2"/>
</subsystem>
<!--<subsystem name="telemetry" type="superbitrf"/>-->
<!-- Comment the previous line and uncomment the following one if you need
to use the debug serial interface for telemetry. -->
<subsystem name="telemetry" type="superbitrf"/>
<subsystem name="imu" type="lisa_s_v1.0">
<define name="LISA_S_UPSIDE_DOWN"/> <!-- Upside down is a relative term. :) -->
</subsystem>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="1"/>
<define name="AHRS_USE_GPS_HEADING" value="0"/>
</subsystem>
<subsystem name="ins"/>
</firmware>
</airframe>
+24
View File
@@ -0,0 +1,24 @@
<conf>
<aircraft
name="DreamCatcher"
ac_id="230"
airframe="airframes/BR/DreamCacher_bart.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic_superbitrf_from_hand.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/superbitrf.xml"
settings_modules="modules/gps_ubx_ucenter.xml"
gui_color="white"
/>
<aircraft
name="LadyLisaBart"
ac_id="231"
airframe="airframes/BR/ladybird_kit_bart.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic_superbitrf_from_hand.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/superbitrf.xml"
settings_modules="modules/gps_ubx_ucenter.xml"
gui_color="blue"
/>
</conf>
+264
View File
@@ -0,0 +1,264 @@
<!-- this is a LadyBird quadrotor frame equiped with Lisa/S 1.0 -->
<!-- The LadyBird frame comes with four brushed motors in an X configuration. -->
<!--
The motor and rotor configuration is the following:
Front
^
|
Motor3(NW) Motor0(NE)
CW CCW
\ /
,___,
| |
| |
|___|
/ \
CCW CW
Motor2(SW) Motor1(SE)
The Lisa/S is rotated by 13deg CCW against the frame.
-->
<!--
Applicable configuration:
airframe="airframes/examples/ladybird_lisa_s.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
-->
<airframe name="quadrotor_lisa_s">
<servos driver="Pwm">
<servo name="NE" no="0" min="0" neutral="50" max="1000"/>
<servo name="SE" no="1" min="0" neutral="50" max="1000"/>
<servo name="SW" no="2" min="0" neutral="50" max="1000"/>
<servo name="NW" no="3" min="0" neutral="50" max="1000"/>
</servos>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="NE" value="motor_mixing.commands[0]"/>
<set servo="SE" value="motor_mixing.commands[1]"/>
<set servo="SW" value="motor_mixing.commands[2]"/>
<set servo="NW" value="motor_mixing.commands[3]"/>
</command_laws>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="256"/>
<define name="ROLL_COEF" value="{ -256, -256, 256, 256 }"/>
<define name="PITCH_COEF" value="{ 256, -256, -256, 256 }"/>
<define name="YAW_COEF" value="{ 256, -256, 256, -256 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
</section>
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="-13." unit="deg"/>
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<!-- MAGNETO CALIBRATION ANTWERPEN
<define name="MAG_X_NEUTRAL" value="-147"/>
<define name="MAG_Y_NEUTRAL" value="152"/>
<define name="MAG_Z_NEUTRAL" value="-5"/>
<define name="MAG_X_SENS" value="3.56142779007" integer="16"/>
<define name="MAG_Y_SENS" value="3.82223743533" integer="16"/>
<define name="MAG_Z_SENS" value="4.15618897164" integer="16"/> -->
<!-- MAGNETO CALIBRATION DELFT-->
<define name="MAG_X_NEUTRAL" value="10"/>
<define name="MAG_Y_NEUTRAL" value="238"/>
<define name="MAG_Z_NEUTRAL" value="-74"/>
<define name="MAG_X_SENS" value="3.67001348283" integer="16"/>
<define name="MAG_Y_SENS" value="3.98840260231" integer="16"/>
<define name="MAG_Z_SENS" value="4.32568461736" integer="16"/>
<!-- MAGNETO CURRENT CALIBRATION -->
<define name= "MAG_X_CURRENT_COEF" value="-0.0176704713698"/>
<define name= "MAG_Y_CURRENT_COEF" value="0.0174222594634"/>
<define name= "MAG_Z_CURRENT_COEF" value="0.0187570316492"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="14000"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="2.8" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="3.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="3.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="4.1" unit="V"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="15000"/>
<define name="SP_MAX_Q" value="15000"/>
<define name="SP_MAX_R" value="15000"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="418"/>
<define name="IGAIN_P" value="301"/>
<define name="IGAIN_Q" value="302"/>
<define name="IGAIN_R" value="303"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_PSI" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="SP_MAX_P" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="500"/>
<define name="PSI_DGAIN" value="300"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PHI_AGAIN" value="0"/>
<define name="THETA_AGAIN" value="0"/>
<define name="PSI_AGAIN" value="0"/>
</section>
<section name="INS" prefix="INS_">
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="276"/>
<define name="HOVER_KD" value="455"/>
<define name="HOVER_KI" value="100"/>
<define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value="0.5"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- Delft magnetic field -->
<define name="H_X" value="0.39049610"/>
<define name="H_Y" value="0.00278894"/>
<define name="H_Z" value="0.92060036"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="90"/>
<define name="DGAIN" value="50"/>
<define name="IGAIN" value="24"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;, &quot;nw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_x_quad_ccw&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
<modules main_freq="512">
<load name="gps_ubx_ucenter.xml"/>
<load name="send_imu_mag_current.xml"/>
</modules>
<firmware name="rotorcraft">
<target name="ap" board="lisa_s_1.0">
<subsystem name="radio_control" type="superbitrf_rc">
<define name="RADIO_MODE" value="RADIO_GEAR"/>
<!--<define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>-->
<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
</subsystem>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
</target>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<!-- This airframe is using the servo PWM outputs directly to drive mosfets, which in turn directly drive brushed motors.
With the following settings we will be driving the motors with a 36KHz signal that should be well above anyone's hearing range.
If we went with the 500Hz that woul be the default we would be hearing a very annoying high pitched sound coming from the motors.
-->
<!-- Setting the PWM timer base frequency to 36MHz -->
<define name="PWM_BASE_FREQ" value="36000000"/>
<!-- Setting the PWM interval to 36KHz -->
<define name="SERVO_HZ" value="36000"/>
<define name="USE_SERVOS_1AND2"/>
</subsystem>
<!--<subsystem name="telemetry" type="superbitrf"/>-->
<!-- Comment the previous line and uncomment the following one if you need
to use the debug serial interface for telemetry. -->
<subsystem name="telemetry" type="superbitrf"/>
<subsystem name="imu" type="lisa_s_v1.0">
<define name="LISA_S_UPSIDE_DOWN"/> <!-- Upside down is a relative term. :) -->
</subsystem>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="1"/>
<define name="AHRS_USE_GPS_HEADING" value="0"/>
</subsystem>
<subsystem name="ins"/>
</firmware>
</airframe>
+1 -1
View File
@@ -145,7 +145,7 @@
<section name="DIGITAL_CAMERA" prefix="DC_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
<define name="AUTOSHOOT_DISTANCE_INTERVAL" value="50" unit="meter"/>
</section>
+1
View File
@@ -13,6 +13,7 @@
<!-- ************************* FIRMWARE ************************* -->
<firmware name="fixedwing">
<define name="RADIO_CONTROL_NB_CHANNEL" value="8" />
<target name="ap" board="lisa_l_1.0">
<configure name="SEPARATE_FBW" value="1"/>
<configure name="AHRS_ALIGNER_LED" value="1"/>
+2 -3
View File
@@ -79,8 +79,8 @@
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="FRONT" value="motor_mixing.commands[0]"/>
<set servo="BACK" value="motor_mixing.commands[1]"/>
<set servo="RIGHT" value="motor_mixing.commands[2]"/>
<set servo="LEFT" value="motor_mixing.commands[3]"/>
<set servo="LEFT" value="motor_mixing.commands[2]"/>
<set servo="RIGHT" value="motor_mixing.commands[3]"/>
</command_laws>
<!-- ************************* SENSORS ************************* -->
@@ -179,7 +179,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;, &quot;nw_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="JSBSIM_MODEL" value="&quot;simple_x_quad&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
+1 -1
View File
@@ -172,7 +172,7 @@
<section name="DIGITAL_CAMERA" prefix="DC_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="8" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
<define name="AUTOSHOOT_DISTANCE_INTERVAL" value="50" unit="meter"/>
</section>
<firmware name="fixedwing">
+1 -1
View File
@@ -145,7 +145,7 @@
<section name="DIGITAL_CAMERA" prefix="DC_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
<define name="AUTOSHOOT_DISTANCE_INTERVAL" value="50" unit="meter"/>
</section>
+3 -2
View File
@@ -160,8 +160,9 @@
<firmware name="setup">
<target name="tunnel" board="tiny_1.1" />
<target name="usb_tunnel_0" board="tiny_1.1" />
<target name="usb_tunnel_1" board="tiny_1.1" />
<target name="usb_tunnel" board="tiny_1.1">
<configure name="TUNNEL_PORT" value="UART0"/>
</target>
<target name="setup_actuators" board="tiny_1.1" />
</firmware>
+1 -1
View File
@@ -145,7 +145,7 @@
<section name="DIGITAL_CAMERA" prefix="DC_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
<define name="AUTOSHOOT_DISTANCE_INTERVAL" value="50" unit="meter"/>
</section>
+1 -1
View File
@@ -195,7 +195,7 @@
<section name="DIGITAL_CAMERA" prefix="DC_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
<define name="AUTOSHOOT_DISTANCE_INTERVAL" value="50" unit="meter"/>
</section>
+1 -1
View File
@@ -152,7 +152,7 @@ YAPA + XSens + XBee
<section name="DIGITAL_CAMERA" prefix="DC_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
<define name="AUTOSHOOT_DISTANCE_INTERVAL" value="50" unit="meter"/>
</section>
-1
View File
@@ -214,7 +214,6 @@ LiPo/LiIo-Zellen: 2-3
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
+1 -1
View File
@@ -207,7 +207,7 @@
<section name="DIGITAL_CAMERA" prefix="DC_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
<define name="AUTOSHOOT_DISTANCE_INTERVAL" value="50" unit="meter"/>
</section>
</airframe>
@@ -0,0 +1,34 @@
<airframe>
<section name="IMU" prefix="IMU_">
<!-- Accelero -->
<define name="ACCEL_X_NEUTRAL" value="2036"/>
<define name="ACCEL_Y_NEUTRAL" value="2063"/>
<define name="ACCEL_Z_NEUTRAL" value="2050"/>
<define name="ACCEL_X_SENS" value="19.8215198025" integer="16"/>
<define name="ACCEL_Y_SENS" value="19.3612647775" integer="16"/>
<define name="ACCEL_Z_SENS" value="19.6308115492" integer="16"/>
<!-- Magneto calibration -->
<!--define name="MAG_X_NEUTRAL" value="56"/>
<define name="MAG_Y_NEUTRAL" value="17"/>
<define name="MAG_Z_NEUTRAL" value="-64"/>
<define name="MAG_X_SENS" value="17.4978698304" integer="16"/>
<define name="MAG_Y_SENS" value="17.7032280166" integer="16"/>
<define name="MAG_Z_SENS" value="17.8657801029" integer="16"/-->
<define name="MAG_X_NEUTRAL" value="21"/>
<define name="MAG_Y_NEUTRAL" value="-4"/>
<define name="MAG_Z_NEUTRAL" value="-1"/>
<define name="MAG_X_SENS" value="16.510274159" integer="16"/>
<define name="MAG_Y_SENS" value="16.5086412339" integer="16"/>
<define name="MAG_Z_SENS" value="17.4503678708" integer="16"/>
<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
</section>
</airframe>
@@ -0,0 +1,34 @@
<airframe>
<section name="IMU" prefix="IMU_">
<!-- Accelero -->
<define name="ACCEL_X_NEUTRAL" value="2033"/>
<define name="ACCEL_Y_NEUTRAL" value="2050"/>
<define name="ACCEL_Z_NEUTRAL" value="2045"/>
<define name="ACCEL_X_SENS" value="19.4125494887" integer="16"/>
<define name="ACCEL_Y_SENS" value="19.5801376185" integer="16"/>
<define name="ACCEL_Z_SENS" value="19.4761009738" integer="16"/>
<!-- Magneto calibration -->
<!--define name="MAG_X_NEUTRAL" value="64"/>
<define name="MAG_Y_NEUTRAL" value="7"/>
<define name="MAG_Z_NEUTRAL" value="-28"/>
<define name="MAG_X_SENS" value="15.0972908192" integer="16"/>
<define name="MAG_Y_SENS" value="14.9806854769" integer="16"/>
<define name="MAG_Z_SENS" value="16.0049628842" integer="16"/-->
<define name="MAG_X_NEUTRAL" value="48"/>
<define name="MAG_Y_NEUTRAL" value="-23"/>
<define name="MAG_Z_NEUTRAL" value="-9"/>
<define name="MAG_X_SENS" value="15.3920639363" integer="16"/>
<define name="MAG_Y_SENS" value="15.3302290802" integer="16"/>
<define name="MAG_Z_SENS" value="16.236687484" integer="16"/>
<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
</section>
</airframe>
@@ -0,0 +1,36 @@
<airframe>
<section name="IMU" prefix="IMU_">
<!-- Accelero -->
<define name="ACCEL_X_NEUTRAL" value="2046"/>
<define name="ACCEL_Y_NEUTRAL" value="2050"/>
<define name="ACCEL_Z_NEUTRAL" value="2053"/>
<define name="ACCEL_X_SENS" value="19.6123236866" integer="16"/>
<define name="ACCEL_Y_SENS" value="19.6657552603" integer="16"/>
<define name="ACCEL_Z_SENS" value="19.7736759183" integer="16"/>
<!-- Magneto calibration -->
<!--define name="MAG_X_NEUTRAL" value="23"/>
<define name="MAG_Y_NEUTRAL" value="-13"/>
<define name="MAG_Z_NEUTRAL" value="-8"/>
<define name="MAG_X_SENS" value="15.5472944024" integer="16"/>
<define name="MAG_Y_SENS" value="15.5179439561" integer="16"/>
<define name="MAG_Z_SENS" value="16.8234333541" integer="16"/-->
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="24"/>
<define name="MAG_Y_NEUTRAL" value="-20"/>
<define name="MAG_Z_NEUTRAL" value="-9"/>
<define name="MAG_X_SENS" value="15.3774561292" integer="16"/>
<define name="MAG_Y_SENS" value="15.5082167794" integer="16"/>
<define name="MAG_Z_SENS" value="16.5687991924" integer="16"/>
<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
</section>
</airframe>
@@ -0,0 +1,27 @@
<airframe>
<section name="IMU" prefix="IMU_">
<!-- Accelero -->
<define name="ACCEL_X_NEUTRAL" value="2061"/>
<define name="ACCEL_Y_NEUTRAL" value="2087"/>
<define name="ACCEL_Z_NEUTRAL" value="2062"/>
<define name="ACCEL_X_SENS" value="19.8086810127" integer="16"/>
<define name="ACCEL_Y_SENS" value="19.6883612768" integer="16"/>
<define name="ACCEL_Z_SENS" value="19.3409944706" integer="16"/>
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="46"/>
<define name="MAG_Y_NEUTRAL" value="-50"/>
<define name="MAG_Z_NEUTRAL" value="-28"/>
<define name="MAG_X_SENS" value="16.1913174133" integer="16"/>
<define name="MAG_Y_SENS" value="15.2287916857" integer="16"/>
<define name="MAG_Z_SENS" value="16.5815975397" integer="16"/>
<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
</section>
</airframe>
@@ -0,0 +1,166 @@
<!-- shared control loops gain -->
<airframe>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/>
<servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/>
<servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/>
<servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="-200"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="255"/>
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
<define name="ROLL_COEF" value="{ 256, -256, -256, 256 }"/>
<define name="PITCH_COEF" value="{ 256, 256, -256, -256 }"/>
<define name="YAW_COEF" value="{ -256, 256, -256, 256 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[0]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[1]"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[2]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[3]"/>
</command_laws>
<section name="INS_BASE" prefix="INS_">
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
<define name="SONAR_MAX_RANGE" value="4.0"/>
<define name="SONAR_MIN_RANGE" value="0.01"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="200" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="900" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="900" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="600" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="40." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="1100"/>
<define name="PHI_DGAIN" value="420"/>
<define name="PHI_IGAIN" value="120"/>
<define name="THETA_PGAIN" value="1100"/>
<define name="THETA_DGAIN" value="420"/>
<define name="THETA_IGAIN" value="120"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="400"/>
<define name="PSI_IGAIN" value="30"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="70"/>
<define name="THETA_DDGAIN" value="70"/>
<define name="PSI_DDGAIN" value="50"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="REF_MIN_ZDD" value="-0.8*9.81"/>
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="200"/>
<define name="HOVER_KD" value="100"/>
<define name="HOVER_KI" value="30"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.65"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.5"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="REF_MAX_SPEED" value="3." unit="m/s"/>
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="40"/>
<define name="DGAIN" value="60"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_ardrone2&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_ard_inv.h&quot;"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
@@ -0,0 +1,81 @@
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
<airframe name="ardrone2_digit">
<firmware name="rotorcraft">
<define name="DEBUG_VFF_EXTENDED"/>
<configure name="HOST" value="192.168.1.$(AC_ID)"/>
<target name="ap" board="ardrone2_raw">
<define name="USE_SONAR"/>
<define name="USE_BARO_MEDIAN_FILTER"/>
<define name="AUTOPILOT_DISABLE_AHRS_KILL"/>
<subsystem name="telemetry" type="transparent_udp"/>
<subsystem name="radio_control" type="datalink"/>
</target>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>
<!-- Subsystem section -->
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="ardrone2"/>
<subsystem name="imu" type="ardrone2"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<!-- AHRS + INS for indoor or outdoor -->
<!--subsystem name="ahrs" type="int_cmpl_euler"/>
<subsystem name="ins" type="extended"/-->
<!-- INS for outdoor only -->
<subsystem name="ins" type="float_invariant"/>
</firmware>
<modules main_freq="512">
<!--load name="gps_ubx_ucenter.xml"/-->
<load name="agl_dist.xml">
<define name="USE_SONAR"/>
</load>
<load name="image_nc_send.xml"/>
<load name="rotorcraft_cam.xml"/>
</modules>
<!-- include common control -->
<include href="conf/airframes/ENAC/quadrotor/ard2_base_control.xml"/>
<!-- include arframe calibration -->
<include href="conf/airframes/ENAC/quadrotor/ard2_$AC_ID.xml"/>
<!-- local magnetic field -->
<!-- http://paparazzi.enac.fr/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- Toulouse -->
<!--define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/-->
<!-- Delft -->
<!--define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/ -->
</section>
<section name="INS" prefix="INS_">
<!-- Toulouse -->
<define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/>
<!-- Delft -->
<!--define name="H_X" value="0.387766"/>
<define name="H_Y" value="0.00648212"/>
<define name="H_Z" value="0.921725"/ -->
<!-- trust more the baro over the gps alt -->
<define name="INV_NXZ" value="0.3"/>
<define name="INV_NH" value="2.0"/>
</section>
<section name="GCS">
<define name="ALT_SHIFT_PLUS_PLUS" value="5"/>
<define name="ALT_SHIFT_PLUS" value="1"/>
<define name="ALT_SHIFT_MINUS" value="-1"/>
</section>
</airframe>
@@ -0,0 +1,86 @@
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
<airframe name="ardrone2_vision">
<firmware name="rotorcraft">
<define name="DEBUG_VFF_EXTENDED"/>
<configure name="HOST" value="192.168.1.$(AC_ID)"/>
<target name="ap" board="ardrone2_raw">
<define name="USE_SONAR"/>
<define name="USE_BARO_MEDIAN_FILTER"/>
<define name="AUTOPILOT_DISABLE_AHRS_KILL"/>
<subsystem name="telemetry" type="transparent_udp"/>
<subsystem name="radio_control" type="datalink"/>
</target>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>
<!-- Subsystem section -->
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="ardrone2"/>
<subsystem name="imu" type="ardrone2"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<!-- AHRS + INS for indoor or outdoor -->
<!--subsystem name="ahrs" type="int_cmpl_euler"/>
<subsystem name="ins" type="extended"/-->
<!-- INS for outdoor only -->
<subsystem name="ins" type="float_invariant"/>
</firmware>
<modules main_freq="512">
<!--load name="gps_ubx_ucenter.xml"/-->
<load name="agl_dist.xml">
<define name="USE_SONAR"/>
</load>
<load name="video_rtp_stream.xml">
<define name="VIDEO_SOCK_OUT_OFFSET" value="$(AC_ID)"/>
<define name="VIDEO_DOWNSIZE_FACTOR" value="2"/>
<define name="VIDEO_QUALITY_FACTOR" value="80"/>
<define name="VIDEO_FPS" value="1"/>
</load>
<load name="rotorcraft_cam.xml"/>
</modules>
<!-- include common control -->
<include href="conf/airframes/ENAC/quadrotor/ard2_base_control.xml"/>
<!-- include arframe calibration -->
<include href="conf/airframes/ENAC/quadrotor/ard2_$AC_ID.xml"/>
<!-- local magnetic field -->
<!-- http://paparazzi.enac.fr/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- Toulouse -->
<!--define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/-->
<!-- Delft -->
<!--define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/ -->
</section>
<section name="INS" prefix="INS_">
<!-- Toulouse -->
<define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/>
<!-- Delft -->
<!--define name="H_X" value="0.387766"/>
<define name="H_Y" value="0.00648212"/>
<define name="H_Z" value="0.921725"/ -->
<!-- trust more the baro over the gps alt -->
<define name="INV_NXZ" value="0.3"/>
<define name="INV_NH" value="2.0"/>
</section>
<section name="GCS">
<define name="ALT_SHIFT_PLUS_PLUS" value="5"/>
<define name="ALT_SHIFT_PLUS" value="1"/>
<define name="ALT_SHIFT_MINUS" value="-1"/>
</section>
</airframe>
+24
View File
@@ -0,0 +1,24 @@
<conf>
<aircraft
name="Altura"
ac_id="151"
airframe="airframes/LS/quadrotor_altura_lisa_m_2_0.xml"
radio="radios/spektrum.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/mavtec_outdoor_demo.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml"
settings_modules="modules/gps_ubx_ucenter.xml"
gui_color="#00000000ffff"
/>
<aircraft
name="MavTecLS"
ac_id="150"
airframe="airframes/LS/quadrotor_mavtec_lisa_m_2_0.xml"
radio="radios/spektrum.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/mavtec_outdoor_demo.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml"
settings_modules=""
gui_color="#ffff00000000"
/>
</conf>
@@ -0,0 +1,214 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="Quadrotor LisaM_2.0 pwm">
<firmware name="rotorcraft">
<target name="ap" board="lisa_m_2.0">
<subsystem name="radio_control" type="spektrum">
<define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>
<define name="RADIO_MODE" value="RADIO_FLAP"/>
<!--define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="1"/-->
</subsystem>
<configure name="NO_LUFTBOOT" value="1"/>
<configure name="FLASH_MODE" value="SWD" />
<!-- MPU6000 is configured to output data at 2kHz, but polled at 512Hz PERIODIC_FREQUENCY -->
</target>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/>
<define name="USE_SERVOS_7AND8"/>
</subsystem>
<subsystem name="telemetry" type="xbee_api"/>
<subsystem name="imu" type="mpu6000_hmc5883">
<!-- the default i2c2 port is used -->
</subsystem>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</subsystem>
<subsystem name="ins"/>
</firmware>
<modules>
<load name="gps_ubx_ucenter.xml"/>
</modules>
<servos driver="Pwm">
<servo name="MOTOR_1" no="0" min="1000" neutral="1100" max="1900"/>
<servo name="MOTOR_2" no="1" min="1000" neutral="1100" max="1900"/>
<servo name="MOTOR_3" no="2" min="1000" neutral="1100" max="1900"/>
<servo name="MOTOR_4" no="3" min="1000" neutral="1100" max="1900"/>
<servo name="MOTOR_5" no="4" min="1000" neutral="1100" max="1900"/>
<servo name="MOTOR_6" no="5" min="1000" neutral="1100" max="1900"/>
<servo name="MOTOR_7" no="6" min="1000" neutral="1100" max="1900"/>
<servo name="MOTOR_8" no="7" min="1000" neutral="1100" max="1900"/>
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="8"/>
<define name="SCALE" value="256"/>
<!-- order (and rotation direction) : MOTOR 1 (CW), MOTOR 2 (CCW), MOTOR 3 (CW), MOTOR 4 (CCW) -->
<!-- MOTOR 5 (CCW), MOTOR 6 (CW), MOTOR 7 (CCW), MOTOR 8 (CW) -->
<define name="ROLL_COEF" value="{ -256, -256, 256, 256, -256, -256, 256, 256 }"/>
<define name="PITCH_COEF" value="{ 256, -256, -256, 256, 256, -256, -256, 256 }"/>
<define name="YAW_COEF" value="{ -256, 256, -256, 256, 256, -256, 256, -256 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256, 256, 256, 256, 256 }"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="MOTOR_1" value="motor_mixing.commands[0]"/>
<set servo="MOTOR_2" value="motor_mixing.commands[1]"/>
<set servo="MOTOR_3" value="motor_mixing.commands[2]"/>
<set servo="MOTOR_4" value="motor_mixing.commands[3]"/>
<set servo="MOTOR_5" value="motor_mixing.commands[4]"/>
<set servo="MOTOR_6" value="motor_mixing.commands[5]"/>
<set servo="MOTOR_7" value="motor_mixing.commands[6]"/>
<set servo="MOTOR_8" value="motor_mixing.commands[7]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_X_NEUTRAL" value="-9"/>
<define name="ACCEL_Y_NEUTRAL" value="24"/>
<define name="ACCEL_Z_NEUTRAL" value="65"/>
<define name="ACCEL_X_SENS" value="4.90393762843" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.919330721" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.85197383186" integer="16"/>
<!-- replace this with your own calibration -->
<define name="MAG_X_NEUTRAL" value="-40"/>
<define name="MAG_Y_NEUTRAL" value="-57"/>
<define name="MAG_Z_NEUTRAL" value="66"/>
<define name="MAG_X_SENS" value="4.5080259094" integer="16"/>
<define name="MAG_Y_SENS" value="4.56447191049" integer="16"/>
<define name="MAG_Z_SENS" value="5.11168950826" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="180." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="90." unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
</section>
<section name="INS" prefix="INS_">
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>
<!-- feedback -->
<define name="GAIN_P" value="1"/>
<define name="GAIN_Q" value="1"/>
<define name="GAIN_R" value="1"/>
<define name="IGAIN_P" value="0"/>
<define name="IGAIN_Q" value="0"/>
<define name="IGAIN_R" value="0"/>
<!-- feedforward -->
<define name="DDGAIN_P" value="0"/>
<define name="DDGAIN_Q" value="0"/>
<define name="DDGAIN_R" value="0"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="300"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="0"/>
<define name="THETA_PGAIN" value="300"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="0"/>
<define name="PSI_PGAIN" value="200"/>
<define name="PSI_DGAIN" value="400"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="0"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="70"/>
<define name="IGAIN" value="20"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="12.4" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="12.8" unit="V"/>
<define name="LOW_BAT_LEVEL" value="13.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="16.8" unit="V"/>
</section>
</airframe>
@@ -0,0 +1,218 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- this is a quadrotor frame in X-configuration equiped with
* Autopilot: Lisa-m
* Actuators: HobbyKing Avro Slim 20A
* GPS: Ublox http://wiki.paparazziuav.org/wiki/Subsystem/gps
* RC: one Spektrum sat http://wiki.paparazziuav.org/wiki/Subsystem/radio_control#Spektrum
-->
<airframe name="Quadrotor MavTec">
<firmware name="rotorcraft">
<target name="ap" board="lisa_m_2.0">
<subsystem name="radio_control" type="spektrum">
<define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>
<define name="RADIO_MODE" value="RADIO_FLAP"/>
<!--define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="1"/-->
</subsystem>
<configure name="NO_LUFTBOOT" value="1"/>
<!-- MPU6000 is configured to output data at 2kHz, but polled at 512Hz PERIODIC_FREQUENCY -->
</target>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<define name="SERVO_HZ" value="400"/>
</subsystem>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="aspirin_v2.2"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</subsystem>
<subsystem name="ins"/>
</firmware>
<firmware name="test_progs">
</firmware>
<modules>
</modules>
<servos driver="Pwm">
<servo name="MOTOR_1" no="0" min="1000" neutral="1100" max="1900"/>
<servo name="MOTOR_2" no="1" min="1000" neutral="1100" max="1900"/>
<servo name="MOTOR_3" no="2" min="1000" neutral="1100" max="1900"/>
<servo name="MOTOR_4" no="3" min="1000" neutral="1100" max="1900"/>
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="256"/>
<!-- order (and rotation direction) : NE (CW), SE (CCW), SW (CW), NW (CCW) -->
<define name="ROLL_COEF" value="{ -256, -256, 256, 256 }"/>
<define name="PITCH_COEF" value="{ 256, -256, -256, 256 }"/>
<define name="YAW_COEF" value="{ -256, 256, -256, 256 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="MOTOR_1" value="motor_mixing.commands[0]"/>
<set servo="MOTOR_2" value="motor_mixing.commands[1]"/>
<set servo="MOTOR_3" value="motor_mixing.commands[2]"/>
<set servo="MOTOR_4" value="motor_mixing.commands[3]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_X_NEUTRAL" value="-9"/>
<define name="ACCEL_Y_NEUTRAL" value="24"/>
<define name="ACCEL_Z_NEUTRAL" value="65"/>
<define name="ACCEL_X_SENS" value="4.90393762843" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.919330721" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.85197383186" integer="16"/>
<!-- replace this with your own calibration -->
<define name="MAG_X_NEUTRAL" value="-88"/>
<define name="MAG_Y_NEUTRAL" value="42"/>
<define name="MAG_Z_NEUTRAL" value="44"/>
<define name="MAG_X_SENS" value="3.55840131062" integer="16"/>
<define name="MAG_Y_SENS" value="3.59312782171" integer="16"/>
<define name="MAG_Z_SENS" value="4.16199901926" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="270." unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
<define name="USE_GPS_HEADING" value="0"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>
<section name="INS" prefix="INS_">
<!-- Use GPS altitude measurments and set the R gain -->
<!-- define name="USE_GPS_ALT" value="1"/ -->
<!-- define name="VFF_R_GPS" value="0.01"/ -->
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="900"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="900"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="900"/>
<define name="PSI_DGAIN" value="200"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="75"/>
<define name="THETA_DDGAIN" value="75"/>
<define name="PSI_DDGAIN" value="75"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="70"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;, &quot;nw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_x_quad&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
@@ -0,0 +1,24 @@
<conf>
<aircraft
name="Tester"
ac_id="1"
airframe="airframes/examples/microjet.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing.xml"
flight_plan="flight_plans/basic.xml"
settings="settings/fixedwing_basic.xml"
settings_modules="modules/nav_smooth.xml modules/infrared_adc.xml modules/tune_airspeed.xml"
gui_color="blue"
/>
<aircraft
name="VivifyMK1"
ac_id="217"
airframe="airframes/OpenUAS/openuas_vivify_mk1.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_fixedwing_imu_9k6.xml"
flight_plan="flight_plans/OpenUAS/openuas_obc2014_kingaroy.xml"
settings="settings/control/ctl_energy.xml [settings/estimation/ins_neutrals.xml] settings/estimation/ac_char.xml settings/fixedwing_basic.xml"
settings_modules="modules/air_data.xml modules/gps_ubx_ucenter.xml modules/airspeed_adc.xml modules/tune_airspeed.xml modules/photogrammetry_calculator.xml modules/digital_cam_uart.xml modules/nav_survey_poly_osam.xml modules/nav_smooth.xml"
gui_color="#fffffac7c07a"
/>
</conf>
@@ -0,0 +1,177 @@
<control_panel name="paparazzi control panel">
<section name="variables">
<variable name="downlink_serial_port" value="/dev/ttyUSB0"/>
<variable name="fbw_serial_port" value="/dev/ttyS1"/>
<variable name="ap_serial_port" value="/dev/ttyS0"/>
<variable name="ivy_bus" value="127:2010"/>
<variable name="map" value="muret_UTM.xml"/>
<variable name="flight_plan" value="flight_plans/muret1.xml"/>
</section>
<section name="programs">
<program name="Server" command="sw/ground_segment/tmtc/server">
<arg flag="-b" variable="ivy_bus"/>
</program>
<program name="Data Link" command="sw/ground_segment/tmtc/link">
<arg flag="-b" variable="ivy_bus"/>
</program>
<program name="Link Combiner" command="sw/ground_segment/python/redundant_link/link_combiner.py"/>
<program name="Vision Results" command="sw/ext/ardrone2_vision/modules/ObstacleAvoidSkySegmentation/visionresult/visionresult"/>
<program name="GCS" command="sw/ground_segment/cockpit/gcs">
<arg flag="-speech"/>
<arg flag="-b" variable="ivy_bus"/>
</program>
<program name="Flight Plan Editor" command="sw/ground_segment/cockpit/gcs -edit"/>
<program name="Messages" command="sw/ground_segment/tmtc/messages">
<arg flag="-b" variable="ivy_bus"/>
</program>
<program name="Messages (Python)" command="sw/ground_segment/python/messages_app/messagesapp.py"/>
<program name="Settings" command="sw/ground_segment/tmtc/settings">
<arg flag="-b" variable="ivy_bus"/>
</program>
<program name="Settings (Python)" command="sw/ground_segment/python/settings_app/settingsapp.py"/>
<program name="GPSd position display" command="sw/ground_segment/tmtc/gpsd2ivy"/>
<program name="Log Plotter" command="sw/logalizer/plot"/>
<program name="Real-time Plotter" command="sw/logalizer/plotter"/>
<program name="Log File Player" command="sw/logalizer/play">
<arg flag="-b" variable="ivy_bus"/>
</program>
<program name="Simulator" command="sw/simulator/pprzsim-launch">
<arg flag="-b" variable="ivy_bus"/>
</program>
<program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
<arg flag="-b" variable="ivy_bus"/>
</program>
<program name="Hardware in the Loop" command="sw/simulator/simhitl">
<arg flag="-fbw" variable="fbw_serial_port"/>
<arg flag="-ap" variable="ap_serial_port"/>
</program>
<program name="Environment Simulator" command="sw/simulator/gaia">
<arg flag="-b" variable="ivy_bus"/>
</program>
<program name="Http Server" command="sw/ground_segment/tmtc/boa"/>
<program name="Plot Meteo Profile" command="sw/logalizer/plotprofile"/>
<program name="Weather Station" command="sw/ground_segment/misc/davis2ivy">
<arg flag="-b" variable="ivy_bus"/>
<arg flag="-d" constant="/dev/ttyUSB1"/>
</program>
<program name="IvySerialBridge" command="sw/ground_segment/tmtc/ivy_serial_bridge"/>
<program name="Photolist" command="sw/tools/photolist/build/photolist | tee photolist.log -a"/>
<program name="Ivy2NMEAout" command="sw/ground_segment/misc/ivy2nmeaout">
<arg flag="-b" variable="ivy_bus"/>
<arg flag="-d" constant="/dev/ttyUSB1"/>
<arg flag="-s" constant="9600"/>
</program>
</section>
<section name="sessions">
<session name="OBC2014 Simulation FG">
<program name="GCS">
<arg flag="-layout" constant="./OpenUAS/openuas_obc2014_kingaroy.xml"/>
<arg flag="-maps_fill"/>
<arg flag="-zoom" constant=".5"/>
<arg flag="-track_size" constant="200"/>
<arg flag="-speech"/>
<arg flag="-maps_no_http"/>
</program>
<program name="Server">
<arg flag="-n"/>
</program>
<program name="Simulator">
<arg flag="-a" constant="VivifyMK1"/>
<arg flag="-t" constant="sim"/>
<arg flag="--boot"/>
<arg flag="--norc"/>
<arg flag="--fg_host" constant="127.0.0.1"/>
<arg flag="--fg_time_offset" constant="21600"/>
</program>
<program name="Environment Simulator"/>
</session>
<session name="OpenUAS Fieldtest">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-uplink"/>
<arg flag="-s" constant="9600"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech"/>
<arg flag="-maps_fill"/>
<arg flag="-mercator"/>
<arg flag="-maps_no_http"/>
<arg flag="-track_size" constant="300"/>
<arg flag="-zoom" constant="0.5"/>
</program>
<program name="Messages"/>
</session>
<session name="OpenUAS OBC2014">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-uplink"/>
<arg flag="-s" constant="9600"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech"/>
<arg flag="-mercator"/>
<arg flag="-maps_no_http"/>
<arg flag="-track_size" constant="300"/>
<arg flag="-zoom" constant="0.5"/>
</program>
<program name="Messages"/>
</session>
<session name="HITL">
<program name="Hardware in the Loop">
<arg flag="-a" constant="HITL"/>
<arg flag="-noground"/>
<arg flag="-boot"/>
</program>
<program name="GCS"/>
<program name="Data Link">
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
</session>
<session name="OpenUAS Simulation with OBC 2014 GUI">
<program name="GCS">
<arg flag="-speech"/>
<arg flag="-layout" constant="./OpenUAS/openuas_searchandrescue2014.xml"/>
<arg flag="-center" constant="'WGS84"/>
<arg flag="51.990" constant="4.378'"/>
<arg flag="-ref" constant="'WGS84"/>
<arg flag="51.990" constant="4.378'"/>
</program>
<program name="Server">
<arg flag="-n"/>
</program>
<program name="Simulator">
<arg flag="-a" constant="VivfifyMK1"/>
<arg flag="-boot"/>
<arg flag="-norc"/>
</program>
<program name="Environment Simulator"/>
</session>
<session name="OpenUAS Forward AC via UDP">
<program name="Ivy2Udp">
<arg flag="-b" constant="127:2010"/>
<arg flag="-h" constant="192.168.78.1"/>
<arg flag="-p" constant="4242"/>
<arg flag="-dp" constant="4243"/>
<arg flag="-id" constant="16"/>
</program>
</session>
<session name="OpenUAS USB0: Xtend Transparent @9600 with HW flowcontrol">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyUSB0"/>
<arg flag="-uplink"/>
<arg flag="-s" constant="9600"/>
<arg flag="-hfc"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech"/>
<arg flag="-layout" constant="./OpenUAS/gcsHeering.xml"/>
</program>
<program name="Messages"/>
</session>
</section>
</control_panel>
File diff suppressed because it is too large Load Diff
@@ -169,7 +169,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
@@ -171,7 +171,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
@@ -133,7 +133,6 @@ Lisa/S v1.0 board (http://wiki.paparazziuav.org/wiki/Lisa/S)
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="JSBSIM_MODEL" value="&quot;simple_quad&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
@@ -206,7 +206,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="JSBSIM_MODEL" value="&quot;simple_quad&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
@@ -231,7 +231,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="JSBSIM_MODEL" value="&quot;simple_quad&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
-1
View File
@@ -200,7 +200,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_ardrone2&quot;"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_ardrone2.h&quot;"/>
</section>
@@ -217,7 +217,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_ardrone2&quot;"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_ardrone2.h&quot;"/>
</section>
+216
View File
@@ -0,0 +1,216 @@
<!DOCTYPE airframe SYSTEM "airframe.dtd">
<airframe name="bebop">
<firmware name="rotorcraft">
<target name="ap" board="bebop">
<subsystem name="telemetry" type="transparent_udp"/>
<subsystem name="radio_control" type="datalink"/>
</target>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>
<!--define name="USE_SONAR" value="TRUE"/-->
<!-- Subsystem section -->
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="bebop"/>
<subsystem name="imu" type="bebop"/>
<subsystem name="gps" type="furuno"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="extended"/>
</firmware>
<modules main_freq="512">
<load name="send_imu_mag_current.xml"/>
<load name="file_logger.xml">
<define name="FILE_LOGGER_PATH" value="\\\"/data/ftp/internal_000/\\\""/>
</load>
</modules>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="6000"/>
</commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="3000" neutral="3000" max="12000"/>
<servo name="TOP_RIGHT" no="1" min="3000" neutral="3000" max="12000"/>
<servo name="BOTTOM_RIGHT" no="2" min="3000" neutral="3000" max="12000"/>
<servo name="BOTTOM_LEFT" no="3" min="3000" neutral="3000" max="12000"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="255"/>
<define name="MAX_SATURATION_OFFSET" value="3*MAX_PPRZ"/>
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
<define name="ROLL_COEF" value="{ -256, 256, 256, -256 }"/>
<define name="PITCH_COEF" value="{ 256, 256, -256, -256 }"/>
<define name="YAW_COEF" value="{ 256, -256, 256, -256 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[0]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[1]"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[2]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[3]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="18"/>
<define name="MAG_Y_NEUTRAL" value="157"/>
<define name="MAG_Z_NEUTRAL" value="49"/>
<define name="MAG_X_SENS" value="10.5007722373" integer="16"/>
<define name="MAG_Y_SENS" value="11.1147400462" integer="16"/>
<define name="MAG_Z_SENS" value="11.6479371722" integer="16"/>
<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- Toulouse -->
<!--define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/-->
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="300" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="650"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="0"/>
<define name="THETA_PGAIN" value="650"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="0"/>
<define name="PSI_PGAIN" value="4000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="0"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="283"/>
<define name="HOVER_KD" value="82"/>
<define name="HOVER_KI" value="13"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="79"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="30"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_ardrone2&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_ardrone2.h&quot;"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
-1
View File
@@ -206,7 +206,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="JSBSIM_MODEL" value="&quot;simple_quad&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
-1
View File
@@ -193,7 +193,6 @@ B2L -> CW
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
@@ -176,7 +176,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
-1
View File
@@ -190,7 +190,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
-1
View File
@@ -151,7 +151,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
-1
View File
@@ -165,7 +165,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
-1
View File
@@ -191,7 +191,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>

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