航空研究会 / Mbed 2 deprecated hassi_11

初期値を調整

Dependencies:   mbed MPU6050_2 HMC5883L_2

main.cpp@11:d44d137831b9, 2019-02-10 (annotated)

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