rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/stac-hw-uavobj'

Browse Source
This commit is contained in:
James Cotton 2011-07-07 23:21:11 -05:00
commit 5fb1f7f5f7
43 changed files with 1999 additions and 1761 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
flight/AHRS/pios_board.c
View File

@ -54,7 +54,7 @@ static const struct pios_spi_cfg pios_spi_op_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_SPI_op_irq_handler, .handler = NULL,
.flags = .flags =
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
DMA1_FLAG_GL4), DMA1_FLAG_GL4),
@ -153,11 +153,11 @@ void PIOS_SPI_op_irq_handler(void)
extern void PIOS_ADC_handler(void); extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one // Remap the ADC DMA handler to this one
const struct pios_adc_cfg pios_adc_cfg = { static const struct pios_adc_cfg pios_adc_cfg = {
.dma = { .dma = {
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_ADC_DMA_Handler, .handler = NULL,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn, .NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -205,17 +205,10 @@ void PIOS_ADC_handler() {
/* /*
* AUX USART * AUX USART
*/ */
void PIOS_USART_aux_irq_handler(void); static const struct pios_usart_cfg pios_usart_aux_cfg = {
void USART3_IRQHandler()
__attribute__ ((alias("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART3, .regs = USART3,
.init = { .init = {
#if defined (PIOS_USART_BAUDRATE) .USART_BaudRate = 230400,
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No, .USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1, .USART_StopBits = USART_StopBits_1,
@ -224,7 +217,7 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx, .USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_aux_irq_handler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannel = USART3_IRQn, .NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
@ -250,12 +243,6 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
}, },
}; };
static uint32_t pios_usart_aux_id;
void PIOS_USART_aux_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_aux_id);
}
#endif /* PIOS_INCLUDE_USART */ #endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_COM)
@ -279,7 +266,7 @@ void I2C1_EV_IRQHandler()
void I2C1_ER_IRQHandler() void I2C1_ER_IRQHandler()
__attribute__ ((alias("PIOS_I2C_main_adapter_er_irq_handler"))); __attribute__ ((alias("PIOS_I2C_main_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
.regs = I2C1, .regs = I2C1,
.init = { .init = {
.I2C_Mode = I2C_Mode_I2C, .I2C_Mode = I2C_Mode_I2C,
@ -307,7 +294,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
}, },
}, },
.event = { .event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler, .handler = NULL,
.flags = 0, /* FIXME: check this */ .flags = 0, /* FIXME: check this */
.init = { .init = {
.NVIC_IRQChannel = I2C1_EV_IRQn, .NVIC_IRQChannel = I2C1_EV_IRQn,
@ -317,7 +304,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
}, },
}, },
.error = { .error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler, .handler = NULL,
.flags = 0, /* FIXME: check this */ .flags = 0, /* FIXME: check this */
.init = { .init = {
.NVIC_IRQChannel = I2C1_ER_IRQn, .NVIC_IRQChannel = I2C1_ER_IRQn,
@ -388,6 +375,7 @@ void PIOS_Board_Init(void) {
/* Communication system */ /* Communication system */
#if !defined(PIOS_ENABLE_DEBUG_PINS) #if !defined(PIOS_ENABLE_DEBUG_PINS)
#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_COM)
uint32_t pios_usart_aux_id;
if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }

2
flight/Bootloaders/AHRS/pios_board.c
View File

@ -55,7 +55,7 @@ static const struct pios_spi_cfg
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_SPI_op_irq_handler, .handler = NULL,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn, .NVIC_IRQChannel = DMA1_Channel4_IRQn,

16
flight/Bootloaders/OpenPilot/pios_board.c
View File

@ -60,7 +60,7 @@ const struct pios_spi_cfg
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_SPI_ahrs_irq_handler, .handler = NULL,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn, .NVIC_IRQChannel = DMA1_Channel4_IRQn,
@ -143,9 +143,9 @@ void PIOS_SPI_ahrs_irq_handler(void) {
/* /*
* Telemetry USART * Telemetry USART
*/ */
void PIOS_USART_telem_irq_handler(void); const struct pios_usart_cfg pios_usart_telem_cfg = {
void USART2_IRQHandler() __attribute__ ((alias ("PIOS_USART_telem_irq_handler"))); .regs = USART2,
const struct pios_usart_cfg pios_usart_telem_cfg = { .regs = USART2, .init = { .init = {
#if defined (PIOS_COM_TELEM_BAUDRATE) #if defined (PIOS_COM_TELEM_BAUDRATE)
.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE, .USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
#else #else
@ -157,7 +157,7 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { .regs = USART2, .init = {
.USART_HardwareFlowControl = USART_HardwareFlowControl_None, .USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx, .USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, .irq = { }, .irq = {
.handler = PIOS_USART_telem_irq_handler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannel = USART2_IRQn, .NVIC_IRQChannel = USART2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
@ -180,11 +180,6 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { .regs = USART2, .init = {
}, },
}, }; }, };
static uint32_t pios_usart_telem_rf_id;
void PIOS_USART_telem_irq_handler(void) {
PIOS_USART_IRQ_Handler(pios_usart_telem_rf_id);
}
#endif /* PIOS_INCLUDE_USART */ #endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_COM)
@ -218,6 +213,7 @@ void PIOS_Board_Init(void) {
/* Initialize the PiOS library */ /* Initialize the PiOS library */
#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_COM)
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }

32
flight/CopterControl/Makefile
View File

@ -45,10 +45,7 @@ ENABLE_DEBUG_PINS ?= NO
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs # Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
ENABLE_AUX_UART ?= NO ENABLE_AUX_UART ?= NO
USE_TELEMETRY ?= YES
USE_GPS ?= NO USE_GPS ?= NO
USE_SPEKTRUM ?= NO
USE_SBUS ?= NO
USE_I2C ?= NO USE_I2C ?= NO
@ -169,6 +166,7 @@ SRC += $(OPUAVSYNTHDIR)/mixersettings.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
#${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c} #${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c}
#SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c} #SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c}
# Cant use until i can automatically generate list of UAVObjects # Cant use until i can automatically generate list of UAVObjects
@ -211,6 +209,7 @@ SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_i2c_esc.c
SRC += $(PIOSCOMMON)/pios_iap.c SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/pios_bl_helper.c SRC += $(PIOSCOMMON)/pios_bl_helper.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/printf-stdarg.c SRC += $(PIOSCOMMON)/printf-stdarg.c
## Libraries for flight calculations ## Libraries for flight calculations
SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/fifo_buffer.c
@ -384,35 +383,8 @@ ifeq ($(ERASE_FLASH), YES)
CDEFS += -DERASE_FLASH CDEFS += -DERASE_FLASH
endif endif
# USART port functions. NO means do not use this function.
# YES means use with default USART port number (hardware-dependent).
# Number means use with specified USART port number (this value).
ifneq ($(USE_TELEMETRY), NO)
CDEFS += -DUSE_TELEMETRY
ifneq ($(USE_TELEMETRY), YES)
CDEFS += -DPIOS_PORT_TELEMETRY=$(USE_TELEMETRY)
endif
endif
ifneq ($(USE_GPS), NO) ifneq ($(USE_GPS), NO)
CDEFS += -DUSE_GPS CDEFS += -DUSE_GPS
ifneq ($(USE_GPS), YES)
CDEFS += -DPIOS_PORT_GPS=$(USE_GPS)
endif
endif
ifneq ($(USE_SPEKTRUM), NO)
CDEFS += -DUSE_SPEKTRUM
ifneq ($(USE_SPEKTRUM), YES)
CDEFS += -DPIOS_PORT_SPEKTRUM=$(USE_SPEKTRUM)
endif
endif
ifneq ($(USE_SBUS), NO)
CDEFS += -DUSE_SBUS
ifneq ($(USE_SBUS), YES)
CDEFS += -DPIOS_PORT_SBUS=$(USE_SBUS)
endif
endif endif
ifeq ($(USE_I2C), YES) ifeq ($(USE_I2C), YES)

2
flight/CopterControl/System/inc/FreeRTOSConfig.h
View File

@ -29,7 +29,7 @@
#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) #define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 )
#define configTICK_RATE_HZ ( ( portTickType ) 1000 ) #define configTICK_RATE_HZ ( ( portTickType ) 1000 )
#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) #define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 )
#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 128 ) #define configMINIMAL_STACK_SIZE ( ( unsigned short ) 48 )
#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 14 * 1024 ) ) #define configTOTAL_HEAP_SIZE ( ( size_t ) ( 14 * 1024 ) )
#define configMAX_TASK_NAME_LEN ( 16 ) #define configMAX_TASK_NAME_LEN ( 16 )
#define configUSE_TRACE_FACILITY 0 #define configUSE_TRACE_FACILITY 0

224
flight/CopterControl/System/inc/pios_config.h
View File

@ -1,116 +1,108 @@
/** /**
****************************************************************************** ******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System * @addtogroup OpenPilotSystem OpenPilot System
* @{ * @{
* @addtogroup OpenPilotCore OpenPilot Core * @addtogroup OpenPilotCore OpenPilot Core
* @{ * @{
* *
* @file pios_config.h * @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header. * @brief PiOS configuration header.
* Central compile time config for the project. * Central compile time config for the project.
* In particular, pios_config.h is where you define which PiOS libraries * In particular, pios_config.h is where you define which PiOS libraries
* and features are included in the firmware. * and features are included in the firmware.
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
/* /*
* This program is free software; you can redistribute it and/or modify * This program 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.
* *
* This program is distributed in the hope that it will be useful, but * This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details. * for more details.
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef PIOS_CONFIG_H #ifndef PIOS_CONFIG_H
#define PIOS_CONFIG_H #define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */ /* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC #define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY #define PIOS_INCLUDE_DELAY
#if defined(USE_I2C) #if defined(USE_I2C)
#define PIOS_INCLUDE_I2C #define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_I2C_ESC #define PIOS_INCLUDE_I2C_ESC
#endif #endif
#define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED #define PIOS_INCLUDE_LED
/* Receiver interfaces - only one allowed */ #define PIOS_INCLUDE_RCVR
#if !defined(USE_SPEKTRUM) && !defined(USE_SBUS)
//#define PIOS_INCLUDE_PPM /* Supported receiver interfaces */
#define PIOS_INCLUDE_PWM #define PIOS_INCLUDE_SPEKTRUM
#endif #define PIOS_INCLUDE_SBUS
//#define PIOS_INCLUDE_PPM
/* USART-based PIOS modules */ #define PIOS_INCLUDE_PWM
#if defined(USE_TELEMETRY)
#define PIOS_INCLUDE_TELEMETRY_RF /* Supported USART-based PIOS modules */
#endif #define PIOS_INCLUDE_TELEMETRY_RF
#if defined(USE_GPS) //#define PIOS_INCLUDE_GPS
#define PIOS_INCLUDE_GPS
#endif #define PIOS_INCLUDE_SERVO
#if defined(USE_SPEKTRUM) #define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SPEKTRUM #define PIOS_INCLUDE_SYS
#endif #define PIOS_INCLUDE_USART
#if defined(USE_SBUS) #define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_COM
#endif #define PIOS_INCLUDE_SETTINGS
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_SERVO #define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_RTC
#define PIOS_INCLUDE_USART #define PIOS_INCLUDE_WDG
#define PIOS_INCLUDE_USB_HID #define PIOS_INCLUDE_BL_HELPER
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_SETTINGS #define PIOS_INCLUDE_ADXL345
#define PIOS_INCLUDE_FREERTOS #define PIOS_INCLUDE_FLASH
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI /* A really shitty setting saving implementation */
#define PIOS_INCLUDE_RTC #define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
#define PIOS_INCLUDE_WDG
#define PIOS_INCLUDE_BL_HELPER /* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
#define PIOS_INCLUDE_ADXL345 #define STARTUP_LOG_ENABLED 1
#define PIOS_INCLUDE_FLASH
/* COM Module */
/* A really shitty setting saving implementation */ #define GPS_BAUDRATE 19200
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS #define TELEM_BAUDRATE 19200
#define AUXUART_ENABLED 0
/* Defaults for Logging */ #define AUXUART_BAUDRATE 19200
#define LOG_FILENAME "PIOS.LOG"
#define STARTUP_LOG_ENABLED 1 /* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 220
/* COM Module */ #define HEAP_LIMIT_CRITICAL 40
#define GPS_BAUDRATE 19200 #define CPULOAD_LIMIT_WARNING 85
#define TELEM_BAUDRATE 19200 #define CPULOAD_LIMIT_CRITICAL 95
#define AUXUART_ENABLED 0
#define AUXUART_BAUDRATE 19200 /* Task stack sizes */
#define PIOS_ACTUATOR_STACK_SIZE 1020
/* Alarm Thresholds */ #define PIOS_MANUAL_STACK_SIZE 724
#define HEAP_LIMIT_WARNING 220 #define PIOS_SYSTEM_STACK_SIZE 560
#define HEAP_LIMIT_CRITICAL 150 #define PIOS_STABILIZATION_STACK_SIZE 524
#define CPULOAD_LIMIT_WARNING 80 #define PIOS_TELEM_STACK_SIZE 500
#define CPULOAD_LIMIT_CRITICAL 95
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998
/* Task stack sizes */ //#define PIOS_QUATERNION_STABILIZATION
#define PIOS_ACTUATOR_STACK_SIZE 1020
#define PIOS_MANUAL_STACK_SIZE 724 #endif /* PIOS_CONFIG_H */
#define PIOS_SYSTEM_STACK_SIZE 560 /**
#define PIOS_STABILIZATION_STACK_SIZE 524 * @}
#define PIOS_TELEM_STACK_SIZE 500 * @}
*/
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998
//#define PIOS_QUATERNION_STABILIZATION
#endif /* PIOS_CONFIG_H */
/**
* @}
* @}
*/

813
flight/CopterControl/System/pios_board.c
View File

@ -30,6 +30,8 @@
#include <pios.h> #include <pios.h>
#include <openpilot.h> #include <openpilot.h>
#include <uavobjectsinit.h> #include <uavobjectsinit.h>
#include <hwsettings.h>
#include <manualcontrolsettings.h>
#if defined(PIOS_INCLUDE_SPI) #if defined(PIOS_INCLUDE_SPI)
@ -43,7 +45,7 @@
void PIOS_SPI_flash_accel_irq_handler(void); void PIOS_SPI_flash_accel_irq_handler(void);
void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler"))); void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler")));
void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler"))); void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler")));
const struct pios_spi_cfg pios_spi_flash_accel_cfg = { static const struct pios_spi_cfg pios_spi_flash_accel_cfg = {
.regs = SPI2, .regs = SPI2,
.init = { .init = {
.SPI_Mode = SPI_Mode_Master, .SPI_Mode = SPI_Mode_Master,
@ -61,7 +63,7 @@ const struct pios_spi_cfg pios_spi_flash_accel_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_SPI_flash_accel_irq_handler, .handler = NULL,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn, .NVIC_IRQChannel = DMA1_Channel4_IRQn,
@ -150,11 +152,11 @@ void PIOS_SPI_flash_accel_irq_handler(void)
extern void PIOS_ADC_handler(void); extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one // Remap the ADC DMA handler to this one
const struct pios_adc_cfg pios_adc_cfg = { static const struct pios_adc_cfg pios_adc_cfg = {
.dma = { .dma = {
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_ADC_DMA_Handler, .handler = NULL,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn, .NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -199,147 +201,14 @@ void PIOS_ADC_handler() {
#include "pios_usart_priv.h" #include "pios_usart_priv.h"
/*
* Serial port configuration.
* TODO: This should be dynamic in the future.
* But for now define any compatile combination of:
* USE_I2C (shared with USART3)
* USE_TELEMETRY
* USE_GPS
* USE_SPEKTRUM
* USE_SBUS (USART1 only, it needs an invertor)
* and optionally define PIOS_PORT_* to USART port numbers
*/
/* Serial telemetry: USART1 or USART3 */
#if !defined(PIOS_PORT_TELEMETRY)
#define PIOS_PORT_TELEMETRY 1
#endif
/* GPS receiver: USART1 or USART3 */
#if !defined(PIOS_PORT_GPS)
#define PIOS_PORT_GPS 3
#endif
/* Spektrum satellite receiver: USART1 or USART3 */
#if !defined(PIOS_PORT_SPEKTRUM)
#define PIOS_PORT_SPEKTRUM 3
#endif
/* Futaba S.Bus receiver: USART1 only (needs invertor) */
#if !defined(PIOS_PORT_SBUS)
#define PIOS_PORT_SBUS 1
#endif
/*
* Define USART port configurations and check for conflicts
* making sure they do not conflict with each other and with I2C.
*/
#define USART_GPIO(port) (((port) == 1) ? GPIOA : GPIOB)
#define USART_RXIO(port) (((port) == 1) ? GPIO_Pin_10 : GPIO_Pin_11)
#define USART_TXIO(port) (((port) == 1) ? GPIO_Pin_9 : GPIO_Pin_10)
#if defined(USE_TELEMETRY)
#if defined(USE_I2C) && (PIOS_PORT_TELEMETRY == 3)
#error defined(USE_I2C) && (PIOS_PORT_TELEMETRY == 3)
#endif
#if (PIOS_PORT_TELEMETRY == 1)
#define PIOS_USART_TELEMETRY USART1
#define PIOS_IRQH_TELEMETRY USART1_IRQHandler
#define PIOS_IRQC_TELEMETRY USART1_IRQn
#else
#define PIOS_USART_TELEMETRY USART3
#define PIOS_IRQH_TELEMETRY USART3_IRQHandler
#define PIOS_IRQC_TELEMETRY USART3_IRQn
#endif
#define PIOS_GPIO_TELEMETRY USART_GPIO(PIOS_PORT_TELEMETRY)
#define PIOS_RXIO_TELEMETRY USART_RXIO(PIOS_PORT_TELEMETRY)
#define PIOS_TXIO_TELEMETRY USART_TXIO(PIOS_PORT_TELEMETRY)
#endif
#if defined(USE_GPS)
#if defined(USE_I2C) && (PIOS_PORT_GPS == 3)
#error defined(USE_I2C) && (PIOS_PORT_GPS == 3)
#endif
#if defined(USE_TELEMETRY) && (PIOS_PORT_TELEMETRY == PIOS_PORT_GPS)
#error defined(USE_TELEMETRY) && (PIOS_PORT_TELEMETRY == PIOS_PORT_GPS)
#endif
#if (PIOS_PORT_GPS == 1)
#define PIOS_USART_GPS USART1
#define PIOS_IRQH_GPS USART1_IRQHandler
#define PIOS_IRQC_GPS USART1_IRQn
#else
#define PIOS_USART_GPS USART3
#define PIOS_IRQH_GPS USART3_IRQHandler
#define PIOS_IRQC_GPS USART3_IRQn
#endif
#define PIOS_GPIO_GPS USART_GPIO(PIOS_PORT_GPS)
#define PIOS_RXIO_GPS USART_RXIO(PIOS_PORT_GPS)
#define PIOS_TXIO_GPS USART_TXIO(PIOS_PORT_GPS)
#endif
#if defined(USE_SPEKTRUM)
#if defined(USE_I2C) && (PIOS_PORT_SPEKTRUM == 3)
#error defined(USE_I2C) && (PIOS_PORT_SPEKTRUM == 3)
#endif
#if defined(USE_TELEMETRY) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_TELEMETRY)
#error defined(USE_TELEMETRY) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_TELEMETRY)
#endif
#if defined(USE_GPS) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_GPS)
#error defined(USE_GPS) && (PIOS_PORT_SPEKTRUM == PIOS_PORT_GPS)
#endif
#if defined(USE_SBUS)
#error defined(USE_SPEKTRUM) && defined(USE_SBUS)
#endif
#if (PIOS_PORT_SPEKTRUM == 1)
#define PIOS_USART_SPEKTRUM USART1
#define PIOS_IRQH_SPEKTRUM USART1_IRQHandler
#define PIOS_IRQC_SPEKTRUM USART1_IRQn
#else
#define PIOS_USART_SPEKTRUM USART3
#define PIOS_IRQH_SPEKTRUM USART3_IRQHandler
#define PIOS_IRQC_SPEKTRUM USART3_IRQn
#endif
#define PIOS_GPIO_SPEKTRUM USART_GPIO(PIOS_PORT_SPEKTRUM)
#define PIOS_RXIO_SPEKTRUM USART_RXIO(PIOS_PORT_SPEKTRUM)
#define PIOS_TXIO_SPEKTRUM USART_TXIO(PIOS_PORT_SPEKTRUM)
#endif
#if defined(USE_SBUS)
#if (PIOS_PORT_SBUS != 1)
#error (PIOS_PORT_SBUS != 1)
#endif
#if defined(USE_TELEMETRY) && (PIOS_PORT_SBUS == PIOS_PORT_TELEMETRY)
#error defined(USE_TELEMETRY) && (PIOS_PORT_SBUS == PIOS_PORT_TELEMETRY)
#endif
#if defined(USE_GPS) && (PIOS_PORT_SBUS == PIOS_PORT_GPS)
#error defined(USE_GPS) && (PIOS_PORT_SBUS == PIOS_PORT_GPS)
#endif
#if defined(USE_SPEKTRUM)
#error defined(USE_SPEKTRUM) && defined(USE_SBUS)
#endif
#define PIOS_USART_SBUS USART1
#define PIOS_IRQH_SBUS USART1_IRQHandler
#define PIOS_IRQC_SBUS USART1_IRQn
#define PIOS_GPIO_SBUS USART_GPIO(PIOS_PORT_SBUS)
#define PIOS_RXIO_SBUS USART_RXIO(PIOS_PORT_SBUS)
#define PIOS_TXIO_SBUS USART_TXIO(PIOS_PORT_SBUS)
#endif
#if defined(PIOS_INCLUDE_TELEMETRY_RF) #if defined(PIOS_INCLUDE_TELEMETRY_RF)
/* /*
* Telemetry USART * Telemetry USART
*/ */
void PIOS_USART_telem_irq_handler(void); static const struct pios_usart_cfg pios_usart_telem_main_cfg = {
void PIOS_IRQH_TELEMETRY() __attribute__ ((alias ("PIOS_USART_telem_irq_handler"))); .regs = USART1,
const struct pios_usart_cfg pios_usart_telem_cfg = {
.regs = PIOS_USART_TELEMETRY,
.init = { .init = {
#if defined (PIOS_COM_TELEM_BAUDRATE) .USART_BaudRate = 57600,
.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No, .USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1, .USART_StopBits = USART_StopBits_1,
@ -347,26 +216,63 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx, .USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_telem_irq_handler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannel = PIOS_IRQC_TELEMETRY, .NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE, .NVIC_IRQChannelCmd = ENABLE,
}, },
}, },
.rx = { .rx = {
.gpio = PIOS_GPIO_TELEMETRY, .gpio = GPIOA,
.init = { .init = {
.GPIO_Pin = PIOS_RXIO_TELEMETRY, .GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU, .GPIO_Mode = GPIO_Mode_IPU,
}, },
}, },
.tx = { .tx = {
.gpio = PIOS_GPIO_TELEMETRY, .gpio = GPIOA,
.init = { .init = {
.GPIO_Pin = PIOS_TXIO_TELEMETRY, .GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
static const struct pios_usart_cfg pios_usart_telem_flexi_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = NULL,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP, .GPIO_Mode = GPIO_Mode_AF_PP,
}, },
@ -378,16 +284,10 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
/* /*
* GPS USART * GPS USART
*/ */
void PIOS_USART_gps_irq_handler(void); static const struct pios_usart_cfg pios_usart_gps_main_cfg = {
void PIOS_IRQH_GPS() __attribute__ ((alias ("PIOS_USART_gps_irq_handler"))); .regs = USART1,
const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = PIOS_USART_GPS,
.init = { .init = {
#if defined (PIOS_COM_GPS_BAUDRATE) .USART_BaudRate = 57600,
.USART_BaudRate = PIOS_COM_GPS_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No, .USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1, .USART_StopBits = USART_StopBits_1,
@ -395,47 +295,80 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx, .USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_gps_irq_handler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannel = PIOS_IRQC_GPS, .NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE, .NVIC_IRQChannelCmd = ENABLE,
}, },
}, },
.rx = { .rx = {
.gpio = PIOS_GPIO_GPS, .gpio = GPIOA,
.init = { .init = {
.GPIO_Pin = PIOS_RXIO_GPS, .GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU, .GPIO_Mode = GPIO_Mode_IPU,
}, },
}, },
.tx = { .tx = {
.gpio = PIOS_GPIO_GPS, .gpio = GPIOA,
.init = { .init = {
.GPIO_Pin = PIOS_TXIO_GPS, .GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP, .GPIO_Mode = GPIO_Mode_AF_PP,
}, },
}, },
}; };
#endif
static const struct pios_usart_cfg pios_usart_gps_flexi_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = NULL,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#endif /* PIOS_INCLUDE_GPS */
#if defined(PIOS_INCLUDE_SPEKTRUM) #if defined(PIOS_INCLUDE_SPEKTRUM)
/* /*
* SPEKTRUM USART * SPEKTRUM USART
*/ */
void PIOS_USART_spektrum_irq_handler(void); #include <pios_spektrum_priv.h>
void PIOS_IRQH_SPEKTRUM() __attribute__ ((alias ("PIOS_USART_spektrum_irq_handler")));
const struct pios_usart_cfg pios_usart_spektrum_cfg = { static const struct pios_usart_cfg pios_usart_spektrum_main_cfg = {
.regs = PIOS_USART_SPEKTRUM, .regs = USART1,
.init = { .init = {
#if defined (PIOS_COM_SPEKTRUM_BAUDRATE) .USART_BaudRate = 115200,
.USART_BaudRate = PIOS_COM_SPEKTRUM_BAUDRATE,
#else
.USART_BaudRate = 115200,
#endif
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No, .USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1, .USART_StopBits = USART_StopBits_1,
@ -443,88 +376,105 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.USART_Mode = USART_Mode_Rx, .USART_Mode = USART_Mode_Rx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_spektrum_irq_handler, .handler = PIOS_SPEKTRUM_irq_handler,
.init = { .init = {
.NVIC_IRQChannel = PIOS_IRQC_SPEKTRUM, .NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE, .NVIC_IRQChannelCmd = ENABLE,
}, },
}, },
.rx = { .rx = {
.gpio = PIOS_GPIO_SPEKTRUM, .gpio = GPIOA,
.init = { .init = {
.GPIO_Pin = PIOS_RXIO_SPEKTRUM, .GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU, .GPIO_Mode = GPIO_Mode_IPU,
}, },
}, },
.tx = { .tx = {
.gpio = PIOS_GPIO_SPEKTRUM, .gpio = GPIOA,
.init = { .init = {
.GPIO_Pin = PIOS_TXIO_SPEKTRUM, .GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING, .GPIO_Mode = GPIO_Mode_IN_FLOATING,
}, },
}, },
}; };
static uint32_t pios_usart_spektrum_id; static const struct pios_spektrum_cfg pios_spektrum_main_cfg = {
void PIOS_USART_spektrum_irq_handler(void) .bind = {
{ .gpio = GPIOA,
SPEKTRUM_IRQHandler(pios_usart_spektrum_id); .init = {
} .GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
#include <pios_spektrum_priv.h> .GPIO_Mode = GPIO_Mode_Out_PP,
void RTC_IRQHandler();
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_SUPV_irq_handler")));
const struct pios_spektrum_cfg pios_spektrum_cfg = {
.pios_usart_spektrum_cfg = &pios_usart_spektrum_cfg,
.gpio_init = { //used for bind feature
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
.remap = 0,
.irq = {
.handler = RTC_IRQHandler,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
}, },
}, },
.port = PIOS_GPIO_SPEKTRUM, .remap = 0,
.pin = PIOS_RXIO_SPEKTRUM,
}; };
void PIOS_SUPV_irq_handler() { static const struct pios_usart_cfg pios_usart_spektrum_flexi_cfg = {
if (RTC_GetITStatus(RTC_IT_SEC)) .regs = USART3,
{ .init = {
/* Call the right handler */ .USART_BaudRate = 115200,
PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id); .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.handler = PIOS_SPEKTRUM_irq_handler,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
static const struct pios_spektrum_cfg pios_spektrum_flexi_cfg = {
.bind = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
.remap = 0,
};
/* Wait until last write operation on RTC registers has finished */
RTC_WaitForLastTask();
/* Clear the RTC Second interrupt */
RTC_ClearITPendingBit(RTC_IT_SEC);
}
}
#endif /* PIOS_INCLUDE_SPEKTRUM */ #endif /* PIOS_INCLUDE_SPEKTRUM */
#if defined(PIOS_INCLUDE_SBUS) #if defined(PIOS_INCLUDE_SBUS)
/* /*
* SBUS USART * SBUS USART
*/ */
void PIOS_USART_sbus_irq_handler(void); #include <pios_sbus_priv.h>
void PIOS_IRQH_SBUS() __attribute__ ((alias ("PIOS_USART_sbus_irq_handler")));
const struct pios_usart_cfg pios_usart_sbus_cfg = { static const struct pios_usart_cfg pios_usart_sbus_main_cfg = {
.regs = PIOS_USART_SBUS, .regs = USART1,
.init = { .init = {
#if defined (PIOS_COM_SBUS_BAUDRATE) .USART_BaudRate = 100000,
.USART_BaudRate = PIOS_COM_SBUS_BAUDRATE,
#else
.USART_BaudRate = 100000,
#endif
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_Even, .USART_Parity = USART_Parity_Even,
.USART_StopBits = USART_StopBits_2, .USART_StopBits = USART_StopBits_2,
@ -532,88 +482,50 @@ const struct pios_usart_cfg pios_usart_sbus_cfg = {
.USART_Mode = USART_Mode_Rx, .USART_Mode = USART_Mode_Rx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_sbus_irq_handler, .handler = PIOS_SBUS_irq_handler,
.init = { .init = {
.NVIC_IRQChannel = PIOS_IRQC_SBUS, .NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE, .NVIC_IRQChannelCmd = ENABLE,
}, },
}, },
.rx = { .rx = {
.gpio = PIOS_GPIO_SBUS, .gpio = GPIOA,
.init = { .init = {
.GPIO_Pin = PIOS_RXIO_SBUS, .GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU, .GPIO_Mode = GPIO_Mode_IPU,
}, },
}, },
.tx = { .tx = {
.gpio = PIOS_GPIO_SBUS, .gpio = GPIOA,
.init = { .init = {
.GPIO_Pin = PIOS_TXIO_SBUS, .GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz, .GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING, .GPIO_Mode = GPIO_Mode_IN_FLOATING,
}, },
}, },
}; };
static uint32_t pios_usart_sbus_id; static const struct pios_sbus_cfg pios_sbus_cfg = {
void PIOS_USART_sbus_irq_handler(void) /* Inverter configuration */
{ .inv = {
SBUS_IRQHandler(pios_usart_sbus_id); .gpio = GPIOB,
} .init = {
.GPIO_Pin = GPIO_Pin_2,
#include <pios_sbus_priv.h> .GPIO_Mode = GPIO_Mode_Out_PP,
void RTC_IRQHandler(); .GPIO_Speed = GPIO_Speed_2MHz,
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_SUPV_irq_handler"))); },
const struct pios_sbus_cfg pios_sbus_cfg = { },
/* USART configuration structure */
.pios_usart_sbus_cfg = &pios_usart_sbus_cfg,
/* Invertor configuration */
.gpio_clk_func = RCC_APB2PeriphClockCmd, .gpio_clk_func = RCC_APB2PeriphClockCmd,
.gpio_clk_periph = RCC_APB2Periph_GPIOB, .gpio_clk_periph = RCC_APB2Periph_GPIOB,
.gpio_inv_port = GPIOB,
.gpio_inv_init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
.gpio_inv_enable = Bit_SET, .gpio_inv_enable = Bit_SET,
}; };
void PIOS_SUPV_irq_handler() {
if (RTC_GetITStatus(RTC_IT_SEC))
{
/* Call the right handler */
PIOS_SBUS_irq_handler(pios_usart_sbus_id);
/* Wait until last write operation on RTC registers has finished */
RTC_WaitForLastTask();
/* Clear the RTC Second interrupt */
RTC_ClearITPendingBit(RTC_IT_SEC);
}
}
#endif /* PIOS_INCLUDE_SBUS */ #endif /* PIOS_INCLUDE_SBUS */
#if defined(PIOS_INCLUDE_TELEMETRY_RF) #endif /* PIOS_INCLUDE_USART */
static uint32_t pios_usart_telem_rf_id;
void PIOS_USART_telem_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_telem_rf_id);
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_GPS)
static uint32_t pios_usart_gps_id;
void PIOS_USART_gps_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_gps_id);
}
#endif /* PIOS_INCLUDE_GPS */
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_COM)
@ -621,11 +533,40 @@ void PIOS_USART_gps_irq_handler(void)
#endif /* PIOS_INCLUDE_COM */ #endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_RTC)
/*
* Realtime Clock (RTC)
*/
#include <pios_rtc_priv.h>
void PIOS_RTC_IRQ_Handler (void);
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler")));
static const struct pios_rtc_cfg pios_rtc_main_cfg = {
.clksrc = RCC_RTCCLKSource_HSE_Div128,
.prescaler = 100,
.irq = {
.handler = NULL,
.init = {
.NVIC_IRQChannel = RTC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
void PIOS_RTC_IRQ_Handler (void)
{
PIOS_RTC_irq_handler ();
}
#endif
/* /*
* Servo outputs * Servo outputs
*/ */
#include <pios_servo_priv.h> #include <pios_servo_priv.h>
const struct pios_servo_channel pios_servo_channels[] = { static const struct pios_servo_channel pios_servo_channels[] = {
{ {
.timer = TIM4, .timer = TIM4,
.port = GPIOB, .port = GPIOB,
@ -691,13 +632,65 @@ const struct pios_servo_cfg pios_servo_cfg = {
.num_channels = NELEMENTS(pios_servo_channels), .num_channels = NELEMENTS(pios_servo_channels),
}; };
#if defined(PIOS_INCLUDE_PWM) && defined(PIOS_INCLUDE_PPM)
#error Cannot define both PIOS_INCLUDE_PWM and PIOS_INCLUDE_PPM at the same time (yet)
#endif
/*
* PPM Inputs
*/
#if defined(PIOS_INCLUDE_PPM)
#include <pios_ppm_priv.h>
void TIM4_IRQHandler();
void TIM4_IRQHandler() __attribute__ ((alias ("PIOS_TIM4_irq_handler")));
const struct pios_ppm_cfg pios_ppm_cfg = {
.tim_base_init = {
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = 0xFFFF, /* shared timer, make sure init correctly in outputs */
.TIM_RepetitionCounter = 0x0000,
},
.tim_ic_init = {
.TIM_Channel = TIM_Channel_1,
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
.gpio_init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
.remap = 0,
.irq = {
.handler = TIM4_IRQHandler,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.timer = TIM4,
.port = GPIOB,
.ccr = TIM_IT_CC1,
};
void PIOS_TIM4_irq_handler()
{
PIOS_PPM_irq_handler();
}
#endif /* PIOS_INCLUDE_PPM */
/* /*
* PWM Inputs * PWM Inputs
*/ */
#if defined(PIOS_INCLUDE_PWM) #if defined(PIOS_INCLUDE_PWM)
#include <pios_pwm_priv.h> #include <pios_pwm_priv.h>
const struct pios_pwm_channel pios_pwm_channels[] = {
static const struct pios_pwm_channel pios_pwm_channels[] = {
{ {
.timer = TIM4, .timer = TIM4,
.port = GPIOB, .port = GPIOB,
@ -768,7 +761,7 @@ const struct pios_pwm_cfg pios_pwm_cfg = {
}, },
.remap = 0, .remap = 0,
.irq = { .irq = {
.handler = TIM2_IRQHandler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelSubPriority = 0,
@ -805,7 +798,7 @@ void PIOS_I2C_main_adapter_er_irq_handler(void);
void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler"))); void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler")));
void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler"))); void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
.regs = I2C2, .regs = I2C2,
.init = { .init = {
.I2C_Mode = I2C_Mode_I2C, .I2C_Mode = I2C_Mode_I2C,
@ -833,7 +826,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
}, },
}, },
.event = { .event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler, .handler = NULL,
.flags = 0, /* FIXME: check this */ .flags = 0, /* FIXME: check this */
.init = { .init = {
.NVIC_IRQChannel = I2C2_EV_IRQn, .NVIC_IRQChannel = I2C2_EV_IRQn,
@ -843,7 +836,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
}, },
}, },
.error = { .error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler, .handler = NULL,
.flags = 0, /* FIXME: check this */ .flags = 0, /* FIXME: check this */
.init = { .init = {
.NVIC_IRQChannel = I2C2_ER_IRQn, .NVIC_IRQChannel = I2C2_ER_IRQn,
@ -869,13 +862,18 @@ void PIOS_I2C_main_adapter_er_irq_handler(void)
#endif /* PIOS_INCLUDE_I2C */ #endif /* PIOS_INCLUDE_I2C */
#if defined(PIOS_INCLUDE_RCVR)
#include "pios_rcvr_priv.h"
uint32_t pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_DEVS];
uint32_t pios_rcvr_max_channel;
#endif /* PIOS_INCLUDE_RCVR */
extern const struct pios_com_driver pios_usb_com_driver; extern const struct pios_com_driver pios_usb_com_driver;
uint32_t pios_com_telem_rf_id; uint32_t pios_com_telem_rf_id;
uint32_t pios_com_telem_usb_id; uint32_t pios_com_telem_usb_id;
uint32_t pios_com_gps_id; uint32_t pios_com_gps_id;
uint32_t pios_com_spektrum_id;
uint32_t pios_com_sbus_id;
/** /**
* PIOS_Board_Init() * PIOS_Board_Init()
@ -885,70 +883,220 @@ uint32_t pios_com_sbus_id;
void PIOS_Board_Init(void) { void PIOS_Board_Init(void) {
/* Delay system */ /* Delay system */
PIOS_DELAY_Init(); PIOS_DELAY_Init();
/* Set up the SPI interface to the serial flash */ /* Set up the SPI interface to the serial flash */
if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg)) { if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_Assert(0);
} }
PIOS_Flash_W25X_Init(pios_spi_flash_accel_id); PIOS_Flash_W25X_Init(pios_spi_flash_accel_id);
PIOS_ADXL345_Attach(pios_spi_flash_accel_id); PIOS_ADXL345_Attach(pios_spi_flash_accel_id);
PIOS_FLASHFS_Init(); PIOS_FLASHFS_Init();
#if defined(PIOS_INCLUDE_SPEKTRUM)
/* SPEKTRUM init must come before comms */
PIOS_SPEKTRUM_Init();
if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_spektrum_id, &pios_usart_com_driver, pios_usart_spektrum_id)) {
PIOS_DEBUG_Assert(0);
}
#endif
/* Initialize UAVObject libraries */ /* Initialize UAVObject libraries */
EventDispatcherInitialize(); EventDispatcherInitialize();
UAVObjInitialize(); UAVObjInitialize();
UAVObjectsInitializeAll(); UAVObjectsInitializeAll();
#if defined(PIOS_INCLUDE_RTC)
/* Initialize the real-time clock and its associated tick */
PIOS_RTC_Init(&pios_rtc_main_cfg);
#endif
/* Initialize the alarms library */ /* Initialize the alarms library */
AlarmsInitialize(); AlarmsInitialize();
/* Initialize the task monitor library */ /* Initialize the task monitor library */
TaskMonitorInitialize(); TaskMonitorInitialize();
/* Initialize the PiOS library */ /* Configure the main IO port */
#if defined(PIOS_INCLUDE_COM) uint8_t hwsettings_cc_mainport;
#if defined(PIOS_INCLUDE_TELEMETRY_RF) HwSettingsCC_MainPortGet(&hwsettings_cc_mainport);
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) {
PIOS_DEBUG_Assert(0);
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_GPS)
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) {
PIOS_DEBUG_Assert(0);
}
#endif /* PIOS_INCLUDE_GPS */
#if defined(PIOS_INCLUDE_SBUS)
PIOS_SBUS_Init();
if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_cfg)) { switch (hwsettings_cc_mainport) {
PIOS_DEBUG_Assert(0); case HWSETTINGS_CC_MAINPORT_DISABLED:
break;
case HWSETTINGS_CC_MAINPORT_TELEMETRY:
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
{
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) {
PIOS_Assert(0);
}
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
break;
case HWSETTINGS_CC_MAINPORT_SBUS:
#if defined(PIOS_INCLUDE_SBUS)
{
PIOS_SBUS_Init(&pios_sbus_cfg);
uint32_t pios_usart_sbus_id;
if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_SBUS */
break;
case HWSETTINGS_CC_MAINPORT_GPS:
#if defined(PIOS_INCLUDE_GPS)
{
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_main_cfg)) {
PIOS_Assert(0);
}
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
break;
case HWSETTINGS_CC_MAINPORT_SPEKTRUM:
#if defined(PIOS_INCLUDE_SPEKTRUM)
{
/* SPEKTRUM init must come before usart init since it may use Rx pin for bind */
PIOS_SPEKTRUM_Init(&pios_spektrum_main_cfg, false);
uint32_t pios_usart_spektrum_id;
if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_main_cfg)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_SPEKTRUM */
break;
case HWSETTINGS_CC_MAINPORT_COMAUX:
break;
} }
if (PIOS_COM_Init(&pios_com_sbus_id, &pios_usart_com_driver, pios_usart_sbus_id)) {
PIOS_DEBUG_Assert(0); /* Configure the flexi port */
uint8_t hwsettings_cc_flexiport;
HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport);
switch (hwsettings_cc_flexiport) {
case HWSETTINGS_CC_FLEXIPORT_DISABLED:
break;
case HWSETTINGS_CC_FLEXIPORT_TELEMETRY:
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
{
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_flexi_cfg)) {
PIOS_Assert(0);
}
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
break;
case HWSETTINGS_CC_FLEXIPORT_GPS:
#if defined(PIOS_INCLUDE_GPS)
{
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_flexi_cfg)) {
PIOS_Assert(0);
}
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
break;
case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM:
#if defined(PIOS_INCLUDE_SPEKTRUM)
{
/* SPEKTRUM init must come before usart init since it may use Rx pin for bind */
PIOS_SPEKTRUM_Init(&pios_spektrum_flexi_cfg, false);
uint32_t pios_usart_spektrum_id;
if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_flexi_cfg)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_SPEKTRUM */
break;
case HWSETTINGS_CC_FLEXIPORT_COMAUX:
break;
case HWSETTINGS_CC_FLEXIPORT_I2C:
#if defined(PIOS_INCLUDE_I2C)
{
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_I2C */
break;
} }
/* Configure the selected receiver */
uint8_t manualcontrolsettings_inputmode;
ManualControlSettingsInputModeGet(&manualcontrolsettings_inputmode);
switch (manualcontrolsettings_inputmode) {
case MANUALCONTROLSETTINGS_INPUTMODE_PWM:
#if defined(PIOS_INCLUDE_PWM)
PIOS_PWM_Init();
for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS && i < PIOS_RCVR_MAX_DEVS; i++) {
if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel],
&pios_pwm_rcvr_driver,
i)) {
pios_rcvr_max_channel++;
} else {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_PWM */
break;
case MANUALCONTROLSETTINGS_INPUTMODE_PPM:
#if defined(PIOS_INCLUDE_PPM)
PIOS_PPM_Init();
for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS && i < PIOS_RCVR_MAX_DEVS; i++) {
if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel],
&pios_ppm_rcvr_driver,
i)) {
pios_rcvr_max_channel++;
} else {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_PPM */
break;
case MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM:
#if defined(PIOS_INCLUDE_SPEKTRUM)
if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SPEKTRUM ||
hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_SPEKTRUM) {
for (uint8_t i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS && i < PIOS_RCVR_MAX_DEVS; i++) {
if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel],
&pios_spektrum_rcvr_driver,
i)) {
pios_rcvr_max_channel++;
} else {
PIOS_Assert(0);
}
}
}
#endif /* PIOS_INCLUDE_SPEKTRUM */
break;
case MANUALCONTROLSETTINGS_INPUTMODE_SBUS:
#if defined(PIOS_INCLUDE_SBUS)
if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SBUS) {
for (uint8_t i = 0; i < SBUS_NUMBER_OF_CHANNELS && i < PIOS_RCVR_MAX_DEVS; i++) {
if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel],
&pios_sbus_rcvr_driver,
i)) {
pios_rcvr_max_channel++;
} else {
PIOS_Assert(0);
}
}
}
#endif /* PIOS_INCLUDE_SBUS */ #endif /* PIOS_INCLUDE_SBUS */
#endif /* PIOS_INCLUDE_COM */ break;
}
/* Remap AFIO pin */ /* Remap AFIO pin */
GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);
@ -957,26 +1105,15 @@ void PIOS_Board_Init(void) {
PIOS_ADC_Init(); PIOS_ADC_Init();
PIOS_GPIO_Init(); PIOS_GPIO_Init();
#if defined(PIOS_INCLUDE_PWM)
PIOS_PWM_Init();
#endif
#if defined(PIOS_INCLUDE_PPM)
PIOS_PPM_Init();
#endif
#if defined(PIOS_INCLUDE_USB_HID) #if defined(PIOS_INCLUDE_USB_HID)
PIOS_USB_HID_Init(0); PIOS_USB_HID_Init(0);
#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_COM)
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) { if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) {
PIOS_DEBUG_Assert(0); PIOS_Assert(0);
} }
#endif /* PIOS_INCLUDE_COM */ #endif /* PIOS_INCLUDE_COM */
#endif #endif /* PIOS_INCLUDE_USB_HID */
#if defined(PIOS_INCLUDE_I2C)
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
}
#endif /* PIOS_INCLUDE_I2C */
PIOS_IAP_Init(); PIOS_IAP_Init();
PIOS_WDG_Init(); PIOS_WDG_Init();
} }

51
flight/INS/pios_board.c
View File

@ -61,7 +61,7 @@ static const struct pios_spi_cfg pios_spi_op_mag_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_SPI_op_mag_irq_handler, .handler = NULL,
.flags = .flags =
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
DMA1_FLAG_GL4), DMA1_FLAG_GL4),
@ -175,7 +175,7 @@ static const struct pios_spi_cfg pios_spi_accel_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_SPI_accel_irq_handler, .handler = NULL,
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel2_IRQn, .NVIC_IRQChannel = DMA1_Channel2_IRQn,
@ -265,17 +265,10 @@ void PIOS_SPI_accel_irq_handler(void)
/* /*
* GPS USART * GPS USART
*/ */
void PIOS_USART_gps_irq_handler(void); static const struct pios_usart_cfg pios_usart_gps_cfg = {
void USART1_IRQHandler()
__attribute__ ((alias("PIOS_USART_gps_irq_handler")));
const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = USART1, .regs = USART1,
.init = { .init = {
#if defined (PIOS_USART_BAUDRATE)
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600, .USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No, .USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1, .USART_StopBits = USART_StopBits_1,
@ -284,7 +277,7 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx, .USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_gps_irq_handler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -310,29 +303,16 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
}, },
}; };
static uint32_t pios_usart_gps_id;
void PIOS_USART_gps_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_gps_id);
}
#endif /* PIOS_INCLUDE_GPS */ #endif /* PIOS_INCLUDE_GPS */
#ifdef PIOS_COM_AUX #ifdef PIOS_COM_AUX
/* /*
* AUX USART * AUX USART
*/ */
void PIOS_USART_aux_irq_handler(void); static const struct pios_usart_cfg pios_usart_aux_cfg = {
void USART4_IRQHandler()
__attribute__ ((alias("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART4, .regs = USART4,
.init = { .init = {
#if defined (PIOS_USART_BAUDRATE)
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600, .USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No, .USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1, .USART_StopBits = USART_StopBits_1,
@ -341,7 +321,7 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx, .USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_aux_irq_handler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannel = USART4_IRQn, .NVIC_IRQChannel = USART4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
@ -367,12 +347,6 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
}, },
}; };
static uint32_t pios_usart_aux_id;
void PIOS_USART_aux_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_aux_id);
}
#endif /* PIOS_COM_AUX */ #endif /* PIOS_COM_AUX */
@ -391,7 +365,7 @@ void I2C1_EV_IRQHandler()
void I2C1_ER_IRQHandler() void I2C1_ER_IRQHandler()
__attribute__ ((alias("PIOS_I2C_pres_mag_adapter_er_irq_handler"))); __attribute__ ((alias("PIOS_I2C_pres_mag_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = { static const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
.regs = I2C1, .regs = I2C1,
.init = { .init = {
.I2C_Mode = I2C_Mode_I2C, .I2C_Mode = I2C_Mode_I2C,
@ -419,7 +393,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
}, },
}, },
.event = { .event = {
.handler = PIOS_I2C_pres_mag_adapter_ev_irq_handler, .handler = NULL,
.flags = 0, /* FIXME: check this */ .flags = 0, /* FIXME: check this */
.init = { .init = {
.NVIC_IRQChannel = I2C1_EV_IRQn, .NVIC_IRQChannel = I2C1_EV_IRQn,
@ -429,7 +403,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
}, },
}, },
.error = { .error = {
.handler = PIOS_I2C_pres_mag_adapter_er_irq_handler, .handler = NULL,
.flags = 0, /* FIXME: check this */ .flags = 0, /* FIXME: check this */
.init = { .init = {
.NVIC_IRQChannel = I2C1_ER_IRQn, .NVIC_IRQChannel = I2C1_ER_IRQn,
@ -459,7 +433,7 @@ void PIOS_I2C_gyro_adapter_er_irq_handler(void);
void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_ev_irq_handler"))); void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_ev_irq_handler")));
void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_er_irq_handler"))); void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = { static const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
.regs = I2C2, .regs = I2C2,
.init = { .init = {
.I2C_Mode = I2C_Mode_I2C, .I2C_Mode = I2C_Mode_I2C,
@ -487,7 +461,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
}, },
}, },
.event = { .event = {
.handler = PIOS_I2C_gyro_adapter_ev_irq_handler, .handler = NULL,
.flags = 0, /* FIXME: check this */ .flags = 0, /* FIXME: check this */
.init = { .init = {
.NVIC_IRQChannel = I2C2_EV_IRQn, .NVIC_IRQChannel = I2C2_EV_IRQn,
@ -497,7 +471,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
}, },
}, },
.error = { .error = {
.handler = PIOS_I2C_gyro_adapter_er_irq_handler, .handler = NULL,
.flags = 0, /* FIXME: check this */ .flags = 0, /* FIXME: check this */
.init = { .init = {
.NVIC_IRQChannel = I2C2_ER_IRQn, .NVIC_IRQChannel = I2C2_ER_IRQn,
@ -547,6 +521,7 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_GPS)
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }

1253
flight/Modules/ManualControl/manualcontrol.c
View File

@ -1,630 +1,623 @@
/** /**
****************************************************************************** ******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules * @addtogroup OpenPilotModules OpenPilot Modules
* @{ * @{
* @addtogroup ManualControlModule Manual Control Module * @addtogroup ManualControlModule Manual Control Module
* @brief Provide manual control or allow it alter flight mode. * @brief Provide manual control or allow it alter flight mode.
* @{ * @{
* *
* Reads in the ManualControlCommand FlightMode setting from receiver then either * Reads in the ManualControlCommand FlightMode setting from receiver then either
* pass the settings straght to ActuatorDesired object (manual mode) or to * pass the settings straght to ActuatorDesired object (manual mode) or to
* AttitudeDesired object (stabilized mode) * AttitudeDesired object (stabilized mode)
* *
* @file manualcontrol.c * @file manualcontrol.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief ManualControl module. Handles safety R/C link and flight mode. * @brief ManualControl module. Handles safety R/C link and flight mode.
* *
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ *****************************************************************************/
/* /*
* This program is free software; you can redistribute it and/or modify * This program 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.
* *
* This program is distributed in the hope that it will be useful, but * This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details. * for more details.
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#include "openpilot.h" #include "openpilot.h"
#include "manualcontrol.h" #include "manualcontrol.h"
#include "manualcontrolsettings.h" #include "manualcontrolsettings.h"
#include "stabilizationsettings.h" #include "stabilizationsettings.h"
#include "manualcontrolcommand.h" #include "manualcontrolcommand.h"
#include "actuatordesired.h" #include "actuatordesired.h"
#include "stabilizationdesired.h" #include "stabilizationdesired.h"
#include "flighttelemetrystats.h" #include "flighttelemetrystats.h"
#include "flightstatus.h" #include "flightstatus.h"
#include "accessorydesired.h" #include "accessorydesired.h"
// Private constants // Private constants
#if defined(PIOS_MANUAL_STACK_SIZE) #if defined(PIOS_MANUAL_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE #define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
#else #else
#define STACK_SIZE_BYTES 824 #define STACK_SIZE_BYTES 824
#endif #endif
#define TASK_PRIORITY (tskIDLE_PRIORITY+4) #define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define UPDATE_PERIOD_MS 20 #define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1 #define THROTTLE_FAILSAFE -0.1
#define FLIGHT_MODE_LIMIT 1.0/3.0 #define FLIGHT_MODE_LIMIT 1.0/3.0
#define ARMED_TIME_MS 1000 #define ARMED_TIME_MS 1000
#define ARMED_THRESHOLD 0.50 #define ARMED_THRESHOLD 0.50
//safe band to allow a bit of calibration error or trim offset (in microseconds) //safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 150 #define CONNECTION_OFFSET 150
// Private types // Private types
typedef enum typedef enum
{ {
ARM_STATE_DISARMED, ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED, ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT ARM_STATE_DISARMING_TIMEOUT
} ArmState_t; } ArmState_t;
// Private variables // Private variables
static xTaskHandle taskHandle; static xTaskHandle taskHandle;
static ArmState_t armState; static ArmState_t armState;
static portTickType lastSysTime; static portTickType lastSysTime;
// Private functions // Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode); static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void manualControlTask(void *parameters); static void manualControlTask(void *parameters);
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool okToArm(void); static bool okToArm(void);
static bool validInputRange(int16_t min, int16_t max, uint16_t value); static bool validInputRange(int16_t min, int16_t max, uint16_t value);
#define assumptions1 ( \ #define assumptions1 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
) )
#define assumptions3 ( \ #define assumptions3 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
) )
#define assumptions5 ( \ #define assumptions5 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
) )
#define ARMING_CHANNEL_ROLL 0 #define ARMING_CHANNEL_ROLL 0
#define ARMING_CHANNEL_PITCH 1 #define ARMING_CHANNEL_PITCH 1
#define ARMING_CHANNEL_YAW 2 #define ARMING_CHANNEL_YAW 2
#define assumptions7 ( \ #define assumptions7 ( \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \ ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \
) )
#define assumptions8 ( \ #define assumptions8 ( \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \ ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \
) )
#define assumptions_flightmode ( \ #define assumptions_flightmode ( \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
) )
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) #define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
/** /**
* Module initialization * Module initialization
*/ */
int32_t ManualControlInitialize() int32_t ManualControlInitialize()
{ {
/* Check the assumptions about uavobject enum's are correct */ /* Check the assumptions about uavobject enum's are correct */
if(!assumptions) if(!assumptions)
return -1; return -1;
// Start main task // Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0; return 0;
} }
/** /**
* Module task * Module task
*/ */
static void manualControlTask(void *parameters) static void manualControlTask(void *parameters)
{ {
ManualControlSettingsData settings; ManualControlSettingsData settings;
ManualControlCommandData cmd; ManualControlCommandData cmd;
FlightStatusData flightStatus; FlightStatusData flightStatus;
float flightMode = 0; float flightMode = 0;
uint8_t disconnected_count = 0; uint8_t disconnected_count = 0;
uint8_t connected_count = 0; uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used // this includes not even registering it if not used
AccessoryDesiredCreateInstance(); AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance(); AccessoryDesiredCreateInstance();
// Make sure unarmed on power up // Make sure unarmed on power up
ManualControlCommandGet(&cmd); ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED; armState = ARM_STATE_DISARMED;
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
while (1) { while (1) {
float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM];
// Wait until next update // Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
// Read settings // Read settings
ManualControlSettingsGet(&settings); ManualControlSettingsGet(&settings);
if (ManualControlCommandReadOnly(&cmd)) { if (ManualControlCommandReadOnly(&cmd)) {
FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats); FlightTelemetryStatsGet(&flightTelemStats);
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */ /* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata; UAVObjMetadata metadata;
UAVObjGetMetadata(&cmd, &metadata); UAVObjGetMetadata(&cmd, &metadata);
metadata.access = ACCESS_READWRITE; metadata.access = ACCESS_READWRITE;
UAVObjSetMetadata(&cmd, &metadata); UAVObjSetMetadata(&cmd, &metadata);
} }
} }
if (!ManualControlCommandReadOnly(&cmd)) { if (!ManualControlCommandReadOnly(&cmd)) {
// Read channel values in us // Read channel values in us
// TODO: settings.InputMode is currently ignored because PIOS will not allow runtime for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
// selection of PWM and PPM. The configuration is currently done at compile time in if (pios_rcvr_channel_to_id_map[n]) {
// the pios_config.h file. cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_channel_to_id_map[n]);
for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { } else {
#if defined(PIOS_INCLUDE_PWM) cmd.Channel[n] = -1;
cmd.Channel[n] = PIOS_PWM_Get(n); }
#elif defined(PIOS_INCLUDE_PPM) scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
cmd.Channel[n] = PIOS_PPM_Get(n); }
#elif defined(PIOS_INCLUDE_SPEKTRUM)
cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); // Check settings, if error raise alarm
#elif defined(PIOS_INCLUDE_SBUS) if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
cmd.Channel[n] = PIOS_SBUS_Get(n); settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
#endif settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE ||
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
} settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
// Check settings, if error raise alarm cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || ManualControlCommandSet(&cmd);
settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || continue;
settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || }
settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { // decide if we have valid manual input or not
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) &&
ManualControlCommandSet(&cmd); validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
continue; validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
}
// Implement hysteresis loop on connection status
// decide if we have valid manual input or not if (valid_input_detected && (++connected_count > 10)) {
bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) && cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) && connected_count = 0;
validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) && disconnected_count = 0;
validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]); } else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
// Implement hysteresis loop on connection status connected_count = 0;
if (valid_input_detected && (++connected_count > 10)) { disconnected_count = 0;
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; }
connected_count = 0;
disconnected_count = 0; if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
} else if (!valid_input_detected && (++disconnected_count > 10)) { cmd.Throttle = -1; // Shut down engine with no control
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Roll = 0;
connected_count = 0; cmd.Yaw = 0;
disconnected_count = 0; cmd.Pitch = 0;
} //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { // or leave throttle at IDLE speed or above when going into AUTO-failsafe.
cmd.Throttle = -1; // Shut down engine with no control AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
cmd.Roll = 0; ManualControlCommandSet(&cmd);
cmd.Yaw = 0; } else {
cmd.Pitch = 0; AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, // Scale channels to -1 -> +1 range
// or leave throttle at IDLE speed or above when going into AUTO-failsafe. cmd.Roll = scaledChannel[settings.Roll];
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); cmd.Pitch = scaledChannel[settings.Pitch];
ManualControlCommandSet(&cmd); cmd.Yaw = scaledChannel[settings.Yaw];
} else { cmd.Throttle = scaledChannel[settings.Throttle];
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); flightMode = scaledChannel[settings.FlightMode];
// Scale channels to -1 -> +1 range AccessoryDesiredData accessory;
cmd.Roll = scaledChannel[settings.Roll]; // Set Accessory 0
cmd.Pitch = scaledChannel[settings.Pitch]; if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) {
cmd.Yaw = scaledChannel[settings.Yaw]; accessory.AccessoryVal = scaledChannel[settings.Accessory0];
cmd.Throttle = scaledChannel[settings.Throttle]; if(AccessoryDesiredInstSet(0, &accessory) != 0)
flightMode = scaledChannel[settings.FlightMode]; AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
AccessoryDesiredData accessory; // Set Accessory 1
// Set Accessory 0 if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) {
if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) { accessory.AccessoryVal = scaledChannel[settings.Accessory1];
accessory.AccessoryVal = scaledChannel[settings.Accessory0]; if(AccessoryDesiredInstSet(1, &accessory) != 0)
if(AccessoryDesiredInstSet(0, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); }
} // Set Accsesory 2
// Set Accessory 1 if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) {
if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) { accessory.AccessoryVal = scaledChannel[settings.Accessory2];
accessory.AccessoryVal = scaledChannel[settings.Accessory1]; if(AccessoryDesiredInstSet(2, &accessory) != 0)
if(AccessoryDesiredInstSet(1, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); }
}
// Set Accsesory 2
if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) { processFlightMode(&settings, flightMode);
accessory.AccessoryVal = scaledChannel[settings.Accessory2]; processArm(&cmd, &settings);
if(AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); // Update cmd object
} ManualControlCommandSet(&cmd);
}
processFlightMode(&settings, flightMode);
processArm(&cmd, &settings); } else {
ManualControlCommandGet(&cmd); /* Under GCS control */
// Update cmd object }
ManualControlCommandSet(&cmd);
} FlightStatusGet(&flightStatus);
} else { // Depending on the mode update the Stabilization or Actuator objects
ManualControlCommandGet(&cmd); /* Under GCS control */ switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
} case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
FlightStatusGet(&flightStatus); break;
case FLIGHTMODE_MANUAL:
// Depending on the mode update the Stabilization or Actuator objects updateActuatorDesired(&cmd);
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { break;
case FLIGHTMODE_UNDEFINED: case FLIGHTMODE_STABILIZED:
// This reflects a bug in the code architecture! updateStabilizationDesired(&cmd, &settings);
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); break;
break; case FLIGHTMODE_GUIDANCE:
case FLIGHTMODE_MANUAL: // TODO: Implement
updateActuatorDesired(&cmd); break;
break; }
case FLIGHTMODE_STABILIZED: }
updateStabilizationDesired(&cmd, &settings); }
break;
case FLIGHTMODE_GUIDANCE: static void updateActuatorDesired(ManualControlCommandData * cmd)
// TODO: Implement {
break; ActuatorDesiredData actuator;
} ActuatorDesiredGet(&actuator);
} actuator.Roll = cmd->Roll;
} actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw;
static void updateActuatorDesired(ManualControlCommandData * cmd) actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
{ ActuatorDesiredSet(&actuator);
ActuatorDesiredData actuator; }
ActuatorDesiredGet(&actuator);
actuator.Roll = cmd->Roll; static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
actuator.Pitch = cmd->Pitch; {
actuator.Yaw = cmd->Yaw; StabilizationDesiredData stabilization;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; StabilizationDesiredGet(&stabilization);
ActuatorDesiredSet(&actuator);
} StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{ uint8_t * stab_settings;
StabilizationDesiredData stabilization; FlightStatusData flightStatus;
StabilizationDesiredGet(&stabilization); FlightStatusGet(&flightStatus);
switch(flightStatus.FlightMode) {
StabilizationSettingsData stabSettings; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
StabilizationSettingsGet(&stabSettings); stab_settings = settings->Stabilization1Settings;
break;
uint8_t * stab_settings; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
FlightStatusData flightStatus; stab_settings = settings->Stabilization2Settings;
FlightStatusGet(&flightStatus); break;
switch(flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: stab_settings = settings->Stabilization3Settings;
stab_settings = settings->Stabilization1Settings; break;
break; default:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: // Major error, this should not occur because only enter this block when one of these is true
stab_settings = settings->Stabilization2Settings; AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break; return;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: }
stab_settings = settings->Stabilization3Settings;
break; // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
default: stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
// Major error, this should not occur because only enter this block when one of these is true stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
return;
} stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; 0; // this is an invalid mode
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; ;
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : 0; // this is an invalid mode
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
; (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : 0; // this is an invalid mode
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : }
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) : /**
0; // this is an invalid mode * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
StabilizationDesiredSet(&stabilization); {
} float valueScaled;
/** // Scale
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. if ((max > min && value >= neutral) || (min > max && value <= neutral))
*/ {
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral) if (max != neutral)
{ valueScaled = (float)(value - neutral) / (float)(max - neutral);
float valueScaled; else
valueScaled = 0;
// Scale }
if ((max > min && value >= neutral) || (min > max && value <= neutral)) else
{ {
if (max != neutral) if (min != neutral)
valueScaled = (float)(value - neutral) / (float)(max - neutral); valueScaled = (float)(value - neutral) / (float)(neutral - min);
else else
valueScaled = 0; valueScaled = 0;
} }
else
{ // Bound
if (min != neutral) if (valueScaled > 1.0) valueScaled = 1.0;
valueScaled = (float)(value - neutral) / (float)(neutral - min); else
else if (valueScaled < -1.0) valueScaled = -1.0;
valueScaled = 0;
} return valueScaled;
}
// Bound
if (valueScaled > 1.0) valueScaled = 1.0; static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
else if(end_time > start_time)
if (valueScaled < -1.0) valueScaled = -1.0; return (end_time - start_time) * portTICK_RATE_MS;
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
return valueScaled; }
}
/**
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { * @brief Determine if the aircraft is safe to arm
if(end_time > start_time) * @returns True if safe to arm, false otherwise
return (end_time - start_time) * portTICK_RATE_MS; */
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; static bool okToArm(void)
} {
// read alarms
/** SystemAlarmsData alarms;
* @brief Determine if the aircraft is safe to arm SystemAlarmsGet(&alarms);
* @returns True if safe to arm, false otherwise
*/
static bool okToArm(void) // Check each alarm
{ for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
// read alarms {
SystemAlarmsData alarms; if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
SystemAlarmsGet(&alarms); { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
continue;
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) return false;
{ }
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) }
{ // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) return true;
continue; }
return false; /**
} * @brief Update the flightStatus object only if value changed. Reduces callbacks
} * @param[in] val The new value
*/
return true; static void setArmedIfChanged(uint8_t val) {
} FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks if(flightStatus.Armed != val) {
* @param[in] val The new value flightStatus.Armed = val;
*/ FlightStatusSet(&flightStatus);
static void setArmedIfChanged(uint8_t val) { }
FlightStatusData flightStatus; }
FlightStatusGet(&flightStatus);
/**
if(flightStatus.Armed != val) { * @brief Process the inputs and determine whether to arm or not
flightStatus.Armed = val; * @param[out] cmd The structure to set the armed in
FlightStatusSet(&flightStatus); * @param[in] settings Settings indicating the necessary position
} */
} static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
/**
* @brief Process the inputs and determine whether to arm or not bool lowThrottle = cmd->Throttle <= 0;
* @param[out] cmd The structure to set the armed in
* @param[in] settings Settings indicating the necessary position if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
*/ // In this configuration we always disarm
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
{ } else {
// Not really needed since this function not called when disconnected
bool lowThrottle = cmd->Throttle <= 0; if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
return;
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm // The throttle is not low, in case we where arming or disarming, abort
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); if (!lowThrottle) {
} else { switch(armState) {
// Not really needed since this function not called when disconnected case ARM_STATE_DISARMING_MANUAL:
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) case ARM_STATE_DISARMING_TIMEOUT:
return; armState = ARM_STATE_ARMED;
break;
// The throttle is not low, in case we where arming or disarming, abort case ARM_STATE_ARMING_MANUAL:
if (!lowThrottle) { armState = ARM_STATE_DISARMED;
switch(armState) { break;
case ARM_STATE_DISARMING_MANUAL: default:
case ARM_STATE_DISARMING_TIMEOUT: // Nothing needs to be done in the other states
armState = ARM_STATE_ARMED; break;
break; }
case ARM_STATE_ARMING_MANUAL: return;
armState = ARM_STATE_DISARMED; }
break;
default: // The rest of these cases throttle is low
// Nothing needs to be done in the other states if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
break; // In this configuration, we go into armed state as soon as the throttle is low, never disarm
} setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return; return;
} }
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { // When the configuration is not "Always armed" and no "Always disarmed",
// In this configuration, we go into armed state as soon as the throttle is low, never disarm // the state will not be changed when the throttle is not low
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); static portTickType armedDisarmStart;
return; float armingInputLevel = 0;
}
// Calc channel see assumptions7
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
// When the configuration is not "Always armed" and no "Always disarmed", switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
// the state will not be changed when the throttle is not low case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
static portTickType armedDisarmStart; case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
float armingInputLevel = 0; case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
}
// Calc channel see assumptions7
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1; bool manualArm = false;
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { bool manualDisarm = false;
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break; if (armingInputLevel <= -ARMED_THRESHOLD)
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break; manualArm = true;
} else if (armingInputLevel >= +ARMED_THRESHOLD)
manualDisarm = true;
bool manualArm = false;
bool manualDisarm = false; switch(armState) {
case ARM_STATE_DISARMED:
if (armingInputLevel <= -ARMED_THRESHOLD) setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD) // only allow arming if it's OK too
manualDisarm = true; if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
switch(armState) { armState = ARM_STATE_ARMING_MANUAL;
case ARM_STATE_DISARMED: }
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); break;
// only allow arming if it's OK too case ARM_STATE_ARMING_MANUAL:
if (manualArm && okToArm()) { setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL; if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
} armState = ARM_STATE_ARMED;
break; else if (!manualArm)
armState = ARM_STATE_DISARMED;
case ARM_STATE_ARMING_MANUAL: break;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
case ARM_STATE_ARMED:
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) // When we get here, the throttle is low,
armState = ARM_STATE_ARMED; // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
else if (!manualArm) armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMED; armState = ARM_STATE_DISARMING_TIMEOUT;
break; setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low, case ARM_STATE_DISARMING_TIMEOUT:
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled // We get here when armed while throttle low, even when the arming timeout is not enabled
armedDisarmStart = lastSysTime; if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armState = ARM_STATE_DISARMING_TIMEOUT; armState = ARM_STATE_DISARMED;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break; // Switch to disarming due to manual control when needed
if (manualDisarm) {
case ARM_STATE_DISARMING_TIMEOUT: armedDisarmStart = lastSysTime;
// We get here when armed while throttle low, even when the arming timeout is not enabled armState = ARM_STATE_DISARMING_MANUAL;
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) }
armState = ARM_STATE_DISARMED; break;
// Switch to disarming due to manual control when needed case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm) { if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMED;
armState = ARM_STATE_DISARMING_MANUAL; else if (!manualDisarm)
} armState = ARM_STATE_ARMED;
break; break;
} // End Switch
case ARM_STATE_DISARMING_MANUAL: }
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) }
armState = ARM_STATE_DISARMED;
else if (!manualDisarm) /**
armState = ARM_STATE_ARMED; * @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
break; * @param[out] cmd Pointer to the command structure to set the flight mode in
} // End Switch * @param[in] settings The settings which indicate which position is which mode
} * @param[in] flightMode the value of the switch position
} */
static void processFlightMode(ManualControlSettingsData * settings, float flightMode)
/** {
* @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly FlightStatusData flightStatus;
* @param[out] cmd Pointer to the command structure to set the flight mode in FlightStatusGet(&flightStatus);
* @param[in] settings The settings which indicate which position is which mode
* @param[in] flightMode the value of the switch position uint8_t newMode;
*/ // Note here the code is ass
static void processFlightMode(ManualControlSettingsData * settings, float flightMode) if (flightMode < -FLIGHT_MODE_LIMIT)
{ newMode = settings->FlightModePosition[0];
FlightStatusData flightStatus; else if (flightMode > FLIGHT_MODE_LIMIT)
FlightStatusGet(&flightStatus); newMode = settings->FlightModePosition[2];
else
uint8_t newMode; newMode = settings->FlightModePosition[1];
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT) if(flightStatus.FlightMode != newMode) {
newMode = settings->FlightModePosition[0]; flightStatus.FlightMode = newMode;
else if (flightMode > FLIGHT_MODE_LIMIT) FlightStatusSet(&flightStatus);
newMode = settings->FlightModePosition[2]; }
else
newMode = settings->FlightModePosition[1]; }
if(flightStatus.FlightMode != newMode) { /**
flightStatus.FlightMode = newMode; * @brief Determine if the manual input value is within acceptable limits
FlightStatusSet(&flightStatus); * @returns return TRUE if so, otherwise return FALSE
} */
bool validInputRange(int16_t min, int16_t max, uint16_t value)
} {
if (min > max)
/** {
* @brief Determine if the manual input value is within acceptable limits int16_t tmp = min;
* @returns return TRUE if so, otherwise return FALSE min = max;
*/ max = tmp;
bool validInputRange(int16_t min, int16_t max, uint16_t value) }
{ return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
if (min > max) }
{
int16_t tmp = min; /**
min = max; * @}
max = tmp; * @}
} */
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
}
/**
* @}
* @}
*/

13
flight/OpenPilot/Makefile
View File

@ -43,10 +43,6 @@ ENABLE_DEBUG_PINS ?= NO
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs # Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
ENABLE_AUX_UART ?= NO ENABLE_AUX_UART ?= NO
USE_SPEKTRUM ?= NO
USE_SBUS ?= NO
# Set to YES when using Code Sourcery toolchain # Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES CODE_SOURCERY ?= YES
@ -196,6 +192,7 @@ SRC += $(PIOSCOMMON)/pios_hcsr04.c
SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_i2c_esc.c
SRC += $(PIOSCOMMON)/pios_iap.c SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/pios_bl_helper.c SRC += $(PIOSCOMMON)/pios_bl_helper.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/printf-stdarg.c SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(FLIGHTLIB)/ahrs_spi_comm.c SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
SRC += $(FLIGHTLIB)/ahrs_comm_objects.c SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
@ -365,14 +362,6 @@ ifeq ($(ENABLE_AUX_UART), YES)
CDEFS += -DPIOS_ENABLE_AUX_UART CDEFS += -DPIOS_ENABLE_AUX_UART
endif endif
ifeq ($(USE_SPEKTRUM), YES)
CDEFS += -DUSE_SPEKTRUM
endif
ifeq ($(USE_SBUS), YES)
CDEFS += -DUSE_SBUS
endif
# Place project-specific -D and/or -U options for # Place project-specific -D and/or -U options for
# Assembler with preprocessor here. # Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER #ADEFS = -DUSE_IRQ_ASM_WRAPPER

10
flight/OpenPilot/System/inc/pios_config.h
View File

@ -42,14 +42,12 @@
#define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED #define PIOS_INCLUDE_LED
#if defined(USE_SPEKTRUM) #define PIOS_INCLUDE_RCVR
#define PIOS_INCLUDE_SPEKTRUM #define PIOS_INCLUDE_SPEKTRUM
#elif defined(USE_SBUS) //#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_SBUS
#else
//#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_PWM #define PIOS_INCLUDE_PWM
#endif //#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_SERVO #define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_SPI

274
flight/OpenPilot/System/pios_board.c
View File

@ -46,7 +46,7 @@
void PIOS_SPI_sdcard_irq_handler(void); void PIOS_SPI_sdcard_irq_handler(void);
void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler"))); void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler")));
void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler"))); void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler")));
const struct pios_spi_cfg pios_spi_sdcard_cfg = { static const struct pios_spi_cfg pios_spi_sdcard_cfg = {
.regs = SPI1, .regs = SPI1,
.init = { .init = {
.SPI_Mode = SPI_Mode_Master, .SPI_Mode = SPI_Mode_Master,
@ -63,7 +63,7 @@ const struct pios_spi_cfg pios_spi_sdcard_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_SPI_sdcard_irq_handler, .handler = NULL,
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel2_IRQn, .NVIC_IRQChannel = DMA1_Channel2_IRQn,
@ -144,7 +144,7 @@ const struct pios_spi_cfg pios_spi_sdcard_cfg = {
void PIOS_SPI_ahrs_irq_handler(void); void PIOS_SPI_ahrs_irq_handler(void);
void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler"))); void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler"))); void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
const struct pios_spi_cfg pios_spi_ahrs_cfg = { static const struct pios_spi_cfg pios_spi_ahrs_cfg = {
.regs = SPI2, .regs = SPI2,
.init = { .init = {
.SPI_Mode = SPI_Mode_Master, .SPI_Mode = SPI_Mode_Master,
@ -162,7 +162,7 @@ const struct pios_spi_cfg pios_spi_ahrs_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_SPI_ahrs_irq_handler, .handler = NULL,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn, .NVIC_IRQChannel = DMA1_Channel4_IRQn,
@ -258,11 +258,11 @@ void PIOS_SPI_ahrs_irq_handler(void)
extern void PIOS_ADC_handler(void); extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one // Remap the ADC DMA handler to this one
const struct pios_adc_cfg pios_adc_cfg = { static const struct pios_adc_cfg pios_adc_cfg = {
.dma = { .dma = {
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_ADC_DMA_Handler, .handler = NULL,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn, .NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -310,16 +310,10 @@ void PIOS_ADC_handler() {
/* /*
* Telemetry USART * Telemetry USART
*/ */
void PIOS_USART_telem_irq_handler(void); static const struct pios_usart_cfg pios_usart_telem_cfg = {
void USART2_IRQHandler() __attribute__ ((alias ("PIOS_USART_telem_irq_handler")));
const struct pios_usart_cfg pios_usart_telem_cfg = {
.regs = USART2, .regs = USART2,
.init = { .init = {
#if defined (PIOS_COM_TELEM_BAUDRATE) .USART_BaudRate = 57600,
.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No, .USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1, .USART_StopBits = USART_StopBits_1,
@ -327,7 +321,7 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx, .USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_telem_irq_handler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannel = USART2_IRQn, .NVIC_IRQChannel = USART2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -356,17 +350,11 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
/* /*
* GPS USART * GPS USART
*/ */
void PIOS_USART_gps_irq_handler(void); static const struct pios_usart_cfg pios_usart_gps_cfg = {
void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_gps_irq_handler")));
const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = USART3, .regs = USART3,
.remap = GPIO_PartialRemap_USART3, .remap = GPIO_PartialRemap_USART3,
.init = { .init = {
#if defined (PIOS_COM_GPS_BAUDRATE) .USART_BaudRate = 57600,
.USART_BaudRate = PIOS_COM_GPS_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No, .USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1, .USART_StopBits = USART_StopBits_1,
@ -374,7 +362,7 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx, .USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_gps_irq_handler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannel = USART3_IRQn, .NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -404,16 +392,9 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
/* /*
* AUX USART * AUX USART
*/ */
void PIOS_USART_aux_irq_handler(void); static const struct pios_usart_cfg pios_usart_aux_cfg = {
void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART1, .regs = USART1,
.init = { .init = {
#if defined (PIOS_COM_AUX_BAUDRATE)
.USART_BaudRate = PIOS_COM_AUX_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600, .USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No, .USART_Parity = USART_Parity_No,
@ -422,7 +403,7 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx, .USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_aux_irq_handler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -450,20 +431,45 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
}; };
#endif #endif
#if defined(PIOS_INCLUDE_RTC)
/*
* Realtime Clock (RTC)
*/
#include <pios_rtc_priv.h>
void PIOS_RTC_IRQ_Handler (void);
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler")));
static const struct pios_rtc_cfg pios_rtc_main_cfg = {
.clksrc = RCC_RTCCLKSource_HSE_Div128,
.prescaler = 100,
.irq = {
.handler = NULL,
.init = {
.NVIC_IRQChannel = RTC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
void PIOS_RTC_IRQ_Handler (void)
{
PIOS_RTC_irq_handler ();
}
#endif
#ifdef PIOS_COM_SPEKTRUM #ifdef PIOS_COM_SPEKTRUM
/* /*
* SPEKTRUM USART * SPEKTRUM USART
*/ */
void PIOS_USART_spektrum_irq_handler(void); #include <pios_spektrum_priv.h>
void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_spektrum_irq_handler")));
const struct pios_usart_cfg pios_usart_spektrum_cfg = { static const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.regs = USART1, .regs = USART1,
.init = { .init = {
#if defined (PIOS_COM_SPEKTRUM_BAUDRATE) .USART_BaudRate = 115200,
.USART_BaudRate = PIOS_COM_SPEKTRUM_BAUDRATE,
#else
.USART_BaudRate = 115200,
#endif
.USART_WordLength = USART_WordLength_8b, .USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No, .USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1, .USART_StopBits = USART_StopBits_1,
@ -471,7 +477,7 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.USART_Mode = USART_Mode_Rx, .USART_Mode = USART_Mode_Rx,
}, },
.irq = { .irq = {
.handler = PIOS_USART_spektrum_irq_handler, .handler = PIOS_SPEKTRUM_irq_handler,
.init = { .init = {
.NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
@ -497,79 +503,31 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
}, },
}; };
#include <pios_spektrum_priv.h>
static uint32_t pios_usart_spektrum_id; static uint32_t pios_usart_spektrum_id;
void PIOS_USART_spektrum_irq_handler(void) void PIOS_USART_spektrum_irq_handler(void)
{ {
SPEKTRUM_IRQHandler(pios_usart_spektrum_id); PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id);
} }
#include <pios_spektrum_priv.h> static const struct pios_spektrum_cfg pios_spektrum_cfg = {
void RTC_IRQHandler(); .bind = {
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_SUPV_irq_handler"))); .gpio = GPIOA,
const struct pios_spektrum_cfg pios_spektrum_cfg = { .init = {
.pios_usart_spektrum_cfg = &pios_usart_spektrum_cfg, .GPIO_Pin = GPIO_Pin_10,
.gpio_init = { //used for bind feature .GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_Out_PP, .GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
.remap = 0,
.irq = {
.handler = RTC_IRQHandler,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
}, },
}, },
.port = GPIOA, .remap = 0,
.pin = GPIO_Pin_10,
}; };
void PIOS_SUPV_irq_handler() {
if (RTC_GetITStatus(RTC_IT_SEC))
{
/* Call the right handler */
PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id);
/* Wait until last write operation on RTC registers has finished */
RTC_WaitForLastTask();
/* Clear the RTC Second interrupt */
RTC_ClearITPendingBit(RTC_IT_SEC);
}
}
#endif /* PIOS_COM_SPEKTRUM */ #endif /* PIOS_COM_SPEKTRUM */
#if defined(PIOS_INCLUDE_SBUS) #if defined(PIOS_INCLUDE_SBUS)
#error PIOS_INCLUDE_SBUS not implemented #error PIOS_INCLUDE_SBUS not implemented
#endif /* PIOS_INCLUDE_SBUS */ #endif /* PIOS_INCLUDE_SBUS */
static uint32_t pios_usart_telem_rf_id;
void PIOS_USART_telem_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_telem_rf_id);
}
static uint32_t pios_usart_gps_id;
void PIOS_USART_gps_irq_handler(void)
{
#ifdef USART_GPS_DEBUG_PIN
PIOS_DEBUG_PinHigh(USART_GPS_DEBUG_PIN);
#endif
PIOS_USART_IRQ_Handler(pios_usart_gps_id);
#ifdef USART_GPS_DEBUG_PIN
PIOS_DEBUG_PinLow(USART_GPS_DEBUG_PIN);
#endif
}
#ifdef PIOS_COM_AUX
static uint32_t pios_usart_aux_id;
void PIOS_USART_aux_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_aux_id);
}
#endif
#endif /* PIOS_INCLUDE_USART */ #endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_COM)
@ -582,7 +540,7 @@ void PIOS_USART_aux_irq_handler(void)
* Pios servo configuration structures * Pios servo configuration structures
*/ */
#include <pios_servo_priv.h> #include <pios_servo_priv.h>
const struct pios_servo_channel pios_servo_channels[] = { static const struct pios_servo_channel pios_servo_channels[] = {
{ {
.timer = TIM4, .timer = TIM4,
.port = GPIOB, .port = GPIOB,
@ -666,7 +624,7 @@ const struct pios_servo_cfg pios_servo_cfg = {
*/ */
#if defined(PIOS_INCLUDE_PWM) #if defined(PIOS_INCLUDE_PWM)
#include <pios_pwm_priv.h> #include <pios_pwm_priv.h>
const struct pios_pwm_channel pios_pwm_channels[] = { static const struct pios_pwm_channel pios_pwm_channels[] = {
{ {
.timer = TIM1, .timer = TIM1,
.port = GPIOA, .port = GPIOA,
@ -751,7 +709,7 @@ const struct pios_pwm_cfg pios_pwm_cfg = {
}, },
.remap = GPIO_PartialRemap_TIM3, .remap = GPIO_PartialRemap_TIM3,
.irq = { .irq = {
.handler = TIM1_CC_IRQHandler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelSubPriority = 0,
@ -782,7 +740,7 @@ void PIOS_TIM5_irq_handler()
#include <pios_ppm_priv.h> #include <pios_ppm_priv.h>
void TIM6_IRQHandler(); void TIM6_IRQHandler();
void TIM6_IRQHandler() __attribute__ ((alias ("PIOS_TIM6_irq_handler"))); void TIM6_IRQHandler() __attribute__ ((alias ("PIOS_TIM6_irq_handler")));
const struct pios_ppmsv_cfg pios_ppmsv_cfg = { static const struct pios_ppmsv_cfg pios_ppmsv_cfg = {
.tim_base_init = { .tim_base_init = {
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */ .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */
.TIM_ClockDivision = TIM_CKD_DIV1, .TIM_ClockDivision = TIM_CKD_DIV1,
@ -791,7 +749,7 @@ const struct pios_ppmsv_cfg pios_ppmsv_cfg = {
.TIM_RepetitionCounter = 0x0000, .TIM_RepetitionCounter = 0x0000,
}, },
.irq = { .irq = {
.handler = TIM6_IRQHandler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelSubPriority = 0,
@ -802,14 +760,14 @@ const struct pios_ppmsv_cfg pios_ppmsv_cfg = {
.ccr = TIM_IT_Update, .ccr = TIM_IT_Update,
}; };
void PIOS_TIM6_irq_handler() void PIOS_TIM6_irq_handler(void)
{ {
PIOS_PPMSV_irq_handler(); PIOS_PPMSV_irq_handler();
} }
void TIM1_CC_IRQHandler(); void TIM1_CC_IRQHandler();
void TIM1_CC_IRQHandler() __attribute__ ((alias ("PIOS_TIM1_CC_irq_handler"))); void TIM1_CC_IRQHandler() __attribute__ ((alias ("PIOS_TIM1_CC_irq_handler")));
const struct pios_ppm_cfg pios_ppm_cfg = { static const struct pios_ppm_cfg pios_ppm_cfg = {
.tim_base_init = { .tim_base_init = {
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */ .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */
.TIM_ClockDivision = TIM_CKD_DIV1, .TIM_ClockDivision = TIM_CKD_DIV1,
@ -831,7 +789,7 @@ const struct pios_ppm_cfg pios_ppm_cfg = {
}, },
.remap = 0, .remap = 0,
.irq = { .irq = {
.handler = TIM1_CC_IRQHandler, .handler = NULL,
.init = { .init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelSubPriority = 0,
@ -844,7 +802,7 @@ const struct pios_ppm_cfg pios_ppm_cfg = {
.ccr = TIM_IT_CC2, .ccr = TIM_IT_CC2,
}; };
void PIOS_TIM1_CC_irq_handler() void PIOS_TIM1_CC_irq_handler(void)
{ {
PIOS_PPM_irq_handler(); PIOS_PPM_irq_handler();
} }
@ -864,7 +822,7 @@ void PIOS_I2C_main_adapter_er_irq_handler(void);
void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler"))); void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler")));
void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler"))); void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
.regs = I2C2, .regs = I2C2,
.init = { .init = {
.I2C_Mode = I2C_Mode_I2C, .I2C_Mode = I2C_Mode_I2C,
@ -892,7 +850,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
}, },
}, },
.event = { .event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler, .handler = NULL,
.flags = 0, /* FIXME: check this */ .flags = 0, /* FIXME: check this */
.init = { .init = {
.NVIC_IRQChannel = I2C2_EV_IRQn, .NVIC_IRQChannel = I2C2_EV_IRQn,
@ -902,7 +860,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
}, },
}, },
.error = { .error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler, .handler = NULL,
.flags = 0, /* FIXME: check this */ .flags = 0, /* FIXME: check this */
.init = { .init = {
.NVIC_IRQChannel = I2C2_ER_IRQn, .NVIC_IRQChannel = I2C2_ER_IRQn,
@ -1013,6 +971,13 @@ static const struct stm32_gpio pios_debug_pins[] = {
#endif /* PIOS_ENABLE_DEBUG_PINS */ #endif /* PIOS_ENABLE_DEBUG_PINS */
#if defined(PIOS_INCLUDE_RCVR)
#include "pios_rcvr_priv.h"
uint32_t pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_DEVS];
uint32_t pios_rcvr_max_channel;
#endif /* PIOS_INCLUDE_RCVR */
extern const struct pios_com_driver pios_usb_com_driver; extern const struct pios_com_driver pios_usb_com_driver;
uint32_t pios_com_telem_rf_id; uint32_t pios_com_telem_rf_id;
@ -1042,7 +1007,7 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_SPI) #if defined(PIOS_INCLUDE_SPI)
/* Set up the SPI interface to the SD card */ /* Set up the SPI interface to the SD card */
if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_Assert(0);
} }
/* Enable and mount the SDCard */ /* Enable and mount the SDCard */
@ -1050,23 +1015,16 @@ void PIOS_Board_Init(void) {
PIOS_SDCARD_MountFS(0); PIOS_SDCARD_MountFS(0);
#endif /* PIOS_INCLUDE_SPI */ #endif /* PIOS_INCLUDE_SPI */
#if defined(PIOS_INCLUDE_SPEKTRUM)
/* SPEKTRUM init must come before comms */
PIOS_RTC_Init(); // Spektrum uses RTC to check for frame failures
PIOS_SPEKTRUM_Init();
if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_spektrum_id, &pios_usart_com_driver, pios_usart_spektrum_id)) {
PIOS_DEBUG_Assert(0);
}
#endif
/* Initialize UAVObject libraries */ /* Initialize UAVObject libraries */
EventDispatcherInitialize(); EventDispatcherInitialize();
UAVObjInitialize(); UAVObjInitialize();
UAVObjectsInitializeAll(); UAVObjectsInitializeAll();
#if defined(PIOS_INCLUDE_RTC)
/* Initialize the real-time clock and its associated tick */
PIOS_RTC_Init(&pios_rtc_main_cfg);
#endif
/* Initialize the alarms library */ /* Initialize the alarms library */
AlarmsInitialize(); AlarmsInitialize();
@ -1078,7 +1036,7 @@ void PIOS_Board_Init(void) {
/* Set up the SPI interface to the AHRS */ /* Set up the SPI interface to the AHRS */
if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) { if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_Assert(0);
} }
/* Bind the AHRS comms layer to the AHRS SPI link */ /* Bind the AHRS comms layer to the AHRS SPI link */
@ -1086,43 +1044,95 @@ void PIOS_Board_Init(void) {
/* Initialize the PiOS library */ /* Initialize the PiOS library */
#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_Assert(0);
} }
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) { if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) {
PIOS_DEBUG_Assert(0); PIOS_Assert(0);
} }
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_GPS)
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_Assert(0);
} }
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) { if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) {
PIOS_DEBUG_Assert(0); PIOS_Assert(0);
} }
#endif /* PIOS_INCLUDE_GPS */
#endif #endif
PIOS_Servo_Init(); PIOS_Servo_Init();
PIOS_ADC_Init(); PIOS_ADC_Init();
PIOS_GPIO_Init(); PIOS_GPIO_Init();
#if defined(PIOS_INCLUDE_SPEKTRUM)
#if (PIOS_SPEKTRUM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS)
#error More receiver inputs than available devices
#endif
/* SPEKTRUM init must come before comms */
PIOS_SPEKTRUM_Init(&pios_spektrum_cfg, false);
uint32_t pios_usart_spektrum_id;
if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) {
PIOS_Assert(0);
}
for (uint8_t i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS && i < PIOS_RCVR_MAX_DEVS; i++) {
if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel],
&pios_spektrum_rcvr_driver,
i)) {
pios_rcvr_max_channel++;
} else {
PIOS_Assert(0);
}
}
#endif
#if defined(PIOS_INCLUDE_PWM) #if defined(PIOS_INCLUDE_PWM)
#if (PIOS_PWM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS)
#error More receiver inputs than available devices
#endif
PIOS_PWM_Init(); PIOS_PWM_Init();
for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS && i < PIOS_RCVR_MAX_DEVS; i++) {
if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel],
&pios_pwm_rcvr_driver,
i)) {
pios_rcvr_max_channel++;
} else {
PIOS_Assert(0);
}
}
#endif #endif
#if defined(PIOS_INCLUDE_PPM) #if defined(PIOS_INCLUDE_PPM)
#if (PIOS_PPM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS)
#error More receiver inputs than available devices
#endif
PIOS_PPM_Init(); PIOS_PPM_Init();
for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS && i < PIOS_RCVR_MAX_DEVS; i++) {
if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel],
&pios_ppm_rcvr_driver,
i)) {
pios_rcvr_max_channel++;
} else {
PIOS_Assert(0);
}
}
#endif #endif
#if defined(PIOS_INCLUDE_USB_HID) #if defined(PIOS_INCLUDE_USB_HID)
PIOS_USB_HID_Init(0); PIOS_USB_HID_Init(0);
#if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_COM)
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) { if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) {
PIOS_DEBUG_Assert(0); PIOS_Assert(0);
} }
#endif /* PIOS_INCLUDE_COM */ #endif /* PIOS_INCLUDE_COM */
#endif /* PIOS_INCLUDE_USB_HID */ #endif /* PIOS_INCLUDE_USB_HID */
#if defined(PIOS_INCLUDE_I2C) #if defined(PIOS_INCLUDE_I2C)
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_Assert(0);
} }
#endif /* PIOS_INCLUDE_I2C */ #endif /* PIOS_INCLUDE_I2C */
PIOS_IAP_Init(); PIOS_IAP_Init();

1
flight/PiOS/Boards/STM32103CB_AHRS.h
View File

@ -120,7 +120,6 @@ extern uint32_t pios_i2c_main_adapter_id;
#define PIOS_USART_MAX_DEVS 2 #define PIOS_USART_MAX_DEVS 2
#define PIOS_USART_RX_BUFFER_SIZE 256 #define PIOS_USART_RX_BUFFER_SIZE 256
#define PIOS_USART_TX_BUFFER_SIZE 256 #define PIOS_USART_TX_BUFFER_SIZE 256
#define PIOS_USART_BAUDRATE 230400
//------------------------- //-------------------------
// PIOS_COM // PIOS_COM

24
flight/PiOS/Boards/STM32103CB_CC_Rev1.h
View File

@ -141,13 +141,11 @@ extern uint32_t pios_i2c_main_adapter_id;
//------------------------- //-------------------------
#define PIOS_COM_MAX_DEVS 4 #define PIOS_COM_MAX_DEVS 4
#define PIOS_COM_TELEM_BAUDRATE 57600
extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_telem_rf_id;
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) #define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_DEBUG PIOS_COM_TELEM_RF #define PIOS_COM_DEBUG PIOS_COM_TELEM_RF
#if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_GPS)
#define PIOS_COM_GPS_BAUDRATE 57600
extern uint32_t pios_com_gps_id; extern uint32_t pios_com_gps_id;
#define PIOS_COM_GPS (pios_com_gps_id) #define PIOS_COM_GPS (pios_com_gps_id)
#endif /* PIOS_INCLUDE_GPS */ #endif /* PIOS_INCLUDE_GPS */
@ -156,13 +154,11 @@ extern uint32_t pios_com_telem_usb_id;
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) #define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#ifdef PIOS_INCLUDE_SPEKTRUM #ifdef PIOS_INCLUDE_SPEKTRUM
#define PIOS_COM_SPEKTRUM_BAUDRATE 115200
extern uint32_t pios_com_spektrum_id; extern uint32_t pios_com_spektrum_id;
#define PIOS_COM_SPEKTRUM (pios_com_spektrum_id) #define PIOS_COM_SPEKTRUM (pios_com_spektrum_id)
#endif #endif
#ifdef PIOS_INCLUDE_SBUS #ifdef PIOS_INCLUDE_SBUS
#define PIOS_COM_SBUS_BAUDRATE 100000
extern uint32_t pios_com_sbus_id; extern uint32_t pios_com_sbus_id;
#define PIOS_COM_SBUS (pios_com_sbus_id) #define PIOS_COM_SBUS (pios_com_sbus_id)
#endif #endif
@ -225,10 +221,26 @@ extern uint32_t pios_com_sbus_id;
#define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) #define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2))
#define PIOS_ADC_MAX_OVERSAMPLING 36 #define PIOS_ADC_MAX_OVERSAMPLING 36
//------------------------
// PIOS_RCVR
// See also pios_board.c
//------------------------
#define PIOS_RCVR_MAX_DEVS 12
//------------------------- //-------------------------
// Receiver PWM inputs // Receiver PPM input
//------------------------- //-------------------------
#define PIOS_PWM_MAX_INPUTS 6 #define PIOS_PPM_NUM_INPUTS 6 //Could be more if needed
//-------------------------
// Receiver PWM input
//-------------------------
#define PIOS_PWM_NUM_INPUTS 6
//-------------------------
// Receiver SPEKTRUM input
//-------------------------
#define PIOS_SPEKTRUM_NUM_INPUTS 12
//------------------------- //-------------------------
// Servo outputs // Servo outputs

2
flight/PiOS/Boards/STM3210E_INS.h
View File

@ -135,12 +135,10 @@ extern uint32_t pios_i2c_gyro_adapter_id;
//------------------------- //-------------------------
#define PIOS_COM_MAX_DEVS 2 #define PIOS_COM_MAX_DEVS 2
#define PIOS_COM_GPS_BAUDRATE 57600
extern uint32_t pios_com_gps_id; extern uint32_t pios_com_gps_id;
#define PIOS_COM_GPS (pios_com_gps_id) #define PIOS_COM_GPS (pios_com_gps_id)
#ifdef PIOS_ENABLE_AUX_UART #ifdef PIOS_ENABLE_AUX_UART
#define PIOS_COM_AUX_BAUDRATE 57600
extern uint32_t pios_com_aux_id; extern uint32_t pios_com_aux_id;
#define PIOS_COM_AUX (pios_com_aux_id) #define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_DEBUG PIOS_COM_AUX #define PIOS_COM_DEBUG PIOS_COM_AUX

35
flight/PiOS/Boards/STM3210E_OP.h
View File

@ -149,11 +149,9 @@ extern uint32_t pios_i2c_main_adapter_id;
//------------------------- //-------------------------
#define PIOS_COM_MAX_DEVS 4 #define PIOS_COM_MAX_DEVS 4
#define PIOS_COM_TELEM_BAUDRATE 57600
extern uint32_t pios_com_telem_rf_id; extern uint32_t pios_com_telem_rf_id;
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) #define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_GPS_BAUDRATE 57600
extern uint32_t pios_com_gps_id; extern uint32_t pios_com_gps_id;
#define PIOS_COM_GPS (pios_com_gps_id) #define PIOS_COM_GPS (pios_com_gps_id)
@ -161,20 +159,17 @@ extern uint32_t pios_com_telem_usb_id;
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) #define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#ifdef PIOS_ENABLE_AUX_UART #ifdef PIOS_ENABLE_AUX_UART
#define PIOS_COM_AUX_BAUDRATE 57600
extern uint32_t pios_com_aux_id; extern uint32_t pios_com_aux_id;
#define PIOS_COM_AUX (pios_com_aux_id) #define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_DEBUG PIOS_COM_AUX #define PIOS_COM_DEBUG PIOS_COM_AUX
#endif #endif
#ifdef PIOS_INCLUDE_SPEKTRUM #ifdef PIOS_INCLUDE_SPEKTRUM
#define PIOS_COM_SPEKTRUM_BAUDRATE 115200
extern uint32_t pios_com_spektrum_id; extern uint32_t pios_com_spektrum_id;
#define PIOS_COM_SPEKTRUM (pios_com_spektrum_id) #define PIOS_COM_SPEKTRUM (pios_com_spektrum_id)
#endif #endif
#ifdef PIOS_INCLUDE_SBUS #ifdef PIOS_INCLUDE_SBUS
#define PIOS_COM_SBUS_BAUDRATE 100000
extern uint32_t pios_com_sbus_id; extern uint32_t pios_com_sbus_id;
#define PIOS_COM_SBUS (pios_com_sbus_id) #define PIOS_COM_SBUS (pios_com_sbus_id)
#endif #endif
@ -199,37 +194,33 @@ extern uint32_t pios_com_sbus_id;
#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... #define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc...
#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... #define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc...
//------------------------- //------------------------
// Receiver PWM inputs // PIOS_RCVR
//------------------------- // See also pios_board.c
/*#define PIOS_PWM_SUPV_ENABLED 1 //------------------------
#define PIOS_PWM_SUPV_TIMER TIM6 #define PIOS_RCVR_MAX_DEVS 12
#define PIOS_PWM_SUPV_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE)
#define PIOS_PWM_SUPV_HZ 25
#define PIOS_PWM_SUPV_IRQ_CHANNEL TIM6_IRQn
#define PIOS_PWM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void)*/
//------------------------- //-------------------------
// Receiver PPM input // Receiver PPM input
//------------------------- //-------------------------
#define PIOS_PPM_NUM_INPUTS 8 //Could be more if needed #define PIOS_PPM_NUM_INPUTS 8 //Could be more if needed
#define PIOS_PPM_SUPV_ENABLED 1 #define PIOS_PPM_SUPV_ENABLED 1
//------------------------- //-------------------------
// SPEKTRUM input // Receiver PWM input
//------------------------- //-------------------------
//#define PIOS_SPEKTRUM_SUPV_ENABLED 1 #define PIOS_PWM_NUM_INPUTS 8
//#define PIOS_SPEKTRUM_SUPV_TIMER TIM6
//#define PIOS_SPEKTRUM_SUPV_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE) //-------------------------
//#define PIOS_SPEKTRUM_SUPV_HZ 60 // 1/22ms // Receiver SPEKTRUM input
//#define PIOS_SPEKTRUM_SUPV_IRQ_CHANNEL TIM6_IRQn //-------------------------
//#define PIOS_SPEKTRUM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void) #define PIOS_SPEKTRUM_NUM_INPUTS 12
//------------------------- //-------------------------
// Servo outputs // Servo outputs
//------------------------- //-------------------------
#define PIOS_SERVO_UPDATE_HZ 50 #define PIOS_SERVO_UPDATE_HZ 50
#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
#define PIOS_PWM_MAX_INPUTS 8
//------------------------- //-------------------------
// ADC // ADC

88
flight/PiOS/Common/pios_rcvr.c Normal file
View File

@ -0,0 +1,88 @@
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_RCVR)
#include <pios_rcvr_priv.h>
static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev)
{
return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC);
}
#if defined(PIOS_INCLUDE_FREERTOS) && 0
static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
{
struct pios_rcvr_dev * rcvr_dev;
rcvr_dev = (struct pios_rcvr_dev *)malloc(sizeof(*rcvr_dev));
if (!rcvr_dev) return (NULL);
rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
return(rcvr_dev);
}
#else
static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS];
static uint8_t pios_rcvr_num_devs;
static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
{
struct pios_rcvr_dev * rcvr_dev;
if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) {
return (NULL);
}
rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++];
rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
return (rcvr_dev);
}
#endif
/**
* Initialises RCVR layer
* \param[out] handle
* \param[in] driver
* \param[in] id
* \return < 0 if initialisation failed
*/
int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id)
{
PIOS_DEBUG_Assert(rcvr_id);
PIOS_DEBUG_Assert(driver);
struct pios_rcvr_dev * rcvr_dev;
rcvr_dev = (struct pios_rcvr_dev *) PIOS_RCVR_alloc();
if (!rcvr_dev) goto out_fail;
rcvr_dev->driver = driver;
rcvr_dev->id = lower_id;
*rcvr_id = (uint32_t)rcvr_dev;
return(0);
out_fail:
return(-1);
}
int32_t PIOS_RCVR_Read(uint32_t rcvr_id)
{
struct pios_rcvr_dev * rcvr_dev = (struct pios_rcvr_dev *)rcvr_id;
if (!PIOS_RCVR_validate(rcvr_dev)) {
/* Undefined RCVR port for this board (see pios_board.c) */
PIOS_DEBUG_Assert(0);
}
PIOS_DEBUG_Assert(rcvr_dev->driver->read);
return rcvr_dev->driver->read(rcvr_dev->id);
}
#endif
/**
* @}
* @}
*/

156
flight/PiOS/STM32F10x/pios_ppm.c
View File

@ -34,22 +34,29 @@
#if defined(PIOS_INCLUDE_PPM) #if defined(PIOS_INCLUDE_PPM)
/* Local Variables */ /* Provide a RCVR driver */
static int32_t PIOS_PPM_Get(uint32_t chan_id);
const struct pios_rcvr_driver pios_ppm_rcvr_driver = {
.read = PIOS_PPM_Get,
};
/* Local Variables */
static TIM_ICInitTypeDef TIM_ICInitStructure; static TIM_ICInitTypeDef TIM_ICInitStructure;
static uint8_t PulseIndex; static uint8_t PulseIndex;
static uint32_t PreviousValue; static uint32_t PreviousValue;
static uint32_t CurrentValue; static uint32_t CurrentValue;
static uint32_t CapturedValue; static uint32_t CapturedValue;
static uint32_t CaptureValue[PIOS_PPM_NUM_INPUTS]; static uint32_t CaptureValue[PIOS_PPM_NUM_INPUTS];
static uint8_t SupervisorState = 0;
static uint32_t CapCounter[PIOS_PPM_NUM_INPUTS]; static uint32_t CapCounter[PIOS_PPM_NUM_INPUTS];
static uint16_t TimerCounter;
static uint8_t supv_timer = 0;
static uint8_t SupervisorState = 0;
static uint32_t CapCounterPrev[PIOS_PPM_NUM_INPUTS]; static uint32_t CapCounterPrev[PIOS_PPM_NUM_INPUTS];
/** static void PIOS_PPM_Supervisor(uint32_t ppm_id);
* Initialises all the LED's
*/
void PIOS_PPM_Init(void) void PIOS_PPM_Init(void)
{ {
/* Flush counter variables */ /* Flush counter variables */
@ -59,6 +66,7 @@ void PIOS_PPM_Init(void)
PreviousValue = 0; PreviousValue = 0;
CurrentValue = 0; CurrentValue = 0;
CapturedValue = 0; CapturedValue = 0;
TimerCounter = 0;
for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) { for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
CaptureValue[i] = 0; CaptureValue[i] = 0;
@ -121,13 +129,12 @@ void PIOS_PPM_Init(void)
TIM_TimeBaseInit(pios_ppm_cfg.timer, &TIM_TimeBaseStructure); TIM_TimeBaseInit(pios_ppm_cfg.timer, &TIM_TimeBaseStructure);
/* Enable the Capture Compare Interrupt Request */ /* Enable the Capture Compare Interrupt Request */
TIM_ITConfig(pios_ppm_cfg.timer, pios_ppm_cfg.ccr, ENABLE); TIM_ITConfig(pios_ppm_cfg.timer, pios_ppm_cfg.ccr | TIM_IT_Update, ENABLE);
/* Enable timers */ /* Enable timers */
TIM_Cmd(pios_ppm_cfg.timer, ENABLE); TIM_Cmd(pios_ppm_cfg.timer, ENABLE);
/* Supervisor Setup */ /* Supervisor Setup */
#if (PIOS_PPM_SUPV_ENABLED)
/* Flush counter variables */ /* Flush counter variables */
for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) { for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
CapCounter[i] = 0; CapCounter[i] = 0;
@ -136,70 +143,15 @@ void PIOS_PPM_Init(void)
CapCounterPrev[i] = 0; CapCounterPrev[i] = 0;
} }
NVIC_InitStructure = pios_ppmsv_cfg.irq.init;
/* Enable appropriate clock to timer module */
switch((int32_t) pios_ppmsv_cfg.timer) {
case (int32_t)TIM1:
NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
break;
case (int32_t)TIM2:
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
break;
case (int32_t)TIM3:
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
break;
case (int32_t)TIM4:
NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
break;
#ifdef STM32F10X_HD
case (int32_t)TIM5:
NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
break;
case (int32_t)TIM6:
NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
break;
case (int32_t)TIM7:
NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
break;
case (int32_t)TIM8:
NVIC_InitStructure.NVIC_IRQChannel = TIM8_CC_IRQn;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
break;
#endif
}
/* Configure interrupts */
NVIC_Init(&NVIC_InitStructure);
/* Time base configuration */
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure = pios_ppmsv_cfg.tim_base_init;
TIM_TimeBaseInit(pios_ppmsv_cfg.timer, &TIM_TimeBaseStructure);
/* Enable the CCx Interrupt Request */
TIM_ITConfig(pios_ppmsv_cfg.timer, pios_ppmsv_cfg.ccr, ENABLE);
/* Clear update pending flag */
TIM_ClearFlag(pios_ppmsv_cfg.timer, TIM_FLAG_Update);
/* Enable counter */
TIM_Cmd(pios_ppmsv_cfg.timer, ENABLE);
#endif
/* Setup local variable which stays in this scope */ /* Setup local variable which stays in this scope */
/* Doing this here and using a local variable saves doing it in the ISR */ /* Doing this here and using a local variable saves doing it in the ISR */
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;
if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, 0)) {
PIOS_DEBUG_Assert(0);
}
} }
/** /**
@ -208,13 +160,13 @@ void PIOS_PPM_Init(void)
* \output -1 Channel not available * \output -1 Channel not available
* \output >0 Channel value * \output >0 Channel value
*/ */
int32_t PIOS_PPM_Get(int8_t Channel) static int32_t PIOS_PPM_Get(uint32_t chan_id)
{ {
/* Return error if channel not available */ /* Return error if channel not available */
if (Channel >= PIOS_PPM_NUM_INPUTS) { if (chan_id >= PIOS_PPM_NUM_INPUTS) {
return -1; return -1;
} }
return CaptureValue[Channel]; return CaptureValue[chan_id];
} }
/** /**
@ -224,6 +176,15 @@ int32_t PIOS_PPM_Get(int8_t Channel)
*/ */
void PIOS_PPM_irq_handler(void) void PIOS_PPM_irq_handler(void)
{ {
if (TIM_GetITStatus(pios_ppm_cfg.timer, TIM_IT_Update) == SET) {
TimerCounter+=pios_ppm_cfg.timer->ARR;
TIM_ClearITPendingBit(pios_ppm_cfg.timer, TIM_IT_Update);
if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) != SET) {
return;
}
}
/* Do this as it's more efficient */ /* Do this as it's more efficient */
if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) == SET) { if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) == SET) {
PreviousValue = CurrentValue; PreviousValue = CurrentValue;
@ -241,37 +202,44 @@ void PIOS_PPM_irq_handler(void)
CurrentValue = TIM_GetCapture4(pios_ppm_cfg.timer); CurrentValue = TIM_GetCapture4(pios_ppm_cfg.timer);
break; break;
} }
} CurrentValue+=TimerCounter;
if(CurrentValue > 0xFFFF) {
CurrentValue-=0xFFFF;
}
/* Clear TIMx Capture compare interrupt pending bit */ /* Clear TIMx Capture compare interrupt pending bit */
TIM_ClearITPendingBit(pios_ppm_cfg.timer, pios_ppm_cfg.ccr); TIM_ClearITPendingBit(pios_ppm_cfg.timer, pios_ppm_cfg.ccr);
/* Capture computation */ /* Capture computation */
if (CurrentValue > PreviousValue) { if (CurrentValue > PreviousValue) {
CapturedValue = (CurrentValue - PreviousValue); CapturedValue = (CurrentValue - PreviousValue);
} else { } else {
CapturedValue = ((0xFFFF - PreviousValue) + CurrentValue); CapturedValue = ((0xFFFF - PreviousValue) + CurrentValue);
} }
/* sync pulse */ /* sync pulse */
if (CapturedValue > 8000) { if (CapturedValue > 8000) {
PulseIndex = 0; PulseIndex = 0;
/* trying to detect bad pulses, not sure this is working correctly yet. I need a scope :P */ /* trying to detect bad pulses, not sure this is working correctly yet. I need a scope :P */
} else if (CapturedValue > 750 && CapturedValue < 2500) { } else if (CapturedValue > 750 && CapturedValue < 2500) {
if (PulseIndex < PIOS_PPM_NUM_INPUTS) { if (PulseIndex < PIOS_PPM_NUM_INPUTS) {
CaptureValue[PulseIndex] = CapturedValue; CaptureValue[PulseIndex] = CapturedValue;
CapCounter[PulseIndex]++; CapCounter[PulseIndex]++;
PulseIndex++; PulseIndex++;
}
} }
} }
} }
/** static void PIOS_PPM_Supervisor(uint32_t ppm_id) {
* This function handles TIM3 global interrupt request. /*
*/ * RTC runs at 625Hz so divide down the base rate so
void PIOS_PPMSV_irq_handler(void) { * that this loop runs at 25Hz.
/* Clear timer interrupt pending bit */ */
TIM_ClearITPendingBit(pios_ppmsv_cfg.timer, pios_ppmsv_cfg.ccr); if(++supv_timer < 25) {
return;
}
supv_timer = 0;
/* Simple state machine */ /* Simple state machine */
if (SupervisorState == 0) { if (SupervisorState == 0) {

151
flight/PiOS/STM32F10x/pios_pwm.c
View File

@ -34,15 +34,20 @@
#if defined(PIOS_INCLUDE_PWM) #if defined(PIOS_INCLUDE_PWM)
/* Local Variables */ /* Provide a RCVR driver */
static uint8_t CaptureState[PIOS_PWM_MAX_INPUTS]; static int32_t PIOS_PWM_Get(uint32_t chan_id);
static uint16_t RiseValue[PIOS_PWM_MAX_INPUTS];
static uint16_t FallValue[PIOS_PWM_MAX_INPUTS];
static uint32_t CaptureValue[PIOS_PWM_MAX_INPUTS];
//static uint8_t SupervisorState = 0; const struct pios_rcvr_driver pios_pwm_rcvr_driver = {
static uint32_t CapCounter[PIOS_PWM_MAX_INPUTS]; .read = PIOS_PWM_Get,
//static uint32_t CapCounterPrev[MAX_CHANNELS]; };
/* Local Variables */
static uint8_t CaptureState[PIOS_PWM_NUM_INPUTS];
static uint16_t RiseValue[PIOS_PWM_NUM_INPUTS];
static uint16_t FallValue[PIOS_PWM_NUM_INPUTS];
static uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS];
static uint32_t CapCounter[PIOS_PWM_NUM_INPUTS];
/** /**
* Initialises all the pins * Initialises all the pins
@ -127,52 +132,6 @@ void PIOS_PWM_Init(void)
/* Warning, I don't think this will work for multiple remaps at once */ /* Warning, I don't think this will work for multiple remaps at once */
GPIO_PinRemapConfig(pios_pwm_cfg.remap, ENABLE); GPIO_PinRemapConfig(pios_pwm_cfg.remap, ENABLE);
} }
#if 0
/* Supervisor Setup */
#if (PIOS_PWM_SUPV_ENABLED)
/* Flush counter variables */
for (i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
CapCounter[i] = 0;
}
for (i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
CapCounterPrev[i] = 0;
}
/* Enable timer clock */
PIOS_PWM_SUPV_TIMER_RCC_FUNC;
/* Configure interrupts */
NVIC_InitStructure.NVIC_IRQChannel = PIOS_PWM_SUPV_IRQ_CHANNEL;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* Time base configuration */
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Period = ((1000000 / PIOS_PWM_SUPV_HZ) - 1);
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; /* For 1 uS accuracy */
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(PIOS_PWM_SUPV_TIMER, &TIM_TimeBaseStructure);
/* Enable the CC2 Interrupt Request */
TIM_ITConfig(PIOS_PWM_SUPV_TIMER, TIM_IT_Update, ENABLE);
/* Clear update pending flag */
TIM_ClearFlag(TIM2, TIM_FLAG_Update);
/* Enable counter */
TIM_Cmd(PIOS_PWM_SUPV_TIMER, ENABLE);
#endif
/* Setup local variable which stays in this scope */
/* Doing this here and using a local variable saves doing it in the ISR */
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
#endif
} }
/** /**
@ -181,13 +140,13 @@ void PIOS_PWM_Init(void)
* \output -1 Channel not available * \output -1 Channel not available
* \output >0 Channel value * \output >0 Channel value
*/ */
int32_t PIOS_PWM_Get(int8_t Channel) static int32_t PIOS_PWM_Get(uint32_t chan_id)
{ {
/* Return error if channel not available */ /* Return error if channel not available */
if (Channel >= pios_pwm_cfg.num_channels) { if (chan_id >= pios_pwm_cfg.num_channels) {
return -1; return -1;
} }
return CaptureValue[Channel]; return CaptureValue[chan_id];
} }
void PIOS_PWM_irq_handler(TIM_TypeDef * timer) void PIOS_PWM_irq_handler(TIM_TypeDef * timer)
@ -254,84 +213,6 @@ void PIOS_PWM_irq_handler(TIM_TypeDef * timer)
} }
} }
#if 0
/**
* Handle TIM5 global interrupt request
*/
void TIM5_IRQHandler(void)
{
/* Do this as it's more efficient */
if (TIM_GetITStatus(PIOS_PWM_TIM_PORT[2], PIOS_PWM_TIM_CCR[2]) == SET) {
if (CaptureState[2] == 0) {
RiseValue[2] = TIM_GetCapture1(PIOS_PWM_TIM_PORT[2]);
} else {
FallValue[2] = TIM_GetCapture1(PIOS_PWM_TIM_PORT[2]);
}
/* Clear TIM3 Capture compare interrupt pending bit */
TIM_ClearITPendingBit(PIOS_PWM_TIM_PORT[2], PIOS_PWM_TIM_CCR[2]);
/* Simple rise or fall state machine */
if (CaptureState[2] == 0) {
/* Switch states */
CaptureState[2] = 1;
/* Switch polarity of input capture */
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInitStructure.TIM_Channel = PIOS_PWM_TIM_CHANNEL[2];
TIM_ICInit(PIOS_PWM_TIM_PORT[2], &TIM_ICInitStructure);
} else {
/* Capture computation */
if (FallValue[2] > RiseValue[2]) {
CaptureValue[2] = (FallValue[2] - RiseValue[2]);
} else {
CaptureValue[2] = ((0xFFFF - RiseValue[2]) + FallValue[2]);
}
/* Switch states */
CaptureState[2] = 0;
/* Increase supervisor counter */
CapCounter[2]++;
/* Switch polarity of input capture */
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_Channel = PIOS_PWM_TIM_CHANNEL[2];
TIM_ICInit(PIOS_PWM_TIM_PORT[2], &TIM_ICInitStructure);
}
}
}
/**
* This function handles TIM3 global interrupt request.
*/
PIOS_PWM_SUPV_IRQ_FUNC {
/* Clear timer interrupt pending bit */
TIM_ClearITPendingBit(PIOS_PWM_SUPV_TIMER, TIM_IT_Update);
/* Simple state machine */
if (SupervisorState == 0) {
/* Save this states values */
for (int32_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
CapCounterPrev[i] = CapCounter[i];
}
/* Move to next state */
SupervisorState = 1;
} else {
/* See what channels have been updated */
for (int32_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
if (CapCounter[i] == CapCounterPrev[i]) {
CaptureValue[i] = 0;
}
}
/* Move to next state */
SupervisorState = 0;
}
}
#endif
#endif #endif
/** /**

65
flight/PiOS/STM32F10x/pios_rtc.c
View File

@ -32,30 +32,39 @@
#include "pios.h" #include "pios.h"
#if defined(PIOS_INCLUDE_RTC) #if defined(PIOS_INCLUDE_RTC)
#include <pios_rtc_priv.h>
#ifndef PIOS_RTC_PRESCALAR #ifndef PIOS_RTC_PRESCALER
#define PIOS_RTC_PRESCALAR 100 #define PIOS_RTC_PRESCALER 100
#endif #endif
void PIOS_RTC_Init() struct rtc_callback_entry {
void (*fn)(uint32_t);
uint32_t data;
};
#define PIOS_RTC_MAX_CALLBACKS 3
struct rtc_callback_entry rtc_callback_list[PIOS_RTC_MAX_CALLBACKS];
static uint8_t rtc_callback_next = 0;
void PIOS_RTC_Init(const struct pios_rtc_cfg * cfg)
{ {
RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR, RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR,
ENABLE); ENABLE);
PWR_BackupAccessCmd(ENABLE); PWR_BackupAccessCmd(ENABLE);
RCC_RTCCLKConfig(RCC_RTCCLKSource_HSE_Div128); RCC_RTCCLKConfig(cfg->clksrc);
RCC_RTCCLKCmd(ENABLE); RCC_RTCCLKCmd(ENABLE);
RTC_WaitForLastTask(); RTC_WaitForLastTask();
RTC_WaitForSynchro(); RTC_WaitForSynchro();
RTC_WaitForLastTask(); RTC_WaitForLastTask();
#if defined(PIOS_INCLUDE_SPEKTRUM) || defined(PIOS_INCLUDE_SBUS) /* Configure and enable the RTC Second interrupt */
/* Enable the RTC Second interrupt */ NVIC_Init(&cfg->irq.init);
RTC_ITConfig( RTC_IT_SEC, ENABLE ); RTC_ITConfig( RTC_IT_SEC, ENABLE );
/* Wait until last write operation on RTC registers has finished */
RTC_WaitForLastTask(); RTC_WaitForLastTask();
#endif
RTC_SetPrescaler(PIOS_RTC_PRESCALAR); // counting at 8e6 / 128 RTC_SetPrescaler(cfg->prescaler);
RTC_WaitForLastTask(); RTC_WaitForLastTask();
RTC_SetCounter(0); RTC_SetCounter(0);
RTC_WaitForLastTask(); RTC_WaitForLastTask();
@ -66,9 +75,12 @@ uint32_t PIOS_RTC_Counter()
return RTC_GetCounter(); return RTC_GetCounter();
} }
/* FIXME: This shouldn't use hard-coded clock rates, dividers or prescalers.
* Should get these from the cfg struct passed to init.
*/
float PIOS_RTC_Rate() float PIOS_RTC_Rate()
{ {
return (float) (8e6 / 128) / (1 + PIOS_RTC_PRESCALAR); return (float) (8e6 / 128) / (1 + PIOS_RTC_PRESCALER);
} }
float PIOS_RTC_MsPerTick() float PIOS_RTC_MsPerTick()
@ -76,6 +88,39 @@ float PIOS_RTC_MsPerTick()
return 1000.0f / PIOS_RTC_Rate(); return 1000.0f / PIOS_RTC_Rate();
} }
/* TODO: This needs a mutex around rtc_callbacks[] */
bool PIOS_RTC_RegisterTickCallback(void (*fn)(uint32_t id), uint32_t data)
{
struct rtc_callback_entry * cb;
if (rtc_callback_next >= PIOS_RTC_MAX_CALLBACKS) {
return false;
}
cb = &rtc_callback_list[rtc_callback_next++];
cb->fn = fn;
cb->data = data;
return true;
}
void PIOS_RTC_irq_handler (void)
{
if (RTC_GetITStatus(RTC_IT_SEC))
{
/* Call all registered callbacks */
for (uint8_t i = 0; i < rtc_callback_next; i++) {
struct rtc_callback_entry * cb = &rtc_callback_list[i];
if (cb->fn) {
(cb->fn)(cb->data);
}
}
/* Wait until last write operation on RTC registers has finished */
RTC_WaitForLastTask();
/* Clear the RTC Second interrupt */
RTC_ClearITPendingBit(RTC_IT_SEC);
}
}
#endif #endif
/** /**

58
flight/PiOS/STM32F10x/pios_sbus.c
View File

@ -36,6 +36,13 @@
/* Global Variables */ /* Global Variables */
/* Provide a RCVR driver */
static int32_t PIOS_SBUS_Get(uint32_t chan_id);
const struct pios_rcvr_driver pios_sbus_rcvr_driver = {
.read = PIOS_SBUS_Get,
};
/* Local Variables */ /* Local Variables */
static uint16_t channel_data[SBUS_NUMBER_OF_CHANNELS]; static uint16_t channel_data[SBUS_NUMBER_OF_CHANNELS];
static uint8_t received_data[SBUS_FRAME_LENGTH - 2]; static uint8_t received_data[SBUS_FRAME_LENGTH - 2];
@ -43,6 +50,8 @@ static uint8_t receive_timer;
static uint8_t failsafe_timer; static uint8_t failsafe_timer;
static uint8_t frame_found; static uint8_t frame_found;
static void PIOS_SBUS_Supervisor(uint32_t sbus_id);
/** /**
* reset_channels() function clears all channel data in case of * reset_channels() function clears all channel data in case of
* lost signal or explicit failsafe flag from the S.Bus data stream * lost signal or explicit failsafe flag from the S.Bus data stream
@ -126,26 +135,18 @@ static void process_byte(uint8_t b)
/** /**
* Initialise S.Bus receiver interface * Initialise S.Bus receiver interface
*/ */
void PIOS_SBUS_Init(void) void PIOS_SBUS_Init(const struct pios_sbus_cfg *cfg)
{ {
/* Enable USART input invertor clock and enable the invertor */ /* Enable inverter clock and enable the inverter */
(*pios_sbus_cfg.gpio_clk_func)(pios_sbus_cfg.gpio_clk_periph, ENABLE); (*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE);
GPIO_Init(pios_sbus_cfg.gpio_inv_port, &pios_sbus_cfg.gpio_inv_init); GPIO_Init(cfg->inv.gpio, &cfg->inv.init);
GPIO_WriteBit(pios_sbus_cfg.gpio_inv_port, GPIO_WriteBit(cfg->inv.gpio,
pios_sbus_cfg.gpio_inv_init.GPIO_Pin, cfg->inv.init.GPIO_Pin,
pios_sbus_cfg.gpio_inv_enable); cfg->gpio_inv_enable);
/* Init RTC supervisor timer interrupt */ if (!PIOS_RTC_RegisterTickCallback(PIOS_SBUS_Supervisor, 0)) {
static const NVIC_InitTypeDef NVIC_InitStructure = { PIOS_Assert(0);
.NVIC_IRQChannel = RTC_IRQn, }
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
};
NVIC_Init(&NVIC_InitStructure);
/* Init RTC clock */
PIOS_RTC_Init();
} }
/** /**
@ -154,23 +155,28 @@ void PIOS_SBUS_Init(void)
* \output -1 channel not available * \output -1 channel not available
* \output >0 channel value * \output >0 channel value
*/ */
int16_t PIOS_SBUS_Get(int8_t channel) static int32_t PIOS_SBUS_Get(uint32_t chan_id)
{ {
/* return error if channel is not available */ /* return error if channel is not available */
if (channel >= SBUS_NUMBER_OF_CHANNELS) { if (chan_id >= SBUS_NUMBER_OF_CHANNELS) {
return -1; return -1;
} }
return channel_data[channel]; return channel_data[chan_id];
} }
/** /**
* Interrupt handler for USART * Interrupt handler for USART
*/ */
void SBUS_IRQHandler(uint32_t usart_id) void PIOS_SBUS_irq_handler(uint32_t usart_id)
{ {
/* Grab the config for this device from the underlying USART device */
const struct pios_usart_cfg * cfg;
cfg = PIOS_USART_GetConfig(usart_id);
PIOS_Assert(cfg);
/* by always reading DR after SR make sure to clear any error interrupts */ /* by always reading DR after SR make sure to clear any error interrupts */
volatile uint16_t sr = pios_sbus_cfg.pios_usart_sbus_cfg->regs->SR; volatile uint16_t sr = cfg->regs->SR;
volatile uint8_t b = pios_sbus_cfg.pios_usart_sbus_cfg->regs->DR; volatile uint8_t b = cfg->regs->DR;
/* process received byte if one has arrived */ /* process received byte if one has arrived */
if (sr & USART_SR_RXNE) { if (sr & USART_SR_RXNE) {
@ -182,7 +188,7 @@ void SBUS_IRQHandler(uint32_t usart_id)
/* ignore TXE interrupts */ /* ignore TXE interrupts */
if (sr & USART_SR_TXE) { if (sr & USART_SR_TXE) {
/* disable TXE interrupt (TXEIE=0) */ /* disable TXE interrupt (TXEIE=0) */
USART_ITConfig(pios_sbus_cfg.pios_usart_sbus_cfg->regs, USART_IT_TXE, DISABLE); USART_ITConfig(cfg->regs, USART_IT_TXE, DISABLE);
} }
} }
@ -198,7 +204,7 @@ void SBUS_IRQHandler(uint32_t usart_id)
* data reception. If no new data received in 100ms, we must call the * data reception. If no new data received in 100ms, we must call the
* failsafe function which clears all channels. * failsafe function which clears all channels.
*/ */
void PIOS_SBUS_irq_handler() static void PIOS_SBUS_Supervisor(uint32_t sbus_id)
{ {
/* waiting for new frame if no bytes were received in 3.2ms */ /* waiting for new frame if no bytes were received in 3.2ms */
if (++receive_timer > 2) { if (++receive_timer > 2) {

100
flight/PiOS/STM32F10x/pios_spektrum.c
View File

@ -34,12 +34,6 @@
#include "pios_spektrum_priv.h" #include "pios_spektrum_priv.h"
#if defined(PIOS_INCLUDE_SPEKTRUM) #if defined(PIOS_INCLUDE_SPEKTRUM)
#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_SBUS)
#error "Both SPEKTRUM and either of PWM or SBUS inputs defined, choose only one"
#endif
#if defined(PIOS_COM_AUX)
#error "AUX com cannot be used with SPEKTRUM"
#endif
/** /**
* @Note Framesyncing: * @Note Framesyncing:
@ -51,31 +45,35 @@
/* Global Variables */ /* Global Variables */
/* Provide a RCVR driver */
static int32_t PIOS_SPEKTRUM_Get(uint32_t chan_id);
const struct pios_rcvr_driver pios_spektrum_rcvr_driver = {
.read = PIOS_SPEKTRUM_Get,
};
/* Local Variables */ /* Local Variables */
static uint16_t CaptureValue[12],CaptureValueTemp[12]; static uint16_t CaptureValue[PIOS_SPEKTRUM_NUM_INPUTS],CaptureValueTemp[PIOS_SPEKTRUM_NUM_INPUTS];
static uint8_t prev_byte = 0xFF, sync = 0, bytecount = 0, datalength=0, frame_error=0, byte_array[20] = { 0 }; static uint8_t prev_byte = 0xFF, sync = 0, bytecount = 0, datalength=0, frame_error=0, byte_array[20] = { 0 };
uint8_t sync_of = 0; uint8_t sync_of = 0;
uint16_t supv_timer=0; uint16_t supv_timer=0;
static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id);
static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg);
/** /**
* Bind and Initialise Spektrum satellite receiver * Bind and Initialise Spektrum satellite receiver
*/ */
void PIOS_SPEKTRUM_Init(void) void PIOS_SPEKTRUM_Init(const struct pios_spektrum_cfg * cfg, bool bind)
{ {
// TODO: need setting flag for bind on next powerup // TODO: need setting flag for bind on next powerup
if (0) { if (bind) {
PIOS_SPEKTRUM_Bind(); PIOS_SPEKTRUM_Bind(cfg);
} }
/* Init RTC supervisor timer interrupt */ if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, 0)) {
NVIC_InitTypeDef NVIC_InitStructure; PIOS_DEBUG_Assert(0);
NVIC_InitStructure.NVIC_IRQChannel = RTC_IRQn; }
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* Init RTC clock */
PIOS_RTC_Init();
} }
/** /**
@ -84,59 +82,56 @@ void PIOS_SPEKTRUM_Init(void)
* \output -1 Channel not available * \output -1 Channel not available
* \output >0 Channel value * \output >0 Channel value
*/ */
int16_t PIOS_SPEKTRUM_Get(int8_t Channel) static int32_t PIOS_SPEKTRUM_Get(uint32_t chan_id)
{ {
/* Return error if channel not available */ /* Return error if channel not available */
if (Channel >= 12) { if (chan_id >= PIOS_SPEKTRUM_NUM_INPUTS) {
return -1; return -1;
} }
return CaptureValue[Channel]; return CaptureValue[chan_id];
} }
/** /**
* Spektrum bind function * Spektrum bind function
* \output 1 Successful bind * \output true Successful bind
* \output 0 Bind failed * \output false Bind failed
* \note Applications shouldn't call these functions directly
*/ */
uint8_t PIOS_SPEKTRUM_Bind(void) static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg)
{ {
GPIO_InitTypeDef GPIO_InitStructure = pios_spektrum_cfg.gpio_init; GPIO_Init(cfg->bind.gpio, &cfg->bind.init);
GPIO_InitStructure.GPIO_Pin = pios_spektrum_cfg.pin;
GPIO_Init(pios_spektrum_cfg.port, &GPIO_InitStructure);
pios_spektrum_cfg.port->BRR = pios_spektrum_cfg.pin; GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
//PIOS_DELAY_WaitmS(75); //PIOS_DELAY_WaitmS(75);
/* RX line, drive high for 10us */ /* RX line, drive high for 10us */
pios_spektrum_cfg.port->BSRR = pios_spektrum_cfg.pin; GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
PIOS_DELAY_WaituS(10); PIOS_DELAY_WaituS(10);
/* RX line, drive low for 120us */ /* RX line, drive low for 120us */
pios_spektrum_cfg.port->BRR = pios_spektrum_cfg.pin; GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
PIOS_DELAY_WaituS(120); PIOS_DELAY_WaituS(120);
/* RX line, drive high for 120us */ /* RX line, drive high for 120us */
pios_spektrum_cfg.port->BSRR = pios_spektrum_cfg.pin; GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
PIOS_DELAY_WaituS(120); PIOS_DELAY_WaituS(120);
/* RX line, drive low for 120us */ /* RX line, drive low for 120us */
pios_spektrum_cfg.port->BRR = pios_spektrum_cfg.pin; GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
PIOS_DELAY_WaituS(120); PIOS_DELAY_WaituS(120);
/* RX line, drive high for 120us */ /* RX line, drive high for 120us */
pios_spektrum_cfg.port->BSRR = pios_spektrum_cfg.pin; GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
PIOS_DELAY_WaituS(120); PIOS_DELAY_WaituS(120);
/* RX line, drive low for 120us */ /* RX line, drive low for 120us */
pios_spektrum_cfg.port->BRR = pios_spektrum_cfg.pin; GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
PIOS_DELAY_WaituS(120); PIOS_DELAY_WaituS(120);
/* RX line, drive high for 120us */ /* RX line, drive high for 120us */
pios_spektrum_cfg.port->BSRR = pios_spektrum_cfg.pin; GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
PIOS_DELAY_WaituS(120); PIOS_DELAY_WaituS(120);
/* RX line, drive low for 120us */ /* RX line, drive low for 120us */
pios_spektrum_cfg.port->BRR = pios_spektrum_cfg.pin; GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
PIOS_DELAY_WaituS(120); PIOS_DELAY_WaituS(120);
/* RX line, drive high for 120us */ /* RX line, drive high for 120us */
pios_spektrum_cfg.port->BSRR = pios_spektrum_cfg.pin; GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
PIOS_DELAY_WaituS(120); PIOS_DELAY_WaituS(120);
/* RX line, set input and wait for data, PIOS_SPEKTRUM_Init */ /* RX line, set input and wait for data, PIOS_SPEKTRUM_Init */
return 1; return true;
} }
/** /**
@ -147,7 +142,7 @@ uint8_t PIOS_SPEKTRUM_Bind(void)
* \return -2 if buffer full (retry) * \return -2 if buffer full (retry)
* \note Applications shouldn't call these functions directly * \note Applications shouldn't call these functions directly
*/ */
int32_t PIOS_SPEKTRUM_Decode(uint8_t b) static int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
{ {
static uint16_t channel = 0; /*, sync_word = 0;*/ static uint16_t channel = 0; /*, sync_word = 0;*/
uint8_t channeln = 0, frame = 0; uint8_t channeln = 0, frame = 0;
@ -201,7 +196,7 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
{ {
frame_error=1; frame_error=1;
} }
if (channeln < 12 && !frame_error) if (channeln < PIOS_SPEKTRUM_NUM_INPUTS && !frame_error)
CaptureValueTemp[channeln] = data; CaptureValueTemp[channeln] = data;
} }
} }
@ -212,7 +207,7 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
sync_of = 0; sync_of = 0;
if (!frame_error) if (!frame_error)
{ {
for(int i=0;i<12;i++) for(int i=0;i<PIOS_SPEKTRUM_NUM_INPUTS;i++)
{ {
CaptureValue[i] = CaptureValueTemp[i]; CaptureValue[i] = CaptureValueTemp[i];
} }
@ -223,11 +218,16 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
return 0; return 0;
} }
/* Interrupt handler for USART */ /* Custom interrupt handler for USART */
void SPEKTRUM_IRQHandler(uint32_t usart_id) { void PIOS_SPEKTRUM_irq_handler(uint32_t usart_id) {
/* Grab the config for this device from the underlying USART device */
const struct pios_usart_cfg * cfg;
cfg = PIOS_USART_GetConfig(usart_id);
PIOS_Assert(cfg);
/* by always reading DR after SR make sure to clear any error interrupts */ /* by always reading DR after SR make sure to clear any error interrupts */
volatile uint16_t sr = pios_spektrum_cfg.pios_usart_spektrum_cfg->regs->SR; volatile uint16_t sr = cfg->regs->SR;
volatile uint8_t b = pios_spektrum_cfg.pios_usart_spektrum_cfg->regs->DR; volatile uint8_t b = cfg->regs->DR;
/* check if RXNE flag is set */ /* check if RXNE flag is set */
if (sr & USART_SR_RXNE) { if (sr & USART_SR_RXNE) {
@ -238,7 +238,7 @@ void SPEKTRUM_IRQHandler(uint32_t usart_id) {
if (sr & USART_SR_TXE) { // check if TXE flag is set if (sr & USART_SR_TXE) { // check if TXE flag is set
/* Disable TXE interrupt (TXEIE=0) */ /* Disable TXE interrupt (TXEIE=0) */
USART_ITConfig(pios_spektrum_cfg.pios_usart_spektrum_cfg->regs, USART_IT_TXE, DISABLE); USART_ITConfig(cfg->regs, USART_IT_TXE, DISABLE);
} }
/* byte arrived so clear "watchdog" timer */ /* byte arrived so clear "watchdog" timer */
supv_timer=0; supv_timer=0;
@ -248,7 +248,7 @@ void SPEKTRUM_IRQHandler(uint32_t usart_id) {
*@brief This function is called between frames and when a spektrum word hasnt been decoded for too long *@brief This function is called between frames and when a spektrum word hasnt been decoded for too long
*@brief clears the channel values *@brief clears the channel values
*/ */
void PIOS_SPEKTRUM_irq_handler() { static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id) {
/* 125hz */ /* 125hz */
supv_timer++; supv_timer++;
if(supv_timer > 5) { if(supv_timer > 5) {
@ -262,7 +262,7 @@ void PIOS_SPEKTRUM_irq_handler() {
if (sync_of > 12) { if (sync_of > 12) {
/* signal lost */ /* signal lost */
sync_of = 0; sync_of = 0;
for (int i = 0; i < 12; i++) { for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) {
CaptureValue[i] = 0; CaptureValue[i] = 0;
CaptureValueTemp[i] = 0; CaptureValueTemp[i] = 0;
} }

77
flight/PiOS/STM32F10x/pios_usart.c
View File

@ -51,6 +51,23 @@ const struct pios_com_driver pios_usart_com_driver = {
.rx_avail = PIOS_USART_RxBufferUsed, .rx_avail = PIOS_USART_RxBufferUsed,
}; };
enum pios_usart_dev_magic {
PIOS_USART_DEV_MAGIC = 0x11223344,
};
struct pios_usart_dev {
enum pios_usart_dev_magic magic;
const struct pios_usart_cfg * cfg;
// align to 32-bit to try and provide speed improvement;
uint8_t rx_buffer[PIOS_USART_RX_BUFFER_SIZE] __attribute__ ((aligned(4)));
t_fifo_buffer rx;
// align to 32-bit to try and provide speed improvement;
uint8_t tx_buffer[PIOS_USART_TX_BUFFER_SIZE] __attribute__ ((aligned(4)));
t_fifo_buffer tx;
};
static bool PIOS_USART_validate(struct pios_usart_dev * usart_dev) static bool PIOS_USART_validate(struct pios_usart_dev * usart_dev)
{ {
return (usart_dev->magic == PIOS_USART_DEV_MAGIC); return (usart_dev->magic == PIOS_USART_DEV_MAGIC);
@ -85,6 +102,34 @@ static struct pios_usart_dev * PIOS_USART_alloc(void)
} }
#endif #endif
/* Bind Interrupt Handlers
*
* Map all valid USART IRQs to the common interrupt handler
* and provide storage for a 32-bit device id IRQ to map
* each physical IRQ to a specific registered device instance.
*/
static void PIOS_USART_generic_irq_handler(uint32_t usart_id);
static uint32_t PIOS_USART_1_id;
void USART1_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_1_irq_handler")));
static void PIOS_USART_1_irq_handler (void)
{
PIOS_USART_generic_irq_handler (PIOS_USART_1_id);
}
static uint32_t PIOS_USART_2_id;
void USART2_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_2_irq_handler")));
static void PIOS_USART_2_irq_handler (void)
{
PIOS_USART_generic_irq_handler (PIOS_USART_2_id);
}
static uint32_t PIOS_USART_3_id;
void USART3_IRQHandler(void) __attribute__ ((alias ("PIOS_USART_3_irq_handler")));
static void PIOS_USART_3_irq_handler (void)
{
PIOS_USART_generic_irq_handler (PIOS_USART_3_id);
}
/** /**
* Initialise a single USART device * Initialise a single USART device
@ -134,6 +179,11 @@ int32_t PIOS_USART_Init(uint32_t * usart_id, const struct pios_usart_cfg * cfg)
*usart_id = (uint32_t)usart_dev; *usart_id = (uint32_t)usart_dev;
/* Configure USART Interrupts */ /* Configure USART Interrupts */
switch ((uint32_t)usart_dev->cfg->regs) {
case (uint32_t)USART1: PIOS_USART_1_id = (uint32_t)usart_dev;
case (uint32_t)USART2: PIOS_USART_2_id = (uint32_t)usart_dev;
case (uint32_t)USART3: PIOS_USART_3_id = (uint32_t)usart_dev;
}
NVIC_Init(&usart_dev->cfg->irq.init); NVIC_Init(&usart_dev->cfg->irq.init);
USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE);
USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE);
@ -147,6 +197,19 @@ out_fail:
return(-1); return(-1);
} }
const struct pios_usart_cfg * PIOS_USART_GetConfig(uint32_t usart_id)
{
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
bool valid = PIOS_USART_validate(usart_dev);
if (!valid) {
return (NULL);
}
return usart_dev->cfg;
}
/** /**
* Changes the baud rate of the USART peripheral without re-initialising. * Changes the baud rate of the USART peripheral without re-initialising.
* \param[in] usart_id USART name (GPS, TELEM, AUX) * \param[in] usart_id USART name (GPS, TELEM, AUX)
@ -157,7 +220,7 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud)
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
bool valid = PIOS_USART_validate(usart_dev); bool valid = PIOS_USART_validate(usart_dev);
PIOS_Assert(valid) PIOS_Assert(valid);
USART_InitTypeDef USART_InitStructure; USART_InitTypeDef USART_InitStructure;
@ -337,12 +400,20 @@ static int32_t PIOS_USART_TxBufferPutMore(uint32_t usart_id, const uint8_t *buff
return rc; return rc;
} }
void PIOS_USART_IRQ_Handler(uint32_t usart_id) static void PIOS_USART_generic_irq_handler(uint32_t usart_id)
{ {
struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
bool valid = PIOS_USART_validate(usart_dev); bool valid = PIOS_USART_validate(usart_dev);
PIOS_Assert(valid) PIOS_Assert(valid);
/* Call any user provided callback function instead of processing
* the interrupt ourselves.
*/
if (usart_dev->cfg->irq.handler) {
(usart_dev->cfg->irq.handler)(usart_id);
return;
}
/* Force read of dr after sr to make sure to clear error flags */ /* Force read of dr after sr to make sure to clear error flags */
volatile uint16_t sr = usart_dev->cfg->regs->SR; volatile uint16_t sr = usart_dev->cfg->regs->SR;

4
flight/PiOS/inc/pios_ppm.h
View File

@ -30,8 +30,4 @@
#ifndef PIOS_PPM_H #ifndef PIOS_PPM_H
#define PIOS_PPM_H #define PIOS_PPM_H
/* Public Functions */
extern void PIOS_PPM_Init(void);
extern int32_t PIOS_PPM_Get(int8_t Channel);
#endif /* PIOS_PPM_H */ #endif /* PIOS_PPM_H */

13
flight/PiOS/inc/pios_ppm_priv.h
View File

@ -34,13 +34,6 @@
#include <pios.h> #include <pios.h>
#include <pios_stm32.h> #include <pios_stm32.h>
struct pios_ppmsv_cfg {
TIM_TimeBaseInitTypeDef tim_base_init;
struct stm32_irq irq;
TIM_TypeDef * timer;
uint16_t ccr;
};
struct pios_ppm_cfg { struct pios_ppm_cfg {
TIM_TimeBaseInitTypeDef tim_base_init; TIM_TimeBaseInitTypeDef tim_base_init;
TIM_ICInitTypeDef tim_ic_init; TIM_ICInitTypeDef tim_ic_init;
@ -53,11 +46,13 @@ struct pios_ppm_cfg {
}; };
extern void PIOS_PPM_irq_handler(); extern void PIOS_PPM_irq_handler();
extern void PIOS_PPMSV_irq_handler();
extern uint8_t pios_ppm_num_channels; extern uint8_t pios_ppm_num_channels;
extern const struct pios_ppm_cfg pios_ppm_cfg; extern const struct pios_ppm_cfg pios_ppm_cfg;
extern const struct pios_ppmsv_cfg pios_ppmsv_cfg;
extern const struct pios_rcvr_driver pios_ppm_rcvr_driver;
extern void PIOS_PPM_Init(void);
#endif /* PIOS_PPM_PRIV_H */ #endif /* PIOS_PPM_PRIV_H */

5
flight/PiOS/inc/pios_pwm.h
View File

@ -30,9 +30,4 @@
#ifndef PIOS_PWM_H #ifndef PIOS_PWM_H
#define PIOS_PWM_H #define PIOS_PWM_H
/* Public Functions */
extern void PIOS_PWM_Init(void);
extern int32_t PIOS_PWM_Get(int8_t Channel);
//extern void PIOS_PWM_irq_handler(TIM_TypeDef * timer);
#endif /* PIOS_PWM_H */ #endif /* PIOS_PWM_H */

4
flight/PiOS/inc/pios_pwm_priv.h
View File

@ -57,6 +57,10 @@ extern void PIOS_PWM_irq_handler(TIM_TypeDef * timer);
extern uint8_t pios_pwm_num_channels; extern uint8_t pios_pwm_num_channels;
extern const struct pios_pwm_cfg pios_pwm_cfg; extern const struct pios_pwm_cfg pios_pwm_cfg;
extern const struct pios_rcvr_driver pios_pwm_rcvr_driver;
extern void PIOS_PWM_Init(void);
#endif /* PIOS_PWM_PRIV_H */ #endif /* PIOS_PWM_PRIV_H */
/** /**

49
flight/PiOS/inc/pios_rcvr.h Normal file
View File

@ -0,0 +1,49 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_RCVR RCVR layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_rcvr.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief RCVR layer functions header
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_RCVR_H
#define PIOS_RCVR_H
extern uint32_t pios_rcvr_channel_to_id_map[];
struct pios_rcvr_driver {
void (*init)(uint32_t id);
int32_t (*read)(uint32_t id);
};
/* Public Functions */
extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id);
#endif /* PIOS_RCVR_H */
/**
* @}
* @}
*/

58
flight/PiOS/inc/pios_rcvr_priv.h Normal file
View File

@ -0,0 +1,58 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_RCVR RCVR Functions
* @brief PIOS interface for RCVR drivers
* @{
*
* @file pios_rcvr_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief USART private definitions.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_RCVR_PRIV_H
#define PIOS_RCVR_PRIV_H
#include <pios.h>
extern uint32_t pios_rcvr_max_channel;
enum pios_rcvr_dev_magic {
PIOS_RCVR_DEV_MAGIC = 0x99aabbcc,
};
struct pios_rcvr_dev {
enum pios_rcvr_dev_magic magic;
uint32_t id;
const struct pios_rcvr_driver * driver;
};
extern int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id);
extern void PIOS_RCVR_IRQ_Handler(uint32_t rcvr_id);
#endif /* PIOS_RCVR_PRIV_H */
/**
* @}
* @}
*/

6
flight/PiOS/inc/pios_rtc.h
View File

@ -30,13 +30,15 @@
#ifndef PIOS_RTC_H #ifndef PIOS_RTC_H
#define PIOS_RTC_H #define PIOS_RTC_H
#include <stdbool.h>
/* Public Functions */ /* Public Functions */
extern void PIOS_RTC_Init();
extern uint32_t PIOS_RTC_Counter(); extern uint32_t PIOS_RTC_Counter();
extern float PIOS_RTC_Rate(); extern float PIOS_RTC_Rate();
extern float PIOS_RTC_MsPerTick(); extern float PIOS_RTC_MsPerTick();
extern bool PIOS_RTC_RegisterTickCallback(void (*fn)(uint32_t id), uint32_t data);
#endif /* PIOS_SERVO_H */ #endif /* PIOS_RTC_H */
/** /**
* @} * @}

52
flight/PiOS/inc/pios_rtc_priv.h Normal file
View File

@ -0,0 +1,52 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_RTC RTC Functions
* @brief PIOS interface for RTC tick
* @{
*
* @file pios_rtc_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief ADC private definitions.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_RTC_PRIV_H
#define PIOS_RTC_PRIV_H
#include <pios.h>
#include <pios_stm32.h>
struct pios_rtc_cfg {
uint32_t clksrc;
uint32_t prescaler;
struct stm32_irq irq;
};
extern void PIOS_RTC_Init(const struct pios_rtc_cfg * cfg);
extern void PIOS_RTC_irq_handler(void);
#endif /* PIOS_RTC_PRIV_H */
/**
* @}
* @}
*/

3
flight/PiOS/inc/pios_sbus.h
View File

@ -33,9 +33,6 @@
/* Global Types */ /* Global Types */
/* Public Functions */ /* Public Functions */
extern void PIOS_SBUS_Init(void);
extern int16_t PIOS_SBUS_Get(int8_t Channel);
extern void SBUS_IRQHandler(uint32_t usart_id);
#endif /* PIOS_SBUS_H */ #endif /* PIOS_SBUS_H */

11
flight/PiOS/inc/pios_sbus_priv.h
View File

@ -66,20 +66,21 @@
#define SBUS_NUMBER_OF_CHANNELS 8 #define SBUS_NUMBER_OF_CHANNELS 8
/* /*
* S.Bus configuration includes USART and programmable invertor * S.Bus configuration programmable invertor
*/ */
struct pios_sbus_cfg { struct pios_sbus_cfg {
const struct pios_usart_cfg *pios_usart_sbus_cfg; struct stm32_gpio inv;
void (*gpio_clk_func)(uint32_t periph, FunctionalState state); void (*gpio_clk_func)(uint32_t periph, FunctionalState state);
uint32_t gpio_clk_periph; uint32_t gpio_clk_periph;
GPIO_TypeDef* gpio_inv_port;
GPIO_InitTypeDef gpio_inv_init;
BitAction gpio_inv_enable; BitAction gpio_inv_enable;
}; };
extern const struct pios_sbus_cfg pios_sbus_cfg;
extern void PIOS_SBUS_irq_handler(); extern void PIOS_SBUS_irq_handler();
extern const struct pios_rcvr_driver pios_sbus_rcvr_driver;
extern void PIOS_SBUS_Init(const struct pios_sbus_cfg * cfg);
#endif /* PIOS_SBUS_PRIV_H */ #endif /* PIOS_SBUS_PRIV_H */
/** /**

5
flight/PiOS/inc/pios_spektrum.h
View File

@ -34,11 +34,6 @@
/* Global Types */ /* Global Types */
/* Public Functions */ /* Public Functions */
extern void PIOS_SPEKTRUM_Init(void);
extern uint8_t PIOS_SPEKTRUM_Bind(void);
extern int32_t PIOS_SPEKTRUM_Decode(uint8_t b);
extern int16_t PIOS_SPEKTRUM_Get(int8_t Channel);
extern void SPEKTRUM_IRQHandler(uint32_t usart_id);
#endif /* PIOS_SPEKTRUM_H */ #endif /* PIOS_SPEKTRUM_H */

11
flight/PiOS/inc/pios_spektrum_priv.h
View File

@ -36,18 +36,15 @@
#include <pios_usart_priv.h> #include <pios_usart_priv.h>
struct pios_spektrum_cfg { struct pios_spektrum_cfg {
const struct pios_usart_cfg * pios_usart_spektrum_cfg; struct stm32_gpio bind;
GPIO_InitTypeDef gpio_init;
uint32_t remap; /* GPIO_Remap_* */ uint32_t remap; /* GPIO_Remap_* */
struct stm32_irq irq;
GPIO_TypeDef * port;
uint16_t pin;
}; };
extern void PIOS_SPEKTRUM_irq_handler(); extern void PIOS_SPEKTRUM_irq_handler();
extern uint8_t pios_spektrum_num_channels; extern const struct pios_rcvr_driver pios_spektrum_rcvr_driver;
extern const struct pios_spektrum_cfg pios_spektrum_cfg;
extern void PIOS_SPEKTRUM_Init(const struct pios_spektrum_cfg * cfg, bool bind);
#endif /* PIOS_PWM_PRIV_H */ #endif /* PIOS_PWM_PRIV_H */

2
flight/PiOS/inc/pios_stm32.h
View File

@ -32,7 +32,7 @@
#define PIOS_STM32_H #define PIOS_STM32_H
struct stm32_irq { struct stm32_irq {
void (*handler) (void); void (*handler) (uint32_t);
uint32_t flags; uint32_t flags;
NVIC_InitTypeDef init; NVIC_InitTypeDef init;
}; };

20
flight/PiOS/inc/pios_usart_priv.h
View File

@ -48,26 +48,8 @@ struct pios_usart_cfg {
struct stm32_irq irq; struct stm32_irq irq;
}; };
enum pios_usart_dev_magic {
PIOS_USART_DEV_MAGIC = 0x11223344,
};
struct pios_usart_dev {
enum pios_usart_dev_magic magic;
const struct pios_usart_cfg * cfg;
// align to 32-bit to try and provide speed improvement;
uint8_t rx_buffer[PIOS_USART_RX_BUFFER_SIZE] __attribute__ ((aligned(4)));
t_fifo_buffer rx;
// align to 32-bit to try and provide speed improvement;
uint8_t tx_buffer[PIOS_USART_TX_BUFFER_SIZE] __attribute__ ((aligned(4)));
t_fifo_buffer tx;
};
extern int32_t PIOS_USART_Init(uint32_t * usart_id, const struct pios_usart_cfg * cfg); extern int32_t PIOS_USART_Init(uint32_t * usart_id, const struct pios_usart_cfg * cfg);
extern const struct pios_usart_cfg * PIOS_USART_GetConfig(uint32_t usart_id);
extern void PIOS_USART_IRQ_Handler(uint32_t usart_id);
#endif /* PIOS_USART_PRIV_H */ #endif /* PIOS_USART_PRIV_H */

1
flight/PiOS/pios.h
View File

@ -78,6 +78,7 @@
#include <pios_spi.h> #include <pios_spi.h>
#include <pios_ppm.h> #include <pios_ppm.h>
#include <pios_pwm.h> #include <pios_pwm.h>
#include <pios_rcvr.h>
#include <pios_spektrum.h> #include <pios_spektrum.h>
#include <pios_sbus.h> #include <pios_sbus.h>
#include <pios_usb_hid.h> #include <pios_usb_hid.h>

20
flight/PipXtreme/pios_board.c
View File

@ -90,7 +90,7 @@ static const struct pios_spi_cfg pios_spi_port_cfg =
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = .irq =
{ {
.handler = PIOS_SPI_port_irq_handler, .handler = NULL,
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel2_IRQn, .NVIC_IRQChannel = DMA1_Channel2_IRQn,
@ -189,11 +189,11 @@ void PIOS_SPI_port_irq_handler(void)
extern void PIOS_ADC_handler(void); extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one // Remap the ADC DMA handler to this one
const struct pios_adc_cfg pios_adc_cfg = { static const struct pios_adc_cfg pios_adc_cfg = {
.dma = { .dma = {
.ahb_clk = RCC_AHBPeriph_DMA1, .ahb_clk = RCC_AHBPeriph_DMA1,
.irq = { .irq = {
.handler = PIOS_ADC_DMA_Handler, .handler = NULL,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = { .init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn, .NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -245,10 +245,7 @@ void PIOS_ADC_handler() {
/* /*
* SERIAL USART * SERIAL USART
*/ */
void PIOS_USART_serial_irq_handler(void); static const struct pios_usart_cfg pios_usart_serial_cfg =
void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_serial_irq_handler")));
const struct pios_usart_cfg pios_usart_serial_cfg =
{ {
.regs = USART1, .regs = USART1,
.init = .init =
@ -262,7 +259,7 @@ const struct pios_usart_cfg pios_usart_serial_cfg =
}, },
.irq = .irq =
{ {
.handler = PIOS_USART_serial_irq_handler, .handler = NULL,
.init = .init =
{ {
.NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannel = USART1_IRQn,
@ -293,12 +290,6 @@ const struct pios_usart_cfg pios_usart_serial_cfg =
}, },
}; };
static uint32_t pios_usart_serial_id;
void PIOS_USART_serial_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_serial_id);
}
#endif /* PIOS_INCLUDE_USART */ #endif /* PIOS_INCLUDE_USART */
// *********************************************************************************** // ***********************************************************************************
@ -334,6 +325,7 @@ void PIOS_Board_Init(void) {
// Delay system // Delay system
PIOS_DELAY_Init(); PIOS_DELAY_Init();
uint32_t pios_usart_serial_id;
if (PIOS_USART_Init(&pios_usart_serial_id, &pios_usart_serial_cfg)) { if (PIOS_USART_Init(&pios_usart_serial_id, &pios_usart_serial_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }

2
flight/UAVObjects/eventdispatcher.c
View File

@ -28,7 +28,7 @@
// Private constants // Private constants
#define MAX_QUEUE_SIZE 20 #define MAX_QUEUE_SIZE 20
#define STACK_SIZE configMINIMAL_STACK_SIZE #define STACK_SIZE 96
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define MAX_UPDATE_PERIOD_MS 1000 #define MAX_UPDATE_PERIOD_MS 1000

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -68,6 +68,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/nedaccel.h \ $$UAVOBJECT_SYNTHETICS/nedaccel.h \
$$UAVOBJECT_SYNTHETICS/sonaraltitude.h \ $$UAVOBJECT_SYNTHETICS/sonaraltitude.h \
$$UAVOBJECT_SYNTHETICS/flightstatus.h \ $$UAVOBJECT_SYNTHETICS/flightstatus.h \
$$UAVOBJECT_SYNTHETICS/hwsettings.h \
$$UAVOBJECT_SYNTHETICS/attitudesettings.h $$UAVOBJECT_SYNTHETICS/attitudesettings.h
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
@ -116,4 +117,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/sonaraltitude.cpp \ $$UAVOBJECT_SYNTHETICS/sonaraltitude.cpp \
$$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \ $$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \
$$UAVOBJECT_SYNTHETICS/flightstatus.cpp \ $$UAVOBJECT_SYNTHETICS/flightstatus.cpp \
$$UAVOBJECT_SYNTHETICS/hwsettings.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesettings.cpp $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp

11
shared/uavobjectdefinition/hwsettings.xml Normal file
View File

@ -0,0 +1,11 @@
<xml>
<object name="HwSettings" singleinstance="true" settings="true">
<description>Selection of optional hardware configurations.</description>
<field name="CC_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,Spektrum,ComAux,I2C" defaultvalue="Disabled"/>
<field name="CC_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,S.Bus,GPS,Spektrum,ComAux" defaultvalue="Telemetry"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="never" period="0"/>
</object>
</xml>

2
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -1,7 +1,7 @@
<xml> <xml>
<object name="ManualControlSettings" singleinstance="true" settings="true"> <object name="ManualControlSettings" singleinstance="true" settings="true">
<description>Settings to indicate how to decode receiver input by @ref ManualControlModule.</description> <description>Settings to indicate how to decode receiver input by @ref ManualControlModule.</description>
<field name="InputMode" units="" type="enum" elements="1" options="PWM,PPM,Spektrum" defaultvalue="PWM"/> <field name="InputMode" units="" type="enum" elements="1" options="PWM,PPM,Spektrum,S.Bus" defaultvalue="PWM"/>
<field name="Roll" units="channel" type="enum" elements="1" options="Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,Channel8,None" defaultvalue="None"/> <field name="Roll" units="channel" type="enum" elements="1" options="Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,Channel8,None" defaultvalue="None"/>
<field name="Pitch" units="channel" type="enum" elements="1" options="Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,Channel8,None" defaultvalue="None"/> <field name="Pitch" units="channel" type="enum" elements="1" options="Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,Channel8,None" defaultvalue="None"/>
<field name="Yaw" units="channel" type="enum" elements="1" options="Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,Channel8,None" defaultvalue="None"/> <field name="Yaw" units="channel" type="enum" elements="1" options="Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,Channel8,None" defaultvalue="None"/>