dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' into gpio_led

This commit is contained in:
Anton Babushkin 2013-05-17 12:48:46 +04:00
parent 9a9e41f7a2 fa816d0fd6
commit 2f280bb4ca
31 changed files with 932 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Makefile
View File

@ -159,11 +159,11 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
.PHONY: clean
clean:
$(Q) $(RMDIR) $(BUILD_DIR)*.build
$(Q) $(REMOVE) -f $(IMAGE_DIR)*.px4
$(Q) $(REMOVE) $(IMAGE_DIR)*.px4
.PHONY: distclean
distclean: clean
$(Q) $(REMOVE) -f $(ARCHIVE_DIR)*.export
$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
#
@ -196,6 +196,11 @@ help:
@echo " distclean"
@echo " Remove all compilation products, including NuttX RTOS archives."
@echo ""
@echo " upload"
@echo " When exactly one config is being built, add this target to upload the"
@echo " firmware to the board when the build is complete. Not supported for"
@echo " all configurations."
@echo ""
@echo " Common options:"
@echo " ---------------"
@echo ""

6
ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
View File

@ -20,10 +20,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

6
ROMFS/px4fmu_common/init.d/rc.IO_QUAD
View File

@ -13,10 +13,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

6
ROMFS/px4fmu_common/init.d/rc.PX4IO
View File

@ -13,10 +13,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

8
ROMFS/px4fmu_common/init.d/rc.PX4IOAR
View File

@ -17,13 +17,13 @@ echo "[init] doing PX4IOAR startup..."
uorb start
#
# Init the parameter storage
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

6
ROMFS/px4fmu_common/init.d/rc.hil
View File

@ -17,10 +17,10 @@ hil mode_pwm
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

8
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -7,6 +7,14 @@
# Start sensor drivers here.
#
#
# Check for UORB
#
if uorb start
then
echo "uORB started"
fi
ms5611 start
adc start

71
makefiles/README.txt Normal file
View File

@ -0,0 +1,71 @@
PX4 Build System
================
The files in this directory implement the PX4 runtime firmware build system
and configuration for the standard PX4 boards and software, in conjunction
with Makefile in the parent directory.
../Makefile
Top-level makefile for the PX4 build system. This makefile supports
building NuttX archives, as well as supervising the building of all
of the defined PX4 firmware configurations.
Try 'make help' in the parent directory for documentation.
firmware.mk
Manages the build for one specific firmware configuration.
See the comments at the top of this file for detailed documentation.
Builds modules, builtin command lists and the ROMFS (if configured).
This is the makefile directly used by external build systems; it can
be configured to compile modules both inside and outside the PX4
source tree. When used in this mode, at least BOARD, MODULES and
CONFIG_FILE must be set.
module.mk
Called by firmware.mk to build individual modules.
See the comments at the top of this file for detailed documentation.
Not normally used other than by firmware.mk.
nuttx.mk
Called by ../Makefile to build or download the NuttX archives.
upload.mk
Called by ../Makefile to upload files to a target board. Can be used
by external build systems as well.
setup.mk
Provides common path and tool definitions. Implements host system-specific
compatibility hacks.
board_<boardname>.mk
Board-specific configuration for <boardname>. Typically sets CONFIG_ARCH
and then includes the toolchain definition for the board.
config_<boardname>_<configname>.mk
Parameters for a specific configuration on a specific board.
The board name is derived from the filename. Sets MODULES to select
source modules to be included in the configuration, may also set
ROMFS_ROOT to build a ROMFS and BUILTIN_COMMANDS to include non-module
commands (e.g. from NuttX)
toolchain_<toolchainname>.mk
Provides macros used to compile and link source files.
Accepts EXTRADEFINES to add additional pre-processor symbol definitions,
EXTRACFLAGS, EXTRACXXFLAGS, EXTRAAFLAGS and EXTRALDFLAGS to pass
additional flags to the C compiler, C++ compiler, assembler and linker
respectively.
Defines the COMPILE, COMPILEXX, ASSEMBLE, PRELINK, ARCHIVE and LINK
macros that are used elsewhere in the build system.

4
makefiles/config_px4fmu_default.mk
View File

@ -106,6 +106,10 @@ MODULES += modules/uORB
# https://pixhawk.ethz.ch/px4/dev/debug_values
#MODULES += examples/px4_mavlink_debug
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
MODULES += examples/fixedwing_control
#
# Transitional support - add commands from the NuttX export archive.
#

33
makefiles/firmware.mk
View File

@ -201,9 +201,9 @@ MODULES := $(sort $(MODULES))
# locate the first instance of a module by full path or by looking on the
# module search path
define MODULE_SEARCH
$(abspath $(firstword $(wildcard $(1)/module.mk) \
$(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk)) \
MISSING_$1))
$(firstword $(abspath $(wildcard $(1)/module.mk)) \
$(abspath $(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk))) \
MISSING_$1)
endef
# make a list of module makefiles and check that we found them all
@ -223,12 +223,15 @@ MODULE_OBJS := $(foreach path,$(dir $(MODULE_MKFILES)),$(WORK_DIR)$(path)module
.PHONY: $(MODULE_OBJS)
$(MODULE_OBJS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
$(MODULE_OBJS): mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath))
$(MODULE_OBJS): workdir = $(@D)
$(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
$(Q) $(MKDIR) -p $(workdir)
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \
MODULE_WORK_DIR=$(dir $@) \
-C $(workdir) \
MODULE_WORK_DIR=$(workdir) \
MODULE_OBJ=$@ \
MODULE_MK=$(mkfile) \
MODULE_NAME=$(lastword $(subst /, ,$(@D))) \
MODULE_NAME=$(lastword $(subst /, ,$(workdir))) \
module
# make a list of phony clean targets for modules
@ -266,14 +269,18 @@ endif
#
# Add dependencies on anything in the ROMFS root
ROMFS_DEPS += $(wildcard \
(ROMFS_ROOT)/* \
(ROMFS_ROOT)/*/* \
(ROMFS_ROOT)/*/*/* \
(ROMFS_ROOT)/*/*/*/* \
(ROMFS_ROOT)/*/*/*/*/* \
(ROMFS_ROOT)/*/*/*/*/*/*)
ROMFS_IMG = $(WORK_DIR)romfs.img
ROMFS_FILES += $(wildcard \
$(ROMFS_ROOT)/* \
$(ROMFS_ROOT)/*/* \
$(ROMFS_ROOT)/*/*/* \
$(ROMFS_ROOT)/*/*/*/* \
$(ROMFS_ROOT)/*/*/*/*/* \
$(ROMFS_ROOT)/*/*/*/*/*/*)
ifeq ($(ROMFS_FILES),)
$(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files)
endif
ROMFS_DEPS += $(ROMFS_FILES)
ROMFS_IMG = romfs.img
ROMFS_CSRC = $(ROMFS_IMG:.img=.c)
ROMFS_OBJ = $(ROMFS_CSRC:.c=.o)
LIBS += $(ROMFS_OBJ)

51
makefiles/module.mk
View File

@ -39,6 +39,10 @@
# symbols, as the global namespace is shared between all modules. Normally an
# module will just export one or more <command>_main functions.
#
# IMPORTANT NOTE:
#
# This makefile assumes it is being invoked in the module's output directory.
#
#
# Variables that can be set by the module's module.mk:
@ -179,26 +183,30 @@ CXXFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibi
#
module: $(MODULE_OBJ) $(MODULE_COMMAND_FILES)
##
## Locate sources (allows relative source paths in module.mk)
##
#define SRC_SEARCH
# $(abspath $(firstword $(wildcard $1) $(wildcard $(MODULE_SRC)/$1) MISSING_$1))
#endef
#
# Locate sources (allows relative source paths in module.mk)
#ABS_SRCS ?= $(foreach src,$(SRCS),$(call SRC_SEARCH,$(src)))
#MISSING_SRCS := $(subst MISSING_,,$(filter MISSING_%,$(ABS_SRCS)))
#ifneq ($(MISSING_SRCS),)
#$(error $(MODULE_MK): missing in SRCS: $(MISSING_SRCS))
#endif
#ifeq ($(ABS_SRCS),)
#$(error $(MODULE_MK): nothing to compile in SRCS)
#endif
#
define SRC_SEARCH
$(abspath $(firstword $(wildcard $1) $(wildcard $(MODULE_SRC)/$1) MISSING_$1))
endef
##
## Object files we will generate from sources
##
#OBJS := $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
ABS_SRCS ?= $(foreach src,$(SRCS),$(call SRC_SEARCH,$(src)))
MISSING_SRCS := $(subst MISSING_,,$(filter MISSING_%,$(ABS_SRCS)))
ifneq ($(MISSING_SRCS),)
$(error $(MODULE_MK): missing in SRCS: $(MISSING_SRCS))
endif
ifeq ($(ABS_SRCS),)
$(error $(MODULE_MK): nothing to compile in SRCS)
endif
#
# Object files we will generate from sources
#
OBJS := $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
OBJS = $(addsuffix .o,$(SRCS))
$(info SRCS $(SRCS))
$(info OBJS $(OBJS))
#
# SRCS -> OBJS rules
@ -206,13 +214,16 @@ OBJS := $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
$(OBJS): $(GLOBAL_DEPS)
$(filter %.c.o,$(OBJS)): $(MODULE_WORK_DIR)%.c.o: %.c $(GLOBAL_DEPS)
vpath %.c $(MODULE_SRC)
$(filter %.c.o,$(OBJS)): %.c.o: %.c $(GLOBAL_DEPS)
$(call COMPILE,$<,$@)
$(filter %.cpp.o,$(OBJS)): $(MODULE_WORK_DIR)%.cpp.o: %.cpp $(GLOBAL_DEPS)
vpath %.cpp $(MODULE_SRC)
$(filter %.cpp.o,$(OBJS)): %.cpp.o: %.cpp $(GLOBAL_DEPS)
$(call COMPILEXX,$<,$@)
$(filter %.S.o,$(OBJS)): $(MODULE_WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS)
vpath %.S $(MODULE_SRC)
$(filter %.S.o,$(OBJS)): %.S.o: %.S $(GLOBAL_DEPS)
$(call ASSEMBLE,$<,$@)
#

15
makefiles/toolchain_gnu-arm-eabi.mk
View File

@ -254,6 +254,20 @@ endef
# - relink the object and insert the binary file
# - edit symbol names to suit
#
# NOTE: exercise caution using this with absolute pathnames; it looks
# like the MinGW tools insert an extra _ in the binary symbol name; e.g.
# the path:
#
# /d/px4/firmware/Build/px4fmu_default.build/romfs.img
#
# is assigned symbols like:
#
# _binary_d__px4_firmware_Build_px4fmu_default_build_romfs_img_size
#
# when we would expect
#
# _binary__d_px4_firmware_Build_px4fmu_default_build_romfs_img_size
#
define BIN_SYM_PREFIX
_binary_$(subst /,_,$(subst .,_,$1))
endef
@ -267,4 +281,5 @@ define BIN_TO_OBJ
--redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \
--redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \
--strip-symbol $(call BIN_SYM_PREFIX,$1)_end
$(Q) $(REMOVE) $2.c $2.c.o
endef

8
src/drivers/ardrone_interface/ardrone_motor_control.c
View File

@ -482,10 +482,10 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
/* send motors via UART */
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);

6
src/drivers/drv_pwm_output.h
View File

@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm);
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
/** set the 'ARM ok' bit, which activates the safety switch */
#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5)
/** clear the 'ARM ok' bit, which deactivates the safety switch */
#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

5
src/drivers/px4fmu/fmu.cpp
View File

@ -606,6 +606,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(true);
break;
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
// these are no-ops, as no safety switch
break;
case PWM_SERVO_DISARM:
up_pwm_servo_arm(false);
break;

45
src/drivers/px4io/px4io.cpp
View File

@ -416,7 +416,7 @@ PX4IO::init()
* already armed, and has been configured for in-air restart
*/
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
(reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
(reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
@ -500,10 +500,9 @@ PX4IO::init()
/* dis-arm IO before touching anything */
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
PX4IO_P_SETUP_ARMING_ARM_OK |
PX4IO_P_SETUP_ARMING_FMU_ARMED |
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
/* publish RC config to IO */
ret = io_set_rc_config();
@ -702,16 +701,18 @@ PX4IO::io_set_arming_state()
uint16_t set = 0;
uint16_t clear = 0;
if (armed.armed) {
set |= PX4IO_P_SETUP_ARMING_ARM_OK;
if (armed.armed && !armed.lockdown) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
clear |= PX4IO_P_SETUP_ARMING_ARM_OK;
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
if (vstatus.flag_vector_flight_mode_ok) {
set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
if (vstatus.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
@ -1277,9 +1278,9 @@ PX4IO::print_status()
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
_battery_amp_per_volt,
_battery_amp_bias,
_battery_mamphour_total);
(double)_battery_amp_per_volt,
(double)_battery_amp_bias,
(double)_battery_mamphour_total);
printf("actuators");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
@ -1311,9 +1312,9 @@ PX4IO::print_status()
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@ -1354,12 +1355,22 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
switch (cmd) {
case PWM_SERVO_ARM:
/* set the 'armed' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED);
break;
case PWM_SERVO_SET_ARM_OK:
/* set the 'OK to arm' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK);
break;
case PWM_SERVO_CLEAR_ARM_OK:
/* clear the 'OK to arm' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0);
break;
case PWM_SERVO_DISARM:
/* clear the 'armed' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
break;
case PWM_SERVO_SET_UPDATE_RATE:

474
src/examples/fixedwing_control/main.c Normal file
View File

@ -0,0 +1,474 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file main.c
* Implementation of a fixed wing attitude controller. This file is a complete
* fixed wing controller flying manual attitude control or auto waypoint control.
* There is no need to touch any other system components to extend / modify the
* complete control architecture.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
/* process-specific header files */
#include "params.h"
/* Prototypes */
/**
* Daemon management function.
*/
__EXPORT int ex_fixedwing_control_main(int argc, char *argv[]);
/**
* Mainloop of daemon.
*/
int fixedwing_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators);
void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
/* Variables */
static bool thread_should_exit = false; /**< Daemon exit flag */
static bool thread_running = false; /**< Daemon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static struct params p;
static struct param_handles ph;
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators)
{
/*
* The PX4 architecture provides a mixer outside of the controller.
* The mixer is fed with a default vector of actuator controls, representing
* moments applied to the vehicle frame. This vector
* is structured as:
*
* Control Group 0 (attitude):
*
* 0 - roll (-1..+1)
* 1 - pitch (-1..+1)
* 2 - yaw (-1..+1)
* 3 - thrust ( 0..+1)
* 4 - flaps (-1..+1)
* ...
*
* Control Group 1 (payloads / special):
*
* ...
*/
/*
* Calculate roll error and apply P gain
*/
float roll_err = att->roll - att_sp->roll_body;
actuators->control[0] = roll_err * p.roll_p;
/*
* Calculate pitch error and apply P gain
*/
float pitch_err = att->pitch - att_sp->pitch_body;
actuators->control[1] = pitch_err * p.pitch_p;
}
void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
{
/*
* Calculate heading error of current position to desired position
*/
/* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */
float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
/* calculate heading error */
float yaw_err = att->yaw - bearing;
/* apply control gain */
att_sp->roll_body = yaw_err * p.hdng_p;
}
/* Main Thread */
int fixedwing_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user (warnx prints a line, including an appended\n, with variable arguments */
warnx("[example fixedwing control] started");
/* initialize parameters, first the handles, then the values */
parameters_init(&ph);
parameters_update(&ph, &p);
/* declare and safely initialize all structs to zero */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
struct vehicle_global_position_setpoint_s global_sp;
memset(&global_sp, 0, sizeof(global_sp));
/* output structs */
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* subscribe */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
float speed_body[3] = {0.0f, 0.0f, 0.0f};
struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }};
while (!thread_should_exit) {
/*
* Wait for a sensor or param update, check for exit condition every 500 ms.
* This means that the execution will block here without consuming any resources,
* but will continue to execute the very moment a new attitude measurement or
* a param update is published. So no latency in contrast to the polling
* design pattern (do not confuse the poll() system call with polling).
*
* This design pattern makes the controller also agnostic of the attitude
* update speed - it runs as fast as the attitude updates with minimal latency.
*/
int ret = poll(fds, 2, 500);
if (ret < 0) {
/* poll error, this will not really happen in practice */
warnx("poll error");
} else if (ret == 0) {
/* no return value = nothing changed for 500 ms, ignore */
} else {
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag (uORB API requirement) */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* if a param update occured, re-read our parameters */
parameters_update(&ph, &p);
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
/* Check if there is a new position measurement or position setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_sp_sub, &global_sp_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (global_sp_updated)
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (att.R_valid) {
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
} else {
speed_body[0] = 0;
speed_body[1] = 0;
speed_body[2] = 0;
warnx("Did not get a valid R\n");
}
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
/* control */
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* simple heading control */
control_heading(&global_pos, &global_sp, &att, &att_sp);
/* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */
actuators.control[1] = 0.0f;
actuators.control[2] = 0.0f;
/* simple attitude control */
control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
att_sp.yaw_body = 0;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
/* attitude control */
control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* sanity check and publish actuator outputs */
if (isfinite(actuators.control[0]) &&
isfinite(actuators.control[1]) &&
isfinite(actuators.control[2]) &&
isfinite(actuators.control[3])) {
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
}
}
}
printf("[ex_fixedwing_control] exiting, stopping all motors.\n");
thread_running = false;
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
fflush(stdout);
return 0;
}
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n");
exit(1);
}
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int ex_fixedwing_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("ex_fixedwing_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("ex_fixedwing_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tex_fixedwing_control is running\n");
} else {
printf("\tex_fixedwing_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}

41
src/examples/fixedwing_control/module.mk Normal file
View File

@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Fixedwing Attitude Control Demo / Example Application
#
MODULE_COMMAND = ex_fixedwing_control
SRCS = main.c \
params.c

77
src/examples/fixedwing_control/params.c Normal file
View File

@ -0,0 +1,77 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file params.c
*
* Parameters for fixedwing demo
*/
#include "params.h"
/* controller parameters, use max. 15 characters for param name! */
/**
*
*/
PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f);
/**
*
*/
PARAM_DEFINE_FLOAT(EXFW_ROLL_P, 0.2f);
/**
*
*/
PARAM_DEFINE_FLOAT(EXFW_PITCH_P, 0.2f);
int parameters_init(struct param_handles *h)
{
/* PID parameters */
h->hdng_p = param_find("EXFW_HDNG_P");
h->roll_p = param_find("EXFW_ROLL_P");
h->pitch_p = param_find("EXFW_PITCH_P");
return OK;
}
int parameters_update(const struct param_handles *h, struct params *p)
{
param_get(h->hdng_p, &(p->hdng_p));
param_get(h->roll_p, &(p->roll_p));
param_get(h->pitch_p, &(p->pitch_p));
return OK;
}

65
src/examples/fixedwing_control/params.h Normal file
View File

@ -0,0 +1,65 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file params.h
*
* Definition of parameters for fixedwing example
*/
#include <systemlib/param/param.h>
struct params {
float hdng_p;
float roll_p;
float pitch_p;
};
struct param_handles {
param_t hdng_p;
param_t roll_p;
param_t pitch_p;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct param_handles *h, struct params *p);

28
src/modules/commander/commander.c
View File

@ -1503,21 +1503,39 @@ int commander_thread_main(int argc, char *argv[])
if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
current_status.state_machine == SYSTEM_STATE_AUTO ||
current_status.state_machine == SYSTEM_STATE_MANUAL)) {
/* armed */
led_toggle(LED_BLUE);
/* armed, solid */
led_on(LED_AMBER);
} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* not armed */
led_toggle(LED_AMBER);
}
if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
/* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
&& (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
/* GPS lock */
led_on(LED_BLUE);
} else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* no GPS lock, but GPS module is aquiring lock */
led_toggle(LED_BLUE);
}
/* toggle error led at 5 Hz in HIL mode */
} else {
/* no GPS info, don't light the blue led */
led_off(LED_BLUE);
}
/* toggle GPS led at 5 Hz in HIL mode */
if (current_status.flag_hil_enabled) {
/* hil enabled */
led_toggle(LED_AMBER);
led_toggle(LED_BLUE);
} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
/* toggle error (red) at 5 Hz on low battery or error */
/* toggle arming (red) at 5 Hz on low battery or error */
led_toggle(LED_AMBER);
} else {

5
src/modules/commander/state_machine_helper.c
View File

@ -249,6 +249,11 @@ void publish_armed_status(const struct vehicle_status_s *current_status)
{
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
/* XXX allow arming by external components on multicopters only if not yet armed by RC */
/* XXX allow arming only if core sensors are ok */
armed.ready_to_arm = true;
/* lock down actuators if required, only in HIL */
armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);

5
src/modules/mathlib/CMSIS/module.mk
View File

@ -38,8 +38,9 @@
#
# Find sources
#
DSPLIB_SRCDIR := $(PX4_MODULE_SRC)/modules/mathlib/CMSIS
ABS_SRCS := $(wildcard $(DSPLIB_SRCDIR)/DSP_Lib/Source/*/*.c)
DSPLIB_SRCDIR := $(dir $(lastword $(MAKEFILE_LIST)))
SRCLIST := $(wildcard $(DSPLIB_SRCDIR)DSP_Lib/Source/*/*.c)
SRCS := $(patsubst $(DSPLIB_SRCDIR)%,%,$(SRCLIST))
INCLUDE_DIRS += $(DSPLIB_SRCDIR)/Include \
$(DSPLIB_SRCDIR)/Device/ARM/ARMCM4/Include \

4
src/modules/px4iofirmware/mixer.cpp
View File

@ -174,7 +174,7 @@ mixer_tick(void)
* here.
*/
bool should_arm = (
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
@ -246,7 +246,7 @@ void
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while fully armed */
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
return;
}

4
src/modules/px4iofirmware/protocol.h
View File

@ -145,9 +145,9 @@
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) /* OK to perform position / vector control (= position lock) */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */

9
src/modules/px4iofirmware/registers.c
View File

@ -142,9 +142,10 @@ volatile uint16_t r_page_setup[] =
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
PX4IO_P_SETUP_ARMING_IO_ARM_OK)
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@ -311,7 +312,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* so that an in-air reset of FMU can not lead to a
* lockup of the IO arming state.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
}
@ -362,7 +363,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
/* do not allow a RC config change while fully armed */
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
break;
}

30
src/modules/px4iofirmware/safety.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -32,7 +32,8 @@
****************************************************************************/
/**
* @file Safety button logic.
* @file safety.c
* Safety button logic.
*/
#include <nuttx/config.h>
@ -56,11 +57,11 @@ static unsigned counter = 0;
/*
* Define the various LED flash sequences for each system state.
*/
#define LED_PATTERN_SAFE 0xffff /**< always on */
#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */
#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */
#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */
#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */
#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */
#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */
#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */
#define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */
#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */
static unsigned blink_counter = 0;
@ -109,7 +110,8 @@ safety_check_button(void *arg)
* state machine, keep ARM_COUNTER_THRESHOLD the same
* length in all cases of the if/else struct below.
*/
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
@ -120,8 +122,6 @@ safety_check_button(void *arg)
counter++;
}
/* Disarm quickly */
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
if (counter < ARM_COUNTER_THRESHOLD) {
@ -138,21 +138,21 @@ safety_check_button(void *arg)
}
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_SAFE;
uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_IO_FMU_ARMED;
} else {
pattern = LED_PATTERN_IO_ARMED;
}
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_FMU_ARMED;
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) {
pattern = LED_PATTERN_FMU_OK_TO_ARM;
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
}
/* Turn the LED on if we have a 1 at the current bit position */

1
src/modules/uORB/topics/actuator_controls.h
View File

@ -69,6 +69,7 @@ ORB_DECLARE(actuator_controls_3);
/** global 'actuator output is live' control. */
struct actuator_armed_s {
bool armed; /**< Set to true if system is armed */
bool ready_to_arm; /**< Set to true if system is ready to be armed */
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
};