航空研究会 / Mbed 2 deprecated hassi_17

sdカードに焦点調整を追加

Dependencies:   mbed MPU6050_2 HMC5883L_2 SDFileSystem3

main.cpp@3:c18342e4fddd, 2019-02-06 (annotated)

Committer:
Skykon
Date:
Wed Feb 06 11:49:05 2019 +0000
Revision:
3:c18342e4fddd
Parent:
2:f30666d7838b
Child:
4:67f705d42f1e
a

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"
Skykon 3:c18342e4fddd 3
Skykon 3:c18342e4fddd 4 //MPU_check用
Skykon 3:c18342e4fddd 5 #define PI 3.14159265358979
HARUKIDELTA 0:84ddd6d354e1 6
HARUKIDELTA 0:84ddd6d354e1 7 #define servo_NEUTRAL_R 1460
HARUKIDELTA 0:84ddd6d354e1 8 #define servo_NEUTRAL_L 1460
HARUKIDELTA 0:84ddd6d354e1 9 #define servo_FORWARD_R 1860
HARUKIDELTA 0:84ddd6d354e1 10 #define servo_FORWARD_L 1860
HARUKIDELTA 0:84ddd6d354e1 11 #define servo_back_R 1060
HARUKIDELTA 0:84ddd6d354e1 12 #define servo_back_L 1060
HARUKIDELTA 0:84ddd6d354e1 13 #define servo_slow_FORWARD_R 1560
HARUKIDELTA 0:84ddd6d354e1 14 #define servo_slow_FORWARD_L 1560
HARUKIDELTA 0:84ddd6d354e1 15 #define servo_slow_back_R 1360
HARUKIDELTA 0:84ddd6d354e1 16 #define servo_slow_back_L 1360
HARUKIDELTA 0:84ddd6d354e1 17
HARUKIDELTA 0:84ddd6d354e1 18 #define MOVE_NEUTRAL 0
HARUKIDELTA 0:84ddd6d354e1 19 #define MOVE_FORWARD 1
HARUKIDELTA 0:84ddd6d354e1 20 #define MOVE_LEFT 2
HARUKIDELTA 0:84ddd6d354e1 21 #define MOVE_RIGHT 3
HARUKIDELTA 0:84ddd6d354e1 22 #define MOVE_BACK 4
Skykon 3:c18342e4fddd 23 #define GOAL_FORWARD 5 //ゴール付近_ゆっくり
HARUKIDELTA 0:84ddd6d354e1 24 #define GOAL_LEFT 6
HARUKIDELTA 0:84ddd6d354e1 25 #define GOAL_RIGHT 7
Skykon 3:c18342e4fddd 26 #define MAX_FORWARD 8 //はやい_姿勢修正用
HARUKIDELTA 0:84ddd6d354e1 27 #define MAX_BACK 9
HARUKIDELTA 0:84ddd6d354e1 28
Skykon 3:c18342e4fddd 29
taknokolat 1:290e621741fd 30 void getSF_Serial_jevois();
taknokolat 1:290e621741fd 31 void getSF_Serial_pi();
taknokolat 1:290e621741fd 32
Skykon 3:c18342e4fddd 33
Skykon 3:c18342e4fddd 34 //MPU_check用
Skykon 3:c18342e4fddd 35 void SensingMPU();
Skykon 3:c18342e4fddd 36 void setup();
Skykon 3:c18342e4fddd 37 void Init_sensors();
Skykon 3:c18342e4fddd 38 void DisplayClock();
Skykon 3:c18342e4fddd 39
Skykon 3:c18342e4fddd 40 static float nowAngle[3] = {0,0,0};
Skykon 3:c18342e4fddd 41 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;
Skykon 3:c18342e4fddd 42
Skykon 3:c18342e4fddd 43 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
Skykon 3:c18342e4fddd 44
Skykon 3:c18342e4fddd 45 Timer t;
Skykon 3:c18342e4fddd 46
Skykon 3:c18342e4fddd 47
HARUKIDELTA 0:84ddd6d354e1 48 PwmOut servoR(PC_6);
Skykon 3:c18342e4fddd 49 //pc6:TIM3_CH1
HARUKIDELTA 0:84ddd6d354e1 50 PwmOut servoL(PC_7);
Skykon 3:c18342e4fddd 51 //pc7:TIM3_CH2
HARUKIDELTA 0:84ddd6d354e1 52
taknokolat 2:f30666d7838b 53 RawSerial pc(PA_2,PA_3,115200); //uart2
Skykon 3:c18342e4fddd 54 //pa2:UART2_TX,pa3:UART2_RX
taknokolat 2:f30666d7838b 55 RawSerial pc2(PB_6,PB_7,115200); //uart1
Skykon 3:c18342e4fddd 56 //pb6:UART1_TX,pb7:UART1_RX
taknokolat 1:290e621741fd 57
taknokolat 2:f30666d7838b 58 char g_landingcommand='N';
taknokolat 1:290e621741fd 59
taknokolat 1:290e621741fd 60 void MoveCansat(char g_landingcommand);
HARUKIDELTA 0:84ddd6d354e1 61
Skykon 3:c18342e4fddd 62 //MPU_check用
Skykon 3:c18342e4fddd 63 MPU6050DMP6 mpu6050(PC_0,&pc);
Skykon 3:c18342e4fddd 64
Skykon 3:c18342e4fddd 65
HARUKIDELTA 0:84ddd6d354e1 66 int main() {
taknokolat 1:290e621741fd 67
Skykon 3:c18342e4fddd 68 //MPU_check
Skykon 3:c18342e4fddd 69 setup();
Skykon 3:c18342e4fddd 70
taknokolat 1:290e621741fd 71 // シリアル通信受信の割り込みイベント登録
taknokolat 1:290e621741fd 72 pc.attach(getSF_Serial_jevois, Serial::RxIrq);
taknokolat 1:290e621741fd 73 pc2.attach(getSF_Serial_pi, Serial::RxIrq);
taknokolat 1:290e621741fd 74
taknokolat 1:290e621741fd 75 NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
taknokolat 1:290e621741fd 76 NVIC_SetPriority(USART2_IRQn,1);
taknokolat 1:290e621741fd 77
HARUKIDELTA 0:84ddd6d354e1 78 while(1) {
taknokolat 1:290e621741fd 79 //pc.printf("Hello World!!");
taknokolat 1:290e621741fd 80 MoveCansat(g_landingcommand);
Skykon 3:c18342e4fddd 81
Skykon 3:c18342e4fddd 82 SensingMPU();
Skykon 3:c18342e4fddd 83 wait_ms(23);
Skykon 3:c18342e4fddd 84 //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
Skykon 3:c18342e4fddd 85 //pc.printf("\r\n");
HARUKIDELTA 0:84ddd6d354e1 86 }
HARUKIDELTA 0:84ddd6d354e1 87 }
HARUKIDELTA 0:84ddd6d354e1 88
Skykon 3:c18342e4fddd 89
taknokolat 1:290e621741fd 90 void MoveCansat(char g_landingcommand)
HARUKIDELTA 0:84ddd6d354e1 91 {
taknokolat 2:f30666d7838b 92 //NVIC_DisableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 93 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 94 switch(g_landingcommand){
taknokolat 2:f30666d7838b 95 case 'N': //MOVE_NEUTRAL
HARUKIDELTA 0:84ddd6d354e1 96 servoR.pulsewidth_us(servo_NEUTRAL_R);
HARUKIDELTA 0:84ddd6d354e1 97 servoL.pulsewidth_us(servo_NEUTRAL_L);
taknokolat 2:f30666d7838b 98 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 99 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 100 break;
taknokolat 1:290e621741fd 101
taknokolat 2:f30666d7838b 102 case 'Y': //MOVE_FORWARD
HARUKIDELTA 0:84ddd6d354e1 103 servoR.pulsewidth_us(servo_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 104 servoL.pulsewidth_us(servo_FORWARD_L);
taknokolat 2:f30666d7838b 105 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 106 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 107 break;
taknokolat 1:290e621741fd 108
taknokolat 1:290e621741fd 109 case 'L': //MOVE_LEFT
HARUKIDELTA 0:84ddd6d354e1 110 servoR.pulsewidth_us(servo_slow_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 111 servoL.pulsewidth_us(servo_slow_back_L);
taknokolat 2:f30666d7838b 112 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 113 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 114
taknokolat 2:f30666d7838b 115 case 'R': //MOVE_RIGHT
HARUKIDELTA 0:84ddd6d354e1 116 servoR.pulsewidth_us(servo_slow_back_R);
HARUKIDELTA 0:84ddd6d354e1 117 servoL.pulsewidth_us(servo_slow_FORWARD_L);
taknokolat 2:f30666d7838b 118 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 119 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 120 break;
taknokolat 1:290e621741fd 121
taknokolat 1:290e621741fd 122 case 'B': //MOVE_BACK
HARUKIDELTA 0:84ddd6d354e1 123 servoR.pulsewidth_us(servo_back_R);
HARUKIDELTA 0:84ddd6d354e1 124 servoL.pulsewidth_us(servo_back_L);
taknokolat 2:f30666d7838b 125 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 126 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 127 break;
taknokolat 1:290e621741fd 128
taknokolat 1:290e621741fd 129 case 'G': //GOAL_FORWARD
HARUKIDELTA 0:84ddd6d354e1 130 servoR.pulsewidth_us(servo_slow_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 131 servoL.pulsewidth_us(servo_slow_FORWARD_L);
taknokolat 2:f30666d7838b 132 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 2:f30666d7838b 133 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 134 break;
taknokolat 1:290e621741fd 135
taknokolat 1:290e621741fd 136 default :
taknokolat 2:f30666d7838b 137 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 138 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 139 break;
taknokolat 1:290e621741fd 140
HARUKIDELTA 0:84ddd6d354e1 141 }
HARUKIDELTA 0:84ddd6d354e1 142 return;
taknokolat 1:290e621741fd 143 }
taknokolat 1:290e621741fd 144
taknokolat 1:290e621741fd 145 void getSF_Serial_jevois(){
taknokolat 1:290e621741fd 146
taknokolat 1:290e621741fd 147
taknokolat 1:290e621741fd 148 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 149
taknokolat 1:290e621741fd 150 static int bufcounter=0;
taknokolat 1:290e621741fd 151
taknokolat 1:290e621741fd 152
taknokolat 1:290e621741fd 153
taknokolat 1:290e621741fd 154 if(pc.readable()) { // 受信確認
taknokolat 1:290e621741fd 155
taknokolat 1:290e621741fd 156 SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 157 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 158 //pc.printf("x");
taknokolat 1:290e621741fd 159 return;
taknokolat 1:290e621741fd 160 }
taknokolat 1:290e621741fd 161
taknokolat 1:290e621741fd 162
taknokolat 1:290e621741fd 163
taknokolat 1:290e621741fd 164 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 165
Skykon 3:c18342e4fddd 166 if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 167
taknokolat 1:290e621741fd 168 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 169
taknokolat 1:290e621741fd 170 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 171 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 172 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 173 //wait_ms(20);
taknokolat 1:290e621741fd 174 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 175 bufcounter = 0;
taknokolat 1:290e621741fd 176 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 177 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 178 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 179 }
taknokolat 1:290e621741fd 180
taknokolat 1:290e621741fd 181 else if(bufcounter>=5){
taknokolat 1:290e621741fd 182 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 183 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 184 bufcounter = 0;
taknokolat 1:290e621741fd 185 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 186 }
taknokolat 1:290e621741fd 187 }
taknokolat 1:290e621741fd 188
taknokolat 1:290e621741fd 189
taknokolat 1:290e621741fd 190 }
taknokolat 1:290e621741fd 191
taknokolat 1:290e621741fd 192
taknokolat 1:290e621741fd 193 void getSF_Serial_pi(){
taknokolat 2:f30666d7838b 194
taknokolat 2:f30666d7838b 195 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 196
taknokolat 1:290e621741fd 197 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 198
taknokolat 1:290e621741fd 199 static int bufcounter=0;
taknokolat 1:290e621741fd 200
taknokolat 1:290e621741fd 201
taknokolat 1:290e621741fd 202
taknokolat 1:290e621741fd 203 if(pc2.readable()) { // 受信確認
taknokolat 1:290e621741fd 204
taknokolat 1:290e621741fd 205 SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 206 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 207 //pc.printf("x");
taknokolat 1:290e621741fd 208 return;
taknokolat 1:290e621741fd 209 }
taknokolat 1:290e621741fd 210
taknokolat 1:290e621741fd 211
taknokolat 1:290e621741fd 212
taknokolat 1:290e621741fd 213 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 214
taknokolat 1:290e621741fd 215 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 216
taknokolat 1:290e621741fd 217 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 218
taknokolat 1:290e621741fd 219 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 220 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 221 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 222 //wait_ms(20);
taknokolat 1:290e621741fd 223 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 224 bufcounter = 0;
taknokolat 1:290e621741fd 225 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 226 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 227 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 228 }
taknokolat 1:290e621741fd 229
taknokolat 1:290e621741fd 230 else if(bufcounter>=5){
taknokolat 1:290e621741fd 231 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 232 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 233 bufcounter = 0;
taknokolat 1:290e621741fd 234 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 235 }
taknokolat 1:290e621741fd 236 }
taknokolat 2:f30666d7838b 237
taknokolat 2:f30666d7838b 238 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 239
Skykon 3:c18342e4fddd 240 }
taknokolat 1:290e621741fd 241
Skykon 3:c18342e4fddd 242
Skykon 3:c18342e4fddd 243 void setup(){
Skykon 3:c18342e4fddd 244
Skykon 3:c18342e4fddd 245 Init_sensors();
Skykon 3:c18342e4fddd 246 //switch2.rise(ResetTrim);
Skykon 3:c18342e4fddd 247
Skykon 3:c18342e4fddd 248 NVIC_SetPriority(USART1_IRQn,0);
Skykon 3:c18342e4fddd 249 NVIC_SetPriority(EXTI0_IRQn,1);
Skykon 3:c18342e4fddd 250 NVIC_SetPriority(TIM5_IRQn,2);
Skykon 3:c18342e4fddd 251 NVIC_SetPriority(EXTI9_5_IRQn,3);
Skykon 3:c18342e4fddd 252 NVIC_SetPriority(USART2_IRQn,4);
Skykon 3:c18342e4fddd 253 DisplayClock();
Skykon 3:c18342e4fddd 254 t.start();
Skykon 3:c18342e4fddd 255
Skykon 3:c18342e4fddd 256 pc.printf("MPU calibration start\r\n");
Skykon 3:c18342e4fddd 257
Skykon 3:c18342e4fddd 258 float offsetstart = t.read();
Skykon 3:c18342e4fddd 259 while(t.read() - offsetstart < 26){
Skykon 3:c18342e4fddd 260 SensingMPU();
Skykon 3:c18342e4fddd 261 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
Skykon 3:c18342e4fddd 262 pc.printf("\r\n");
Skykon 3:c18342e4fddd 263 }
Skykon 3:c18342e4fddd 264
Skykon 3:c18342e4fddd 265 FirstROLL = nowAngle[ROLL];
Skykon 3:c18342e4fddd 266 FirstPITCH = nowAngle[PITCH];
Skykon 3:c18342e4fddd 267 nowAngle[ROLL] -=FirstROLL;
Skykon 3:c18342e4fddd 268 nowAngle[PITCH] -=FirstPITCH;
Skykon 3:c18342e4fddd 269
Skykon 3:c18342e4fddd 270 wait(0.2);
Skykon 3:c18342e4fddd 271
Skykon 3:c18342e4fddd 272 pc.printf("All initialized\r\n");
taknokolat 1:290e621741fd 273 }
taknokolat 1:290e621741fd 274
Skykon 3:c18342e4fddd 275
Skykon 3:c18342e4fddd 276 void SensingMPU(){
Skykon 3:c18342e4fddd 277 //static int16_t deltaT = 0, t_start = 0;
Skykon 3:c18342e4fddd 278 //t_start = t.read_us();
Skykon 3:c18342e4fddd 279
Skykon 3:c18342e4fddd 280 float rpy[3] = {0}, oldrpy[3] = {0};
Skykon 3:c18342e4fddd 281 static uint16_t count_changeRPY = 0;
Skykon 3:c18342e4fddd 282 static bool flg_checkoutlier = false;
Skykon 3:c18342e4fddd 283 NVIC_DisableIRQ(USART1_IRQn);
Skykon 3:c18342e4fddd 284 NVIC_DisableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 285 NVIC_DisableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 286 NVIC_DisableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 287 NVIC_DisableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 288
Skykon 3:c18342e4fddd 289 mpu6050.getRollPitchYaw_Skipper(rpy);
Skykon 3:c18342e4fddd 290
Skykon 3:c18342e4fddd 291 NVIC_EnableIRQ(USART1_IRQn);
Skykon 3:c18342e4fddd 292 NVIC_EnableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 293 NVIC_EnableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 294 NVIC_EnableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 295 NVIC_EnableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 296
Skykon 3:c18342e4fddd 297
Skykon 3:c18342e4fddd 298 //外れ値対策
Skykon 3:c18342e4fddd 299 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
Skykon 3:c18342e4fddd 300 rpy[ROLL] -= FirstROLL;
Skykon 3:c18342e4fddd 301 rpy[PITCH] -= FirstPITCH;
Skykon 3:c18342e4fddd 302 rpy[YAW] -= FirstYAW;
Skykon 3:c18342e4fddd 303
Skykon 3:c18342e4fddd 304 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 305 if(!flg_checkoutlier || count_changeRPY >= 2){
Skykon 3:c18342e4fddd 306 for(uint8_t i=0; i<3; i++){
Skykon 3:c18342e4fddd 307 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
Skykon 3:c18342e4fddd 308 }
Skykon 3:c18342e4fddd 309 count_changeRPY = 0;
Skykon 3:c18342e4fddd 310 }else count_changeRPY++;
Skykon 3:c18342e4fddd 311 flg_checkoutlier = false;
Skykon 3:c18342e4fddd 312
Skykon 3:c18342e4fddd 313 }
Skykon 3:c18342e4fddd 314
Skykon 3:c18342e4fddd 315
Skykon 3:c18342e4fddd 316 void Init_sensors(){
Skykon 3:c18342e4fddd 317 if(mpu6050.setup() == -1){
Skykon 3:c18342e4fddd 318 pc.printf("failed initialize\r\n");
Skykon 3:c18342e4fddd 319 }
Skykon 3:c18342e4fddd 320 }
Skykon 3:c18342e4fddd 321
Skykon 3:c18342e4fddd 322
Skykon 3:c18342e4fddd 323 void DisplayClock(){
Skykon 3:c18342e4fddd 324 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
Skykon 3:c18342e4fddd 325 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
Skykon 3:c18342e4fddd 326 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
Skykon 3:c18342e4fddd 327 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
Skykon 3:c18342e4fddd 328 pc.printf("\r\n");
Skykon 3:c18342e4fddd 329 }