航空研究会 / Mbed 2 deprecated hassi_18

SDの設定が反映されるように修正

Dependencies:   mbed MPU6050_2 HMC5883L_2 SDFileSystem3

main.cpp@6:166746820555, 2019-02-08 (annotated)

Committer:
taknokolat
Date:
Fri Feb 08 10:39:17 2019 +0000
Revision:
6:166746820555
Parent:
5:8bfe95431ec0
Child:
7:8989a4b84695
f;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HARUKIDELTA 0:84ddd6d354e1 1 #include "mbed.h"
Skykon 3:c18342e4fddd 2 #include "MPU6050_DMP6.h"
taknokolat 6:166746820555 3 #include "HMC5883L.h"
Skykon 3:c18342e4fddd 4
Skykon 3:c18342e4fddd 5 //MPU_check用
Skykon 3:c18342e4fddd 6 #define PI 3.14159265358979
HARUKIDELTA 0:84ddd6d354e1 7
taknokolat 6:166746820555 8 #define servo_NEUTRAL_R 1900
taknokolat 6:166746820555 9 #define servo_NEUTRAL_L 1900
HARUKIDELTA 0:84ddd6d354e1 10 #define servo_FORWARD_R 1860
HARUKIDELTA 0:84ddd6d354e1 11 #define servo_FORWARD_L 1860
HARUKIDELTA 0:84ddd6d354e1 12 #define servo_back_R 1060
HARUKIDELTA 0:84ddd6d354e1 13 #define servo_back_L 1060
HARUKIDELTA 0:84ddd6d354e1 14 #define servo_slow_FORWARD_R 1560
HARUKIDELTA 0:84ddd6d354e1 15 #define servo_slow_FORWARD_L 1560
HARUKIDELTA 0:84ddd6d354e1 16 #define servo_slow_back_R 1360
HARUKIDELTA 0:84ddd6d354e1 17 #define servo_slow_back_L 1360
HARUKIDELTA 0:84ddd6d354e1 18
HARUKIDELTA 0:84ddd6d354e1 19 #define MOVE_NEUTRAL 0
HARUKIDELTA 0:84ddd6d354e1 20 #define MOVE_FORWARD 1
HARUKIDELTA 0:84ddd6d354e1 21 #define MOVE_LEFT 2
HARUKIDELTA 0:84ddd6d354e1 22 #define MOVE_RIGHT 3
HARUKIDELTA 0:84ddd6d354e1 23 #define MOVE_BACK 4
Skykon 3:c18342e4fddd 24 #define GOAL_FORWARD 5 //ゴール付近_ゆっくり
HARUKIDELTA 0:84ddd6d354e1 25 #define GOAL_LEFT 6
HARUKIDELTA 0:84ddd6d354e1 26 #define GOAL_RIGHT 7
Skykon 3:c18342e4fddd 27 #define MAX_FORWARD 8 //はやい_姿勢修正用
HARUKIDELTA 0:84ddd6d354e1 28 #define MAX_BACK 9
HARUKIDELTA 0:84ddd6d354e1 29
Skykon 3:c18342e4fddd 30
taknokolat 1:290e621741fd 31 void getSF_Serial_jevois();
taknokolat 1:290e621741fd 32 void getSF_Serial_pi();
taknokolat 1:290e621741fd 33
Skykon 3:c18342e4fddd 34
Skykon 3:c18342e4fddd 35 //MPU_check用
Skykon 3:c18342e4fddd 36 void SensingMPU();
Skykon 3:c18342e4fddd 37 void setup();
Skykon 3:c18342e4fddd 38 void Init_sensors();
Skykon 3:c18342e4fddd 39 void DisplayClock();
taknokolat 4:67f705d42f1e 40 void DebugPrint();
taknokolat 6:166746820555 41 void SensingHMC();
Skykon 3:c18342e4fddd 42
taknokolat 6:166746820555 43 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
taknokolat 6:166746820555 44 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
Skykon 3:c18342e4fddd 45
taknokolat 4:67f705d42f1e 46 bool setupFlag=false;
taknokolat 4:67f705d42f1e 47
Skykon 3:c18342e4fddd 48 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
Skykon 3:c18342e4fddd 49
Skykon 3:c18342e4fddd 50 Timer t;
Skykon 3:c18342e4fddd 51
HARUKIDELTA 5:8bfe95431ec0 52 //PWM pin宣言
taknokolat 6:166746820555 53 PwmOut servoR(PC_6); //TIM3_CH1 車輪右
taknokolat 6:166746820555 54 PwmOut servoL(PC_7); //TIM3_CH2 車輪左
HARUKIDELTA 5:8bfe95431ec0 55 PwmOut servoTurnTable(PB_0); //TIM3_CH3 カメラ台回転Servo
HARUKIDELTA 5:8bfe95431ec0 56 PwmOut servoCameradeg(PB_1); //TIM3_CH4 カメラ角度調節Servo
HARUKIDELTA 5:8bfe95431ec0 57 PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
HARUKIDELTA 5:8bfe95431ec0 58 PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動
HARUKIDELTA 5:8bfe95431ec0 59 PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作
HARUKIDELTA 5:8bfe95431ec0 60 PwmOut servoParachute(PA_11); //skipper USB端子より パラシュート切り離し
Skykon 3:c18342e4fddd 61
HARUKIDELTA 5:8bfe95431ec0 62 /*通信用のpinは
HARUKIDELTA 5:8bfe95431ec0 63 PA_3(UART2_Rx) skipperウラ
HARUKIDELTA 5:8bfe95431ec0 64 PA_12(UART6_Rx) skipperオモテ USB端子より
HARUKIDELTA 5:8bfe95431ec0 65 PB_7 (UART1_Rx) skipperオモテ 6番目 TIM4_CH2
HARUKIDELTA 5:8bfe95431ec0 66 */
HARUKIDELTA 5:8bfe95431ec0 67
HARUKIDELTA 5:8bfe95431ec0 68 /*超音波はRaspberryPiに積む*/
HARUKIDELTA 5:8bfe95431ec0 69
HARUKIDELTA 5:8bfe95431ec0 70 //外付けコンパス
taknokolat 6:166746820555 71 HMC5883L compass(PB_9, PB_8); //コンパスセンサー TIM4_CH3とCH4
HARUKIDELTA 0:84ddd6d354e1 72
taknokolat 2:f30666d7838b 73 RawSerial pc(PA_2,PA_3,115200); //uart2
Skykon 3:c18342e4fddd 74 //pa2:UART2_TX,pa3:UART2_RX
taknokolat 2:f30666d7838b 75 RawSerial pc2(PB_6,PB_7,115200); //uart1
Skykon 3:c18342e4fddd 76 //pb6:UART1_TX,pb7:UART1_RX
taknokolat 1:290e621741fd 77
taknokolat 2:f30666d7838b 78 char g_landingcommand='N';
taknokolat 1:290e621741fd 79
taknokolat 1:290e621741fd 80 void MoveCansat(char g_landingcommand);
HARUKIDELTA 0:84ddd6d354e1 81
Skykon 3:c18342e4fddd 82 //MPU_check用
Skykon 3:c18342e4fddd 83 MPU6050DMP6 mpu6050(PC_0,&pc);
Skykon 3:c18342e4fddd 84
Skykon 3:c18342e4fddd 85
HARUKIDELTA 0:84ddd6d354e1 86 int main() {
taknokolat 1:290e621741fd 87
Skykon 3:c18342e4fddd 88 //MPU_check
Skykon 3:c18342e4fddd 89 setup();
Skykon 3:c18342e4fddd 90
taknokolat 1:290e621741fd 91 // シリアル通信受信の割り込みイベント登録
taknokolat 1:290e621741fd 92 pc.attach(getSF_Serial_jevois, Serial::RxIrq);
taknokolat 1:290e621741fd 93 pc2.attach(getSF_Serial_pi, Serial::RxIrq);
taknokolat 1:290e621741fd 94
taknokolat 1:290e621741fd 95 NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
taknokolat 1:290e621741fd 96 NVIC_SetPriority(USART2_IRQn,1);
taknokolat 1:290e621741fd 97
HARUKIDELTA 0:84ddd6d354e1 98 while(1) {
taknokolat 1:290e621741fd 99 //pc.printf("Hello World!!");
taknokolat 1:290e621741fd 100 MoveCansat(g_landingcommand);
Skykon 3:c18342e4fddd 101
Skykon 3:c18342e4fddd 102 SensingMPU();
taknokolat 6:166746820555 103 SensingHMC();
taknokolat 4:67f705d42f1e 104 DebugPrint();
taknokolat 4:67f705d42f1e 105
Skykon 3:c18342e4fddd 106 wait_ms(23);
HARUKIDELTA 0:84ddd6d354e1 107 }
HARUKIDELTA 0:84ddd6d354e1 108 }
HARUKIDELTA 0:84ddd6d354e1 109
Skykon 3:c18342e4fddd 110
taknokolat 1:290e621741fd 111 void MoveCansat(char g_landingcommand)
HARUKIDELTA 0:84ddd6d354e1 112 {
taknokolat 2:f30666d7838b 113 //NVIC_DisableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 114 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 115 switch(g_landingcommand){
taknokolat 2:f30666d7838b 116 case 'N': //MOVE_NEUTRAL
HARUKIDELTA 0:84ddd6d354e1 117 servoR.pulsewidth_us(servo_NEUTRAL_R);
HARUKIDELTA 0:84ddd6d354e1 118 servoL.pulsewidth_us(servo_NEUTRAL_L);
taknokolat 2:f30666d7838b 119 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 120 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 121 break;
taknokolat 1:290e621741fd 122
taknokolat 2:f30666d7838b 123 case 'Y': //MOVE_FORWARD
HARUKIDELTA 0:84ddd6d354e1 124 servoR.pulsewidth_us(servo_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 125 servoL.pulsewidth_us(servo_FORWARD_L);
taknokolat 2:f30666d7838b 126 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 127 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 128 break;
taknokolat 1:290e621741fd 129
taknokolat 1:290e621741fd 130 case 'L': //MOVE_LEFT
HARUKIDELTA 0:84ddd6d354e1 131 servoR.pulsewidth_us(servo_slow_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 132 servoL.pulsewidth_us(servo_slow_back_L);
taknokolat 2:f30666d7838b 133 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 134 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 135
taknokolat 2:f30666d7838b 136 case 'R': //MOVE_RIGHT
HARUKIDELTA 0:84ddd6d354e1 137 servoR.pulsewidth_us(servo_slow_back_R);
HARUKIDELTA 0:84ddd6d354e1 138 servoL.pulsewidth_us(servo_slow_FORWARD_L);
taknokolat 2:f30666d7838b 139 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 140 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 141 break;
taknokolat 1:290e621741fd 142
taknokolat 1:290e621741fd 143 case 'B': //MOVE_BACK
HARUKIDELTA 0:84ddd6d354e1 144 servoR.pulsewidth_us(servo_back_R);
HARUKIDELTA 0:84ddd6d354e1 145 servoL.pulsewidth_us(servo_back_L);
taknokolat 2:f30666d7838b 146 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 147 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 148 break;
taknokolat 1:290e621741fd 149
taknokolat 1:290e621741fd 150 case 'G': //GOAL_FORWARD
HARUKIDELTA 0:84ddd6d354e1 151 servoR.pulsewidth_us(servo_slow_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 152 servoL.pulsewidth_us(servo_slow_FORWARD_L);
taknokolat 2:f30666d7838b 153 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 154 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 155 break;
taknokolat 1:290e621741fd 156
taknokolat 1:290e621741fd 157 default :
taknokolat 2:f30666d7838b 158 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 159 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 160 break;
taknokolat 1:290e621741fd 161
HARUKIDELTA 0:84ddd6d354e1 162 }
HARUKIDELTA 0:84ddd6d354e1 163 return;
taknokolat 1:290e621741fd 164 }
taknokolat 1:290e621741fd 165
taknokolat 1:290e621741fd 166 void getSF_Serial_jevois(){
taknokolat 1:290e621741fd 167
taknokolat 1:290e621741fd 168
taknokolat 1:290e621741fd 169 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 170
taknokolat 1:290e621741fd 171 static int bufcounter=0;
taknokolat 1:290e621741fd 172
taknokolat 1:290e621741fd 173
taknokolat 1:290e621741fd 174
taknokolat 1:290e621741fd 175 if(pc.readable()) { // 受信確認
taknokolat 1:290e621741fd 176
taknokolat 1:290e621741fd 177 SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 178 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 179 //pc.printf("x");
taknokolat 1:290e621741fd 180 return;
taknokolat 1:290e621741fd 181 }
taknokolat 1:290e621741fd 182
taknokolat 1:290e621741fd 183
taknokolat 1:290e621741fd 184
taknokolat 1:290e621741fd 185 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 186
Skykon 3:c18342e4fddd 187 if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 188
taknokolat 1:290e621741fd 189 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 190
taknokolat 1:290e621741fd 191 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 192 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 193 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 194 //wait_ms(20);
taknokolat 1:290e621741fd 195 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 196 bufcounter = 0;
taknokolat 1:290e621741fd 197 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 198 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 199 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 200 }
taknokolat 1:290e621741fd 201
taknokolat 1:290e621741fd 202 else if(bufcounter>=5){
taknokolat 1:290e621741fd 203 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 204 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 205 bufcounter = 0;
taknokolat 1:290e621741fd 206 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 207 }
taknokolat 1:290e621741fd 208 }
taknokolat 1:290e621741fd 209
taknokolat 1:290e621741fd 210
taknokolat 1:290e621741fd 211 }
taknokolat 1:290e621741fd 212
taknokolat 1:290e621741fd 213
taknokolat 1:290e621741fd 214 void getSF_Serial_pi(){
taknokolat 2:f30666d7838b 215
taknokolat 2:f30666d7838b 216 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 217
taknokolat 1:290e621741fd 218 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 219
taknokolat 1:290e621741fd 220 static int bufcounter=0;
taknokolat 1:290e621741fd 221
taknokolat 1:290e621741fd 222
taknokolat 1:290e621741fd 223
taknokolat 1:290e621741fd 224 if(pc2.readable()) { // 受信確認
taknokolat 1:290e621741fd 225
taknokolat 1:290e621741fd 226 SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 227 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 228 //pc.printf("x");
taknokolat 1:290e621741fd 229 return;
taknokolat 1:290e621741fd 230 }
taknokolat 1:290e621741fd 231
taknokolat 1:290e621741fd 232
taknokolat 1:290e621741fd 233
taknokolat 1:290e621741fd 234 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 235
taknokolat 1:290e621741fd 236 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 237
taknokolat 1:290e621741fd 238 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 239
taknokolat 1:290e621741fd 240 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 241 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 242 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 243 //wait_ms(20);
taknokolat 1:290e621741fd 244 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 245 bufcounter = 0;
taknokolat 1:290e621741fd 246 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 247 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 248 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 249 }
taknokolat 1:290e621741fd 250
taknokolat 1:290e621741fd 251 else if(bufcounter>=5){
taknokolat 1:290e621741fd 252 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 253 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 254 bufcounter = 0;
taknokolat 1:290e621741fd 255 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 256 }
taknokolat 1:290e621741fd 257 }
taknokolat 2:f30666d7838b 258
taknokolat 2:f30666d7838b 259 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 260
Skykon 3:c18342e4fddd 261 }
taknokolat 1:290e621741fd 262
Skykon 3:c18342e4fddd 263
Skykon 3:c18342e4fddd 264 void setup(){
Skykon 3:c18342e4fddd 265
Skykon 3:c18342e4fddd 266 Init_sensors();
Skykon 3:c18342e4fddd 267 //switch2.rise(ResetTrim);
Skykon 3:c18342e4fddd 268
Skykon 3:c18342e4fddd 269 NVIC_SetPriority(USART1_IRQn,0);
Skykon 3:c18342e4fddd 270 NVIC_SetPriority(EXTI0_IRQn,1);
Skykon 3:c18342e4fddd 271 NVIC_SetPriority(TIM5_IRQn,2);
Skykon 3:c18342e4fddd 272 NVIC_SetPriority(EXTI9_5_IRQn,3);
Skykon 3:c18342e4fddd 273 NVIC_SetPriority(USART2_IRQn,4);
Skykon 3:c18342e4fddd 274 DisplayClock();
Skykon 3:c18342e4fddd 275 t.start();
Skykon 3:c18342e4fddd 276
Skykon 3:c18342e4fddd 277 pc.printf("MPU calibration start\r\n");
taknokolat 6:166746820555 278 pc.printf("HMC calibration start\r\n");
Skykon 3:c18342e4fddd 279
Skykon 3:c18342e4fddd 280 float offsetstart = t.read();
Skykon 3:c18342e4fddd 281 while(t.read() - offsetstart < 26){
Skykon 3:c18342e4fddd 282 SensingMPU();
taknokolat 6:166746820555 283 SensingHMC();
Skykon 3:c18342e4fddd 284 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
taknokolat 6:166746820555 285 pc.printf("%3.2f\t",nowAngle_HMC);
Skykon 3:c18342e4fddd 286 pc.printf("\r\n");
Skykon 3:c18342e4fddd 287 }
Skykon 3:c18342e4fddd 288
Skykon 3:c18342e4fddd 289 FirstROLL = nowAngle[ROLL];
Skykon 3:c18342e4fddd 290 FirstPITCH = nowAngle[PITCH];
Skykon 3:c18342e4fddd 291 nowAngle[ROLL] -=FirstROLL;
Skykon 3:c18342e4fddd 292 nowAngle[PITCH] -=FirstPITCH;
taknokolat 6:166746820555 293 FirstYAW = nowAngle[YAW];
taknokolat 6:166746820555 294 nowAngle[YAW] -= FirstYAW;
taknokolat 6:166746820555 295
taknokolat 6:166746820555 296 g_FirstYAW_HMC = nowAngle_HMC;
taknokolat 6:166746820555 297 nowAngle_HMC -=g_FirstYAW_HMC;
taknokolat 6:166746820555 298 if(nowAngle_HMC<0)nowAngle_HMC+=360;
Skykon 3:c18342e4fddd 299
Skykon 3:c18342e4fddd 300 wait(0.2);
Skykon 3:c18342e4fddd 301
Skykon 3:c18342e4fddd 302 pc.printf("All initialized\r\n");
taknokolat 4:67f705d42f1e 303 setupFlag=true;
taknokolat 1:290e621741fd 304 }
taknokolat 1:290e621741fd 305
Skykon 3:c18342e4fddd 306
Skykon 3:c18342e4fddd 307 void SensingMPU(){
Skykon 3:c18342e4fddd 308 //static int16_t deltaT = 0, t_start = 0;
Skykon 3:c18342e4fddd 309 //t_start = t.read_us();
Skykon 3:c18342e4fddd 310
Skykon 3:c18342e4fddd 311 float rpy[3] = {0}, oldrpy[3] = {0};
Skykon 3:c18342e4fddd 312 static uint16_t count_changeRPY = 0;
Skykon 3:c18342e4fddd 313 static bool flg_checkoutlier = false;
Skykon 3:c18342e4fddd 314 NVIC_DisableIRQ(USART1_IRQn);
Skykon 3:c18342e4fddd 315 NVIC_DisableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 316 NVIC_DisableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 317 NVIC_DisableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 318 NVIC_DisableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 319
Skykon 3:c18342e4fddd 320 mpu6050.getRollPitchYaw_Skipper(rpy);
Skykon 3:c18342e4fddd 321
Skykon 3:c18342e4fddd 322 NVIC_EnableIRQ(USART1_IRQn);
Skykon 3:c18342e4fddd 323 NVIC_EnableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 324 NVIC_EnableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 325 NVIC_EnableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 326 NVIC_EnableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 327
Skykon 3:c18342e4fddd 328
Skykon 3:c18342e4fddd 329 //外れ値対策
Skykon 3:c18342e4fddd 330 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
Skykon 3:c18342e4fddd 331 rpy[ROLL] -= FirstROLL;
Skykon 3:c18342e4fddd 332 rpy[PITCH] -= FirstPITCH;
taknokolat 4:67f705d42f1e 333 if(!setupFlag){
taknokolat 4:67f705d42f1e 334 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 335 }else{
taknokolat 4:67f705d42f1e 336 if(rpy[YAW] >= FirstYAW){
taknokolat 4:67f705d42f1e 337 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 338 }else{
taknokolat 4:67f705d42f1e 339 rpy[YAW] += 360.0f;
taknokolat 4:67f705d42f1e 340 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 341 }
taknokolat 4:67f705d42f1e 342 }
Skykon 3:c18342e4fddd 343
Skykon 3:c18342e4fddd 344 for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
Skykon 3:c18342e4fddd 345 if(!flg_checkoutlier || count_changeRPY >= 2){
Skykon 3:c18342e4fddd 346 for(uint8_t i=0; i<3; i++){
Skykon 3:c18342e4fddd 347 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
Skykon 3:c18342e4fddd 348 }
Skykon 3:c18342e4fddd 349 count_changeRPY = 0;
Skykon 3:c18342e4fddd 350 }else count_changeRPY++;
Skykon 3:c18342e4fddd 351 flg_checkoutlier = false;
Skykon 3:c18342e4fddd 352
Skykon 3:c18342e4fddd 353 }
Skykon 3:c18342e4fddd 354
Skykon 3:c18342e4fddd 355
Skykon 3:c18342e4fddd 356 void Init_sensors(){
Skykon 3:c18342e4fddd 357 if(mpu6050.setup() == -1){
Skykon 3:c18342e4fddd 358 pc.printf("failed initialize\r\n");
Skykon 3:c18342e4fddd 359 }
Skykon 3:c18342e4fddd 360 }
Skykon 3:c18342e4fddd 361
Skykon 3:c18342e4fddd 362
Skykon 3:c18342e4fddd 363 void DisplayClock(){
Skykon 3:c18342e4fddd 364 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
Skykon 3:c18342e4fddd 365 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
Skykon 3:c18342e4fddd 366 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
Skykon 3:c18342e4fddd 367 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
Skykon 3:c18342e4fddd 368 pc.printf("\r\n");
taknokolat 4:67f705d42f1e 369 }
taknokolat 6:166746820555 370
taknokolat 6:166746820555 371 void SensingHMC(){
taknokolat 6:166746820555 372 //static int16_t deltaT = 0, t_start = 0;
taknokolat 6:166746820555 373 //t_start = t.read_us();
taknokolat 6:166746820555 374
taknokolat 6:166746820555 375 float rpy=0, oldrpy=0;
taknokolat 6:166746820555 376 static uint16_t count_changeRPY = 0;
taknokolat 6:166746820555 377 static bool flg_checkoutlier = false;
taknokolat 6:166746820555 378 NVIC_DisableIRQ(USART1_IRQn);
taknokolat 6:166746820555 379 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 6:166746820555 380 NVIC_DisableIRQ(TIM5_IRQn);
taknokolat 6:166746820555 381 NVIC_DisableIRQ(EXTI0_IRQn);
taknokolat 6:166746820555 382 NVIC_DisableIRQ(EXTI9_5_IRQn);
taknokolat 4:67f705d42f1e 383
taknokolat 6:166746820555 384 rpy= compass.getHeadingXYDeg(20,50);
taknokolat 6:166746820555 385
taknokolat 6:166746820555 386 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 6:166746820555 387 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 6:166746820555 388 NVIC_EnableIRQ(TIM5_IRQn);
taknokolat 6:166746820555 389 NVIC_EnableIRQ(EXTI0_IRQn);
taknokolat 6:166746820555 390 NVIC_EnableIRQ(EXTI9_5_IRQn);
taknokolat 6:166746820555 391
taknokolat 6:166746820555 392
taknokolat 6:166746820555 393 //外れ値対策
taknokolat 6:166746820555 394 //rpy*= 180.0f/PI;
taknokolat 6:166746820555 395 if(!setupFlag){
taknokolat 6:166746820555 396 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 397 }else{
taknokolat 6:166746820555 398 if(rpy >= g_FirstYAW_HMC){
taknokolat 6:166746820555 399 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 400 }else{
taknokolat 6:166746820555 401 rpy += 360.0f;
taknokolat 6:166746820555 402 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 403 }
taknokolat 6:166746820555 404 }
taknokolat 6:166746820555 405
taknokolat 6:166746820555 406 if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;}
taknokolat 6:166746820555 407 if(!flg_checkoutlier || count_changeRPY >= 2){
taknokolat 6:166746820555 408
taknokolat 6:166746820555 409 nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f; //2つの移動平均
taknokolat 6:166746820555 410
taknokolat 6:166746820555 411 count_changeRPY = 0;
taknokolat 6:166746820555 412 }else count_changeRPY++;
taknokolat 6:166746820555 413 flg_checkoutlier = false;
taknokolat 6:166746820555 414
taknokolat 6:166746820555 415 }
taknokolat 6:166746820555 416
taknokolat 4:67f705d42f1e 417 void DebugPrint(){
taknokolat 4:67f705d42f1e 418 //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
taknokolat 4:67f705d42f1e 419 pc.printf("%3.2f\t",nowAngle[2]);
taknokolat 6:166746820555 420 pc.printf("%3.2f\t",nowAngle_HMC);
taknokolat 4:67f705d42f1e 421 pc.printf("\r\n");
taknokolat 6:166746820555 422 }