マイナスの値が出ないように修正
Dependencies: mbed HMC5883L_2
Revision 2:ad66e39cb4a1, committed 2019-02-08
- Comitter:
- taknokolat
- Date:
- Fri Feb 08 07:10:06 2019 +0000
- Parent:
- 1:c91577293e84
- Commit message:
- a
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Feb 07 06:14:43 2019 +0000 +++ b/main.cpp Fri Feb 08 07:10:06 2019 +0000 @@ -1,17 +1,29 @@ #include "mbed.h" #include "HMC5883L.h" +#define PI 3.14159265358979 + +void setup(); +void DisplayClock(); +void SensingHMC(); + HMC5883L compass(PB_9, PB_8); +RawSerial pc(PA_2,PA_3, 115200); + +Timer t; +double g_HMC; +static float nowAngle_HMC=0; +float g_FirstYAW_HMC; +bool setupFlag=false; int main(){ - int16_t a[3]; - double b; + + + setup(); while(1){ - compass.init(); - b= compass.getHeadingXYDeg(20,50); - printf("%lf\r\n",b); - + SensingHMC(); + pc.printf("%3.2f\t\r\n",nowAngle_HMC); // wait(0.); } @@ -24,3 +36,100 @@ }*/ } + +void setup(){ + + + + + + //Init_sensors(); + //switch2.rise(ResetTrim); + + NVIC_SetPriority(USART1_IRQn,0); + NVIC_SetPriority(EXTI0_IRQn,1); + NVIC_SetPriority(TIM5_IRQn,2); + NVIC_SetPriority(EXTI9_5_IRQn,3); + NVIC_SetPriority(USART2_IRQn,4); + DisplayClock(); + t.start(); + + + pc.printf("HMC calibration start\r\n"); + + float offsetstart = t.read(); + while(t.read() - offsetstart < 26){ + SensingHMC(); + pc.printf("%3.2f\t",nowAngle_HMC); + pc.printf("\r\n"); + } + + g_FirstYAW_HMC = nowAngle_HMC; + nowAngle_HMC -=g_FirstYAW_HMC; + if(nowAngle_HMC<0)nowAngle_HMC+=360; + wait(0.2); + + + pc.printf("All initialized\r\n"); + setupFlag=true; +} + +/*void Init_sensors(){ + if(mpu6050.setup() == -1){ + pc.printf("failed initialize\r\n"); + } +}*/ + +void DisplayClock(){ + pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); + pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); + pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); + pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); + pc.printf("\r\n"); +} + +void SensingHMC(){ + //static int16_t deltaT = 0, t_start = 0; + //t_start = t.read_us(); + + float rpy=0, oldrpy=0; + static uint16_t count_changeRPY = 0; + static bool flg_checkoutlier = false; + NVIC_DisableIRQ(USART1_IRQn); + NVIC_DisableIRQ(USART2_IRQn); + NVIC_DisableIRQ(TIM5_IRQn); + NVIC_DisableIRQ(EXTI0_IRQn); + NVIC_DisableIRQ(EXTI9_5_IRQn); + + rpy= compass.getHeadingXYDeg(20,50); + + NVIC_EnableIRQ(USART1_IRQn); + NVIC_EnableIRQ(USART2_IRQn); + NVIC_EnableIRQ(TIM5_IRQn); + NVIC_EnableIRQ(EXTI0_IRQn); + NVIC_EnableIRQ(EXTI9_5_IRQn); + + + //外れ値対策 + //rpy*= 180.0f/PI; + if(!setupFlag){ + rpy -= g_FirstYAW_HMC; + }else{ + if(rpy >= g_FirstYAW_HMC){ + rpy -= g_FirstYAW_HMC; + }else{ + rpy += 360.0f; + rpy -= g_FirstYAW_HMC; + } + } + + if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;} + if(!flg_checkoutlier || count_changeRPY >= 2){ + + nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f; //2つの移動平均 + + count_changeRPY = 0; + }else count_changeRPY++; + flg_checkoutlier = false; + +}