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