Skip to content

Commit

Permalink
Merge pull request #90 from rosflight/RC1.0
Browse files Browse the repository at this point in the history
Release Candidate 0.1.0
  • Loading branch information
dpkoch committed Jun 14, 2017
2 parents 4ae102c + 963f11f commit 2427d47
Show file tree
Hide file tree
Showing 86 changed files with 5,411 additions and 1,975 deletions.
1 change: 1 addition & 0 deletions .astylerc
Original file line number Diff line number Diff line change
Expand Up @@ -7,3 +7,4 @@ align-pointer=name
align-reference=name
max-code-length=120
suffix=none
--ignore-exclude-errors-x
5 changes: 4 additions & 1 deletion .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,10 @@
*.orig
*.user.*
*.o
/build/*
/boards/*/build/
/site/

param_description.md

*.aux
*.fdb_latexmk
Expand Down
8 changes: 4 additions & 4 deletions .gitmodules
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
[submodule "lib/breezystm32"]
path = lib/breezystm32
url = https://github.com/simondlevy/BreezySTM32.git
[submodule "lib/mavlink/v1.0"]
path = lib/mavlink/v1.0
url = https://github.com/BYU-MAGICC/mavlink_c_library.git
url = https://github.com/rosflight/mavlink_c_library.git
[submodule "boards/naze/lib/breezystm32"]
path = boards/naze/lib/breezystm32
url = https://github.com/rosflight/BreezySTM32.git
29 changes: 29 additions & 0 deletions LICENSE.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
BSD 3-Clause License

Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

* 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.

* Neither the name of the copyright holder 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 HOLDER 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.
212 changes: 41 additions & 171 deletions Makefile
Original file line number Diff line number Diff line change
@@ -1,190 +1,60 @@
###############################################################################
# "THE BEER-WARE LICENSE" (Revision 42):
# <msmith@FreeBSD.ORG> wrote this file. As long as you retain this notice you
# can do whatever you want with this stuff. If we meet some day, and you think
# this stuff is worth it, you can buy me a beer in return

###############################################################################

# external libraries
BREEZY_DIR = lib/breezystm32
TURBOTRIG_DIR = lib/turbotrig

# project source files
PROJECT_SRC = src/main.c \
src/estimator.c \
src/flash.c \
src/mavlink.c \
src/mavlink_param.c \
src/mavlink_receive.c \
src/mavlink_stream.c \
src/mavlink_util.c \
src/mixer.c \
src/param.c \
src/rc.c \
src/sensors.c \
src/mux.c \
src/controller.c \
src/mode.c \

###############################################################################
################################################################################
#
# Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
#
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright notice, this
# list of conditions and the following disclaimer.
#
# * 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.
#
# * Neither the name of the copyright holder 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 HOLDER 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.

# You probably shouldn't modify anything below here!

TARGET ?= rosflight2
TARGET = rosflight

# Compile-time options
OPTIONS ?=
BOARD ?= naze

# Debugger options, must be empty or GDB
DEBUG ?=

# Serial port/device for flashing
SERIAL_DEVICE ?= /dev/ttyUSB0

# Working directories
ROOT = $(dir $(lastword $(MAKEFILE_LIST)))
SRC_DIR = $(ROOT)
CMSIS_DIR = $(BREEZY_DIR)/lib/CMSIS
STDPERIPH_DIR = $(BREEZY_DIR)/lib/STM32F10x_StdPeriph_Driver
OBJECT_DIR = $(ROOT)/build
BIN_DIR = $(ROOT)/build

rosflight2_SRC =$(BREEZY_DIR)/drv_gpio.c \
$(BREEZY_DIR)/drv_i2c.c \
$(BREEZY_DIR)/drv_adc.c \
$(BREEZY_DIR)/drv_spi.c \
$(BREEZY_DIR)/drv_pwm.c \
$(BREEZY_DIR)/drv_system.c \
$(BREEZY_DIR)/drv_serial.c \
$(BREEZY_DIR)/drv_uart.c \
$(BREEZY_DIR)/drv_timer.c \
$(BREEZY_DIR)/drv_mpu6050.c \
$(BREEZY_DIR)/drv_ms4525.c \
$(BREEZY_DIR)/drv_mb1242.c \
$(BREEZY_DIR)/drv_ms5611.c \
$(BREEZY_DIR)/printf.c \
$(BREEZY_DIR)/startup_stm32f10x_md_gcc.S \
$(TURBOTRIG_DIR)/turbotrig.c \
$(TURBOTRIG_DIR)/turbovec.c \
$(PROJECT_SRC) \
$(CMSIS_SRC) \
$(STDPERIPH_SRC)

VPATH := $(SRC_DIR):$(SRC_DIR)/startups
PARALLEL_JOBS ?= 4

# Search path and source files for the CMSIS sources
VPATH := $(VPATH):$(CMSIS_DIR)/CM3/CoreSupport:$(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x
CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM3/CoreSupport/*.c \
$(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x/*.c))
# Build configuration
BOARD_DIR = boards/$(BOARD)

# Search path and source files for the ST stdperiph library
VPATH := $(VPATH):$(STDPERIPH_DIR)/src
STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c))

###############################################################################
# Things that might need changing to use different tools
#

# Tool names
CC = arm-none-eabi-gcc -std=gnu99
OBJCOPY = arm-none-eabi-objcopy

#
# Tool options.
#
INCLUDE_DIRS = include \
lib \
$(BREEZY_DIR) \
$(STDPERIPH_DIR)/inc \
$(CMSIS_DIR)/CM3/CoreSupport \
$(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x \
.PHONY: all flash clean

ARCH_FLAGS = -mthumb -mcpu=cortex-m3

ifeq ($(DEBUG),GDB)
OPTIMIZE = -Og

else
OPTIMIZE = -Os
LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE)
endif

DEBUG_FLAGS = -ggdb3

CFLAGS = $(ARCH_FLAGS) \
$(LTO_FLAGS) \
$(addprefix -D,$(OPTIONS)) \
$(addprefix -I,$(INCLUDE_DIRS)) \
$(DEBUG_FLAGS) \
-Wall -pedantic -Wextra -Wshadow -Wunsafe-loop-optimizations \
-ffunction-sections \
-fdata-sections \
-DSTM32F10X_MD \
-DUSE_STDPERIPH_DRIVER \
-D$(TARGET)

ASFLAGS = $(ARCH_FLAGS) \
-x assembler-with-cpp \
$(addprefix -I,$(INCLUDE_DIRS))

LD_SCRIPT = $(BREEZY_DIR)/stm32_flash.ld
LDFLAGS = -lm \
-nostartfiles \
--specs=nano.specs \
-lc \
-lnosys \
$(ARCH_FLAGS) \
$(LTO_FLAGS) \
$(DEBUG_FLAGS) \
-static \
-Wl,-gc-sections,-Map,$(TARGET_MAP) \
-T$(LD_SCRIPT)

#
# Things we will build
#

TARGET_HEX = $(BIN_DIR)/$(TARGET).hex
TARGET_ELF = $(BIN_DIR)/$(TARGET).elf
TARGET_OBJS = $(addsuffix .o,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $($(TARGET)_SRC))))
TARGET_MAP = $(OBJECT_DIR)/$(TARGET).map

# List of buildable ELF files and their object dependencies.
# It would be nice to compute these lists, but that seems to be just beyond make.

$(TARGET_HEX): $(TARGET_ELF)
$(OBJCOPY) -O ihex --set-start 0x8000000 $< $@

$(TARGET_ELF): $(TARGET_OBJS)
$(CC) -o $@ $^ $(LDFLAGS)

MKDIR_OBJDIR = @mkdir -p $(dir $@)

# Compile
$(OBJECT_DIR)/$(TARGET)/%.o: %.c
$(MKDIR_OBJDIR)
@echo %% $(notdir $<)
@$(CC) -c -o $@ $(CFLAGS) $<

# Assemble
$(OBJECT_DIR)/$(TARGET)/%.o: %.S
$(MKDIR_OBJDIR)
@echo %% $(notdir $<)
@$(CC) -c -o $@ $(ASFLAGS) $<
all:
cd $(BOARD_DIR) && make -j$(PARALLEL_JOBS) DEBUG=$(DEBUG) SERIAL_DEVICE=$(SERIAL_DEVICE)

clean:
rm -rf $(TARGET_HEX) $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP) $(BIN_DIR) $(OBJECT_DIR)

flash_$(TARGET): $(TARGET_HEX)
stty -F $(SERIAL_DEVICE) raw speed 921600 -crtscts cs8 -parenb -cstopb -ixon
stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 921600 $(SERIAL_DEVICE)

flash: flash_$(TARGET)

unbrick: $(TARGET_HEX)
stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon
stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE)
cd $(BOARD_DIR) && make clean

listen:
miniterm.py $(SERIAL_DEVICE) 115200
flash:
cd $(BOARD_DIR) && make flash
24 changes: 7 additions & 17 deletions README.md
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
# ROSflight
[![Build Status](https://travis-ci.org/byu-magicc/ROSflight.svg?branch=master)](https://travis-ci.org/byu-magicc/ROSflight) [![Documentation](https://codedocs.xyz/byu-magicc/ROSflight.svg)](https://codedocs.xyz/byu-magicc/ROSflight/)
[![Build Status](https://travis-ci.org/rosflight/firmware.svg?branch=master)](https://travis-ci.org/byu-magicc/ROSflight)
[![Documentation Status](https://readthedocs.org/projects/rosflight/badge/?version=latest)](http://docs.rosflight.org/en/latest/?badge=latest)
<!--[![Documentation](https://codedocs.xyz/byu-magicc/ROSflight.svg)](https://codedocs.xyz/byu-magicc/ROSflight/) -->

This is the firmware required for STM32F10x-based flight controllers (naze32, flip32 etc...) to run ROSflight. ROSflight is a software architecture which uses a simple, inexpensive flight controller in tandem with a much more capable onboard computer running ROS. The onboard computer is given a high-bandwidth connection to the flight controller to access sensor information and perform actuator commands at high rates. This architectures provides direct control of lower-level functions via the embedded processor while also enabling more complicated functionality such as vision processing and optimization via the ROS middleware.

Expand All @@ -14,15 +16,7 @@ These objectives will allow researchers to more easily develop, test and field U

## How to Use ##

This repository is the firmware for the STM32F10x. The ROS driver for the flashed flight controller can be found at BYU-MAGICC/fcu_io2. To flash the firmware to a flight controller, simply follow the directions in the wiki. To get access to senors and actuators, simply install and run the ROS driver.

Parameters can be retrived, set, and saved to the EEPROM in real-time using the rosservice API in fcu_io2. They may also be set by changing the default values in param.c, then re-building and flashing the firmware which is likely the easiest method until the ROSflight Configurator is completed.

Sensors are published on ROS topics after they are received from the flight controller. To change the rate at which sensors are published, change the corresponding parameter on the FCU.

Commands are accepted via the extended_command ROS topic.

See the documentation in fcu_io2 for more information regarding ExtendedCommands, accelerometer calibration, sensor publications and other ROS-specific details.
Documentation is located at http://docs.rosflight.org.

## Implementation Details ##

Expand All @@ -32,24 +26,20 @@ ROSflight uses MAVlink to communicate over a USB cable. MAVlink has primarily b
#### Sensors
ROSflight provides access to the onboard 6-axis gyroscope and accelerometer at up to 1000Hz. IMU measurements are very carefully time-stamped and these stamps are accurate to within a microsecond. Using MAVlink time synchronization, this stamp is relayed to ROS, and is also accurate to within a microsecond. However, depending on the order in which measurements are sent over the serial line, IMU messages may not be relayed at a constant rate, which means the inter-arrival time between messages may vary over time on the onboard computer. However, the time stamps in the IMU message header are accurate and should be trusted more than the inter-arrival time.

The MB1242 I2C sonar and MS4252 I2C differential pressure sensor are also currently supported, as wel as the onboard magnetometer and barometer. These additional sensors have been tested at rates up to 50 Hz without any performance degredation. GPS is currently not supported on the flight control board, and is instead supported via the GPS node of fcu_common and connecting a serial GPS to the onboard computer directly using an FTDI cable. Other sensors have not been directly implemented, but could be given a little effort. Contact us via an issue if the sensor you need is not available.
The MB1242 I2C sonar and MS4252 I2C differential pressure sensor are also currently supported, as wel as the onboard magnetometer and barometer. These additional sensors have been tested at rates up to 50 Hz without any performance degredation. GPS is currently not supported on the flight control board, and should instead be connected to the onboard computer directly using an FTDI cable. Other sensors have not been directly implemented, but could be given a little effort. Contact us via an issue if the sensor you need is not available.

#### Estimation
Onboard estimation is performed using a quaternion-based Mahoney Filter, with the addition of a quadratic approximation of angular rates, and the use of a matrix exponential during the propagation step. Details can be found in the documentation (doc/estimator.tex)
Onboard estimation is performed using a quaternion-based Mahoney Filter, with the addition of a quadratic approximation of angular rates, and the use of a matrix exponential during the propagation step. Details can be found in the documentation (reports/estimator.tex)

#### Control
Control is performed using a standard PID control scheme with default gains found in param.c. Control is performed in four modes:
Control is performed using a standard PID control scheme with default gains found in param.c. Control is performed in three modes:

1. Pass-Through - The pass-through mode is meant primarily for operating fixed-wing UAVs, and maps the four input channels directly to actuator outputs.

2. Rate - Rate mode controls the angular rate of the 3 body-fixed axes, and overall throttle response. This is much like "Acro" mode of other multirotor autopilots. This mode is primarily meant for multirotor UAVs.

3. Angle - Angle mode is another multirotor UAV control scheme in which the angle of the body-fixed x and y axes are controlled, while the z axis is controlled to a specific angular rate, and overall throttle is directly passed through. This is nearly identical to other "Angle" modes of other multirotor autopilots.

4. Altitude - Altitude mode uses feedback from the sonar and barometer to perform altitude control in addition to x and y angles, and z angular rate.

More in-depth discussion and implementation details, as well as a discussion of response can be found in the documentation (doc/controller.tex).

#### Process Priority
Tasks are prioritized according to the following scheme:

Expand Down

0 comments on commit 2427d47

Please sign in to comment.