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

moved source files around in preparation to adding makefile build way

Browse source 
added makefile (thx gke)
added linker script (thx gke)
moved startups into src directory as well
no code changes.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@105 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-03-06 04:41:23 +00:00
parent aae8ef4c3d
commit 5091f3e9ff
45 changed files with 229 additions and 14182 deletions
Download patch file Download diff file Expand all files Collapse all files

349
src/baseflight_startups/startup_stm32f10x_md.s Normal file
View file

@ -0,0 +1,349 @@
;******************** (C) COPYRIGHT 2011 STMicroelectronics ********************
;* File Name : startup_stm32f10x_md.s
;* Author : MCD Application Team
;* Version : V3.5.0
;* Date : 11-March-2011
;* Description : STM32F10x Medium Density Devices vector table for MDK-ARM
;* toolchain.
;* This module performs:
;* - Set the initial SP
;* - Set the initial PC == Reset_Handler
;* - Set the vector table entries with the exceptions ISR address
;* - Configure the clock system
;* - Branches to __main in the C library (which eventually
;* calls main()).
;* After Reset the CortexM3 processor is in Thread mode,
;* priority is Privileged, and the Stack is set to Main.
;* <<< Use Configuration Wizard in Context Menu >>>
;*******************************************************************************
; THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
; WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
; AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
; INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
; CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
; INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
;*******************************************************************************
; Amount of memory (in bytes) allocated for Stack
; Tailor this value to your application needs
; <h> Stack Configuration
; <o> Stack Size (in Bytes) <0x0-0xFFFFFFFF:8>
; </h>
Stack_Size EQU 0x00001000
AREA STACK, NOINIT, READWRITE, ALIGN=3
Stack_Mem SPACE Stack_Size
__initial_sp
; <h> Heap Configuration
; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
; </h>
Heap_Size EQU 0x00000200
AREA HEAP, NOINIT, READWRITE, ALIGN=3
__heap_base
Heap_Mem SPACE Heap_Size
__heap_limit
PRESERVE8
THUMB
; Vector Table Mapped to Address 0 at Reset
AREA RESET, DATA, READONLY
EXPORT __Vectors
EXPORT __Vectors_End
EXPORT __Vectors_Size
__Vectors DCD __initial_sp ; Top of Stack
DCD Reset_Handler ; Reset Handler
DCD NMI_Handler ; NMI Handler
DCD HardFault_Handler ; Hard Fault Handler
DCD MemManage_Handler ; MPU Fault Handler
DCD BusFault_Handler ; Bus Fault Handler
DCD UsageFault_Handler ; Usage Fault Handler
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD SVC_Handler ; SVCall Handler
DCD DebugMon_Handler ; Debug Monitor Handler
DCD 0 ; Reserved
DCD PendSV_Handler ; PendSV Handler
DCD SysTick_Handler ; SysTick Handler
; External Interrupts
DCD WWDG_IRQHandler ; Window Watchdog
DCD PVD_IRQHandler ; PVD through EXTI Line detect
DCD TAMPER_IRQHandler ; Tamper
DCD RTC_IRQHandler ; RTC
DCD FLASH_IRQHandler ; Flash
DCD RCC_IRQHandler ; RCC
DCD EXTI0_IRQHandler ; EXTI Line 0
DCD EXTI1_IRQHandler ; EXTI Line 1
DCD EXTI2_IRQHandler ; EXTI Line 2
DCD EXTI3_IRQHandler ; EXTI Line 3
DCD EXTI4_IRQHandler ; EXTI Line 4
DCD DMA1_Channel1_IRQHandler ; DMA1 Channel 1
DCD DMA1_Channel2_IRQHandler ; DMA1 Channel 2
DCD DMA1_Channel3_IRQHandler ; DMA1 Channel 3
DCD DMA1_Channel4_IRQHandler ; DMA1 Channel 4
DCD DMA1_Channel5_IRQHandler ; DMA1 Channel 5
DCD DMA1_Channel6_IRQHandler ; DMA1 Channel 6
DCD DMA1_Channel7_IRQHandler ; DMA1 Channel 7
DCD ADC1_2_IRQHandler ; ADC1_2
DCD USB_HP_CAN1_TX_IRQHandler ; USB High Priority or CAN1 TX
DCD USB_LP_CAN1_RX0_IRQHandler ; USB Low Priority or CAN1 RX0
DCD CAN1_RX1_IRQHandler ; CAN1 RX1
DCD CAN1_SCE_IRQHandler ; CAN1 SCE
DCD EXTI9_5_IRQHandler ; EXTI Line 9..5
DCD TIM1_BRK_IRQHandler ; TIM1 Break
DCD TIM1_UP_IRQHandler ; TIM1 Update
DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation
DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare
DCD TIM2_IRQHandler ; TIM2
DCD TIM3_IRQHandler ; TIM3
DCD TIM4_IRQHandler ; TIM4
DCD I2C1_EV_IRQHandler ; I2C1 Event
DCD I2C1_ER_IRQHandler ; I2C1 Error
DCD I2C2_EV_IRQHandler ; I2C2 Event
DCD I2C2_ER_IRQHandler ; I2C2 Error
DCD SPI1_IRQHandler ; SPI1
DCD SPI2_IRQHandler ; SPI2
DCD USART1_IRQHandler ; USART1
DCD USART2_IRQHandler ; USART2
DCD USART3_IRQHandler ; USART3
DCD EXTI15_10_IRQHandler ; EXTI Line 15..10
DCD RTCAlarm_IRQHandler ; RTC Alarm through EXTI Line
DCD USBWakeUp_IRQHandler ; USB Wakeup from suspend
__Vectors_End
__Vectors_Size EQU __Vectors_End - __Vectors
AREA |.text|, CODE, READONLY
; Reset handler
Reset_Handler PROC
EXPORT Reset_Handler [WEAK]
IMPORT __main
IMPORT SystemInit
LDR R0, =0x20004FF0
LDR R1, =0xDEADBEEF
LDR R2, [R0, #0]
STR R0, [R0, #0] ; Invalidate
CMP R2, R1
BEQ Reboot_Loader
LDR R0, =SystemInit
BLX R0
LDR R0, =__main
BX R0
ENDP
RCC_APB2ENR EQU 0x40021018
GPIO_AFIO_MASK EQU 0x00000009
GPIOB_CRL EQU 0x40010C00
GPIOB_BRR EQU 0x40010C14
AFIO_MAPR EQU 0x40010004
Reboot_Loader PROC
EXPORT Reboot_Loader
; RCC Enable GPIOB+AFIO
LDR R6, =RCC_APB2ENR
LDR R0, =GPIO_AFIO_MASK
STR R0, [R6]
; MAPR pt1
LDR R0, =AFIO_MAPR
LDR R1, [R0]
BIC R1, R1, #0xF000000
STR R1, [R0]
; MAPR pt2
LSLS R1, R0, #9
STR R1, [R0]
; BRR
LDR R4, =GPIOB_BRR
MOVS R0, #0x18
STR R0, [R4]
; CRL
LDR R1, =GPIOB_CRL
LDR R0, =0x44433444
STR R0, [R1]
; Reboot to ROM
LDR R0, =0x1FFFF000
LDR SP,[R0, #0]
LDR R0,[R0, #4]
BX R0
ENDP
; Dummy Exception Handlers (infinite loops which can be modified)
NMI_Handler PROC
EXPORT NMI_Handler [WEAK]
B .
ENDP
HardFault_Handler\
PROC
EXPORT HardFault_Handler [WEAK]
B .
ENDP
MemManage_Handler\
PROC
EXPORT MemManage_Handler [WEAK]
B .
ENDP
BusFault_Handler\
PROC
EXPORT BusFault_Handler [WEAK]
B .
ENDP
UsageFault_Handler\
PROC
EXPORT UsageFault_Handler [WEAK]
B .
ENDP
SVC_Handler PROC
EXPORT SVC_Handler [WEAK]
B .
ENDP
DebugMon_Handler\
PROC
EXPORT DebugMon_Handler [WEAK]
B .
ENDP
PendSV_Handler PROC
EXPORT PendSV_Handler [WEAK]
B .
ENDP
SysTick_Handler PROC
EXPORT SysTick_Handler [WEAK]
B .
ENDP
Default_Handler PROC
EXPORT WWDG_IRQHandler [WEAK]
EXPORT PVD_IRQHandler [WEAK]
EXPORT TAMPER_IRQHandler [WEAK]
EXPORT RTC_IRQHandler [WEAK]
EXPORT FLASH_IRQHandler [WEAK]
EXPORT RCC_IRQHandler [WEAK]
EXPORT EXTI0_IRQHandler [WEAK]
EXPORT EXTI1_IRQHandler [WEAK]
EXPORT EXTI2_IRQHandler [WEAK]
EXPORT EXTI3_IRQHandler [WEAK]
EXPORT EXTI4_IRQHandler [WEAK]
EXPORT DMA1_Channel1_IRQHandler [WEAK]
EXPORT DMA1_Channel2_IRQHandler [WEAK]
EXPORT DMA1_Channel3_IRQHandler [WEAK]
EXPORT DMA1_Channel4_IRQHandler [WEAK]
EXPORT DMA1_Channel5_IRQHandler [WEAK]
EXPORT DMA1_Channel6_IRQHandler [WEAK]
EXPORT DMA1_Channel7_IRQHandler [WEAK]
EXPORT ADC1_2_IRQHandler [WEAK]
EXPORT USB_HP_CAN1_TX_IRQHandler [WEAK]
EXPORT USB_LP_CAN1_RX0_IRQHandler [WEAK]
EXPORT CAN1_RX1_IRQHandler [WEAK]
EXPORT CAN1_SCE_IRQHandler [WEAK]
EXPORT EXTI9_5_IRQHandler [WEAK]
EXPORT TIM1_BRK_IRQHandler [WEAK]
EXPORT TIM1_UP_IRQHandler [WEAK]
EXPORT TIM1_TRG_COM_IRQHandler [WEAK]
EXPORT TIM1_CC_IRQHandler [WEAK]
EXPORT TIM2_IRQHandler [WEAK]
EXPORT TIM3_IRQHandler [WEAK]
EXPORT TIM4_IRQHandler [WEAK]
EXPORT I2C1_EV_IRQHandler [WEAK]
EXPORT I2C1_ER_IRQHandler [WEAK]
EXPORT I2C2_EV_IRQHandler [WEAK]
EXPORT I2C2_ER_IRQHandler [WEAK]
EXPORT SPI1_IRQHandler [WEAK]
EXPORT SPI2_IRQHandler [WEAK]
EXPORT USART1_IRQHandler [WEAK]
EXPORT USART2_IRQHandler [WEAK]
EXPORT USART3_IRQHandler [WEAK]
EXPORT EXTI15_10_IRQHandler [WEAK]
EXPORT RTCAlarm_IRQHandler [WEAK]
EXPORT USBWakeUp_IRQHandler [WEAK]
WWDG_IRQHandler
PVD_IRQHandler
TAMPER_IRQHandler
RTC_IRQHandler
FLASH_IRQHandler
RCC_IRQHandler
EXTI0_IRQHandler
EXTI1_IRQHandler
EXTI2_IRQHandler
EXTI3_IRQHandler
EXTI4_IRQHandler
DMA1_Channel1_IRQHandler
DMA1_Channel2_IRQHandler
DMA1_Channel3_IRQHandler
DMA1_Channel4_IRQHandler
DMA1_Channel5_IRQHandler
DMA1_Channel6_IRQHandler
DMA1_Channel7_IRQHandler
ADC1_2_IRQHandler
USB_HP_CAN1_TX_IRQHandler
USB_LP_CAN1_RX0_IRQHandler
CAN1_RX1_IRQHandler
CAN1_SCE_IRQHandler
EXTI9_5_IRQHandler
TIM1_BRK_IRQHandler
TIM1_UP_IRQHandler
TIM1_TRG_COM_IRQHandler
TIM1_CC_IRQHandler
TIM2_IRQHandler
TIM3_IRQHandler
TIM4_IRQHandler
I2C1_EV_IRQHandler
I2C1_ER_IRQHandler
I2C2_EV_IRQHandler
I2C2_ER_IRQHandler
SPI1_IRQHandler
SPI2_IRQHandler
USART1_IRQHandler
USART2_IRQHandler
USART3_IRQHandler
EXTI15_10_IRQHandler
RTCAlarm_IRQHandler
USBWakeUp_IRQHandler
B .
ENDP
ALIGN
;*******************************************************************************
; User Stack and Heap initialization
;*******************************************************************************
IF :DEF:__MICROLIB
EXPORT __initial_sp
EXPORT __heap_base
EXPORT __heap_limit
ELSE
IMPORT __use_two_region_memory
EXPORT __user_initial_stackheap
__user_initial_stackheap
LDR R0, = Heap_Mem
LDR R1, =(Stack_Mem + Stack_Size)
LDR R2, = (Heap_Mem + Heap_Size)
LDR R3, = Stack_Mem
BX LR
ALIGN
ENDIF
END
;******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE*****

410
src/baseflight_startups/startup_stm32f10x_md_gcc.s Normal file
View file

@ -0,0 +1,410 @@
/**
******************************************************************************
* @file startup_stm32f10x_md.s
* @author MCD Application Team
* @version V3.5.0
* @date 11-March-2011
* @brief STM32F10x Medium Density Devices vector table for Atollic toolchain.
* This module performs:
* - Set the initial SP
* - Set the initial PC == Reset_Handler,
* - Set the vector table entries with the exceptions ISR address
* - Configure the clock system
* - Branches to main in the C library (which eventually
* calls main()).
* After Reset the Cortex-M3 processor is in Thread mode,
* priority is Privileged, and the Stack is set to Main.
******************************************************************************
* @attention
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* <h2><center>&copy; COPYRIGHT 2011 STMicroelectronics</center></h2>
******************************************************************************
*/
.syntax unified
.cpu cortex-m3
.fpu softvfp
.thumb
.global g_pfnVectors
.global Default_Handler
/* start address for the initialization values of the .data section.
defined in linker script */
.word _sidata
/* start address for the .data section. defined in linker script */
.word _sdata
/* end address for the .data section. defined in linker script */
.word _edata
/* start address for the .bss section. defined in linker script */
.word _sbss
/* end address for the .bss section. defined in linker script */
.word _ebss
.equ BootRAM, 0xF108F85F
/**
* @brief This is the code that gets called when the processor first
* starts execution following a reset event. Only the absolutely
* necessary set is performed, after which the application
* supplied main() routine is called.
* @param None
* @retval : None
*/
.section .text.Reset_Handler
.weak Reset_Handler
.type Reset_Handler, %function
Reset_Handler:
ldr r0, =0x20004FF0 // HJI - TC bootloader entry on reset mod
ldr r1, =0xDEADBEEF // HJI - TC bootloader entry on reset mod
ldr r2, [r0, #0] // HJI - TC bootloader entry on reset mod
str r0, [r0, #0] // HJI - TC bootloader entry on reset mod
cmp r2, r1 // HJI - TC bootloader entry on reset mod
beq Reboot_Loader // HJI - TC bootloader entry on reset mod
/* Copy the data segment initializers from flash to SRAM */
movs r1, #0
b LoopCopyDataInit
CopyDataInit:
ldr r3, =_sidata
ldr r3, [r3, r1]
str r3, [r0, r1]
adds r1, r1, #4
LoopCopyDataInit:
ldr r0, =_sdata
ldr r3, =_edata
adds r2, r0, r1
cmp r2, r3
bcc CopyDataInit
ldr r2, =_sbss
b LoopFillZerobss
/* Zero fill the bss segment. */
FillZerobss:
movs r3, #0
str r3, [r2], #4
LoopFillZerobss:
ldr r3, = _ebss
cmp r2, r3
bcc FillZerobss
/* Call the clock system intitialization function.*/
bl SystemInit
/* Call static constructors */
bl __libc_init_array
/* Call the application's entry point.*/
bl main
/* Atollic update, branch LoopForever */
LoopForever:
b LoopForever
.equ RCC_APB2ENR, 0x40021018 // HJI - TC bootloader entry on reset mod
.equ GPIO_AFIO_MASK, 0x00000009 // HJI - TC bootloader entry on reset mod
.equ GPIOB_CRL, 0x40010C00 // HJI - TC bootloader entry on reset mod
.equ GPIOB_BRR, 0x40010C14 // HJI - TC bootloader entry on reset mod
.equ AFIO_MAPR, 0x40010004 // HJI - TC bootloader entry on reset mod
Reboot_Loader: // HJI - TC bootloader entry on reset mod
// RCC Enable GPIOB+AFIO // HJI - TC bootloader entry on reset mod
ldr r6, =RCC_APB2ENR // HJI - TC bootloader entry on reset mod
ldr r0, =GPIO_AFIO_MASK // HJI - TC bootloader entry on reset mod
str R0, [r6]; // HJI - TC bootloader entry on reset mod
// MAPR pt1 // HJI - TC bootloader entry on reset mod
ldr r0, =AFIO_MAPR // HJI - TC bootloader entry on reset mod
ldr r1, [r0] // HJI - TC bootloader entry on reset mod
bic r1, r1, #0x0F000000 // HJI - TC bootloader entry on reset mod
str r1, [r0] // HJI - TC bootloader entry on reset mod
// MAPR pt2 // HJI - TC bootloader entry on reset mod
lsls r1, r0, #9 // HJI - TC bootloader entry on reset mod
str r1, [r0] // HJI - TC bootloader entry on reset mod
// BRR // HJI - TC bootloader entry on reset mod
ldr r4, =GPIOB_BRR // HJI - TC bootloader entry on reset mod
movs r0, #0x18 // HJI - TC bootloader entry on reset mod
str r0, [r4] // HJI - TC bootloader entry on reset mod
// CRL // HJI - TC bootloader entry on reset mod
ldr r1, =GPIOB_CRL // HJI - TC bootloader entry on reset mod
ldr r0, =0x44433444 // HJI - TC bootloader entry on reset mod
str r0, [r1] // HJI - TC bootloader entry on reset mod
// Reboot to ROM // HJI - TC bootloader entry on reset mod
ldr r0, =0x1FFFF000 // HJI - TC bootloader entry on reset mod
ldr sp,[r0, #0] // HJI - TC bootloader entry on reset mod
ldr r0,[r0, #4] // HJI - TC bootloader entry on reset mod
bx r0 // HJI - TC bootloader entry on reset mod
.size Reset_Handler, .-Reset_Handler
/**
* @brief This is the code that gets called when the processor receives an
* unexpected interrupt. This simply enters an infinite loop, preserving
* the system state for examination by a debugger.
*
* @param None
* @retval : None
*/
.section .text.Default_Handler,"ax",%progbits
Default_Handler:
Infinite_Loop:
b Infinite_Loop
.size Default_Handler, .-Default_Handler
/******************************************************************************
*
* The minimal vector table for a Cortex M3. Note that the proper constructs
* must be placed on this to ensure that it ends up at physical address
* 0x0000.0000.
*
******************************************************************************/
.section .isr_vector,"a",%progbits
.type g_pfnVectors, %object
.size g_pfnVectors, .-g_pfnVectors
g_pfnVectors:
.word _estack
.word Reset_Handler
.word NMI_Handler
.word HardFault_Handler
.word MemManage_Handler
.word BusFault_Handler
.word UsageFault_Handler
.word 0
.word 0
.word 0
.word 0
.word SVC_Handler
.word DebugMon_Handler
.word 0
.word PendSV_Handler
.word SysTick_Handler
.word WWDG_IRQHandler
.word PVD_IRQHandler
.word TAMPER_IRQHandler
.word RTC_IRQHandler
.word FLASH_IRQHandler
.word RCC_IRQHandler
.word EXTI0_IRQHandler
.word EXTI1_IRQHandler
.word EXTI2_IRQHandler
.word EXTI3_IRQHandler
.word EXTI4_IRQHandler
.word DMA1_Channel1_IRQHandler
.word DMA1_Channel2_IRQHandler
.word DMA1_Channel3_IRQHandler
.word DMA1_Channel4_IRQHandler
.word DMA1_Channel5_IRQHandler
.word DMA1_Channel6_IRQHandler
.word DMA1_Channel7_IRQHandler
.word ADC1_2_IRQHandler
.word USB_HP_CAN1_TX_IRQHandler
.word USB_LP_CAN1_RX0_IRQHandler
.word CAN1_RX1_IRQHandler
.word CAN1_SCE_IRQHandler
.word EXTI9_5_IRQHandler
.word TIM1_BRK_IRQHandler
.word TIM1_UP_IRQHandler
.word TIM1_TRG_COM_IRQHandler
.word TIM1_CC_IRQHandler
.word TIM2_IRQHandler
.word TIM3_IRQHandler
.word TIM4_IRQHandler
.word I2C1_EV_IRQHandler
.word I2C1_ER_IRQHandler
.word I2C2_EV_IRQHandler
.word I2C2_ER_IRQHandler
.word SPI1_IRQHandler
.word SPI2_IRQHandler
.word USART1_IRQHandler
.word USART2_IRQHandler
.word USART3_IRQHandler
.word EXTI15_10_IRQHandler
.word RTCAlarm_IRQHandler
.word USBWakeUp_IRQHandler
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word BootRAM /* @0x108. This is for boot in RAM mode for
STM32F10x Medium Density devices. */
/*******************************************************************************
*
* Provide weak aliases for each Exception handler to the Default_Handler.
* As they are weak aliases, any function with the same name will override
* this definition.
*
*******************************************************************************/
.weak NMI_Handler
.thumb_set NMI_Handler,Default_Handler
.weak HardFault_Handler
.thumb_set HardFault_Handler,Default_Handler
.weak MemManage_Handler
.thumb_set MemManage_Handler,Default_Handler
.weak BusFault_Handler
.thumb_set BusFault_Handler,Default_Handler
.weak UsageFault_Handler
.thumb_set UsageFault_Handler,Default_Handler
.weak SVC_Handler
.thumb_set SVC_Handler,Default_Handler
.weak DebugMon_Handler
.thumb_set DebugMon_Handler,Default_Handler
.weak PendSV_Handler
.thumb_set PendSV_Handler,Default_Handler
.weak SysTick_Handler
.thumb_set SysTick_Handler,Default_Handler
.weak WWDG_IRQHandler
.thumb_set WWDG_IRQHandler,Default_Handler
.weak PVD_IRQHandler
.thumb_set PVD_IRQHandler,Default_Handler
.weak TAMPER_IRQHandler
.thumb_set TAMPER_IRQHandler,Default_Handler
.weak RTC_IRQHandler
.thumb_set RTC_IRQHandler,Default_Handler
.weak FLASH_IRQHandler
.thumb_set FLASH_IRQHandler,Default_Handler
.weak RCC_IRQHandler
.thumb_set RCC_IRQHandler,Default_Handler
.weak EXTI0_IRQHandler
.thumb_set EXTI0_IRQHandler,Default_Handler
.weak EXTI1_IRQHandler
.thumb_set EXTI1_IRQHandler,Default_Handler
.weak EXTI2_IRQHandler
.thumb_set EXTI2_IRQHandler,Default_Handler
.weak EXTI3_IRQHandler
.thumb_set EXTI3_IRQHandler,Default_Handler
.weak EXTI4_IRQHandler
.thumb_set EXTI4_IRQHandler,Default_Handler
.weak DMA1_Channel1_IRQHandler
.thumb_set DMA1_Channel1_IRQHandler,Default_Handler
.weak DMA1_Channel2_IRQHandler
.thumb_set DMA1_Channel2_IRQHandler,Default_Handler
.weak DMA1_Channel3_IRQHandler
.thumb_set DMA1_Channel3_IRQHandler,Default_Handler
.weak DMA1_Channel4_IRQHandler
.thumb_set DMA1_Channel4_IRQHandler,Default_Handler
.weak DMA1_Channel5_IRQHandler
.thumb_set DMA1_Channel5_IRQHandler,Default_Handler
.weak DMA1_Channel6_IRQHandler
.thumb_set DMA1_Channel6_IRQHandler,Default_Handler
.weak DMA1_Channel7_IRQHandler
.thumb_set DMA1_Channel7_IRQHandler,Default_Handler
.weak ADC1_2_IRQHandler
.thumb_set ADC1_2_IRQHandler,Default_Handler
.weak USB_HP_CAN1_TX_IRQHandler
.thumb_set USB_HP_CAN1_TX_IRQHandler,Default_Handler
.weak USB_LP_CAN1_RX0_IRQHandler
.thumb_set USB_LP_CAN1_RX0_IRQHandler,Default_Handler
.weak CAN1_RX1_IRQHandler
.thumb_set CAN1_RX1_IRQHandler,Default_Handler
.weak CAN1_SCE_IRQHandler
.thumb_set CAN1_SCE_IRQHandler,Default_Handler
.weak EXTI9_5_IRQHandler
.thumb_set EXTI9_5_IRQHandler,Default_Handler
.weak TIM1_BRK_IRQHandler
.thumb_set TIM1_BRK_IRQHandler,Default_Handler
.weak TIM1_UP_IRQHandler
.thumb_set TIM1_UP_IRQHandler,Default_Handler
.weak TIM1_TRG_COM_IRQHandler
.thumb_set TIM1_TRG_COM_IRQHandler,Default_Handler
.weak TIM1_CC_IRQHandler
.thumb_set TIM1_CC_IRQHandler,Default_Handler
.weak TIM2_IRQHandler
.thumb_set TIM2_IRQHandler,Default_Handler
.weak TIM3_IRQHandler
.thumb_set TIM3_IRQHandler,Default_Handler
.weak TIM4_IRQHandler
.thumb_set TIM4_IRQHandler,Default_Handler
.weak I2C1_EV_IRQHandler
.thumb_set I2C1_EV_IRQHandler,Default_Handler
.weak I2C1_ER_IRQHandler
.thumb_set I2C1_ER_IRQHandler,Default_Handler
.weak I2C2_EV_IRQHandler
.thumb_set I2C2_EV_IRQHandler,Default_Handler
.weak I2C2_ER_IRQHandler
.thumb_set I2C2_ER_IRQHandler,Default_Handler
.weak SPI1_IRQHandler
.thumb_set SPI1_IRQHandler,Default_Handler
.weak SPI2_IRQHandler
.thumb_set SPI2_IRQHandler,Default_Handler
.weak USART1_IRQHandler
.thumb_set USART1_IRQHandler,Default_Handler
.weak USART2_IRQHandler
.thumb_set USART2_IRQHandler,Default_Handler
.weak USART3_IRQHandler
.thumb_set USART3_IRQHandler,Default_Handler
.weak EXTI15_10_IRQHandler
.thumb_set EXTI15_10_IRQHandler,Default_Handler
.weak RTCAlarm_IRQHandler
.thumb_set RTCAlarm_IRQHandler,Default_Handler
.weak USBWakeUp_IRQHandler
.thumb_set USBWakeUp_IRQHandler,Default_Handler
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/

69
src/board.h Executable file
View file

@ -0,0 +1,69 @@
#pragma once
#include <stdbool.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <string.h>
#include "stm32f10x_conf.h"
#include "core_cm3.h"
#include "drv_system.h" // timers, delays, etc
#include "drv_adc.h"
#include "drv_adxl345.h"
#include "drv_bmp085.h"
#include "drv_hmc5883l.h"
#include "drv_i2c.h"
#include "drv_mpu3050.h"
#include "drv_pwm.h"
#include "drv_uart.h"
typedef enum {
SENSOR_ACC = 1 << 0,
SENSOR_BARO = 1 << 1,
SENSOR_MAG = 1 << 2,
SENSOR_SONAR = 1 << 3,
SENSOR_GPS = 1 << 4,
} AvailableSensors;
typedef enum {
FEATURE_PPM = 1 << 0,
FEATURE_VBAT = 1 << 1,
FEATURE_SERVO = 1 << 2,
FEATURE_DIGITAL_SERVO = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4,
FEATURE_SERVO_TILT = 1 << 5,
FEATURE_CAMTRIG = 1 << 6,
FEATURE_GYRO_SMOOTHING = 1 << 7,
} AvailableFeatures;
#define digitalHi(p, i) { p->BSRR = i; }
#define digitalLo(p, i) { p->BRR = i; }
#define digitalToggle(p, i) { p->ODR ^= i; }
// Hardware GPIO
#define LED0_GPIO GPIOB
#define LED0_PIN GPIO_Pin_3
#define LED1_GPIO GPIOB
#define LED1_PIN GPIO_Pin_4
#define BEEP_GPIO GPIOA
#define BEEP_PIN GPIO_Pin_12
#define BARO_GPIO GPIOC
#define BARO_PIN GPIO_Pin_13
// Helpful macros
#define LED0_TOGGLE digitalToggle(LED0_GPIO, LED0_PIN);
#define LED0_OFF digitalHi(LED0_GPIO, LED0_PIN);
#define LED0_ON digitalLo(LED0_GPIO, LED0_PIN);
#define LED1_TOGGLE digitalToggle(LED1_GPIO, LED1_PIN);
#define LED1_OFF digitalHi(LED1_GPIO, LED1_PIN);
#define LED1_ON digitalLo(LED1_GPIO, LED1_PIN);
#define BEEP_TOGGLE digitalToggle(BEEP_GPIO, BEEP_PIN);
#define BEEP_OFF digitalHi(BEEP_GPIO, BEEP_PIN);
#define BEEP_ON digitalLo(BEEP_GPIO, BEEP_PIN);
#define BARO_OFF digitalLo(BARO_GPIO, BARO_PIN);
#define BARO_ON digitalHi(BARO_GPIO, BARO_PIN);

133
src/cli.c Normal file
View file

@ -0,0 +1,133 @@
#include "board.h"
#include "mw.h"
// we unset this on 'exit'
extern uint8_t cliMode;
static void cliExit(char *cmdline);
static void cliHelp(char *cmdline);
static void cliRMode(char *cmdline);
static void cliVersion(char *cmdline);
// buffer
char cliBuffer[32];
uint8_t bufferIndex = 0;
typedef struct {
char *name;
char *param;
void (*func)(char *cmdline);
} cliCmd;
// should be sorted a..z for bsearch()
const cliCmd cmdTable[] = {
{ "exit", "", cliExit },
{ "help", "", cliHelp },
{ "rmode", "pwm / ppm", cliRMode },
{ "version", "", cliVersion },
};
#define CMD_COUNT (sizeof cmdTable / sizeof cmdTable[0])
static void cliPrompt(void)
{
uartPrint("\r\n# ");
memset(cliBuffer, 0, sizeof(cliBuffer));
bufferIndex = 0;
}
static int cliCompare(const void *a, const void *b)
{
const cliCmd *ca = a, *cb = b;
return strncasecmp(ca->name, cb->name, strlen(cb->name));
}
static void cliExit(char *cmdline)
{
uartPrint("Leaving CLI mode...\r\n");
memset(cliBuffer, 0, sizeof(cliBuffer));
bufferIndex = 0;
cliMode = 0;
}
static void cliHelp(char *cmdline)
{
uint8_t i = 0;
uartPrint("Available commands:\r\n");
for (i = 0; i < CMD_COUNT; i++) {
uartPrint(cmdTable[i].name);
uartWrite(' ');
uartPrint(cmdTable[i].param);
uartPrint("\r\n");
}
}
static void cliRMode(char *cmdline)
{
if (strncasecmp(cmdline, "pwm", 3) == 0) {
uartPrint("PWM Mode");
featureClear(FEATURE_PPM);
} else {
uartPrint("PPM Mode");
featureSet(FEATURE_PPM);
}
writeParams();
systemReset(false);
}
static void cliVersion(char *cmdline)
{
uartPrint("Afro32 CLI version 2.0-pre1");
}
void cliProcess(void)
{
while (uartAvailable()) {
uint8_t c = uartRead();
cliBuffer[bufferIndex++] = c;
if (bufferIndex == sizeof(cliBuffer)) {
bufferIndex--;
c = '\n';
}
if (bufferIndex && (c == '\n' || c == '\r')) {
// enter pressed
cliCmd *cmd = NULL;
cliCmd target;
uartPrint("\r\n");
cliBuffer[bufferIndex] = 0; // null terminate
target.name = cliBuffer;
target.param = NULL;
cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare);
if (cmd)
cmd->func(cliBuffer + strlen(cmd->name) + 1);
else
uartPrint("ERR: Unknown command, try 'HELP'");
// 'exit' will reset this flag, so we don't need to print prompt again
if (cliMode)
cliPrompt();
} else if (c == 127) {
// backspace
if (bufferIndex > 1) {
cliBuffer[bufferIndex - 2] = 0;
uartPrint("\r# ");
uartPrint(cliBuffer);
uartWrite(' ');
uartPrint("\r# ");
uartPrint(cliBuffer);
bufferIndex -= 2;
}
} else if (c < 32 || c > 126) {
// non-printable ascii
bufferIndex--;
} else {
uartWrite(c);
}
}
}

158
src/config.c Executable file
View file

@ -0,0 +1,158 @@
#include "board.h"
#include "mw.h"
#include <string.h>
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * 63) // use the last KB for storage
config_t cfg;
static uint32_t enabledSensors = 0;
static uint8_t checkNewConf = 2;
void readEEPROM(void)
{
uint8_t i;
// Read flash
memcpy(&cfg, (char *)FLASH_WRITE_ADDR, sizeof(config_t));
#if defined(POWERMETER)
pAlarm = (uint32_t) cfg.powerTrigger1 *(uint32_t) PLEVELSCALE *(uint32_t) PLEVELDIV; // need to cast before multiplying
#endif
for (i = 0; i < 7; i++)
lookupRX[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 1250;
cfg.wing_left_mid = constrain(cfg.wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
cfg.wing_right_mid = constrain(cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
}
void writeParams(void)
{
FLASH_Status status;
uint32_t i;
FLASH_Unlock();
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
if (FLASH_ErasePage(FLASH_WRITE_ADDR) == FLASH_COMPLETE) {
for (i = 0; i < sizeof(config_t); i += 4) {
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *)((char *)&cfg + i));
if (status != FLASH_COMPLETE)
break; // TODO: fail
}
}
FLASH_Lock();
readEEPROM();
blinkLED(15, 20, 1);
}
void checkFirstTime(void)
{
uint8_t test_val, i;
test_val = *(uint8_t *)FLASH_WRITE_ADDR;
if (test_val == checkNewConf)
return;
// Default settings
cfg.version = checkNewConf;
cfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll();
featureSet(FEATURE_VBAT | FEATURE_PPM);
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
cfg.D8[ROLL] = 23;
cfg.P8[PITCH] = 40;
cfg.I8[PITCH] = 30;
cfg.D8[PITCH] = 23;
cfg.P8[YAW] = 85;
cfg.I8[YAW] = 0;
cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 16;
cfg.I8[PIDALT] = 15;
cfg.D8[PIDALT] = 7;
cfg.P8[PIDGPS] = 50;
cfg.I8[PIDGPS] = 0;
cfg.D8[PIDGPS] = 15;
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
cfg.P8[PIDLEVEL] = 90;
cfg.I8[PIDLEVEL] = 45;
cfg.D8[PIDLEVEL] = 100;
cfg.P8[PIDMAG] = 40;
cfg.rcRate8 = 45; // = 0.9 in GUI
cfg.rcExpo8 = 65;
cfg.rollPitchRate = 0;
cfg.yawRate = 0;
cfg.dynThrPID = 0;
for (i = 0; i < CHECKBOXITEMS; i++) {
cfg.activate1[i] = 0;
cfg.activate2[i] = 0;
}
cfg.accTrim[0] = 0;
cfg.accTrim[1] = 0;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.powerTrigger1 = 0;
// Radio/ESC
cfg.midrc = 1500;
cfg.minthrottle = 1150;
cfg.maxthrottle = 1850;
cfg.mincommand = 1000;
// servos
cfg.yaw_direction = 1;
cfg.wing_left_mid = 1500;
cfg.wing_right_mid = 1500;
cfg.tri_yaw_middle = 1500;
// gimbal
cfg.tilt_pitch_prop = 10;
cfg.tilt_roll_prop = 10;
writeParams();
}
bool sensors(uint32_t mask)
{
return enabledSensors & mask;
}
void sensorsSet(uint32_t mask)
{
enabledSensors |= mask;
}
void sensorsClear(uint32_t mask)
{
enabledSensors &= ~(mask);
}
bool feature(uint32_t mask)
{
return cfg.enabledFeatures & mask;
}
void featureSet(uint32_t mask)
{
cfg.enabledFeatures |= mask;
}
void featureClear(uint32_t mask)
{
cfg.enabledFeatures &= ~(mask);
}
void featureClearAll()
{
cfg.enabledFeatures = 0;
}

55
src/drv_adc.c Executable file
View file

@ -0,0 +1,55 @@
#include "board.h"
static volatile uint16_t adc1Ch4Value = 0;
void adcInit(void)
{
ADC_InitTypeDef ADC_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
// ADC assumes all the GPIO was already placed in 'AIN' mode
DMA_DeInit(DMA1_Channel1);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&adc1Ch4Value;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
DMA_InitStructure.DMA_BufferSize = 1;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Disable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA1_Channel1, &DMA_InitStructure);
/* Enable DMA1 channel1 */
DMA_Cmd(DMA1_Channel1, ENABLE);
ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
ADC_InitStructure.ADC_ScanConvMode = DISABLE;
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
ADC_InitStructure.ADC_NbrOfChannel = 1;
ADC_Init(ADC1, &ADC_InitStructure);
ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 1, ADC_SampleTime_28Cycles5);
ADC_DMACmd(ADC1, ENABLE);
ADC_Cmd(ADC1, ENABLE);
// Calibrate ADC
ADC_ResetCalibration(ADC1);
while(ADC_GetResetCalibrationStatus(ADC1));
ADC_StartCalibration(ADC1);
while(ADC_GetCalibrationStatus(ADC1));
// Fire off ADC
ADC_SoftwareStartConvCmd(ADC1, ENABLE);
}
uint16_t adcGetBattery(void)
{
return adc1Ch4Value;
}

4
src/drv_adc.h Executable file
View file

@ -0,0 +1,4 @@
#pragma once
void adcInit(void);
uint16_t adcGetBattery(void);

69
src/drv_adxl345.c Executable file
View file

@ -0,0 +1,69 @@
#include "board.h"
// ADXL345, Alternative address mode 0x53
#define ADXL345_ADDRESS 0x53
#define ADXL345_BW_RATE 0x2C
#define ADXL345_POWER_CTL 0x2D
#define ADXL345_INT_ENABLE 0x2E
#define ADXL345_DATA_FORMAT 0x31
#define ADXL345_DATA_OUT 0x32
#define ADXL345_FIFO_CTL 0x38
#define ADXL345_BW_RATE_200 0x0B
#define ADXL345_POWER_MEAS 0x08
#define ADXL345_FULL_RANGE 0x08
#define ADXL345_RANGE_16G 0x03
bool adxl345Detect(void)
{
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(ADXL345_ADDRESS, 0x00, 1, &sig);
if (!ack || sig != 0xE5)
return false;
return true;
}
#define ADXL_RATE_100 0x0A
#define ADXL_RATE_200 0x0B
#define ADXL_RATE_400 0x0C
#define ADXL_RATE_800 0x0D
#define ADXL_RATE_1600 0x0E
#define ADXL_RATE_3200 0x0F
#define ADXL_FULL_RES 0x08
#define ADXL_RANGE_2G 0x00
#define ADXL_RANGE_4G 0x01
#define ADXL_RANGE_8G 0x02
#define ADXL_RANGE_16G 0x03
void adxl345Init(void)
{
#ifdef FREEFLIGHT
i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_BW_RATE_200);
i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS);
i2cWrite(ADXL345_ADDRESS, ADXL345_INT_ENABLE, 0);
i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_16G);
i2cWrite(ADXL345_ADDRESS, ADXL345_FIFO_CTL, 0);
#else
// MWC defaults
i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, 1 << 3); // register: Power CTRL -- value: Set measure bit 3 on
// i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, 0x0B); // register: DATA_FORMAT -- value: Set bits 3(full range) and 1 0 on (+/- 16g-range)
// i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, 0x09); // register: BW_RATE -- value: rate=50hz, bw=20hz
i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, (ADXL_RANGE_8G & 0x03) | ADXL_FULL_RES); // register: DATA_FORMAT -- value: Set bits 3(full range) and 1 0 on (+/- 16g-range)
i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL_RATE_800); // register: BW_RATE -- value: rate=50hz, bw=20hz
#endif
}
void adxl345Read(int16_t *accelData)
{
uint8_t buf[6];
i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
accelData[0] = buf[1] << 8 | buf[0];
accelData[1] = buf[3] << 8 | buf[2];
accelData[2] = buf[5] << 8 | buf[4];
}

5
src/drv_adxl345.h Executable file
View file

@ -0,0 +1,5 @@
#pragma once
bool adxl345Detect(void);
void adxl345Init(void);
void adxl345Read(int16_t *accelData);

295
src/drv_bmp085.c Executable file
View file

@ -0,0 +1,295 @@
#include "board.h"
// BMP085, Standard address 0x77
static bool convDone = false;
// EXTI14 for BMP085 End of Conversion Interrupt
void EXTI15_10_IRQHandler(void)
{
if (EXTI_GetITStatus(EXTI_Line14) == SET) {
EXTI_ClearITPendingBit(EXTI_Line14);
convDone = true;
}
}
typedef struct {
int16_t ac1;
int16_t ac2;
int16_t ac3;
uint16_t ac4;
uint16_t ac5;
uint16_t ac6;
int16_t b1;
int16_t b2;
int16_t mb;
int16_t mc;
int16_t md;
} bmp085_smd500_calibration_param_t;
typedef struct {
bmp085_smd500_calibration_param_t cal_param;
uint8_t mode;
uint8_t chip_id, ml_version, al_version;
uint8_t dev_addr;
uint8_t sensortype;
int32_t param_b5;
int32_t number_of_samples;
int16_t oversampling_setting;
int16_t smd500_t_resolution, smd500_masterclock;
// BMP085_BUS_WR_RETURN_TYPE (*bus_write)( BMP085_BUS_WR_PARAM_TYPES );
// BMP085_BUS_RD_RETURN_TYPE (*bus_read)( BMP085_BUS_RD_PARAM_TYPES );
// BMP085_MDELAY_RETURN_TYPE (*delay_msec)( BMP085_MDELAY_DATA_TYPE );
} bmp085_t;
#define BMP085_I2C_ADDR 0x77
#define BMP085_CHIP_ID 0x55
#define BOSCH_PRESSURE_BMP085 85
#define BMP085_CHIP_ID_REG 0xD0
#define BMP085_VERSION_REG 0xD1
#define E_SENSOR_NOT_DETECTED (char) 0
#define BMP085_PROM_START__ADDR 0xaa
#define BMP085_PROM_DATA__LEN 22
#define BMP085_T_MEASURE 0x2E // temperature measurent
#define BMP085_P_MEASURE 0x34 // pressure measurement
#define BMP085_CTRL_MEAS_REG 0xF4
#define BMP085_ADC_OUT_MSB_REG 0xF6
#define BMP085_ADC_OUT_LSB_REG 0xF7
#define BMP085_CHIP_ID__POS 0
#define BMP085_CHIP_ID__MSK 0xFF
#define BMP085_CHIP_ID__LEN 8
#define BMP085_CHIP_ID__REG BMP085_CHIP_ID_REG
#define BMP085_ML_VERSION__POS 0
#define BMP085_ML_VERSION__LEN 4
#define BMP085_ML_VERSION__MSK 0x0F
#define BMP085_ML_VERSION__REG BMP085_VERSION_REG
#define BMP085_AL_VERSION__POS 4
#define BMP085_AL_VERSION__LEN 4
#define BMP085_AL_VERSION__MSK 0xF0
#define BMP085_AL_VERSION__REG BMP085_VERSION_REG
#define BMP085_GET_BITSLICE(regvar, bitname) (regvar & bitname##__MSK) >> bitname##__POS
#define BMP085_SET_BITSLICE(regvar, bitname, val) (regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK)
#define SMD500_PARAM_MG 3038 //calibration parameter
#define SMD500_PARAM_MH -7357 //calibration parameter
#define SMD500_PARAM_MI 3791 //calibration parameter
static bmp085_t bmp085 = { { 0, } };
static bmp085_t *p_bmp085 = &bmp085; /**< pointer to SMD500 / BMP085 device area */
static bool bmp085InitDone = false;
static void bmp085_get_cal_param(void);
bool bmp085Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
uint8_t data;
if (bmp085InitDone)
return true;
// PC13, PC14 (Barometer XCLR reset output, EOC input)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOC, &GPIO_InitStructure);
BARO_ON;
// EXTI interrupt for barometer EOC
GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
EXTI_InitStructure.EXTI_Line = EXTI_Line14;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
// Enable and set EXTI10-15 Interrupt to the lowest priority
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
delay(12); // datasheet says 10ms, we'll be careful and do 12.
p_bmp085->sensortype = E_SENSOR_NOT_DETECTED;
p_bmp085->dev_addr = BMP085_I2C_ADDR; /* preset BMP085 I2C_addr */
i2cRead(p_bmp085->dev_addr, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
p_bmp085->chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
p_bmp085->number_of_samples = 1;
p_bmp085->oversampling_setting = 2;
if (p_bmp085->chip_id == BMP085_CHIP_ID) { /* get bitslice */
p_bmp085->sensortype = BOSCH_PRESSURE_BMP085;
i2cRead(p_bmp085->dev_addr, BMP085_VERSION_REG, 1, &data); /* read Version reg */
p_bmp085->ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */
p_bmp085->al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
bmp085InitDone = true;
return true;
}
return false;
}
int16_t bmp085_read_temperature(void)
{
convDone = false;
bmp085_start_ut();
while (!convDone);
return bmp085_get_temperature(bmp085_get_ut());
}
int32_t bmp085_read_pressure(void)
{
convDone = false;
bmp085_start_up();
while (!convDone);
return bmp085_get_pressure(bmp085_get_up());
}
int16_t bmp085_get_temperature(uint32_t ut)
{
int16_t temperature;
int32_t x1, x2;
if (p_bmp085->sensortype == BOSCH_PRESSURE_BMP085) {
x1 = (((int32_t) ut - (int32_t) p_bmp085->cal_param.ac6) * (int32_t) p_bmp085->cal_param.ac5) >> 15;
x2 = ((int32_t) p_bmp085->cal_param.mc << 11) / (x1 + p_bmp085->cal_param.md);
p_bmp085->param_b5 = x1 + x2;
}
temperature = ((p_bmp085->param_b5 + 8) >> 4); // temperature in 0.1°C
return temperature;
}
int32_t bmp085_get_pressure(uint32_t up)
{
int32_t pressure, x1, x2, x3, b3, b6;
uint32_t b4, b7;
b6 = p_bmp085->param_b5 - 4000;
// *****calculate B3************
x1 = (b6 * b6) >> 12;
x1 *= p_bmp085->cal_param.b2;
x1 >>= 11;
x2 = (p_bmp085->cal_param.ac2 * b6);
x2 >>= 11;
x3 = x1 + x2;
b3 = (((((int32_t)p_bmp085->cal_param.ac1) * 4 + x3) << p_bmp085->oversampling_setting) + 2) >> 2;
// *****calculate B4************
x1 = (p_bmp085->cal_param.ac3 * b6) >> 13;
x2 = (p_bmp085->cal_param.b1 * ((b6 * b6) >> 12) ) >> 16;
x3 = ((x1 + x2) + 2) >> 2;
b4 = (p_bmp085->cal_param.ac4 * (uint32_t) (x3 + 32768)) >> 15;
b7 = ((uint32_t)(up - b3) * (50000 >> p_bmp085->oversampling_setting));
if (b7 < 0x80000000) {
pressure = (b7 << 1) / b4;
} else {
pressure = (b7 / b4) << 1;
}
x1 = pressure >> 8;
x1 *= x1;
x1 = (x1 * SMD500_PARAM_MG) >> 16;
x2 = (pressure * SMD500_PARAM_MH) >> 16;
pressure += (x1 + x2 + SMD500_PARAM_MI) >> 4; // pressure in Pa
return pressure;
}
void bmp085_start_ut(void)
{
convDone = false;
i2cWrite(p_bmp085->dev_addr, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
}
uint16_t bmp085_get_ut(void)
{
uint16_t ut;
uint8_t data[2];
uint16_t timeout = 10000;
// wait in case of cockup
while (!convDone && timeout-- > 0) {
__NOP();
}
i2cRead(p_bmp085->dev_addr, BMP085_ADC_OUT_MSB_REG, 2, data);
ut = (data[0] << 8) | data[1];
return ut;
}
void bmp085_start_up(void)
{
uint8_t ctrl_reg_data;
ctrl_reg_data = BMP085_P_MEASURE + (p_bmp085->oversampling_setting << 6);
convDone = false;
i2cWrite(p_bmp085->dev_addr, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
}
/** read out up for pressure conversion
depending on the oversampling ratio setting up can be 16 to 19 bit
\return up parameter that represents the uncompensated pressure value
*/
uint32_t bmp085_get_up(void)
{
uint32_t up = 0;
uint8_t data[3];
uint16_t timeout = 10000;
// wait in case of cockup
while (!convDone && timeout-- > 0) {
__NOP();
}
i2cRead(p_bmp085->dev_addr, BMP085_ADC_OUT_MSB_REG, 3, data);
up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - p_bmp085->oversampling_setting);
p_bmp085->number_of_samples = 1;
return up;
}
static void bmp085_get_cal_param(void)
{
uint8_t data[22];
i2cRead(p_bmp085->dev_addr, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data);
/*parameters AC1-AC6*/
p_bmp085->cal_param.ac1 = (data[0] <<8) | data[1];
p_bmp085->cal_param.ac2 = (data[2] <<8) | data[3];
p_bmp085->cal_param.ac3 = (data[4] <<8) | data[5];
p_bmp085->cal_param.ac4 = (data[6] <<8) | data[7];
p_bmp085->cal_param.ac5 = (data[8] <<8) | data[9];
p_bmp085->cal_param.ac6 = (data[10] <<8) | data[11];
/*parameters B1,B2*/
p_bmp085->cal_param.b1 = (data[12] <<8) | data[13];
p_bmp085->cal_param.b2 = (data[14] <<8) | data[15];
/*parameters MB,MC,MD*/
p_bmp085->cal_param.mb = (data[16] <<8) | data[17];
p_bmp085->cal_param.mc = (data[18] <<8) | data[19];
p_bmp085->cal_param.md = (data[20] <<8) | data[21];
}

15
src/drv_bmp085.h Executable file
View file

@ -0,0 +1,15 @@
#pragma once
bool bmp085Init(void);
// polled versions
int16_t bmp085_read_temperature(void);
int32_t bmp085_read_pressure(void);
// interrupt versions
void bmp085_start_ut(void);
uint16_t bmp085_get_ut(void);
void bmp085_start_up(void);
uint32_t bmp085_get_up(void);
int16_t bmp085_get_temperature(uint32_t ut);
int32_t bmp085_get_pressure(uint32_t up);

50
src/drv_hmc5883l.c Executable file
View file

@ -0,0 +1,50 @@
#include "board.h"
// HMC5883L, default address 0x1E
#define MAG_ADDRESS 0x1E
#define MAG_DATA_REGISTER 0x03
bool hmc5883lDetect(void)
{
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(MAG_ADDRESS, 0x0A, 1, &sig);
if (!ack || sig != 'H')
return false;
return true;
}
void hmc5883lInit(void)
{
delay(100);
// force positiveBias
i2cWrite(MAG_ADDRESS, 0x00, 0x71); //Configuration Register A -- 0 11 100 01 num samples: 8 ; output rate: 15Hz ; positive bias
delay(50);
// set gains for calibration
i2cWrite(MAG_ADDRESS, 0x01, 0x60); //Configuration Register B -- 011 00000 configuration gain 2.5Ga
i2cWrite(MAG_ADDRESS, 0x02, 0x01); //Mode register -- 000000 01 single Conversion Mode
// this enters test mode
}
void hmc5883lFinishCal(void)
{
// leave test mode
i2cWrite(MAG_ADDRESS, 0x00, 0x70); //Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
i2cWrite(MAG_ADDRESS, 0x01, 0x20); //Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_ADDRESS, 0x02, 0x00); //Mode register -- 000000 00 continuous Conversion Mode
}
void hmc5883lRead(int16_t *magData)
{
uint8_t buf[6];
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
magData[0] = buf[0] << 8 | buf[1];
magData[1] = buf[2] << 8 | buf[3];
magData[2] = buf[4] << 8 | buf[5];
}

6
src/drv_hmc5883l.h Executable file
View file

@ -0,0 +1,6 @@
#pragma once
bool hmc5883lDetect(void);
void hmc5883lInit(void);
void hmc5883lFinishCal(void);
void hmc5883lRead(int16_t *magData);

336
src/drv_i2c.c Executable file
View file

@ -0,0 +1,336 @@
#include "board.h"
// I2C2
// SCL PB10
// SDA PB11
static I2C_TypeDef *I2Cx;
static void i2c_er_handler(void);
static void i2c_ev_handler(void);
static void i2cUnstick(void);
void I2C1_ER_IRQHandler(void)
{
i2c_er_handler();
}
void I2C1_EV_IRQHandler(void)
{
i2c_ev_handler();
}
void I2C2_ER_IRQHandler(void)
{
i2c_er_handler();
}
void I2C2_EV_IRQHandler(void)
{
i2c_ev_handler();
}
#define I2C_DEFAULT_TIMEOUT 30000
static volatile uint16_t i2cErrorCount = 0;
static volatile bool error = false;
static volatile bool busy;
static volatile uint8_t addr;
static volatile uint8_t reg;
static volatile uint8_t bytes;
static volatile uint8_t writing;
static volatile uint8_t reading;
static volatile uint8_t* write_p;
static volatile uint8_t* read_p;
static void i2c_er_handler(void)
{
volatile uint32_t SR1Register, SR2Register;
/* Read the I2C1 status register */
SR1Register = I2Cx->SR1;
if (SR1Register & 0x0F00) { //an error
// I2C1error.error = ((SR1Register & 0x0F00) >> 8); //save error
// I2C1error.job = job; //the task
}
/* If AF, BERR or ARLO, abandon the current job and commence new if there are jobs */
if (SR1Register & 0x0700) {
SR2Register = I2Cx->SR2; //read second status register to clear ADDR if it is set (note that BTF will not be set after a NACK)
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully)
if (!(SR1Register & 0x0200) && !(I2Cx->CR1 & 0x0200)) { //if we dont have an ARLO error, ensure sending of a stop
if (I2Cx->CR1 & 0x0100) { //We are currently trying to send a start, this is very bad as start,stop will hang the peripheral
while (I2Cx->CR1 & 0x0100); //wait for any start to finish sending
I2C_GenerateSTOP(I2Cx, ENABLE); //send stop to finalise bus transaction
while (I2Cx->CR1 & 0x0200); //wait for stop to finish sending
i2cInit(I2Cx); //reset and configure the hardware
} else {
I2C_GenerateSTOP(I2Cx, ENABLE); //stop to free up the bus
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); //Disable EVT and ERR interrupts while bus inactive
}
}
}
I2Cx->SR1 &= ~0x0F00; //reset all the error bits to clear the interrupt
busy = 0;
}
bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
{
uint8_t my_data[1];
uint32_t timeout = I2C_DEFAULT_TIMEOUT;
addr = addr_ << 1;
reg = reg_;
writing = 1;
reading = 0;
my_data[0] = data;
write_p = my_data;
read_p = my_data;
bytes = 1;
busy = 1;
if (!(I2Cx->CR2 & I2C_IT_EVT)) { //if we are restarting the driver
if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start
while (I2Cx->CR1 & 0x0200) { ; } //wait for any stop to finish sending
I2C_GenerateSTART(I2Cx, ENABLE); //send the start for the new job
}
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); //allow the interrupts to fire off again
}
while (busy && --timeout > 0);
if (timeout == 0) {
i2cErrorCount++;
// reinit peripheral + clock out garbage
i2cInit(I2Cx);
return false;
}
return true;
}
bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
{
uint32_t timeout = I2C_DEFAULT_TIMEOUT;
addr = addr_ << 1;
reg = reg_;
writing = 0;
reading = 1;
read_p = buf;
write_p = buf;
bytes = len;
busy = 1;
if (!(I2Cx->CR2 & I2C_IT_EVT)) { //if we are restarting the driver
if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start
while (I2Cx->CR1 & 0x0200) { ; } //wait for any stop to finish sending
I2C_GenerateSTART(I2Cx, ENABLE); //send the start for the new job
}
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); //allow the interrupts to fire off again
}
while (busy && --timeout > 0);
if (timeout == 0) {
i2cErrorCount++;
// reinit peripheral + clock out garbage
i2cInit(I2Cx);
return false;
}
return true;
}
void i2c_ev_handler(void)
{
static uint8_t subaddress_sent, final_stop; //flag to indicate if subaddess sent, flag to indicate final bus condition
static int8_t index; //index is signed -1==send the subaddress
uint8_t SReg_1 = I2Cx->SR1; //read the status register here
if (SReg_1 & 0x0001) { //we just sent a start - EV5 in ref manual
I2Cx->CR1 &= ~0x0800; //reset the POS bit so ACK/NACK applied to the current byte
I2C_AcknowledgeConfig(I2Cx, ENABLE); //make sure ACK is on
index = 0; //reset the index
if (reading && (subaddress_sent || 0xFF == reg)) { //we have sent the subaddr
subaddress_sent = 1; //make sure this is set in case of no subaddress, so following code runs correctly
if (bytes == 2)
I2Cx->CR1 |= 0x0800; //set the POS bit so NACK applied to the final byte in the two byte read
I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Receiver); //send the address and set hardware mode
} else { //direction is Tx, or we havent sent the sub and rep start
I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Transmitter); //send the address and set hardware mode
if (reg != 0xFF) //0xFF as subaddress means it will be ignored, in Tx or Rx mode
index = -1; //send a subaddress
}
} else if (SReg_1 & 0x0002) { //we just sent the address - EV6 in ref manual
//Read SR1,2 to clear ADDR
volatile uint8_t a;
__DMB(); // memory fence to control hardware
if (bytes == 1 && reading && subaddress_sent) { //we are receiving 1 byte - EV6_3
I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK
__DMB();
a = I2Cx->SR2; //clear ADDR after ACK is turned off
I2C_GenerateSTOP(I2Cx, ENABLE); //program the stop
final_stop = 1;
I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); //allow us to have an EV7
} else { //EV6 and EV6_1
a = I2Cx->SR2; //clear the ADDR here
__DMB();
if (bytes == 2 && reading && subaddress_sent) { //rx 2 bytes - EV6_1
I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to fill
} else if (bytes == 3 && reading && subaddress_sent) //rx 3 bytes
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //make sure RXNE disabled so we get a BTF in two bytes time
else //receiving greater than three bytes, sending subaddress, or transmitting
I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE);
}
} else if (SReg_1 & 0x004) { //Byte transfer finished - EV7_2, EV7_3 or EV8_2
final_stop = 1;
if (reading && subaddress_sent) { //EV7_2, EV7_3
if (bytes > 2) { //EV7_2
I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK
read_p[index++] = I2C_ReceiveData(I2Cx); //read data N-2
I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop
final_stop = 1; //reuired to fix hardware
read_p[index++] = I2C_ReceiveData(I2Cx); //read data N-1
I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); //enable TXE to allow the final EV7
} else { //EV7_3
if (final_stop)
I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop
else
I2C_GenerateSTART(I2Cx, ENABLE); //program a rep start
read_p[index++] = I2C_ReceiveData(I2Cx); //read data N-1
read_p[index++] = I2C_ReceiveData(I2Cx); //read data N
index++; //to show job completed
}
} else { //EV8_2, which may be due to a subaddress sent or a write completion
if (subaddress_sent || (writing)) {
if (final_stop)
I2C_GenerateSTOP(I2Cx, ENABLE); //program the Stop
else
I2C_GenerateSTART(I2Cx, ENABLE); //program a rep start
index++; //to show that the job is complete
} else { //We need to send a subaddress
I2C_GenerateSTART(I2Cx, ENABLE); //program the repeated Start
subaddress_sent = 1; //this is set back to zero upon completion of the current task
}
}
//we must wait for the start to clear, otherwise we get constant BTF
while (I2Cx->CR1 & 0x0100) { ; }
} else if (SReg_1 & 0x0040) { //Byte received - EV7
read_p[index++] = I2C_ReceiveData(I2Cx);
if (bytes == (index + 3))
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to flush so we can get an EV7_2
if (bytes == index) //We have completed a final EV7
index++; //to show job is complete
} else if (SReg_1 & 0x0080) { //Byte transmitted -EV8/EV8_1
if (index != -1) { //we dont have a subaddress to send
I2C_SendData(I2Cx, write_p[index++]);
if (bytes == index) //we have sent all the data
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to flush
} else {
index++;
I2C_SendData(I2Cx, reg); //send the subaddress
if (reading || !bytes) //if receiving or sending 0 bytes, flush now
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to flush
}
}
if (index == bytes + 1) { //we have completed the current job
//Completion Tasks go here
//End of completion tasks
subaddress_sent = 0; //reset this here
// I2Cx->CR1 &= ~0x0800; //reset the POS bit so NACK applied to the current byte
if (final_stop) //If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTF
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); //Disable EVT and ERR interrupts while bus inactive
busy = 0;
}
}
void i2cInit(I2C_TypeDef *I2C)
{
NVIC_InitTypeDef NVIC_InitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
I2C_InitTypeDef I2C_InitStructure;
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
GPIO_Init(GPIOB, &GPIO_InitStructure);
I2Cx = I2C;
// clock out stuff to make sure slaves arent stuck
i2cUnstick();
// Init I2C
I2C_DeInit(I2Cx);
I2C_StructInit(&I2C_InitStructure);
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); //Enable EVT and ERR interrupts - they are enabled by the first request
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_ClockSpeed = 400000;
I2C_Cmd(I2Cx, ENABLE);
I2C_Init(I2Cx, &I2C_InitStructure);
NVIC_PriorityGroupConfig(0x500);
// I2C ER Interrupt
NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// I2C EV Interrupt
NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_Init(&NVIC_InitStructure);
}
uint16_t i2cGetErrorCounter(void)
{
return i2cErrorCount;
}
static void i2cUnstick(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
uint8_t i;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_SetBits(GPIOB, GPIO_Pin_10 | GPIO_Pin_11);
for (i = 0; i < 8; i++) {
// Wait for any clock stretching to finish
while (!GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_10))
delayMicroseconds(10);
// Pull low
GPIO_ResetBits(GPIOB, GPIO_Pin_10); //Set bus low
delayMicroseconds(10);
// Release high again
GPIO_SetBits(GPIOB, GPIO_Pin_10); //Set bus high
delayMicroseconds(10);
}
// Generate a start then stop condition
// SCL PB10
// SDA PB11
GPIO_ResetBits(GPIOB, GPIO_Pin_11); // Set bus data low
delayMicroseconds(10);
GPIO_ResetBits(GPIOB, GPIO_Pin_10); // Set bus scl low
delayMicroseconds(10);
GPIO_SetBits(GPIOB, GPIO_Pin_10); // Set bus scl high
delayMicroseconds(10);
GPIO_SetBits(GPIOB, GPIO_Pin_11); // Set bus sda high
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
GPIO_Init(GPIOB, &GPIO_InitStructure);
}

6
src/drv_i2c.h Executable file
View file

@ -0,0 +1,6 @@
#pragma once
void i2cInit(I2C_TypeDef *I2Cx);
bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data);
bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf);
uint16_t i2cGetErrorCounter(void);

60
src/drv_mpu3050.c Executable file
View file

@ -0,0 +1,60 @@
#include "board.h"
// MPU3050, Standard address 0x68
#define MPU3050_ADDRESS 0x68
// Registers
#define MPU3050_SMPLRT_DIV 0x15
#define MPU3050_DLPF_FS_SYNC 0x16
#define MPU3050_INT_CFG 0x17
#define MPU3050_TEMP_OUT 0x1B
#define MPU3050_GYRO_OUT 0x1D
#define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E
// Bits
#define MPU3050_FS_SEL_2000DPS 0x18
#define MPU3050_DLPF_20HZ 0x04
#define MPU3050_DLPF_42HZ 0x03
#define MPU3050_DLPF_98HZ 0x02
#define MPU3050_DLPF_188HZ 0x01
#define MPU3050_DLPF_256HZ 0x00
#define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01
static uint8_t mpuLowPassFliter = MPU3050_DLPF_42HZ;
void mpu3050Init(void)
{
bool ack;
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
if (!ack)
failureMode(3);
i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFliter);
i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
void mpu3050Read(int16_t *gyroData)
{
uint8_t buf[6];
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
gyroData[0] = buf[0] << 8 | buf[1];
gyroData[1] = buf[2] << 8 | buf[3];
gyroData[2] = buf[4] << 8 | buf[5];
}
int16_t mpu3050ReadTemp(void)
{
uint8_t buf[2];
i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf);
return 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280;
}

5
src/drv_mpu3050.h Executable file
View file

@ -0,0 +1,5 @@
#pragma once
void mpu3050Init(void);
void mpu3050Read(int16_t *gyroData);
int16_t mpu3050ReadTemp(void);

315
src/drv_pwm.c Executable file
View file

@ -0,0 +1,315 @@
#include "board.h"
#define PULSE_1MS (1000) // 1ms pulse width
#define PULSE_PERIOD (2500) // pulse period (400Hz)
#define PULSE_PERIOD_SERVO_DIGITAL (5000) // pulse period for digital servo (200Hz)
#define PULSE_PERIOD_SERVO_ANALOG (20000) // pulse period for analog servo (50Hz)
// Forward declaration
static void pwmIRQHandler(TIM_TypeDef *tim);
static void ppmIRQHandler(TIM_TypeDef *tim);
// local vars
static struct TIM_Channel {
TIM_TypeDef *tim;
uint16_t channel;
uint16_t cc;
} Channels[] = {
{ TIM2, TIM_Channel_1, TIM_IT_CC1 },
{ TIM2, TIM_Channel_2, TIM_IT_CC2 },
{ TIM2, TIM_Channel_3, TIM_IT_CC3 },
{ TIM2, TIM_Channel_4, TIM_IT_CC4 },
{ TIM3, TIM_Channel_1, TIM_IT_CC1 },
{ TIM3, TIM_Channel_2, TIM_IT_CC2 },
{ TIM3, TIM_Channel_3, TIM_IT_CC3 },
{ TIM3, TIM_Channel_4, TIM_IT_CC4 },
};
static volatile uint16_t *OutputChannels[] = {
&(TIM1->CCR1),
&(TIM1->CCR4),
&(TIM4->CCR1),
&(TIM4->CCR2),
&(TIM4->CCR3),
&(TIM4->CCR4),
};
static struct PWM_State {
uint8_t state;
uint16_t rise;
uint16_t fall;
uint16_t capture;
} Inputs[8] = { { 0, } };
static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, };
static bool usePPMFlag = false;
void TIM2_IRQHandler(void)
{
if (usePPMFlag)
ppmIRQHandler(TIM2);
else
pwmIRQHandler(TIM2);
}
void TIM3_IRQHandler(void)
{
pwmIRQHandler(TIM3);
}
static void ppmIRQHandler(TIM_TypeDef *tim)
{
uint16_t diff;
static uint16_t now;
static uint16_t last = 0;
static uint8_t chan = 0;
if (TIM_GetITStatus(TIM2, TIM_IT_CC1) == SET) {
last = now;
now = TIM_GetCapture1(TIM2);
}
TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
if (now > last) {
diff = (now - last);
} else {
diff = ((0xFFFF - last) + now);
}
if (diff > 4000) {
chan = 0;
} else {
if (diff > 750 && diff < 2250 && chan < 8) { // 750 to 2250 ms is our 'valid' channel range
Inputs[chan].capture = diff;
}
chan++;
}
}
static void pwmIRQHandler(TIM_TypeDef *tim)
{
uint8_t i;
uint16_t val = 0;
for (i = 0; i < 8; i++) {
struct TIM_Channel channel = Channels[i];
struct PWM_State *state = &Inputs[i];
if (channel.tim == tim && (TIM_GetITStatus(tim, channel.cc) == SET)) {
TIM_ClearITPendingBit(channel.tim, channel.cc);
switch (channel.channel) {
case TIM_Channel_1:
val = TIM_GetCapture1(channel.tim);
break;
case TIM_Channel_2:
val = TIM_GetCapture2(channel.tim);
break;
case TIM_Channel_3:
val = TIM_GetCapture3(channel.tim);
break;
case TIM_Channel_4:
val = TIM_GetCapture4(channel.tim);
break;
}
if (state->state == 0)
state->rise = val;
else
state->fall = val;
if (state->state == 0) {
// switch states
state->state = 1;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInitStructure.TIM_Channel = channel.channel;
TIM_ICInit(channel.tim, &TIM_ICInitStructure);
} else {
// compute capture
if (state->fall > state->rise)
state->capture = (state->fall - state->rise);
else
state->capture = ((0xffff - state->rise) + state->fall);
// switch state
state->state = 0;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_Channel = channel.channel;
TIM_ICInit(channel.tim, &TIM_ICInitStructure);
}
}
}
}
void pwmInit(bool usePPM, bool useServos, bool useDigitalServos)
{
uint8_t i;
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
TIM_OCInitTypeDef TIM_OCInitStructure = { 0, };
NVIC_InitTypeDef NVIC_InitStructure = { 0, };
// Inputs
// RX1 TIM2_CH1 PA0 [also PPM]
// RX2 TIM2_CH2 PA1
// RX3 TIM2_CH3 PA2
// RX4 TIM2_CH4 PA3
// RX5 TIM3_CH1 PA6
// RX6 TIM3_CH2 PA7
// RX7 TIM3_CH3 PB0
// RX8 TIM3_CH4 PB1
// Outputs
// PWM1 TIM1_CH1 PA8
// PWM2 TIM1_CH4 PA11
// PWM3 TIM4_CH1 PB6
// PWM4 TIM4_CH2 PB7
// PWM5 TIM4_CH3 PB8
// PWM6 TIM4_CH4 PB9
// use PPM or PWM input
usePPMFlag = usePPM;
// preset channels to center
for (i = 0; i < 8; i++)
Inputs[i].capture = 1500;
// Timers run at 1mhz.
// TODO: clean this shit up. Make it all dynamic etc.
// Input pins
if (usePPM) {
// Configure TIM2_CH1 for PPM input
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// Input timer on TIM2 only for PPM
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// TIM2 timebase
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);
TIM_TimeBaseStructure.TIM_Period = 0xffff;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
// Input capture on TIM2_CH1 for PPM
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
TIM_ICInit(TIM2, &TIM_ICInitStructure);
// TIM2_CH1 capture compare interrupt enable
TIM_ITConfig(TIM2, TIM_IT_CC1, ENABLE);
TIM_Cmd(TIM2, ENABLE);
} else {
// Configure TIM2, TIM3 all 4 channels
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_Init(GPIOB, &GPIO_InitStructure);
// Input timers on TIM2 and TIM3 for PWM
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
NVIC_Init(&NVIC_InitStructure);
// TIM2 and TIM3 timebase
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);
TIM_TimeBaseStructure.TIM_Period = 0xffff;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
// PWM Input capture
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
for (i = 0; i < 8; i++) {
TIM_ICInitStructure.TIM_Channel = Channels[i].channel;
TIM_ICInit(Channels[i].tim, &TIM_ICInitStructure);
}
TIM_ITConfig(TIM2, TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4, ENABLE);
TIM_ITConfig(TIM3, TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4, ENABLE);
TIM_Cmd(TIM2, ENABLE);
TIM_Cmd(TIM3, ENABLE);
}
// Output pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
GPIO_Init(GPIOB, &GPIO_InitStructure);
// Output timers
if (useServos) {
// 50Hz/200Hz period on ch1, 2 for servo
if (useDigitalServos)
TIM_TimeBaseStructure.TIM_Period = PULSE_PERIOD_SERVO_DIGITAL - 1;
else
TIM_TimeBaseStructure.TIM_Period = PULSE_PERIOD_SERVO_ANALOG - 1;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Period = PULSE_PERIOD - 1;
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
} else {
TIM_TimeBaseStructure.TIM_Period = PULSE_PERIOD - 1;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
}
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
TIM_OCInitStructure.TIM_Pulse = PULSE_1MS;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
// PWM1,2
TIM_OC1Init(TIM1, &TIM_OCInitStructure);
TIM_OC4Init(TIM1, &TIM_OCInitStructure);
// PWM3,4,5,6
TIM_OC1Init(TIM4, &TIM_OCInitStructure);
TIM_OC2Init(TIM4, &TIM_OCInitStructure);
TIM_OC3Init(TIM4, &TIM_OCInitStructure);
TIM_OC4Init(TIM4, &TIM_OCInitStructure);
TIM_Cmd(TIM1, ENABLE);
TIM_Cmd(TIM4, ENABLE);
TIM_CtrlPWMOutputs(TIM1, ENABLE);
TIM_CtrlPWMOutputs(TIM4, ENABLE);
}
void pwmWrite(uint8_t channel, uint16_t value)
{
*OutputChannels[channel] = value;
}
uint16_t pwmRead(uint8_t channel)
{
return Inputs[channel].capture;
}

5
src/drv_pwm.h Executable file
View file

@ -0,0 +1,5 @@
#pragma once
void pwmInit(bool usePPM, bool useServos, bool useDigitalServos);
void pwmWrite(uint8_t channel, uint16_t value);
uint16_t pwmRead(uint8_t channel);

169
src/drv_system.c Executable file
View file

@ -0,0 +1,169 @@
#include "board.h"
// Cycle counter stuff - these should be defined by CMSIS, but they aren't
#define DWT_CTRL (*(volatile uint32_t *)0xE0001000)
#define DWT_CYCCNT ((volatile uint32_t *)0xE0001004)
#define CYCCNTENA (1 << 0)
// cycles per microsecond
static volatile uint32_t usTicks = 0;
// current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
static volatile uint32_t sysTickUptime = 0;
static volatile uint32_t sysTickCycleCounter = 0;
static void cycleCounterInit(void)
{
RCC_ClocksTypeDef clocks;
RCC_GetClocksFreq(&clocks);
usTicks = clocks.SYSCLK_Frequency / 1000000;
// enable DWT access
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
// enable the CPU cycle counter
DWT_CTRL |= CYCCNTENA;
}
// SysTick
void SysTick_Handler(void)
{
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
}
// Return system uptime in microseconds (rollover in 70minutes)
uint32_t micros(void)
{
register uint32_t oldCycle, cycle, timeMs;
__disable_irq();
cycle = *DWT_CYCCNT;
oldCycle = sysTickCycleCounter;
timeMs = sysTickUptime;
__enable_irq();
return (timeMs * 1000) + (cycle - oldCycle) / usTicks;
}
// Return system uptime in milliseconds (rollover in 49 days)
uint32_t millis(void)
{
return sysTickUptime;
}
void systemInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// Turn on clocks for stuff we use
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE);
RCC_ClearFlag();
// Make all GPIO in by default to save power and reduce noise
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_Init(GPIOC, &GPIO_InitStructure);
// Turn off JTAG port 'cause we're using the GPIO for leds
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
// Configure gpio
// PB3, PB4 (LEDs)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_4;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOB, &GPIO_InitStructure);
LED0_OFF;
LED1_OFF;
// PA12 (Buzzer)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
GPIO_Init(GPIOA, &GPIO_InitStructure);
BEEP_OFF;
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
// Configure the rest of the stuff
adcInit();
i2cInit(I2C2);
uartInit();
// sleep for 100ms
delay(100);
}
void delayMicroseconds(uint32_t us)
{
uint32_t elapsed = 0;
uint32_t lastCount = *DWT_CYCCNT;
for (;;) {
register uint32_t current_count = *DWT_CYCCNT;
uint32_t elapsed_us;
// measure the time elapsed since the last time we checked
elapsed += current_count - lastCount;
lastCount = current_count;
// convert to microseconds
elapsed_us = elapsed / usTicks;
if (elapsed_us >= us)
break;
// reduce the delay by the elapsed time
us -= elapsed_us;
// keep fractional microseconds for the next iteration
elapsed %= usTicks;
}
}
void delay(uint32_t ms)
{
while (ms--)
delayMicroseconds(1000);
}
void failureMode(uint8_t mode)
{
LED1_ON;
LED0_OFF;
while (1) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(475);
BEEP_ON
delay(25);
BEEP_OFF;
}
}
#define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000)
void systemReset(bool toBootloader)
{
if (toBootloader) {
// 1FFFF000 -> 20000200 -> SP
// 1FFFF004 -> 1FFFF021 -> PC
*((uint32_t *)0x20004FF0) = 0xDEADBEEF; // 20KB STM32F103
}
// Generate system reset
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
}

14
src/drv_system.h Executable file
View file

@ -0,0 +1,14 @@
#pragma once
void systemInit(void);
void delayMicroseconds(uint32_t us);
void delay(uint32_t ms);
uint32_t micros(void);
uint32_t millis(void);
// failure
void failureMode(uint8_t mode);
// bootloader/IAP
void systemReset(bool toBootloader);

147
src/drv_uart.c Executable file
View file

@ -0,0 +1,147 @@
#include "board.h"
/*
DMA UART routines idea lifted from AutoQuad
Copyright © 2011 Bill Nesbitt
*/
#define UART_BUFFER_SIZE 256
// Receive buffer, circular DMA
volatile uint8_t rxBuffer[UART_BUFFER_SIZE];
uint32_t rxDMAPos = 0;
volatile uint8_t txBuffer[UART_BUFFER_SIZE];
uint32_t txBufferTail = 0;
uint32_t txBufferHead = 0;
static void uartTxDMA(void)
{
DMA1_Channel4->CMAR = (uint32_t)&txBuffer[txBufferTail];
if (txBufferHead > txBufferTail) {
DMA1_Channel4->CNDTR = txBufferHead - txBufferTail;
txBufferTail = txBufferHead;
} else {
DMA1_Channel4->CNDTR = UART_BUFFER_SIZE - txBufferTail;
txBufferTail = 0;
}
DMA_Cmd(DMA1_Channel4, ENABLE);
}
void DMA1_Channel4_IRQHandler(void)
{
DMA_ClearITPendingBit(DMA1_IT_TC4);
DMA_Cmd(DMA1_Channel4, DISABLE);
if (txBufferHead != txBufferTail)
uartTxDMA();
}
void uartInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
// UART
// USART1_TX PA9
// USART1_RX PA10
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// DMA TX Interrupt
NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
USART_InitStructure.USART_BaudRate = 115200;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART1, &USART_InitStructure);
// Receive DMA into a circular buffer
DMA_DeInit(DMA1_Channel5);
DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USART1->DR;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)rxBuffer;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_BufferSize = UART_BUFFER_SIZE;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_Init(DMA1_Channel5, &DMA_InitStructure);
DMA_Cmd(DMA1_Channel5, ENABLE);
USART_DMACmd(USART1, USART_DMAReq_Rx, ENABLE);
rxDMAPos = DMA_GetCurrDataCounter(DMA1_Channel5);
// Transmit DMA
DMA_DeInit(DMA1_Channel4);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USART1->DR;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_Init(DMA1_Channel4, &DMA_InitStructure);
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
DMA1_Channel4->CNDTR = 0;
USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
USART_Cmd(USART1, ENABLE);
}
uint16_t uartAvailable(void)
{
return (DMA_GetCurrDataCounter(DMA1_Channel5) != rxDMAPos) ? true : false;
}
uint8_t uartRead(void)
{
uint8_t ch;
ch = rxBuffer[UART_BUFFER_SIZE - rxDMAPos];
// go back around the buffer
if (--rxDMAPos == 0)
rxDMAPos = UART_BUFFER_SIZE;
return ch;
}
uint8_t uartReadPoll(void)
{
while (!uartAvailable()); // wait for some bytes
return uartRead();
}
void uartWrite(uint8_t ch)
{
txBuffer[txBufferHead] = ch;
txBufferHead = (txBufferHead + 1) % UART_BUFFER_SIZE;
// if DMA wasn't enabled, fire it up
if (!(DMA1_Channel4->CCR & 1))
uartTxDMA();
}
void uartPrint(char *str)
{
while (*str)
uartWrite(*(str++));
}

8
src/drv_uart.h Executable file
View file

@ -0,0 +1,8 @@
#pragma once
void uartInit(void);
uint16_t uartAvailable(void);
uint8_t uartRead(void);
uint8_t uartReadPoll(void);
void uartWrite(uint8_t ch);
void uartPrint(char *str);

350
src/imu.c Executable file
View file

@ -0,0 +1,350 @@
#include "board.h"
#include "mw.h"
#define M_PI 3.14159265358979323846
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
int16_t acc_25deg = 0;
int32_t BaroAlt;
int32_t EstAlt; // in cm
int16_t BaroPID = 0;
int32_t AltHold;
int16_t errorAltitudeI = 0;
// **************
// gyro+acc IMU
// **************
int16_t gyroData[3] = { 0, 0, 0 };
int16_t gyroZero[3] = { 0, 0, 0 };
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int8_t smallAngle25 = 1;
static void getEstimatedAttitude(void);
void imuInit(void)
{
acc_25deg = acc_1G * 0.423;
// if mag sensor is enabled, use it
if (sensors(SENSOR_MAG))
Mag_init();
}
void computeIMU(void)
{
uint8_t axis;
static int16_t gyroADCprevious[3] = { 0, 0, 0 };
int16_t gyroADCp[3];
int16_t gyroADCinter[3];
static uint32_t timeInterleave = 0;
static int16_t gyroYawSmooth = 0;
if (sensors(SENSOR_ACC)) {
ACC_getADC();
getEstimatedAttitude();
}
Gyro_getADC();
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis];
timeInterleave = micros();
annexCode();
if ((micros() - timeInterleave) > 650) {
annex650_overrun_count++;
} else {
while ((micros() - timeInterleave) < 650); //empirical, interleaving delay between 2 consecutive reads
}
Gyro_getADC();
for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis] + 1) / 3;
gyroADCprevious[axis] = gyroADCinter[axis] / 2;
if (!sensors(SENSOR_ACC))
accADC[axis] = 0;
}
if (feature(FEATURE_GYRO_SMOOTHING)) {
static uint8_t Smoothing[3] = { 0, 0, 0 };
static int16_t gyroSmooth[3] = { 0, 0, 0 };
if (Smoothing[0] == 0) {
// initialize
Smoothing[ROLL] = (cfg.gyro_smoothing_factor >> 16) & 0xff;
Smoothing[PITCH] = (cfg.gyro_smoothing_factor >> 8) & 0xff;
Smoothing[YAW] = (cfg.gyro_smoothing_factor) & 0xff;
}
for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (gyroSmooth[axis] * (Smoothing[axis] - 1) + gyroData[axis] + 1) / Smoothing[axis];
gyroSmooth[axis] = gyroData[axis];
}
} else if (cfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW] + 1) / 3;
gyroYawSmooth = gyroData[YAW];
}
}
// **************************************************
// Simplified IMU based on "Complementary Filter"
// Inspired by http://starlino.com/imu_guide.html
//
// adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// Modified: 19/04/2011 by ziss_dm
// Version: V1.1
//
// code size deduction and tmp vector intermediate step for vector rotation computation: October 2011 by Alex
// **************************************************
//****** advanced users settings *******************
/* Set the Low Pass Filter factor for ACC */
/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/
/* Comment this if you do not want filter at all.*/
/* Default WMC value: 8*/
#define ACC_LPF_FACTOR 4
/* Set the Low Pass Filter factor for Magnetometer */
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
/* Comment this if you do not want filter at all.*/
/* Default WMC value: n/a*/
//#define MG_LPF_FACTOR 4
/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
/* Default WMC value: 300*/
#define GYR_CMPF_FACTOR 310.0f
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
/* Default WMC value: n/a*/
#define GYR_CMPFM_FACTOR 200.0f
//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
#define GYRO_SCALE ((2380 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //should be 2279.44 but 2380 gives better result
// +-2000/sec deg scale
//#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)
// +- 200/sec deg scale
// 1.5 is emperical, not sure what it means
// should be in rad/sec
typedef struct fp_vector {
float X;
float Y;
float Z;
} t_fp_vector_def;
typedef union {
float A[3];
t_fp_vector_def V;
} t_fp_vector;
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v, float *delta)
{
struct fp_vector v_tmp = *v;
v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y;
v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y;
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
}
static int16_t _atan2f(float y, float x)
{
// no need for aidsy inaccurate shortcuts on a proper platform
return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
}
static void getEstimatedAttitude(void)
{
uint8_t axis;
int32_t accMag = 0;
static t_fp_vector EstG;
static t_fp_vector EstM;
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth[3];
#endif
#if defined(ACC_LPF_FACTOR)
static int16_t accTemp[3]; //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer
#endif
static uint32_t previousT;
uint32_t currentT = micros();
float scale, deltaGyroAngle[3];
scale = (currentT - previousT) * GYRO_SCALE;
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale;
#if defined(ACC_LPF_FACTOR)
accTemp[axis] = (accTemp[axis] - (accTemp[axis] >> ACC_LPF_FACTOR)) + accADC[axis];
accSmooth[axis] = accTemp[axis] >> ACC_LPF_FACTOR;
#define ACC_VALUE accSmooth[axis]
#else
accSmooth[axis] = accADC[axis];
#define ACC_VALUE accADC[axis]
#endif
accMag += (int32_t) ACC_VALUE * ACC_VALUE;
if (sensors(SENSOR_MAG)) {
#if defined(MG_LPF_FACTOR)
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
#define MAG_VALUE mgSmooth[axis]
#else
#define MAG_VALUE magADC[axis]
#endif
}
}
accMag = accMag * 100 / ((int32_t) acc_1G * acc_1G);
rotateV(&EstG.V, deltaGyroAngle);
if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, deltaGyroAngle);
}
if (abs(accSmooth[ROLL]) < acc_25deg && abs(accSmooth[PITCH]) < acc_25deg && accSmooth[YAW] > 0)
smallAngle25 = 1;
else
smallAngle25 = 0;
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if ((36 < accMag && accMag < 196) || smallAngle25)
for (axis = 0; axis < 3; axis++) {
int16_t acc = ACC_VALUE;
#if !defined(TRUSTED_ACCZ)
if (smallAngle25 && axis == YAW)
//We consider ACCZ = acc_1G when the acc on other axis is small.
//It's a tweak to deal with some configs where ACC_Z tends to a value < acc_1G when high throttle is applied.
//This tweak applies only when the multi is not in inverted position
acc = acc_1G;
#endif
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + acc) * INV_GYR_CMPF_FACTOR;
}
if (sensors(SENSOR_MAG)) {
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
}
// Attitude of the estimated vector
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = _atan2f(EstG.V.Y, EstG.V.Z);
if (sensors(SENSOR_MAG)) {
#define GHETTO
#ifdef GHETTO
// Attitude of the cross product vector GxM
heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z) / 10;
#else
static float Cos_Roll, Sin_Roll, Cos_Pitch, Sin_Pitch;
static float Mx1, My1, Mz1, xh, yh;
static float rollRadians;
static float pitchRadians;
// proper tilt compensation
// Get pitch and roll in radians
rollRadians = angle[ROLL] / 1800.0 * M_PI;
pitchRadians = angle[PITCH] /1800.0 * M_PI;
//rollRadians = _atan2f(accADC[ROLL], accADC[YAW])/1800.0*M_PI;
//pitchRadians = _atan2f(accADC[PITCH], accADC[YAW])/1800.0*M_PI;
// Mx2 and My2 are the corrected values
// Mx1, My1 and Mz1 are the floating point values from the mag sensor
//Mx1 = magADC[ROLL];
//My1 = magADC[PITCH];
//Mz1 = magADC[YAW];
Mx1 = EstM.V.X;
My1 = EstM.V.Y;
Mz1 = EstM.V.Z;
// These are used more than once, so pre-calculate for efficiency
Cos_Roll = cosf(rollRadians);
Cos_Pitch = cosf(pitchRadians);
Sin_Roll = sinf(rollRadians);
Sin_Pitch = sinf(pitchRadians);
// The tilt-compensation equations are as follows
//X_h=X*cos(pitch)+Y*sin(roll)sin(pitch)-Z*cos(roll)*sin(pitch)
//Y_h=Y*cos(roll)+Z*sin(roll)
xh = (Mx1 * Cos_Pitch) + (My1 * Sin_Roll * Sin_Pitch) - (Mz1 * Sin_Pitch * Cos_Roll); // Correct x axis
yh = (My1 * Cos_Roll) + (Mz1 * Sin_Roll); // Correct y axis
// Tilt-adjusted heading in degrees
heading = _atan2f(yh, xh) / 10;
#endif
}
}
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 40
void getEstimatedAltitude(void)
{
uint8_t index;
static uint32_t deadLine = INIT_DELAY;
static int16_t BaroHistTab[BARO_TAB_SIZE];
static int8_t BaroHistIdx;
int32_t BaroHigh = 0;
int32_t BaroLow = 0;
int32_t temp32;
int16_t last;
if (currentTime < deadLine)
return;
deadLine = currentTime + UPDATE_INTERVAL;
//**** Alt. Set Point stabilization PID ****
//calculate speed for D calculation
last = BaroHistTab[BaroHistIdx];
BaroHistTab[BaroHistIdx] = BaroAlt / 10;
BaroHigh += BaroHistTab[BaroHistIdx];
index = (BaroHistIdx + (BARO_TAB_SIZE / 2)) % BARO_TAB_SIZE;
BaroHigh -= BaroHistTab[index];
BaroLow += BaroHistTab[index];
BaroLow -= last;
BaroHistIdx++;
if (BaroHistIdx >= BARO_TAB_SIZE)
BaroHistIdx = 0;
BaroPID = 0;
//D
temp32 = cfg.D8[PIDALT] * (BaroHigh - BaroLow) / 40;
BaroPID -= temp32;
EstAlt = BaroHigh * 10 / (BARO_TAB_SIZE / 2);
temp32 = AltHold - EstAlt;
if (abs(temp32) < 10 && BaroPID < 10)
BaroPID = 0; // remove small D parametr to reduce noise near zoro position
// P
BaroPID += cfg.P8[PIDALT] * constrain(temp32, (-2) * cfg.P8[PIDALT], 2 * cfg.P8[PIDALT]) / 100;
BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150
// I
errorAltitudeI += temp32 * cfg.I8[PIDALT] / 50;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
temp32 = errorAltitudeI / 500; // I in range +/-60
BaroPID += temp32;
}

65
src/main.c Executable file
View file

@ -0,0 +1,65 @@
#include "board.h"
#include "mw.h"
int main(void)
{
uint8_t i;
#if 0
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOB->BRR = 0x18; // set low 4 & 3
GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
#endif
systemInit();
readEEPROM();
checkFirstTime();
// We have these sensors
sensorsSet(SENSOR_ACC | SENSOR_BARO | SENSOR_MAG);
mixerInit(); // this will configure FEATURE_SERVO depending on mixer type
pwmInit(feature(FEATURE_PPM), feature(FEATURE_SERVO), feature(FEATURE_DIGITAL_SERVO));
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
// drop out any sensors that don't seem to work
sensorsAutodetect();
// Init sensors
if (sensors(SENSOR_BARO))
bmp085Init();
if (sensors(SENSOR_ACC))
adxl345Init();
// if this fails, we get a beep + blink pattern. we're doomed.
mpu3050Init();
imuInit(); // Mag is initialized inside imuInit
previousTime = micros();
calibratingG = 400;
#if defined(POWERMETER)
for (i = 0; i <= PMOTOR_SUM; i++)
pMeter[i] = 0;
#endif
// loopy
while (1) {
loop();
}
}

305
src/mixer.c Executable file
View file

@ -0,0 +1,305 @@
#include "board.h"
#include "mw.h"
static uint8_t numberMotor = 4;
int16_t motor[8];
int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
void mixerInit(void)
{
// enable servos for mixes that require them. note, this shifts motor counts.
if (cfg.mixerConfiguration == MULTITYPE_BI || cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_GIMBAL || cfg.mixerConfiguration == MULTITYPE_FLYING_WING)
featureSet(FEATURE_SERVO);
// if we want camstab/trig, that also enabled servos. this is kinda lame. maybe rework feature bits later.
if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CAMTRIG))
featureSet(FEATURE_SERVO);
switch (cfg.mixerConfiguration) {
case MULTITYPE_GIMBAL:
numberMotor = 0;
break;
case MULTITYPE_FLYING_WING:
numberMotor = 1;
break;
case MULTITYPE_BI:
numberMotor = 2;
break;
case MULTITYPE_TRI:
numberMotor = 3;
break;
case MULTITYPE_QUADP:
case MULTITYPE_QUADX:
case MULTITYPE_Y4:
case MULTITYPE_VTAIL4:
numberMotor = 4;
break;
case MULTITYPE_Y6:
case MULTITYPE_HEX6:
case MULTITYPE_HEX6X:
numberMotor = 6;
break;
case MULTITYPE_OCTOX8:
case MULTITYPE_OCTOFLATP:
case MULTITYPE_OCTOFLATX:
numberMotor = 8;
break;
}
}
void writeServos(void)
{
if (!feature(FEATURE_SERVO))
return;
if (cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_BI) {
/* One servo on Motor #4 */
pwmWrite(0, servo[4]);
if (cfg.mixerConfiguration == MULTITYPE_BI)
pwmWrite(1, servo[5]);
} else {
/* Two servos for camstab or FLYING_WING */
pwmWrite(0, servo[0]);
pwmWrite(1, servo[1]);
}
}
void writeMotors(void)
{
uint8_t i;
uint8_t offset = 0;
// when servos are enabled, motor outputs 1..2 are for servos only
if (feature(FEATURE_SERVO))
offset = 2;
for (i = 0; i < numberMotor; i++)
pwmWrite(i + offset, motor[i]);
}
void writeAllMotors(int16_t mc)
{
uint8_t i;
// Sends commands to all motors
for (i = 0; i < numberMotor; i++)
motor[i] = mc;
writeMotors();
}
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL] * X + axisPID[PITCH] * Y + cfg.yaw_direction * axisPID[YAW] * Z
void mixTable(void)
{
int16_t maxMotor;
uint8_t i;
static uint8_t camCycle = 0;
static uint8_t camState = 0;
static uint32_t camTime = 0;
if (numberMotor > 3) {
//prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
}
switch (cfg.mixerConfiguration) {
case MULTITYPE_BI:
motor[0] = PIDMIX(+1, 0, 0); //LEFT
motor[1] = PIDMIX(-1, 0, 0); //RIGHT
servo[4] = constrain(1500 + cfg.yaw_direction * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT
servo[5] = constrain(1500 + cfg.yaw_direction * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT
break;
case MULTITYPE_TRI:
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
servo[4] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
break;
case MULTITYPE_QUADP:
motor[0] = PIDMIX(0, +1, -1); //REAR
motor[1] = PIDMIX(-1, 0, +1); //RIGHT
motor[2] = PIDMIX(+1, 0, +1); //LEFT
motor[3] = PIDMIX(0, -1, -1); //FRONT
break;
case MULTITYPE_QUADX:
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
break;
case MULTITYPE_Y4:
motor[0] = PIDMIX(+0, +1, -1); //REAR_1 CW
motor[1] = PIDMIX(-1, -1, 0); //FRONT_R CCW
motor[2] = PIDMIX(+0, +1, +1); //REAR_2 CCW
motor[3] = PIDMIX(+1, -1, 0); //FRONT_L CW
break;
case MULTITYPE_Y6:
motor[0] = PIDMIX(+0, +4 / 3, +1); //REAR
motor[1] = PIDMIX(-1, -2 / 3, -1); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, -1); //LEFT
motor[3] = PIDMIX(+0, +4 / 3, -1); //UNDER_REAR
motor[4] = PIDMIX(-1, -2 / 3, +1); //UNDER_RIGHT
motor[5] = PIDMIX(+1, -2 / 3, +1); //UNDER_LEFT
break;
case MULTITYPE_HEX6:
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
motor[1] = PIDMIX(-1 / 2, -1 / 2, -1); //FRONT_R
motor[2] = PIDMIX(+1 / 2, +1 / 2, +1); //REAR_L
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
motor[4] = PIDMIX(+0, -1, +1); //FRONT
motor[5] = PIDMIX(+0, +1, -1); //REAR
break;
case MULTITYPE_HEX6X:
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
motor[1] = PIDMIX(-1 / 2, -1 / 2, +1); //FRONT_R
motor[2] = PIDMIX(+1 / 2, +1 / 2, -1); //REAR_L
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
motor[4] = PIDMIX(-1, +0, -1); //RIGHT
motor[5] = PIDMIX(+1, +0, +1); //LEFT
break;
case MULTITYPE_OCTOX8:
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
motor[4] = PIDMIX(-1, +1, +1); //UNDER_REAR_R
motor[5] = PIDMIX(-1, -1, -1); //UNDER_FRONT_R
motor[6] = PIDMIX(+1, +1, -1); //UNDER_REAR_L
motor[7] = PIDMIX(+1, -1, +1); //UNDER_FRONT_L
break;
case MULTITYPE_OCTOFLATP:
motor[0] = PIDMIX(+7 / 10, -7 / 10, +1); //FRONT_L
motor[1] = PIDMIX(-7 / 10, -7 / 10, +1); //FRONT_R
motor[2] = PIDMIX(-7 / 10, +7 / 10, +1); //REAR_R
motor[3] = PIDMIX(+7 / 10, +7 / 10, +1); //REAR_L
motor[4] = PIDMIX(+0, -1, -1); //FRONT
motor[5] = PIDMIX(-1, +0, -1); //RIGHT
motor[6] = PIDMIX(+0, +1, -1); //REAR
motor[7] = PIDMIX(+1, +0, -1); //LEFT
break;
case MULTITYPE_OCTOFLATX:
motor[0] = PIDMIX(+1, -1 / 2, +1); //MIDFRONT_L
motor[1] = PIDMIX(-1 / 2, -1, +1); //FRONT_R
motor[2] = PIDMIX(-1, +1 / 2, +1); //MIDREAR_R
motor[3] = PIDMIX(+1 / 2, +1, +1); //REAR_L
motor[4] = PIDMIX(+1 / 2, -1, -1); //FRONT_L
motor[5] = PIDMIX(-1, -1 / 2, -1); //MIDFRONT_R
motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R
motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L
break;
case MULTITYPE_VTAIL4:
motor[0] = PIDMIX(+0, +1, -1 / 2); //REAR_R
motor[1] = PIDMIX(-1, -1, +2 / 10); //FRONT_R
motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L
motor[3] = PIDMIX(+1, -1, -2 / 10); //FRONT_L
break;
case MULTITYPE_GIMBAL:
servo[0] = constrain(TILT_PITCH_MIDDLE + cfg.tilt_pitch_prop * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[1] = constrain(TILT_ROLL_MIDDLE + cfg.tilt_roll_prop * angle[ROLL] / 16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
break;
case MULTITYPE_FLYING_WING:
motor[0] = rcCommand[THROTTLE];
if (passThruMode) { // do not use sensors for correction, simple 2 channel mixing
servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_L * (rcData[ROLL] - cfg.midrc);
servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_R * (rcData[ROLL] - cfg.midrc);
} else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
servo[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL];
servo[1] = PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL];
}
servo[0] = constrain(servo[0] + cfg.wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX);
servo[1] = constrain(servo[1] + cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX);
break;
}
// do camstab
if (feature(FEATURE_SERVO_TILT)) {
servo[0] = TILT_PITCH_MIDDLE + rcData[AUX3] - 1500;
servo[1] = TILT_ROLL_MIDDLE + rcData[AUX4] - 1500;
if (rcOptions[BOXCAMSTAB]) {
servo[0] += cfg.tilt_pitch_prop * angle[PITCH] / 16;
servo[1] += cfg.tilt_roll_prop * angle[ROLL] / 16;
}
servo[0] = constrain(servo[0], TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[1] = constrain(servo[1], TILT_ROLL_MIN, TILT_ROLL_MAX);
}
// do camtrig (this doesn't actually work)
if (feature(FEATURE_CAMTRIG)) {
if (camCycle == 1) {
if (camState == 0) {
servo[2] = CAM_SERVO_HIGH;
camState = 1;
camTime = millis();
} else if (camState == 1) {
if ((millis() - camTime) > CAM_TIME_HIGH) {
servo[2] = CAM_SERVO_LOW;
camState = 2;
camTime = millis();
}
} else { //camState ==2
if ((millis() - camTime) > CAM_TIME_LOW) {
camState = 0;
camCycle = 0;
}
}
}
if (rcOptions[BOXCAMTRIG])
camCycle = 1;
}
maxMotor = motor[0];
for (i = 1; i < numberMotor; i++)
if (motor[i] > maxMotor)
maxMotor = motor[i];
for (i = 0; i < numberMotor; i++) {
if (maxMotor > cfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - cfg.maxthrottle;
motor[i] = constrain(motor[i], cfg.minthrottle, cfg.maxthrottle);
if ((rcData[THROTTLE]) < MINCHECK) {
if (!feature(FEATURE_MOTOR_STOP))
motor[i] = cfg.minthrottle;
else
motor[i] = cfg.mincommand;
}
if (armed == 0)
motor[i] = cfg.mincommand;
}
#if (LOG_VALUES == 2) || defined(POWERMETER_SOFT)
uint32_t amp;
/* true cubic function; when divided by vbat_max=126 (12.6V) for 3 cell battery this gives maximum value of ~ 500 */
/* Lookup table moved to PROGMEM 11/21/2001 by Danal */
static uint16_t amperes[64] = {
0, 2, 6, 15, 30, 52, 82, 123, 175, 240, 320, 415, 528, 659, 811, 984, 1181, 1402, 1648, 1923, 2226, 2559, 2924, 3322, 3755, 4224, 4730, 5276, 5861, 6489, 7160, 7875, 8637, 9446, 10304, 11213, 12173, 13187, 14256, 15381, 16564, 17805, 19108, 20472, 21900, 23392, 24951, 26578, 28274, 30041, 31879, 33792, 35779, 37843, 39984, 42205, 44507, 46890, 49358, 51910, 54549, 57276, 60093, 63000};
if (vbat) { // by all means - must avoid division by zero
for (i = 0; i < NUMBER_MOTOR; i++) {
amp = amperes[((motor[i] - 1000) >> 4)] / vbat; // range mapped from [1000:2000] => [0:1000]; then break that up into 64 ranges; lookup amp
#if (LOG_VALUES == 2)
pMeter[i] += amp; // sum up over time the mapped ESC input
#endif
#if defined(POWERMETER_SOFT)
pMeter[PMOTOR_SUM] += amp; // total sum over all motors
#endif
}
}
#endif
}

678
src/mw.c Executable file
View file

@ -0,0 +1,678 @@
#include "board.h"
#include "mw.h"
// March 2012 V2.0_pre_version_1
#define CHECKBOXITEMS 11
#define PIDITEMS 8
int16_t debug1, debug2, debug3, debug4;
uint8_t buzzerState = 0;
uint32_t currentTime = 0;
uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
uint8_t GPSModeHome = 0; // if GPS RTH is activated
uint8_t GPSModeHold = 0; // if GPS PH is activated
uint8_t headFreeMode = 0; // if head free mode is a activated
uint8_t passThruMode = 0; // if passthrough mode is activated
int16_t headFreeModeHold;
int16_t annex650_overrun_count = 0;
uint8_t armed = 0;
uint8_t vbat; // battery voltage in 0.1V steps
volatile int16_t failsafeCnt = 0;
int16_t failsafeEvents = 0;
int16_t rcData[8]; // interval [1000;2000]
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
//uint8_t rcRate8;
//uint8_t rcExpo8;
int16_t lookupRX[7]; // lookup table for expo & RC rate
// uint8_t P8[8], I8[8], D8[8]; //8 bits is much faster and the code is much shorter
uint8_t dynP8[3], dynI8[3], dynD8[3];
// uint8_t rollPitchRate;
// uint8_t yawRate;
// uint8_t dynThrPID;
// uint8_t activate1[CHECKBOXITEMS];
// uint8_t activate2[CHECKBOXITEMS];
uint8_t rcOptions[CHECKBOXITEMS];
uint8_t okToArm = 0;
uint8_t accMode = 0; // if level mode is a activated
uint8_t magMode = 0; // if compass heading hold is a activated
uint8_t baroMode = 0; // if altitude hold is activated
int16_t axisPID[3];
volatile uint16_t rcValue[18] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // interval [1000;2000]
uint8_t rcChannel[8] = { ROLL, PITCH, THROTTLE, YAW, AUX1, AUX2, AUX3, AUX4 };
// **********************
// GPS
// **********************
int32_t GPS_latitude, GPS_longitude;
int32_t GPS_latitude_home, GPS_longitude_home;
uint8_t GPS_fix, GPS_fix_home = 0;
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome; // in meters
int16_t GPS_directionToHome = 0; // in degrees
uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters
int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
//Automatic ACC Offset Calibration
// **********************
uint16_t InflightcalibratingA = 0;
int16_t AccInflightCalibrationArmed;
uint16_t AccInflightCalibrationMeasurementDone = 0;
uint16_t AccInflightCalibrationSavetoEEProm = 0;
uint16_t AccInflightCalibrationActive = 0;
// **********************
// power meter
// **********************
#define PMOTOR_SUM 8 // index into pMeter[] for sum
uint32_t pMeter[PMOTOR_SUM + 1]; //we use [0:7] for eight motors,one extra for sum
uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop()
uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]
// uint8_t powerTrigger1 = 0;
uint16_t powerValue = 0; // last known current
uint16_t intPowerMeterSum, intPowerTrigger1;
void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
{
uint8_t i, r;
for (r = 0; r < repeat; r++) {
for (i = 0; i < num; i++) {
LED0_TOGGLE; // switch LEDPIN state
BEEP_ON;
delay(wait);
BEEP_OFF;
}
delay(60);
}
}
void annexCode(void)
{ //this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds
static uint32_t buzzerTime, calibratedAccTime;
#if defined(LCD_TELEMETRY)
static uint16_t telemetryTimer = 0, telemetryAutoTimer = 0, psensorTimer = 0;
#endif
#if defined(LCD_TELEMETRY)
static uint8_t telemetryAutoIndex = 0;
static char telemetryAutoSequence[] = LCD_TELEMETRY_AUTO;
#endif
static uint8_t vbatTimer = 0;
static uint8_t buzzerFreq; //delay between buzzer ring
uint8_t axis, prop1, prop2;
#if defined(POWERMETER_HARD)
uint16_t pMeterRaw; //used for current reading
#endif
static uint8_t ind = 0;
uint16_t vbatRaw = 0;
static uint16_t vbatRawArray[8];
uint8_t i;
//PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < 1500) {
prop2 = 100;
} else if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t) cfg.dynThrPID *(rcData[THROTTLE] - 1500) / 500;
} else {
prop2 = 100 - cfg.dynThrPID;
}
for (axis = 0; axis < 3; axis++) {
uint16_t tmp = min(abs(rcData[axis] - cfg.midrc), 500);
#if defined(DEADBAND)
if (tmp > DEADBAND) {
tmp -= DEADBAND;
} else {
tmp = 0;
}
#endif
if (axis != 2) { //ROLL & PITCH
uint16_t tmp2 = tmp / 100;
rcCommand[axis] = lookupRX[tmp2] + (tmp - tmp2 * 100) * (lookupRX[tmp2 + 1] - lookupRX[tmp2]) / 100;
prop1 = 100 - (uint16_t) cfg.rollPitchRate *tmp / 500;
prop1 = (uint16_t) prop1 *prop2 / 100;
} else { //YAW
rcCommand[axis] = tmp;
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
}
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < cfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
rcCommand[THROTTLE] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - MINCHECK) / (2000 - MINCHECK);
if (headFreeMode) {
float radDiff = (heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
#if defined(POWERMETER_HARD)
if (!(++psensorTimer % PSENSORFREQ)) {
pMeterRaw = analogRead(PSENSORPIN);
powerValue = (PSENSORNULL > pMeterRaw ? PSENSORNULL - pMeterRaw : pMeterRaw - PSENSORNULL); // do not use abs(), it would induce implicit cast to uint and overrun
if (powerValue < 333) { // only accept reasonable values. 333 is empirical
#ifdef LOG_VALUES
if (powerValue > powerMax)
powerMax = powerValue;
#endif
} else {
powerValue = 333;
}
pMeter[PMOTOR_SUM] += (uint32_t) powerValue;
}
#endif
#define ADC_REF_VOLTAGE 3.3f
#define ADC_TO_VOLTAGE (ADC_REF_VOLTAGE / (1<<12)) // 12 bit ADC resolution
#define ADC_VOLTS_PRECISION 12
#define ADC_VOLTS_SLOPE ((10.0f + 1.0f) / 1.0f) // Rtop = 10K, Rbot = 1.0K
#define ADC_TO_VOLTS ((ADC_TO_VOLTAGE / ((1<<(ADC_VOLTS_PRECISION))+1)) * ADC_VOLTS_SLOPE)
if (feature(FEATURE_VBAT)) {
if (!(++vbatTimer % VBATFREQ)) {
// avgVolts = adcAvgVolts * ADC_TO_VOLTS;
vbatRawArray[(ind++) % 8] = adcGetBattery();
for (i = 0; i < 8; i++)
vbatRaw += vbatRawArray[i];
vbat = vbatRaw / (VBATSCALE / 2); // result is Vbatt in 0.1V steps
}
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
buzzerFreq = 7;
} else if (((vbat > VBATLEVEL1_3S)
#if defined(POWERMETER)
&& ((pMeter[PMOTOR_SUM] < pAlarm) || (pAlarm == 0))
#endif
) || (NO_VBAT > vbat)) // ToLuSe
{ //VBAT ok AND powermeter ok, buzzer off
buzzerFreq = 0;
buzzerState = 0;
BEEP_OFF;
#if defined(POWERMETER)
} else if (pMeter[PMOTOR_SUM] > pAlarm) { // sound alarm for powermeter
buzzerFreq = 4;
#endif
} else if (vbat > VBATLEVEL2_3S)
buzzerFreq = 1;
else if (vbat > VBATLEVEL3_3S)
buzzerFreq = 2;
else
buzzerFreq = 4;
if (buzzerFreq) {
if (buzzerState && (currentTime > buzzerTime + 250000)) {
buzzerState = 0;
BEEP_OFF;
buzzerTime = currentTime;
} else if (!buzzerState && (currentTime > (buzzerTime + (2000000 >> buzzerFreq)))) {
buzzerState = 1;
BEEP_ON;
buzzerTime = currentTime;
}
}
}
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
LED0_TOGGLE;
} else {
if (calibratedACC == 1) {
LED0_OFF;
}
if (armed) {
LED0_ON;
}
}
#if defined(LED_RING)
static uint32_t LEDTime;
if (currentTime > LEDTime) {
LEDTime = currentTime + 50000;
i2CLedRingState();
}
#endif
if (currentTime > calibratedAccTime) {
if (smallAngle25 == 0) {
calibratedACC = 0; //the multi uses ACC and is not calibrated or is too much inclinated
LED0_TOGGLE;
calibratedAccTime = currentTime + 500000;
} else
calibratedACC = 1;
}
serialCom();
#if defined(POWERMETER)
intPowerMeterSum = (pMeter[PMOTOR_SUM] / PLEVELDIV);
intPowerTrigger1 = cfg.powerTrigger1 * PLEVELSCALE;
#endif
#ifdef LCD_TELEMETRY_AUTO
if ((telemetry_auto)
&& (!(++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ))) {
telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)];
LCDclear(); // make sure to clear away remnants
}
#endif
#ifdef LCD_TELEMETRY
if (!(++telemetryTimer % LCD_TELEMETRY_FREQ)) {
#if (LCD_TELEMETRY_DEBUG+0 > 0)
telemetry = LCD_TELEMETRY_DEBUG;
#endif
if (telemetry)
lcd_telemetry();
}
#endif
#if GPS
static uint32_t GPSLEDTime;
if (currentTime > GPSLEDTime && (GPS_fix_home == 1)) {
GPSLEDTime = currentTime + 150000;
LEDPIN_TOGGLE;
}
#endif
}
uint16_t readRawRC(uint8_t chan)
{
uint16_t data;
failsafeCnt = 0;
data = pwmRead(rcChannel[chan]);
if (data < 750 || data > 2250)
data = 1500;
return data;
}
void computeRC(void)
{
static int16_t rcData4Values[8][4], rcDataMean[8];
static uint8_t rc4ValuesIndex = 0;
uint8_t chan, a;
#if defined(SBUS)
readSBus();
#endif
rc4ValuesIndex++;
for (chan = 0; chan < 8; chan++) {
rcData4Values[chan][rc4ValuesIndex % 4] = readRawRC(chan);
rcDataMean[chan] = 0;
for (a = 0; a < 4; a++)
rcDataMean[chan] += rcData4Values[chan][a];
rcDataMean[chan] = (rcDataMean[chan] + 2) / 4;
if (rcDataMean[chan] < rcData[chan] - 3)
rcData[chan] = rcDataMean[chan] + 2;
if (rcDataMean[chan] > rcData[chan] + 3)
rcData[chan] = rcDataMean[chan] - 2;
}
}
void loop(void)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
uint8_t axis, i;
int16_t error, errorAngle;
int16_t delta, deltaSum;
int16_t PTerm, ITerm, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static int16_t delta1[3], delta2[3];
static int16_t errorGyroI[3] = { 0, 0, 0 };
static int16_t errorAngleI[2] = { 0, 0 };
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
#if defined(SPEKTRUM)
if (rcFrameComplete)
computeRC();
#endif
if (currentTime > rcTime) { // 50Hz
rcTime = currentTime + 20000;
#if !(defined(SPEKTRUM) ||defined(BTSERIAL))
computeRC();
#endif
// Failsafe routine - added by MIS
#if defined(FAILSAFE)
if (failsafeCnt > (5 * FAILSAVE_DELAY) && armed == 1) { // Stabilize, and set Throttle to specified level
for (i = 0; i < 3; i++)
rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = FAILSAVE_THR0TTLE;
if (failsafeCnt > 5 * (FAILSAVE_DELAY + FAILSAVE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
armed = 0; //This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
okToArm = 0; //to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
failsafeCnt++;
#endif
// end of failsave routine - next change is made with RcOptions setting
if (rcData[THROTTLE] < MINCHECK) {
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK && armed == 0) {
if (rcDelayCommand == 20)
calibratingG = 400;
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] > MAXCHECK && armed == 0) {
if (rcDelayCommand == 20) {
servo[0] = 1500; //we center the yaw gyro in conf mode
writeServos();
#if defined(LCD_CONF)
configurationLoop(); //beginning LCD configuration
#endif
previousTime = micros();
}
}
#if defined(InflightAccCalibration)
else if (armed == 0 && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK) {
if (rcDelayCommand == 20) {
if (AccInflightCalibrationMeasurementDone) { //trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
} else {
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
if (AccInflightCalibrationArmed) {
blinkLED(10, 1, 2);
} else {
blinkLED(10, 10, 3);
}
}
}
}
#endif
else if ((cfg.activate1[BOXARM] > 0) || (cfg.activate2[BOXARM] > 0)) {
if (rcOptions[BOXARM] && okToArm) {
armed = 1;
headFreeModeHold = heading;
} else if (armed)
armed = 0;
rcDelayCommand = 0;
} else if ((rcData[YAW] < MINCHECK || rcData[ROLL] < MINCHECK)
&& armed == 1) {
if (rcDelayCommand == 20)
armed = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ((rcData[YAW] > MAXCHECK || rcData[ROLL] > MAXCHECK)
&& rcData[PITCH] < MAXCHECK && armed == 0 && calibratingG == 0 && calibratedACC == 1) {
if (rcDelayCommand == 20) {
armed = 1;
headFreeModeHold = heading;
}
#ifdef LCD_TELEMETRY_AUTO
} else if (rcData[ROLL] < MINCHECK && rcData[PITCH] > MAXCHECK && armed == 0) {
if (rcDelayCommand == 20) {
if (telemetry_auto) {
telemetry_auto = 0;
telemetry = 0;
} else
telemetry_auto = 1;
}
#endif
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > MAXCHECK && armed == 0) {
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK) { //throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20)
calibratingA = 400;
rcDelayCommand++;
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] < MINCHECK) { //throttle=max, yaw=right, pitch=min
if (rcDelayCommand == 20)
calibratingM = 1; // MAG calibration request
rcDelayCommand++;
} else if (rcData[PITCH] > MAXCHECK) {
cfg.accTrim[PITCH] += 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[PITCH] < MINCHECK) {
cfg.accTrim[PITCH] -= 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[ROLL] > MAXCHECK) {
cfg.accTrim[ROLL] += 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[ROLL] < MINCHECK) {
cfg.accTrim[ROLL] -= 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
} else {
rcDelayCommand = 0;
}
}
#ifdef LOG_VALUES
if (cycleTime > cycleTimeMax)
cycleTimeMax = cycleTime; // remember highscore
if (cycleTime < cycleTimeMin)
cycleTimeMin = cycleTime; // remember lowscore
#endif
#if defined(InflightAccCalibration)
if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXPASSTHRU]) { //Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
if (!AccInflightCalibrationArmed) {
AccInflightCalibrationArmed = 1;
InflightcalibratingA = 50;
}
} else if (AccInflightCalibrationMeasurementDone && armed == 0) {
AccInflightCalibrationArmed = 0;
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
#endif
for(i = 0; i < CHECKBOXITEMS; i++) {
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & cfg.activate1[i]) || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & cfg.activate2[i]);
}
//note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
if ((rcOptions[BOXACC] || (failsafeCnt > 5 * FAILSAVE_DELAY)) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
if (!accMode) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
accMode = 1;
}
} else
accMode = 0; // modified by MIS for failsave support
if ((rcOptions[BOXARM]) == 0)
okToArm = 1;
if (accMode == 1) {
LED1_ON;
} else {
LED1_OFF;
}
if (sensors(SENSOR_BARO)) {
if (rcOptions[BOXBARO]) {
if (baroMode == 0) {
baroMode = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID = 0;
}
} else
baroMode = 0;
}
if (sensors(SENSOR_MAG)) {
if (rcOptions[BOXMAG]) {
if (magMode == 0) {
magMode = 1;
magHold = heading;
}
} else
magMode = 0;
if (rcOptions[BOXHEADFREE]) {
if (headFreeMode == 0) {
headFreeMode = 1;
}
} else
headFreeMode = 0;
}
#if defined(GPS)
if (rcOptions[BOXGPSHOME]) {
GPSModeHome = 1;
} else
GPSModeHome = 0;
if (rcOptions[BOXGPSHOLD]) {
if (GPSModeHold == 0) {
GPSModeHold = 1;
GPS_latitude_hold = GPS_latitude;
GPS_longitude_hold = GPS_longitude;
}
} else {
GPSModeHold = 0;
}
#endif
if (rcOptions[BOXPASSTHRU]) {
passThruMode = 1;
} else
passThruMode = 0;
} else { // not in rc loop
static int8_t taskOrder = 0; //never call all function in the same loop, to avoid high delay spikes
switch (taskOrder) {
case 0:
taskOrder++;
if (sensors(SENSOR_MAG))
Mag_getADC();
break;
case 1:
taskOrder++;
if (sensors(SENSOR_BARO))
Baro_update();
break;
case 2:
taskOrder++;
if (sensors(SENSOR_BARO))
getEstimatedAltitude();
break;
case 3:
taskOrder++;
#if GPS
GPS_NewData();
#endif
break;
default:
taskOrder = 0;
break;
}
}
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;
if (sensors(SENSOR_MAG)) {
if (abs(rcCommand[YAW]) < 70 && magMode) {
int16_t dif = heading - magHold;
if (dif <= -180)
dif += 360;
if (dif >= +180)
dif -= 360;
if (smallAngle25)
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; //18 deg
} else
magHold = heading;
}
if (sensors(SENSOR_BARO)) {
if (baroMode) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) {
baroMode = 0; // so that a new althold reference is defined
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
}
#if GPS
uint16_t GPS_dist;
int16_t GPS_dir;
if ((GPSModeHome == 0 && GPSModeHold == 0) || (GPS_fix_home == 0)) {
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
} else {
if (GPSModeHome == 1) {
GPS_dist = GPS_distanceToHome;
GPS_dir = GPS_directionToHome;
}
if (GPSModeHold == 1) {
GPS_dist = GPS_distanceToHold;
GPS_dir = GPS_directionToHold;
}
float radDiff = (GPS_dir - heading) * 0.0174533f;
GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sin(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // with P=5.0, a distance of 1 meter = 0.5deg inclination
GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cos(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // max inclination = D deg
}
#endif
//**** PITCH & ROLL & YAW PID ****
for (axis = 0; axis < 3; axis++) {
if (accMode == 1 && axis < 2) { //LEVEL MODE
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.accTrim[axis]; //16 bits is ok here
#ifdef LEVEL_PDF
PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100;
#else
PTerm = (int32_t) errorAngle * cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTerm = constrain(PTerm, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); //WindUp //16 bits is ok here
ITerm = ((int32_t) errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
} else { //ACRO MODE or YAW axis
error = (int32_t) rcCommand[axis] * 10 * 8 / cfg.P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error -= gyroData[axis];
PTerm = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); //WindUp //16 bits is ok here
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
}
mixTable();
writeServos();
writeMotors();
}

277
src/mw.h Executable file
View file

@ -0,0 +1,277 @@
#pragma once
#define MINCHECK 1100
#define MAXCHECK 1900
/* this is the value for the ESCs when they are not armed
in some cases, this value must be lowered down to 900 for some specific ESCs */
// #define MINCOMMAND 1000
/* This option should be uncommented if ACC Z is accurate enough when motors are running*/
/* should now be ok with BMA020 and BMA180 ACC */
#define TRUSTED_ACCZ
/* Failsave settings - added by MIS
Failsafe check pulse on THROTTLE channel. If the pulse is OFF (on only THROTTLE or on all channels) the failsafe procedure is initiated.
After FAILSAVE_DELAY time of pulse absence, the level mode is on (if ACC or nunchuk is avaliable), PITCH, ROLL and YAW is centered
and THROTTLE is set to FAILSAVE_THR0TTLE value. You must set this value to descending about 1m/s or so for best results.
This value is depended from your configuration, AUW and some other params.
Next, afrer FAILSAVE_OFF_DELAY the copter is disarmed, and motors is stopped.
If RC pulse coming back before reached FAILSAVE_OFF_DELAY time, after the small quard time the RC control is returned to normal.
If you use serial sum PPM, the sum converter must completly turn off the PPM SUM pusles for this FailSafe functionality.*/
// #define FAILSAFE // Alex: comment this line if you want to deactivate the failsafe function
#define FAILSAVE_DELAY 10 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example
#define FAILSAVE_OFF_DELAY 200 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example
#define FAILSAVE_THR0TTLE (MINTHROTTLE + 200) // Throttle level used for landing - may be relative to MINTHROTTLE - as in this case
/* you can change the tricopter servo travel here */
#define TRI_YAW_CONSTRAINT_MIN 1020
#define TRI_YAW_CONSTRAINT_MAX 2000
/* Flying Wing: you can change change servo orientation and servo min/max values here */
/* valid for all flight modes, even passThrough mode */
/* need to setup servo directions here; no need to swap servos amongst channels at rx */
#define PITCH_DIRECTION_L 1 // left servo - pitch orientation
#define PITCH_DIRECTION_R -1 // right servo - pitch orientation (opposite sign to PITCH_DIRECTION_L, if servos are mounted in mirrored orientation)
#define ROLL_DIRECTION_L 1 // left servo - roll orientation
#define ROLL_DIRECTION_R 1 // right servo - roll orientation (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation)
#define WING_LEFT_MIN 1020 // limit servo travel range must be inside [1020;2000]
#define WING_LEFT_MAX 2000 // limit servo travel range must be inside [1020;2000]
#define WING_RIGHT_MIN 1020 // limit servo travel range must be inside [1020;2000]
#define WING_RIGHT_MAX 2000 // limit servo travel range must be inside [1020;2000]
/* The following lines apply only for a pitch/roll tilt stabilization system
On promini board, it is not compatible with config with 6 motors or more
Uncomment the first line to activate it */
#define TILT_PITCH_MIN 1020 //servo travel min, don't set it below 1020
#define TILT_PITCH_MAX 2000 //servo travel max, max value=2000
#define TILT_PITCH_MIDDLE 1500 //servo neutral value
#define TILT_ROLL_MIN 1020
#define TILT_ROLL_MAX 2000
#define TILT_ROLL_MIDDLE 1500
/* experimental
camera trigger function : activated via Rc Options in the GUI, servo output=A2 on promini */
#define CAM_SERVO_HIGH 2000 // the position of HIGH state servo
#define CAM_SERVO_LOW 1020 // the position of LOW state servo
#define CAM_TIME_HIGH 1000 // the duration of HIGH state servo expressed in ms
#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms
/* for V BAT monitoring
after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN
with R1=33k and R2=51k
vbat = [0;1023]*16/VBATSCALE */
#define VBATFREQ 6 // to read battery voltage - keep equal to PSENSORFREQ (6) unless you know what you are doing
#define VBATSCALE 151 // change this value if readed Battery voltage is different than real voltage
#define VBATLEVEL1_3S 107 // 10,7V
#define VBATLEVEL2_3S 103 // 10,3V
#define VBATLEVEL3_3S 99 // 9.9V
#define NO_VBAT 16 // Avoid beeping without any battery
// Moving Average Gyros by Magnetron1 (Michele Ardito) ########## beta
//#define MMGYRO // Active Moving Average Function for Gyros
//#define MMGYROVECTORLENGHT 10 // Lenght of Moving Average Vector
// Moving Average ServoGimbal Signal Output
//#define MMSERVOGIMBAL // Active Output Moving Average Function for Servos Gimbal
//#define MMSERVOGIMBALVECTORLENGHT 32 // Lenght of Moving Average Vector
#define VERSION 201
// Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization.
typedef enum MultiType
{
MULTITYPE_TRI = 1, // XA
MULTITYPE_QUADP = 2, // XB
MULTITYPE_QUADX = 3, // XC
MULTITYPE_BI = 4, // XD
MULTITYPE_GIMBAL = 5, // XE
MULTITYPE_Y6 = 6, // XF
MULTITYPE_HEX6 = 7, // XG
MULTITYPE_FLYING_WING = 8, // XH
MULTITYPE_Y4 = 9, // XI
MULTITYPE_HEX6X = 10, // XJ
MULTITYPE_OCTOX8 = 11, // XK
MULTITYPE_OCTOFLATP = 12, // XL the GUI is the same for all 8 motor configs
MULTITYPE_OCTOFLATX = 13, // XM the GUI is the same for all 8 motor configs
MULTITYPE_AIRPLANE = 14, // XN
MULTITYPE_HELI_120_CCPM = 15, // XO simple model
MULTITYPE_HELI_90_DEG = 16, // XP simple model
MULTITYPE_VTAIL4 = 17, // XO
MULTITYPE_LAST = 18
} MultiType;
/*********** RC alias *****************/
#define ROLL 0
#define PITCH 1
#define YAW 2
#define THROTTLE 3
#define AUX1 4
#define AUX2 5
#define AUX3 6
#define AUX4 7
#define PIDALT 3
#define PIDVEL 4
#define PIDGPS 5
#define PIDLEVEL 6
#define PIDMAG 7
#define BOXACC 0
#define BOXBARO 1
#define BOXMAG 2
#define BOXCAMSTAB 3
#define BOXCAMTRIG 4
#define BOXARM 5
#define BOXGPSHOME 6
#define BOXGPSHOLD 7
#define BOXPASSTHRU 8
#define BOXHEADFREE 9
#define BOXBEEPERON 10
#define CHECKBOXITEMS 11
#define PIDITEMS 8
#define ACC_ORIENTATION(X, Y, Z) { accADC[ROLL] = Y; accADC[PITCH] = -X; accADC[YAW] = Z; }
#define GYRO_ORIENTATION(X, Y, Z) { gyroADC[ROLL] = -Y; gyroADC[PITCH] = X; gyroADC[YAW] = Z; }
#define min(a,b) ((a)<(b)?(a):(b))
#define max(a,b) ((a)>(b)?(a):(b))
#define abs(x) ((x)>0?(x):-(x))
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
typedef struct config_t {
uint8_t version;
uint8_t mixerConfiguration;
uint32_t enabledFeatures;
uint8_t P8[8];
uint8_t I8[8];
uint8_t D8[8];
uint8_t rcRate8;
uint8_t rcExpo8;
uint8_t rollPitchRate;
uint8_t yawRate;
uint8_t dynThrPID;
int16_t accZero[3];
int16_t magZero[3];
int16_t accTrim[2];
uint32_t gyro_smoothing_factor; // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively
uint8_t activate1[CHECKBOXITEMS];
uint8_t activate2[CHECKBOXITEMS];
uint8_t powerTrigger1; // trigger for alarm based on power consumption
// Radio/ESC-related configuration
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
// mixer-related configuration
int8_t yaw_direction;
uint16_t wing_left_mid; // left servo center pos. - use this for initial trim
uint16_t wing_right_mid; // right servo center pos. - use this for initial trim
uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim
// gimbal-related configuration
int8_t tilt_pitch_prop; // servo proportional (tied to angle) ; can be negative to invert movement
int8_t tilt_roll_prop; // servo proportional (tied to angle) ; can be negative to invert movement
} config_t;
extern int16_t gyroZero[3];
extern int16_t gyroData[3];
extern int16_t angle[2];
extern int16_t axisPID[3];
extern int16_t rcCommand[4];
extern uint8_t rcOptions[CHECKBOXITEMS];
extern int16_t debug1, debug2, debug3, debug4;
extern uint8_t armed;
extern int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
extern uint16_t acc_1G;
extern uint32_t currentTime;
extern uint32_t previousTime;
extern uint16_t cycleTime;
extern uint8_t calibratedACC;
extern uint16_t calibratingA;
extern uint8_t calibratingM;
extern uint16_t calibratingG;
extern int16_t heading;
extern int16_t annex650_overrun_count;
extern int32_t pressure;
extern int32_t BaroAlt;
extern int32_t EstAlt;
extern int32_t AltHold;
extern int16_t errorAltitudeI;
extern int16_t BaroPID;
extern uint8_t headFreeMode;
extern int16_t headFreeModeHold;
extern uint8_t passThruMode;
extern int8_t smallAngle25;
extern int16_t zVelocity;
extern int16_t heading, magHold;
extern int16_t motor[8];
extern int16_t servo[8];
extern int16_t rcData[8];
extern uint8_t accMode;
extern uint8_t magMode;
extern uint8_t baroMode;
extern uint16_t intPowerMeterSum, intPowerTrigger1;
extern int32_t GPS_latitude, GPS_longitude;
extern int32_t GPS_latitude_home, GPS_longitude_home;
extern uint8_t GPS_fix, GPS_fix_home;
extern uint8_t GPS_numSat;
extern uint16_t GPS_distanceToHome;
extern int16_t GPS_directionToHome;
extern uint8_t GPS_update;
extern uint8_t GPSModeHome;
extern uint8_t GPSModeHold;
extern uint8_t vbat;
extern int16_t lookupRX[7]; // lookup table for expo & RC rate
extern config_t cfg;
// main
void loop(void);
// IMU
void imuInit(void);
void annexCode(void);
void computeIMU(void);
void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat);
void getEstimatedAltitude(void);
// Sensors
void sensorsAutodetect(void);
void ACC_getADC(void);
void Baro_update(void);
void Gyro_getADC(void);
void Mag_init(void);
void Mag_getADC(void);
// Output
void mixerInit(void);
void writeServos(void);
void writeMotors(void);
void mixTable(void);
// Serial
void serialCom(void);
// Config
void readEEPROM(void);
void writeParams(void);
void checkFirstTime(void);
bool sensors(uint32_t mask);
void sensorsSet(uint32_t mask);
void sensorsClear(uint32_t mask);
bool feature(uint32_t mask);
void featureSet(uint32_t mask);
void featureClear(uint32_t mask);
void featureClearAll(void);
// cli
void cliProcess(void);

301
src/sensors.c Executable file
View file

@ -0,0 +1,301 @@
#include "board.h"
#include "mw.h"
uint8_t calibratedACC = 0;
uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
uint16_t calibratingG = 0;
uint8_t calibratingM = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration
int16_t heading, magHold;
void sensorsAutodetect(void)
{
if (!adxl345Detect())
sensorsClear(SENSOR_ACC);
if (!bmp085Init())
sensorsClear(SENSOR_BARO);
if (!hmc5883lDetect())
sensorsClear(SENSOR_MAG);
}
static void ACC_Common(void)
{
static int32_t a[3];
uint8_t axis;
if (calibratingA > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (calibratingA == 400)
a[axis] = 0;
// Sum up 400 readings
a[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
cfg.accZero[axis] = 0;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) {
cfg.accZero[ROLL] = a[ROLL] / 400;
cfg.accZero[PITCH] = a[PITCH] / 400;
cfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
cfg.accTrim[ROLL] = 0;
cfg.accTrim[PITCH] = 0;
writeParams(); // write accZero in EEPROM
}
calibratingA--;
}
#if defined(InflightAccCalibration)
static int32_t b[3];
static int16_t accZero_saved[3] = { 0, 0, 0 };
static int16_t accTrim_saved[2] = { 0, 0 };
//Saving old zeropoints before measurement
if (InflightcalibratingA == 50) {
accZero_saved[ROLL] = accZero[ROLL];
accZero_saved[PITCH] = accZero[PITCH];
accZero_saved[YAW] = accZero[YAW];
accTrim_saved[ROLL] = accTrim[ROLL];
accTrim_saved[PITCH] = accTrim[PITCH];
}
if (InflightcalibratingA > 0) {
for (uint8_t axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (InflightcalibratingA == 50)
b[axis] = 0;
// Sum up 50 readings
b[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
accZero[axis] = 0;
}
//all values are measured
if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = 0;
AccInflightCalibrationMeasurementDone = 1;
blinkLED(10, 10, 2); //buzzer for indicatiing the start inflight
// recover saved values to maintain current flight behavior until new values are transferred
accZero[ROLL] = accZero_saved[ROLL];
accZero[PITCH] = accZero_saved[PITCH];
accZero[YAW] = accZero_saved[YAW];
accTrim[ROLL] = accTrim_saved[ROLL];
accTrim[PITCH] = accTrim_saved[PITCH];
}
InflightcalibratingA--;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm == 1) { //the copter is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = 0;
accZero[ROLL] = b[ROLL] / 50;
accZero[PITCH] = b[PITCH] / 50;
accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
accTrim[ROLL] = 0;
accTrim[PITCH] = 0;
writeParams(); // write accZero in EEPROM
}
#endif
accADC[ROLL] -= cfg.accZero[ROLL];
accADC[PITCH] -= cfg.accZero[PITCH];
accADC[YAW] -= cfg.accZero[YAW];
}
void ACC_getADC(void)
{
int16_t rawADC[3];
adxl345Read(rawADC);
ACC_ORIENTATION(-(rawADC[1]), (rawADC[0]), (rawADC[2]));
ACC_Common();
}
static uint32_t baroDeadline = 0;
static uint8_t baroState = 0;
static uint16_t baroUT = 0;
static uint32_t baroUP = 0;
static int16_t baroTemp = 0;
void Baro_update(void)
{
int32_t pressure;
if (currentTime < baroDeadline)
return;
baroDeadline = currentTime;
switch (baroState) {
case 0:
bmp085_start_ut();
baroState++;
baroDeadline += 4600;
break;
case 1:
baroUT = bmp085_get_ut();
baroState++;
break;
case 2:
bmp085_start_up();
baroState++;
baroDeadline += 14000;
break;
case 3:
baroUP = bmp085_get_up();
baroTemp = bmp085_get_temperature(baroUT);
pressure = bmp085_get_pressure(baroUP);
BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter
baroState = 0;
baroDeadline += 5000;
break;
}
}
static void GYRO_Common(void)
{
static int16_t previousGyroADC[3] = { 0, 0, 0 };
static int32_t g[3];
uint8_t axis;
#if defined MMGYRO
// Moving Average Gyros by Magnetron1
//---------------------------------------------------
static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGTH];
static int32_t mediaMobileGyroADCSum[3];
static uint8_t mediaMobileGyroIDX;
//---------------------------------------------------
#endif
if (calibratingG > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == 400)
g[axis] = 0;
// Sum up 400 readings
g[axis] += gyroADC[axis];
// Clear global variables for next reading
gyroADC[axis] = 0;
gyroZero[axis] = 0;
if (calibratingG == 1) {
gyroZero[axis] = g[axis] / 400;
blinkLED(10, 15, 1);
}
}
calibratingG--;
}
#ifdef MMGYRO
mediaMobileGyroIDX = ++mediaMobileGyroIDX % MMGYROVECTORLENGTH;
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
mediaMobileGyroADCSum[axis] -= mediaMobileGyroADC[axis][mediaMobileGyroIDX];
//anti gyro glitch, limit the variation between two consecutive readings
mediaMobileGyroADC[axis][mediaMobileGyroIDX] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
mediaMobileGyroADCSum[axis] += mediaMobileGyroADC[axis][mediaMobileGyroIDX];
gyroADC[axis] = mediaMobileGyroADCSum[axis] / MMGYROVECTORLENGTH;
previousGyroADC[axis] = gyroADC[axis];
}
#else
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
//anti gyro glitch, limit the variation between two consecutive readings
gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
previousGyroADC[axis] = gyroADC[axis];
}
#endif
}
void Gyro_getADC(void)
{
int16_t rawADC[3];
mpu3050Read(rawADC);
// range: +/- 8192; +/- 2000 deg/sec
GYRO_ORIENTATION(+((rawADC[1]) / 4), -((rawADC[0]) / 4), -((rawADC[2]) / 4));
gyroADC[ROLL] = rawADC[0] / 4;
gyroADC[PITCH] = rawADC[1] / 4;
gyroADC[YAW] = -rawADC[2] / 4;
GYRO_Common();
}
static float magCal[3] = { 1.0, 1.0, 1.0 }; // gain for each axis, populated at sensor init
static uint8_t magInit = 0;
static void Mag_getRawADC(void)
{
static int16_t rawADC[3];
hmc5883lRead(rawADC);
// no way? is this finally the proper orientation??
magADC[ROLL] = -rawADC[2]; // X
magADC[PITCH] = rawADC[0]; // Y
magADC[YAW] = rawADC[1]; // Z
}
void Mag_init(void)
{
// initial calibration
hmc5883lInit();
delay(100);
Mag_getRawADC();
delay(10);
magCal[ROLL] = 1000.0 / abs(magADC[ROLL]);
magCal[PITCH] = 1000.0 / abs(magADC[PITCH]);
magCal[YAW] = 1000.0 / abs(magADC[YAW]);
hmc5883lFinishCal();
magInit = 1;
}
void Mag_getADC(void)
{
static uint32_t t, tCal = 0;
static int16_t magZeroTempMin[3];
static int16_t magZeroTempMax[3];
uint8_t axis;
if (currentTime < t)
return; //each read is spaced by 100ms
t = currentTime + 100000;
// Read mag sensor
Mag_getRawADC();
if (calibratingM == 1) {
tCal = t;
for (axis = 0; axis < 3; axis++) {
cfg.magZero[axis] = 0;
magZeroTempMin[axis] = 0;
magZeroTempMax[axis] = 0;
}
calibratingM = 0;
}
magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
magADC[YAW] = magADC[YAW] * magCal[YAW];
if (magInit) { // we apply offset only once mag calibration is done
magADC[ROLL] -= cfg.magZero[ROLL];
magADC[PITCH] -= cfg.magZero[PITCH];
magADC[YAW] -= cfg.magZero[YAW];
}
if (tCal != 0) {
if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
LED0_TOGGLE;
for (axis = 0; axis < 3; axis++) {
if (magADC[axis] < magZeroTempMin[axis])
magZeroTempMin[axis] = magADC[axis];
if (magADC[axis] > magZeroTempMax[axis])
magZeroTempMax[axis] = magADC[axis];
}
} else {
tCal = 0;
for (axis = 0; axis < 3; axis++)
cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2;
writeParams();
}
}
}

274
src/serial.c Executable file
View file

@ -0,0 +1,274 @@
#include "board.h"
#include "mw.h"
// signal that we're in cli mode
uint8_t cliMode = 0;
void serialize16(int16_t a)
{
uartWrite(a);
uartWrite(a >> 8 & 0xff);
}
void serialize8(uint8_t a)
{
uartWrite(a);
}
void UartSendData()
{
// Data transmission acivated when the ring is not empty
}
void serialCom(void)
{
uint8_t i;
// in cli mode, all uart stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
if (uartAvailable()) {
switch (uartRead()) {
case '#':
uartPrint("\r\nEntering CLI Mode, type 'exit' to return\r\n");
cliMode = 1;
break;
#ifdef BTSERIAL
case 'K': //receive RC data from Bluetooth Serial adapter as a remote
rcData[THROTTLE] = (SerialRead(0) * 4) + 1000;
rcData[ROLL] = (SerialRead(0) * 4) + 1000;
rcData[PITCH] = (SerialRead(0) * 4) + 1000;
rcData[YAW] = (SerialRead(0) * 4) + 1000;
rcData[AUX1] = (SerialRead(0) * 4) + 1000;
break;
#endif
#ifdef LCD_TELEMETRY
case 'A': // button A press
case '1':
if (telemetry == 1)
telemetry = 0;
else {
telemetry = 1;
LCDclear();
}
break;
case 'B': // button B press
case '2':
if (telemetry == 2)
telemetry = 0;
else {
telemetry = 2;
LCDclear();
}
break;
case 'C': // button C press
case '3':
if (telemetry == 3)
telemetry = 0;
else {
telemetry = 3;
LCDclear();
#if defined(LOG_VALUES) && defined(DEBUG)
cycleTimeMax = 0; // reset min/max on transition on->off
cycleTimeMin = 65535;
#endif
}
break;
case 'D': // button D press
case '4':
if (telemetry == 4)
telemetry = 0;
else {
telemetry = 4;
LCDclear();
}
break;
case '5':
if (telemetry == 5)
telemetry = 0;
else {
telemetry = 5;
LCDclear();
}
break;
case '6':
if (telemetry == 6)
telemetry = 0;
else {
telemetry = 6;
LCDclear();
}
break;
case '7':
if (telemetry == 7)
telemetry = 0;
else {
telemetry = 7;
LCDclear();
}
break;
#if defined(LOG_VALUES) && defined(DEBUG)
case 'R':
//Reset logvalues
if (telemetry == 'R')
telemetry = 0;
else {
telemetry = 'R';
LCDclear();
}
break;
#endif
#ifdef DEBUG
case 'F':
{
if (telemetry == 'F')
telemetry = 0;
else {
telemetry = 'F';
LCDclear();
}
break;
}
#endif
case 'a': // button A release
case 'b': // button B release
case 'c': // button C release
case 'd': // button D release
break;
#endif
case 'M': // Multiwii @ arduino to GUI all data
serialize8('M');
serialize8(VERSION); // MultiWii Firmware version
for (i = 0; i < 3; i++)
serialize16(accSmooth[i]);
for (i = 0; i < 3; i++)
serialize16(gyroData[i]);
for (i = 0; i < 3; i++)
serialize16(magADC[i]);
serialize16(EstAlt);
serialize16(heading); // compass
for (i = 0; i < 8; i++)
serialize16(servo[i]);
for (i = 0; i < 8; i++)
serialize16(motor[i]);
for (i = 0; i < 8; i++)
serialize16(rcData[i]);
serialize8(sensors(SENSOR_ACC) << 1 | sensors(SENSOR_BARO) << 2 | sensors(SENSOR_MAG) << 3 | sensors(SENSOR_GPS) << 4);
serialize8(accMode | baroMode << 1 | magMode << 2 | (GPSModeHome | GPSModeHold) << 3);
#if defined(LOG_VALUES)
serialize16(cycleTimeMax);
cycleTimeMax = 0;
#else
serialize16(cycleTime);
#endif
serialize16(i2cGetErrorCounter());
for (i = 0; i < 2; i++)
serialize16(angle[i]);
serialize8(cfg.mixerConfiguration);
for (i = 0; i < PIDITEMS; i++) {
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
serialize8(cfg.rcRate8);
serialize8(cfg.rcExpo8);
serialize8(cfg.rollPitchRate);
serialize8(cfg.yawRate);
serialize8(cfg.dynThrPID);
for (i = 0; i < CHECKBOXITEMS; i++) {
serialize8(cfg.activate1[i]);
serialize8(cfg.activate2[i] | (rcOptions[i] << 7)); // use highest bit to transport state in mwc
}
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome);
serialize8(GPS_numSat);
serialize8(GPS_fix);
serialize8(GPS_update);
serialize16(intPowerMeterSum);
serialize16(intPowerTrigger1);
serialize8(vbat);
serialize16(BaroAlt / 10); // 4 variables are here for general monitoring purpose
serialize16(debug2); // debug2
serialize16(debug3); // debug3
serialize16(debug4); // debug4
serialize8('M');
// UartSendData();
break;
case 'O': // arduino to OSD data - contribution from MIS
serialize8('O');
for (i = 0; i < 3; i++)
serialize16(accSmooth[i]);
for (i = 0; i < 3; i++)
serialize16(gyroData[i]);
serialize16(EstAlt * 10.0f);
serialize16(heading); // compass - 16 bytes
for (i = 0; i < 2; i++)
serialize16(angle[i]); //20
for (i = 0; i < 6; i++)
serialize16(motor[i]); //32
for (i = 0; i < 6; i++) {
serialize16(rcData[i]);
} //44
serialize8(sensors(SENSOR_ACC) << 1 | sensors(SENSOR_BARO) << 2 | sensors(SENSOR_MAG) << 3);
serialize8(accMode | baroMode << 1 | magMode << 2);
serialize8(vbat); // Vbatt 47
serialize8(VERSION); // MultiWii Firmware version
serialize8('O'); //49
// UartSendData();
break;
case 'R': // reboot to bootloader (oops, apparently this w as used for other trash, fix later)
systemReset(true);
break;
case 'X': // dynamic mixer
i = uartReadPoll();
if (i > 64 && i < 64 + MULTITYPE_LAST) {
serialize8('O');
serialize8('K');
cfg.mixerConfiguration = i - '@'; // A..B..C.. index
writeParams();
systemReset(false);
break;
}
serialize8('N');
serialize8('G');
break;
case 'W': //GUI write params to eeprom @ arduino
// while (uartAvailable() < (7 + 3 * PIDITEMS + 2 * CHECKBOXITEMS)) { }
for (i = 0; i < PIDITEMS; i++) {
cfg.P8[i] = uartReadPoll();
cfg.I8[i] = uartReadPoll();
cfg.D8[i] = uartReadPoll();
}
cfg.rcRate8 = uartReadPoll();
cfg.rcExpo8 = uartReadPoll(); //2
cfg.rollPitchRate = uartReadPoll();
cfg.yawRate = uartReadPoll(); //4
cfg.dynThrPID = uartReadPoll(); //5
for (i = 0; i < CHECKBOXITEMS; i++) {
cfg.activate1[i] = uartReadPoll();
cfg.activate2[i] = uartReadPoll();
}
#if defined(POWERMETER)
cfg.powerTrigger1 = (uartReadPoll() + 256 * uartReadPoll()) / PLEVELSCALE; // we rely on writeParams() to compute corresponding pAlarm value
#else
uartReadPoll();
uartReadPoll(); //7 so we unload the two bytes
#endif
writeParams();
break;
case 'S': //GUI to arduino ACC calibration request
calibratingA = 400;
break;
case 'E': //GUI to arduino MAG calibration request
calibratingM = 1;
break;
}
}
}