マイナスの値が出ないように修正
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?
User | Revision | Line number | New 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 | } |