CORE / Mbed 2 deprecated 2017_03_Kaiho

2017年3月，伊豆大島共同打上実験 CORE＿缶ロケチーム電装

Dependencies:   MPU6050 MS5607 mbed SDFileSystem

main.cpp@15:01d3969b89d0, 2017-03-10 (annotated)

Committer:
mikawataru
Date:
Fri Mar 10 04:28:18 2017 +0000
Revision:
15:01d3969b89d0
Parent:
14:cbee1bfdfca7
Child:
16:0a239047eb29
fixed a power shortage for servo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikawataru 5:bcf55d8fe7a7 1 /*
mikawataru 5:bcf55d8fe7a7 2 2017年3月　伊豆大島共同打上実験
mikawataru 5:bcf55d8fe7a7 3 団体名：CORE
mikawataru 5:bcf55d8fe7a7 4 チーム名：ヌペリオル
mikawataru 5:bcf55d8fe7a7 5 該当電装：ロケット搭載用
mikawataru 5:bcf55d8fe7a7 6
mikawataru 5:bcf55d8fe7a7 7 使用部品
mikawataru 5:bcf55d8fe7a7 8 ・LPC1768(マイコン)
mikawataru 5:bcf55d8fe7a7 9 ・MPU6050(加速度・ジャイロセンサ)
mikawataru 5:bcf55d8fe7a7 10 ・MS5607(気圧・気温センサ)
mikawataru 5:bcf55d8fe7a7 11 ・MicroSDスロット
mikawataru 5:bcf55d8fe7a7 12 ・MG995(サーボモータ)×4
mikawataru 5:bcf55d8fe7a7 13
mikawataru 7:5f693667d0e9 14 使用ライブラリ
mikawataru 7:5f693667d0e9 15 ・MPU6050.h
mikawataru 7:5f693667d0e9 16 https://developer.mbed.org/teams/mbed/code/SDFileSystem/
mikawataru 7:5f693667d0e9 17 ・MS5607.h
mikawataru 7:5f693667d0e9 18 https://developer.mbed.org/users/yamaguch/code/MS5607/
mikawataru 7:5f693667d0e9 19 ・SDFileSystem.h
mikawataru 7:5f693667d0e9 20 https://developer.mbed.org/teams/mbed/code/SDFileSystem/
mikawataru 7:5f693667d0e9 21
mikawataru 5:bcf55d8fe7a7 22 ピン配置
mikawataru 5:bcf55d8fe7a7 23 LPC1768 MPU6050 MS5607 MicroSD　MG995_1 MG995_2 MG995_3 MG995_4
mikawataru 5:bcf55d8fe7a7 24 VIN--------------------------------------------------------------------4.5~9.0V
mikawataru 5:bcf55d8fe7a7 25 -----------------------------------VIN------VIN------VIN------VIN------5V
mikawataru 5:bcf55d8fe7a7 26 Vout-----VCC------VIN-----VDD------------------------------------------3.3V
mikawataru 6:15204813ef87 27 \-ADO \-PS
mikawataru 5:bcf55d8fe7a7 28 GND------GND------GND-----VSS------GND------GND------GND------GND------0V
mikawataru 6:15204813ef87 29 \-CS
mikawataru 5:bcf55d8fe7a7 30 p9-------SDA------SDA----------------------------------------------------
mikawataru 5:bcf55d8fe7a7 31 p10------SCL------SCL----------------------------------------------------
mikawataru 5:bcf55d8fe7a7 32 p11-----------------------CLK--------------------------------------------
mikawataru 5:bcf55d8fe7a7 33 p12-----------------------DAT0-------------------------------------------
mikawataru 5:bcf55d8fe7a7 34 p13-----------------------CMD--------------------------------------------
mikawataru 5:bcf55d8fe7a7 35 p14-----------------------CD/DAT0----------------------------------------
mikawataru 5:bcf55d8fe7a7 36 p23--------------------------------CTRL----------------------------------
mikawataru 5:bcf55d8fe7a7 37 p24-----------------------------------------CTRL-------------------------
mikawataru 5:bcf55d8fe7a7 38 p25--------------------------------------------------CTRL----------------
mikawataru 5:bcf55d8fe7a7 39 p26-----------------------------------------------------------CTRL-------
mikawataru 5:bcf55d8fe7a7 40 */
mikawataru 0:10a9c9c5ce83 41 #include "mbed.h"
mikawataru 7:5f693667d0e9 42 #include "math.h"
mikawataru 0:10a9c9c5ce83 43 #include "MS5607I2C.h"
mikawataru 0:10a9c9c5ce83 44 #include "MPU6050.h"
mikawataru 1:ec75f428c6b3 45 #include "SDFileSystem.h"
mikawataru 14:cbee1bfdfca7 46
mikawataru 15:01d3969b89d0 47 #define ACC_LAUNCH 0//FIXME:本番は4g
mikawataru 14:cbee1bfdfca7 48 #define TOP_DROP_AMOUNT 1.5
mikawataru 14:cbee1bfdfca7 49 #define ALT_UAV_DROP 150
mikawataru 14:cbee1bfdfca7 50 #define TIME_REACH_TOP 15
mikawataru 14:cbee1bfdfca7 51
mikawataru 14:cbee1bfdfca7 52 #define RATE_LOG 10
mikawataru 14:cbee1bfdfca7 53 #define RATE_OPEN 10
mikawataru 3:9cd74af355cc 54 /*サーボ動作*/
mikawataru 14:cbee1bfdfca7 55 #define LOCK 0.0005
mikawataru 14:cbee1bfdfca7 56 #define UNLOCK 0.0015
mikawataru 14:cbee1bfdfca7 57 /*モード定義*/
mikawataru 14:cbee1bfdfca7 58 #define STANDBY 0
mikawataru 14:cbee1bfdfca7 59 #define TEST 1
mikawataru 14:cbee1bfdfca7 60 #define FLIGHT 2
mikawataru 14:cbee1bfdfca7 61 /*開放フェーズ定義*/
mikawataru 14:cbee1bfdfca7 62 #define SETUP 0
mikawataru 14:cbee1bfdfca7 63 #define LAUNCH 1
mikawataru 14:cbee1bfdfca7 64 #define RISE 2
mikawataru 14:cbee1bfdfca7 65 #define DROP 3
mikawataru 12:211d2c6d2afc 66 /**/
mikawataru 14:cbee1bfdfca7 67 #define P0 1013.25f//海面気圧[hPa]
mikawataru 14:cbee1bfdfca7 68 #define ACC 4096.0f//加速度オフセット値
mikawataru 7:5f693667d0e9 69 /*ピン指定*/
mikawataru 14:cbee1bfdfca7 70 MS5607I2C ms5607(p9, p10, false);
mikawataru 14:cbee1bfdfca7 71 MPU6050 mpu(p9,p10);
mikawataru 14:cbee1bfdfca7 72 BusOut myled(LED1,LED2,LED3,LED4);
mikawataru 14:cbee1bfdfca7 73 SDFileSystem sd(p11, p12, p13, p14, "sd");
mikawataru 14:cbee1bfdfca7 74 Serial device(USBTX, USBRX);
mikawataru 14:cbee1bfdfca7 75 PwmOut Door_1_1(p23);
mikawataru 14:cbee1bfdfca7 76 PwmOut Door_1_2(p24);
mikawataru 14:cbee1bfdfca7 77 PwmOut Door_2_1(p25);
mikawataru 14:cbee1bfdfca7 78 PwmOut Door_2_2(p26);
mikawataru 7:5f693667d0e9 79 /*タイマー類*/
mikawataru 14:cbee1bfdfca7 80 Timer timer;
mikawataru 3:9cd74af355cc 81 Ticker loop_log;
mikawataru 3:9cd74af355cc 82 Ticker loop_open;
mikawataru 5:bcf55d8fe7a7 83 /*ログカウンタ*/
mikawataru 14:cbee1bfdfca7 84 bool row = 0;
mikawataru 3:9cd74af355cc 85 int8_t col = 0;
mikawataru 5:bcf55d8fe7a7 86 /*ログ格納用*/
mikawataru 11:b61d0fcc2ed3 87 float altitude[2][RATE_LOG],pressure[2][RATE_LOG],temperature[2][RATE_LOG];
mikawataru 3:9cd74af355cc 88 float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3];
mikawataru 3:9cd74af355cc 89 float t[2][RATE_LOG];
mikawataru 7:5f693667d0e9 90 FILE *fp;
mikawataru 11:b61d0fcc2ed3 91 /*フェーズ変数*/
mikawataru 14:cbee1bfdfca7 92 int8_t Phase = SETUP;
mikawataru 14:cbee1bfdfca7 93 int8_t Mode = STANDBY;
mikawataru 7:5f693667d0e9 94 /*判定用*/
mikawataru 14:cbee1bfdfca7 95 float alt_buff[RATE_OPEN];
mikawataru 14:cbee1bfdfca7 96 float alt_max,alt_launch;
mikawataru 14:cbee1bfdfca7 97 float t_drop,t_top,t_launch;
mikawataru 11:b61d0fcc2ed3 98 int8_t cnt_judge = 0;
mikawataru 14:cbee1bfdfca7 99 int8_t col_open = 0;
mikawataru 12:211d2c6d2afc 100 /*入力用*/
mikawataru 12:211d2c6d2afc 101 int8_t input_buff[3] = {};
mikawataru 14:cbee1bfdfca7 102 int8_t input_cnt = 0;
mikawataru 7:5f693667d0e9 103 /*関数*/
mikawataru 14:cbee1bfdfca7 104 void _Open();
mikawataru 14:cbee1bfdfca7 105 void _Servo(int8_t door_num, float motion);
mikawataru 14:cbee1bfdfca7 106 void _Log();
mikawataru 14:cbee1bfdfca7 107 void _Input();
mikawataru 14:cbee1bfdfca7 108 float _Measure_Alt(float press, float temp);
mikawataru 11:b61d0fcc2ed3 109 float _Median(float data[], int num);
mikawataru 11:b61d0fcc2ed3 110
mikawataru 7:5f693667d0e9 111 /*---------------------------------------------------------------------*/
mikawataru 0:10a9c9c5ce83 112 int main() {
mikawataru 11:b61d0fcc2ed3 113 mpu.setAcceleroRange(2);
mikawataru 11:b61d0fcc2ed3 114 mpu.setGyroRange(2);
mikawataru 1:ec75f428c6b3 115 timer.start();
mikawataru 5:bcf55d8fe7a7 116 Door_1_1.period_ms(20);
mikawataru 5:bcf55d8fe7a7 117 Door_1_2.period_ms(20);
mikawataru 5:bcf55d8fe7a7 118 Door_2_1.period_ms(20);
mikawataru 5:bcf55d8fe7a7 119 Door_2_2.period_ms(20);
mikawataru 6:15204813ef87 120 _Servo(0,LOCK);
mikawataru 2:b6eb08d059cc 121 fp = fopen("/sd/log.txt", "w");
mikawataru 1:ec75f428c6b3 122 fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
mikawataru 12:211d2c6d2afc 123 device.attach(&_Input,Serial::RxIrq);
mikawataru 15:01d3969b89d0 124 loop_open.attach(&_Open,1.0/RATE_OPEN);
mikawataru 3:9cd74af355cc 125 while(1);
mikawataru 3:9cd74af355cc 126 }
mikawataru 6:15204813ef87 127 /*開放用関数　RATE_OPEN[Hz]で判定を行う*/
mikawataru 3:9cd74af355cc 128 void _Open(){
mikawataru 3:9cd74af355cc 129 myled = 1 << (Phase-1);
mikawataru 15:01d3969b89d0 130 // device.printf("%f,%f,%f\r\n",alt_buff[col_open],alt_launch,alt_max);
mikawataru 3:9cd74af355cc 131 switch (Phase) {
mikawataru 14:cbee1bfdfca7 132 case SETUP://セットアップモード（発射判定不可）
mikawataru 3:9cd74af355cc 133 break;
mikawataru 7:5f693667d0e9 134 case LAUNCH://点火モード（発射判定可）
mikawataru 14:cbee1bfdfca7 135 float acc_buff = (float)mpu.getAcceleroRawZ()/(ACC*0.981);
mikawataru 14:cbee1bfdfca7 136 alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature());
mikawataru 14:cbee1bfdfca7 137 if(acc_buff>ACC_LAUNCH){
mikawataru 11:b61d0fcc2ed3 138 if(cnt_judge++==3){
mikawataru 14:cbee1bfdfca7 139 Phase = RISE;
mikawataru 14:cbee1bfdfca7 140 alt_launch = _Median(alt_buff, RATE_OPEN);
mikawataru 11:b61d0fcc2ed3 141 cnt_judge = 0;
mikawataru 11:b61d0fcc2ed3 142 }
mikawataru 7:5f693667d0e9 143 t_launch = timer.read();
mikawataru 11:b61d0fcc2ed3 144 alt_max = alt_launch;
mikawataru 7:5f693667d0e9 145 }else{
mikawataru 11:b61d0fcc2ed3 146 if(timer.read()>t_launch+1.0) cnt_judge = 0;
mikawataru 3:9cd74af355cc 147 }
mikawataru 4:cc266df87f3e 148 break;
mikawataru 14:cbee1bfdfca7 149 case RISE://上昇中（パラシュート開放判定）
mikawataru 14:cbee1bfdfca7 150 alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature());
mikawataru 14:cbee1bfdfca7 151 if(alt_buff[col_open]>alt_max) alt_max = alt_buff[col_open];
mikawataru 14:cbee1bfdfca7 152 if(alt_buff[col_open]<alt_max-TOP_DROP_AMOUNT){
mikawataru 11:b61d0fcc2ed3 153 if(cnt_judge++==3){
mikawataru 7:5f693667d0e9 154 _Servo(1,UNLOCK);
mikawataru 7:5f693667d0e9 155 Phase = DROP;
mikawataru 11:b61d0fcc2ed3 156 cnt_judge = 0;
mikawataru 7:5f693667d0e9 157 }
mikawataru 7:5f693667d0e9 158 t_top = timer.read();
mikawataru 7:5f693667d0e9 159 }else{
mikawataru 11:b61d0fcc2ed3 160 if(timer.read()>t_top+1.0) cnt_judge = 0;
mikawataru 7:5f693667d0e9 161 }
mikawataru 14:cbee1bfdfca7 162 if(timer.read()-t_launch>TIME_REACH_TOP){
mikawataru 7:5f693667d0e9 163 _Servo(1,UNLOCK);
mikawataru 7:5f693667d0e9 164 Phase = DROP;
mikawataru 11:b61d0fcc2ed3 165 cnt_judge = 0;
mikawataru 7:5f693667d0e9 166 }
mikawataru 7:5f693667d0e9 167 break;
mikawataru 7:5f693667d0e9 168 case DROP://降下中（缶サット開放判定)
mikawataru 14:cbee1bfdfca7 169 if(alt_buff[col_open] < alt_launch+ALT_UAV_DROP){
mikawataru 11:b61d0fcc2ed3 170 if(cnt_judge++==3){
mikawataru 11:b61d0fcc2ed3 171 _Servo(2,UNLOCK);
mikawataru 11:b61d0fcc2ed3 172 cnt_judge = 0;
mikawataru 11:b61d0fcc2ed3 173 }
mikawataru 7:5f693667d0e9 174 t_drop = timer.read();
mikawataru 7:5f693667d0e9 175 }else{
mikawataru 11:b61d0fcc2ed3 176 if(timer.read()>t_drop+1.0)cnt_judge = 0;
mikawataru 7:5f693667d0e9 177 }
mikawataru 3:9cd74af355cc 178 break;
mikawataru 0:10a9c9c5ce83 179 }
mikawataru 11:b61d0fcc2ed3 180 if(col_open++==RATE_OPEN) col_open = 0;
mikawataru 0:10a9c9c5ce83 181 }
mikawataru 6:15204813ef87 182 /*記録用関数　RATE_LOG[Hz]で記録を行う*/
mikawataru 3:9cd74af355cc 183 void _Log(){
mikawataru 14:cbee1bfdfca7 184 t[row][col] = timer.read();
mikawataru 14:cbee1bfdfca7 185 pressure[row][col] = ms5607.getPressure()/100;
mikawataru 3:9cd74af355cc 186 temperature[row][col] = ms5607.getTemperature();
mikawataru 3:9cd74af355cc 187 mpu.getAccelero(&acc[row][col][0]);
mikawataru 3:9cd74af355cc 188 mpu.getGyro(&gyro[row][col][0]);
mikawataru 3:9cd74af355cc 189 fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
mikawataru 11:b61d0fcc2ed3 190 t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0]/10.0,
mikawataru 11:b61d0fcc2ed3 191 acc[row][col][1]/10.0,acc[row][col][2]/10.0,gyro[row][col][0],gyro[row][col][1],gyro[row][col][2]
mikawataru 3:9cd74af355cc 192 );
mikawataru 3:9cd74af355cc 193 if(col++==RATE_LOG){
mikawataru 3:9cd74af355cc 194 fclose(fp);
mikawataru 14:cbee1bfdfca7 195 fp = fopen("/sd/log.txt", "a");
mikawataru 3:9cd74af355cc 196 row =! row;
mikawataru 14:cbee1bfdfca7 197 col = 0;
mikawataru 3:9cd74af355cc 198 }
mikawataru 3:9cd74af355cc 199 }
mikawataru 6:15204813ef87 200 /*サーボ動作用関数 _Servo(int8_t 扉番号,float pwm波長)*/
mikawataru 15:01d3969b89d0 201 // TODO:サーボ回転量の調整
mikawataru 6:15204813ef87 202 void _Servo(int8_t door_num, float motion){
mikawataru 6:15204813ef87 203 if(door_num==0){//全扉
mikawataru 6:15204813ef87 204 Door_1_1.pulsewidth(motion);
mikawataru 6:15204813ef87 205 Door_1_2.pulsewidth(motion);
mikawataru 6:15204813ef87 206 Door_2_1.pulsewidth(motion);
mikawataru 6:15204813ef87 207 Door_2_2.pulsewidth(motion);
mikawataru 6:15204813ef87 208 }else if(door_num==1){//扉１
mikawataru 6:15204813ef87 209 Door_1_1.pulsewidth(motion);
mikawataru 15:01d3969b89d0 210 wait(0.1);
mikawataru 6:15204813ef87 211 Door_1_2.pulsewidth(motion);
mikawataru 6:15204813ef87 212 }else if(door_num==2){//扉２
mikawataru 6:15204813ef87 213 Door_2_1.pulsewidth(motion);
mikawataru 15:01d3969b89d0 214 wait(0.1);
mikawataru 6:15204813ef87 215 Door_2_2.pulsewidth(motion);
mikawataru 6:15204813ef87 216 }else{
mikawataru 12:211d2c6d2afc 217 device.printf("servo error:%d\r\n",door_num);
mikawataru 6:15204813ef87 218 }
mikawataru 3:9cd74af355cc 219 }
mikawataru 7:5f693667d0e9 220
mikawataru 12:211d2c6d2afc 221 /*入力用*/
mikawataru 15:01d3969b89d0 222 // TODO:テストモード実装
mikawataru 15:01d3969b89d0 223 // FIXME:android端末だと入力崩れが起こる
mikawataru 15:01d3969b89d0 224 // FIXME:サーボ入力がたぶん逆
mikawataru 12:211d2c6d2afc 225 void _Input(){
mikawataru 14:cbee1bfdfca7 226 input_buff[input_cnt] = device.getc();
mikawataru 14:cbee1bfdfca7 227 device.printf("\r\n");
mikawataru 14:cbee1bfdfca7 228 device.printf("input_cnt:%d\r\n",input_cnt);
mikawataru 15:01d3969b89d0 229 device.printf("input_buff:%s\r\n",input_buff);
mikawataru 14:cbee1bfdfca7 230 switch (Mode) {
mikawataru 14:cbee1bfdfca7 231 case STANDBY:
mikawataru 14:cbee1bfdfca7 232 if(input_cnt==0){
mikawataru 14:cbee1bfdfca7 233 if(input_buff[0]=='S'){
mikawataru 14:cbee1bfdfca7 234 device.printf("P >> Parachute\r\n");
mikawataru 14:cbee1bfdfca7 235 device.printf("C >> CanSat\r\n");
mikawataru 14:cbee1bfdfca7 236 }else if(input_buff[0]=='M'){
mikawataru 14:cbee1bfdfca7 237 device.printf("S >> STANDBY\r\n");
mikawataru 14:cbee1bfdfca7 238 device.printf("T >> TEST\r\n");
mikawataru 14:cbee1bfdfca7 239 device.printf("F >> FLIGHT\r\n");
mikawataru 14:cbee1bfdfca7 240 }else{
mikawataru 14:cbee1bfdfca7 241 device.printf("This command is not found >> %c\r\n",input_buff[0]);
mikawataru 14:cbee1bfdfca7 242 device.printf(">>MAINMENU<<\r\n");
mikawataru 14:cbee1bfdfca7 243 device.printf("S >> Servo Operation\r\n");
mikawataru 14:cbee1bfdfca7 244 device.printf("M >> Mode Change\r\n");
mikawataru 14:cbee1bfdfca7 245 device.printf("-->>");
mikawataru 14:cbee1bfdfca7 246 return;
mikawataru 14:cbee1bfdfca7 247 }
mikawataru 14:cbee1bfdfca7 248 }else if(input_cnt==1){
mikawataru 14:cbee1bfdfca7 249 if(input_buff[0]=='S'){
mikawataru 14:cbee1bfdfca7 250 if(input_buff[1]=='P'||input_buff[1]=='C'){
mikawataru 14:cbee1bfdfca7 251 device.printf("U >> UNLOCK\r\n");
mikawataru 14:cbee1bfdfca7 252 device.printf("L >> LOCK\r\n");
mikawataru 14:cbee1bfdfca7 253 }else{
mikawataru 14:cbee1bfdfca7 254 device.printf("This command is not found >> %c\r\n",input_buff[1]);
mikawataru 14:cbee1bfdfca7 255 device.printf("P >> Parachute\r\n");
mikawataru 14:cbee1bfdfca7 256 device.printf("C >> CanSat\r\n");
mikawataru 14:cbee1bfdfca7 257 device.printf("-->>");
mikawataru 14:cbee1bfdfca7 258 return;
mikawataru 14:cbee1bfdfca7 259 }
mikawataru 14:cbee1bfdfca7 260 }else if(input_buff[0]=='M'){
mikawataru 14:cbee1bfdfca7 261 if(input_buff[1]=='S'){
mikawataru 14:cbee1bfdfca7 262 Mode = STANDBY;
mikawataru 14:cbee1bfdfca7 263 device.printf(">>MAINMENU<<\r\n");
mikawataru 14:cbee1bfdfca7 264 device.printf("S >> Servo Operation\r\n");
mikawataru 14:cbee1bfdfca7 265 device.printf("M >> Mode Change\r\n");
mikawataru 14:cbee1bfdfca7 266 device.printf("-->>");
mikawataru 14:cbee1bfdfca7 267 }else if(input_buff[1]=='T'){
mikawataru 14:cbee1bfdfca7 268 Mode = TEST;
mikawataru 15:01d3969b89d0 269 Phase = RISE;
mikawataru 15:01d3969b89d0 270 alt_launch = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature());
mikawataru 14:cbee1bfdfca7 271 }else if(input_buff[1]=='F'){
mikawataru 14:cbee1bfdfca7 272 Mode = FLIGHT;
mikawataru 15:01d3969b89d0 273 Phase = LAUNCH;
mikawataru 15:01d3969b89d0 274 loop_log.attach(&_Log,1.0/RATE_LOG);
mikawataru 14:cbee1bfdfca7 275 device.printf("FLIGHT-Mode ON!\r\n");
mikawataru 14:cbee1bfdfca7 276 device.printf("***alert***\r\n");
mikawataru 14:cbee1bfdfca7 277 device.printf("You will be able to reset only!\r\n");
mikawataru 14:cbee1bfdfca7 278 }else{
mikawataru 14:cbee1bfdfca7 279 device.printf("This command is not found >> %c\r\n",input_buff[1]);
mikawataru 14:cbee1bfdfca7 280 device.printf("S >> STANDBY\r\n");
mikawataru 14:cbee1bfdfca7 281 device.printf("T >> TEST\r\n");
mikawataru 14:cbee1bfdfca7 282 device.printf("F >> FLIGHT\r\n");
mikawataru 14:cbee1bfdfca7 283 device.printf("-->>");
mikawataru 14:cbee1bfdfca7 284 return;
mikawataru 14:cbee1bfdfca7 285 }
mikawataru 14:cbee1bfdfca7 286 input_cnt = 0;
mikawataru 14:cbee1bfdfca7 287 }
mikawataru 14:cbee1bfdfca7 288 }else if(input_cnt==2){
mikawataru 14:cbee1bfdfca7 289 if(input_buff[2]=='U'){
mikawataru 14:cbee1bfdfca7 290 if(input_buff[1]=='P') _Servo(1,UNLOCK);
mikawataru 14:cbee1bfdfca7 291 if(input_buff[1]=='C') _Servo(2,UNLOCK);
mikawataru 14:cbee1bfdfca7 292 }else if(input_buff[2]=='L'){
mikawataru 14:cbee1bfdfca7 293 if(input_buff[1]=='P') _Servo(1,LOCK);
mikawataru 14:cbee1bfdfca7 294 if(input_buff[1]=='C') _Servo(2,LOCK);
mikawataru 14:cbee1bfdfca7 295 }else{
mikawataru 14:cbee1bfdfca7 296 device.printf("This command is not found >> %c\r\n",input_buff[2]);
mikawataru 14:cbee1bfdfca7 297 device.printf("U >> UNLOCK\r\n");
mikawataru 14:cbee1bfdfca7 298 device.printf("L >> LOCK\r\n");
mikawataru 14:cbee1bfdfca7 299 device.printf("-->>");
mikawataru 14:cbee1bfdfca7 300 return;
mikawataru 14:cbee1bfdfca7 301 }
mikawataru 14:cbee1bfdfca7 302 input_cnt = 0;
mikawataru 14:cbee1bfdfca7 303 device.printf(">>MAINMENU<<\r\n");
mikawataru 14:cbee1bfdfca7 304 device.printf("S >> Servo Operation\r\n");
mikawataru 14:cbee1bfdfca7 305 device.printf("M >> Mode Change\r\n");
mikawataru 14:cbee1bfdfca7 306 device.printf("-->>");
mikawataru 14:cbee1bfdfca7 307 return;
mikawataru 12:211d2c6d2afc 308 }
mikawataru 14:cbee1bfdfca7 309 device.printf("-->>");
mikawataru 14:cbee1bfdfca7 310 input_cnt++;
mikawataru 12:211d2c6d2afc 311 break;
mikawataru 14:cbee1bfdfca7 312 case TEST:
mikawataru 12:211d2c6d2afc 313 break;
mikawataru 14:cbee1bfdfca7 314 case FLIGHT://reset only
mikawataru 12:211d2c6d2afc 315 break;
mikawataru 12:211d2c6d2afc 316 }
mikawataru 12:211d2c6d2afc 317 }
mikawataru 12:211d2c6d2afc 318
mikawataru 12:211d2c6d2afc 319 /*その他雑関数*/
mikawataru 14:cbee1bfdfca7 320 float _Measure_Alt(float press/*[hPa]*/, float temp/*[℃]*/){
mikawataru 14:cbee1bfdfca7 321 return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
mikawataru 7:5f693667d0e9 322 }
mikawataru 14:cbee1bfdfca7 323 float _Median(float data[], int num){
mikawataru 11:b61d0fcc2ed3 324 float median;
mikawataru 11:b61d0fcc2ed3 325 float *sort = (float *)malloc(sizeof(float)*num);
mikawataru 11:b61d0fcc2ed3 326 for(int i=0; i<num; i++) sort[i] = data[i];
mikawataru 11:b61d0fcc2ed3 327 for(int i=0; i<num; i++){
mikawataru 11:b61d0fcc2ed3 328 for(int j=0; j<num-i-1; j++){
mikawataru 11:b61d0fcc2ed3 329 if(sort[j]>sort[j+1]){
mikawataru 11:b61d0fcc2ed3 330 float buff = sort[j+1];
mikawataru 11:b61d0fcc2ed3 331 sort[j+1] = sort[j];
mikawataru 11:b61d0fcc2ed3 332 sort[j] = buff;
mikawataru 11:b61d0fcc2ed3 333 }
mikawataru 11:b61d0fcc2ed3 334 }
mikawataru 11:b61d0fcc2ed3 335 }
mikawataru 11:b61d0fcc2ed3 336 if(num%2!=0)median = sort[num/2];
mikawataru 11:b61d0fcc2ed3 337 else median = (sort[num/2-1]+sort[num/2])/2.0;
mikawataru 11:b61d0fcc2ed3 338 free(sort);
mikawataru 11:b61d0fcc2ed3 339 return median;
mikawataru 11:b61d0fcc2ed3 340 }