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

OP-423:

Browse Source 
Also reduce heap has it does not fit in SRAM anymore (not with current compiler).
(that's ok since if there is more space available, it will be reclaimed).

Merge branch 'master' into OP-423_Mathieu_Change_Init_To_Reduce_Memory_Footprint

Conflicts:
	flight/CopterControl/System/inc/pios_config.h
	flight/Modules/ManualControl/manualcontrol.c
This commit is contained in:
Mathieu Rondonneau 2011-07-08 06:29:34 -07:00
commit d38f6167e3
43 changed files with 2009 additions and 1776 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,
.irq = {
.handler = PIOS_SPI_op_irq_handler,
.handler = NULL,
.flags =
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
DMA1_FLAG_GL4),
@ -153,11 +153,11 @@ void PIOS_SPI_op_irq_handler(void)
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// 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 = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_ADC_DMA_Handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -205,17 +205,10 @@ void PIOS_ADC_handler() {
/*
* AUX USART
*/
void PIOS_USART_aux_irq_handler(void);
void USART3_IRQHandler()
__attribute__ ((alias("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART3,
.init = {
#if defined (PIOS_USART_BAUDRATE)
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 230400,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.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,
},
.irq = {
.handler = PIOS_USART_aux_irq_handler,
.handler = NULL,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.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 */
#if defined(PIOS_INCLUDE_COM)
@ -279,7 +266,7 @@ void I2C1_EV_IRQHandler()
void I2C1_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 = I2C1,
.init = {
.I2C_Mode = I2C_Mode_I2C,
@ -307,7 +294,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler,
.handler = NULL,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_EV_IRQn,
@ -317,7 +304,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler,
.handler = NULL,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_ER_IRQn,
@ -388,6 +375,7 @@ void PIOS_Board_Init(void) {
/* Communication system */
#if !defined(PIOS_ENABLE_DEBUG_PINS)
#if defined(PIOS_INCLUDE_COM)
uint32_t pios_usart_aux_id;
if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
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,
.irq = {
.handler = PIOS_SPI_op_irq_handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.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,
.irq = {
.handler = PIOS_SPI_ahrs_irq_handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
@ -143,9 +143,9 @@ void PIOS_SPI_ahrs_irq_handler(void) {
/*
* Telemetry USART
*/
void PIOS_USART_telem_irq_handler(void);
void USART2_IRQHandler() __attribute__ ((alias ("PIOS_USART_telem_irq_handler")));
const struct pios_usart_cfg pios_usart_telem_cfg = { .regs = USART2, .init = {
const struct pios_usart_cfg pios_usart_telem_cfg = {
.regs = USART2,
.init = {
#if defined (PIOS_COM_TELEM_BAUDRATE)
.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
#else
@ -157,7 +157,7 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { .regs = USART2, .init = {
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, .irq = {
.handler = PIOS_USART_telem_irq_handler,
.handler = NULL,
.init = {
.NVIC_IRQChannel = USART2_IRQn,
.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 */
#if defined(PIOS_INCLUDE_COM)
@ -218,6 +213,7 @@ void PIOS_Board_Init(void) {
/* Initialize the PiOS library */
#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)) {
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
ENABLE_AUX_UART ?= NO
USE_TELEMETRY ?= YES
USE_GPS ?= NO
USE_SPEKTRUM ?= NO
USE_SBUS ?= NO
USE_I2C ?= NO
@ -166,6 +163,7 @@ SRC += $(OPUAVSYNTHDIR)/mixersettings.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
#${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c}
#SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c}
# Cant use until i can automatically generate list of UAVObjects
@ -208,6 +206,7 @@ SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/pios_i2c_esc.c
SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/pios_bl_helper.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
## Libraries for flight calculations
SRC += $(FLIGHTLIB)/fifo_buffer.c
@ -381,35 +380,8 @@ ifeq ($(ERASE_FLASH), YES)
CDEFS += -DERASE_FLASH
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)
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
ifeq ($(USE_I2C), YES)

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

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

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

@ -1,118 +1,110 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* Central compile time config for the project.
* In particular, pios_config.h is where you define which PiOS libraries
* and features are included in the firmware.
* @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_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY
#if defined(USE_I2C)
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_I2C_ESC
#endif
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
/* Receiver interfaces - only one allowed */
#if !defined(USE_SPEKTRUM) && !defined(USE_SBUS)
//#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_PWM
#endif
/* USART-based PIOS modules */
#if defined(USE_TELEMETRY)
#define PIOS_INCLUDE_TELEMETRY_RF
#endif
#if defined(USE_GPS)
#define PIOS_INCLUDE_GPS
#endif
#if defined(USE_SPEKTRUM)
#define PIOS_INCLUDE_SPEKTRUM
#endif
#if defined(USE_SBUS)
#define PIOS_INCLUDE_SBUS
#endif
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_SETTINGS
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_RTC
#define PIOS_INCLUDE_WDG
#define PIOS_INCLUDE_BL_HELPER
#define PIOS_INCLUDE_ADXL345
#define PIOS_INCLUDE_FLASH
/* A really shitty setting saving implementation */
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
#define STARTUP_LOG_ENABLED 1
/* COM Module */
#define GPS_BAUDRATE 19200
#define TELEM_BAUDRATE 19200
#define AUXUART_ENABLED 0
#define AUXUART_BAUDRATE 19200
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* Central compile time config for the project.
* In particular, pios_config.h is where you define which PiOS libraries
* and features are included in the firmware.
* @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_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY
#if defined(USE_I2C)
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_I2C_ESC
#endif
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_RCVR
/* Supported receiver interfaces */
#define PIOS_INCLUDE_SPEKTRUM
#define PIOS_INCLUDE_SBUS
//#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_PWM
/* Supported USART-based PIOS modules */
#define PIOS_INCLUDE_TELEMETRY_RF
//#define PIOS_INCLUDE_GPS
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_SETTINGS
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_RTC
#define PIOS_INCLUDE_WDG
#define PIOS_INCLUDE_BL_HELPER
#define PIOS_INCLUDE_ADXL345
#define PIOS_INCLUDE_FLASH
/* A really shitty setting saving implementation */
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
#define STARTUP_LOG_ENABLED 1
/* COM Module */
#define GPS_BAUDRATE 19200
#define TELEM_BAUDRATE 19200
#define AUXUART_ENABLED 0
#define AUXUART_BAUDRATE 19200
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 220
#define HEAP_LIMIT_CRITICAL 150
#define IRQSTACK_LIMIT_WARNING 100
#define HEAP_LIMIT_WARNING 220
#define HEAP_LIMIT_CRITICAL 40
#define IRQSTACK_LIMIT_WARNING 100
#define IRQSTACK_LIMIT_CRITICAL 60
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
/* Task stack sizes */
#define PIOS_ACTUATOR_STACK_SIZE 1020
#define PIOS_MANUAL_STACK_SIZE 724
#define PIOS_SYSTEM_STACK_SIZE 360
#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 */
/**
* @}
* @}
*/
#define CPULOAD_LIMIT_WARNING 85
#define CPULOAD_LIMIT_CRITICAL 95
/* Task stack sizes */
#define PIOS_ACTUATOR_STACK_SIZE 1020
#define PIOS_MANUAL_STACK_SIZE 724
#define PIOS_SYSTEM_STACK_SIZE 460
#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 <openpilot.h>
#include <uavobjectsinit.h>
#include <hwsettings.h>
#include <manualcontrolsettings.h>
#if defined(PIOS_INCLUDE_SPI)
@ -43,7 +45,7 @@
void PIOS_SPI_flash_accel_irq_handler(void);
void DMA1_Channel4_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,
.init = {
.SPI_Mode = SPI_Mode_Master,
@ -61,7 +63,7 @@ const struct pios_spi_cfg pios_spi_flash_accel_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_flash_accel_irq_handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
@ -150,11 +152,11 @@ void PIOS_SPI_flash_accel_irq_handler(void)
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// 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 = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_ADC_DMA_Handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -199,147 +201,14 @@ void PIOS_ADC_handler() {
#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)
/*
* Telemetry USART
*/
void PIOS_USART_telem_irq_handler(void);
void PIOS_IRQH_TELEMETRY() __attribute__ ((alias ("PIOS_USART_telem_irq_handler")));
const struct pios_usart_cfg pios_usart_telem_cfg = {
.regs = PIOS_USART_TELEMETRY,
static const struct pios_usart_cfg pios_usart_telem_main_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_COM_TELEM_BAUDRATE)
.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.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,
},
.irq = {
.handler = PIOS_USART_telem_irq_handler,
.handler = NULL,
.init = {
.NVIC_IRQChannel = PIOS_IRQC_TELEMETRY,
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = PIOS_GPIO_TELEMETRY,
.gpio = GPIOA,
.init = {
.GPIO_Pin = PIOS_RXIO_TELEMETRY,
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = PIOS_GPIO_TELEMETRY,
.gpio = GPIOA,
.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_Mode = GPIO_Mode_AF_PP,
},
@ -378,16 +284,10 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
/*
* GPS USART
*/
void PIOS_USART_gps_irq_handler(void);
void PIOS_IRQH_GPS() __attribute__ ((alias ("PIOS_USART_gps_irq_handler")));
const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = PIOS_USART_GPS,
static const struct pios_usart_cfg pios_usart_gps_main_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_COM_GPS_BAUDRATE)
.USART_BaudRate = PIOS_COM_GPS_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.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,
},
.irq = {
.handler = PIOS_USART_gps_irq_handler,
.handler = NULL,
.init = {
.NVIC_IRQChannel = PIOS_IRQC_GPS,
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = PIOS_GPIO_GPS,
.gpio = GPIOA,
.init = {
.GPIO_Pin = PIOS_RXIO_GPS,
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = PIOS_GPIO_GPS,
.gpio = GPIOA,
.init = {
.GPIO_Pin = PIOS_TXIO_GPS,
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.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)
/*
* SPEKTRUM USART
*/
void PIOS_USART_spektrum_irq_handler(void);
void PIOS_IRQH_SPEKTRUM() __attribute__ ((alias ("PIOS_USART_spektrum_irq_handler")));
const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.regs = PIOS_USART_SPEKTRUM,
#include <pios_spektrum_priv.h>
static const struct pios_usart_cfg pios_usart_spektrum_main_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_COM_SPEKTRUM_BAUDRATE)
.USART_BaudRate = PIOS_COM_SPEKTRUM_BAUDRATE,
#else
.USART_BaudRate = 115200,
#endif
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -443,88 +376,105 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.handler = PIOS_USART_spektrum_irq_handler,
.handler = PIOS_SPEKTRUM_irq_handler,
.init = {
.NVIC_IRQChannel = PIOS_IRQC_SPEKTRUM,
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = PIOS_GPIO_SPEKTRUM,
.gpio = GPIOA,
.init = {
.GPIO_Pin = PIOS_RXIO_SPEKTRUM,
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = PIOS_GPIO_SPEKTRUM,
.gpio = GPIOA,
.init = {
.GPIO_Pin = PIOS_TXIO_SPEKTRUM,
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
static uint32_t pios_usart_spektrum_id;
void PIOS_USART_spektrum_irq_handler(void)
{
SPEKTRUM_IRQHandler(pios_usart_spektrum_id);
}
#include <pios_spektrum_priv.h>
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,
static const struct pios_spektrum_cfg pios_spektrum_main_cfg = {
.bind = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
.port = PIOS_GPIO_SPEKTRUM,
.pin = PIOS_RXIO_SPEKTRUM,
.remap = 0,
};
void PIOS_SUPV_irq_handler() {
if (RTC_GetITStatus(RTC_IT_SEC))
{
/* Call the right handler */
PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id);
static const struct pios_usart_cfg pios_usart_spektrum_flexi_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 115200,
.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 */
#if defined(PIOS_INCLUDE_SBUS)
/*
* SBUS USART
*/
void PIOS_USART_sbus_irq_handler(void);
void PIOS_IRQH_SBUS() __attribute__ ((alias ("PIOS_USART_sbus_irq_handler")));
const struct pios_usart_cfg pios_usart_sbus_cfg = {
.regs = PIOS_USART_SBUS,
#include <pios_sbus_priv.h>
static const struct pios_usart_cfg pios_usart_sbus_main_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_COM_SBUS_BAUDRATE)
.USART_BaudRate = PIOS_COM_SBUS_BAUDRATE,
#else
.USART_BaudRate = 100000,
#endif
.USART_BaudRate = 100000,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_Even,
.USART_StopBits = USART_StopBits_2,
@ -532,88 +482,50 @@ const struct pios_usart_cfg pios_usart_sbus_cfg = {
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.handler = PIOS_USART_sbus_irq_handler,
.handler = PIOS_SBUS_irq_handler,
.init = {
.NVIC_IRQChannel = PIOS_IRQC_SBUS,
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = PIOS_GPIO_SBUS,
.gpio = GPIOA,
.init = {
.GPIO_Pin = PIOS_RXIO_SBUS,
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = PIOS_GPIO_SBUS,
.gpio = GPIOA,
.init = {
.GPIO_Pin = PIOS_TXIO_SBUS,
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
static uint32_t pios_usart_sbus_id;
void PIOS_USART_sbus_irq_handler(void)
{
SBUS_IRQHandler(pios_usart_sbus_id);
}
#include <pios_sbus_priv.h>
void RTC_IRQHandler();
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 */
static const struct pios_sbus_cfg pios_sbus_cfg = {
/* Inverter configuration */
.inv = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.gpio_clk_func = RCC_APB2PeriphClockCmd,
.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,
};
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 */
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
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 */
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM)
@ -621,11 +533,40 @@ void PIOS_USART_gps_irq_handler(void)
#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
*/
#include <pios_servo_priv.h>
const struct pios_servo_channel pios_servo_channels[] = {
static const struct pios_servo_channel pios_servo_channels[] = {
{
.timer = TIM4,
.port = GPIOB,
@ -691,13 +632,65 @@ const struct pios_servo_cfg pios_servo_cfg = {
.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
*/
#if defined(PIOS_INCLUDE_PWM)
#include <pios_pwm_priv.h>
const struct pios_pwm_channel pios_pwm_channels[] = {
static const struct pios_pwm_channel pios_pwm_channels[] = {
{
.timer = TIM4,
.port = GPIOB,
@ -768,7 +761,7 @@ const struct pios_pwm_cfg pios_pwm_cfg = {
},
.remap = 0,
.irq = {
.handler = TIM2_IRQHandler,
.handler = NULL,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.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_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,
.init = {
.I2C_Mode = I2C_Mode_I2C,
@ -833,7 +826,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler,
.handler = NULL,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_EV_IRQn,
@ -843,7 +836,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler,
.handler = NULL,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_ER_IRQn,
@ -869,13 +862,18 @@ void PIOS_I2C_main_adapter_er_irq_handler(void)
#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;
uint32_t pios_com_telem_rf_id;
uint32_t pios_com_telem_usb_id;
uint32_t pios_com_gps_id;
uint32_t pios_com_spektrum_id;
uint32_t pios_com_sbus_id;
/**
* PIOS_Board_Init()
@ -885,70 +883,220 @@ uint32_t pios_com_sbus_id;
void PIOS_Board_Init(void) {
/* Delay system */
PIOS_DELAY_Init();
PIOS_DELAY_Init();
/* Set up the SPI interface to the serial flash */
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_ADXL345_Attach(pios_spi_flash_accel_id);
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 */
EventDispatcherInitialize();
UAVObjInitialize();
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 */
AlarmsInitialize();
/* Initialize the task monitor library */
TaskMonitorInitialize();
/* Initialize the PiOS library */
#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
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();
/* Configure the main IO port */
uint8_t hwsettings_cc_mainport;
HwSettingsCC_MainPortGet(&hwsettings_cc_mainport);
if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_cfg)) {
PIOS_DEBUG_Assert(0);
switch (hwsettings_cc_mainport) {
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_COM */
break;
}
/* Remap AFIO pin */
GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);
@ -957,26 +1105,15 @@ void PIOS_Board_Init(void) {
PIOS_ADC_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)
PIOS_USB_HID_Init(0);
#if defined(PIOS_INCLUDE_COM)
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
#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_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,
.irq = {
.handler = PIOS_SPI_op_mag_irq_handler,
.handler = NULL,
.flags =
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
DMA1_FLAG_GL4),
@ -175,7 +175,7 @@ static const struct pios_spi_cfg pios_spi_accel_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_accel_irq_handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = {
.NVIC_IRQChannel = DMA1_Channel2_IRQn,
@ -265,17 +265,10 @@ void PIOS_SPI_accel_irq_handler(void)
/*
* GPS USART
*/
void PIOS_USART_gps_irq_handler(void);
void USART1_IRQHandler()
__attribute__ ((alias("PIOS_USART_gps_irq_handler")));
const struct pios_usart_cfg pios_usart_gps_cfg = {
static const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_USART_BAUDRATE)
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.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,
},
.irq = {
.handler = PIOS_USART_gps_irq_handler,
.handler = NULL,
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.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 */
#ifdef PIOS_COM_AUX
/*
* AUX USART
*/
void PIOS_USART_aux_irq_handler(void);
void USART4_IRQHandler()
__attribute__ ((alias("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART4,
.init = {
#if defined (PIOS_USART_BAUDRATE)
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.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,
},
.irq = {
.handler = PIOS_USART_aux_irq_handler,
.handler = NULL,
.init = {
.NVIC_IRQChannel = USART4_IRQn,
.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 */
@ -391,7 +365,7 @@ void I2C1_EV_IRQHandler()
void I2C1_ER_IRQHandler()
__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,
.init = {
.I2C_Mode = I2C_Mode_I2C,
@ -419,7 +393,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
},
},
.event = {
.handler = PIOS_I2C_pres_mag_adapter_ev_irq_handler,
.handler = NULL,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_EV_IRQn,
@ -429,7 +403,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
},
},
.error = {
.handler = PIOS_I2C_pres_mag_adapter_er_irq_handler,
.handler = NULL,
.flags = 0, /* FIXME: check this */
.init = {
.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_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,
.init = {
.I2C_Mode = I2C_Mode_I2C,
@ -487,7 +461,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
},
},
.event = {
.handler = PIOS_I2C_gyro_adapter_ev_irq_handler,
.handler = NULL,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_EV_IRQn,
@ -497,7 +471,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
},
},
.error = {
.handler = PIOS_I2C_gyro_adapter_er_irq_handler,
.handler = NULL,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_ER_IRQn,
@ -547,6 +521,7 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_GPS)
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_DEBUG_Assert(0);
}

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

@ -1,644 +1,632 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControlModule Manual Control Module
* @brief Provide manual control or allow it alter flight mode.
* @{
*
* Reads in the ManualControlCommand FlightMode setting from receiver then either
* pass the settings straght to ActuatorDesired object (manual mode) or to
* AttitudeDesired object (stabilized mode)
*
* @file manualcontrol.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief ManualControl module. Handles safety R/C link and flight mode.
*
* @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
*/
#include "openpilot.h"
#include "manualcontrol.h"
#include "manualcontrolsettings.h"
#include "stabilizationsettings.h"
#include "manualcontrolcommand.h"
#include "actuatordesired.h"
#include "stabilizationdesired.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
#include "accessorydesired.h"
// Private constants
#if defined(PIOS_MANUAL_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
#else
#define STACK_SIZE_BYTES 824
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1
#define FLIGHT_MODE_LIMIT 1.0/3.0
#define ARMED_TIME_MS 1000
#define ARMED_THRESHOLD 0.50
//safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 150
// Private types
typedef enum
{
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// Private variables
static xTaskHandle taskHandle;
static ArmState_t armState;
static portTickType lastSysTime;
// Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void manualControlTask(void *parameters);
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 bool okToArm(void);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
#define assumptions1 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions3 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions5 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define ARMING_CHANNEL_ROLL 0
#define ARMING_CHANNEL_PITCH 1
#define ARMING_CHANNEL_YAW 2
#define assumptions7 ( \
( ((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_PITCHFORWARD -(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_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \
)
#define assumptions8 ( \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(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_PITCHAFT -(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) \
)
#define assumptions_flightmode ( \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
)
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
/**
* Module initialization
*/
int32_t ManualControlStart()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0;
}
/**
* Module initialization
*/
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
// Start main task
//ManualControlStart();
return 0;
}
MODULE_INITCALL(ManualControlInitialize, 0, ManualControlStart, 0, MODULE_EXEC_NOORDER_FLAG)
/**
* Module task
*/
static void manualControlTask(void *parameters)
{
ManualControlSettingsData settings;
ManualControlCommandData cmd;
FlightStatusData flightStatus;
float flightMode = 0;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM];
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
// Read settings
ManualControlSettingsGet(&settings);
if (ManualControlCommandReadOnly(&cmd)) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
UAVObjGetMetadata(&cmd, &metadata);
metadata.access = ACCESS_READWRITE;
UAVObjSetMetadata(&cmd, &metadata);
}
}
if (!ManualControlCommandReadOnly(&cmd)) {
// Read channel values in us
// TODO: settings.InputMode is currently ignored because PIOS will not allow runtime
// selection of PWM and PPM. The configuration is currently done at compile time in
// the pios_config.h file.
for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
#if defined(PIOS_INCLUDE_PWM)
cmd.Channel[n] = PIOS_PWM_Get(n);
#elif defined(PIOS_INCLUDE_PPM)
cmd.Channel[n] = PIOS_PPM_Get(n);
#elif defined(PIOS_INCLUDE_SPEKTRUM)
cmd.Channel[n] = PIOS_SPEKTRUM_Get(n);
#elif defined(PIOS_INCLUDE_SBUS)
cmd.Channel[n] = PIOS_SBUS_Get(n);
#endif
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
}
// Check settings, if error raise alarm
if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE ||
settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
continue;
}
// decide if we have valid manual input or not
bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) &&
validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 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,
// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
ManualControlCommandSet(&cmd);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[settings.Roll];
cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw];
cmd.Throttle = scaledChannel[settings.Throttle];
flightMode = scaledChannel[settings.FlightMode];
AccessoryDesiredData accessory;
// Set Accessory 0
if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory0];
if(AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory1];
if(AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accsesory 2
if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory2];
if(AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
processFlightMode(&settings, flightMode);
processArm(&cmd, &settings);
// Update cmd object
ManualControlCommandSet(&cmd);
}
} else {
ManualControlCommandGet(&cmd); /* Under GCS control */
}
FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break;
case FLIGHTMODE_MANUAL:
updateActuatorDesired(&cmd);
break;
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
break;
case FLIGHTMODE_GUIDANCE:
// TODO: Implement
break;
}
}
}
static void updateActuatorDesired(ManualControlCommandData * cmd)
{
ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator);
actuator.Roll = cmd->Roll;
actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
ActuatorDesiredSet(&actuator);
}
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
uint8_t * stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
;
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
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] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) :
0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
}
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{
float valueScaled;
// Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral))
{
if (max != neutral)
valueScaled = (float)(value - neutral) / (float)(max - neutral);
else
valueScaled = 0;
}
else
{
if (min != neutral)
valueScaled = (float)(value - neutral) / (float)(neutral - min);
else
valueScaled = 0;
}
// Bound
if (valueScaled > 1.0) valueScaled = 1.0;
else
if (valueScaled < -1.0) valueScaled = -1.0;
return valueScaled;
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
if(end_time > start_time)
return (end_time - start_time) * portTICK_RATE_MS;
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
}
/**
* @brief Determine if the aircraft is safe to arm
* @returns True if safe to arm, false otherwise
*/
static bool okToArm(void)
{
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
{
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
{ // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
continue;
return false;
}
}
return true;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Process the inputs and determine whether to arm or not
* @param[out] cmd The structure to set the armed in
* @param[in] settings Settings indicating the necessary position
*/
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
bool lowThrottle = cmd->Throttle <= 0;
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
return;
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch(armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
}
bool manualArm = false;
bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD)
manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD)
manualDisarm = true;
switch(armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_ARMED;
else if (!manualArm)
armState = ARM_STATE_DISARMED;
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armState = ARM_STATE_DISARMED;
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_DISARMED;
else if (!manualDisarm)
armState = ARM_STATE_ARMED;
break;
} // End Switch
}
}
/**
* @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
* @param[out] cmd Pointer to the command structure to set the flight mode in
* @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)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t newMode;
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[0];
else if (flightMode > FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[2];
else
newMode = settings->FlightModePosition[1];
if(flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Determine if the manual input value is within acceptable limits
* @returns return TRUE if so, otherwise return FALSE
*/
bool validInputRange(int16_t min, int16_t max, uint16_t value)
{
if (min > max)
{
int16_t tmp = min;
min = max;
max = tmp;
}
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
}
/**
* @}
* @}
*/
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControlModule Manual Control Module
* @brief Provide manual control or allow it alter flight mode.
* @{
*
* Reads in the ManualControlCommand FlightMode setting from receiver then either
* pass the settings straght to ActuatorDesired object (manual mode) or to
* AttitudeDesired object (stabilized mode)
*
* @file manualcontrol.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief ManualControl module. Handles safety R/C link and flight mode.
*
* @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
*/
#include "openpilot.h"
#include "manualcontrol.h"
#include "manualcontrolsettings.h"
#include "stabilizationsettings.h"
#include "manualcontrolcommand.h"
#include "actuatordesired.h"
#include "stabilizationdesired.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
#include "accessorydesired.h"
// Private constants
#if defined(PIOS_MANUAL_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
#else
#define STACK_SIZE_BYTES 824
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1
#define FLIGHT_MODE_LIMIT 1.0/3.0
#define ARMED_TIME_MS 1000
#define ARMED_THRESHOLD 0.50
//safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 150
// Private types
typedef enum
{
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// Private variables
static xTaskHandle taskHandle;
static ArmState_t armState;
static portTickType lastSysTime;
// Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void manualControlTask(void *parameters);
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 bool okToArm(void);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
#define assumptions1 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions3 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions5 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define ARMING_CHANNEL_ROLL 0
#define ARMING_CHANNEL_PITCH 1
#define ARMING_CHANNEL_YAW 2
#define assumptions7 ( \
( ((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_PITCHFORWARD -(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_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \
)
#define assumptions8 ( \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(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_PITCHAFT -(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) \
)
#define assumptions_flightmode ( \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
)
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
/**
* Module initialization
*/
int32_t ManualControlStart()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0;
}
/**
* Module initialization
*/
int32_t ManualControlInitialize()
{
return 0;
}
MODULE_INITCALL(ManualControlInitialize, 0, ManualControlStart, 0, MODULE_EXEC_NOORDER_FLAG)
/**
* Module task
*/
static void manualControlTask(void *parameters)
{
ManualControlSettingsData settings;
ManualControlCommandData cmd;
FlightStatusData flightStatus;
float flightMode = 0;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM];
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
// Read settings
ManualControlSettingsGet(&settings);
if (ManualControlCommandReadOnly(&cmd)) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
UAVObjGetMetadata(&cmd, &metadata);
metadata.access = ACCESS_READWRITE;
UAVObjSetMetadata(&cmd, &metadata);
}
}
if (!ManualControlCommandReadOnly(&cmd)) {
// Read channel values in us
for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
if (pios_rcvr_channel_to_id_map[n]) {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_channel_to_id_map[n]);
} else {
cmd.Channel[n] = -1;
}
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
}
// Check settings, if error raise alarm
if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE ||
settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
continue;
}
// decide if we have valid manual input or not
bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) &&
validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 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,
// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
ManualControlCommandSet(&cmd);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[settings.Roll];
cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw];
cmd.Throttle = scaledChannel[settings.Throttle];
flightMode = scaledChannel[settings.FlightMode];
AccessoryDesiredData accessory;
// Set Accessory 0
if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory0];
if(AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory1];
if(AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accsesory 2
if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory2];
if(AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
processFlightMode(&settings, flightMode);
processArm(&cmd, &settings);
// Update cmd object
ManualControlCommandSet(&cmd);
}
} else {
ManualControlCommandGet(&cmd); /* Under GCS control */
}
FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break;
case FLIGHTMODE_MANUAL:
updateActuatorDesired(&cmd);
break;
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
break;
case FLIGHTMODE_GUIDANCE:
// TODO: Implement
break;
}
}
}
static void updateActuatorDesired(ManualControlCommandData * cmd)
{
ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator);
actuator.Roll = cmd->Roll;
actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
ActuatorDesiredSet(&actuator);
}
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
uint8_t * stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
;
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
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] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) :
0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
}
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{
float valueScaled;
// Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral))
{
if (max != neutral)
valueScaled = (float)(value - neutral) / (float)(max - neutral);
else
valueScaled = 0;
}
else
{
if (min != neutral)
valueScaled = (float)(value - neutral) / (float)(neutral - min);
else
valueScaled = 0;
}
// Bound
if (valueScaled > 1.0) valueScaled = 1.0;
else
if (valueScaled < -1.0) valueScaled = -1.0;
return valueScaled;
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
if(end_time > start_time)
return (end_time - start_time) * portTICK_RATE_MS;
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
}
/**
* @brief Determine if the aircraft is safe to arm
* @returns True if safe to arm, false otherwise
*/
static bool okToArm(void)
{
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
{
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
{ // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
continue;
return false;
}
}
return true;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Process the inputs and determine whether to arm or not
* @param[out] cmd The structure to set the armed in
* @param[in] settings Settings indicating the necessary position
*/
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
bool lowThrottle = cmd->Throttle <= 0;
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
return;
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch(armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
}
bool manualArm = false;
bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD)
manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD)
manualDisarm = true;
switch(armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_ARMED;
else if (!manualArm)
armState = ARM_STATE_DISARMED;
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armState = ARM_STATE_DISARMED;
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_DISARMED;
else if (!manualDisarm)
armState = ARM_STATE_ARMED;
break;
} // End Switch
}
}
/**
* @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
* @param[out] cmd Pointer to the command structure to set the flight mode in
* @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)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t newMode;
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[0];
else if (flightMode > FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[2];
else
newMode = settings->FlightModePosition[1];
if(flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Determine if the manual input value is within acceptable limits
* @returns return TRUE if so, otherwise return FALSE
*/
bool validInputRange(int16_t min, int16_t max, uint16_t value)
{
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
ENABLE_AUX_UART ?= NO
USE_SPEKTRUM ?= NO
USE_SBUS ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES
@ -193,6 +189,7 @@ SRC += $(PIOSCOMMON)/pios_hcsr04.c
SRC += $(PIOSCOMMON)/pios_i2c_esc.c
SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/pios_bl_helper.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
@ -362,14 +359,6 @@ ifeq ($(ENABLE_AUX_UART), YES)
CDEFS += -DPIOS_ENABLE_AUX_UART
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
# Assembler with preprocessor here.
#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_LED
#if defined(USE_SPEKTRUM)
#define PIOS_INCLUDE_RCVR
#define PIOS_INCLUDE_SPEKTRUM
#elif defined(USE_SBUS)
#define PIOS_INCLUDE_SBUS
#else
//#define PIOS_INCLUDE_PPM
//#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_PWM
#endif
//#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_SERVO
#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 DMA1_Channel2_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,
.init = {
.SPI_Mode = SPI_Mode_Master,
@ -63,7 +63,7 @@ const struct pios_spi_cfg pios_spi_sdcard_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_sdcard_irq_handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = {
.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 DMA1_Channel4_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,
.init = {
.SPI_Mode = SPI_Mode_Master,
@ -162,7 +162,7 @@ const struct pios_spi_cfg pios_spi_ahrs_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_ahrs_irq_handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
@ -258,11 +258,11 @@ void PIOS_SPI_ahrs_irq_handler(void)
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// 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 = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_ADC_DMA_Handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -310,16 +310,10 @@ void PIOS_ADC_handler() {
/*
* Telemetry USART
*/
void PIOS_USART_telem_irq_handler(void);
void USART2_IRQHandler() __attribute__ ((alias ("PIOS_USART_telem_irq_handler")));
const struct pios_usart_cfg pios_usart_telem_cfg = {
static const struct pios_usart_cfg pios_usart_telem_cfg = {
.regs = USART2,
.init = {
#if defined (PIOS_COM_TELEM_BAUDRATE)
.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.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,
},
.irq = {
.handler = PIOS_USART_telem_irq_handler,
.handler = NULL,
.init = {
.NVIC_IRQChannel = USART2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -356,17 +350,11 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
/*
* GPS USART
*/
void PIOS_USART_gps_irq_handler(void);
void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_gps_irq_handler")));
const struct pios_usart_cfg pios_usart_gps_cfg = {
static const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = USART3,
.remap = GPIO_PartialRemap_USART3,
.init = {
#if defined (PIOS_COM_GPS_BAUDRATE)
.USART_BaudRate = PIOS_COM_GPS_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.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,
},
.irq = {
.handler = PIOS_USART_gps_irq_handler,
.handler = NULL,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -404,16 +392,9 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
/*
* AUX USART
*/
void PIOS_USART_aux_irq_handler(void);
void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_COM_AUX_BAUDRATE)
.USART_BaudRate = PIOS_COM_AUX_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.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,
},
.irq = {
.handler = PIOS_USART_aux_irq_handler,
.handler = NULL,
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -450,20 +431,45 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
};
#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
/*
* SPEKTRUM USART
*/
void PIOS_USART_spektrum_irq_handler(void);
void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_spektrum_irq_handler")));
const struct pios_usart_cfg pios_usart_spektrum_cfg = {
#include <pios_spektrum_priv.h>
static const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_COM_SPEKTRUM_BAUDRATE)
.USART_BaudRate = PIOS_COM_SPEKTRUM_BAUDRATE,
#else
.USART_BaudRate = 115200,
#endif
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -471,7 +477,7 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.handler = PIOS_USART_spektrum_irq_handler,
.handler = PIOS_SPEKTRUM_irq_handler,
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.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;
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>
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,
static const struct pios_spektrum_cfg pios_spektrum_cfg = {
.bind = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
.port = GPIOA,
.pin = GPIO_Pin_10,
.remap = 0,
};
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 */
#if defined(PIOS_INCLUDE_SBUS)
#error PIOS_INCLUDE_SBUS not implemented
#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 */
#if defined(PIOS_INCLUDE_COM)
@ -582,7 +540,7 @@ void PIOS_USART_aux_irq_handler(void)
* Pios servo configuration structures
*/
#include <pios_servo_priv.h>
const struct pios_servo_channel pios_servo_channels[] = {
static const struct pios_servo_channel pios_servo_channels[] = {
{
.timer = TIM4,
.port = GPIOB,
@ -666,7 +624,7 @@ const struct pios_servo_cfg pios_servo_cfg = {
*/
#if defined(PIOS_INCLUDE_PWM)
#include <pios_pwm_priv.h>
const struct pios_pwm_channel pios_pwm_channels[] = {
static const struct pios_pwm_channel pios_pwm_channels[] = {
{
.timer = TIM1,
.port = GPIOA,
@ -751,7 +709,7 @@ const struct pios_pwm_cfg pios_pwm_cfg = {
},
.remap = GPIO_PartialRemap_TIM3,
.irq = {
.handler = TIM1_CC_IRQHandler,
.handler = NULL,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
@ -782,7 +740,7 @@ void PIOS_TIM5_irq_handler()
#include <pios_ppm_priv.h>
void TIM6_IRQHandler();
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_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */
.TIM_ClockDivision = TIM_CKD_DIV1,
@ -791,7 +749,7 @@ const struct pios_ppmsv_cfg pios_ppmsv_cfg = {
.TIM_RepetitionCounter = 0x0000,
},
.irq = {
.handler = TIM6_IRQHandler,
.handler = NULL,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
@ -802,14 +760,14 @@ const struct pios_ppmsv_cfg pios_ppmsv_cfg = {
.ccr = TIM_IT_Update,
};
void PIOS_TIM6_irq_handler()
void PIOS_TIM6_irq_handler(void)
{
PIOS_PPMSV_irq_handler();
}
void TIM1_CC_IRQHandler();
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_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */
.TIM_ClockDivision = TIM_CKD_DIV1,
@ -831,7 +789,7 @@ const struct pios_ppm_cfg pios_ppm_cfg = {
},
.remap = 0,
.irq = {
.handler = TIM1_CC_IRQHandler,
.handler = NULL,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
@ -844,7 +802,7 @@ const struct pios_ppm_cfg pios_ppm_cfg = {
.ccr = TIM_IT_CC2,
};
void PIOS_TIM1_CC_irq_handler()
void PIOS_TIM1_CC_irq_handler(void)
{
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_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,
.init = {
.I2C_Mode = I2C_Mode_I2C,
@ -892,7 +850,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler,
.handler = NULL,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_EV_IRQn,
@ -902,7 +860,7 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler,
.handler = NULL,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_ER_IRQn,
@ -1013,6 +971,13 @@ static const struct stm32_gpio pios_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;
uint32_t pios_com_telem_rf_id;
@ -1042,7 +1007,7 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_SPI)
/* Set up the SPI interface to the SD card */
if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) {
PIOS_DEBUG_Assert(0);
PIOS_Assert(0);
}
/* Enable and mount the SDCard */
@ -1050,23 +1015,16 @@ void PIOS_Board_Init(void) {
PIOS_SDCARD_MountFS(0);
#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 */
EventDispatcherInitialize();
UAVObjInitialize();
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 */
AlarmsInitialize();
@ -1078,7 +1036,7 @@ void PIOS_Board_Init(void) {
/* Set up the SPI interface to the AHRS */
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 */
@ -1086,43 +1044,95 @@ void PIOS_Board_Init(void) {
/* Initialize the PiOS library */
#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)) {
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)) {
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)) {
PIOS_DEBUG_Assert(0);
PIOS_Assert(0);
}
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
PIOS_Servo_Init();
PIOS_ADC_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 (PIOS_PWM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS)
#error More receiver inputs than available devices
#endif
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
#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();
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
#if defined(PIOS_INCLUDE_USB_HID)
PIOS_USB_HID_Init(0);
#if defined(PIOS_INCLUDE_COM)
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_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);
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_I2C */
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_RX_BUFFER_SIZE 256
#define PIOS_USART_TX_BUFFER_SIZE 256
#define PIOS_USART_BAUDRATE 230400
//-------------------------
// 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_TELEM_BAUDRATE 57600
extern uint32_t pios_com_telem_rf_id;
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_DEBUG PIOS_COM_TELEM_RF
#if defined(PIOS_INCLUDE_GPS)
#define PIOS_COM_GPS_BAUDRATE 57600
extern uint32_t pios_com_gps_id;
#define PIOS_COM_GPS (pios_com_gps_id)
#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)
#ifdef PIOS_INCLUDE_SPEKTRUM
#define PIOS_COM_SPEKTRUM_BAUDRATE 115200
extern uint32_t pios_com_spektrum_id;
#define PIOS_COM_SPEKTRUM (pios_com_spektrum_id)
#endif
#ifdef PIOS_INCLUDE_SBUS
#define PIOS_COM_SBUS_BAUDRATE 100000
extern uint32_t pios_com_sbus_id;
#define PIOS_COM_SBUS (pios_com_sbus_id)
#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_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

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_GPS_BAUDRATE 57600
extern uint32_t pios_com_gps_id;
#define PIOS_COM_GPS (pios_com_gps_id)
#ifdef PIOS_ENABLE_AUX_UART
#define PIOS_COM_AUX_BAUDRATE 57600
extern uint32_t pios_com_aux_id;
#define PIOS_COM_AUX (pios_com_aux_id)
#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_TELEM_BAUDRATE 57600
extern uint32_t 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;
#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)
#ifdef PIOS_ENABLE_AUX_UART
#define PIOS_COM_AUX_BAUDRATE 57600
extern uint32_t pios_com_aux_id;
#define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_DEBUG PIOS_COM_AUX
#endif
#ifdef PIOS_INCLUDE_SPEKTRUM
#define PIOS_COM_SPEKTRUM_BAUDRATE 115200
extern uint32_t pios_com_spektrum_id;
#define PIOS_COM_SPEKTRUM (pios_com_spektrum_id)
#endif
#ifdef PIOS_INCLUDE_SBUS
#define PIOS_COM_SBUS_BAUDRATE 100000
extern uint32_t pios_com_sbus_id;
#define PIOS_COM_SBUS (pios_com_sbus_id)
#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_HIGHEST 4 // for USART etc...
//-------------------------
// Receiver PWM inputs
//-------------------------
/*#define PIOS_PWM_SUPV_ENABLED 1
#define PIOS_PWM_SUPV_TIMER TIM6
#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)*/
//------------------------
// PIOS_RCVR
// See also pios_board.c
//------------------------
#define PIOS_RCVR_MAX_DEVS 12
//-------------------------
// Receiver PPM input
//-------------------------
#define PIOS_PPM_NUM_INPUTS 8 //Could be more if needed
#define PIOS_PPM_SUPV_ENABLED 1
//-------------------------
// SPEKTRUM input
// Receiver PWM input
//-------------------------
//#define PIOS_SPEKTRUM_SUPV_ENABLED 1
//#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
//#define PIOS_SPEKTRUM_SUPV_IRQ_CHANNEL TIM6_IRQn
//#define PIOS_SPEKTRUM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void)
#define PIOS_PWM_NUM_INPUTS 8
//-------------------------
// Receiver SPEKTRUM input
//-------------------------
#define PIOS_SPEKTRUM_NUM_INPUTS 12
//-------------------------
// Servo outputs
//-------------------------
#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_PWM_MAX_INPUTS 8
//-------------------------
// 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)
/* 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 uint8_t PulseIndex;
static uint32_t PreviousValue;
static uint32_t CurrentValue;
static uint32_t CapturedValue;
static uint32_t CaptureValue[PIOS_PPM_NUM_INPUTS];
static uint8_t SupervisorState = 0;
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];
/**
* Initialises all the LED's
*/
static void PIOS_PPM_Supervisor(uint32_t ppm_id);
void PIOS_PPM_Init(void)
{
/* Flush counter variables */
@ -59,6 +66,7 @@ void PIOS_PPM_Init(void)
PreviousValue = 0;
CurrentValue = 0;
CapturedValue = 0;
TimerCounter = 0;
for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
CaptureValue[i] = 0;
@ -121,13 +129,12 @@ void PIOS_PPM_Init(void)
TIM_TimeBaseInit(pios_ppm_cfg.timer, &TIM_TimeBaseStructure);
/* 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 */
TIM_Cmd(pios_ppm_cfg.timer, ENABLE);
/* Supervisor Setup */
#if (PIOS_PPM_SUPV_ENABLED)
/* Flush counter variables */
for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
CapCounter[i] = 0;
@ -136,70 +143,15 @@ void PIOS_PPM_Init(void)
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 */
/* 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;
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 >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 */
if (Channel >= PIOS_PPM_NUM_INPUTS) {
if (chan_id >= PIOS_PPM_NUM_INPUTS) {
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)
{
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 */
if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) == SET) {
PreviousValue = CurrentValue;
@ -241,37 +202,44 @@ void PIOS_PPM_irq_handler(void)
CurrentValue = TIM_GetCapture4(pios_ppm_cfg.timer);
break;
}
}
CurrentValue+=TimerCounter;
if(CurrentValue > 0xFFFF) {
CurrentValue-=0xFFFF;
}
/* Clear TIMx Capture compare interrupt pending bit */
TIM_ClearITPendingBit(pios_ppm_cfg.timer, pios_ppm_cfg.ccr);
/* Clear TIMx Capture compare interrupt pending bit */
TIM_ClearITPendingBit(pios_ppm_cfg.timer, pios_ppm_cfg.ccr);
/* Capture computation */
if (CurrentValue > PreviousValue) {
CapturedValue = (CurrentValue - PreviousValue);
} else {
CapturedValue = ((0xFFFF - PreviousValue) + CurrentValue);
}
/* Capture computation */
if (CurrentValue > PreviousValue) {
CapturedValue = (CurrentValue - PreviousValue);
} else {
CapturedValue = ((0xFFFF - PreviousValue) + CurrentValue);
}
/* sync pulse */
if (CapturedValue > 8000) {
PulseIndex = 0;
/* trying to detect bad pulses, not sure this is working correctly yet. I need a scope :P */
} else if (CapturedValue > 750 && CapturedValue < 2500) {
if (PulseIndex < PIOS_PPM_NUM_INPUTS) {
CaptureValue[PulseIndex] = CapturedValue;
CapCounter[PulseIndex]++;
PulseIndex++;
/* sync pulse */
if (CapturedValue > 8000) {
PulseIndex = 0;
/* trying to detect bad pulses, not sure this is working correctly yet. I need a scope :P */
} else if (CapturedValue > 750 && CapturedValue < 2500) {
if (PulseIndex < PIOS_PPM_NUM_INPUTS) {
CaptureValue[PulseIndex] = CapturedValue;
CapCounter[PulseIndex]++;
PulseIndex++;
}
}
}
}
/**
* This function handles TIM3 global interrupt request.
*/
void PIOS_PPMSV_irq_handler(void) {
/* Clear timer interrupt pending bit */
TIM_ClearITPendingBit(pios_ppmsv_cfg.timer, pios_ppmsv_cfg.ccr);
static void PIOS_PPM_Supervisor(uint32_t ppm_id) {
/*
* RTC runs at 625Hz so divide down the base rate so
* that this loop runs at 25Hz.
*/
if(++supv_timer < 25) {
return;
}
supv_timer = 0;
/* Simple state machine */
if (SupervisorState == 0) {

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

@ -34,15 +34,20 @@
#if defined(PIOS_INCLUDE_PWM)
/* Local Variables */
static uint8_t CaptureState[PIOS_PWM_MAX_INPUTS];
static uint16_t RiseValue[PIOS_PWM_MAX_INPUTS];
static uint16_t FallValue[PIOS_PWM_MAX_INPUTS];
static uint32_t CaptureValue[PIOS_PWM_MAX_INPUTS];
/* Provide a RCVR driver */
static int32_t PIOS_PWM_Get(uint32_t chan_id);
//static uint8_t SupervisorState = 0;
static uint32_t CapCounter[PIOS_PWM_MAX_INPUTS];
//static uint32_t CapCounterPrev[MAX_CHANNELS];
const struct pios_rcvr_driver pios_pwm_rcvr_driver = {
.read = PIOS_PWM_Get,
};
/* 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
@ -127,52 +132,6 @@ void PIOS_PWM_Init(void)
/* Warning, I don't think this will work for multiple remaps at once */
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 >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 */
if (Channel >= pios_pwm_cfg.num_channels) {
if (chan_id >= pios_pwm_cfg.num_channels) {
return -1;
}
return CaptureValue[Channel];
return CaptureValue[chan_id];
}
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
/**

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

@ -32,30 +32,39 @@
#include "pios.h"
#if defined(PIOS_INCLUDE_RTC)
#include <pios_rtc_priv.h>
#ifndef PIOS_RTC_PRESCALAR
#define PIOS_RTC_PRESCALAR 100
#ifndef PIOS_RTC_PRESCALER
#define PIOS_RTC_PRESCALER 100
#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,
ENABLE);
PWR_BackupAccessCmd(ENABLE);
RCC_RTCCLKConfig(RCC_RTCCLKSource_HSE_Div128);
RCC_RTCCLKConfig(cfg->clksrc);
RCC_RTCCLKCmd(ENABLE);
RTC_WaitForLastTask();
RTC_WaitForSynchro();
RTC_WaitForLastTask();
#if defined(PIOS_INCLUDE_SPEKTRUM) || defined(PIOS_INCLUDE_SBUS)
/* Enable the RTC Second interrupt */
/* Configure and enable the RTC Second interrupt */
NVIC_Init(&cfg->irq.init);
RTC_ITConfig( RTC_IT_SEC, ENABLE );
/* Wait until last write operation on RTC registers has finished */
RTC_WaitForLastTask();
#endif
RTC_SetPrescaler(PIOS_RTC_PRESCALAR); // counting at 8e6 / 128
RTC_SetPrescaler(cfg->prescaler);
RTC_WaitForLastTask();
RTC_SetCounter(0);
RTC_WaitForLastTask();
@ -66,9 +75,12 @@ uint32_t PIOS_RTC_Counter()
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()
{
return (float) (8e6 / 128) / (1 + PIOS_RTC_PRESCALAR);
return (float) (8e6 / 128) / (1 + PIOS_RTC_PRESCALER);
}
float PIOS_RTC_MsPerTick()
@ -76,6 +88,39 @@ float PIOS_RTC_MsPerTick()
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
/**

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

@ -36,6 +36,13 @@
/* 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 */
static uint16_t channel_data[SBUS_NUMBER_OF_CHANNELS];
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 frame_found;
static void PIOS_SBUS_Supervisor(uint32_t sbus_id);
/**
* reset_channels() function clears all channel data in case of
* 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
*/
void PIOS_SBUS_Init(void)
void PIOS_SBUS_Init(const struct pios_sbus_cfg *cfg)
{
/* Enable USART input invertor clock and enable the invertor */
(*pios_sbus_cfg.gpio_clk_func)(pios_sbus_cfg.gpio_clk_periph, ENABLE);
GPIO_Init(pios_sbus_cfg.gpio_inv_port, &pios_sbus_cfg.gpio_inv_init);
GPIO_WriteBit(pios_sbus_cfg.gpio_inv_port,
pios_sbus_cfg.gpio_inv_init.GPIO_Pin,
pios_sbus_cfg.gpio_inv_enable);
/* Enable inverter clock and enable the inverter */
(*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE);
GPIO_Init(cfg->inv.gpio, &cfg->inv.init);
GPIO_WriteBit(cfg->inv.gpio,
cfg->inv.init.GPIO_Pin,
cfg->gpio_inv_enable);
/* Init RTC supervisor timer interrupt */
static const NVIC_InitTypeDef NVIC_InitStructure = {
.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();
if (!PIOS_RTC_RegisterTickCallback(PIOS_SBUS_Supervisor, 0)) {
PIOS_Assert(0);
}
}
/**
@ -154,23 +155,28 @@ void PIOS_SBUS_Init(void)
* \output -1 channel not available
* \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 */
if (channel >= SBUS_NUMBER_OF_CHANNELS) {
if (chan_id >= SBUS_NUMBER_OF_CHANNELS) {
return -1;
}
return channel_data[channel];
return channel_data[chan_id];
}
/**
* 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 */
volatile uint16_t sr = pios_sbus_cfg.pios_usart_sbus_cfg->regs->SR;
volatile uint8_t b = pios_sbus_cfg.pios_usart_sbus_cfg->regs->DR;
volatile uint16_t sr = cfg->regs->SR;
volatile uint8_t b = cfg->regs->DR;
/* process received byte if one has arrived */
if (sr & USART_SR_RXNE) {
@ -182,7 +188,7 @@ void SBUS_IRQHandler(uint32_t usart_id)
/* ignore TXE interrupts */
if (sr & USART_SR_TXE) {
/* 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
* 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 */
if (++receive_timer > 2) {

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

@ -34,12 +34,6 @@
#include "pios_spektrum_priv.h"
#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:
@ -51,31 +45,35 @@
/* 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 */
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 };
uint8_t sync_of = 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
*/
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
if (0) {
PIOS_SPEKTRUM_Bind();
if (bind) {
PIOS_SPEKTRUM_Bind(cfg);
}
/* Init RTC supervisor timer interrupt */
NVIC_InitTypeDef NVIC_InitStructure;
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();
if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, 0)) {
PIOS_DEBUG_Assert(0);
}
}
/**
@ -84,59 +82,56 @@ void PIOS_SPEKTRUM_Init(void)
* \output -1 Channel not available
* \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 */
if (Channel >= 12) {
if (chan_id >= PIOS_SPEKTRUM_NUM_INPUTS) {
return -1;
}
return CaptureValue[Channel];
return CaptureValue[chan_id];
}
/**
* Spektrum bind function
* \output 1 Successful bind
* \output 0 Bind failed
* \note Applications shouldn't call these functions directly
* \output true Successful bind
* \output false Bind failed
*/
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_InitStructure.GPIO_Pin = pios_spektrum_cfg.pin;
GPIO_Init(pios_spektrum_cfg.port, &GPIO_InitStructure);
GPIO_Init(cfg->bind.gpio, &cfg->bind.init);
pios_spektrum_cfg.port->BRR = pios_spektrum_cfg.pin;
GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
//PIOS_DELAY_WaitmS(75);
/* 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);
/* 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);
/* 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);
/* 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);
/* 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);
/* 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);
/* 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);
/* 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);
/* 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);
/* 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)
* \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;*/
uint8_t channeln = 0, frame = 0;
@ -201,7 +196,7 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
{
frame_error=1;
}
if (channeln < 12 && !frame_error)
if (channeln < PIOS_SPEKTRUM_NUM_INPUTS && !frame_error)
CaptureValueTemp[channeln] = data;
}
}
@ -212,7 +207,7 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
sync_of = 0;
if (!frame_error)
{
for(int i=0;i<12;i++)
for(int i=0;i<PIOS_SPEKTRUM_NUM_INPUTS;i++)
{
CaptureValue[i] = CaptureValueTemp[i];
}
@ -223,11 +218,16 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
return 0;
}
/* Interrupt handler for USART */
void SPEKTRUM_IRQHandler(uint32_t usart_id) {
/* Custom interrupt handler for USART */
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 */
volatile uint16_t sr = pios_spektrum_cfg.pios_usart_spektrum_cfg->regs->SR;
volatile uint8_t b = pios_spektrum_cfg.pios_usart_spektrum_cfg->regs->DR;
volatile uint16_t sr = cfg->regs->SR;
volatile uint8_t b = cfg->regs->DR;
/* check if RXNE flag is set */
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
/* 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 */
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 clears the channel values
*/
void PIOS_SPEKTRUM_irq_handler() {
static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id) {
/* 125hz */
supv_timer++;
if(supv_timer > 5) {
@ -262,7 +262,7 @@ void PIOS_SPEKTRUM_irq_handler() {
if (sync_of > 12) {
/* signal lost */
sync_of = 0;
for (int i = 0; i < 12; i++) {
for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) {
CaptureValue[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,
};
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)
{
return (usart_dev->magic == PIOS_USART_DEV_MAGIC);
@ -85,6 +102,34 @@ static struct pios_usart_dev * PIOS_USART_alloc(void)
}
#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
@ -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;
/* 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);
USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE);
USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE);
@ -147,6 +197,19 @@ out_fail:
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.
* \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;
bool valid = PIOS_USART_validate(usart_dev);
PIOS_Assert(valid)
PIOS_Assert(valid);
USART_InitTypeDef USART_InitStructure;
@ -337,12 +400,20 @@ static int32_t PIOS_USART_TxBufferPutMore(uint32_t usart_id, const uint8_t *buff
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;
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 */
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
#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 */

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

@ -34,13 +34,6 @@
#include <pios.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 {
TIM_TimeBaseInitTypeDef tim_base_init;
TIM_ICInitTypeDef tim_ic_init;
@ -53,11 +46,13 @@ struct pios_ppm_cfg {
};
extern void PIOS_PPM_irq_handler();
extern void PIOS_PPMSV_irq_handler();
extern uint8_t pios_ppm_num_channels;
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 */

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

@ -30,9 +30,4 @@
#ifndef 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 */

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

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
#define PIOS_RTC_H
#include <stdbool.h>
/* Public Functions */
extern void PIOS_RTC_Init();
extern uint32_t PIOS_RTC_Counter();
extern float PIOS_RTC_Rate();
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 */
/* 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 */

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

@ -66,20 +66,21 @@
#define SBUS_NUMBER_OF_CHANNELS 8
/*
* S.Bus configuration includes USART and programmable invertor
* S.Bus configuration programmable invertor
*/
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);
uint32_t gpio_clk_periph;
GPIO_TypeDef* gpio_inv_port;
GPIO_InitTypeDef gpio_inv_init;
BitAction gpio_inv_enable;
};
extern const struct pios_sbus_cfg pios_sbus_cfg;
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 */
/**

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

@ -34,11 +34,6 @@
/* Global Types */
/* 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 */

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

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

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

@ -32,7 +32,7 @@
#define PIOS_STM32_H
struct stm32_irq {
void (*handler) (void);
void (*handler) (uint32_t);
uint32_t flags;
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;
};
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 void PIOS_USART_IRQ_Handler(uint32_t usart_id);
extern const struct pios_usart_cfg * PIOS_USART_GetConfig(uint32_t usart_id);
#endif /* PIOS_USART_PRIV_H */

1
flight/PiOS/pios.h
View File

@ -78,6 +78,7 @@
#include <pios_spi.h>
#include <pios_ppm.h>
#include <pios_pwm.h>
#include <pios_rcvr.h>
#include <pios_spektrum.h>
#include <pios_sbus.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,
.irq =
{
.handler = PIOS_SPI_port_irq_handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = {
.NVIC_IRQChannel = DMA1_Channel2_IRQn,
@ -189,11 +189,11 @@ void PIOS_SPI_port_irq_handler(void)
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// 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 = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_ADC_DMA_Handler,
.handler = NULL,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -245,10 +245,7 @@ void PIOS_ADC_handler() {
/*
* SERIAL USART
*/
void PIOS_USART_serial_irq_handler(void);
void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_serial_irq_handler")));
const struct pios_usart_cfg pios_usart_serial_cfg =
static const struct pios_usart_cfg pios_usart_serial_cfg =
{
.regs = USART1,
.init =
@ -262,7 +259,7 @@ const struct pios_usart_cfg pios_usart_serial_cfg =
},
.irq =
{
.handler = PIOS_USART_serial_irq_handler,
.handler = NULL,
.init =
{
.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 */
// ***********************************************************************************
@ -334,6 +325,7 @@ void PIOS_Board_Init(void) {
// Delay system
PIOS_DELAY_Init();
uint32_t pios_usart_serial_id;
if (PIOS_USART_Init(&pios_usart_serial_id, &pios_usart_serial_cfg)) {
PIOS_DEBUG_Assert(0);
}

2
flight/UAVObjects/eventdispatcher.c
View File

@ -28,7 +28,7 @@
// Private constants
#define MAX_QUEUE_SIZE 20
#define STACK_SIZE configMINIMAL_STACK_SIZE
#define STACK_SIZE 96
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#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/sonaraltitude.h \
$$UAVOBJECT_SYNTHETICS/flightstatus.h \
$$UAVOBJECT_SYNTHETICS/hwsettings.h \
$$UAVOBJECT_SYNTHETICS/attitudesettings.h
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
@ -116,4 +117,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/sonaraltitude.cpp \
$$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \
$$UAVOBJECT_SYNTHETICS/flightstatus.cpp \
$$UAVOBJECT_SYNTHETICS/hwsettings.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>
<object name="ManualControlSettings" singleinstance="true" settings="true">
<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="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"/>