マイナスの値が出ないように修正
Dependencies: mbed HMC5883L_2
main.cpp
- Committer:
- taknokolat
- Date:
- 2019-02-08
- Revision:
- 2:ad66e39cb4a1
- Parent:
- 1:c91577293e84
File content as of revision 2:ad66e39cb4a1:
#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(){ setup(); while(1){ SensingHMC(); pc.printf("%3.2f\t\r\n",nowAngle_HMC); // wait(0.); } /*while(1){ compass.getXYZ(a); //printf("%d, %d, %d,\r\n",a[0],a[2]-150,a[1]); printf("%d, %d\r\n",a[0]+20,a[2]+50); //wait(1);} }*/ } 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; }