diff --git a/imu.c b/imu.c index 17a964432e..6d688f70a8 100755 --- a/imu.c +++ b/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 } } diff --git a/startup_stm32f10x_md_gcc.s b/startup_stm32f10x_md_gcc.s new file mode 100644 index 0000000000..28159cb160 --- /dev/null +++ b/startup_stm32f10x_md_gcc.s @@ -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. + * + *

© COPYRIGHT 2011 STMicroelectronics

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