航空研究会 / Mbed 2 deprecated HMC5883L_Calibration_kai

マイナスの値が出ないように修正

Dependencies:   mbed HMC5883L_2

main.cpp@2:ad66e39cb4a1, 2019-02-08 (annotated)

Committer:
taknokolat
Date:
Fri Feb 08 07:10:06 2019 +0000
Revision:
2:ad66e39cb4a1
Parent:
1:c91577293e84 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TUATBM 0:26409a44f1f3 1 #include "mbed.h"
TUATBM 0:26409a44f1f3 2 #include "HMC5883L.h"
TUATBM 0:26409a44f1f3 3
taknokolat 2:ad66e39cb4a1 4 #define PI 3.14159265358979
taknokolat 2:ad66e39cb4a1 5
taknokolat 2:ad66e39cb4a1 6 void setup();
taknokolat 2:ad66e39cb4a1 7 void DisplayClock();
taknokolat 2:ad66e39cb4a1 8 void SensingHMC();
taknokolat 2:ad66e39cb4a1 9
TUATBM 0:26409a44f1f3 10 HMC5883L compass(PB_9, PB_8);
taknokolat 2:ad66e39cb4a1 11 RawSerial pc(PA_2,PA_3, 115200);
taknokolat 2:ad66e39cb4a1 12
taknokolat 2:ad66e39cb4a1 13 Timer t;
taknokolat 2:ad66e39cb4a1 14 double g_HMC;
taknokolat 2:ad66e39cb4a1 15 static float nowAngle_HMC=0;
taknokolat 2:ad66e39cb4a1 16 float g_FirstYAW_HMC;
taknokolat 2:ad66e39cb4a1 17 bool setupFlag=false;
TUATBM 0:26409a44f1f3 18
TUATBM 0:26409a44f1f3 19 int main(){
taknokolat 2:ad66e39cb4a1 20
taknokolat 2:ad66e39cb4a1 21
taknokolat 2:ad66e39cb4a1 22 setup();
TUATBM 0:26409a44f1f3 23
TUATBM 0:26409a44f1f3 24 while(1){
taknokolat 2:ad66e39cb4a1 25 SensingHMC();
taknokolat 2:ad66e39cb4a1 26 pc.printf("%3.2f\t\r\n",nowAngle_HMC);
taknokolat 1:c91577293e84 27 // wait(0.);
TUATBM 0:26409a44f1f3 28 }
TUATBM 0:26409a44f1f3 29
TUATBM 0:26409a44f1f3 30 /*while(1){
TUATBM 0:26409a44f1f3 31 compass.getXYZ(a);
taknokolat 1:c91577293e84 32 //printf("%d, %d, %d,\r\n",a[0],a[2]-150,a[1]);
taknokolat 1:c91577293e84 33 printf("%d, %d\r\n",a[0]+20,a[2]+50);
taknokolat 1:c91577293e84 34 //wait(1);}
taknokolat 1:c91577293e84 35
TUATBM 0:26409a44f1f3 36
taknokolat 1:c91577293e84 37 }*/
taknokolat 1:c91577293e84 38 }
taknokolat 2:ad66e39cb4a1 39
taknokolat 2:ad66e39cb4a1 40 void setup(){
taknokolat 2:ad66e39cb4a1 41
taknokolat 2:ad66e39cb4a1 42
taknokolat 2:ad66e39cb4a1 43
taknokolat 2:ad66e39cb4a1 44
taknokolat 2:ad66e39cb4a1 45
taknokolat 2:ad66e39cb4a1 46 //Init_sensors();
taknokolat 2:ad66e39cb4a1 47 //switch2.rise(ResetTrim);
taknokolat 2:ad66e39cb4a1 48
taknokolat 2:ad66e39cb4a1 49 NVIC_SetPriority(USART1_IRQn,0);
taknokolat 2:ad66e39cb4a1 50 NVIC_SetPriority(EXTI0_IRQn,1);
taknokolat 2:ad66e39cb4a1 51 NVIC_SetPriority(TIM5_IRQn,2);
taknokolat 2:ad66e39cb4a1 52 NVIC_SetPriority(EXTI9_5_IRQn,3);
taknokolat 2:ad66e39cb4a1 53 NVIC_SetPriority(USART2_IRQn,4);
taknokolat 2:ad66e39cb4a1 54 DisplayClock();
taknokolat 2:ad66e39cb4a1 55 t.start();
taknokolat 2:ad66e39cb4a1 56
taknokolat 2:ad66e39cb4a1 57
taknokolat 2:ad66e39cb4a1 58 pc.printf("HMC calibration start\r\n");
taknokolat 2:ad66e39cb4a1 59
taknokolat 2:ad66e39cb4a1 60 float offsetstart = t.read();
taknokolat 2:ad66e39cb4a1 61 while(t.read() - offsetstart < 26){
taknokolat 2:ad66e39cb4a1 62 SensingHMC();
taknokolat 2:ad66e39cb4a1 63 pc.printf("%3.2f\t",nowAngle_HMC);
taknokolat 2:ad66e39cb4a1 64 pc.printf("\r\n");
taknokolat 2:ad66e39cb4a1 65 }
taknokolat 2:ad66e39cb4a1 66
taknokolat 2:ad66e39cb4a1 67 g_FirstYAW_HMC = nowAngle_HMC;
taknokolat 2:ad66e39cb4a1 68 nowAngle_HMC -=g_FirstYAW_HMC;
taknokolat 2:ad66e39cb4a1 69 if(nowAngle_HMC<0)nowAngle_HMC+=360;
taknokolat 2:ad66e39cb4a1 70 wait(0.2);
taknokolat 2:ad66e39cb4a1 71
taknokolat 2:ad66e39cb4a1 72
taknokolat 2:ad66e39cb4a1 73 pc.printf("All initialized\r\n");
taknokolat 2:ad66e39cb4a1 74 setupFlag=true;
taknokolat 2:ad66e39cb4a1 75 }
taknokolat 2:ad66e39cb4a1 76
taknokolat 2:ad66e39cb4a1 77 /*void Init_sensors(){
taknokolat 2:ad66e39cb4a1 78 if(mpu6050.setup() == -1){
taknokolat 2:ad66e39cb4a1 79 pc.printf("failed initialize\r\n");
taknokolat 2:ad66e39cb4a1 80 }
taknokolat 2:ad66e39cb4a1 81 }*/
taknokolat 2:ad66e39cb4a1 82
taknokolat 2:ad66e39cb4a1 83 void DisplayClock(){
taknokolat 2:ad66e39cb4a1 84 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
taknokolat 2:ad66e39cb4a1 85 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
taknokolat 2:ad66e39cb4a1 86 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
taknokolat 2:ad66e39cb4a1 87 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
taknokolat 2:ad66e39cb4a1 88 pc.printf("\r\n");
taknokolat 2:ad66e39cb4a1 89 }
taknokolat 2:ad66e39cb4a1 90
taknokolat 2:ad66e39cb4a1 91 void SensingHMC(){
taknokolat 2:ad66e39cb4a1 92 //static int16_t deltaT = 0, t_start = 0;
taknokolat 2:ad66e39cb4a1 93 //t_start = t.read_us();
taknokolat 2:ad66e39cb4a1 94
taknokolat 2:ad66e39cb4a1 95 float rpy=0, oldrpy=0;
taknokolat 2:ad66e39cb4a1 96 static uint16_t count_changeRPY = 0;
taknokolat 2:ad66e39cb4a1 97 static bool flg_checkoutlier = false;
taknokolat 2:ad66e39cb4a1 98 NVIC_DisableIRQ(USART1_IRQn);
taknokolat 2:ad66e39cb4a1 99 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 2:ad66e39cb4a1 100 NVIC_DisableIRQ(TIM5_IRQn);
taknokolat 2:ad66e39cb4a1 101 NVIC_DisableIRQ(EXTI0_IRQn);
taknokolat 2:ad66e39cb4a1 102 NVIC_DisableIRQ(EXTI9_5_IRQn);
taknokolat 2:ad66e39cb4a1 103
taknokolat 2:ad66e39cb4a1 104 rpy= compass.getHeadingXYDeg(20,50);
taknokolat 2:ad66e39cb4a1 105
taknokolat 2:ad66e39cb4a1 106 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:ad66e39cb4a1 107 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 2:ad66e39cb4a1 108 NVIC_EnableIRQ(TIM5_IRQn);
taknokolat 2:ad66e39cb4a1 109 NVIC_EnableIRQ(EXTI0_IRQn);
taknokolat 2:ad66e39cb4a1 110 NVIC_EnableIRQ(EXTI9_5_IRQn);
taknokolat 2:ad66e39cb4a1 111
taknokolat 2:ad66e39cb4a1 112
taknokolat 2:ad66e39cb4a1 113 //外れ値対策
taknokolat 2:ad66e39cb4a1 114 //rpy*= 180.0f/PI;
taknokolat 2:ad66e39cb4a1 115 if(!setupFlag){
taknokolat 2:ad66e39cb4a1 116 rpy -= g_FirstYAW_HMC;
taknokolat 2:ad66e39cb4a1 117 }else{
taknokolat 2:ad66e39cb4a1 118 if(rpy >= g_FirstYAW_HMC){
taknokolat 2:ad66e39cb4a1 119 rpy -= g_FirstYAW_HMC;
taknokolat 2:ad66e39cb4a1 120 }else{
taknokolat 2:ad66e39cb4a1 121 rpy += 360.0f;
taknokolat 2:ad66e39cb4a1 122 rpy -= g_FirstYAW_HMC;
taknokolat 2:ad66e39cb4a1 123 }
taknokolat 2:ad66e39cb4a1 124 }
taknokolat 2:ad66e39cb4a1 125
taknokolat 2:ad66e39cb4a1 126 if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;}
taknokolat 2:ad66e39cb4a1 127 if(!flg_checkoutlier || count_changeRPY >= 2){
taknokolat 2:ad66e39cb4a1 128
taknokolat 2:ad66e39cb4a1 129 nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f; //2つの移動平均
taknokolat 2:ad66e39cb4a1 130
taknokolat 2:ad66e39cb4a1 131 count_changeRPY = 0;
taknokolat 2:ad66e39cb4a1 132 }else count_changeRPY++;
taknokolat 2:ad66e39cb4a1 133 flg_checkoutlier = false;
taknokolat 2:ad66e39cb4a1 134
taknokolat 2:ad66e39cb4a1 135 }