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

Changing all line endings to WINDOWS line endings (CR+LF) and removing all End-Of-Line whitespace and using spaces instead of tabs. Please ensure you configure your editors and tools to follow suit. If using git please enable autocrlf in your .git/config file.

Browse source 
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@393 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
dominicc1974@gmail.com 2013-09-06 23:14:48 +00:00
parent 929bbc8c3f
commit 4c191270bf
41 changed files with 2000 additions and 2000 deletions
Download patch file Download diff file Expand all files Collapse all files

454
Makefile
View file

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

36
src/board.h
View file

@ -195,15 +195,15 @@ typedef struct baro_t
#include "drv_spi.h" #include "drv_spi.h"
#include "drv_adxl345.h" #include "drv_adxl345.h"
#include "drv_mpu3050.h" #include "drv_mpu3050.h"
#include "drv_mpu6050.h" #include "drv_mpu6050.h"
#include "drv_l3g4200d.h" #include "drv_l3g4200d.h"
#include "drv_pwm.h" #include "drv_pwm.h"
#include "drv_timer.h" #include "drv_timer.h"
#include "drv_uart.h" #include "drv_uart.h"
#include "drv_softserial.h" #include "drv_softserial.h"
#else #else
// AfroFlight32 // AfroFlight32
#include "drv_adc.h" #include "drv_adc.h"
#include "drv_adxl345.h" #include "drv_adxl345.h"
#include "drv_bmp085.h" #include "drv_bmp085.h"
@ -214,13 +214,13 @@ typedef struct baro_t
#include "drv_ledring.h" #include "drv_ledring.h"
#include "drv_mma845x.h" #include "drv_mma845x.h"
#include "drv_mpu3050.h" #include "drv_mpu3050.h"
#include "drv_mpu6050.h" #include "drv_mpu6050.h"
#include "drv_l3g4200d.h" #include "drv_l3g4200d.h"
#include "drv_pwm.h" #include "drv_pwm.h"
#include "drv_timer.h" #include "drv_timer.h"
#include "drv_uart.h" #include "drv_uart.h"
#include "drv_softserial.h" #include "drv_softserial.h"
#include "drv_hcsr04.h" #include "drv_hcsr04.h"
#endif #endif
#endif #endif

24
src/buzzer.c
View file

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

4
src/cli.c
View file

@ -574,7 +574,7 @@ static void cliDump(char *cmdline)
if (yaw < 0) if (yaw < 0)
printf(" "); printf(" ");
printf("%s\r\n", ftoa(yaw, buf)); printf("%s\r\n", ftoa(yaw, buf));
} }
printf("cmix %d 0 0 0 0\r\n", i + 1); printf("cmix %d 0 0 0 0\r\n", i + 1);
} }
@ -677,7 +677,7 @@ static void cliHelp(char *cmdline)
{ {
uint32_t i = 0; uint32_t i = 0;
cliPrint("Available commands:\r\n"); cliPrint("Available commands:\r\n");
for (i = 0; i < CMD_COUNT; i++) for (i = 0; i < CMD_COUNT; i++)
printf("%s\t%s\r\n", cmdTable[i].name, cmdTable[i].param); printf("%s\t%s\r\n", cmdTable[i].name, cmdTable[i].param);
} }

2
src/config.c
View file

@ -133,7 +133,7 @@ retry:
} }
} }
FLASH_Lock(); FLASH_Lock();
// Flash write failed - just die now // Flash write failed - just die now
if (tries == 3 || !validEEPROM()) { if (tries == 3 || !validEEPROM()) {
failureMode(10); failureMode(10);

4
src/drv_bmp085.c
View file

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

4
src/drv_gpio.h
View file

@ -15,7 +15,7 @@ typedef enum
typedef enum typedef enum
{ {
Speed_10MHz = 1, Speed_10MHz = 1,
Speed_2MHz, Speed_2MHz,
Speed_50MHz Speed_50MHz
} GPIO_Speed; } GPIO_Speed;
@ -46,7 +46,7 @@ typedef struct
GPIO_Mode mode; GPIO_Mode mode;
GPIO_Speed speed; GPIO_Speed speed;
} gpio_config_t; } gpio_config_t;
#define digitalHi(p, i) { p->BSRR = i; } #define digitalHi(p, i) { p->BSRR = i; }
#define digitalLo(p, i) { p->BRR = i; } #define digitalLo(p, i) { p->BRR = i; }
#define digitalToggle(p, i) { p->ODR ^= 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; EXTI_InitTypeDef EXTIInit;
// enable AFIO for EXTI support - already done is drv_system.c // 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) { switch(config) {
case sonar_pwm56: case sonar_pwm56:
@ -69,7 +69,7 @@ void hcsr04_init(sonar_config_t config)
exti_pin_source = GPIO_PinSource9; exti_pin_source = GPIO_PinSource9;
exti_irqn = EXTI9_5_IRQn; exti_irqn = EXTI9_5_IRQn;
break; break;
case sonar_rc78: case sonar_rc78:
trigger_pin = Pin_0; // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) 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 ) echo_pin = Pin_1; // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
exti_line = EXTI_Line1; exti_line = EXTI_Line1;
@ -78,7 +78,7 @@ void hcsr04_init(sonar_config_t config)
break; break;
} }
// tp - trigger pin // tp - trigger pin
gpio.pin = trigger_pin; gpio.pin = trigger_pin;
gpio.mode = Mode_Out_PP; gpio.mode = Mode_Out_PP;
gpio.speed = Speed_2MHz; gpio.speed = Speed_2MHz;
@ -93,12 +93,12 @@ void hcsr04_init(sonar_config_t config)
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, exti_pin_source); GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, exti_pin_source);
EXTI_ClearITPendingBit(exti_line); EXTI_ClearITPendingBit(exti_line);
EXTIInit.EXTI_Line = exti_line; EXTIInit.EXTI_Line = exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt; EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling; EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
EXTIInit.EXTI_LineCmd = ENABLE; EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit); EXTI_Init(&EXTIInit);
NVIC_EnableIRQ(exti_irqn); NVIC_EnableIRQ(exti_irqn);

2
src/drv_hmc5883l.c
View file

@ -56,7 +56,7 @@ void hmc5883lInit(float *calibrationGain)
delay(50); delay(50);
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias 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. // 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, 2 << 5); // Set the Gain
delay(100); delay(100);

4
src/drv_ledring.c
View file

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

2
src/drv_mpu6050.c
View file

@ -649,7 +649,7 @@ static bool mpu6050DmpFifoReady(void)
uint8_t buf[2]; uint8_t buf[2];
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf); i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
dmp_fifoCountL = buf[1]; dmp_fifoCountL = buf[1];
dmpFifoLevel = buf[0] << 8 | buf[1]; dmpFifoLevel = buf[0] << 8 | buf[1];

12
src/drv_ms5611.c
View file

@ -108,18 +108,18 @@ static int8_t ms5611_crc(uint16_t *prom)
return -1; return -1;
for (i = 0; i < 16; i++) { for (i = 0; i < 16; i++) {
if (i & 1) if (i & 1)
res ^= ((prom[i >> 1]) & 0x00FF); res ^= ((prom[i >> 1]) & 0x00FF);
else else
res ^= (prom[i >> 1] >> 8); res ^= (prom[i >> 1] >> 8);
for (j = 8; j > 0; j--) { for (j = 8; j > 0; j--) {
if (res & 0x8000) if (res & 0x8000)
res ^= 0x1800; res ^= 0x1800;
res <<= 1; res <<= 1;
} }
} }
prom[7] |= crc; prom[7] |= crc;
if (crc == ((res >> 12) & 0xF)) if (crc == ((res >> 12) & 0xF))
return 0; return 0;
return -1; return -1;
@ -162,7 +162,7 @@ static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
int64_t sens = ((uint32_t)ms5611_c[1] << 15) + ((dT * ms5611_c[3]) >> 8); int64_t sens = ((uint32_t)ms5611_c[1] << 15) + ((dT * ms5611_c[3]) >> 8);
temp = 2000 + ((dT * ms5611_c[6]) >> 23); 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 = temp - 2000;
delt = 5 * delt * delt; delt = 5 * delt * delt;
off2 = delt >> 1; off2 = delt >> 1;
@ -177,7 +177,7 @@ static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
off -= off2; off -= off2;
sens -= sens2; sens -= sens2;
press = (((ms5611_up * sens ) >> 21) - off) >> 15; press = (((ms5611_up * sens ) >> 21) - off) >> 15;
if (pressure) if (pressure)
*pressure = press; *pressure = press;
if (temperature) if (temperature)

234
src/drv_pwm.c
View file

@ -1,14 +1,14 @@
#include "board.h" #include "board.h"
#define PULSE_1MS (1000) // 1ms pulse width #define PULSE_1MS (1000) // 1ms pulse width
/* /*
Configuration maps: Configuration maps:
1) multirotor PPM input 1) multirotor PPM input
PWM1 used for PPM PWM1 used for PPM
PWM5..8 used for motors PWM5..8 used for motors
PWM9..10 used for servo or else motors PWM9..10 used for servo or else motors
PWM11..14 used for motors PWM11..14 used for motors
2) multirotor PPM input with more servos 2) multirotor PPM input with more servos
@ -28,19 +28,19 @@
PWM11.14 used for servos PWM11.14 used for servos
4) airplane / flying wing with PPM 4) airplane / flying wing with PPM
PWM1 used for PPM PWM1 used for PPM
PWM5..8 used for servos PWM5..8 used for servos
PWM9 used for motor throttle +PWM10 for 2nd motor PWM9 used for motor throttle +PWM10 for 2nd motor
PWM11.14 used for servos PWM11.14 used for servos
*/ */
typedef struct { typedef struct {
volatile uint16_t *ccr; volatile uint16_t *ccr;
uint16_t period; uint16_t period;
// for input only // for input only
uint8_t channel; uint8_t channel;
uint8_t state; uint8_t state;
uint16_t rise; uint16_t rise;
uint16_t fall; uint16_t fall;
uint16_t capture; uint16_t capture;
@ -134,47 +134,47 @@ static const uint8_t * const hardwareMaps[] = {
multiPWM, multiPWM,
multiPPM, multiPPM,
airPWM, airPWM,
airPPM, airPPM,
}; };
#define PWM_TIMER_MHZ 1 #define PWM_TIMER_MHZ 1
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value) static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
{ {
TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
TIM_OCInitStructure.TIM_Pulse = value; TIM_OCInitStructure.TIM_Pulse = value;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
switch (channel) { switch (channel) {
case TIM_Channel_1: case TIM_Channel_1:
TIM_OC1Init(tim, &TIM_OCInitStructure); TIM_OC1Init(tim, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable);
break; break;
case TIM_Channel_2: case TIM_Channel_2:
TIM_OC2Init(tim, &TIM_OCInitStructure); TIM_OC2Init(tim, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable);
break; break;
case TIM_Channel_3: case TIM_Channel_3:
TIM_OC3Init(tim, &TIM_OCInitStructure); TIM_OC3Init(tim, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable); TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable);
break; break;
case TIM_Channel_4: case TIM_Channel_4:
TIM_OC4Init(tim, &TIM_OCInitStructure); TIM_OC4Init(tim, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable);
break; break;
} }
} }
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
{ {
TIM_ICInitTypeDef TIM_ICInitStructure; TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = channel; TIM_ICInitStructure.TIM_Channel = channel;
TIM_ICInitStructure.TIM_ICPolarity = polarity; TIM_ICInitStructure.TIM_ICPolarity = polarity;
@ -195,13 +195,13 @@ static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
gpioInit(gpio, &cfg); gpioInit(gpio, &cfg);
} }
static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value) static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value)
{ {
pwmPortData_t *p = &pwmPorts[port]; pwmPortData_t *p = &pwmPorts[port];
configTimeBase(timerHardware[port].tim, period, PWM_TIMER_MHZ); configTimeBase(timerHardware[port].tim, period, PWM_TIMER_MHZ);
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP); pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP);
pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value); pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value);
// Needed only on TIM1 // Needed only on TIM1
if (timerHardware[port].outputEnable) if (timerHardware[port].outputEnable)
TIM_CtrlPWMOutputs(timerHardware[port].tim, ENABLE); TIM_CtrlPWMOutputs(timerHardware[port].tim, ENABLE);
TIM_Cmd(timerHardware[port].tim, ENABLE); TIM_Cmd(timerHardware[port].tim, ENABLE);
@ -217,32 +217,32 @@ static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value
p->ccr = &timerHardware[port].tim->CCR3; p->ccr = &timerHardware[port].tim->CCR3;
break; break;
case TIM_Channel_4: case TIM_Channel_4:
p->ccr = &timerHardware[port].tim->CCR4; p->ccr = &timerHardware[port].tim->CCR4;
break; break;
} }
return p; return p;
} }
static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uint8_t channel) static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uint8_t channel)
{ {
pwmPortData_t *p = &pwmPorts[port]; pwmPortData_t *p = &pwmPorts[port];
const timerHardware_t *timerHardwarePtr = &(timerHardware[port]); const timerHardware_t *timerHardwarePtr = &(timerHardware[port]);
p->channel = channel; p->channel = channel;
pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPD); pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPD);
pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
timerInConfig(timerHardwarePtr, 0xFFFF, PWM_TIMER_MHZ); timerInConfig(timerHardwarePtr, 0xFFFF, PWM_TIMER_MHZ);
configureTimerCaptureCompareInterrupt(timerHardwarePtr, port, callback); configureTimerCaptureCompareInterrupt(timerHardwarePtr, port, callback);
return p; return p;
} }
static void ppmCallback(uint8_t port, uint16_t capture) static void ppmCallback(uint8_t port, uint16_t capture)
{ {
uint16_t diff; uint16_t diff;
static uint16_t now; static uint16_t now;
static uint16_t last = 0; static uint16_t last = 0;
static uint8_t chan = 0; static uint8_t chan = 0;
static uint8_t GoodPulses; static uint8_t GoodPulses;
@ -307,31 +307,31 @@ bool pwmInit(drv_pwm_config_t *init)
setup = hardwareMaps[i]; setup = hardwareMaps[i];
for (i = 0; i < MAX_PORTS; i++) { for (i = 0; i < MAX_PORTS; i++) {
uint8_t port = setup[i] & 0x0F; uint8_t port = setup[i] & 0x0F;
uint8_t mask = setup[i] & 0xF0; uint8_t mask = setup[i] & 0xF0;
if (setup[i] == 0xFF) // terminator if (setup[i] == 0xFF) // terminator
break; break;
#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER #ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER
// PWM2 is connected to LED2 on the board and cannot be connected unless you cut LED2_E // PWM2 is connected to LED2 on the board and cannot be connected unless you cut LED2_E
if (port == PWM2) if (port == PWM2)
continue; continue;
#endif #endif
// skip UART ports for GPS // skip UART ports for GPS
if (init->useUART && (port == PWM3 || port == PWM4)) if (init->useUART && (port == PWM3 || port == PWM4))
continue; continue;
#ifdef SOFTSERIAL_19200_LOOPBACK #ifdef SOFTSERIAL_19200_LOOPBACK
// skip softSerial ports // skip softSerial ports
if ((port == PWM5 || port == PWM6)) if ((port == PWM5 || port == PWM6))
continue; continue;
#endif #endif
// skip ADC for powerMeter if configured // skip ADC for powerMeter if configured
if (init->adcChannel && (init->adcChannel == port)) if (init->adcChannel && (init->adcChannel == port))
continue; continue;
// hacks to allow current functionality // hacks to allow current functionality
if (mask & (TYPE_IP | TYPE_IW) && !init->enableInput) if (mask & (TYPE_IP | TYPE_IW) && !init->enableInput)

28
src/drv_pwm.h
View file

@ -30,17 +30,17 @@ enum {
PWM9, PWM9,
PWM10, PWM10,
PWM11, PWM11,
PWM12, PWM12,
PWM13, PWM13,
PWM14, PWM14,
MAX_PORTS MAX_PORTS
}; };
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not
void pwmWriteMotor(uint8_t index, uint16_t value); void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmWriteServo(uint8_t index, uint16_t value); void pwmWriteServo(uint8_t index, uint16_t value);
uint16_t pwmRead(uint8_t channel); uint16_t pwmRead(uint8_t channel);
// void pwmWrite(uint8_t channel, uint16_t value); // void pwmWrite(uint8_t channel, uint16_t value);

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_Mode = GPIO_Mode_IPD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure); GPIO_Init(GPIOA, &GPIO_InitStructure);
// TODO Configure EXTI4 1 channel // TODO Configure EXTI4 1 channel
// Input timers on TIM2 for PWM // 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_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0; TIM_ICInitStructure.TIM_ICFilter = 0x0;
for (i = 0; i < 4; i++) { for (i = 0; i < 4; i++) {
TIM_ICInitStructure.TIM_Channel = Channels[i].channel; TIM_ICInitStructure.TIM_Channel = Channels[i].channel;
TIM_ICInit(Channels[i].tim, &TIM_ICInitStructure); TIM_ICInit(Channels[i].tim, &TIM_ICInitStructure);
} }
// TODO EXTI4 // TODO EXTI4
TIM_ITConfig(TIM2, TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4, ENABLE); 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 // use PPM or PWM input
usePPMFlag = init->usePPM; usePPMFlag = init->usePPM;
// preset channels to center // preset channels to center
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
Inputs[i].capture = 1500; Inputs[i].capture = 1500;

406
src/drv_softserial.c
View file

@ -1,203 +1,203 @@
#include "board.h" #include "board.h"
enum serialBitStatus { 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 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
}; };
#define MAX_SOFTSERIAL_PORTS 2 #define MAX_SOFTSERIAL_PORTS 2
softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS]; softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS];
softSerial_t* lookupSoftSerial(uint8_t reference) softSerial_t* lookupSoftSerial(uint8_t reference)
{ {
assert_param(reference >= 0 && reference <= MAX_SOFTSERIAL_PORTS); assert_param(reference >= 0 && reference <= MAX_SOFTSERIAL_PORTS);
return &(softSerialPorts[reference]); return &(softSerialPorts[reference]);
} }
void stopSerialTimer(softSerial_t *softSerial) void stopSerialTimer(softSerial_t *softSerial)
{ {
TIM_Cmd(softSerial->timerHardware->tim, DISABLE); TIM_Cmd(softSerial->timerHardware->tim, DISABLE);
TIM_SetCounter(softSerial->timerHardware->tim, 0); TIM_SetCounter(softSerial->timerHardware->tim, 0);
} }
void startSerialTimer(softSerial_t *softSerial) void startSerialTimer(softSerial_t *softSerial)
{ {
TIM_SetCounter(softSerial->timerHardware->tim, 0); TIM_SetCounter(softSerial->timerHardware->tim, 0);
TIM_Cmd(softSerial->timerHardware->tim, ENABLE); TIM_Cmd(softSerial->timerHardware->tim, ENABLE);
} }
static void waitForFirstBit(softSerial_t *softSerial) static void waitForFirstBit(softSerial_t *softSerial)
{ {
softSerial->state = BIT_0; softSerial->state = BIT_0;
startSerialTimer(softSerial); startSerialTimer(softSerial);
} }
static void onSerialPinChange(uint8_t reference, uint16_t capture) static void onSerialPinChange(uint8_t reference, uint16_t capture)
{ {
softSerial_t *softSerial = lookupSoftSerial(reference); softSerial_t *softSerial = lookupSoftSerial(reference);
if (softSerial->state == WAITING_FOR_START_BIT) { if (softSerial->state == WAITING_FOR_START_BIT) {
waitForFirstBit(softSerial); waitForFirstBit(softSerial);
} }
} }
uint8_t readSerialSignal(softSerial_t *softSerial) uint8_t readSerialSignal(softSerial_t *softSerial)
{ {
return GPIO_ReadInputDataBit(softSerial->timerHardware->gpio, softSerial->timerHardware->pin); return GPIO_ReadInputDataBit(softSerial->timerHardware->gpio, softSerial->timerHardware->pin);
} }
void mergeSignalWithCurrentByte(softSerial_t *softSerial, uint8_t serialSignal) void mergeSignalWithCurrentByte(softSerial_t *softSerial, uint8_t serialSignal)
{ {
softSerial->rxBuffer[softSerial->port.rxBufferTail] |= (serialSignal << softSerial->state); softSerial->rxBuffer[softSerial->port.rxBufferTail] |= (serialSignal << softSerial->state);
} }
inline void initialiseCurrentByteWithFirstSignal(softSerial_t *softSerial, uint8_t serialSignal) inline void initialiseCurrentByteWithFirstSignal(softSerial_t *softSerial, uint8_t serialSignal)
{ {
softSerial->rxBuffer[softSerial->port.rxBufferTail] = serialSignal; softSerial->rxBuffer[softSerial->port.rxBufferTail] = serialSignal;
} }
inline void prepareForNextSignal(softSerial_t *softSerial) { inline void prepareForNextSignal(softSerial_t *softSerial) {
softSerial->state++; softSerial->state++;
} }
void updateBufferIndex(softSerial_t *softSerial) void updateBufferIndex(softSerial_t *softSerial)
{ {
if (softSerial->port.rxBufferTail >= softSerial->port.rxBufferSize - 1) if (softSerial->port.rxBufferTail >= softSerial->port.rxBufferSize - 1)
{ {
softSerial->port.rxBufferTail = 0; //cycling the buffer softSerial->port.rxBufferTail = 0; //cycling the buffer
} else { } else {
softSerial->port.rxBufferTail++; softSerial->port.rxBufferTail++;
} }
} }
void prepareForNextByte(softSerial_t *softSerial) void prepareForNextByte(softSerial_t *softSerial)
{ {
stopSerialTimer(softSerial); stopSerialTimer(softSerial);
softSerial->state = WAITING_FOR_START_BIT; softSerial->state = WAITING_FOR_START_BIT;
updateBufferIndex(softSerial); updateBufferIndex(softSerial);
} }
void onSerialTimer(uint8_t portIndex, uint16_t capture) void onSerialTimer(uint8_t portIndex, uint16_t capture)
{ {
softSerial_t *softSerial = &(softSerialPorts[portIndex]); softSerial_t *softSerial = &(softSerialPorts[portIndex]);
uint8_t serialSignal = readSerialSignal(softSerial); uint8_t serialSignal = readSerialSignal(softSerial);
switch (softSerial->state) { switch (softSerial->state) {
case BIT_0: case BIT_0:
initialiseCurrentByteWithFirstSignal(softSerial, serialSignal); initialiseCurrentByteWithFirstSignal(softSerial, serialSignal);
prepareForNextSignal(softSerial); prepareForNextSignal(softSerial);
break; break;
case BIT_1: case BIT_1:
case BIT_2: case BIT_2:
case BIT_3: case BIT_3:
case BIT_4: case BIT_4:
case BIT_5: case BIT_5:
case BIT_6: case BIT_6:
case BIT_7: case BIT_7:
mergeSignalWithCurrentByte(softSerial, serialSignal); mergeSignalWithCurrentByte(softSerial, serialSignal);
prepareForNextSignal(softSerial); prepareForNextSignal(softSerial);
break; break;
case WAITING_FOR_STOP_BIT: case WAITING_FOR_STOP_BIT:
prepareForNextByte(softSerial); prepareForNextByte(softSerial);
break; break;
} }
} }
#define SOFT_SERIAL_TIMER_MHZ 1 #define SOFT_SERIAL_TIMER_MHZ 1
#define SOFT_SERIAL_1_TIMER_HARDWARE 4 #define SOFT_SERIAL_1_TIMER_HARDWARE 4
static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
{ {
gpio_config_t cfg; gpio_config_t cfg;
cfg.pin = pin; cfg.pin = pin;
cfg.mode = mode; cfg.mode = mode;
cfg.speed = Speed_2MHz; cfg.speed = Speed_2MHz;
gpioInit(gpio, &cfg); gpioInit(gpio, &cfg);
} }
void serialInputPortConfig(const timerHardware_t *timerHardwarePtr, uint16_t baud, uint8_t reference, timerCCCallbackPtr callback) void serialInputPortConfig(const timerHardware_t *timerHardwarePtr, uint16_t baud, uint8_t reference, timerCCCallbackPtr callback)
{ {
softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU); softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
int oneBitPeriod = (SOFT_SERIAL_TIMER_MHZ * 1000000) / baud; int oneBitPeriod = (SOFT_SERIAL_TIMER_MHZ * 1000000) / baud;
timerInConfig(timerHardwarePtr, oneBitPeriod, SOFT_SERIAL_TIMER_MHZ); timerInConfig(timerHardwarePtr, oneBitPeriod, SOFT_SERIAL_TIMER_MHZ);
configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, callback); configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, callback);
} }
void setupSoftSerial1(uint32_t baud) void setupSoftSerial1(uint32_t baud)
{ {
int portIndex = 0; int portIndex = 0;
softSerial_t *softSerial = &(softSerialPorts[portIndex]); softSerial_t *softSerial = &(softSerialPorts[portIndex]);
softSerial->timerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_HARDWARE]); softSerial->timerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_HARDWARE]);
softSerial->timerIndex = SOFT_SERIAL_1_TIMER_HARDWARE; softSerial->timerIndex = SOFT_SERIAL_1_TIMER_HARDWARE;
softSerial->state = WAITING_FOR_START_BIT; softSerial->state = WAITING_FOR_START_BIT;
softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE; softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE;
softSerial->port.rxBuffer = softSerial->rxBuffer; softSerial->port.rxBuffer = softSerial->rxBuffer;
softSerial->port.rxBufferTail = 0; softSerial->port.rxBufferTail = 0;
softSerial->port.rxBufferHead = 0; softSerial->port.rxBufferHead = 0;
softSerial->port.txBuffer = 0; softSerial->port.txBuffer = 0;
softSerial->port.txBufferSize = 0; softSerial->port.txBufferSize = 0;
softSerial->port.txBufferTail = 0; softSerial->port.txBufferTail = 0;
softSerial->port.txBufferHead = 0; softSerial->port.txBufferHead = 0;
softSerial->port.baudRate = baud; softSerial->port.baudRate = baud;
softSerial->port.mode = MODE_RX; softSerial->port.mode = MODE_RX;
// FIXME this uart specific initialisation doesn't belong really here // FIXME this uart specific initialisation doesn't belong really here
softSerial->port.txDMAChannel = 0; softSerial->port.txDMAChannel = 0;
softSerial->port.txDMAChannel = 0; softSerial->port.txDMAChannel = 0;
configureTimerChannelCallback(softSerial->timerHardware->tim, TIM_Channel_2, portIndex, onSerialTimer); configureTimerChannelCallback(softSerial->timerHardware->tim, TIM_Channel_2, portIndex, onSerialTimer);
TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE); TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE);
stopSerialTimer(softSerial); stopSerialTimer(softSerial);
serialInputPortConfig(softSerial->timerHardware, baud, portIndex, onSerialPinChange); serialInputPortConfig(softSerial->timerHardware, baud, portIndex, onSerialPinChange);
} }
bool serialAvailable(softSerial_t *softSerial) bool serialAvailable(softSerial_t *softSerial)
{ {
if (softSerial->port.rxBufferTail == softSerial->port.rxBufferHead) { if (softSerial->port.rxBufferTail == softSerial->port.rxBufferHead) {
return 0; return 0;
} }
int availableBytes; int availableBytes;
if (softSerial->port.rxBufferTail > softSerial->port.rxBufferHead) { if (softSerial->port.rxBufferTail > softSerial->port.rxBufferHead) {
availableBytes = softSerial->port.rxBufferTail - softSerial->port.rxBufferHead; availableBytes = softSerial->port.rxBufferTail - softSerial->port.rxBufferHead;
} else { } else {
availableBytes = softSerial->port.rxBufferTail + softSerial->port.rxBufferSize - softSerial->port.rxBufferHead; availableBytes = softSerial->port.rxBufferTail + softSerial->port.rxBufferSize - softSerial->port.rxBufferHead;
} }
return availableBytes; return availableBytes;
} }
static void moveHeadToNextByte(softSerial_t *softSerial) static void moveHeadToNextByte(softSerial_t *softSerial)
{ {
if (softSerial->port.rxBufferHead < softSerial->port.rxBufferSize - 1) { if (softSerial->port.rxBufferHead < softSerial->port.rxBufferSize - 1) {
softSerial->port.rxBufferHead++; softSerial->port.rxBufferHead++;
} else { } else {
softSerial->port.rxBufferHead = 0; softSerial->port.rxBufferHead = 0;
} }
} }
uint8_t serialReadByte(softSerial_t *softSerial) uint8_t serialReadByte(softSerial_t *softSerial)
{ {
if (serialAvailable(softSerial) == 0) { if (serialAvailable(softSerial) == 0) {
return 0; return 0;
} }
char b = softSerial->port.rxBuffer[softSerial->port.rxBufferHead]; char b = softSerial->port.rxBuffer[softSerial->port.rxBufferHead];
moveHeadToNextByte(softSerial); moveHeadToNextByte(softSerial);
return b; return b;
} }

50
src/drv_softserial.h
View file

@ -1,25 +1,25 @@
/* /*
* drv_softserial.h * drv_softserial.h
* *
* Created on: 23 Aug 2013 * Created on: 23 Aug 2013
* Author: Hydra * Author: Hydra
*/ */
#pragma once #pragma once
#define SOFT_SERIAL_BUFFER_SIZE 256 #define SOFT_SERIAL_BUFFER_SIZE 256
typedef struct softSerial_s { typedef struct softSerial_s {
const timerHardware_t *timerHardware; const timerHardware_t *timerHardware;
uint8_t timerIndex; uint8_t timerIndex;
serialPort_t port; serialPort_t port;
volatile int state; volatile int state;
volatile uint8_t rxBuffer[SOFT_SERIAL_BUFFER_SIZE]; volatile uint8_t rxBuffer[SOFT_SERIAL_BUFFER_SIZE];
} softSerial_t; } softSerial_t;
void setupSoftSerial1(uint32_t baud); void setupSoftSerial1(uint32_t baud);
uint8_t serialReadByte(softSerial_t *softSerial); uint8_t serialReadByte(softSerial_t *softSerial);
bool serialAvailable(softSerial_t *softSerial); bool serialAvailable(softSerial_t *softSerial);
extern timerHardware_t* serialTimerHardware; extern timerHardware_t* serialTimerHardware;
extern softSerial_t softSerialPorts[]; extern softSerial_t softSerialPorts[];

2
src/drv_spi.c
View file

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

18
src/drv_system.c
View file

@ -43,18 +43,18 @@ void systemInit(void)
GPIO_TypeDef *gpio; GPIO_TypeDef *gpio;
gpio_config_t cfg; gpio_config_t cfg;
} gpio_setup[] = { } gpio_setup[] = {
{ {
.gpio = LED0_GPIO, .gpio = LED0_GPIO,
.cfg = { LED0_PIN, Mode_Out_PP, Speed_2MHz } .cfg = { LED0_PIN, Mode_Out_PP, Speed_2MHz }
}, },
{ {
.gpio = LED1_GPIO, .gpio = LED1_GPIO,
.cfg = { LED1_PIN, Mode_Out_PP, Speed_2MHz } .cfg = { LED1_PIN, Mode_Out_PP, Speed_2MHz }
}, },
#ifdef BUZZER #ifdef BUZZER
{ {
.gpio = BEEP_GPIO, .gpio = BEEP_GPIO,
.cfg = { BEEP_PIN, Mode_Out_OD, Speed_2MHz } .cfg = { BEEP_PIN, Mode_Out_OD, Speed_2MHz }
}, },
#endif #endif
}; };

458
src/drv_timer.c
View file

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

44
src/drv_timer.h
View file

@ -1,22 +1,22 @@
#pragma once #pragma once
typedef void timerCCCallbackPtr(uint8_t port, uint16_t capture); typedef void timerCCCallbackPtr(uint8_t port, uint16_t capture);
typedef struct { typedef struct {
TIM_TypeDef *tim; TIM_TypeDef *tim;
GPIO_TypeDef *gpio; GPIO_TypeDef *gpio;
uint32_t pin; uint32_t pin;
uint8_t channel; uint8_t channel;
uint8_t irq; uint8_t irq;
uint8_t outputEnable; uint8_t outputEnable;
} timerHardware_t; } timerHardware_t;
extern const timerHardware_t timerHardware[]; extern const timerHardware_t timerHardware[];
void configTimeBase(TIM_TypeDef *tim, uint32_t period, uint8_t mhz); void configTimeBase(TIM_TypeDef *tim, uint32_t period, uint8_t mhz);
void timerInConfig(const timerHardware_t *timerHardwarePtr, uint32_t period, uint8_t mhz); void timerInConfig(const timerHardware_t *timerHardwarePtr, uint32_t period, uint8_t mhz);
void timerNVICConfig(uint8_t irq); void timerNVICConfig(uint8_t irq);
void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel); void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel);
void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback); void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback);
void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback); void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback);

28
src/drv_uart.c
View file

@ -238,20 +238,20 @@ void uartWrite(serialPort_t *s, uint8_t ch)
uartStartTxDMA(s); uartStartTxDMA(s);
} else { } else {
USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE); USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE);
} }
} }
void uartPrint(serialPort_t *s, const char *str) void uartPrint(serialPort_t *s, const char *str)
{ {
uint8_t ch; uint8_t ch;
while ((ch = *(str++))) { while ((ch = *(str++))) {
uartWrite(s, ch); uartWrite(s, ch);
} }
} }
// Handlers // Handlers
// USART1 Tx DMA Handler // USART1 Tx DMA Handler
void DMA1_Channel4_IRQHandler(void) void DMA1_Channel4_IRQHandler(void)
{ {
serialPort_t *s = &serialPort1; serialPort_t *s = &serialPort1;

68
src/drv_uart.h
View file

@ -12,40 +12,40 @@
typedef enum portmode_t { typedef enum portmode_t {
MODE_RX = 1, MODE_RX = 1,
MODE_TX = 2, MODE_TX = 2,
MODE_RXTX = 3 MODE_RXTX = 3
} portmode_t; } 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 // 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 { typedef struct {
portmode_t mode; portmode_t mode;
uint32_t baudRate; uint32_t baudRate;
uint32_t rxBufferSize; uint32_t rxBufferSize;
uint32_t txBufferSize; uint32_t txBufferSize;
volatile uint8_t *rxBuffer; volatile uint8_t *rxBuffer;
volatile uint8_t *txBuffer; volatile uint8_t *txBuffer;
uint32_t rxBufferHead; uint32_t rxBufferHead;
uint32_t rxBufferTail; uint32_t rxBufferTail;
uint32_t txBufferHead; uint32_t txBufferHead;
uint32_t txBufferTail; uint32_t txBufferTail;
// FIXME rename callback type so something more generic // FIXME rename callback type so something more generic
uartReceiveCallbackPtr callback; uartReceiveCallbackPtr callback;
// FIXME these are uart specific and do not belong in here // FIXME these are uart specific and do not belong in here
DMA_Channel_TypeDef *rxDMAChannel; DMA_Channel_TypeDef *rxDMAChannel;
DMA_Channel_TypeDef *txDMAChannel; DMA_Channel_TypeDef *txDMAChannel;
uint32_t rxDMAIrq; uint32_t rxDMAIrq;
uint32_t txDMAIrq; uint32_t txDMAIrq;
uint32_t rxDMAPos; uint32_t rxDMAPos;
bool txDMAEmpty; bool txDMAEmpty;
USART_TypeDef *USARTx; USART_TypeDef *USARTx;
} serialPort_t; } serialPort_t;
serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode); serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode);
void uartChangeBaud(serialPort_t *s, uint32_t baudRate); void uartChangeBaud(serialPort_t *s, uint32_t baudRate);
bool isUartAvailable(serialPort_t *s); bool isUartAvailable(serialPort_t *s);
bool isUartTransmitEmpty(serialPort_t *s); bool isUartTransmitEmpty(serialPort_t *s);

118
src/gps.c
View file

@ -927,46 +927,46 @@ typedef struct {
uint32_t speed_2d; uint32_t speed_2d;
int32_t heading_2d; int32_t heading_2d;
uint32_t speed_accuracy; uint32_t speed_accuracy;
uint32_t heading_accuracy; uint32_t heading_accuracy;
} ubx_nav_velned; } ubx_nav_velned;
typedef struct typedef struct
{ {
uint8_t chn; // Channel number, 255 for SVx not assigned to channel uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask uint8_t flags; // Bitmask
uint8_t quality; // Bitfield uint8_t quality; // Bitfield
uint8_t cno; // Carrier to Noise Ratio (Signal Strength) uint8_t cno; // Carrier to Noise Ratio (Signal Strength)
uint8_t elev; // Elevation in integer degrees uint8_t elev; // Elevation in integer degrees
int16_t azim; // Azimuth in integer degrees int16_t azim; // Azimuth in integer degrees
int32_t prRes; // Pseudo range residual in centimetres int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel; } ubx_nav_svinfo_channel;
typedef struct typedef struct
{ {
uint32_t time; // GPS Millisecond time of week uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6 uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
uint16_t reserved2; // Reserved uint16_t reserved2; // Reserved
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo; } ubx_nav_svinfo;
enum { enum {
PREAMBLE1 = 0xb5, PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62, PREAMBLE2 = 0x62,
CLASS_NAV = 0x01, CLASS_NAV = 0x01,
CLASS_ACK = 0x05, CLASS_ACK = 0x05,
CLASS_CFG = 0x06, CLASS_CFG = 0x06,
MSG_ACK_NACK = 0x00, MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01, MSG_ACK_ACK = 0x01,
MSG_POSLLH = 0x2, MSG_POSLLH = 0x2,
MSG_STATUS = 0x3, MSG_STATUS = 0x3,
MSG_SOL = 0x6, MSG_SOL = 0x6,
MSG_VELNED = 0x12, MSG_VELNED = 0x12,
MSG_SVINFO = 0x30, MSG_SVINFO = 0x30,
MSG_CFG_PRT = 0x00, MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08, MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01, MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24 MSG_CFG_NAV_SETTINGS = 0x24
} ubs_protocol_bytes; } ubs_protocol_bytes;
@ -1005,14 +1005,14 @@ static bool _new_speed;
// Receive buffer // Receive buffer
static union { static union {
ubx_nav_posllh posllh; ubx_nav_posllh posllh;
ubx_nav_status status; ubx_nav_status status;
ubx_nav_solution solution; ubx_nav_solution solution;
ubx_nav_velned velned; ubx_nav_velned velned;
ubx_nav_svinfo svinfo; ubx_nav_svinfo svinfo;
uint8_t bytes[200]; uint8_t bytes[200];
} _buffer; } _buffer;
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b) void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
{ {
while (len--) { while (len--) {
*ck_a += *data; *ck_a += *data;
@ -1113,24 +1113,24 @@ static bool UBLOX_parse_gps(void)
case MSG_VELNED: case MSG_VELNED:
// speed_3d = _buffer.velned.speed_3d; // cm/s // speed_3d = _buffer.velned.speed_3d; // cm/s
GPS_speed = _buffer.velned.speed_2d; // 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 GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true; _new_speed = true;
break; break;
case MSG_SVINFO: case MSG_SVINFO:
GPS_numCh = _buffer.svinfo.numCh; GPS_numCh = _buffer.svinfo.numCh;
if (GPS_numCh > 16) if (GPS_numCh > 16)
GPS_numCh = 16; GPS_numCh = 16;
for (i = 0; i < GPS_numCh; i++){ for (i = 0; i < GPS_numCh; i++){
GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn; GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn;
GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid; GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid;
GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality; GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality;
GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno; GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno;
} }
break; break;
default: default:
return false; return false;
} }
// we only return true when we get new position and speed data // we only return true when we get new position and speed data
// this ensures we don't use stale data // this ensures we don't use stale data
if (_new_position && _new_speed) { if (_new_position && _new_speed) {

10
src/imu.c
View file

@ -355,7 +355,7 @@ static void getEstimatedAttitude(void)
if (cfg.throttle_angle_correction) { if (cfg.throttle_angle_correction) {
int cosZ = EstG.V.Z / acc_1G * 100.0f; int cosZ = EstG.V.Z / acc_1G * 100.0f;
throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8; throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
} }
} }
@ -391,9 +391,9 @@ int getEstimatedAltitude(void)
// pressure relative to ground pressure with temperature compensation (fast!) // pressure relative to ground pressure with temperature compensation (fast!)
// baroGroundPressure is not supposed to be 0 here // baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp // 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_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 BaroAlt = BaroAlt * cfg.baro_noise_lpf + BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
// Integrator - velocity, cm/sec // Integrator - velocity, cm/sec
vel += (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount; vel += (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount;
@ -420,14 +420,14 @@ int getEstimatedAltitude(void)
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000); errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
BaroPID += errorAltitudeI / 512; // I in range +/-60 BaroPID += errorAltitudeI / 512; // I in range +/-60
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime; baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt; lastBaroAlt = BaroAlt;
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero 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 // 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); vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);

274
src/main.c
View file

@ -1,19 +1,19 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
core_t core; core_t core;
extern rcReadRawDataPtr rcReadRawFunc; extern rcReadRawDataPtr rcReadRawFunc;
// two receiver read functions // two receiver read functions
extern uint16_t pwmReadRawRC(uint8_t chan); extern uint16_t pwmReadRawRC(uint8_t chan);
extern uint16_t spektrumReadRawRC(uint8_t chan); extern uint16_t spektrumReadRawRC(uint8_t chan);
#ifdef USE_LAME_PRINTF #ifdef USE_LAME_PRINTF
// gcc/GNU version // gcc/GNU version
static void _putc(void *p, char c) static void _putc(void *p, char c)
{ {
uartWrite(core.mainport, c); uartWrite(core.mainport, c);
} }
#else #else
// keil/armcc version // keil/armcc version
@ -24,136 +24,136 @@ int fputc(int c, FILE *f)
uartWrite(core.mainport, c); uartWrite(core.mainport, c);
return c; return c;
} }
#endif #endif
int main(void) int main(void)
{ {
uint8_t i; uint8_t i;
drv_pwm_config_t pwm_params; drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params; drv_adc_config_t adc_params;
systemInit(); systemInit();
#ifdef USE_LAME_PRINTF #ifdef USE_LAME_PRINTF
init_printf(NULL, _putc); init_printf(NULL, _putc);
#endif #endif
checkFirstTime(false); checkFirstTime(false);
readEEPROM(); readEEPROM();
// configure power ADC // configure power ADC
if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9)) if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
adc_params.powerAdcChannel = mcfg.power_adc_channel; adc_params.powerAdcChannel = mcfg.power_adc_channel;
else { else {
adc_params.powerAdcChannel = 0; adc_params.powerAdcChannel = 0;
mcfg.power_adc_channel = 0; mcfg.power_adc_channel = 0;
} }
adcInit(&adc_params); adcInit(&adc_params);
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET); sensorsSet(SENSORS_SET);
mixerInit(); // this will set core.useServo var depending on mixer type mixerInit(); // this will set core.useServo var depending on mixer type
// when using airplane/wing mixer, servo/motor outputs are remapped // when using airplane/wing mixer, servo/motor outputs are remapped
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true; pwm_params.airplane = true;
else else
pwm_params.airplane = false; 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_SPEKTRUM); // spektrum support uses UART too
pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.useServos = core.useServo; pwm_params.useServos = core.useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate; pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate; pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold; pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) { switch (mcfg.power_adc_channel) {
case 1: case 1:
pwm_params.adcChannel = PWM2; pwm_params.adcChannel = PWM2;
break; break;
case 9: case 9:
pwm_params.adcChannel = PWM8; pwm_params.adcChannel = PWM8;
break; break;
default: default:
pwm_params.adcChannel = 0; pwm_params.adcChannel = 0;
break; break;
} }
pwmInit(&pwm_params); pwmInit(&pwm_params);
// configure PWM/CPPM read function. spektrum below will override that // configure PWM/CPPM read function. spektrum below will override that
rcReadRawFunc = pwmReadRawRC; rcReadRawFunc = pwmReadRawRC;
if (feature(FEATURE_SPEKTRUM)) { if (feature(FEATURE_SPEKTRUM)) {
spektrumInit(); spektrumInit();
rcReadRawFunc = spektrumReadRawRC; rcReadRawFunc = spektrumReadRawRC;
} else { } else {
// spektrum and GPS are mutually exclusive // 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. // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
if (feature(FEATURE_GPS)) if (feature(FEATURE_GPS))
gpsInit(mcfg.gps_baudrate); gpsInit(mcfg.gps_baudrate);
} }
#ifdef SONAR #ifdef SONAR
// sonar stuff only works with PPM // sonar stuff only works with PPM
if (feature(FEATURE_PPM)) { if (feature(FEATURE_PPM)) {
if (feature(FEATURE_SONAR)) if (feature(FEATURE_SONAR))
Sonar_init(); Sonar_init();
} }
#endif #endif
LED1_ON; LED1_ON;
LED0_OFF; LED0_OFF;
for (i = 0; i < 10; i++) { for (i = 0; i < 10; i++) {
LED1_TOGGLE; LED1_TOGGLE;
LED0_TOGGLE; LED0_TOGGLE;
delay(25); delay(25);
BEEP_ON; BEEP_ON;
delay(25); delay(25);
BEEP_OFF; BEEP_OFF;
} }
LED0_OFF; LED0_OFF;
LED1_OFF; LED1_OFF;
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsAutodetect(); sensorsAutodetect();
imuInit(); // Mag is initialized inside imuInit imuInit(); // Mag is initialized inside imuInit
// Check battery type/voltage // Check battery type/voltage
if (feature(FEATURE_VBAT)) if (feature(FEATURE_VBAT))
batteryInit(); batteryInit();
#ifdef SOFTSERIAL_19200_LOOPBACK #ifdef SOFTSERIAL_19200_LOOPBACK
serialInit(19200); serialInit(19200);
setupSoftSerial1(19200); setupSoftSerial1(19200);
uartPrint(core.mainport, "LOOPBACK 19200 ENABLED"); uartPrint(core.mainport, "LOOPBACK 19200 ENABLED");
#else #else
serialInit(mcfg.serial_baudrate); serialInit(mcfg.serial_baudrate);
#endif #endif
previousTime = micros(); previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400; calibratingA = 400;
calibratingG = 1000; calibratingG = 1000;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLES_25 = 1; f.SMALL_ANGLES_25 = 1;
// loopy // loopy
while (1) { while (1) {
loop(); loop();
#ifdef SOFTSERIAL_19200_LOOPBACK #ifdef SOFTSERIAL_19200_LOOPBACK
while (serialAvailable(&softSerialPorts[0])) { while (serialAvailable(&softSerialPorts[0])) {
uint8_t b = serialReadByte(&softSerialPorts[0]); uint8_t b = serialReadByte(&softSerialPorts[0]);
uartWrite(core.mainport, b); uartWrite(core.mainport, b);
}; };
#endif #endif
} }
} }
void HardFault_Handler(void) void HardFault_Handler(void)
{ {
// fall out of the sky // fall out of the sky
writeAllMotors(mcfg.mincommand); writeAllMotors(mcfg.mincommand);
while (1); while (1);
} }

4
src/mixer.c
View file

@ -274,8 +274,8 @@ static void airplaneMixer(void)
motor[0] = rcData[THROTTLE]; motor[0] = rcData[THROTTLE];
if (cfg.flaperons) { if (cfg.flaperons) {
} }
if (cfg.flaps) { if (cfg.flaps) {

22
src/mw.c
View file

@ -48,17 +48,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 uint16_t GPS_ground_course = 0; // degrees * 10
uint8_t GPS_Present = 0; // Checksum from Gps serial uint8_t GPS_Present = 0; // Checksum from Gps serial
uint8_t GPS_Enable = 0; uint8_t GPS_Enable = 0;
int16_t nav[2]; int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother 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 int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
uint8_t GPS_numCh; // Number of channels uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[16]; // Channel number uint8_t GPS_svinfo_chn[16]; // Channel number
uint8_t GPS_svinfo_svid[16]; // Satellite ID uint8_t GPS_svinfo_svid[16]; // Satellite ID
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
// Automatic ACC Offset Calibration // Automatic ACC Offset Calibration
uint16_t InflightcalibratingA = 0; uint16_t InflightcalibratingA = 0;
int16_t AccInflightCalibrationArmed; int16_t AccInflightCalibrationArmed;
uint16_t AccInflightCalibrationMeasurementDone = 0; uint16_t AccInflightCalibrationMeasurementDone = 0;
uint16_t AccInflightCalibrationSavetoEEProm = 0; uint16_t AccInflightCalibrationSavetoEEProm = 0;

28
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 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_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 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 // Failsafe related configuration
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
@ -228,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 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; } config_t;
// System-wide // System-wide
typedef struct master_t { typedef struct master_t {
uint8_t version; uint8_t version;
uint16_t size; uint16_t size;
@ -331,7 +331,7 @@ extern int16_t debug[4];
extern int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; extern int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
extern int32_t accSum[3]; extern int32_t accSum[3];
extern uint16_t acc_1G; extern uint16_t acc_1G;
extern uint32_t accTimeSum; extern uint32_t accTimeSum;
extern int accSumCount; extern int accSumCount;
extern uint32_t currentTime; extern uint32_t currentTime;
extern uint32_t previousTime; extern uint32_t previousTime;
@ -377,18 +377,18 @@ extern int16_t GPS_angle[2]; // it's the angles
extern uint16_t GPS_ground_course; // degrees*10 extern uint16_t GPS_ground_course; // degrees*10
extern uint8_t GPS_Present; // Checksum from Gps serial extern uint8_t GPS_Present; // Checksum from Gps serial
extern uint8_t GPS_Enable; extern uint8_t GPS_Enable;
extern int16_t nav[2]; extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode 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 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_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID extern uint8_t GPS_svinfo_svid[16]; // Satellite ID
extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
extern core_t core; extern core_t core;
extern master_t mcfg; extern master_t mcfg;
extern config_t cfg; extern config_t cfg;
extern flags_t f; extern flags_t f;
extern sensor_t acc; extern sensor_t acc;
extern sensor_t gyro; extern sensor_t gyro;

32
src/printf.c
View file

@ -3,28 +3,28 @@
* *
* All rights reserved. * 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: * 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. * 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 * list of conditions and the following disclaimer in the documentation and/or other
* materials provided with the distribution. * materials provided with the distribution.
* *
* Neither the name of the Kustaa Nyholm or SpareTimeLabs nor the names of its * 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 * contributors may be used to endorse or promote products derived from this software
* without specific prior written permission. * without specific prior written permission.
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * 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 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. * 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, * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, * 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, * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * 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 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
* OF SUCH DAMAGE. * OF SUCH DAMAGE.
*/ */

74
src/printf.h
View file

@ -1,32 +1,32 @@
/* /*
File: printf.h File: printf.h
Copyright (c) 2004,2012 Kustaa Nyholm / SpareTimeLabs Copyright (c) 2004,2012 Kustaa Nyholm / SpareTimeLabs
All rights reserved. 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: 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. 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 list of conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution. materials provided with the distribution.
Neither the name of the Kustaa Nyholm or SpareTimeLabs nor the names of its 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 contributors may be used to endorse or promote products derived from this software
without specific prior written permission. without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 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 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 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, IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 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, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 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 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE. OF SUCH DAMAGE.
@ -34,35 +34,35 @@ OF SUCH DAMAGE.
This library is realy just two files: 'printf.h' and 'printf.c'. 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. 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. debugger at all.
They are distributed in source form, so to use them, just compile them They are distributed in source form, so to use them, just compile them
into your project. 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'. The formats supported by this implementation are: 'd' 'u' 'c' 's' 'x' 'X'.
Zero padding and field width are also supported. 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 long specifier is also
supported. Note that this will pull in some long math routines (pun intended!) supported. Note that this will pull in some long math routines (pun intended!)
and thus make your executable noticably longer. and thus make your executable noticably longer.
The memory foot print of course depends on the target cpu, compiler and 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 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. 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 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 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 functionality and flexibility versus code size is close to optimal for
many embedded systems. 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 : something like :
void putc ( void* p, char c) void putc ( void* p, char c)
@ -71,25 +71,25 @@ void putc ( void* p, char c)
SERIAL_PORT_TX_REGISTER = 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: character output function with something like:
init_printf(NULL,putc); init_printf(NULL,putc);
Notice the 'NULL' in 'init_printf' and the parameter 'void* p' in '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 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 passed to your 'putc' routine. This allows you to pass some storage space (or
anything realy) to the character output function, if necessary. anything realy) to the character output function, if necessary.
This is not often needed but it was implemented like that because it made 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). implementing the sprintf function so neat (look at the source code).
The code is re-entrant, except for the 'init_printf' function, so it 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. 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! 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 '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'. 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 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. or struct members with these names, things will explode in your face.

24
src/sensors.c
View file

@ -64,14 +64,14 @@ retry:
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050; accHardware = ACC_MPU6050;
if (mcfg.acc_hardware == ACC_MPU6050) if (mcfg.acc_hardware == ACC_MPU6050)
break; break;
} }
; // fallthrough ; // fallthrough
#ifndef OLIMEXINO #ifndef OLIMEXINO
case 3: // MMA8452 case 3: // MMA8452
if (mma8452Detect(&acc)) { if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452; accHardware = ACC_MMA8452;
if (mcfg.acc_hardware == ACC_MMA8452) if (mcfg.acc_hardware == ACC_MMA8452)
break; break;
} }
#endif #endif
@ -289,7 +289,7 @@ int Baro_update(void)
return 0; return 0;
baroDeadline = currentTime; baroDeadline = currentTime;
if (state) { if (state) {
baro.get_up(); baro.get_up();
baro.start_ut(); baro.start_ut();
@ -470,21 +470,21 @@ int Mag_getADC(void)
writeEEPROM(1, true); writeEEPROM(1, true);
} }
} }
return 1; return 1;
} }
#endif #endif
#ifdef SONAR #ifdef SONAR
void Sonar_init(void) void Sonar_init(void)
{ {
hcsr04_init(sonar_rc78); hcsr04_init(sonar_rc78);
sensorsSet(SENSOR_SONAR); sensorsSet(SENSOR_SONAR);
sonarAlt = 0; sonarAlt = 0;
} }
void Sonar_update(void) void Sonar_update(void)
{ {
hcsr04_get_distance(&sonarAlt); hcsr04_get_distance(&sonarAlt);
} }

56
src/serial.c
View file

@ -1,7 +1,7 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
// Multiwii Serial Protocol 0 // Multiwii Serial Protocol 0
#define MSP_VERSION 0 #define MSP_VERSION 0
#define PLATFORM_32BIT ((uint32_t)1 << 31) #define PLATFORM_32BIT ((uint32_t)1 << 31)
@ -50,17 +50,17 @@
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 #define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
// Additional commands that are not compatible with MultiWii // Additional commands that are not compatible with MultiWii
#define MSP_UID 160 //out message Unique device ID #define MSP_UID 160 //out message Unique device ID
#define MSP_ACC_TRIM 240 //out message get acc angle trim values #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_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) #define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
#define INBUF_SIZE 64 #define INBUF_SIZE 64
struct box_t { struct box_t {
const uint8_t boxIndex; // this is from boxnames enum const uint8_t boxIndex; // this is from boxnames enum
const char *boxName; // GUI-readable box name const char *boxName; // GUI-readable box name
const uint8_t permanentId; // const uint8_t permanentId; //
} boxes[] = { } boxes[] = {
{ BOXARM, "ARM;", 0 }, { BOXARM, "ARM;", 0 },
{ BOXANGLE, "ANGLE;", 1 }, { BOXANGLE, "ANGLE;", 1 },
@ -120,7 +120,7 @@ const uint8_t boxids[] = { // permanent IDs associated to boxes. This way,
4, // "VARIO;" 4, // "VARIO;"
5, // "MAG;" 5, // "MAG;"
6, // "HEADFREE;" 6, // "HEADFREE;"
7, // "HEADADJ;" 7, // "HEADADJ;"
8, // "CAMSTAB;" 8, // "CAMSTAB;"
9, // "CAMTRIG;" 9, // "CAMTRIG;"
10, // "GPS HOME;" 10, // "GPS HOME;"
@ -251,7 +251,7 @@ void serializeBoxNamesReply(void)
strcat(buf, boxes[i].boxName); strcat(buf, boxes[i].boxName);
} }
} }
headSerialReply(strlen(buf)); headSerialReply(strlen(buf));
for (c = buf; *c; c++) for (c = buf; *c; c++)
serialize8(*c); serialize8(*c);
@ -561,7 +561,7 @@ static void evaluateCommand(void)
serialize32(lon); serialize32(lon);
serialize32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps serialize32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
serialize16(0); // heading will come here (deg) 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 serialize8(0); // nav flag will come here
break; break;
case MSP_SET_WP: case MSP_SET_WP:
@ -625,22 +625,22 @@ static void evaluateCommand(void)
case MSP_UID: case MSP_UID:
headSerialReply(12); headSerialReply(12);
serialize32(U_ID_0); serialize32(U_ID_0);
serialize32(U_ID_1); serialize32(U_ID_1);
serialize32(U_ID_2); serialize32(U_ID_2);
break; break;
case MSP_GPSSVINFO: case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4)); headSerialReply(1 + (GPS_numCh * 4));
serialize8(GPS_numCh); serialize8(GPS_numCh);
for (i = 0; i < GPS_numCh; i++){ for (i = 0; i < GPS_numCh; i++){
serialize8(GPS_svinfo_chn[i]); serialize8(GPS_svinfo_chn[i]);
serialize8(GPS_svinfo_svid[i]); serialize8(GPS_svinfo_svid[i]);
serialize8(GPS_svinfo_quality[i]); serialize8(GPS_svinfo_quality[i]);
serialize8(GPS_svinfo_cno[i]); serialize8(GPS_svinfo_cno[i]);
} }
break; break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M! default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0); headSerialError(0);
break; break;
} }
tailSerialReply(); tailSerialReply();
} }

6
src/spektrum.c
View file

@ -41,7 +41,7 @@ static void spektrumDataReceive(uint16_t c)
spekTime = micros(); spekTime = micros();
spekTimeInterval = spekTime - spekTimeLast; spekTimeInterval = spekTime - spekTimeLast;
spekTimeLast = spekTime; spekTimeLast = spekTime;
if (spekTimeInterval > 5000) if (spekTimeInterval > 5000)
spekFramePosition = 0; spekFramePosition = 0;
spekFrame[spekFramePosition] = (uint8_t)c; spekFrame[spekFramePosition] = (uint8_t)c;
if (spekFramePosition == SPEK_FRAME_SIZE - 1) { if (spekFramePosition == SPEK_FRAME_SIZE - 1) {
@ -68,7 +68,7 @@ uint16_t spektrumReadRawRC(uint8_t chan)
if (rcFrameComplete) { if (rcFrameComplete) {
for (b = 3; b < SPEK_FRAME_SIZE; b += 2) { for (b = 3; b < SPEK_FRAME_SIZE; b += 2) {
uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); 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]; spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
} }
rcFrameComplete = false; rcFrameComplete = false;
@ -82,6 +82,6 @@ uint16_t spektrumReadRawRC(uint8_t chan)
else else
data = 988 + spekChannelData[mcfg.rcmap[chan]]; // 1024 mode data = 988 + spekChannelData[mcfg.rcmap[chan]]; // 1024 mode
} }
return data; return data;
} }

2
src/telemetry.c
View file

@ -1,4 +1,4 @@
/* /*
* FrSky Telemetry implementation by silpstream @ rcgroups * FrSky Telemetry implementation by silpstream @ rcgroups
*/ */
#include "board.h" #include "board.h"

28
support/stmloader/Makefile
View file

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

244
support/stmloader/loader.c
View file

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

358
support/stmloader/serial.c
View file

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

76
support/stmloader/serial.h
View file

@ -1,38 +1,38 @@
/* /*
This file is part of AutoQuad. This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
AutoQuad is distributed in the hope that it will be useful, AutoQuad is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with AutoQuad. If not, see <http://www.gnu.org/licenses/>. along with AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt Copyright © 2011 Bill Nesbitt
*/ */
#ifndef _serial_h #ifndef _serial_h
#define _serial_h #define _serial_h
#define INPUT_BUFFER_SIZE 1024 #define INPUT_BUFFER_SIZE 1024
typedef struct { typedef struct {
int fd; int fd;
} serialStruct_t; } serialStruct_t;
extern serialStruct_t *initSerial(const char *port, unsigned int baud, char ctsRts); 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 serialWrite(serialStruct_t *s, const char *str, unsigned int len);
extern void serialWriteChar(serialStruct_t *s, unsigned char c); extern void serialWriteChar(serialStruct_t *s, unsigned char c);
extern unsigned char serialAvailable(serialStruct_t *s); extern unsigned char serialAvailable(serialStruct_t *s);
extern void serialFlush(serialStruct_t *s); extern void serialFlush(serialStruct_t *s);
extern unsigned char serialRead(serialStruct_t *s); extern unsigned char serialRead(serialStruct_t *s);
extern void serialEvenParity(serialStruct_t *s); extern void serialEvenParity(serialStruct_t *s);
extern void serialNoParity(serialStruct_t *s); extern void serialNoParity(serialStruct_t *s);
extern void serialFree(serialStruct_t *s); extern void serialFree(serialStruct_t *s);
#endif #endif

686
support/stmloader/stmbootloader.c
View file

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

54
support/stmloader/stmbootloader.h
View file

@ -1,27 +1,27 @@
/* /*
This file is part of AutoQuad. This file is part of AutoQuad.
AutoQuad is free software: you can redistribute it and/or modify AutoQuad is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
AutoQuad is distributed in the hope that it will be useful, AutoQuad is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with AutoQuad. If not, see <http://www.gnu.org/licenses/>. along with AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011 Bill Nesbitt Copyright © 2011 Bill Nesbitt
*/ */
#ifndef _stmbootloader_h #ifndef _stmbootloader_h
#define _stmbootloader_h #define _stmbootloader_h
#include <stdio.h> #include <stdio.h>
#include "serial.h" #include "serial.h"
extern void stmLoader(serialStruct_t *s, FILE *fp, unsigned char overrideParity, unsigned char noSendR); extern void stmLoader(serialStruct_t *s, FILE *fp, unsigned char overrideParity, unsigned char noSendR);
#endif #endif