mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
added startup assembler for gcc-based build
(broken) proper tilt compensation (not currently enabled). git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@100 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop
parent
0545d79457
commit
14e12ce0bf
2 changed files with 456 additions and 1 deletions
47
imu.c
47
imu.c
|
|
@ -173,7 +173,7 @@ static void getEstimatedAttitude(void)
|
|||
static uint32_t previousT;
|
||||
uint32_t currentT = micros();
|
||||
float scale, deltaGyroAngle[3];
|
||||
|
||||
|
||||
scale = (currentT - previousT) * GYRO_SCALE;
|
||||
previousT = currentT;
|
||||
|
||||
|
|
@ -237,8 +237,53 @@ static void getEstimatedAttitude(void)
|
|||
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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
410
startup_stm32f10x_md_gcc.s
Normal file
410
startup_stm32f10x_md_gcc.s
Normal 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>© 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****/
|
||||
|
||||
Loading…
Add table
Add a link
Reference in a new issue