Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

Merge branch 'softserial'

Browse source 
Conflicts:
	src/board.h
	src/drv_pwm.c
	src/drv_softserial.c
	src/drv_softserial.h
	src/drv_system.c
	src/drv_timer.c
	src/drv_uart.c
	src/drv_uart.h
	src/main.c
	src/sensors.c
This commit is contained in:
Dominic Clifton 2013-10-01 22:16:25 +01:00
commit 750fea4b80
54 changed files with 5720 additions and 5677 deletions
Download patch file Download diff file Expand all files Collapse all files

458
Makefile
View file

@ -1,227 +1,231 @@
###############################################################################
# "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
###############################################################################
#
# Makefile for building the baseflight firmware.
#
# Invoke this with 'make help' to see the list of supported targets.
#
###############################################################################
# Things that the user might override on the commandline
#
# The target to build, must be one of NAZE, FY90Q OR OLIMEXINO
TARGET ?= NAZE
# Compile-time options
OPTIONS ?=
# Debugger optons, must be empty or GDB
DEBUG ?=
# Serial port/Device for flashing
SERIAL_DEVICE ?= /dev/ttyUSB0
###############################################################################
# Things that need to be maintained as the source changes
#
VALID_TARGETS = NAZE FY90Q OLIMEXINO
# Working directories
ROOT = $(dir $(lastword $(MAKEFILE_LIST)))
SRC_DIR = $(ROOT)/src
CMSIS_DIR = $(ROOT)/lib/CMSIS
STDPERIPH_DIR = $(ROOT)/lib/STM32F10x_StdPeriph_Driver
OBJECT_DIR = $(ROOT)/obj
BIN_DIR = $(ROOT)/obj
# Source files common to all targets
COMMON_SRC = startup_stm32f10x_md_gcc.S \
buzzer.c \
cli.c \
config.c \
gps.c \
imu.c \
main.c \
mixer.c \
mw.c \
sensors.c \
serial.c \
spektrum.c \
telemetry.c \
drv_gpio.c \
drv_i2c.c \
drv_i2c_soft.c \
drv_system.c \
drv_uart.c \
printf.c \
$(CMSIS_SRC) \
$(STDPERIPH_SRC)
# Source files for the NAZE target
NAZE_SRC = drv_spi.c \
drv_adc.c \
drv_adxl345.c \
drv_bmp085.c \
drv_ms5611.c \
drv_hcsr04.c \
drv_hmc5883l.c \
drv_ledring.c \
drv_mma845x.c \
drv_mpu3050.c \
drv_mpu6050.c \
drv_l3g4200d.c \
drv_pwm.c \
drv_timer.c \
$(COMMON_SRC)
# Source files for the FY90Q target
FY90Q_SRC = drv_adc_fy90q.c \
drv_pwm_fy90q.c \
$(COMMON_SRC)
# Source files for the OLIMEXINO target
OLIMEXINO_SRC = drv_spi.c \
drv_adc.c \
drv_adxl345.c \
drv_mpu3050.c \
drv_mpu6050.c \
drv_l3g4200d.c \
drv_pwm.c \
drv_timer.c \
drv_softserial.c \
$(COMMON_SRC)
# Search path for baseflight sources
VPATH := $(SRC_DIR):$(SRC_DIR)/baseflight_startups
# 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))
# 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
OBJCOPY = arm-none-eabi-objcopy
#
# Tool options.
#
INCLUDE_DIRS = $(SRC_DIR) \
$(STDPERIPH_DIR)/inc \
$(CMSIS_DIR)/CM3/CoreSupport \
$(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x \
ARCH_FLAGS = -mthumb -mcpu=cortex-m3
BASE_CFLAGS = $(ARCH_FLAGS) \
$(addprefix -D,$(OPTIONS)) \
$(addprefix -I,$(INCLUDE_DIRS)) \
-Wall \
-ffunction-sections \
-fdata-sections \
-DSTM32F10X_MD \
-DUSE_STDPERIPH_DRIVER \
-D$(TARGET)
ASFLAGS = $(ARCH_FLAGS) \
-x assembler-with-cpp \
$(addprefix -I,$(INCLUDE_DIRS))
# XXX Map/crossref output?
LD_SCRIPT = $(ROOT)/stm32_flash.ld
LDFLAGS = -lm \
$(ARCH_FLAGS) \
-static \
-Wl,-gc-sections,-Map,$(TARGET_MAP) \
-T$(LD_SCRIPT)
###############################################################################
# No user-serviceable parts below
###############################################################################
#
# Things we will build
#
ifeq ($(filter $(TARGET),$(VALID_TARGETS)),)
$(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS))
endif
ifeq ($(DEBUG),GDB)
CFLAGS = $(BASE_CFLAGS) \
-ggdb \
-O0
else
CFLAGS = $(BASE_CFLAGS) \
-Os
endif
TARGET_HEX = $(BIN_DIR)/baseflight_$(TARGET).hex
TARGET_ELF = $(BIN_DIR)/baseflight_$(TARGET).elf
TARGET_OBJS = $(addsuffix .o,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $($(TARGET)_SRC))))
TARGET_MAP = $(OBJECT_DIR)/baseflight_$(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 $< $@
$(TARGET_ELF): $(TARGET_OBJS)
$(CC) -o $@ $^ $(LDFLAGS)
# Compile
$(OBJECT_DIR)/$(TARGET)/%.o: %.c
@mkdir -p $(dir $@)
@echo %% $(notdir $<)
@$(CC) -c -o $@ $(CFLAGS) $<
# Assemble
$(OBJECT_DIR)/$(TARGET)/%.o: %.s
@mkdir -p $(dir $@)
@echo %% $(notdir $<)
@$(CC) -c -o $@ $(ASFLAGS) $<
$(OBJECT_DIR)/$(TARGET)/%.o): %.S
@mkdir -p $(dir $@)
@echo %% $(notdir $<)
@$(CC) -c -o $@ $(ASFLAGS) $<
clean:
rm -f $(TARGET_HEX) $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP)
flash_$(TARGET): $(TARGET_HEX)
stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon
echo -n 'R' >$(SERIAL_DEVICE)
stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE)
flash: flash_$(TARGET)
unbrick_$(TARGET): $(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)
unbrick: unbrick_$(TARGET)
help:
@echo ""
@echo "Makefile for the baseflight firmware"
@echo ""
@echo "Usage:"
@echo " make [TARGET=<target>] [OPTIONS=\"<options>\"]"
@echo ""
@echo "Valid TARGET values are: $(VALID_TARGETS)"
@echo ""
###############################################################################
# "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
###############################################################################
#
# Makefile for building the baseflight firmware.
#
# Invoke this with 'make help' to see the list of supported targets.
#
###############################################################################
# Things that the user might override on the commandline
#
# The target to build, must be one of NAZE, FY90Q OR OLIMEXINO
TARGET ?= NAZE
# Compile-time options
OPTIONS ?=
# Debugger optons, must be empty or GDB
DEBUG ?=
# Serial port/Device for flashing
SERIAL_DEVICE ?= /dev/ttyUSB0
###############################################################################
# Things that need to be maintained as the source changes
#
VALID_TARGETS = NAZE FY90Q OLIMEXINO
# Working directories
ROOT = $(dir $(lastword $(MAKEFILE_LIST)))
SRC_DIR = $(ROOT)/src
CMSIS_DIR = $(ROOT)/lib/CMSIS
STDPERIPH_DIR = $(ROOT)/lib/STM32F10x_StdPeriph_Driver
OBJECT_DIR = $(ROOT)/obj
BIN_DIR = $(ROOT)/obj
# Source files common to all targets
COMMON_SRC = startup_stm32f10x_md_gcc.S \
buzzer.c \
cli.c \
config.c \
gps.c \
imu.c \
main.c \
mixer.c \
mw.c \
sensors.c \
serial.c \
sbus.c \
spektrum.c \
telemetry.c \
drv_gpio.c \
drv_i2c.c \
drv_i2c_soft.c \
drv_system.c \
drv_serial.c \
drv_uart.c \
printf.c \
utils.c \
$(CMSIS_SRC) \
$(STDPERIPH_SRC)
# Source files for the NAZE target
NAZE_SRC = drv_spi.c \
drv_adc.c \
drv_adxl345.c \
drv_bma280.c \
drv_bmp085.c \
drv_ms5611.c \
drv_hcsr04.c \
drv_hmc5883l.c \
drv_ledring.c \
drv_mma845x.c \
drv_mpu3050.c \
drv_mpu6050.c \
drv_l3g4200d.c \
drv_pwm.c \
drv_timer.c \
$(COMMON_SRC)
# Source files for the FY90Q target
FY90Q_SRC = drv_adc_fy90q.c \
drv_pwm_fy90q.c \
$(COMMON_SRC)
# Source files for the OLIMEXINO target
OLIMEXINO_SRC = drv_spi.c \
drv_adc.c \
drv_adxl345.c \
drv_mpu3050.c \
drv_mpu6050.c \
drv_l3g4200d.c \
drv_pwm.c \
drv_timer.c \
drv_softserial.c \
$(COMMON_SRC)
# Search path for baseflight sources
VPATH := $(SRC_DIR):$(SRC_DIR)/baseflight_startups
# 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))
# 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
OBJCOPY = arm-none-eabi-objcopy
#
# Tool options.
#
INCLUDE_DIRS = $(SRC_DIR) \
$(STDPERIPH_DIR)/inc \
$(CMSIS_DIR)/CM3/CoreSupport \
$(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x \
ARCH_FLAGS = -mthumb -mcpu=cortex-m3
BASE_CFLAGS = $(ARCH_FLAGS) \
$(addprefix -D,$(OPTIONS)) \
$(addprefix -I,$(INCLUDE_DIRS)) \
-Wall \
-ffunction-sections \
-fdata-sections \
-DSTM32F10X_MD \
-DUSE_STDPERIPH_DRIVER \
-D$(TARGET)
ASFLAGS = $(ARCH_FLAGS) \
-x assembler-with-cpp \
$(addprefix -I,$(INCLUDE_DIRS))
# XXX Map/crossref output?
LD_SCRIPT = $(ROOT)/stm32_flash.ld
LDFLAGS = -lm \
$(ARCH_FLAGS) \
-static \
-Wl,-gc-sections,-Map,$(TARGET_MAP) \
-T$(LD_SCRIPT)
###############################################################################
# No user-serviceable parts below
###############################################################################
#
# Things we will build
#
ifeq ($(filter $(TARGET),$(VALID_TARGETS)),)
$(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS))
endif
ifeq ($(DEBUG),GDB)
CFLAGS = $(BASE_CFLAGS) \
-ggdb \
-O0
else
CFLAGS = $(BASE_CFLAGS) \
-Os
endif
TARGET_HEX = $(BIN_DIR)/baseflight_$(TARGET).hex
TARGET_ELF = $(BIN_DIR)/baseflight_$(TARGET).elf
TARGET_OBJS = $(addsuffix .o,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $($(TARGET)_SRC))))
TARGET_MAP = $(OBJECT_DIR)/baseflight_$(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 $< $@
$(TARGET_ELF): $(TARGET_OBJS)
$(CC) -o $@ $^ $(LDFLAGS)
# Compile
$(OBJECT_DIR)/$(TARGET)/%.o: %.c
@mkdir -p $(dir $@)
@echo %% $(notdir $<)
@$(CC) -c -o $@ $(CFLAGS) $<
# Assemble
$(OBJECT_DIR)/$(TARGET)/%.o: %.s
@mkdir -p $(dir $@)
@echo %% $(notdir $<)
@$(CC) -c -o $@ $(ASFLAGS) $<
$(OBJECT_DIR)/$(TARGET)/%.o): %.S
@mkdir -p $(dir $@)
@echo %% $(notdir $<)
@$(CC) -c -o $@ $(ASFLAGS) $<
clean:
rm -f $(TARGET_HEX) $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP)
flash_$(TARGET): $(TARGET_HEX)
stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon
echo -n 'R' >$(SERIAL_DEVICE)
stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE)
flash: flash_$(TARGET)
unbrick_$(TARGET): $(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)
unbrick: unbrick_$(TARGET)
help:
@echo ""
@echo "Makefile for the baseflight firmware"
@echo ""
@echo "Usage:"
@echo " make [TARGET=<target>] [OPTIONS=\"<options>\"]"
@echo ""
@echo "Valid TARGET values are: $(VALID_TARGETS)"
@echo ""

60
baseflight.uvproj
View file

@ -462,6 +462,16 @@
<FileType>1</FileType>
<FilePath>.\src\telemetry.c</FilePath>
</File>
<File>
<FileName>utils.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\utils.c</FilePath>
</File>
<File>
<FileName>sbus.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\sbus.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -657,6 +667,16 @@
<FileType>1</FileType>
<FilePath>.\src\drv_softserial.c</FilePath>
</File>
<File>
<FileName>drv_bma280.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_bma280.c</FilePath>
</File>
<File>
<FileName>drv_serial.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_serial.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1293,6 +1313,16 @@
<FileType>1</FileType>
<FilePath>.\src\telemetry.c</FilePath>
</File>
<File>
<FileName>utils.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\utils.c</FilePath>
</File>
<File>
<FileName>sbus.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\sbus.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1488,6 +1518,16 @@
<FileType>1</FileType>
<FilePath>.\src\drv_softserial.c</FilePath>
</File>
<File>
<FileName>drv_bma280.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_bma280.c</FilePath>
</File>
<File>
<FileName>drv_serial.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_serial.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -2068,6 +2108,16 @@
<FileType>1</FileType>
<FilePath>.\src\telemetry.c</FilePath>
</File>
<File>
<FileName>utils.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\utils.c</FilePath>
</File>
<File>
<FileName>sbus.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\sbus.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -2503,6 +2553,16 @@
<FileType>1</FileType>
<FilePath>.\src\drv_softserial.c</FilePath>
</File>
<File>
<FileName>drv_bma280.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_bma280.c</FilePath>
</File>
<File>
<FileName>drv_serial.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_serial.c</FilePath>
</File>
</Files>
</Group>
<Group>

6263
obj/baseflight.hex
View file

File diff suppressed because it is too large Load diff

76
src/board.h
View file

@ -28,11 +28,11 @@
#endif /* M_PI */
#define RADX10 (M_PI / 1800.0f) // 0.001745329252f
#define RAD (M_PI / 180.0f)
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
#define abs(x) ((x) > 0 ? (x) : -(x))
#define constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
// Chip Unique ID on F103
#define U_ID_0 (*(uint32_t*)0x1FFFF7E8)
@ -40,11 +40,12 @@
#define U_ID_2 (*(uint32_t*)0x1FFFF7F0)
typedef enum {
SENSOR_ACC = 1 << 0,
SENSOR_BARO = 1 << 1,
SENSOR_MAG = 1 << 2,
SENSOR_SONAR = 1 << 3,
SENSOR_GPS = 1 << 4,
SENSOR_GYRO = 1 << 0, // always present
SENSOR_ACC = 1 << 1,
SENSOR_BARO = 1 << 2,
SENSOR_MAG = 1 << 3,
SENSOR_SONAR = 1 << 4,
SENSOR_GPS = 1 << 5,
} AvailableSensors;
// Type of accelerometer used/detected
@ -53,13 +54,15 @@ typedef enum AccelSensors {
ACC_ADXL345 = 1,
ACC_MPU6050 = 2,
ACC_MMA8452 = 3,
ACC_BMA280 = 4,
ACC_NONE = 5
} AccelSensors;
typedef enum {
FEATURE_PPM = 1 << 0,
FEATURE_VBAT = 1 << 1,
FEATURE_INFLIGHT_ACC_CAL = 1 << 2,
FEATURE_SPEKTRUM = 1 << 3,
FEATURE_SERIALRX = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4,
FEATURE_SERVO_TILT = 1 << 5,
FEATURE_GYRO_SMOOTHING = 1 << 6,
@ -73,16 +76,57 @@ typedef enum {
FEATURE_3D = 1 << 14,
} AvailableFeatures;
typedef enum {
SERIALRX_SPEKTRUM1024 = 0,
SERIALRX_SPEKTRUM2048 = 1,
SERIALRX_SBUS = 2,
} SerialRXType;
typedef enum {
GPS_NMEA = 0,
GPS_UBLOX,
GPS_MTK,
} GPSHardware;
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype
typedef enum {
X = 0,
Y,
Z
} sensor_axis_e;
typedef enum {
ALIGN_DEFAULT = 0, // driver-provided alignment
CW0_DEG = 1,
CW90_DEG = 2,
CW180_DEG = 3,
CW270_DEG = 4,
CW0_DEG_FLIP = 5,
CW90_DEG_FLIP = 6,
CW180_DEG_FLIP = 7,
CW270_DEG_FLIP = 8
} sensor_align_e;
enum {
GYRO_UPDATED = 1 << 0,
ACC_UPDATED = 1 << 1,
MAG_UPDATED = 1 << 2,
TEMP_UPDATED = 1 << 3
};
typedef struct sensor_data_t
{
int16_t gyro[3];
int16_t acc[3];
int16_t mag[3];
float temperature;
int updated;
} sensor_data_t;
typedef void (* sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype
typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (* baroOpFuncPtr)(void); // baro start operation
typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
typedef void (* uartReceiveCallbackPtr)(uint16_t data); // used by uart2 driver to return frames to app
typedef void (* serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app
typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data
typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype
@ -90,7 +134,6 @@ typedef struct sensor_t
{
sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr align; // sensor align
sensorReadFuncPtr temperature; // read temperature if available
float scale; // scalefactor (currently used for gyro only, todo for accel)
} sensor_t;
@ -99,10 +142,10 @@ typedef struct baro_t
{
uint16_t ut_delay;
uint16_t up_delay;
sensorInitFuncPtr start_ut;
sensorInitFuncPtr get_ut;
sensorInitFuncPtr start_up;
sensorInitFuncPtr get_up;
baroOpFuncPtr start_ut;
baroOpFuncPtr get_ut;
baroOpFuncPtr start_up;
baroOpFuncPtr get_up;
baroCalculateFuncPtr calculate;
} baro_t;
@ -204,6 +247,8 @@ typedef struct baro_t
#undef SOFT_I2C // enable to test software i2c
#include "utils.h"
#ifdef FY90Q
// FY90Q
#include "drv_adc.h"
@ -223,6 +268,7 @@ typedef struct baro_t
#include "drv_l3g4200d.h"
#include "drv_pwm.h"
#include "drv_timer.h"
#include "drv_serial.h"
#include "drv_uart.h"
#include "drv_softserial.h"
#else
@ -230,6 +276,7 @@ typedef struct baro_t
// AfroFlight32
#include "drv_adc.h"
#include "drv_adxl345.h"
#include "drv_bma280.h"
#include "drv_bmp085.h"
#include "drv_ms5611.h"
#include "drv_hmc5883l.h"
@ -242,6 +289,7 @@ typedef struct baro_t
#include "drv_l3g4200d.h"
#include "drv_pwm.h"
#include "drv_timer.h"
#include "drv_serial.h"
#include "drv_uart.h"
#include "drv_softserial.h"
#include "drv_hcsr04.h"

24
src/buzzer.c
View file

@ -14,7 +14,7 @@ void buzzer(uint8_t warn_vbat)
static uint8_t warn_runtime = 0;
//===================== BeeperOn via rcOptions =====================
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
beeperOnBox = 1;
} else {
beeperOnBox = 0;
@ -24,7 +24,7 @@ void buzzer(uint8_t warn_vbat)
if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) {
warn_failsafe = 1; //set failsafe warning level to 1 while landing
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay))
warn_failsafe = 2; //start "find me" signal after landing
warn_failsafe = 2; //start "find me" signal after landing
}
if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED)
warn_failsafe = 2; // tx turned off while motors are off: start "find me" signal
@ -62,7 +62,7 @@ void buzzer(uint8_t warn_vbat)
beep_code('S','S','M','N'); // Runtime warning
else if (toggleBeep > 0)
beep(50); // fast confirmation beep
else {
else {
buzzerIsOn = 0;
BEEP_OFF;
}
@ -79,20 +79,20 @@ void beep_code(char first, char second, char third, char pause)
patternChar[2] = third;
patternChar[3] = pause;
switch(patternChar[icnt]) {
case 'M':
Duration = 100;
case 'M':
Duration = 100;
break;
case 'L':
Duration = 200;
case 'L':
Duration = 200;
break;
case 'D':
Duration = 2000;
case 'D':
Duration = 2000;
break;
case 'N':
Duration = 0;
case 'N':
Duration = 0;
break;
default:
Duration = 50;
Duration = 50;
break;
}

31
src/cli.c
View file

@ -42,7 +42,7 @@ static const char * const mixerNames[] = {
// sync this with AvailableFeatures enum from board.h
static const char * const featureNames[] = {
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SERIALRX", "MOTOR_STOP",
"SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS",
"FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D",
NULL
@ -54,7 +54,7 @@ static const char * const sensorNames[] = {
};
static const char * const accNames[] = {
"", "ADXL345", "MPU6050", "MMA845x", NULL
"", "ADXL345", "MPU6050", "MMA845x", "BMA280", "None", NULL
};
typedef struct {
@ -116,21 +116,16 @@ const clivalue_t valueTable[] = {
{ "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 },
{ "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 },
{ "gps_baudrate", VAR_UINT32, &mcfg.gps_baudrate, 1200, 115200 },
{ "spektrum_hires", VAR_UINT8, &mcfg.spektrum_hires, 0, 1 },
{ "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 2 },
{ "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 },
{ "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 },
{ "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 },
{ "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 },
{ "align_gyro_x", VAR_INT8, &mcfg.align[ALIGN_GYRO][0], -3, 3 },
{ "align_gyro_y", VAR_INT8, &mcfg.align[ALIGN_GYRO][1], -3, 3 },
{ "align_gyro_z", VAR_INT8, &mcfg.align[ALIGN_GYRO][2], -3, 3 },
{ "align_acc_x", VAR_INT8, &mcfg.align[ALIGN_ACCEL][0], -3, 3 },
{ "align_acc_y", VAR_INT8, &mcfg.align[ALIGN_ACCEL][1], -3, 3 },
{ "align_acc_z", VAR_INT8, &mcfg.align[ALIGN_ACCEL][2], -3, 3 },
{ "align_mag_x", VAR_INT8, &mcfg.align[ALIGN_MAG][0], -3, 3 },
{ "align_mag_y", VAR_INT8, &mcfg.align[ALIGN_MAG][1], -3, 3 },
{ "align_mag_z", VAR_INT8, &mcfg.align[ALIGN_MAG][2], -3, 3 },
{ "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 3 },
{ "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 },
{ "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 },
{ "align_mag", VAR_UINT8, &mcfg.mag_align, 0, 8 },
{ "yaw_control_direction", VAR_INT8, &mcfg.yaw_control_direction, -1, 1 },
{ "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 5 },
{ "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 },
{ "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 },
{ "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },
@ -152,7 +147,9 @@ const clivalue_t valueTable[] = {
{ "failsafe_off_delay", VAR_UINT8, &cfg.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16, &cfg.failsafe_throttle, 1000, 2000 },
{ "failsafe_detect_threshold", VAR_UINT16, &cfg.failsafe_detect_threshold, 100, 2000 },
{ "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 4 },
{ "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
{ "tri_unarmed_servo", VAR_INT8, &cfg.tri_unarmed_servo, 0, 1 },
{ "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 },
{ "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 },
{ "tri_yaw_max", VAR_UINT16, &cfg.tri_yaw_max, 0, 2000 },
@ -573,7 +570,7 @@ static void cliDump(char *cmdline)
if (yaw < 0)
printf(" ");
printf("%s\r\n", ftoa(yaw, buf));
}
}
printf("cmix %d 0 0 0 0\r\n", i + 1);
}
@ -676,7 +673,7 @@ static void cliHelp(char *cmdline)
{
uint32_t i = 0;
cliPrint("Available commands:\r\n");
cliPrint("Available commands:\r\n");
for (i = 0; i < CMD_COUNT; i++)
printf("%s\t%s\r\n", cmdTable[i].name, cmdTable[i].param);
}
@ -934,8 +931,8 @@ void cliProcess(void)
cliPrompt();
}
while (isUartAvailable(core.mainport)) {
uint8_t c = uartRead(core.mainport);
while (serialTotalBytesWaiting(core.mainport)) {
uint8_t c = serialRead(core.mainport);
if (c == '\t' || c == '?') {
// do tab completion
const clicmd_t *cmd, *pstart = NULL, *pend = NULL;

14
src/config.c
View file

@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234";
static const uint8_t EEPROM_CONF_VERSION = 49;
static const uint8_t EEPROM_CONF_VERSION = 51;
static uint32_t enabledSensors = 0;
static void resetConf(void);
@ -133,7 +133,7 @@ retry:
}
}
FLASH_Lock();
// Flash write failed - just die now
if (tries == 3 || !validEEPROM()) {
failureMode(10);
@ -159,7 +159,6 @@ void checkFirstTime(bool reset)
static void resetConf(void)
{
int i;
const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { -2, -3, 1 } };
// Clear all configuration
memset(&mcfg, 0, sizeof(master_t));
@ -178,15 +177,18 @@ static void resetConf(void)
mcfg.accZero[0] = 0;
mcfg.accZero[1] = 0;
mcfg.accZero[2] = 0;
memcpy(&mcfg.align, default_align, sizeof(mcfg.align));
mcfg.gyro_align = ALIGN_DEFAULT;
mcfg.acc_align = ALIGN_DEFAULT;
mcfg.mag_align = ALIGN_DEFAULT;
mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect
mcfg.yaw_control_direction = 1;
mcfg.moron_threshold = 32;
mcfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
mcfg.vbatscale = 110;
mcfg.vbatmaxcellvoltage = 43;
mcfg.vbatmincellvoltage = 33;
mcfg.power_adc_channel = 0;
mcfg.spektrum_hires = 0;
mcfg.serialrx_type = 0;
mcfg.midrc = 1500;
mcfg.mincheck = 1100;
mcfg.maxcheck = 1900;
@ -207,6 +209,7 @@ static void resetConf(void)
// serial (USART1) baudrate
mcfg.serial_baudrate = 115200;
mcfg.looptime = 3500;
mcfg.rssi_aux_channel = 0;
cfg.pidController = 0;
cfg.P8[ROLL] = 40;
@ -274,6 +277,7 @@ static void resetConf(void)
// servos
cfg.yaw_direction = 1;
cfg.tri_unarmed_servo = 1;
cfg.tri_yaw_middle = 1500;
cfg.tri_yaw_min = 1020;
cfg.tri_yaw_max = 2000;

28
src/drv_adxl345.c
View file

@ -31,11 +31,11 @@
extern uint16_t acc_1G;
static void adxl345Init(void);
static void adxl345Init(sensor_align_e align);
static void adxl345Read(int16_t *accelData);
static void adxl345Align(int16_t *accelData);
static bool useFifo = false;
static sensor_align_e accAlign = CW0_DEG;
bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc)
{
@ -55,11 +55,10 @@ bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc)
acc->init = adxl345Init;
acc->read = adxl345Read;
acc->align = adxl345Align;
return true;
}
static void adxl345Init(void)
static void adxl345Init(sensor_align_e align)
{
if (useFifo) {
uint8_t fifoDepth = 16;
@ -73,6 +72,9 @@ static void adxl345Init(void)
i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100);
}
acc_1G = 265; // 3.3V operation
if (align > 0)
accAlign = align;
}
uint8_t acc_samples = 0;
@ -80,6 +82,7 @@ uint8_t acc_samples = 0;
static void adxl345Read(int16_t *accelData)
{
uint8_t buf[8];
int16_t data[3];
if (useFifo) {
int32_t x = 0;
@ -96,19 +99,16 @@ static void adxl345Read(int16_t *accelData)
z += (int16_t)(buf[4] + (buf[5] << 8));
samples_remaining = buf[7] & 0x7F;
} while ((i < 32) && (samples_remaining > 0));
accelData[0] = x / i;
accelData[1] = y / i;
accelData[2] = z / i;
data[0] = x / i;
data[1] = y / i;
data[2] = z / i;
acc_samples = i;
} else {
i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
accelData[0] = buf[0] + (buf[1] << 8);
accelData[1] = buf[2] + (buf[3] << 8);
accelData[2] = buf[4] + (buf[5] << 8);
data[0] = buf[0] + (buf[1] << 8);
data[1] = buf[2] + (buf[3] << 8);
data[2] = buf[4] + (buf[5] << 8);
}
}
static void adxl345Align(int16_t *accData)
{
// official direction is RPY, nothing to change here.
alignSensors(data, accelData, accAlign);
}

54
src/drv_bma280.c Normal file
View file

@ -0,0 +1,54 @@
#include "board.h"
// BMA280, default I2C address mode 0x18
#define BMA280_ADDRESS 0x18
#define BMA280_ACC_X_LSB 0x02
#define BMA280_PMU_BW 0x10
#define BMA280_PMU_RANGE 0x0F
extern uint16_t acc_1G;
static void bma280Init(sensor_align_e align);
static void bma280Read(int16_t *accelData);
static sensor_align_e accAlign = CW0_DEG;
bool bma280Detect(sensor_t *acc)
{
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(BMA280_ADDRESS, 0x00, 1, &sig);
if (!ack || sig != 0xFB)
return false;
acc->init = bma280Init;
acc->read = bma280Read;
return true;
}
static void bma280Init(sensor_align_e align)
{
i2cWrite(BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
i2cWrite(BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
acc_1G = 512 * 8;
if (align > 0)
accAlign = align;
}
static void bma280Read(int16_t *accelData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf);
// Data format is lsb<5:0><crap><new_data_bit> | msb<13:6>
data[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
data[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
data[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
alignSensors(data, accelData, accAlign);
}

3
src/drv_bma280.h Normal file
View file

@ -0,0 +1,3 @@
#pragma once
bool bma280Detect(sensor_t *acc);

4
src/drv_bmp085.c
View file

@ -217,7 +217,7 @@ static void bmp085_start_ut(void)
static void bmp085_get_ut(void)
{
uint8_t data[2];
uint8_t data[2];
// wait in case of cockup
if (!convDone)
@ -243,7 +243,7 @@ static void bmp085_start_up(void)
static void bmp085_get_up(void)
{
uint8_t data[3];
// wait in case of cockup
if (!convDone)
convOverrun++;

4
src/drv_gpio.h
View file

@ -15,7 +15,7 @@ typedef enum
typedef enum
{
Speed_10MHz = 1,
Speed_2MHz,
Speed_2MHz,
Speed_50MHz
} GPIO_Speed;
@ -46,7 +46,7 @@ typedef struct
GPIO_Mode mode;
GPIO_Speed speed;
} gpio_config_t;
#define digitalHi(p, i) { p->BSRR = i; }
#define digitalLo(p, i) { p->BRR = i; }
#define digitalToggle(p, i) { p->ODR ^= i; }

12
src/drv_hcsr04.c
View file

@ -59,7 +59,7 @@ void hcsr04_init(sonar_config_t config)
EXTI_InitTypeDef EXTIInit;
// enable AFIO for EXTI support - already done is drv_system.c
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE);
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE);
switch(config) {
case sonar_pwm56:
@ -69,7 +69,7 @@ void hcsr04_init(sonar_config_t config)
exti_pin_source = GPIO_PinSource9;
exti_irqn = EXTI9_5_IRQn;
break;
case sonar_rc78:
case sonar_rc78:
trigger_pin = Pin_0; // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
echo_pin = Pin_1; // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
exti_line = EXTI_Line1;
@ -78,7 +78,7 @@ void hcsr04_init(sonar_config_t config)
break;
}
// tp - trigger pin
// tp - trigger pin
gpio.pin = trigger_pin;
gpio.mode = Mode_Out_PP;
gpio.speed = Speed_2MHz;
@ -93,12 +93,12 @@ void hcsr04_init(sonar_config_t config)
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, exti_pin_source);
EXTI_ClearITPendingBit(exti_line);
EXTIInit.EXTI_Line = exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_EnableIRQ(exti_irqn);

134
src/drv_hmc5883l.c
View file

@ -2,6 +2,62 @@
// HMC5883L, default address 0x1E
// PB12 connected to MAG_DRDY on rev4 hardware
// PC14 connected to MAG_DRDY on rev5 hardware
/* CTRL_REGA: Control Register A
* Read Write
* Default value: 0x10
* 7:5 0 These bits must be cleared for correct operation.
* 4:2 DO2-DO0: Data Output Rate Bits
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
* ------------------------------------------------------
* 0 | 0 | 0 | 0.75
* 0 | 0 | 1 | 1.5
* 0 | 1 | 0 | 3
* 0 | 1 | 1 | 7.5
* 1 | 0 | 0 | 15 (default)
* 1 | 0 | 1 | 30
* 1 | 1 | 0 | 75
* 1 | 1 | 1 | Not Used
* 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Normal
* 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias
* 1 | 1 | Not Used
*
* CTRL_REGB: Control RegisterB
* Read Write
* Default value: 0x20
* 7:5 GN2-GN0: Gain Configuration Bits.
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
* | | | Range[Ga] | [LSB/mGa] |
* ------------------------------------------------------
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800?0x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800?0x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800?0x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800?0x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800?0x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800?0x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800?0x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800?0x07FF (-2048:2047)
* |Not recommended|
*
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
*
* _MODE_REG: Mode Register
* Read Write
* Default value: 0x02
* 7:2 0 These bits must be cleared for correct operation.
* 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode
*/
#define MAG_ADDRESS 0x1E
#define MAG_DATA_REGISTER 0x03
@ -9,17 +65,18 @@
#define HMC58X3_R_CONFA 0
#define HMC58X3_R_CONFB 1
#define HMC58X3_R_MODE 2
#define HMC58X3_X_SELF_TEST_GAUSS (+1.16) // X axis level when bias current is applied.
#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16) // Y axis level when bias current is applied.
#define HMC58X3_Z_SELF_TEST_GAUSS (+1.08) // Y axis level when bias current is applied.
#define SELF_TEST_LOW_LIMIT (243.0 / 390.0) // Low limit when gain is 5.
#define SELF_TEST_HIGH_LIMIT (575.0 / 390.0) // High limit when gain is 5.
#define HMC58X3_X_SELF_TEST_GAUSS (+1.16f) // X axis level when bias current is applied.
#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16f) // Y axis level when bias current is applied.
#define HMC58X3_Z_SELF_TEST_GAUSS (+1.08f) // Z axis level when bias current is applied.
#define SELF_TEST_LOW_LIMIT (243.0f / 390.0f) // Low limit when gain is 5.
#define SELF_TEST_HIGH_LIMIT (575.0f / 390.0f) // High limit when gain is 5.
#define HMC_POS_BIAS 1
#define HMC_NEG_BIAS 2
static int8_t sensor_align[3];
static sensor_align_e magAlign = CW180_DEG;
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
bool hmc5883lDetect(int8_t *align)
bool hmc5883lDetect(sensor_align_e align)
{
bool ack = false;
uint8_t sig = 0;
@ -28,15 +85,15 @@ bool hmc5883lDetect(int8_t *align)
if (!ack || sig != 'H')
return false;
memcpy(sensor_align, align, 3);
if (align > 0)
magAlign = align;
return true;
}
void hmc5883lInit(float *calibrationGain)
void hmc5883lInit(void)
{
gpio_config_t gpio;
float magGain[3];
int16_t magADC[3];
int i;
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
@ -56,9 +113,9 @@ void hmc5883lInit(float *calibrationGain)
delay(50);
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
// Note that the very first measurement after a gain change maintains the same gain as the previous setting.
// Note that the very first measurement after a gain change maintains the same gain as the previous setting.
// The new gain setting is effective from the second measurement and on.
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 2 << 5); // Set the Gain
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
delay(100);
hmc5883lRead(magADC);
@ -68,12 +125,12 @@ void hmc5883lInit(float *calibrationGain)
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[0] += magADC[0];
xyz_total[1] += magADC[1];
xyz_total[2] += magADC[2];
xyz_total[X] += magADC[X];
xyz_total[Y] += magADC[Y];
xyz_total[Z] += magADC[Z];
// Detect saturation.
if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
@ -88,21 +145,21 @@ void hmc5883lInit(float *calibrationGain)
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged.
xyz_total[0] -= magADC[0];
xyz_total[1] -= magADC[1];
xyz_total[2] -= magADC[2];
xyz_total[X] -= magADC[X];
xyz_total[Y] -= magADC[Y];
xyz_total[Z] -= magADC[Z];
// Detect saturation.
if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
LED1_TOGGLE;
}
magGain[0] = fabs(820.0 * HMC58X3_X_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[0]);
magGain[1] = fabs(820.0 * HMC58X3_Y_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[1]);
magGain[2] = fabs(820.0 * HMC58X3_Z_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[2]);
magGain[X] = fabs(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
magGain[Y] = fabs(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
magGain[Z] = fabs(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
// leave test mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
@ -111,16 +168,9 @@ void hmc5883lInit(float *calibrationGain)
delay(100);
if (!bret) { // Something went wrong so get a best guess
magGain[0] = 1.0;
magGain[1] = 1.0;
magGain[2] = 1.0;
}
// if parameter was passed, give calibration values back
if (calibrationGain) {
calibrationGain[0] = magGain[0];
calibrationGain[1] = magGain[1];
calibrationGain[2] = magGain[2];
magGain[X] = 1.0f;
magGain[Y] = 1.0f;
magGain[Z] = 1.0f;
}
}
@ -128,18 +178,12 @@ void hmc5883lRead(int16_t *magData)
{
uint8_t buf[6];
int16_t mag[3];
int i;
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
mag[0] = buf[0] << 8 | buf[1];
mag[1] = buf[2] << 8 | buf[3];
mag[2] = buf[4] << 8 | buf[5];
for (i = 0; i < 3; i++) {
int8_t axis = sensor_align[i];
if (axis > 0)
magData[axis - 1] = mag[i];
else
magData[-axis - 1] = -mag[i];
}
// During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
// After calibration is done, magGain is set to calculated gain values.
mag[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
mag[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
mag[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
alignSensors(mag, magData, magAlign);
}

4
src/drv_hmc5883l.h
View file

@ -1,5 +1,5 @@
#pragma once
bool hmc5883lDetect(int8_t *align);
void hmc5883lInit(float *calibrationGain);
bool hmc5883lDetect(sensor_align_e align);
void hmc5883lInit(void);
void hmc5883lRead(int16_t *magData);

32
src/drv_l3g4200d.c
View file

@ -3,6 +3,7 @@
// L3G4200D, Standard address 0x68
#define L3G4200D_ADDRESS 0x68
#define L3G4200D_ID 0xD3
#define L3G4200D_AUTOINCR 0x80
// Registers
#define L3G4200D_WHO_AM_I 0x0F
@ -13,7 +14,7 @@
#define L3G4200D_CTRL_REG5 0x24
#define L3G4200D_REFERENCE 0x25
#define L3G4200D_STATUS_REG 0x27
#define L3G4200D_GYRO_OUT 0xA8
#define L3G4200D_GYRO_OUT 0x28
// Bits
#define L3G4200D_POWER_ON 0x0F
@ -24,10 +25,10 @@
#define L3G4200D_DLPF_93HZ 0xC0
static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
static sensor_align_e gyroAlign = CW0_DEG;
static void l3g4200dInit(void);
static void l3g4200dInit(sensor_align_e align);
static void l3g4200dRead(int16_t *gyroData);
static void l3g4200dAlign(int16_t *gyroData);
bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
{
@ -41,7 +42,7 @@ bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
gyro->init = l3g4200dInit;
gyro->read = l3g4200dRead;
gyro->align = l3g4200dAlign;
// 14.2857dps/lsb scalefactor
gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
@ -65,7 +66,7 @@ bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
return true;
}
static void l3g4200dInit(void)
static void l3g4200dInit(sensor_align_e align)
{
bool ack;
@ -77,21 +78,20 @@ static void l3g4200dInit(void)
delay(5);
i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
}
static void l3g4200dAlign(int16_t *gyroData)
{
gyroData[0] = -gyroData[0];
gyroData[1] = gyroData[1];
gyroData[2] = -gyroData[2];
if (align > 0)
gyroAlign = align;
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void l3g4200dRead(int16_t *gyroData)
{
uint8_t buf[6];
i2cRead(L3G4200D_ADDRESS, L3G4200D_GYRO_OUT, 6, buf);
gyroData[1] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[0] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
int16_t data[3];
i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
data[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}

4
src/drv_ledring.c
View file

@ -8,7 +8,7 @@ bool ledringDetect(void)
{
bool ack = false;
uint8_t sig = 'e';
ack = i2cWrite(LED_RING_ADDRESS, 0xFF, sig);
if (!ack)
return false;
@ -32,7 +32,7 @@ void ledringState(void)
i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b);
state = 2;
} else if (state == 2) {
b[0] = 'd'; // all unicolor GREEN
b[0] = 'd'; // all unicolor GREEN
b[1] = 1;
if (f.ARMED)
b[2] = 1;

24
src/drv_mma845x.c
View file

@ -49,10 +49,10 @@
extern uint16_t acc_1G;
static uint8_t device_id;
static sensor_align_e accAlign = CW90_DEG;
static void mma8452Init(void);
static void mma8452Init(sensor_align_e align);
static void mma8452Read(int16_t *accelData);
static void mma8452Align(int16_t *accelData);
bool mma8452Detect(sensor_t *acc)
{
@ -69,12 +69,11 @@ bool mma8452Detect(sensor_t *acc)
acc->init = mma8452Init;
acc->read = mma8452Read;
acc->align = mma8452Align;
device_id = sig;
return true;
}
static void mma8452Init(void)
static void mma8452Init(sensor_align_e align)
{
gpio_config_t gpio;
@ -95,21 +94,20 @@ static void mma8452Init(void)
i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G.
acc_1G = 256;
if (align > 0)
accAlign = align;
}
static void mma8452Read(int16_t *accelData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf);
accelData[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
accelData[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
accelData[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
}
data[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
data[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
data[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
static void mma8452Align(int16_t *accelData)
{
accelData[0] = -accelData[0];
accelData[1] = -accelData[1];
accelData[2] = accelData[2];
alignSensors(data, accelData, accAlign);
}

26
src/drv_mpu3050.c
View file

@ -25,10 +25,10 @@
#define MPU3050_CLK_SEL_PLL_GX 0x01
static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
static sensor_align_e gyroAlign = CW0_DEG;
static void mpu3050Init(void);
static void mpu3050Init(sensor_align_e align);
static void mpu3050Read(int16_t *gyroData);
static void mpu3050Align(int16_t *gyroData);
static void mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
@ -43,7 +43,6 @@ bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
gyro->init = mpu3050Init;
gyro->read = mpu3050Read;
gyro->align = mpu3050Align;
gyro->temperature = mpu3050ReadTemp;
// 16.4 dps/lsb scalefactor
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
@ -74,7 +73,7 @@ bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
return true;
}
static void mpu3050Init(void)
static void mpu3050Init(sensor_align_e align)
{
bool ack;
@ -88,24 +87,23 @@ static void mpu3050Init(void)
i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
static void mpu3050Align(int16_t *gyroData)
{
// official direction is RPY
gyroData[0] = gyroData[0];
gyroData[1] = gyroData[1];
gyroData[2] = -gyroData[2];
if (align > 0)
gyroAlign = align;
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void mpu3050Read(int16_t *gyroData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}
static void mpu3050ReadTemp(int16_t *tempData)

599
src/drv_mpu6050.c
View file

@ -4,26 +4,9 @@
// MPU_INT on PB13 on rev4 hardware
#define MPU6050_ADDRESS 0x68
// Experimental DMP support
// #define MPU6050_DMP
#define DMP_MEM_START_ADDR 0x6E
#define DMP_MEM_R_W 0x6F
#define INV_MAX_NUM_ACCEL_SAMPLES (8)
#define DMP_REF_QUATERNION (0)
#define DMP_REF_GYROS (DMP_REF_QUATERNION + 4) // 4
#define DMP_REF_CONTROL (DMP_REF_GYROS + 3) // 7
#define DMP_REF_RAW (DMP_REF_CONTROL + 4) // 11
#define DMP_REF_RAW_EXTERNAL (DMP_REF_RAW + 8) // 19
#define DMP_REF_ACCEL (DMP_REF_RAW_EXTERNAL + 6) // 25
#define DMP_REF_QUANT_ACCEL (DMP_REF_ACCEL + 3) // 28
#define DMP_REF_QUATERNION_6AXIS (DMP_REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES) // 36
#define DMP_REF_EIS (DMP_REF_QUATERNION_6AXIS + 4) // 40
#define DMP_REF_DMP_PACKET (DMP_REF_EIS + 3) // 43
#define DMP_REF_GARBAGE (DMP_REF_DMP_PACKET + 1) // 44
#define DMP_REF_LAST (DMP_REF_GARBAGE + 1) // 45
#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
@ -113,7 +96,7 @@
#define MPU_RA_FIFO_R_W 0x74
#define MPU_RA_WHO_AM_I 0x75
#define MPU6050_SMPLRT_DIV 0 //8000Hz
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
#define MPU6050_LPF_256HZ 0
#define MPU6050_LPF_188HZ 1
@ -124,19 +107,13 @@
#define MPU6050_LPF_5HZ 6
static uint8_t mpuLowPassFilter = MPU6050_LPF_42HZ;
static sensor_align_e gyroAlign = CW0_DEG;
static sensor_align_e accAlign = CW0_DEG;
static void mpu6050AccInit(void);
static void mpu6050AccInit(sensor_align_e align);
static void mpu6050AccRead(int16_t *accData);
static void mpu6050AccAlign(int16_t *accData);
static void mpu6050GyroInit(void);
static void mpu6050GyroInit(sensor_align_e align);
static void mpu6050GyroRead(int16_t *gyroData);
static void mpu6050GyroAlign(int16_t *gyroData);
#ifdef MPU6050_DMP
static void mpu6050DmpInit(void);
float dmpdata[2];
int16_t dmpGyroData[3];
#endif
extern uint16_t acc_1G;
static uint8_t mpuAccelHalf = 0;
@ -186,12 +163,11 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
acc->init = mpu6050AccInit;
acc->read = mpu6050AccRead;
acc->align = mpu6050AccAlign;
gyro->init = mpu6050GyroInit;
gyro->read = mpu6050GyroRead;
gyro->align = mpu6050GyroAlign;
// 16.4 dps/lsb scalefactor
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
// give halfacc (old revision) back to system
if (scale)
@ -223,48 +199,34 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
break;
}
#ifdef MPU6050_DMP
mpu6050DmpInit();
#endif
return true;
}
static void mpu6050AccInit(void)
static void mpu6050AccInit(sensor_align_e align)
{
if (mpuAccelHalf)
acc_1G = 255 * 8;
else
acc_1G = 512 * 8;
if (align > 0)
accAlign = align;
}
static void mpu6050AccRead(int16_t *accData)
{
uint8_t buf[6];
int16_t data[3];
#ifndef MPU6050_DMP
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
#else
accData[0] = accData[1] = accData[2] = 0;
#endif
data[0] = (int16_t)((buf[0] << 8) | buf[1]);
data[1] = (int16_t)((buf[2] << 8) | buf[3]);
data[2] = (int16_t)((buf[4] << 8) | buf[5]);
alignSensors(data, accData, accAlign);
}
static void mpu6050AccAlign(int16_t *accData)
{
int16_t temp[2];
temp[0] = accData[0];
temp[1] = accData[1];
// official direction is RPY
accData[0] = temp[1];
accData[1] = -temp[0];
accData[2] = accData[2];
}
static void mpu6050GyroInit(void)
static void mpu6050GyroInit(sensor_align_e align)
{
gpio_config_t gpio;
@ -277,7 +239,6 @@ static void mpu6050GyroInit(void)
else if (hse_value == 12000000)
gpioInit(GPIOC, &gpio);
#ifndef MPU6050_DMP
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(5);
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
@ -289,522 +250,20 @@ static void mpu6050GyroInit(void)
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
// Accel scale 8g (4096 LSB/g)
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 2 << 3);
#endif
if (align > 0)
gyroAlign = align;
}
static void mpu6050GyroRead(int16_t * gyroData)
static void mpu6050GyroRead(int16_t *gyroData)
{
uint8_t buf[6];
#ifndef MPU6050_DMP
int16_t data[3];
i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
#else
gyroData[0] = dmpGyroData[0] / 4 ;
gyroData[1] = dmpGyroData[1] / 4;
gyroData[2] = dmpGyroData[2] / 4;
#endif
data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}
static void mpu6050GyroAlign(int16_t * gyroData)
{
// official direction is RPY
gyroData[0] = gyroData[0];
gyroData[1] = gyroData[1];
gyroData[2] = -gyroData[2];
}
#ifdef MPU6050_DMP
//This 3D array contains the default DMP memory bank binary that gets loaded during initialization.
//In the Invensense UC3-A3 firmware this is uploaded in 128 byte tranmissions, but the Arduino Wire
//library only supports 32 byte transmissions, including the register address to which you're writing,
//so I broke it up into 16 byte transmission payloads which are sent in the dmp_init() function below.
//
//This was reconstructed from observed I2C traffic generated by the UC3-A3 demo code, and not extracted
//directly from that code. That is true of all transmissions in this sketch, and any documentation has
//been added after the fact by referencing the Invensense code.
const unsigned char dmpMem[8][16][16] = {
{
{0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00},
{0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01},
{0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01},
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00},
{0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00},
{0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82},
{0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC},
{0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4},
{0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10}
},
{
{0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00},
{0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8},
{0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C},
{0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C},
{0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0}
},
{
{0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00},
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
},
{
{0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F},
{0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2},
{0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF},
{0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C},
{0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1},
{0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01},
{0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80},
{0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C},
{0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80},
{0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E},
{0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9},
{0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24},
{0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0},
{0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86},
{0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1},
{0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86}
},
{
{0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA},
{0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C},
{0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8},
{0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3},
{0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84},
{0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5},
{0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3},
{0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1},
{0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5},
{0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D},
{0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9},
{0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D},
{0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9},
{0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A},
{0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8},
{0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87}
},
{
{0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8},
{0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68},
{0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D},
{0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94},
{0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA},
{0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56},
{0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9},
{0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA},
{0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A},
{0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60},
{0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97},
{0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04},
{0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78},
{0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79},
{0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68},
{0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68}
},
{
{0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04},
{0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66},
{0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31},
{0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60},
{0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76},
{0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56},
{0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD},
{0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91},
{0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8},
{0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE},
{0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9},
{0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD},
{0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E},
{0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8},
{0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89},
{0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79}
},
{
{0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8},
{0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA},
{0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB},
{0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3},
{0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3},
{0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3},
{0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3},
{0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC},
{0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF}
}
};
//DMP update transmissions (Bank, Start Address, Update Length, Update Data...)
const uint8_t dmp_updates[29][9] = {
{0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C}, //FCFG_1 inv_set_gyro_calibration
{0x03, 0xAB, 0x03, 0x36, 0x56, 0x76}, //FCFG_3 inv_set_gyro_calibration
{0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2}, //D_0_104 inv_set_gyro_calibration
{0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1}, //D_0_24 inv_set_gyro_calibration
{0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00}, //D_1_152 inv_set_accel_calibration
{0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97}, //FCFG_2 inv_set_accel_calibration
{0x03, 0x89, 0x03, 0x26, 0x46, 0x66}, //FCFG_7 inv_set_accel_calibration
{0x00, 0x6C, 0x02, 0x20, 0x00}, //D_0_108 inv_set_accel_calibration
{0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_00 inv_set_compass_calibration
{0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_01
{0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_02
{0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_10
{0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_11
{0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_12
{0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_20
{0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_21
{0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_22
{0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00}, //D_1_236 inv_apply_endian_accel
{0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97}, //FCFG_2 inv_set_mpu_sensors
{0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D}, //CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
{0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D}, //FCFG_5 inv_set_bias_update
{0x00, 0xA3, 0x01, 0x00}, //D_0_163 inv_set_dead_zone
//SET INT_ENABLE at i=22
{0x07, 0x86, 0x01, 0xFE}, //CFG_6 inv_set_fifo_interupt
{0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38}, //CFG_8 inv_send_quaternion
{0x07, 0x7E, 0x01, 0x30}, //CFG_16 inv_set_footer
{0x07, 0x46, 0x01, 0x9A}, //CFG_GYRO_SOURCE inv_send_gyro
{0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_9 inv_send_gyro -> inv_construct3_fifo
{0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_12 inv_send_accel -> inv_construct3_fifo
{0x02, 0x16, 0x02, 0x00, 0x00}, //D_0_22 inv_set_fifo_rate
};
static long dmp_lastRead = 0;
static uint8_t dmp_processed_packet[8];
static uint8_t dmp_received_packet[50];
static uint8_t dmp_temp = 0;
uint8_t dmp_fifoCountL = 0;
static uint8_t dmp_fifoCountL2 = 0;
static uint8_t dmp_packetCount = 0x00;
static bool dmp_longPacket = false;
static bool dmp_firstPacket = true;
static void mpu6050DmpMemInit(void);
static void mpu6050DmpBankSelect(uint8_t bank);
static bool mpu6050DmpFifoReady(void);
static void mpu6050DmpGetPacket(void);
static void mpu6050DmpProcessQuat(void);
void mpu6050DmpResetFifo(void);
static void mpu6050DmpInit(void)
{
uint8_t temp = 0;
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0xC0); // device reset
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 0x00);
delay(10);
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, 0x70);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x06);
i2cRead(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, 0x00);
/*
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_XG_OFFS_TC);
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_YG_OFFS_TC);
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_ZG_OFFS_TC);
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_USER_CTRL);
*/
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, 0x32); // I2C bypass enabled, latch int enabled, int read clear
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 1, &temp);
delay(5);
mpu6050DmpMemInit();
}
void mpu6050DmpLoop(void)
{
uint8_t temp;
uint8_t buf[2];
if (mpu6050DmpFifoReady()) {
LED1_ON;
mpu6050DmpGetPacket();
i2cRead(MPU6050_ADDRESS, MPU_RA_INT_STATUS, 1, &temp);
if (dmp_firstPacket) {
delay(1);
mpu6050DmpBankSelect(0x00);
mpu6050DmpBankSelect(0x00); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x04\x00\x00\x00"); // data
mpu6050DmpBankSelect(0x01);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x62);
i2cRead(MPU6050_ADDRESS, DMP_MEM_R_W, 2, buf);
dmp_firstPacket = false;
mpu6050DmpFifoReady();
}
if (dmp_fifoCountL == 42) {
mpu6050DmpProcessQuat();
}
LED1_OFF;
}
}
#define dmp_quatTake32(a, b) (((a)[4*(b)+0]<<8) | ((a)[4*(b)+1]<<0))
extern int16_t angle[2];
static void mpu6050DmpProcessQuat(void)
{
float quat0, quat1, quat2, quat3;
int32_t quatl0, quatl1, quatl2, quatl3;
quatl0 = dmp_quatTake32(dmp_received_packet, 0);
quatl1 = dmp_quatTake32(dmp_received_packet, 1);
quatl2 = dmp_quatTake32(dmp_received_packet, 2);
quatl3 = dmp_quatTake32(dmp_received_packet, 3);
if (quatl0 > 32767)
quatl0 -= 65536;
if (quatl1 > 32767)
quatl1 -= 65536;
if (quatl2 > 32767)
quatl2 -= 65536;
if (quatl3 > 32767)
quatl3 -= 65536;
quat0 = ((float) quatl0) / 16384.0f;
quat1 = ((float) quatl1) / 16384.0f;
quat2 = ((float) quatl2) / 16384.0f;
quat3 = ((float) quatl3) / 16384.0f;
dmpdata[0] = atan2f(2 * ((quat0 * quat1) + (quat2 * quat3)), 1.0 - (2 * ((quat1 * quat1) + (quat2 * quat2)))) * (180.0f / M_PI);
dmpdata[1] = asinf(2 * ((quat0 * quat2) - (quat3 * quat1))) * (180.0f / M_PI);
angle[0] = dmpdata[0] * 10;
angle[1] = dmpdata[1] * 10;
dmpGyroData[0] = ((dmp_received_packet[4 * 4 + 0] << 8) | (dmp_received_packet[4 * 4 + 1] << 0));
dmpGyroData[1] = ((dmp_received_packet[4 * 5 + 0] << 8) | (dmp_received_packet[4 * 5 + 1] << 0));
dmpGyroData[2] = ((dmp_received_packet[4 * 6 + 0] << 8) | (dmp_received_packet[4 * 6 + 1] << 0));
}
void mpu6050DmpResetFifo(void)
{
uint8_t ctrl;
i2cRead(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 1, &ctrl);
ctrl |= 0x04;
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, ctrl);
}
static void mpu6050DmpGetPacket(void)
{
if (dmp_fifoCountL > 32) {
dmp_fifoCountL2 = dmp_fifoCountL - 32;
dmp_longPacket = true;
}
if (dmp_longPacket) {
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, 32, dmp_received_packet);
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmp_fifoCountL, dmp_received_packet + 32);
dmp_longPacket = false;
} else {
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmp_fifoCountL, dmp_received_packet);
}
}
uint16_t dmpFifoLevel = 0;
static bool mpu6050DmpFifoReady(void)
{
uint8_t buf[2];
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
dmp_fifoCountL = buf[1];
dmpFifoLevel = buf[0] << 8 | buf[1];
if (dmp_fifoCountL == 42 || dmp_fifoCountL == 44)
return true;
else {
// lame hack to empty out fifo, as dmpResetFifo doesn't actually seem to do it...
if (dmpFifoLevel > 100) {
// clear out fifo
uint8_t crap[16];
do {
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmpFifoLevel > 16 ? 16 : dmpFifoLevel, crap);
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
dmpFifoLevel = buf[0] << 8 | buf[1];
} while (dmpFifoLevel);
}
}
return false;
}
static void mpu6050DmpBankSelect(uint8_t bank)
{
i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, bank);
}
static void mpu6050DmpBankInit(void)
{
uint8_t i, j;
uint8_t incoming[9];
for (i = 0; i < 7; i++) {
mpu6050DmpBankSelect(i);
for (j = 0; j < 16; j++) {
uint8_t start_addy = j * 0x10;
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, start_addy);
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[i][j][0]);
}
}
mpu6050DmpBankSelect(7);
for (j = 0; j < 8; j++) {
uint8_t start_addy = j * 0x10;
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, start_addy);
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[7][j][0]);
}
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, 0x80);
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 9, (uint8_t *) & dmpMem[7][8][0]);
i2cRead(MPU6050_ADDRESS, DMP_MEM_R_W, 8, incoming);
}
static void mpu6050DmpMemInit(void)
{
uint8_t i;
uint8_t temp;
mpu6050DmpBankInit();
// Bank, Start Address, Update Length, Update Data...
for (i = 0; i < 22; i++) {
mpu6050DmpBankSelect(dmp_updates[i][0]); // bank
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
}
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, 0x32);
for (i = 22; i < 29; i++) {
mpu6050DmpBankSelect(dmp_updates[i][0]); // bank
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
}
/*
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_PWR_MGMT_1);
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_PWR_MGMT_2);
*/
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, 0x02); // ??
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL = PLL w Z ref
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x04);
i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); // full scale 2000 deg/s
i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, 0x0B); // ext_sync_set=temp_out_L, accel DLPF 44Hz, gyro DLPF 42Hz
i2cWrite(MPU6050_ADDRESS, MPU_RA_DMP_CFG_1, 0x03);
i2cWrite(MPU6050_ADDRESS, MPU_RA_DMP_CFG_2, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_XG_OFFS_TC, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_YG_OFFS_TC, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZG_OFFS_TC, 0x00);
// clear offsets
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_XG_OFFS_USRH, 6, "\x00\x00\x00\x00\x00\x00"); // data
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0xB2);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 2, "\xFF\xFF"); // data
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x90);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x09\x23\xA1\x35"); // data
i2cRead(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x04); // fifo reset
// Insert FIFO count read?
mpu6050DmpFifoReady();
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x00); // ?? I think this enables a lot of stuff but disables fifo
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL = PLL w Z ref
delay(2);
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 0x00);
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 0x00); // full scale range +/- 2g
delay(2);
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MOT_THR, 0x02);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZRMOT_THR, 0x9C);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MOT_DUR, 0x50);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZRMOT_DUR, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x04); // fifo reset
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0xC8); // fifo enable
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x6A);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 2, "\x06\x00"); // data
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 8, "\x00\x00\x00\x00\x00\x00\x00\x00"); // data
mpu6050DmpBankSelect(0x00); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x40\x00\x00\x00"); // data
}
#else /* MPU6050_DMP */
void mpu6050DmpLoop(void)
{
}
void mpu6050DmpResetFifo(void)
{
}
#endif /* MPU6050_DMP */

38
src/drv_ms5611.c
View file

@ -108,18 +108,18 @@ static int8_t ms5611_crc(uint16_t *prom)
return -1;
for (i = 0; i < 16; i++) {
if (i & 1)
if (i & 1)
res ^= ((prom[i >> 1]) & 0x00FF);
else
else
res ^= (prom[i >> 1] >> 8);
for (j = 8; j > 0; j--) {
if (res & 0x8000)
if (res & 0x8000)
res ^= 0x1800;
res <<= 1;
}
}
prom[7] |= crc;
if (crc == ((res >> 12) & 0xF))
if (crc == ((res >> 12) & 0xF))
return 0;
return -1;
@ -154,30 +154,28 @@ static void ms5611_get_up(void)
static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
{
int32_t temp, off2 = 0, sens2 = 0, delt;
int32_t press;
uint32_t press;
int64_t temp;
int64_t delt;
int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
int64_t dT = ms5611_ut - ((int32_t)ms5611_c[5] << 8);
int64_t off = ((uint32_t)ms5611_c[2] << 16) + ((dT * ms5611_c[4]) >> 7);
int64_t sens = ((uint32_t)ms5611_c[1] << 15) + ((dT * ms5611_c[3]) >> 8);
temp = 2000 + ((dT * ms5611_c[6]) >> 23);
if (temp < 2000) { // temperature lower than 20degC
if (temp < 2000) { // temperature lower than 20degC
delt = temp - 2000;
delt = 5 * delt * delt;
off2 = delt >> 1;
sens2 = delt >> 2;
off -= delt >> 1;
sens -= delt >> 2;
if (temp < -1500) { // temperature lower than -15degC
delt = temp + 1500;
delt = delt * delt;
off2 += 7 * delt;
sens2 += (11 * delt) >> 1;
off -= 7 * delt;
sens -= (11 * delt) >> 1;
}
}
off -= off2;
sens -= sens2;
press = (((ms5611_up * sens ) >> 21) - off) >> 15;
press = ((((int64_t)ms5611_up * sens ) >> 21) - off) >> 15;
if (pressure)
*pressure = press;
if (temperature)

2
src/drv_pwm.c
View file

@ -325,7 +325,7 @@ bool pwmInit(drv_pwm_config_t *init)
#ifdef SOFTSERIAL_19200_LOOPBACK
// skip softSerial ports
if ((port == PWM5 || port == PWM6))
if ((port == PWM5 || port == PWM6 || port == PWM7 || port == PWM8))
continue;
#endif

8
src/drv_pwm_fy90q.c
View file

@ -211,7 +211,7 @@ static void pwmInitializeInput(bool usePPM)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// TODO Configure EXTI4 1 channel
// Input timers on TIM2 for PWM
@ -233,12 +233,12 @@ static void pwmInitializeInput(bool usePPM)
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
for (i = 0; i < 4; i++) {
TIM_ICInitStructure.TIM_Channel = Channels[i].channel;
TIM_ICInit(Channels[i].tim, &TIM_ICInitStructure);
}
// TODO EXTI4
TIM_ITConfig(TIM2, TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4, ENABLE);
@ -280,7 +280,7 @@ bool pwmInit(drv_pwm_config_t *init)
// use PPM or PWM input
usePPMFlag = init->usePPM;
// preset channels to center
// preset channels to center
for (i = 0; i < 8; i++)
Inputs[i].capture = 1500;

40
src/drv_serial.c Normal file
View file

@ -0,0 +1,40 @@
#include "board.h"
void serialPrint(serialPort_t *instance, const char *str)
{
uint8_t ch;
while ((ch = *(str++))) {
serialWrite(instance, ch);
}
}
inline uint32_t serialGetBaudRate(serialPort_t *instance)
{
return instance->baudRate;
}
inline void serialWrite(serialPort_t *instance, uint8_t ch)
{
instance->vTable->serialWrite(instance, ch);
}
inline uint8_t serialTotalBytesWaiting(serialPort_t *instance)
{
return instance->vTable->serialTotalBytesWaiting(instance);
}
inline uint8_t serialRead(serialPort_t *instance)
{
return instance->vTable->serialRead(instance);
}
inline void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate)
{
instance->vTable->serialSetBaudRate(instance, baudRate);
}
inline bool isSerialTransmitBufferEmpty(serialPort_t *instance)
{
return instance->vTable->isSerialTransmitBufferEmpty(instance);
}

48
src/drv_serial.h Normal file
View file

@ -0,0 +1,48 @@
#pragma once
typedef enum portMode_t {
MODE_RX = 1,
MODE_TX = 2,
MODE_RXTX = MODE_RX | MODE_TX
} portMode_t;
typedef struct serialPort {
const struct serialPortVTable *vTable;
portMode_t mode;
uint32_t baudRate;
uint32_t rxBufferSize;
uint32_t txBufferSize;
volatile uint8_t *rxBuffer;
volatile uint8_t *txBuffer;
uint32_t rxBufferHead;
uint32_t rxBufferTail;
uint32_t txBufferHead;
uint32_t txBufferTail;
// FIXME rename member to rxCallback
serialReceiveCallbackPtr callback;
} serialPort_t;
struct serialPortVTable {
void (*serialWrite)(serialPort_t *instance, uint8_t ch);
uint8_t (*serialTotalBytesWaiting)(serialPort_t *instance);
uint8_t (*serialRead)(serialPort_t *instance);
// Specified baud rate may not be allowed by an implementation, use serialGetBaudRate to determine actual baud rate in use.
void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate);
bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance);
};
inline void serialWrite(serialPort_t *instance, uint8_t ch);
inline uint8_t serialTotalBytesWaiting(serialPort_t *instance);
inline uint8_t serialRead(serialPort_t *instance);
inline void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate);
inline bool isSerialTransmitBufferEmpty(serialPort_t *instance);
void serialPrint(serialPort_t *instance, const char *str);
uint32_t serialGetBaudRate(serialPort_t *instance);

346
src/drv_softserial.c
View file

@ -1,12 +1,34 @@
#include "board.h"
enum serialBitStatus {
WAITING_FOR_START_BIT = -1, BIT_0, BIT_1, BIT_2, BIT_3, BIT_4, BIT_5, BIT_6, BIT_7, WAITING_FOR_STOP_BIT
};
// There needs to be a table of timerMhz per baud rate so the system can set the timer correctly.
// See http://www.wormfood.net/avrbaudcalc.php?postbitrate=19200&postclock=72
// Currently defaulting to 3Mhz to support 19200.
#define SOFT_SERIAL_TIMER_MHZ 3
#define SOFT_SERIAL_1_TIMER_RX_HARDWARE 4
#define SOFT_SERIAL_1_TIMER_TX_HARDWARE 5
#define RX_TOTAL_BITS 8
#define TX_TOTAL_BITS 10
#define MAX_SOFTSERIAL_PORTS 2
softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS];
void onSerialTimer(uint8_t portIndex, uint16_t capture);
uint8_t readRxSignal(softSerial_t *softSerial)
{
return GPIO_ReadInputDataBit(softSerial->rxTimerHardware->gpio, softSerial->rxTimerHardware->pin);
}
void setTxSignal(softSerial_t *softSerial, uint8_t state)
{
if (state) {
digitalHi(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
} else {
digitalLo(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
}
}
softSerial_t* lookupSoftSerial(uint8_t reference)
{
assert_param(reference >= 0 && reference <= MAX_SOFTSERIAL_PORTS);
@ -14,50 +36,87 @@ softSerial_t* lookupSoftSerial(uint8_t reference)
return &(softSerialPorts[reference]);
}
void resetSerialTimer(softSerial_t *softSerial)
{
//uint16_t counter = TIM_GetCounter(softSerial->rxTimerHardware->tim);
TIM_SetCounter(softSerial->rxTimerHardware->tim, 0);
//counter = TIM_GetCounter(softSerial->rxTimerHardware->tim);
}
void stopSerialTimer(softSerial_t *softSerial)
{
TIM_Cmd(softSerial->timerHardware->tim, DISABLE);
TIM_SetCounter(softSerial->timerHardware->tim, 0);
TIM_Cmd(softSerial->rxTimerHardware->tim, DISABLE);
}
void startSerialTimer(softSerial_t *softSerial)
{
TIM_SetCounter(softSerial->timerHardware->tim, 0);
TIM_Cmd(softSerial->timerHardware->tim, ENABLE);
TIM_Cmd(softSerial->rxTimerHardware->tim, ENABLE);
}
static void waitForFirstBit(softSerial_t *softSerial)
static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
{
softSerial->state = BIT_0;
startSerialTimer(softSerial);
gpio_config_t cfg;
cfg.pin = pin;
cfg.mode = mode;
cfg.speed = Speed_2MHz;
gpioInit(gpio, &cfg);
}
static void onSerialPinChange(uint8_t reference, uint16_t capture)
void serialInputPortConfig(const timerHardware_t *timerHardwarePtr)
{
softSerial_t *softSerial = lookupSoftSerial(reference);
if (softSerial->state == WAITING_FOR_START_BIT) {
waitForFirstBit(softSerial);
softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
}
}
uint8_t readSerialSignal(softSerial_t *softSerial)
#define TICKS_PER_BIT 3
void serialTimerConfig(const timerHardware_t *timerHardwarePtr, uint32_t baud, uint8_t reference, timerCCCallbackPtr callback)
{
return GPIO_ReadInputDataBit(softSerial->timerHardware->gpio, softSerial->timerHardware->pin);
uint16_t timerPeriod = (SOFT_SERIAL_TIMER_MHZ * 1000000) / (baud * TICKS_PER_BIT);
timerInConfig(timerHardwarePtr, timerPeriod, SOFT_SERIAL_TIMER_MHZ);
configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, callback);
}
void mergeSignalWithCurrentByte(softSerial_t *softSerial, uint8_t serialSignal)
void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
{
softSerial->rxBuffer[softSerial->port.rxBufferTail] |= (serialSignal << softSerial->state);
softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_Out_PP);
}
inline void initialiseCurrentByteWithFirstSignal(softSerial_t *softSerial, uint8_t serialSignal)
void setupSoftSerial1(uint32_t baud)
{
softSerial->rxBuffer[softSerial->port.rxBufferTail] = serialSignal;
}
int portIndex = 0;
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
inline void prepareForNextSignal(softSerial_t *softSerial) {
softSerial->state++;
softSerial->port.vTable = softSerialVTable;
softSerial->port.mode = MODE_RXTX;
softSerial->port.baudRate = baud;
softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_RX_HARDWARE]);
softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_TX_HARDWARE]);
softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE;
softSerial->port.rxBuffer = softSerial->rxBuffer;
softSerial->port.rxBufferTail = 0;
softSerial->port.rxBufferHead = 0;
softSerial->port.txBuffer = softSerial->txBuffer;
softSerial->port.txBufferSize = SOFT_SERIAL_BUFFER_SIZE,
softSerial->port.txBufferTail = 0;
softSerial->port.txBufferHead = 0;
softSerial->isTransmittingData = false;
softSerial->isSearchingForStartBit = true;
softSerial->isSearchingForStopBit = false;
softSerial->timerRxCounter = 1;
serialInputPortConfig(softSerial->rxTimerHardware);
serialOutputPortConfig(softSerial->txTimerHardware);
setTxSignal(softSerial, 1);
delay(50);
serialTimerConfig(softSerial->rxTimerHardware, baud, portIndex, onSerialTimer);
}
void updateBufferIndex(softSerial_t *softSerial)
@ -70,104 +129,135 @@ void updateBufferIndex(softSerial_t *softSerial)
}
}
void prepareForNextByte(softSerial_t *softSerial)
/*********************************************/
void searchForStartBit(softSerial_t *softSerial)
{
stopSerialTimer(softSerial);
softSerial->state = WAITING_FOR_START_BIT;
char rxSignal = readRxSignal(softSerial);
if (rxSignal == 1) {
// start bit not found
softSerial->timerRxCounter = 1; // process on next timer event
return;
}
// timer is aligned to falling signal of start bit.
// three ticks per bit.
softSerial->isSearchingForStartBit = false;
softSerial->internalRxBuffer = 0;
softSerial->timerRxCounter = TICKS_PER_BIT + 1; // align to middle of next bit
softSerial->bitsLeftToReceive = RX_TOTAL_BITS;
softSerial->rxBitSelectionMask = 1;
}
void searchForStopBit(softSerial_t *softSerial)
{
softSerial->timerRxCounter = 1;
char rxSignal = readRxSignal(softSerial);
if (rxSignal != 1) {
// not found
return;
}
softSerial->isSearchingForStopBit = false;
softSerial->isSearchingForStartBit = true;
softSerial->internalRxBuffer &= 0xFF;
softSerial->port.rxBuffer[softSerial->port.rxBufferTail] = softSerial->internalRxBuffer;
updateBufferIndex(softSerial);
}
void readDataBit(softSerial_t *softSerial)
{
softSerial->timerRxCounter = TICKS_PER_BIT; // keep aligned to middle of bit
char rxSignal = readRxSignal(softSerial);
if (rxSignal) {
softSerial->internalRxBuffer |= softSerial->rxBitSelectionMask;
}
softSerial->rxBitSelectionMask <<= 1;
if (--softSerial->bitsLeftToReceive <= 0) {
softSerial->isSearchingForStopBit = true;
softSerial->timerRxCounter = 2;
}
}
void processRxState(softSerial_t *softSerial)
{
//digitalToggle(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
if (--softSerial->timerRxCounter > 0) {
return;
}
if (softSerial->isSearchingForStartBit) {
searchForStartBit(softSerial);
return;
}
if (softSerial->isSearchingForStopBit) {
searchForStopBit(softSerial);
return;
}
readDataBit(softSerial);
}
void processTxState(softSerial_t *softSerial)
{
char mask;
if (!softSerial->isTransmittingData) {
if (isSoftSerialTransmitBufferEmpty((serialPort_t *)softSerial)) {
return;
}
// data to send
char byteToSend = softSerial->port.txBuffer[softSerial->port.txBufferTail++];
if (softSerial->port.txBufferTail >= softSerial->port.txBufferSize) {
softSerial->port.txBufferTail = 0;
}
// build internal buffer, start bit(1) + data bits + stop bit(0)
softSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
softSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
// start immediately
softSerial->timerTxCounter = 1;
softSerial->isTransmittingData = true;
return;
}
if (--softSerial->timerTxCounter <= 0) {
mask = softSerial->internalTxBuffer & 1;
softSerial->internalTxBuffer >>= 1;
setTxSignal(softSerial, mask);
softSerial->timerTxCounter = TICKS_PER_BIT;
if (--softSerial->bitsLeftToTransmit <= 0) {
softSerial->isTransmittingData = false;
}
}
}
void onSerialTimer(uint8_t portIndex, uint16_t capture)
{
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
uint8_t serialSignal = readSerialSignal(softSerial);
switch (softSerial->state) {
case BIT_0:
initialiseCurrentByteWithFirstSignal(softSerial, serialSignal);
prepareForNextSignal(softSerial);
break;
case BIT_1:
case BIT_2:
case BIT_3:
case BIT_4:
case BIT_5:
case BIT_6:
case BIT_7:
mergeSignalWithCurrentByte(softSerial, serialSignal);
prepareForNextSignal(softSerial);
break;
case WAITING_FOR_STOP_BIT:
prepareForNextByte(softSerial);
break;
}
processTxState(softSerial);
processRxState(softSerial);
}
#define SOFT_SERIAL_TIMER_MHZ 1
#define SOFT_SERIAL_1_TIMER_HARDWARE 4
static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
{
gpio_config_t cfg;
cfg.pin = pin;
cfg.mode = mode;
cfg.speed = Speed_2MHz;
gpioInit(gpio, &cfg);
}
void serialInputPortConfig(const timerHardware_t *timerHardwarePtr, uint16_t baud, uint8_t reference, timerCCCallbackPtr callback)
{
softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
int oneBitPeriod = (SOFT_SERIAL_TIMER_MHZ * 1000000) / baud;
timerInConfig(timerHardwarePtr, oneBitPeriod, SOFT_SERIAL_TIMER_MHZ);
configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, callback);
}
void setupSoftSerial1(uint32_t baud)
{
int portIndex = 0;
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
softSerial->timerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_HARDWARE]);
softSerial->timerIndex = SOFT_SERIAL_1_TIMER_HARDWARE;
softSerial->state = WAITING_FOR_START_BIT;
softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE;
softSerial->port.rxBuffer = softSerial->rxBuffer;
softSerial->port.rxBufferTail = 0;
softSerial->port.rxBufferHead = 0;
softSerial->port.txBuffer = 0;
softSerial->port.txBufferSize = 0;
softSerial->port.txBufferTail = 0;
softSerial->port.txBufferHead = 0;
softSerial->port.baudRate = baud;
softSerial->port.mode = MODE_RX;
// FIXME this uart specific initialisation doesn't belong really here
softSerial->port.txDMAChannel = 0;
softSerial->port.txDMAChannel = 0;
configureTimerChannelCallback(softSerial->timerHardware->tim, TIM_Channel_2, portIndex, onSerialTimer);
TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE);
stopSerialTimer(softSerial);
serialInputPortConfig(softSerial->timerHardware, baud, portIndex, onSerialPinChange);
}
bool serialAvailable(softSerial_t *softSerial)
uint8_t softSerialTotalBytesWaiting(serialPort_t *instance)
{
softSerial_t *softSerial = (softSerial_t *)instance;
if (softSerial->port.rxBufferTail == softSerial->port.rxBufferHead) {
return 0;
}
@ -190,14 +280,42 @@ static void moveHeadToNextByte(softSerial_t *softSerial)
}
}
uint8_t serialReadByte(softSerial_t *softSerial)
uint8_t softSerialReadByte(serialPort_t *instance)
{
if (serialAvailable(softSerial) == 0) {
if (softSerialTotalBytesWaiting(instance) == 0) {
return 0;
}
char b = softSerial->port.rxBuffer[softSerial->port.rxBufferHead];
char b = instance->rxBuffer[instance->rxBufferHead];
moveHeadToNextByte(softSerial);
moveHeadToNextByte((softSerial_t *)instance);
return b;
}
void softSerialWriteByte(serialPort_t *s, uint8_t ch)
{
s->txBuffer[s->txBufferHead] = ch;
s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
}
void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
{
// not implemented.
}
bool isSoftSerialTransmitBufferEmpty(serialPort_t *instance)
{
return instance->txBufferHead == instance->txBufferTail;
}
const struct serialPortVTable softSerialVTable[] = {
{
softSerialWriteByte,
softSerialTotalBytesWaiting,
softSerialReadByte,
softSerialSetBaudRate,
isSoftSerialTransmitBufferEmpty
}
};

38
src/drv_softserial.h
View file

@ -10,16 +10,38 @@
#define SOFT_SERIAL_BUFFER_SIZE 256
typedef struct softSerial_s {
const timerHardware_t *timerHardware;
uint8_t timerIndex;
serialPort_t port;
volatile int state;
volatile uint8_t rxBuffer[SOFT_SERIAL_BUFFER_SIZE];
} softSerial_t;
void setupSoftSerial1(uint32_t baud);
uint8_t serialReadByte(softSerial_t *softSerial);
bool serialAvailable(softSerial_t *softSerial);
const timerHardware_t *rxTimerHardware;
volatile uint8_t rxBuffer[SOFT_SERIAL_BUFFER_SIZE];
const timerHardware_t *txTimerHardware;
volatile uint8_t txBuffer[SOFT_SERIAL_BUFFER_SIZE];
uint8_t isSearchingForStopBit;
uint8_t rxBitSelectionMask;
uint8_t isSearchingForStartBit;
uint8_t isTransmittingData;
uint8_t timerRxCounter;
uint8_t timerTxCounter;
uint8_t bitsLeftToReceive;
uint8_t bitsLeftToTransmit;
uint16_t internalRxBuffer; // excluding start/stop bits
uint16_t internalTxBuffer; // includes start and stop bits
} softSerial_t;
extern timerHardware_t* serialTimerHardware;
extern softSerial_t softSerialPorts[];
extern const struct serialPortVTable softSerialVTable[];
void setupSoftSerial1(uint32_t baud);
// serialPort API
void softSerialWriteByte(serialPort_t *instance, uint8_t ch);
uint8_t softSerialTotalBytesWaiting(serialPort_t *instance);
uint8_t softSerialReadByte(serialPort_t *instance);
void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);
bool isSoftSerialTransmitBufferEmpty(serialPort_t *s);

2
src/drv_spi.c
View file

@ -96,6 +96,6 @@ static bool spiTest(void)
spiSelect(false);
if (in[1] == 0xEF)
return true;
return false;
}

47
src/drv_system.c
View file

@ -182,3 +182,50 @@ void systemReset(bool toBootloader)
// Generate system reset
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
}
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
{
switch (rotation) {
case CW0_DEG:
dest[X] = src[X];
dest[Y] = src[Y];
dest[Z] = src[Z];
break;
case CW90_DEG:
dest[X] = src[Y];
dest[Y] = -src[X];
dest[Z] = src[Z];
break;
case CW180_DEG:
dest[X] = -src[X];
dest[Y] = -src[Y];
dest[Z] = src[Z];
break;
case CW270_DEG:
dest[X] = -src[Y];
dest[Y] = src[X];
dest[Z] = src[Z];
break;
case CW0_DEG_FLIP:
dest[X] = -src[X];
dest[Y] = src[Y];
dest[Z] = -src[Z];
break;
case CW90_DEG_FLIP:
dest[X] = src[Y];
dest[Y] = src[X];
dest[Z] = -src[Z];
break;
case CW180_DEG_FLIP:
dest[X] = src[X];
dest[Y] = -src[Y];
dest[Z] = -src[Z];
break;
case CW270_DEG_FLIP:
dest[X] = -src[Y];
dest[Y] = -src[X];
dest[Z] = -src[Z];
break;
default:
break;
}
}

3
src/drv_system.h
View file

@ -15,3 +15,6 @@ void systemReset(bool toBootloader);
// current crystal frequency - 8 or 12MHz
extern uint32_t hse_value;
// sensor orientation
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation);

338
src/drv_timer.c
View file

@ -1,100 +1,100 @@
#include "board.h"
#define PULSE_1MS (1000) // 1ms pulse width
/* FreeFlight/Naze32 timer layout
TIM2_CH1 RC1 PWM1
TIM2_CH2 RC2 PWM2
TIM2_CH3 RC3/UA2_TX PWM3
TIM2_CH4 RC4/UA2_RX PWM4
TIM3_CH1 RC5 PWM5
TIM3_CH2 RC6 PWM6
TIM3_CH3 RC7 PWM7
TIM3_CH4 RC8 PWM8
TIM1_CH1 PWM1 PWM9
TIM1_CH4 PWM2 PWM10
TIM4_CH1 PWM3 PWM11
TIM4_CH2 PWM4 PWM12
TIM4_CH3 PWM5 PWM13
TIM4_CH4 PWM6 PWM14
RX1 TIM2_CH1 PA0 [also PPM] [also used for throttle calibration]
RX2 TIM2_CH2 PA1
RX3 TIM2_CH3 PA2 [also UART2_TX]
RX4 TIM2_CH4 PA3 [also UART2_RX]
RX5 TIM3_CH1 PA6 [also ADC_IN6]
RX6 TIM3_CH2 PA7 [also ADC_IN7]
RX7 TIM3_CH3 PB0 [also ADC_IN8]
RX8 TIM3_CH4 PB1 [also ADC_IN9]
Outputs
PWM1 TIM1_CH1 PA8
PWM2 TIM1_CH4 PA11
PWM3 TIM4_CH1 PB6? [also I2C1_SCL]
PWM4 TIM4_CH2 PB7 [also I2C1_SDA]
PWM5 TIM4_CH3 PB8
PWM6 TIM4_CH4 PB9
Groups that allow running different period (ex 50Hz servos + 400Hz throttle + etc):
TIM2 4 channels
TIM3 4 channels
TIM1 2 channels
TIM4 4 channels
*/
const timerHardware_t timerHardware[] = {
{ TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, }, // PWM1
{ TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, }, // PWM2
{ TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, }, // PWM3
{ TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, }, // PWM4
{ TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, }, // PWM5
{ TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 0, }, // PWM6
{ TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, }, // PWM7
{ TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, }, // PWM8
{ TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, }, // PWM9
{ TIM1, GPIOA, Pin_11, TIM_Channel_4, TIM1_CC_IRQn, 1, }, // PWM10
{ TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, }, // PWM11
{ TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 0, }, // PWM12
{ TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 0, }, // PWM13
{ TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, }, // PWM14
};
#define MAX_TIMERS 4 // TIM1..TIM4
#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4
static const TIM_TypeDef *timers[MAX_TIMERS] = {
TIM1, TIM2, TIM3, TIM4
};
static const uint8_t channels[CC_CHANNELS_PER_TIMER] = {
TIM_Channel_1, TIM_Channel_2, TIM_Channel_3, TIM_Channel_4
};
typedef struct timerConfig_s {
TIM_TypeDef *tim;
#include "board.h"
#define PULSE_1MS (1000) // 1ms pulse width
/* FreeFlight/Naze32 timer layout
TIM2_CH1 RC1 PWM1
TIM2_CH2 RC2 PWM2
TIM2_CH3 RC3/UA2_TX PWM3
TIM2_CH4 RC4/UA2_RX PWM4
TIM3_CH1 RC5 PWM5
TIM3_CH2 RC6 PWM6
TIM3_CH3 RC7 PWM7
TIM3_CH4 RC8 PWM8
TIM1_CH1 PWM1 PWM9
TIM1_CH4 PWM2 PWM10
TIM4_CH1 PWM3 PWM11
TIM4_CH2 PWM4 PWM12
TIM4_CH3 PWM5 PWM13
TIM4_CH4 PWM6 PWM14
RX1 TIM2_CH1 PA0 [also PPM] [also used for throttle calibration]
RX2 TIM2_CH2 PA1
RX3 TIM2_CH3 PA2 [also UART2_TX]
RX4 TIM2_CH4 PA3 [also UART2_RX]
RX5 TIM3_CH1 PA6 [also ADC_IN6]
RX6 TIM3_CH2 PA7 [also ADC_IN7]
RX7 TIM3_CH3 PB0 [also ADC_IN8]
RX8 TIM3_CH4 PB1 [also ADC_IN9]
Outputs
PWM1 TIM1_CH1 PA8
PWM2 TIM1_CH4 PA11
PWM3 TIM4_CH1 PB6? [also I2C1_SCL]
PWM4 TIM4_CH2 PB7 [also I2C1_SDA]
PWM5 TIM4_CH3 PB8
PWM6 TIM4_CH4 PB9
Groups that allow running different period (ex 50Hz servos + 400Hz throttle + etc):
TIM2 4 channels
TIM3 4 channels
TIM1 2 channels
TIM4 4 channels
*/
const timerHardware_t timerHardware[] = {
{ TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, }, // PWM1
{ TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, }, // PWM2
{ TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, }, // PWM3
{ TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, }, // PWM4
{ TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, }, // PWM5
{ TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 0, }, // PWM6
{ TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, }, // PWM7
{ TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, }, // PWM8
{ TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, }, // PWM9
{ TIM1, GPIOA, Pin_11, TIM_Channel_4, TIM1_CC_IRQn, 1, }, // PWM10
{ TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, }, // PWM11
{ TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 0, }, // PWM12
{ TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 0, }, // PWM13
{ TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, }, // PWM14
};
#define MAX_TIMERS 4 // TIM1..TIM4
#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4
static const TIM_TypeDef *timers[MAX_TIMERS] = {
TIM1, TIM2, TIM3, TIM4
};
static const uint8_t channels[CC_CHANNELS_PER_TIMER] = {
TIM_Channel_1, TIM_Channel_2, TIM_Channel_3, TIM_Channel_4
};
typedef struct timerConfig_s {
TIM_TypeDef *tim;
uint8_t channel;
timerCCCallbackPtr *callback;
uint8_t reference;
} timerConfig_t;
timerConfig_t timerConfig[MAX_TIMERS * CC_CHANNELS_PER_TIMER];
static uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
{
uint8_t timerIndex = 0;
while (timers[timerIndex] != tim) {
timerIndex++;
}
return timerIndex;
}
static uint8_t lookupChannelIndex(const uint8_t channel)
{
uint8_t channelIndex = 0;
while (channels[channelIndex] != channel) {
channelIndex++;
}
return channelIndex;
uint8_t reference;
} timerConfig_t;
timerConfig_t timerConfig[MAX_TIMERS * CC_CHANNELS_PER_TIMER];
static uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
{
uint8_t timerIndex = 0;
while (timers[timerIndex] != tim) {
timerIndex++;
}
return timerIndex;
}
static uint8_t lookupChannelIndex(const uint8_t channel)
{
uint8_t channelIndex = 0;
while (channels[channelIndex] != channel) {
channelIndex++;
}
return channelIndex;
}
void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback)
@ -111,24 +111,24 @@ void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t re
timerConfig[timerConfigIndex].channel = channel;
timerConfig[timerConfigIndex].reference = reference;
}
void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel)
{
switch (channel) {
case TIM_Channel_1:
TIM_ITConfig(tim, TIM_IT_CC1, ENABLE);
break;
case TIM_Channel_2:
TIM_ITConfig(tim, TIM_IT_CC2, ENABLE);
break;
case TIM_Channel_3:
TIM_ITConfig(tim, TIM_IT_CC3, ENABLE);
break;
case TIM_Channel_4:
TIM_ITConfig(tim, TIM_IT_CC4, ENABLE);
break;
}
}
void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel)
{
switch (channel) {
case TIM_Channel_1:
TIM_ITConfig(tim, TIM_IT_CC1, ENABLE);
break;
case TIM_Channel_2:
TIM_ITConfig(tim, TIM_IT_CC2, ENABLE);
break;
case TIM_Channel_3:
TIM_ITConfig(tim, TIM_IT_CC3, ENABLE);
break;
case TIM_Channel_4:
TIM_ITConfig(tim, TIM_IT_CC4, ENABLE);
break;
}
}
void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback)
{
@ -145,7 +145,7 @@ void timerNVICConfig(uint8_t irq)
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
}
void configTimeBase(TIM_TypeDef *tim, uint32_t period, uint8_t mhz)
{
@ -170,60 +170,60 @@ void timerInConfig(const timerHardware_t *timerHardwarePtr, uint32_t period, uin
timerConfig_t *findTimerConfig(TIM_TypeDef *tim, uint8_t channel)
{
uint8_t timerConfigIndex = (lookupTimerIndex(tim) * MAX_TIMERS) + lookupChannelIndex(channel);
return &(timerConfig[timerConfigIndex]);
}
static void timCCxHandler(TIM_TypeDef *tim)
{
uint16_t capture;
timerConfig_t *timerConfig;
if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
TIM_ClearITPendingBit(tim, TIM_IT_CC1);
timerConfig = findTimerConfig(tim, TIM_Channel_1);
capture = TIM_GetCapture1(tim);
} else if (TIM_GetITStatus(tim, TIM_IT_CC2) == SET) {
TIM_ClearITPendingBit(tim, TIM_IT_CC2);
timerConfig = findTimerConfig(tim, TIM_Channel_2);
capture = TIM_GetCapture2(tim);
} else if (TIM_GetITStatus(tim, TIM_IT_CC3) == SET) {
TIM_ClearITPendingBit(tim, TIM_IT_CC3);
timerConfig = findTimerConfig(tim, TIM_Channel_3);
capture = TIM_GetCapture3(tim);
} else if (TIM_GetITStatus(tim, TIM_IT_CC4) == SET) {
TIM_ClearITPendingBit(tim, TIM_IT_CC4);
timerConfig = findTimerConfig(tim, TIM_Channel_4);
capture = TIM_GetCapture4(tim);
return &(timerConfig[timerConfigIndex]);
}
static void timCCxHandler(TIM_TypeDef *tim)
{
uint16_t capture;
timerConfig_t *timerConfig;
if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
TIM_ClearITPendingBit(tim, TIM_IT_CC1);
timerConfig = findTimerConfig(tim, TIM_Channel_1);
capture = TIM_GetCapture1(tim);
} else if (TIM_GetITStatus(tim, TIM_IT_CC2) == SET) {
TIM_ClearITPendingBit(tim, TIM_IT_CC2);
timerConfig = findTimerConfig(tim, TIM_Channel_2);
capture = TIM_GetCapture2(tim);
} else if (TIM_GetITStatus(tim, TIM_IT_CC3) == SET) {
TIM_ClearITPendingBit(tim, TIM_IT_CC3);
timerConfig = findTimerConfig(tim, TIM_Channel_3);
capture = TIM_GetCapture3(tim);
} else if (TIM_GetITStatus(tim, TIM_IT_CC4) == SET) {
TIM_ClearITPendingBit(tim, TIM_IT_CC4);
timerConfig = findTimerConfig(tim, TIM_Channel_4);
capture = TIM_GetCapture4(tim);
} else {
return; // avoid uninitialised variable dereference
}
if (!timerConfig->callback) {
return;
}
timerConfig->callback(timerConfig->reference, capture);
}
void TIM1_CC_IRQHandler(void)
{
timCCxHandler(TIM1);
}
void TIM2_IRQHandler(void)
{
timCCxHandler(TIM2);
}
void TIM3_IRQHandler(void)
{
timCCxHandler(TIM3);
}
void TIM4_IRQHandler(void)
{
timCCxHandler(TIM4);
}
}
if (!timerConfig->callback) {
return;
}
timerConfig->callback(timerConfig->reference, capture);
}
void TIM1_CC_IRQHandler(void)
{
timCCxHandler(TIM1);
}
void TIM2_IRQHandler(void)
{
timCCxHandler(TIM2);
}
void TIM3_IRQHandler(void)
{
timCCxHandler(TIM3);
}
void TIM4_IRQHandler(void)
{
timCCxHandler(TIM4);
}

161
src/drv_uart.c
View file

@ -4,23 +4,28 @@
Copyright © 2011 Bill Nesbitt
*/
static serialPort_t serialPort1;
static serialPort_t serialPort2;
static uartPort_t uartPort1;
static uartPort_t uartPort2;
// USART1 - Telemetry (RX/TX by DMA)
serialPort_t *serialUSART1(uint32_t baudRate, portmode_t mode)
uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
{
serialPort_t *s;
uartPort_t *s;
static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE];
static volatile uint8_t tx1Buffer[UART1_TX_BUFFER_SIZE];
gpio_config_t gpio;
NVIC_InitTypeDef NVIC_InitStructure;
s = &serialPort1;
s->rxBufferSize = UART1_RX_BUFFER_SIZE;
s->txBufferSize = UART1_TX_BUFFER_SIZE;
s->rxBuffer = rx1Buffer;
s->txBuffer = tx1Buffer;
s = &uartPort1;
s->port.vTable = uartVTable;
s->port.baudRate = baudRate;
s->port.rxBuffer = rx1Buffer;
s->port.txBuffer = tx1Buffer;
s->port.rxBufferSize = UART1_RX_BUFFER_SIZE;
s->port.txBufferSize = UART1_TX_BUFFER_SIZE;
s->rxDMAChannel = DMA1_Channel5;
s->txDMAChannel = DMA1_Channel4;
@ -48,20 +53,24 @@ serialPort_t *serialUSART1(uint32_t baudRate, portmode_t mode)
}
// USART2 - GPS or Spektrum or ?? (RX + TX by IRQ)
serialPort_t *serialUSART2(uint32_t baudRate, portmode_t mode)
uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode)
{
serialPort_t *s;
uartPort_t *s;
static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE];
static volatile uint8_t tx2Buffer[UART2_TX_BUFFER_SIZE];
gpio_config_t gpio;
NVIC_InitTypeDef NVIC_InitStructure;
s = &serialPort2;
s->baudRate = baudRate;
s->rxBufferSize = UART2_RX_BUFFER_SIZE;
s->txBufferSize = UART2_TX_BUFFER_SIZE;
s->rxBuffer = rx2Buffer;
s->txBuffer = tx2Buffer;
s = &uartPort2;
s->port.vTable = uartVTable;
s->port.baudRate = baudRate;
s->port.rxBufferSize = UART2_RX_BUFFER_SIZE;
s->port.txBufferSize = UART2_TX_BUFFER_SIZE;
s->port.rxBuffer = rx2Buffer;
s->port.txBuffer = tx2Buffer;
s->USARTx = USART2;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
@ -87,11 +96,12 @@ serialPort_t *serialUSART2(uint32_t baudRate, portmode_t mode)
return s;
}
serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode)
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode)
{
DMA_InitTypeDef DMA_InitStructure;
USART_InitTypeDef USART_InitStructure;
serialPort_t *s = NULL;
uartPort_t *s = NULL;
if (USARTx == USART1)
s = serialUSART1(baudRate, mode);
@ -99,11 +109,14 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, u
s = serialUSART2(baudRate, mode);
s->USARTx = USARTx;
s->rxBufferHead = s->rxBufferTail = 0;
s->txBufferHead = s->txBufferTail = 0;
// common serial initialisation code should move to serialPort::init()
s->port.rxBufferHead = s->port.rxBufferTail = 0;
s->port.txBufferHead = s->port.txBufferTail = 0;
// callback for IRQ-based RX ONLY
s->callback = callback;
s->mode = mode;
s->port.callback = callback;
s->port.mode = mode;
s->port.baudRate = baudRate;
USART_InitStructure.USART_BaudRate = baudRate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
@ -130,10 +143,10 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, u
// Receive DMA or IRQ
if (mode & MODE_RX) {
if (s->rxDMAChannel) {
DMA_InitStructure.DMA_BufferSize = s->rxBufferSize;
DMA_InitStructure.DMA_BufferSize = s->port.rxBufferSize;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)s->rxBuffer;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)s->port.rxBuffer;
DMA_DeInit(s->rxDMAChannel);
DMA_Init(s->rxDMAChannel, &DMA_InitStructure);
DMA_Cmd(s->rxDMAChannel, ENABLE);
@ -147,7 +160,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, u
// Transmit DMA or IRQ
if (mode & MODE_TX) {
if (s->txDMAChannel) {
DMA_InitStructure.DMA_BufferSize = s->txBufferSize;
DMA_InitStructure.DMA_BufferSize = s->port.txBufferSize;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_DeInit(s->txDMAChannel);
@ -161,12 +174,13 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, u
}
}
return s;
return (serialPort_t *)s;
}
void uartChangeBaud(serialPort_t *s, uint32_t baudRate)
void uartSetBaudRate(serialPort_t *instance, uint32_t baudRate)
{
USART_InitTypeDef USART_InitStructure;
uartPort_t *s = (uartPort_t *)instance;
USART_InitStructure.USART_BaudRate = baudRate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
@ -174,64 +188,71 @@ void uartChangeBaud(serialPort_t *s, uint32_t baudRate)
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = 0;
if (s->mode & MODE_RX)
if (s->port.mode & MODE_RX)
USART_InitStructure.USART_Mode |= USART_Mode_Rx;
if (s->mode & MODE_TX)
if (s->port.mode & MODE_TX)
USART_InitStructure.USART_Mode |= USART_Mode_Tx;
USART_Init(s->USARTx, &USART_InitStructure);
s->port.baudRate = baudRate;
}
static void uartStartTxDMA(serialPort_t *s)
static void uartStartTxDMA(uartPort_t *s)
{
s->txDMAChannel->CMAR = (uint32_t)&s->txBuffer[s->txBufferTail];
if (s->txBufferHead > s->txBufferTail) {
s->txDMAChannel->CNDTR = s->txBufferHead - s->txBufferTail;
s->txBufferTail = s->txBufferHead;
s->txDMAChannel->CMAR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail];
if (s->port.txBufferHead > s->port.txBufferTail) {
s->txDMAChannel->CNDTR = s->port.txBufferHead - s->port.txBufferTail;
s->port.txBufferTail = s->port.txBufferHead;
} else {
s->txDMAChannel->CNDTR = s->txBufferSize - s->txBufferTail;
s->txBufferTail = 0;
s->txDMAChannel->CNDTR = s->port.txBufferSize - s->port.txBufferTail;
s->port.txBufferTail = 0;
}
s->txDMAEmpty = false;
DMA_Cmd(s->txDMAChannel, ENABLE);
}
bool isUartAvailable(serialPort_t *s)
uint8_t uartTotalBytesWaiting(serialPort_t *instance)
{
uartPort_t *s = (uartPort_t*)instance;
// FIXME always returns 1 or 0, not the amount of bytes waiting
if (s->rxDMAChannel)
return s->rxDMAChannel->CNDTR != s->rxDMAPos;
else
return s->rxBufferTail != s->rxBufferHead;
return s->port.rxBufferTail != s->port.rxBufferHead;
}
// BUGBUG TODO TODO FIXME
bool isUartTransmitEmpty(serialPort_t *s)
// BUGBUG TODO TODO FIXME - What is the bug?
bool isUartTransmitBufferEmpty(serialPort_t *instance)
{
uartPort_t *s = (uartPort_t *)instance;
if (s->txDMAChannel)
return s->txDMAEmpty;
else
return s->txBufferTail == s->txBufferHead;
return s->port.txBufferTail == s->port.txBufferHead;
}
uint8_t uartRead(serialPort_t *s)
uint8_t uartRead(serialPort_t *instance)
{
uint8_t ch;
uartPort_t *s = (uartPort_t *)instance;
if (s->rxDMAChannel) {
ch = s->rxBuffer[s->rxBufferSize - s->rxDMAPos];
ch = s->port.rxBuffer[s->port.rxBufferSize - s->rxDMAPos];
if (--s->rxDMAPos == 0)
s->rxDMAPos = s->rxBufferSize;
s->rxDMAPos = s->port.rxBufferSize;
} else {
ch = s->rxBuffer[s->rxBufferTail];
s->rxBufferTail = (s->rxBufferTail + 1) % s->rxBufferSize;
ch = s->port.rxBuffer[s->port.rxBufferTail];
s->port.rxBufferTail = (s->port.rxBufferTail + 1) % s->port.rxBufferSize;
}
return ch;
}
void uartWrite(serialPort_t *s, uint8_t ch)
void uartWrite(serialPort_t *instance, uint8_t ch)
{
s->txBuffer[s->txBufferHead] = ch;
s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
uartPort_t *s = (uartPort_t *)instance;
s->port.txBuffer[s->port.txBufferHead] = ch;
s->port.txBufferHead = (s->port.txBufferHead + 1) % s->port.txBufferSize;
if (s->txDMAChannel) {
if (!(s->txDMAChannel->CCR & 1))
@ -241,24 +262,26 @@ void uartWrite(serialPort_t *s, uint8_t ch)
}
}
void uartPrint(serialPort_t *s, const char *str)
const struct serialPortVTable uartVTable[] = {
{
uint8_t ch;
while ((ch = *(str++))) {
uartWrite(s, ch);
uartWrite,
uartTotalBytesWaiting,
uartRead,
uartSetBaudRate,
isUartTransmitBufferEmpty
}
}
};
// Handlers
// USART1 Tx DMA Handler
void DMA1_Channel4_IRQHandler(void)
{
serialPort_t *s = &serialPort1;
uartPort_t *s = &uartPort1;
DMA_ClearITPendingBit(DMA1_IT_TC4);
DMA_Cmd(s->txDMAChannel, DISABLE);
if (s->txBufferHead != s->txBufferTail)
if (s->port.txBufferHead != s->port.txBufferTail)
uartStartTxDMA(s);
else
s->txDMAEmpty = true;
@ -267,13 +290,13 @@ void DMA1_Channel4_IRQHandler(void)
// USART1 Tx IRQ Handler
void USART1_IRQHandler(void)
{
serialPort_t *s = &serialPort1;
uartPort_t *s = &uartPort1;
uint16_t SR = s->USARTx->SR;
if (SR & USART_FLAG_TXE) {
if (s->txBufferTail != s->txBufferHead) {
s->USARTx->DR = s->txBuffer[s->txBufferTail];
s->txBufferTail = (s->txBufferTail + 1) % s->txBufferSize;
if (s->port.txBufferTail != s->port.txBufferHead) {
s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
} else {
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
}
@ -283,22 +306,22 @@ void USART1_IRQHandler(void)
// USART2 Rx/Tx IRQ Handler
void USART2_IRQHandler(void)
{
serialPort_t *s = &serialPort2;
uartPort_t *s = &uartPort2;
uint16_t SR = s->USARTx->SR;
if (SR & USART_FLAG_RXNE) {
// If we registered a callback, pass crap there
if (s->callback) {
s->callback(s->USARTx->DR);
if (s->port.callback) {
s->port.callback(s->USARTx->DR);
} else {
s->rxBuffer[s->rxBufferHead] = s->USARTx->DR;
s->rxBufferHead = (s->rxBufferHead + 1) % s->rxBufferSize;
s->port.rxBuffer[s->port.rxBufferHead] = s->USARTx->DR;
s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
}
}
if (SR & USART_FLAG_TXE) {
if (s->txBufferTail != s->txBufferHead) {
s->USARTx->DR = s->txBuffer[s->txBufferTail];
s->txBufferTail = (s->txBufferTail + 1) % s->txBufferSize;
if (s->port.txBufferTail != s->port.txBufferHead) {
s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
} else {
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
}

40
src/drv_uart.h
View file

@ -8,29 +8,10 @@
#define UART2_TX_BUFFER_SIZE 64
#define MAX_SERIAL_PORTS 2
// This is a bitmask
typedef enum portmode_t {
MODE_RX = 1,
MODE_TX = 2,
MODE_RXTX = 3
} portmode_t;
// FIXME this is a uart_t really. Move the generic properties into a separate structure (serialPort_t) and update the code to use it
typedef struct {
portmode_t mode;
uint32_t baudRate;
uint32_t rxBufferSize;
uint32_t txBufferSize;
volatile uint8_t *rxBuffer;
volatile uint8_t *txBuffer;
uint32_t rxBufferHead;
uint32_t rxBufferTail;
uint32_t txBufferHead;
uint32_t txBufferTail;
// FIXME rename callback type so something more generic
uartReceiveCallbackPtr callback;
serialPort_t port;
// FIXME these are uart specific and do not belong in here
DMA_Channel_TypeDef *rxDMAChannel;
@ -43,12 +24,15 @@ typedef struct {
bool txDMAEmpty;
USART_TypeDef *USARTx;
} serialPort_t;
} uartPort_t;
serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode);
void uartChangeBaud(serialPort_t *s, uint32_t baudRate);
bool isUartAvailable(serialPort_t *s);
bool isUartTransmitEmpty(serialPort_t *s);
uint8_t uartRead(serialPort_t *s);
void uartWrite(serialPort_t *s, uint8_t ch);
void uartPrint(serialPort_t *s, const char *str);
extern const struct serialPortVTable uartVTable[];
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode);
// serialPort API
void uartWrite(serialPort_t *instance, uint8_t ch);
uint8_t uartTotalBytesWaiting(serialPort_t *instance);
uint8_t uartRead(serialPort_t *instance);
void uartSetBaudRate(serialPort_t *s, uint32_t baudRate);
bool isUartTransmitBufferEmpty(serialPort_t *s);

137
src/gps.c
View file

@ -10,6 +10,9 @@ const uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 };
static void GPS_NewData(uint16_t c);
static void gpsPrint(const char *str);
#define UBX_INIT_STRING_INDEX 0
#define MTK_INIT_STRING_INDEX 4
static const char * const gpsInitStrings[] = {
"$PUBX,41,1,0003,0001,19200,0*23\r\n", // UBX0..3
"$PUBX,41,1,0003,0001,38400,0*26\r\n",
@ -45,13 +48,15 @@ void gpsInit(uint32_t baudrate)
core.gpsport = uartOpen(USART2, GPS_NewData, baudrate, MODE_RXTX);
if (mcfg.gps_type == GPS_UBLOX)
offset = 0;
offset = UBX_INIT_STRING_INDEX;
else if (mcfg.gps_type == GPS_MTK)
offset = 4;
offset = MTK_INIT_STRING_INDEX;
if (mcfg.gps_type != GPS_NMEA) {
for (i = 0; i < 5; i++) {
uartChangeBaud(core.gpsport, init_speed[i]);
serialSetBaudRate(core.gpsport, init_speed[i]);
// verify the requested change took effect.
baudrate = serialGetBaudRate(core.gpsport);
switch (baudrate) {
case 19200:
gpsPrint(gpsInitStrings[offset]);
@ -70,10 +75,10 @@ void gpsInit(uint32_t baudrate)
}
}
uartChangeBaud(core.gpsport, baudrate);
serialSetBaudRate(core.gpsport, baudrate);
if (mcfg.gps_type == GPS_UBLOX) {
for (i = 0; i < sizeof(ubloxInit); i++) {
uartWrite(core.gpsport, ubloxInit[i]); // send ubx init binary
serialWrite(core.gpsport, ubloxInit[i]); // send ubx init binary
delay(4);
}
} else if (mcfg.gps_type == GPS_MTK) {
@ -90,13 +95,13 @@ void gpsInit(uint32_t baudrate)
static void gpsPrint(const char *str)
{
while (*str) {
uartWrite(core.gpsport, *str);
serialWrite(core.gpsport, *str);
if (mcfg.gps_type == GPS_UBLOX)
delay(4);
str++;
}
// wait to send all
while (!isUartTransmitEmpty(core.gpsport));
while (!isSerialTransmitBufferEmpty(core.gpsport));
delay(30);
}
@ -927,46 +932,46 @@ typedef struct {
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
} ubx_nav_velned;
typedef struct
{
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask
uint8_t quality; // Bitfield
uint8_t cno; // Carrier to Noise Ratio (Signal Strength)
uint8_t elev; // Elevation in integer degrees
int16_t azim; // Azimuth in integer degrees
int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;
typedef struct
{
uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
uint16_t reserved2; // Reserved
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo;
enum {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
uint32_t heading_accuracy;
} ubx_nav_velned;
typedef struct
{
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask
uint8_t quality; // Bitfield
uint8_t cno; // Carrier to Noise Ratio (Signal Strength)
uint8_t elev; // Elevation in integer degrees
int16_t azim; // Azimuth in integer degrees
int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;
typedef struct
{
uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
uint16_t reserved2; // Reserved
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo;
enum {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_VELNED = 0x12,
MSG_SVINFO = 0x30,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_VELNED = 0x12,
MSG_SVINFO = 0x30,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24
} ubs_protocol_bytes;
@ -1005,14 +1010,14 @@ static bool _new_speed;
// Receive buffer
static union {
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
ubx_nav_svinfo svinfo;
uint8_t bytes[200];
} _buffer;
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
ubx_nav_svinfo svinfo;
uint8_t bytes[200];
} _buffer;
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
{
while (len--) {
*ck_a += *data;
@ -1113,24 +1118,24 @@ static bool UBLOX_parse_gps(void)
case MSG_VELNED:
// speed_3d = _buffer.velned.speed_3d; // cm/s
GPS_speed = _buffer.velned.speed_2d; // cm/s
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true;
break;
case MSG_SVINFO:
GPS_numCh = _buffer.svinfo.numCh;
if (GPS_numCh > 16)
GPS_numCh = 16;
for (i = 0; i < GPS_numCh; i++){
GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn;
GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid;
GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality;
GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno;
}
break;
default:
return false;
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true;
break;
case MSG_SVINFO:
GPS_numCh = _buffer.svinfo.numCh;
if (GPS_numCh > 16)
GPS_numCh = 16;
for (i = 0; i < GPS_numCh; i++){
GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn;
GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid;
GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality;
GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno;
}
break;
default:
return false;
}
// we only return true when we get new position and speed data
// this ensures we don't use stale data
if (_new_position && _new_speed) {

207
src/imu.c
View file

@ -5,10 +5,10 @@ int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
int32_t accSum[3];
uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
int16_t acc_25deg = 0;
int16_t accZ_25deg = 0;
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
int32_t baroPressureSum = 0;
uint32_t baroPressureSum = 0;
int32_t BaroAlt = 0;
int32_t sonarAlt; // to think about the unit
int32_t EstAlt; // in cm
@ -26,12 +26,13 @@ float accVelScale;
int16_t gyroData[3] = { 0, 0, 0 };
int16_t gyroZero[3] = { 0, 0, 0 };
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
float anglerad[2] = { 0, 0 }; // absolute angle inclination in radians
static void getEstimatedAttitude(void);
void imuInit(void)
{
acc_25deg = acc_1G * 0.423f;
accZ_25deg = acc_1G * cosf(RAD * 25.0f);
accVelScale = 9.80665f / acc_1G / 10000.0f;
#ifdef MAG
@ -41,42 +42,19 @@ void imuInit(void)
#endif
}
void computeIMU(void)
{
uint32_t axis;
static int16_t gyroADCprevious[3] = { 0, 0, 0 };
int16_t gyroADCp[3];
int16_t gyroADCinter[3];
static uint32_t timeInterleave = 0;
static int16_t gyroYawSmooth = 0;
Gyro_getADC();
if (sensors(SENSOR_ACC)) {
ACC_getADC();
getEstimatedAttitude();
}
Gyro_getADC();
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis];
timeInterleave = micros();
annexCode();
if ((micros() - timeInterleave) > 650) {
annex650_overrun_count++;
} else {
while ((micros() - timeInterleave) < 650); // empirical, interleaving delay between 2 consecutive reads
}
Gyro_getADC();
for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
gyroADCprevious[axis] = gyroADCinter[axis] / 2;
if (!sensors(SENSOR_ACC))
accADC[axis] = 0;
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
if (feature(FEATURE_GYRO_SMOOTHING)) {
@ -95,6 +73,9 @@ void computeIMU(void)
} else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3;
gyroYawSmooth = gyroData[YAW];
} else {
for (axis = 0; axis < 3; axis++)
gyroData[axis] = gyroADC[axis];
}
}
@ -106,24 +87,15 @@ void computeIMU(void)
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// Modified: 19/04/2011 by ziss_dm
// Version: V1.1
//
// code size deduction and tmp vector intermediate step for vector rotation computation: October 2011 by Alex
// **************************************************
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
// #define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000
typedef struct fp_vector {
float X;
float Y;
@ -137,6 +109,19 @@ typedef union {
t_fp_vector EstG;
// Normalize a vector
void normalizeV(struct fp_vector *src, struct fp_vector *dest)
{
float length;
length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
if (length != 0) {
dest->X = src->X / length;
dest->Y = src->Y / length;
dest->Z = src->Z / length;
}
}
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v, float *delta)
{
@ -147,10 +132,10 @@ void rotateV(struct fp_vector *v, float *delta)
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(-delta[PITCH]);
sinx = sinf(-delta[PITCH]);
cosy = cosf(delta[ROLL]);
siny = sinf(delta[ROLL]);
cosx = cosf(delta[ROLL]);
sinx = sinf(delta[ROLL]);
cosy = cosf(delta[PITCH]);
siny = sinf(delta[PITCH]);
cosz = cosf(delta[YAW]);
sinz = sinf(delta[YAW]);
@ -161,13 +146,13 @@ void rotateV(struct fp_vector *v, float *delta)
sinzsinx = sinx * sinz;
mat[0][0] = coszcosy;
mat[0][1] = sinz * cosy;
mat[0][2] = -siny;
mat[1][0] = (coszsinx * siny) - sinzcosx;
mat[1][1] = (sinzsinx * siny) + (coszcosx);
mat[1][2] = cosy * sinx;
mat[2][0] = (coszcosx * siny) + (sinzsinx);
mat[2][1] = (sinzcosx * siny) - (coszsinx);
mat[0][1] = -cosy * sinz;
mat[0][2] = siny;
mat[1][0] = sinzcosx + (coszsinx * siny);
mat[1][1] = coszcosx - (sinzsinx * siny);
mat[1][2] = -sinx * cosy;
mat[2][0] = (sinzsinx) - (coszcosx * siny);
mat[2][1] = (coszsinx) + (sinzcosx * siny);
mat[2][2] = cosy * cosx;
v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
@ -175,12 +160,6 @@ void rotateV(struct fp_vector *v, float *delta)
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
}
static int16_t _atan2f(float y, float x)
{
// no need for aidsy inaccurate shortcuts on a proper platform
return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
}
int32_t applyDeadband(int32_t value, int32_t deadband)
{
if (abs(value) < deadband) {
@ -201,9 +180,9 @@ void acc_calc(uint32_t deltaT)
t_fp_vector accel_ned;
// the accel values have to be rotated into the earth frame
rpy[0] = -(float) angle[ROLL] * RADX10;
rpy[1] = -(float) angle[PITCH] * RADX10;
rpy[2] = -(float) heading * RADX10 * 10;
rpy[0] = -(float)anglerad[ROLL];
rpy[1] = -(float)anglerad[PITCH];
rpy[2] = -(float)heading * RADX10 * 10.0f;
accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1];
@ -243,10 +222,6 @@ void accSum_reset(void)
accTimeSum = 0;
}
// Use original baseflight angle calculation
// #define BASEFLIGHT_CALC
static float invG;
static void getEstimatedAttitude(void)
{
uint32_t axis;
@ -257,13 +232,6 @@ static void getEstimatedAttitude(void)
uint32_t currentT = micros();
uint32_t deltaT;
float scale, deltaGyroAngle[3];
#ifndef BASEFLIGHT_CALC
float sqGZ;
float sqGX;
float sqGY;
float sqGX_sqGZ;
float invmagXZ;
#endif
deltaT = currentT - previousT;
scale = deltaT * gyro.scale;
previousT = currentT;
@ -285,11 +253,6 @@ static void getEstimatedAttitude(void)
if (sensors(SENSOR_MAG))
rotateV(&EstM.V, deltaGyroAngle);
if (abs(accSmooth[ROLL]) < acc_25deg && abs(accSmooth[PITCH]) < acc_25deg && accSmooth[YAW] > 0)
f.SMALL_ANGLES_25 = 1;
else
f.SMALL_ANGLES_25 = 0;
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
@ -303,50 +266,30 @@ static void getEstimatedAttitude(void)
EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
}
if (abs(EstG.A[Z]) > accZ_25deg)
f.SMALL_ANGLES_25 = 1;
else
f.SMALL_ANGLES_25 = 0;
// Attitude of the estimated vector
#ifdef BASEFLIGHT_CALC
// This hack removes gimbal lock (sorta) on pitch, so rolling around doesn't make pitch jump when roll reaches 90deg
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = -asinf(EstG.V.Y / -sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (180.0f / M_PI * 10.0f);
#else
// MW2.2 version
sqGZ = EstG.V.Z * EstG.V.Z;
sqGX = EstG.V.X * EstG.V.X;
sqGY = EstG.V.Y * EstG.V.Y;
sqGX_sqGZ = sqGX + sqGZ;
invmagXZ = 1.0f / sqrtf(sqGX_sqGZ);
invG = 1.0f / sqrtf(sqGX_sqGZ + sqGY);
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = _atan2f(EstG.V.Y, invmagXZ * sqGX_sqGZ);
#endif
anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));
#ifdef MAG
if (sensors(SENSOR_MAG)) {
#ifdef BASEFLIGHT_CALC
// baseflight calculation
float rollRAD = (float)angle[ROLL] * RADX10;
float pitchRAD = -(float)angle[PITCH] * RADX10;
float magX = EstM.A[1]; // Swap X/Y
float magY = EstM.A[0]; // Swap X/Y
float magZ = EstM.A[2];
float cr = cosf(rollRAD);
float sr = sinf(rollRAD);
float cp = cosf(pitchRAD);
float sp = sinf(pitchRAD);
float Xh = magX * cp + magY * sr * sp + magZ * cr * sp;
float Yh = magY * cr - magZ * sr;
float hd = (atan2f(-Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
heading = hd;
#else
// MW 2.2 calculation
heading = _atan2f(EstM.V.Z * EstG.V.X - EstM.V.X * EstG.V.Z, EstM.V.Y * invG * sqGX_sqGZ - (EstM.V.X * EstG.V.X + EstM.V.Z * EstG.V.Z) * invG * EstG.V.Y);
heading = heading + magneticDeclination;
heading = heading / 10;
#endif
if (heading > 180)
heading = heading - 360;
else if (heading < -180)
heading = heading + 360;
// baseflight calculation by Luggi09 originates from arducopter
float cosineRoll = cosf(anglerad[ROLL]);
float sineRoll = sinf(anglerad[ROLL]);
float cosinePitch = cosf(anglerad[PITCH]);
float sinePitch = sinf(anglerad[PITCH]);
float Xh = EstM.A[X] * cosinePitch + EstM.A[Y] * sineRoll * sinePitch + EstM.A[Z] * sinePitch * cosineRoll;
float Yh = EstM.A[Y] * cosineRoll - EstM.A[Z] * sineRoll;
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
heading = lrintf(hd);
if (heading < 0)
heading += 360;
}
#endif
@ -355,7 +298,7 @@ static void getEstimatedAttitude(void)
if (cfg.throttle_angle_correction) {
int cosZ = EstG.V.Z / acc_1G * 100.0f;
throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
}
}
@ -372,6 +315,9 @@ int getEstimatedAltitude(void)
int32_t baroVel;
int32_t vel_tmp;
int32_t BaroAlt_tmp;
float dt;
float PressureScaling;
float vel_acc;
static float vel = 0.0f;
static float accAlt = 0.0f;
static int32_t lastBaroAlt;
@ -388,19 +334,22 @@ int getEstimatedAltitude(void)
accAlt = 0;
}
// pressure relative to ground pressure with temperature compensation (fast!)
// baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
BaroAlt_tmp = logf(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter
BaroAlt = BaroAlt * cfg.baro_noise_lpf + BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
// calculates height from ground via baro readings
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
PressureScaling = (float)baroPressureSum / ((float)baroGroundPressure * (float)(cfg.baro_tab_size - 1));
BaroAlt_tmp = 153.8462f * (baroTemperature + 27315) * (1.0f - expf(0.190259f * logf(PressureScaling))); // in cm
BaroAlt = (float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
dt = accTimeSum * 1e-6; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
vel += (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount;
vel_acc = (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount;
// Integrator - Altitude in cm
accAlt += vel * ((float) accTimeSum * 0.0000005f); // integrate velocity to get distance (x= a/2 * t^2)
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * cfg.baro_cf_alt + (float) BaroAlt *(1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
EstAlt = accAlt;
vel += vel_acc;
#if 0
debug[0] = accSum[2] / accSumCount; // acceleration
@ -413,23 +362,23 @@ int getEstimatedAltitude(void)
//P
error = constrain(AltHold - EstAlt, -300, 300);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -150, +150);
BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -200, +200);
//I
errorAltitudeI += cfg.I8[PIDALT] * error / 64;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
BaroPID += errorAltitudeI / 512; // I in range +/-60
errorAltitudeI = constrain(errorAltitudeI, -50000, 50000);
BaroPID += errorAltitudeI / 512; // I in range +/-100
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h)
// D
vel_tmp = vel;

44
src/main.c
View file

@ -5,23 +5,22 @@ core_t core;
extern rcReadRawDataPtr rcReadRawFunc;
// two receiver read functions
// receiver read function
extern uint16_t pwmReadRawRC(uint8_t chan);
extern uint16_t spektrumReadRawRC(uint8_t chan);
#ifdef USE_LAME_PRINTF
// gcc/GNU version
static void _putc(void *p, char c)
{
uartWrite(core.mainport, c);
serialWrite(core.mainport, c);
}
#else
// keil/armcc version
int fputc(int c, FILE *f)
{
// let DMA catch up a bit when using set or dump, we're too fast.
while (!isUartTransmitEmpty(core.mainport));
uartWrite(core.mainport, c);
while (!isSerialTransmitBufferEmpty(core.mainport));
serialWrite(core.mainport, c);
return c;
}
#endif
@ -59,9 +58,9 @@ int main(void)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SPEKTRUM); // spektrum support uses UART too
pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
pwm_params.useServos = core.useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
@ -81,12 +80,20 @@ int main(void)
pwmInit(&pwm_params);
// configure PWM/CPPM read function. spektrum below will override that
// configure PWM/CPPM read function. spektrum or sbus below will override that
rcReadRawFunc = pwmReadRawRC;
if (feature(FEATURE_SPEKTRUM)) {
spektrumInit();
rcReadRawFunc = spektrumReadRawRC;
if (feature(FEATURE_SERIALRX)) {
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
spektrumInit(&rcReadRawFunc);
break;
case SERIALRX_SBUS:
sbusInit(&rcReadRawFunc);
break;
}
} else {
// spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
@ -122,13 +129,11 @@ int main(void)
if (feature(FEATURE_VBAT))
batteryInit();
serialInit(mcfg.serial_baudrate);
#ifdef SOFTSERIAL_19200_LOOPBACK
serialInit(19200);
setupSoftSerial1(19200);
uartPrint(core.mainport, "LOOPBACK 19200 ENABLED");
#else
serialInit(mcfg.serial_baudrate);
serialPort_t* loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
serialPrint(loopbackPort, "LOOPBACK 19200 ENABLED\r\n");
#endif
previousTime = micros();
@ -142,10 +147,11 @@ int main(void)
while (1) {
loop();
#ifdef SOFTSERIAL_19200_LOOPBACK
while (serialAvailable(&softSerialPorts[0])) {
while (serialTotalBytesWaiting(loopbackPort)) {
uint8_t b = serialReadByte(&softSerialPorts[0]);
uartWrite(core.mainport, b);
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
//serialWrite(core.mainport, b);
};
#endif
}

23
src/mixer.c
View file

@ -206,7 +206,16 @@ void writeServos(void)
break;
case MULTITYPE_TRI:
pwmWriteServo(0, servo[5]);
if (cfg.tri_unarmed_servo) {
// if unarmed flag set, we always move servo
pwmWriteServo(0, servo[5]);
} else {
// otherwise, only move servo when copter is armed
if (f.ARMED)
pwmWriteServo(0, servo[5]);
else
pwmWriteServo(0, 0); // kill servo signal completely.
}
break;
case MULTITYPE_AIRPLANE:
@ -265,8 +274,8 @@ static void airplaneMixer(void)
motor[0] = rcData[THROTTLE];
if (cfg.flaperons) {
}
if (cfg.flaps) {
@ -312,17 +321,17 @@ void mixTable(void)
// motors for non-servo mixes
if (numberMotor > 1)
for (i = 0; i < numberMotor; i++)
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
// airplane / servo mixes
switch (mcfg.mixerConfiguration) {
case MULTITYPE_BI:
servo[4] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT
servo[5] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT
servo[4] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT
servo[5] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT
break;
case MULTITYPE_TRI:
servo[5] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
servo[5] = constrain(cfg.tri_yaw_middle + -cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
break;
case MULTITYPE_GIMBAL:

67
src/mw.c
View file

@ -11,7 +11,6 @@ uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
int16_t headFreeModeHold;
int16_t annex650_overrun_count = 0;
uint8_t vbat; // battery voltage in 0.1V steps
int16_t telemTemperature1; // gyro sensor temperature
@ -48,17 +47,17 @@ int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for
uint16_t GPS_ground_course = 0; // degrees * 10
uint8_t GPS_Present = 0; // Checksum from Gps serial
uint8_t GPS_Enable = 0;
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[16]; // Channel number
uint8_t GPS_svinfo_svid[16]; // Satellite ID
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
// Automatic ACC Offset Calibration
uint16_t InflightcalibratingA = 0;
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[16]; // Channel number
uint8_t GPS_svinfo_svid[16]; // Satellite ID
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
// Automatic ACC Offset Calibration
uint16_t InflightcalibratingA = 0;
int16_t AccInflightCalibrationArmed;
uint16_t AccInflightCalibrationMeasurementDone = 0;
uint16_t AccInflightCalibrationSavetoEEProm = 0;
@ -85,7 +84,6 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
#define BREAKPOINT 1500
// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
void annexCode(void)
{
static uint32_t calibratedAccTime;
@ -125,6 +123,7 @@ void annexCode(void)
prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
prop1 = (uint16_t) prop1 *prop2 / 100;
} else { // YAW
tmp *= -mcfg.yaw_control_direction; //change control direction for yaw needed with new gyro orientation
if (cfg.yawdeadband) {
if (tmp > cfg.yawdeadband) {
tmp -= cfg.yawdeadband;
@ -147,7 +146,7 @@ void annexCode(void)
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if(f.HEADFREE_MODE) {
if (f.HEADFREE_MODE) {
float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
@ -437,14 +436,26 @@ void loop(void)
static uint8_t GPSNavReset = 1;
bool isThrottleLow = false;
// this will return false if spektrum is disabled. shrug.
if (spektrumFrameComplete())
computeRC();
// calculate rc stuff from serial-based receivers (spek/sbus)
if (feature(FEATURE_SERIALRX)) {
bool ready = false;
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
ready = spektrumFrameComplete();
break;
case SERIALRX_SBUS:
ready = sbusFrameComplete();
break;
}
if (ready)
computeRC();
}
if ((int32_t)(currentTime - rcTime) >= 0) { // 50Hz
rcTime = currentTime + 20000;
// TODO clean this up. computeRC should handle this check
if (!feature(FEATURE_SPEKTRUM))
if (!feature(FEATURE_SERIALRX))
computeRC();
// in 3D mode, we need to be able to disarm by switch at any time
@ -453,6 +464,14 @@ void loop(void)
mwDisarm();
}
// Read value of AUX channel as rssi
// 0 is disable, 1-4 is AUX{1..4}
if (mcfg.rssi_aux_channel > 0) {
const int16_t rssiChannelData = rcData[AUX1 + mcfg.rssi_aux_channel - 1];
// Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023];
rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f);
}
// Failsafe routine
if (feature(FEATURE_FAILSAFE)) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
@ -777,6 +796,7 @@ void loop(void)
loopTime = currentTime + mcfg.looptime;
computeIMU();
annexCode();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime);
@ -823,24 +843,23 @@ void loop(void)
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
if (abs(AltHoldCorr) > 500) {
AltHold += AltHoldCorr / 500;
AltHoldCorr %= 500;
}
errorAltitudeI = 0;
AltHold += AltHoldCorr / 2000;
AltHoldCorr %= 2000;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
AltHoldCorr = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle);
}
}
}
#endif
if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) {
rcCommand[THROTTLE]+= throttleAngleCorrection;
rcCommand[THROTTLE] += throttleAngleCorrection;
}
if (sensors(SENSOR_GPS)) {

48
src/mw.h
View file

@ -179,7 +179,7 @@ typedef struct config_t {
uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement
uint8_t throttle_angle_correction; //
uint8_t throttle_angle_correction; //
// Failsafe related configuration
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
@ -189,6 +189,7 @@ typedef struct config_t {
// mixer-related configuration
int8_t yaw_direction;
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim
uint16_t tri_yaw_min; // tail servo min
uint16_t tri_yaw_max; // tail servo max
@ -227,7 +228,7 @@ typedef struct config_t {
uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS
} config_t;
// System-wide
// System-wide
typedef struct master_t {
uint8_t version;
uint16_t size;
@ -250,7 +251,10 @@ typedef struct master_t {
uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz)
// global sensor-related stuff
int8_t align[3][3]; // acc, gyro, mag alignment (ex: with sensor output of X, Y, Z, align of 1 -3 2 would return X, -Z, Y)
sensor_align_e gyro_align; // gyro alignment
sensor_align_e acc_align; // acc alignment
sensor_align_e mag_align; // mag alignment
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter.
@ -269,17 +273,18 @@ typedef struct master_t {
// Radio/ESC-related configuration
uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order
uint8_t spektrum_hires; // spektrum high-resolution y/n (1024/2048bit)
uint8_t serialrx_type; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_SERIALRX first.
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end
uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right
uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable.
// gps-related stuff
uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2+ ??
uint32_t gps_baudrate; // GPS baudrate
// serial(uart1) baudrate
uint32_t serial_baudrate;
config_t profile[3]; // 3 separate profiles
@ -330,7 +335,7 @@ extern int16_t debug[4];
extern int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
extern int32_t accSum[3];
extern uint16_t acc_1G;
extern uint32_t accTimeSum;
extern uint32_t accTimeSum;
extern int accSumCount;
extern uint32_t currentTime;
extern uint32_t previousTime;
@ -339,10 +344,9 @@ extern uint16_t calibratingA;
extern uint16_t calibratingB;
extern uint16_t calibratingG;
extern int16_t heading;
extern int16_t annex650_overrun_count;
extern int32_t baroPressure;
extern int32_t baroTemperature;
extern int32_t baroPressureSum;
extern uint32_t baroPressureSum;
extern int32_t BaroAlt;
extern int32_t sonarAlt;
extern int32_t EstAlt;
@ -376,18 +380,18 @@ extern int16_t GPS_angle[2]; // it's the angles
extern uint16_t GPS_ground_course; // degrees*10
extern uint8_t GPS_Present; // Checksum from Gps serial
extern uint8_t GPS_Enable;
extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode
extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
extern uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID
extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode
extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
extern uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID
extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
extern core_t core;
extern master_t mcfg;
extern config_t cfg;
extern core_t core;
extern master_t mcfg;
extern config_t cfg;
extern flags_t f;
extern sensor_t acc;
extern sensor_t gyro;
@ -444,9 +448,13 @@ void featureClearAll(void);
uint32_t featureMask(void);
// spektrum
void spektrumInit(void);
void spektrumInit(rcReadRawDataPtr *callback);
bool spektrumFrameComplete(void);
// sbus
void sbusInit(rcReadRawDataPtr *callback);
bool sbusFrameComplete(void);
// buzzer
void buzzer(uint8_t warn_vbat);

34
src/printf.c
View file

@ -3,28 +3,28 @@
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* 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
* 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
*
* 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 Kustaa Nyholm or SpareTimeLabs nor the names of its
* contributors may be used to endorse or promote products derived from this software
*
* Neither the name of the Kustaa Nyholm or SpareTimeLabs 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)
*
* 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.
*/
@ -228,7 +228,7 @@ void tfp_printf(char *fmt, ...)
va_start(va, fmt);
tfp_format(stdout_putp, stdout_putf, fmt, va);
va_end(va);
while (!isUartTransmitEmpty(core.mainport));
while (!isSerialTransmitBufferEmpty(core.mainport));
}
static void putcp(void *p, char c)

74
src/printf.h
View file

@ -1,32 +1,32 @@
/*
File: printf.h
Copyright (c) 2004,2012 Kustaa Nyholm / SpareTimeLabs
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
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
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
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 Kustaa Nyholm or SpareTimeLabs nor the names of its
contributors may be used to endorse or promote products derived from this software
Neither the name of the Kustaa Nyholm or SpareTimeLabs 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)
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.
@ -34,35 +34,35 @@ OF SUCH DAMAGE.
This library is realy just two files: 'printf.h' and 'printf.c'.
They provide a simple and small (+200 loc) printf functionality to
They provide a simple and small (+200 loc) printf functionality to
be used in embedded systems.
I've found them so usefull in debugging that I do not bother with a
I've found them so usefull in debugging that I do not bother with a
debugger at all.
They are distributed in source form, so to use them, just compile them
into your project.
They are distributed in source form, so to use them, just compile them
into your project.
Two printf variants are provided: printf and sprintf.
Two printf variants are provided: printf and sprintf.
The formats supported by this implementation are: 'd' 'u' 'c' 's' 'x' 'X'.
Zero padding and field width are also supported.
If the library is compiled with 'PRINTF_SUPPORT_LONG' defined then the
If the library is compiled with 'PRINTF_SUPPORT_LONG' defined then the
long specifier is also
supported. Note that this will pull in some long math routines (pun intended!)
and thus make your executable noticably longer.
The memory foot print of course depends on the target cpu, compiler and
compiler options, but a rough guestimate (based on a H8S target) is about
1.4 kB for code and some twenty 'int's and 'char's, say 60 bytes of stack space.
Not too bad. Your milage may vary. By hacking the source code you can
get rid of some hunred bytes, I'm sure, but personally I feel the balance of
The memory foot print of course depends on the target cpu, compiler and
compiler options, but a rough guestimate (based on a H8S target) is about
1.4 kB for code and some twenty 'int's and 'char's, say 60 bytes of stack space.
Not too bad. Your milage may vary. By hacking the source code you can
get rid of some hunred bytes, I'm sure, but personally I feel the balance of
functionality and flexibility versus code size is close to optimal for
many embedded systems.
To use the printf you need to supply your own character output function,
To use the printf you need to supply your own character output function,
something like :
void putc ( void* p, char c)
@ -71,25 +71,25 @@ void putc ( void* p, char c)
SERIAL_PORT_TX_REGISTER = c;
}
Before you can call printf you need to initialize it to use your
Before you can call printf you need to initialize it to use your
character output function with something like:
init_printf(NULL,putc);
Notice the 'NULL' in 'init_printf' and the parameter 'void* p' in 'putc',
the NULL (or any pointer) you pass into the 'init_printf' will eventually be
passed to your 'putc' routine. This allows you to pass some storage space (or
anything realy) to the character output function, if necessary.
This is not often needed but it was implemented like that because it made
Notice the 'NULL' in 'init_printf' and the parameter 'void* p' in 'putc',
the NULL (or any pointer) you pass into the 'init_printf' will eventually be
passed to your 'putc' routine. This allows you to pass some storage space (or
anything realy) to the character output function, if necessary.
This is not often needed but it was implemented like that because it made
implementing the sprintf function so neat (look at the source code).
The code is re-entrant, except for the 'init_printf' function, so it
is safe to call it from interupts too, although this may result in mixed output.
The code is re-entrant, except for the 'init_printf' function, so it
is safe to call it from interupts too, although this may result in mixed output.
If you rely on re-entrancy, take care that your 'putc' function is re-entrant!
The printf and sprintf functions are actually macros that translate to
The printf and sprintf functions are actually macros that translate to
'tfp_printf' and 'tfp_sprintf'. This makes it possible
to use them along with 'stdio.h' printf's in a single source file.
to use them along with 'stdio.h' printf's in a single source file.
You just need to undef the names before you include the 'stdio.h'.
Note that these are not function like macros, so if you have variables
or struct members with these names, things will explode in your face.

108
src/sbus.c Normal file
View file

@ -0,0 +1,108 @@
#include "board.h"
#include "mw.h"
// driver for SBUS receiver using UART2
#define SBUS_MAX_CHANNEL 8
#define SBUS_FRAME_SIZE 25
#define SBUS_SYNCBYTE 0x0F
#define SBUS_ENDBYTE 0x00
#define SBUS_OFFSET 988
static bool sbusFrameDone = false;
static void sbusDataReceive(uint16_t c);
static uint16_t sbusReadRawRC(uint8_t chan);
// external vars (ugh)
extern int16_t failsafeCnt;
static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
void sbusInit(rcReadRawDataPtr *callback)
{
int b;
for (b = 0; b < SBUS_MAX_CHANNEL; b ++)
sbusChannelData[b] = 2 * (mcfg.midrc - SBUS_OFFSET);
core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, MODE_RX);
if (callback)
*callback = sbusReadRawRC;
}
struct sbus_dat
{
unsigned int chan0 : 11;
unsigned int chan1 : 11;
unsigned int chan2 : 11;
unsigned int chan3 : 11;
unsigned int chan4 : 11;
unsigned int chan5 : 11;
unsigned int chan6 : 11;
unsigned int chan7 : 11;
unsigned int chan8 : 11;
unsigned int chan9 : 11;
unsigned int chan10 : 11;
unsigned int chan11 : 11;
} __attribute__ ((__packed__));
typedef union
{
uint8_t in[SBUS_FRAME_SIZE];
struct sbus_dat msg;
} sbus_msg;
static sbus_msg sbus;
// Receive ISR callback
static void sbusDataReceive(uint16_t c)
{
uint32_t sbusTime;
static uint32_t sbusTimeLast;
static uint8_t sbusFramePosition;
sbusTime = micros();
if ((sbusTime - sbusTimeLast) > 4000)
sbusFramePosition = 0;
sbusTimeLast = sbusTime;
if (sbusFramePosition == 0 && c != SBUS_SYNCBYTE)
return;
sbusFrameDone = false; // lazy main loop didnt fetch the stuff
if (sbusFramePosition != 0)
sbus.in[sbusFramePosition - 1] = (uint8_t)c;
if (sbusFramePosition == SBUS_FRAME_SIZE - 1) {
if (sbus.in[sbusFramePosition - 1] == SBUS_ENDBYTE)
sbusFrameDone = true;
sbusFramePosition = 0;
} else {
sbusFramePosition++;
}
}
bool sbusFrameComplete(void)
{
if (sbusFrameDone) {
if (!((sbus.in[22] >> 3) & 0x0001)) { // failsave flag
failsafeCnt = 0; // clear FailSafe counter
sbusChannelData[0] = sbus.msg.chan0;
sbusChannelData[1] = sbus.msg.chan1;
sbusChannelData[2] = sbus.msg.chan2;
sbusChannelData[3] = sbus.msg.chan3;
sbusChannelData[4] = sbus.msg.chan4;
sbusChannelData[5] = sbus.msg.chan5;
sbusChannelData[6] = sbus.msg.chan6;
sbusChannelData[7] = sbus.msg.chan7;
// need more channels? No problem. Add them.
sbusFrameDone = false;
return true;
}
sbusFrameDone = false;
}
return false;
}
static uint16_t sbusReadRawRC(uint8_t chan)
{
return sbusChannelData[mcfg.rcmap[chan]] / 2 + SBUS_OFFSET;
}

94
src/sensors.c
View file

@ -50,8 +50,11 @@ void sensorsAutodetect(void)
// Accelerometer. Fuck it. Let user break shit.
retry:
switch (mcfg.acc_hardware) {
case 0: // autodetect
case 1: // ADXL345
case ACC_NONE: // disable ACC
sensorsClear(SENSOR_ACC);
break;
case ACC_DEFAULT: // autodetect
case ACC_ADXL345: // ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
if (adxl345Detect(&acc_params, &acc))
@ -59,7 +62,7 @@ retry:
if (mcfg.acc_hardware == ACC_ADXL345)
break;
; // fallthrough
case 2: // MPU6050
case ACC_MPU6050: // MPU6050
if (haveMpu6k) {
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050;
@ -68,12 +71,19 @@ retry:
}
; // fallthrough
#ifndef OLIMEXINO
case 3: // MMA8452
case ACC_MMA8452: // MMA8452
if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452;
if (mcfg.acc_hardware == ACC_MMA8452)
break;
}
; // fallthrough
case ACC_BMA280: // BMA280
if (bma280Detect(&acc)) {
accHardware = ACC_BMA280;
if (mcfg.acc_hardware == ACC_BMA280)
break;
}
#endif
}
@ -102,12 +112,12 @@ retry:
// Now time to init things, acc first
if (sensors(SENSOR_ACC))
acc.init();
acc.init(mcfg.acc_align);
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init();
gyro.init(mcfg.gyro_align);
#ifdef MAG
if (!hmc5883lDetect(mcfg.align[ALIGN_MAG]))
if (!hmc5883lDetect(mcfg.mag_align))
sensorsClear(SENSOR_MAG);
#endif
@ -147,28 +157,6 @@ void batteryInit(void)
batteryWarningVoltage = i * mcfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI
}
// ALIGN_GYRO = 0,
// ALIGN_ACCEL = 1,
// ALIGN_MAG = 2
static void alignSensors(uint8_t type, int16_t *data)
{
int i;
int16_t tmp[3];
// make a copy :(
tmp[0] = data[0];
tmp[1] = data[1];
tmp[2] = data[2];
for (i = 0; i < 3; i++) {
int8_t axis = mcfg.align[type][i];
if (axis > 0)
data[axis - 1] = tmp[i];
else
data[-axis - 1] = -tmp[i];
}
}
static void ACC_Common(void)
{
static int32_t a[3];
@ -254,12 +242,6 @@ static void ACC_Common(void)
void ACC_getADC(void)
{
acc.read(accADC);
// if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
if (mcfg.align[ALIGN_ACCEL][0])
alignSensors(ALIGN_ACCEL, accADC);
else
acc.align(accADC);
ACC_Common();
}
@ -289,7 +271,7 @@ int Baro_update(void)
return 0;
baroDeadline = currentTime;
if (state) {
baro.get_up();
baro.start_ut();
@ -346,7 +328,6 @@ static float devStandardDeviation(stdev_t *dev)
static void GYRO_Common(void)
{
int axis;
static int16_t previousGyroADC[3] = { 0, 0, 0 };
static int32_t g[3];
static stdev_t var[3];
@ -380,42 +361,25 @@ static void GYRO_Common(void)
}
calibratingG--;
}
for (axis = 0; axis < 3; axis++) {
for (axis = 0; axis < 3; axis++)
gyroADC[axis] -= gyroZero[axis];
//anti gyro glitch, limit the variation between two consecutive readings
gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
previousGyroADC[axis] = gyroADC[axis];
}
}
void Gyro_getADC(void)
{
// range: +/- 8192; +/- 2000 deg/sec
gyro.read(gyroADC);
// if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
if (mcfg.align[ALIGN_GYRO][0])
alignSensors(ALIGN_GYRO, gyroADC);
else
gyro.align(gyroADC);
GYRO_Common();
}
#ifdef MAG
static float magCal[3] = { 1.0f, 1.0f, 1.0f }; // gain for each axis, populated at sensor init
static uint8_t magInit = 0;
static void Mag_getRawADC(void)
{
// MAG driver will align itself, so no need to alignSensors()
hmc5883lRead(magADC);
}
void Mag_init(void)
{
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
LED1_ON;
hmc5883lInit(magCal);
hmc5883lInit();
LED1_OFF;
magInit = 1;
}
@ -432,11 +396,7 @@ int Mag_getADC(void)
t = currentTime + 100000;
// Read mag sensor
Mag_getRawADC();
magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
magADC[YAW] = magADC[YAW] * magCal[YAW];
hmc5883lRead(magADC);
if (f.CALIBRATE_MAG) {
tCal = t;
@ -449,9 +409,9 @@ int Mag_getADC(void)
}
if (magInit) { // we apply offset only once mag calibration is done
magADC[ROLL] -= mcfg.magZero[ROLL];
magADC[PITCH] -= mcfg.magZero[PITCH];
magADC[YAW] -= mcfg.magZero[YAW];
magADC[X] -= mcfg.magZero[X];
magADC[Y] -= mcfg.magZero[Y];
magADC[Z] -= mcfg.magZero[Z];
}
if (tCal != 0) {
@ -470,21 +430,21 @@ int Mag_getADC(void)
writeEEPROM(1, true);
}
}
return 1;
}
#endif
#ifdef SONAR
void Sonar_init(void)
void Sonar_init(void)
{
hcsr04_init(sonar_rc78);
sensorsSet(SENSOR_SONAR);
sonarAlt = 0;
}
void Sonar_update(void)
void Sonar_update(void)
{
hcsr04_get_distance(&sonarAlt);
}

78
src/serial.c
View file

@ -1,7 +1,7 @@
#include "board.h"
#include "mw.h"
// Multiwii Serial Protocol 0
// Multiwii Serial Protocol 0
#define MSP_VERSION 0
#define PLATFORM_32BIT ((uint32_t)1 << 31)
@ -50,17 +50,17 @@
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
// Additional commands that are not compatible with MultiWii
#define MSP_UID 160 //out message Unique device ID
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
#define INBUF_SIZE 64
#define MSP_UID 160 //out message Unique device ID
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
#define INBUF_SIZE 64
struct box_t {
const uint8_t boxIndex; // this is from boxnames enum
const char *boxName; // GUI-readable box name
const uint8_t permanentId; //
const uint8_t permanentId; //
} boxes[] = {
{ BOXARM, "ARM;", 0 },
{ BOXANGLE, "ANGLE;", 1 },
@ -120,7 +120,7 @@ const uint8_t boxids[] = { // permanent IDs associated to boxes. This way,
4, // "VARIO;"
5, // "MAG;"
6, // "HEADFREE;"
7, // "HEADADJ;"
7, // "HEADADJ;"
8, // "CAMSTAB;"
9, // "CAMTRIG;"
10, // "GPS HOME;"
@ -157,16 +157,16 @@ void serialize32(uint32_t a)
{
static uint8_t t;
t = a;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 8;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 16;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 24;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
}
@ -174,16 +174,16 @@ void serialize16(int16_t a)
{
static uint8_t t;
t = a;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 8 & 0xff;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
}
void serialize8(uint8_t a)
{
uartWrite(core.mainport, a);
serialWrite(core.mainport, a);
checksum ^= a;
}
@ -251,7 +251,7 @@ void serializeBoxNamesReply(void)
strcat(buf, boxes[i].boxName);
}
}
headSerialReply(strlen(buf));
for (c = buf; *c; c++)
serialize8(*c);
@ -561,7 +561,7 @@ static void evaluateCommand(void)
serialize32(lon);
serialize32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
serialize16(0); // heading will come here (deg)
serialize16(0); // time to stay (ms) will come here
serialize16(0); // time to stay (ms) will come here
serialize8(0); // nav flag will come here
break;
case MSP_SET_WP:
@ -625,22 +625,22 @@ static void evaluateCommand(void)
case MSP_UID:
headSerialReply(12);
serialize32(U_ID_0);
serialize32(U_ID_1);
serialize32(U_ID_2);
break;
case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4));
serialize8(GPS_numCh);
for (i = 0; i < GPS_numCh; i++){
serialize8(GPS_svinfo_chn[i]);
serialize8(GPS_svinfo_svid[i]);
serialize8(GPS_svinfo_quality[i]);
serialize8(GPS_svinfo_cno[i]);
}
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
serialize32(U_ID_1);
serialize32(U_ID_2);
break;
case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4));
serialize8(GPS_numCh);
for (i = 0; i < GPS_numCh; i++){
serialize8(GPS_svinfo_chn[i]);
serialize8(GPS_svinfo_svid[i]);
serialize8(GPS_svinfo_quality[i]);
serialize8(GPS_svinfo_cno[i]);
}
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
}
tailSerialReply();
}
@ -672,14 +672,14 @@ void serialCom(void)
HEADER_CMD,
} c_state = IDLE;
// in cli mode, all uart stuff goes to here. enter cli mode by sending #
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
while (isUartAvailable(core.mainport)) {
c = uartRead(core.mainport);
while (serialTotalBytesWaiting(core.mainport)) {
c = serialRead(core.mainport);
if (c_state == IDLE) {
c_state = (c == '$') ? HEADER_START : IDLE;
@ -715,7 +715,7 @@ void serialCom(void)
c_state = IDLE;
}
}
if (!cliMode && !isUartAvailable(core.telemport) && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
if (!cliMode && !serialTotalBytesWaiting(core.telemport) && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
sendTelemetry();
return;
}

39
src/spektrum.c
View file

@ -8,26 +8,35 @@
static uint8_t spek_chan_shift;
static uint8_t spek_chan_mask;
static bool rcFrameComplete = false;
static bool spekHiRes = false;
static bool spekDataIncoming = false;
volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
static void spektrumDataReceive(uint16_t c);
static uint16_t spektrumReadRawRC(uint8_t chan);
// external vars (ugh)
extern int16_t failsafeCnt;
void spektrumInit(void)
void spektrumInit(rcReadRawDataPtr *callback)
{
if (mcfg.spektrum_hires) {
// 11 bit frames
spek_chan_shift = 3;
spek_chan_mask = 0x07;
} else {
// 10 bit frames
spek_chan_shift = 2;
spek_chan_mask = 0x03;
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM2048:
// 11 bit frames
spek_chan_shift = 3;
spek_chan_mask = 0x07;
spekHiRes = true;
break;
case SERIALRX_SPEKTRUM1024:
// 10 bit frames
spek_chan_shift = 2;
spek_chan_mask = 0x03;
spekHiRes = false;
break;
}
core.rcvrport = uartOpen(USART2, spektrumDataReceive, 115200, MODE_RX);
if (callback)
*callback = spektrumReadRawRC;
}
// Receive ISR callback
@ -41,7 +50,7 @@ static void spektrumDataReceive(uint16_t c)
spekTime = micros();
spekTimeInterval = spekTime - spekTimeLast;
spekTimeLast = spekTime;
if (spekTimeInterval > 5000)
if (spekTimeInterval > 5000)
spekFramePosition = 0;
spekFrame[spekFramePosition] = (uint8_t)c;
if (spekFramePosition == SPEK_FRAME_SIZE - 1) {
@ -57,9 +66,7 @@ bool spektrumFrameComplete(void)
return rcFrameComplete;
}
// static const uint8_t spekRcChannelMap[SPEK_MAX_CHANNEL] = {1, 2, 3, 0, 4, 5, 6};
uint16_t spektrumReadRawRC(uint8_t chan)
static uint16_t spektrumReadRawRC(uint8_t chan)
{
uint16_t data;
static uint32_t spekChannelData[SPEK_MAX_CHANNEL];
@ -68,7 +75,7 @@ uint16_t spektrumReadRawRC(uint8_t chan)
if (rcFrameComplete) {
for (b = 3; b < SPEK_FRAME_SIZE; b += 2) {
uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
if (spekChannel < SPEK_MAX_CHANNEL)
if (spekChannel < SPEK_MAX_CHANNEL)
spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
}
rcFrameComplete = false;
@ -77,11 +84,11 @@ uint16_t spektrumReadRawRC(uint8_t chan)
if (chan >= SPEK_MAX_CHANNEL || !spekDataIncoming) {
data = mcfg.midrc;
} else {
if (mcfg.spektrum_hires)
if (spekHiRes)
data = 988 + (spekChannelData[mcfg.rcmap[chan]] >> 1); // 2048 mode
else
data = 988 + spekChannelData[mcfg.rcmap[chan]]; // 1024 mode
}
return data;
}

18
src/telemetry.c
View file

@ -1,4 +1,4 @@
/*
/*
* FrSky Telemetry implementation by silpstream @ rcgroups
*/
#include "board.h"
@ -50,26 +50,26 @@ extern uint8_t batteryCellCount;
static void sendDataHead(uint8_t id)
{
uartWrite(core.telemport, PROTOCOL_HEADER);
uartWrite(core.telemport, id);
serialWrite(core.telemport, PROTOCOL_HEADER);
serialWrite(core.telemport, id);
}
static void sendTelemetryTail(void)
{
uartWrite(core.telemport, PROTOCOL_TAIL);
serialWrite(core.telemport, PROTOCOL_TAIL);
}
static void serializeFrsky(uint8_t data)
{
// take care of byte stuffing
if (data == 0x5e) {
uartWrite(core.telemport, 0x5d);
uartWrite(core.telemport, 0x3e);
serialWrite(core.telemport, 0x5d);
serialWrite(core.telemport, 0x3e);
} else if (data == 0x5d) {
uartWrite(core.telemport, 0x5d);
uartWrite(core.telemport, 0x3d);
serialWrite(core.telemport, 0x5d);
serialWrite(core.telemport, 0x3d);
} else
uartWrite(core.telemport, data);
serialWrite(core.telemport, data);
}
static void serialize16(int16_t a)

12
src/utils.c Normal file
View file

@ -0,0 +1,12 @@
#include "board.h"
#include "mw.h"
int constrain(int amt, int low, int high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}

3
src/utils.h Normal file
View file

@ -0,0 +1,3 @@
#pragma once
int constrain(int amt, int low, int high);

28
support/stmloader/Makefile
View file

@ -1,14 +1,14 @@
CC = $(CROSS_COMPILE)gcc
AR = $(CROSS_COMPILE)ar
export CC
export AR
all:
$(CC) -g -o stmloader -I./ \
loader.c \
serial.c \
stmbootloader.c \
-Wall
clean:
rm -f stmloader; rm -rf stmloader.dSYM
CC = $(CROSS_COMPILE)gcc
AR = $(CROSS_COMPILE)ar
export CC
export AR
all:
$(CC) -g -o stmloader -I./ \
loader.c \
serial.c \
stmbootloader.c \
-Wall
clean:
rm -f stmloader; rm -rf stmloader.dSYM

244
support/stmloader/loader.c
View file

@ -1,122 +1,122 @@
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad 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 AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt
*/
#include "serial.h"
#include "stmbootloader.h"
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <termios.h>
#include <fcntl.h>
#include <getopt.h>
#include <stdlib.h>
#define DEFAULT_PORT "/dev/ttyUSB0"
#define DEFAULT_BAUD 115200
#define FIRMWARE_FILENAME "STM32.hex"
serialStruct_t *s;
char port[256];
unsigned int baud;
unsigned char overrideParity;
unsigned char noSendR;
char firmFile[256];
void loaderUsage(void) {
fprintf(stderr, "usage: loader <-h> <-p device_file> <-b baud_rate> <-f firmware_file> <-o> <-n>\n");
}
unsigned int loaderOptions(int argc, char **argv) {
int ch;
strncpy(port, DEFAULT_PORT, sizeof(port));
baud = DEFAULT_BAUD;
overrideParity = 0;
noSendR = 0;
strncpy(firmFile, FIRMWARE_FILENAME, sizeof(firmFile));
/* options descriptor */
static struct option longopts[] = {
{ "help", required_argument, NULL, 'h' },
{ "port", required_argument, NULL, 'p' },
{ "baud", required_argument, NULL, 's' },
{ "firm_file", required_argument, NULL, 'f' },
{ "override_parity", no_argument, NULL, 'o' },
{ "no_send_r", no_argument, NULL, 'n' },
{ NULL, 0, NULL, 0 }
};
while ((ch = getopt_long(argc, argv, "hp:b:f:o:n", longopts, NULL)) != -1)
switch (ch) {
case 'h':
loaderUsage();
exit(0);
break;
case 'p':
strncpy(port, optarg, sizeof(port));
break;
case 'b':
baud = atoi(optarg);
break;
case 'f':
strncpy(firmFile, optarg, sizeof(firmFile));
break;
case 'o':
overrideParity = 1;
break;
case 'n':
noSendR = 1;
break;
default:
loaderUsage();
return 0;
}
argc -= optind;
argv += optind;
return 1;
}
int main(int argc, char **argv) {
FILE *fw;
// init
if (!loaderOptions(argc, argv)) {
fprintf(stderr, "Init failed, aborting\n");
return 1;
}
s = initSerial(port, baud, 0);
if (!s) {
fprintf(stderr, "Cannot open serial port '%s', aborting.\n", port);
return 1;
}
fw = fopen(firmFile, "r");
if (!fw) {
fprintf(stderr, "Cannot open firmware file '%s', aborting.\n", firmFile);
return 1;
}
else {
printf("Upgrading STM on port %s from %s...\n", port, firmFile);
stmLoader(s, fw, overrideParity, noSendR);
}
return 0;
}
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad 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 AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt
*/
#include "serial.h"
#include "stmbootloader.h"
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <termios.h>
#include <fcntl.h>
#include <getopt.h>
#include <stdlib.h>
#define DEFAULT_PORT "/dev/ttyUSB0"
#define DEFAULT_BAUD 115200
#define FIRMWARE_FILENAME "STM32.hex"
serialStruct_t *s;
char port[256];
unsigned int baud;
unsigned char overrideParity;
unsigned char noSendR;
char firmFile[256];
void loaderUsage(void) {
fprintf(stderr, "usage: loader <-h> <-p device_file> <-b baud_rate> <-f firmware_file> <-o> <-n>\n");
}
unsigned int loaderOptions(int argc, char **argv) {
int ch;
strncpy(port, DEFAULT_PORT, sizeof(port));
baud = DEFAULT_BAUD;
overrideParity = 0;
noSendR = 0;
strncpy(firmFile, FIRMWARE_FILENAME, sizeof(firmFile));
/* options descriptor */
static struct option longopts[] = {
{ "help", required_argument, NULL, 'h' },
{ "port", required_argument, NULL, 'p' },
{ "baud", required_argument, NULL, 's' },
{ "firm_file", required_argument, NULL, 'f' },
{ "override_parity", no_argument, NULL, 'o' },
{ "no_send_r", no_argument, NULL, 'n' },
{ NULL, 0, NULL, 0 }
};
while ((ch = getopt_long(argc, argv, "hp:b:f:o:n", longopts, NULL)) != -1)
switch (ch) {
case 'h':
loaderUsage();
exit(0);
break;
case 'p':
strncpy(port, optarg, sizeof(port));
break;
case 'b':
baud = atoi(optarg);
break;
case 'f':
strncpy(firmFile, optarg, sizeof(firmFile));
break;
case 'o':
overrideParity = 1;
break;
case 'n':
noSendR = 1;
break;
default:
loaderUsage();
return 0;
}
argc -= optind;
argv += optind;
return 1;
}
int main(int argc, char **argv) {
FILE *fw;
// init
if (!loaderOptions(argc, argv)) {
fprintf(stderr, "Init failed, aborting\n");
return 1;
}
s = initSerial(port, baud, 0);
if (!s) {
fprintf(stderr, "Cannot open serial port '%s', aborting.\n", port);
return 1;
}
fw = fopen(firmFile, "r");
if (!fw) {
fprintf(stderr, "Cannot open firmware file '%s', aborting.\n", firmFile);
return 1;
}
else {
printf("Upgrading STM on port %s from %s...\n", port, firmFile);
stmLoader(s, fw, overrideParity, noSendR);
}
return 0;
}

358
support/stmloader/serial.c
View file

@ -1,179 +1,179 @@
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad 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 AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt
*/
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include "serial.h"
#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <stdlib.h>
#include <sys/select.h>
#include <string.h>
serialStruct_t *initSerial(const char *port, unsigned int baud, char ctsRts) {
serialStruct_t *s;
struct termios options;
unsigned int brate;
s = (serialStruct_t *)calloc(1, sizeof(serialStruct_t));
s->fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
if (s->fd == -1) {
free(s);
return 0;
}
fcntl(s->fd, F_SETFL, 0); // clear all flags on descriptor, enable direct I/O
// bzero(&options, sizeof(options));
// memset(&options, 0, sizeof(options));
tcgetattr(s->fd, &options);
#ifdef B921600
switch (baud) {
case 9600:
brate = B9600;
break;
case 19200:
brate = B19200;
break;
case 38400:
brate = B38400;
break;
case 57600:
brate = B57600;
break;
case 115200:
brate = B115200;
break;
case 230400:
brate = B230400;
break;
case 460800:
brate = B460800;
break;
case 921600:
brate = B921600;
break;
default:
brate = B115200;
break;
}
options.c_cflag = brate;
#else // APPLE
cfsetispeed(&options, baud);
cfsetospeed(&options, baud);
#endif
options.c_cflag |= CRTSCTS | CS8 | CLOCAL | CREAD;
options.c_iflag = IGNPAR;
options.c_iflag &= (~(IXON|IXOFF|IXANY)); // turn off software flow control
options.c_oflag = 0;
/* set input mode (non-canonical, no echo,...) */
options.c_lflag = 0;
options.c_cc[VTIME] = 0; /* inter-character timer unused */
options.c_cc[VMIN] = 1; /* blocking read until 1 chars received */
#ifdef CCTS_OFLOW
options.c_cflag |= CCTS_OFLOW;
#endif
if (!ctsRts)
options.c_cflag &= ~(CRTSCTS); // turn off hardware flow control
// set the new port options
tcsetattr(s->fd, TCSANOW, &options);
return s;
}
void serialFree(serialStruct_t *s) {
if (s) {
if (s->fd)
close(s->fd);
free (s);
}
}
void serialNoParity(serialStruct_t *s) {
struct termios options;
tcgetattr(s->fd, &options); // read serial port options
options.c_cflag &= ~(PARENB | CSTOPB);
tcsetattr(s->fd, TCSANOW, &options);
}
void serialEvenParity(serialStruct_t *s) {
struct termios options;
tcgetattr(s->fd, &options); // read serial port options
options.c_cflag |= (PARENB);
options.c_cflag &= ~(PARODD | CSTOPB);
tcsetattr(s->fd, TCSANOW, &options);
}
void serialWriteChar(serialStruct_t *s, unsigned char c) {
char ret;
ret = write(s->fd, &c, 1);
}
void serialWrite(serialStruct_t *s, const char *str, unsigned int len) {
char ret;
ret = write(s->fd, str, len);
}
unsigned char serialAvailable(serialStruct_t *s) {
fd_set fdSet;
struct timeval timeout;
FD_ZERO(&fdSet);
FD_SET(s->fd, &fdSet);
memset((char *)&timeout, 0, sizeof(timeout));
if (select(s->fd+1, &fdSet, 0, 0, &timeout) == 1)
return 1;
else
return 0;
}
void serialFlush(serialStruct_t *s) {
while (serialAvailable(s))
serialRead(s);
}
unsigned char serialRead(serialStruct_t *s) {
char ret;
unsigned char c;
ret = read(s->fd, &c, 1);
return c;
}
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad 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 AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt
*/
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include "serial.h"
#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <stdlib.h>
#include <sys/select.h>
#include <string.h>
serialStruct_t *initSerial(const char *port, unsigned int baud, char ctsRts) {
serialStruct_t *s;
struct termios options;
unsigned int brate;
s = (serialStruct_t *)calloc(1, sizeof(serialStruct_t));
s->fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
if (s->fd == -1) {
free(s);
return 0;
}
fcntl(s->fd, F_SETFL, 0); // clear all flags on descriptor, enable direct I/O
// bzero(&options, sizeof(options));
// memset(&options, 0, sizeof(options));
tcgetattr(s->fd, &options);
#ifdef B921600
switch (baud) {
case 9600:
brate = B9600;
break;
case 19200:
brate = B19200;
break;
case 38400:
brate = B38400;
break;
case 57600:
brate = B57600;
break;
case 115200:
brate = B115200;
break;
case 230400:
brate = B230400;
break;
case 460800:
brate = B460800;
break;
case 921600:
brate = B921600;
break;
default:
brate = B115200;
break;
}
options.c_cflag = brate;
#else // APPLE
cfsetispeed(&options, baud);
cfsetospeed(&options, baud);
#endif
options.c_cflag |= CRTSCTS | CS8 | CLOCAL | CREAD;
options.c_iflag = IGNPAR;
options.c_iflag &= (~(IXON|IXOFF|IXANY)); // turn off software flow control
options.c_oflag = 0;
/* set input mode (non-canonical, no echo,...) */
options.c_lflag = 0;
options.c_cc[VTIME] = 0; /* inter-character timer unused */
options.c_cc[VMIN] = 1; /* blocking read until 1 chars received */
#ifdef CCTS_OFLOW
options.c_cflag |= CCTS_OFLOW;
#endif
if (!ctsRts)
options.c_cflag &= ~(CRTSCTS); // turn off hardware flow control
// set the new port options
tcsetattr(s->fd, TCSANOW, &options);
return s;
}
void serialFree(serialStruct_t *s) {
if (s) {
if (s->fd)
close(s->fd);
free (s);
}
}
void serialNoParity(serialStruct_t *s) {
struct termios options;
tcgetattr(s->fd, &options); // read serial port options
options.c_cflag &= ~(PARENB | CSTOPB);
tcsetattr(s->fd, TCSANOW, &options);
}
void serialEvenParity(serialStruct_t *s) {
struct termios options;
tcgetattr(s->fd, &options); // read serial port options
options.c_cflag |= (PARENB);
options.c_cflag &= ~(PARODD | CSTOPB);
tcsetattr(s->fd, TCSANOW, &options);
}
void serialWriteChar(serialStruct_t *s, unsigned char c) {
char ret;
ret = write(s->fd, &c, 1);
}
void serialWrite(serialStruct_t *s, const char *str, unsigned int len) {
char ret;
ret = write(s->fd, str, len);
}
unsigned char serialAvailable(serialStruct_t *s) {
fd_set fdSet;
struct timeval timeout;
FD_ZERO(&fdSet);
FD_SET(s->fd, &fdSet);
memset((char *)&timeout, 0, sizeof(timeout));
if (select(s->fd+1, &fdSet, 0, 0, &timeout) == 1)
return 1;
else
return 0;
}
void serialFlush(serialStruct_t *s) {
while (serialAvailable(s))
serialRead(s);
}
unsigned char serialRead(serialStruct_t *s) {
char ret;
unsigned char c;
ret = read(s->fd, &c, 1);
return c;
}

76
support/stmloader/serial.h
View file

@ -1,38 +1,38 @@
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad 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 AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt
*/
#ifndef _serial_h
#define _serial_h
#define INPUT_BUFFER_SIZE 1024
typedef struct {
int fd;
} serialStruct_t;
extern serialStruct_t *initSerial(const char *port, unsigned int baud, char ctsRts);
extern void serialWrite(serialStruct_t *s, const char *str, unsigned int len);
extern void serialWriteChar(serialStruct_t *s, unsigned char c);
extern unsigned char serialAvailable(serialStruct_t *s);
extern void serialFlush(serialStruct_t *s);
extern unsigned char serialRead(serialStruct_t *s);
extern void serialEvenParity(serialStruct_t *s);
extern void serialNoParity(serialStruct_t *s);
extern void serialFree(serialStruct_t *s);
#endif
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad 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 AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt
*/
#ifndef _serial_h
#define _serial_h
#define INPUT_BUFFER_SIZE 1024
typedef struct {
int fd;
} serialStruct_t;
extern serialStruct_t *initSerial(const char *port, unsigned int baud, char ctsRts);
extern void serialWrite(serialStruct_t *s, const char *str, unsigned int len);
extern void serialWriteChar(serialStruct_t *s, unsigned char c);
extern unsigned char serialAvailable(serialStruct_t *s);
extern void serialFlush(serialStruct_t *s);
extern unsigned char serialRead(serialStruct_t *s);
extern void serialEvenParity(serialStruct_t *s);
extern void serialNoParity(serialStruct_t *s);
extern void serialFree(serialStruct_t *s);
#endif

686
support/stmloader/stmbootloader.c
View file

@ -1,343 +1,343 @@
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad 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 AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt
*/
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include "serial.h"
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <ctype.h>
#define STM_RETRIES_SHORT 1000
#define STM_RETRIES_LONG 50000
unsigned char getResults[11];
unsigned char stmHexToChar(const char *hex) {
char hex1, hex2;
unsigned char nibble1, nibble2;
// force data to upper case
hex1 = toupper(hex[0]);
hex2 = toupper(hex[1]);
if (hex1 < 65)
nibble1 = hex1 - 48;
else
nibble1 = hex1 - 55;
if (hex2 < 65)
nibble2 = hex2 - 48;
else
nibble2 = hex2 - 55;
return (nibble1 << 4 | nibble2);
}
unsigned char stmWaitAck(serialStruct_t *s, int retries) {
unsigned char c;
unsigned int i;
for (i = 0; i < retries; i++) {
if (serialAvailable(s)) {
c = serialRead(s);
if (c == 0x79) {
// putchar('+'); fflush(stdout);
return 1;
}
if (c == 0x1f) {
putchar('-'); fflush(stdout);
return 0;
}
else {
printf("?%x?", c); fflush(stdout);
return 0;
}
}
usleep(500);
}
return 0;
}
unsigned char stmWrite(serialStruct_t *s, const char *hex) {
unsigned char c;
unsigned char ck;
unsigned char i;
ck = 0;
i = 0;
while (*hex) {
c = stmHexToChar(hex);
serialWrite(s, (char *)&c, 1);
ck ^= c;
hex += 2;
i++;
}
if (i == 1)
ck = 0xff ^ c;
// send checksum
serialWrite(s, (char *)&ck, 1);
return stmWaitAck(s, STM_RETRIES_LONG);
}
void stmWriteCommand(serialStruct_t *s, char *msb, char *lsb, char *len, char *data) {
char startAddress[9];
char lenPlusData[128];
char c;
strncpy(startAddress, msb, sizeof(startAddress));
strcat(startAddress, lsb);
sprintf(lenPlusData, "%02x%s", stmHexToChar(len) - 1, data);
write:
// send WRITE MEMORY command
do {
c = getResults[5];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
// send address
if (!stmWrite(s, startAddress)) {
putchar('A');
goto write;
}
// send len + data
if (!stmWrite(s, lenPlusData)) {
putchar('D');
goto write;
}
putchar('='); fflush(stdout);
}
char *stmHexLoader(serialStruct_t *s, FILE *fp) {
char hexByteCount[3], hexAddressLSB[5], hexRecordType[3], hexData[128];
char addressMSB[5];
static char addressJump[9];
// bzero(addressJump, sizeof(addressJump));
// bzero(addressMSB, sizeof(addressMSB));
memset(addressJump, 0, sizeof(addressJump));
memset(addressMSB, 0, sizeof(addressMSB));
while (fscanf(fp, ":%2s%4s%2s%s\n", hexByteCount, hexAddressLSB, hexRecordType, hexData) != EOF) {
unsigned int byteCount, addressLSB, recordType;
recordType = stmHexToChar(hexRecordType);
hexData[stmHexToChar(hexByteCount) * 2] = 0; // terminate at CHKSUM
// printf("Record Type: %d\n", recordType);
switch (recordType) {
case 0x00:
stmWriteCommand(s, addressMSB, hexAddressLSB, hexByteCount, hexData);
break;
case 0x01:
// EOF
return addressJump;
break;
case 0x04:
// MSB of destination 32 bit address
strncpy(addressMSB, hexData, 4);
break;
case 0x05:
// 32 bit address to run after load
strncpy(addressJump, hexData, 8);
break;
}
}
return 0;
}
void stmLoader(serialStruct_t *s, FILE *fp, unsigned char overrideParity, unsigned char noSendR) {
char c;
unsigned char b1, b2, b3;
unsigned char i, n;
char *jumpAddress;
// turn on parity generation
if (!overrideParity)
serialEvenParity(s);
if(!noSendR) {
top:
printf("Sending R to place Baseflight in bootloader, press a key to continue");
serialFlush(s);
c = 'R';
serialWrite(s, &c, 1);
getchar();
printf("\n");
}
serialFlush(s);
printf("Poking the MCU to check whether bootloader is alive...");
// poke the MCU
do {
printf("p"); fflush(stdout);
c = 0x7f;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_SHORT));
printf("STM bootloader alive...\n");
// send GET command
do {
c = 0x00;
serialWrite(s, &c, 1);
c = 0xff;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
b1 = serialRead(s); // number of bytes
b2 = serialRead(s); // bootloader version
for (i = 0; i < b1; i++)
getResults[i] = serialRead(s);
stmWaitAck(s, STM_RETRIES_LONG);
printf("Received commands.\n");
// send GET VERSION command
do {
c = getResults[1];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
b1 = serialRead(s);
b2 = serialRead(s);
b3 = serialRead(s);
stmWaitAck(s, STM_RETRIES_LONG);
printf("STM Bootloader version: %d.%d\n", (b1 & 0xf0) >> 4, (b1 & 0x0f));
// send GET ID command
do {
c = getResults[2];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
n = serialRead(s);
printf("STM Device ID: 0x");
for (i = 0; i <= n; i++) {
b1 = serialRead(s);
printf("%02x", b1);
}
stmWaitAck(s, STM_RETRIES_LONG);
printf("\n");
/*
flash_size:
// read Flash size
c = getResults[3];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
// if read not allowed, unprotect (which also erases)
if (!stmWaitAck(s, STM_RETRIES_LONG)) {
// unprotect command
do {
c = getResults[10];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
// wait for results
if (stmWaitAck(s, STM_RETRIES_LONG))
goto top;
}
// send address
if (!stmWrite(s, "1FFFF7E0"))
goto flash_size;
// send # bytes (N-1 = 1)
if (!stmWrite(s, "01"))
goto flash_size;
b1 = serialRead(s);
b2 = serialRead(s);
printf("STM Flash Size: %dKB\n", b2<<8 | b1);
*/
// erase flash
erase_flash:
printf("Global flash erase [command 0x%x]...", getResults[6]); fflush(stdout);
do {
c = getResults[6];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
// global erase
if (getResults[6] == 0x44) {
// mass erase
if (!stmWrite(s, "FFFF"))
goto erase_flash;
}
else {
c = 0xff;
serialWrite(s, &c, 1);
c = 0x00;
serialWrite(s, &c, 1);
if (!stmWaitAck(s, STM_RETRIES_LONG))
goto erase_flash;
}
printf("Done.\n");
// upload hex file
printf("Flashing device...\n");
jumpAddress = stmHexLoader(s, fp);
if (jumpAddress) {
printf("\nFlash complete, cycle power\n");
go:
// send GO command
do {
c = getResults[4];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
// send address
if (!stmWrite(s, jumpAddress))
goto go;
}
else {
printf("\nFlash complete.\n");
}
}
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad 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 AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt
*/
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include "serial.h"
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <ctype.h>
#define STM_RETRIES_SHORT 1000
#define STM_RETRIES_LONG 50000
unsigned char getResults[11];
unsigned char stmHexToChar(const char *hex) {
char hex1, hex2;
unsigned char nibble1, nibble2;
// force data to upper case
hex1 = toupper(hex[0]);
hex2 = toupper(hex[1]);
if (hex1 < 65)
nibble1 = hex1 - 48;
else
nibble1 = hex1 - 55;
if (hex2 < 65)
nibble2 = hex2 - 48;
else
nibble2 = hex2 - 55;
return (nibble1 << 4 | nibble2);
}
unsigned char stmWaitAck(serialStruct_t *s, int retries) {
unsigned char c;
unsigned int i;
for (i = 0; i < retries; i++) {
if (serialAvailable(s)) {
c = serialRead(s);
if (c == 0x79) {
// putchar('+'); fflush(stdout);
return 1;
}
if (c == 0x1f) {
putchar('-'); fflush(stdout);
return 0;
}
else {
printf("?%x?", c); fflush(stdout);
return 0;
}
}
usleep(500);
}
return 0;
}
unsigned char stmWrite(serialStruct_t *s, const char *hex) {
unsigned char c;
unsigned char ck;
unsigned char i;
ck = 0;
i = 0;
while (*hex) {
c = stmHexToChar(hex);
serialWrite(s, (char *)&c, 1);
ck ^= c;
hex += 2;
i++;
}
if (i == 1)
ck = 0xff ^ c;
// send checksum
serialWrite(s, (char *)&ck, 1);
return stmWaitAck(s, STM_RETRIES_LONG);
}
void stmWriteCommand(serialStruct_t *s, char *msb, char *lsb, char *len, char *data) {
char startAddress[9];
char lenPlusData[128];
char c;
strncpy(startAddress, msb, sizeof(startAddress));
strcat(startAddress, lsb);
sprintf(lenPlusData, "%02x%s", stmHexToChar(len) - 1, data);
write:
// send WRITE MEMORY command
do {
c = getResults[5];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
// send address
if (!stmWrite(s, startAddress)) {
putchar('A');
goto write;
}
// send len + data
if (!stmWrite(s, lenPlusData)) {
putchar('D');
goto write;
}
putchar('='); fflush(stdout);
}
char *stmHexLoader(serialStruct_t *s, FILE *fp) {
char hexByteCount[3], hexAddressLSB[5], hexRecordType[3], hexData[128];
char addressMSB[5];
static char addressJump[9];
// bzero(addressJump, sizeof(addressJump));
// bzero(addressMSB, sizeof(addressMSB));
memset(addressJump, 0, sizeof(addressJump));
memset(addressMSB, 0, sizeof(addressMSB));
while (fscanf(fp, ":%2s%4s%2s%s\n", hexByteCount, hexAddressLSB, hexRecordType, hexData) != EOF) {
unsigned int byteCount, addressLSB, recordType;
recordType = stmHexToChar(hexRecordType);
hexData[stmHexToChar(hexByteCount) * 2] = 0; // terminate at CHKSUM
// printf("Record Type: %d\n", recordType);
switch (recordType) {
case 0x00:
stmWriteCommand(s, addressMSB, hexAddressLSB, hexByteCount, hexData);
break;
case 0x01:
// EOF
return addressJump;
break;
case 0x04:
// MSB of destination 32 bit address
strncpy(addressMSB, hexData, 4);
break;
case 0x05:
// 32 bit address to run after load
strncpy(addressJump, hexData, 8);
break;
}
}
return 0;
}
void stmLoader(serialStruct_t *s, FILE *fp, unsigned char overrideParity, unsigned char noSendR) {
char c;
unsigned char b1, b2, b3;
unsigned char i, n;
char *jumpAddress;
// turn on parity generation
if (!overrideParity)
serialEvenParity(s);
if(!noSendR) {
top:
printf("Sending R to place Baseflight in bootloader, press a key to continue");
serialFlush(s);
c = 'R';
serialWrite(s, &c, 1);
getchar();
printf("\n");
}
serialFlush(s);
printf("Poking the MCU to check whether bootloader is alive...");
// poke the MCU
do {
printf("p"); fflush(stdout);
c = 0x7f;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_SHORT));
printf("STM bootloader alive...\n");
// send GET command
do {
c = 0x00;
serialWrite(s, &c, 1);
c = 0xff;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
b1 = serialRead(s); // number of bytes
b2 = serialRead(s); // bootloader version
for (i = 0; i < b1; i++)
getResults[i] = serialRead(s);
stmWaitAck(s, STM_RETRIES_LONG);
printf("Received commands.\n");
// send GET VERSION command
do {
c = getResults[1];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
b1 = serialRead(s);
b2 = serialRead(s);
b3 = serialRead(s);
stmWaitAck(s, STM_RETRIES_LONG);
printf("STM Bootloader version: %d.%d\n", (b1 & 0xf0) >> 4, (b1 & 0x0f));
// send GET ID command
do {
c = getResults[2];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
n = serialRead(s);
printf("STM Device ID: 0x");
for (i = 0; i <= n; i++) {
b1 = serialRead(s);
printf("%02x", b1);
}
stmWaitAck(s, STM_RETRIES_LONG);
printf("\n");
/*
flash_size:
// read Flash size
c = getResults[3];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
// if read not allowed, unprotect (which also erases)
if (!stmWaitAck(s, STM_RETRIES_LONG)) {
// unprotect command
do {
c = getResults[10];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
// wait for results
if (stmWaitAck(s, STM_RETRIES_LONG))
goto top;
}
// send address
if (!stmWrite(s, "1FFFF7E0"))
goto flash_size;
// send # bytes (N-1 = 1)
if (!stmWrite(s, "01"))
goto flash_size;
b1 = serialRead(s);
b2 = serialRead(s);
printf("STM Flash Size: %dKB\n", b2<<8 | b1);
*/
// erase flash
erase_flash:
printf("Global flash erase [command 0x%x]...", getResults[6]); fflush(stdout);
do {
c = getResults[6];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
// global erase
if (getResults[6] == 0x44) {
// mass erase
if (!stmWrite(s, "FFFF"))
goto erase_flash;
}
else {
c = 0xff;
serialWrite(s, &c, 1);
c = 0x00;
serialWrite(s, &c, 1);
if (!stmWaitAck(s, STM_RETRIES_LONG))
goto erase_flash;
}
printf("Done.\n");
// upload hex file
printf("Flashing device...\n");
jumpAddress = stmHexLoader(s, fp);
if (jumpAddress) {
printf("\nFlash complete, cycle power\n");
go:
// send GO command
do {
c = getResults[4];
serialWrite(s, &c, 1);
c = 0xff ^ c;
serialWrite(s, &c, 1);
} while (!stmWaitAck(s, STM_RETRIES_LONG));
// send address
if (!stmWrite(s, jumpAddress))
goto go;
}
else {
printf("\nFlash complete.\n");
}
}

54
support/stmloader/stmbootloader.h
View file

@ -1,27 +1,27 @@
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad 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 AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt
*/
#ifndef _stmbootloader_h
#define _stmbootloader_h
#include <stdio.h>
#include "serial.h"
extern void stmLoader(serialStruct_t *s, FILE *fp, unsigned char overrideParity, unsigned char noSendR);
#endif
/*
This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
AutoQuad 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 AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt
*/
#ifndef _stmbootloader_h
#define _stmbootloader_h
#include <stdio.h>
#include "serial.h"
extern void stmLoader(serialStruct_t *s, FILE *fp, unsigned char overrideParity, unsigned char noSendR);
#endif