imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test

skipper_raspi_uart_test

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

main.cpp@5:8bfe95431ec0, 2019-02-08 (annotated)

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