CORE / Mbed 2 deprecated 20170321_MUSASABI

ムササビチームの電装です

Dependencies:   MPU6050 MS5607 SDFileSystem mbed

main.cpp@0:b021d725d528, 2017-04-05 (annotated)

Committer:
makakensanba
Date:
Wed Apr 05 10:45:35 2017 +0000
Revision:
0:b021d725d528
?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
makakensanba 0:b021d725d528 1 /*
makakensanba 0:b021d725d528 2 2017年3月　伊豆大島共同打上実験
makakensanba 0:b021d725d528 3 団体名：CORE
makakensanba 0:b021d725d528 4 チーム名：ムササビ
makakensanba 0:b021d725d528 5 該当電装：ロケット搭載用
makakensanba 0:b021d725d528 6
makakensanba 0:b021d725d528 7 使用部品
makakensanba 0:b021d725d528 8 ・LPC1768(マイコン)
makakensanba 0:b021d725d528 9 ・MPU6050(加速度・ジャイロセンサ)
makakensanba 0:b021d725d528 10 ・MS5607(気圧・気温センサ)
makakensanba 0:b021d725d528 11 ・MicroSDスロット
makakensanba 0:b021d725d528 12 ・MG995(サーボモータ)×4
makakensanba 0:b021d725d528 13
makakensanba 0:b021d725d528 14 使用ライブラリ
makakensanba 0:b021d725d528 15 ・MPU6050.h
makakensanba 0:b021d725d528 16 https://developer.mbed.org/teams/mbed/code/SDFileSystem/
makakensanba 0:b021d725d528 17 ・MS5607.h
makakensanba 0:b021d725d528 18 https://developer.mbed.org/users/yamaguch/code/MS5607/
makakensanba 0:b021d725d528 19 ・SDFileSystem.h
makakensanba 0:b021d725d528 20 https://developer.mbed.org/teams/mbed/code/SDFileSystem/
makakensanba 0:b021d725d528 21 */
makakensanba 0:b021d725d528 22 #include "mbed.h"
makakensanba 0:b021d725d528 23 #include "math.h"
makakensanba 0:b021d725d528 24 #include "MS5607I2C.h"
makakensanba 0:b021d725d528 25 #include "MPU6050.h"
makakensanba 0:b021d725d528 26 #include "SDFileSystem.h"
makakensanba 0:b021d725d528 27
makakensanba 0:b021d725d528 28 #define ACC_LAUNCH 4.0f//FIXME:本番は4g
makakensanba 0:b021d725d528 29 #define TOP_DROP_AMOUNT 1.5f
makakensanba 0:b021d725d528 30 #define TIME_REACH_TOP 12.5f
makakensanba 0:b021d725d528 31
makakensanba 0:b021d725d528 32 #define RATE_LOG 1
makakensanba 0:b021d725d528 33 #define RATE_OPEN 10
makakensanba 0:b021d725d528 34 /*サーボ動作*/
makakensanba 0:b021d725d528 35 #define LOCK 0
makakensanba 0:b021d725d528 36 #define UNLOCK 1
makakensanba 0:b021d725d528 37 /*モード定義*/
makakensanba 0:b021d725d528 38 #define STANDBY 0
makakensanba 0:b021d725d528 39 #define TEST 1
makakensanba 0:b021d725d528 40 #define FLIGHT 2
makakensanba 0:b021d725d528 41 /*開放フェーズ定義*/
makakensanba 0:b021d725d528 42 #define SETUP 0
makakensanba 0:b021d725d528 43 #define LAUNCH 1
makakensanba 0:b021d725d528 44 #define RISE 2
makakensanba 0:b021d725d528 45 #define DROP 3
makakensanba 0:b021d725d528 46 /**/
makakensanba 0:b021d725d528 47 /*DCモータ動作*/
makakensanba 0:b021d725d528 48 #define FORW 0
makakensanba 0:b021d725d528 49 #define STOP 1
makakensanba 0:b021d725d528 50 #define BACK 2
makakensanba 0:b021d725d528 51 #define P0 1013.25f//海面気圧[hPa]
makakensanba 0:b021d725d528 52 #define ACC 4096.0f//加速度オフセット値
makakensanba 0:b021d725d528 53
makakensanba 0:b021d725d528 54 /*目標地点の座標*/
makakensanba 0:b021d725d528 55 #define L_HOKUI 34.73416 //ムササビチーム目標地点
makakensanba 0:b021d725d528 56 #define L_TOUKEI 139.4227
makakensanba 0:b021d725d528 57 #define ECC2 0.00669437999019758
makakensanba 0:b021d725d528 58 #define A_RADIUS 6378137.000 //長半径(赤道半径)[m]
makakensanba 0:b021d725d528 59 #define B_RADIUS 6356752.314245 //短半径(極半径)(WGS84)->http://yamadarake.jp/trdi/report000001.html
makakensanba 0:b021d725d528 60 #define M_PI 3.1415926535897932384626433832795
makakensanba 0:b021d725d528 61 #define M_PI_2 1.57079632679489661923
makakensanba 0:b021d725d528 62 #define epsilon 1.0E-6
makakensanba 0:b021d725d528 63
makakensanba 0:b021d725d528 64 /*ピン指定*/
makakensanba 0:b021d725d528 65 MS5607I2C ms5607(p9, p10, false);
makakensanba 0:b021d725d528 66 MPU6050 mpu(p9,p10);
makakensanba 0:b021d725d528 67 BusOut myled(LED1,LED2,LED3,LED4);
makakensanba 0:b021d725d528 68 SDFileSystem sd(p5, p6, p7, p8, "sd");
makakensanba 0:b021d725d528 69 Serial device(USBTX, USBRX);
makakensanba 0:b021d725d528 70 DigitalIn flightpin(p19);
makakensanba 0:b021d725d528 71 DigitalOut RED(p30);
makakensanba 0:b021d725d528 72 Serial twe(p13,p14);
makakensanba 0:b021d725d528 73 Serial gps(p28,p27); // tx, rx
makakensanba 0:b021d725d528 74 PwmOut Door_UP(p21);
makakensanba 0:b021d725d528 75 PwmOut Door_BOTTOM(p22);
makakensanba 0:b021d725d528 76 PwmOut DC(p23);
makakensanba 0:b021d725d528 77
makakensanba 0:b021d725d528 78
makakensanba 0:b021d725d528 79 /*タイマー類*/
makakensanba 0:b021d725d528 80 Timer timer;
makakensanba 0:b021d725d528 81 Ticker loop_log;
makakensanba 0:b021d725d528 82 Ticker loop_open;
makakensanba 0:b021d725d528 83 /*ログカウンタ*/
makakensanba 0:b021d725d528 84 bool row = 0;
makakensanba 0:b021d725d528 85 int8_t col = 0;
makakensanba 0:b021d725d528 86 /*ログ格納用*/
makakensanba 0:b021d725d528 87 float alt_drop;
makakensanba 0:b021d725d528 88 FILE *fp;
makakensanba 0:b021d725d528 89 /*フェーズ変数*/
makakensanba 0:b021d725d528 90 int8_t Phase = SETUP;
makakensanba 0:b021d725d528 91 int8_t Mode = STANDBY;
makakensanba 0:b021d725d528 92 char flight[5] = {};
makakensanba 0:b021d725d528 93 int i_f =0;
makakensanba 0:b021d725d528 94 /*判定用*/
makakensanba 0:b021d725d528 95 float alt_buff[RATE_OPEN];
makakensanba 0:b021d725d528 96 static float alt_max,alt_launch;
makakensanba 0:b021d725d528 97 float t_drop,t_top,t_launch;
makakensanba 0:b021d725d528 98 int8_t cnt_judge = 0;
makakensanba 0:b021d725d528 99 int8_t col_open = 0;
makakensanba 0:b021d725d528 100 /*入力用*/
makakensanba 0:b021d725d528 101 int8_t input_buff[2] = {};
makakensanba 0:b021d725d528 102 int8_t input_cnt = 0;
makakensanba 0:b021d725d528 103 int u;
makakensanba 0:b021d725d528 104
makakensanba 0:b021d725d528 105 /*GPS変数*/
makakensanba 0:b021d725d528 106 const float dt =0.06;
makakensanba 0:b021d725d528 107 int i,mode,Step;
makakensanba 0:b021d725d528 108 int j = 0;
makakensanba 0:b021d725d528 109 char gps_data[256];
makakensanba 0:b021d725d528 110 char st,ns,ew;
makakensanba 0:b021d725d528 111 float w_time,hokui,tokei;
makakensanba 0:b021d725d528 112 float vel_norm,vel_head;
makakensanba 0:b021d725d528 113
makakensanba 0:b021d725d528 114 float g_hokui,g_tokei;
makakensanba 0:b021d725d528 115 float d_hokui,m_hokui,d_tokei,m_tokei;
makakensanba 0:b021d725d528 116 float def;
makakensanba 0:b021d725d528 117 unsigned char c;
makakensanba 0:b021d725d528 118 float dy, dx;
makakensanba 0:b021d725d528 119 float yAve,n,m,ecc,w;
makakensanba 0:b021d725d528 120 float xDist, yDist, dist;
makakensanba 0:b021d725d528 121 float x_tgt[3]= {166.7,-199.7,0.0};
makakensanba 0:b021d725d528 122
makakensanba 0:b021d725d528 123 /*関数*/
makakensanba 0:b021d725d528 124 void _Open();
makakensanba 0:b021d725d528 125 //void _Log();
makakensanba 0:b021d725d528 126 void _GPS();
makakensanba 0:b021d725d528 127 void _Motor(int8_t door_num, int8_t motion);
makakensanba 0:b021d725d528 128 void _Input();
makakensanba 0:b021d725d528 129 float _Measure_Alt(float press, float temp);
makakensanba 0:b021d725d528 130 float _Median(float data[], int num);
makakensanba 0:b021d725d528 131 int set_input(bool gps_good, float x_s[], float vg_knot, float hdg_gps, float alt_s, float alt_max);
makakensanba 0:b021d725d528 132 int control(float r_rel[], float vw[], float v[], float hdg, float omg, float dist_tgt);
makakensanba 0:b021d725d528 133 void filter_x(float dt, float C, float x_est[], float x_gps[], float vg_gps[]);
makakensanba 0:b021d725d528 134 void periodize(float &x, float max, float min);
makakensanba 0:b021d725d528 135 float hdg_conv(float hdgN0deg);
makakensanba 0:b021d725d528 136 void windstar(float Vw[], float Vg0[], float Vg1[], float Vg2[]);
makakensanba 0:b021d725d528 137 void segmentBisector(float a[], float v1[], float v2[]);
makakensanba 0:b021d725d528 138 int intersection(float r[], float l[], float m[]);
makakensanba 0:b021d725d528 139 float dot2(float a[], float b[]);
makakensanba 0:b021d725d528 140 float norm2(float v[]);
makakensanba 0:b021d725d528 141
makakensanba 0:b021d725d528 142 /*---------------------------------------------------------------------*/
makakensanba 0:b021d725d528 143 int main() {
makakensanba 0:b021d725d528 144 device.baud(115200);
makakensanba 0:b021d725d528 145 gps.baud(115200);
makakensanba 0:b021d725d528 146 twe.baud(115200);
makakensanba 0:b021d725d528 147 mpu.setAcceleroRange(2);
makakensanba 0:b021d725d528 148 mpu.setGyroRange(2);
makakensanba 0:b021d725d528 149 timer.start();
makakensanba 0:b021d725d528 150 Door_UP.period_ms(20);
makakensanba 0:b021d725d528 151 Door_BOTTOM.period_ms(20);
makakensanba 0:b021d725d528 152 DC.period_ms(20);
makakensanba 0:b021d725d528 153 ms5607.printCoefficients();
makakensanba 0:b021d725d528 154
makakensanba 0:b021d725d528 155 // _Motor(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定？
makakensanba 0:b021d725d528 156 // _Motor(2,UNLOCK);//
makakensanba 0:b021d725d528 157 fp = fopen("/sd/log.txt", "a");
makakensanba 0:b021d725d528 158 device.attach(&_Input,Serial::RxIrq);
makakensanba 0:b021d725d528 159 loop_open.attach(&_Open,1.0/RATE_OPEN);
makakensanba 0:b021d725d528 160 while(1);
makakensanba 0:b021d725d528 161 }
makakensanba 0:b021d725d528 162 /*開放用関数　RATE_OPEN[Hz]で判定を行う*/
makakensanba 0:b021d725d528 163 void _Open(){
makakensanba 0:b021d725d528 164 myled = 1 << (Phase-1);
makakensanba 0:b021d725d528 165 switch (Phase) {
makakensanba 0:b021d725d528 166 case SETUP://セットアップモード（発射判定不可）
makakensanba 0:b021d725d528 167 break;
makakensanba 0:b021d725d528 168 case LAUNCH://点火モード（発射判定可）
makakensanba 0:b021d725d528 169 device.printf("Phase:LAUNCH\r\n");
makakensanba 0:b021d725d528 170
makakensanba 0:b021d725d528 171 float acc_buffx2 = (float)mpu.getAcceleroRawX()/(ACC*0.981);
makakensanba 0:b021d725d528 172 float acc_buffy2 = (float)mpu.getAcceleroRawY()/(ACC*0.981);
makakensanba 0:b021d725d528 173 float acc_buffz2 = (float)mpu.getAcceleroRawZ()/(ACC*0.981);
makakensanba 0:b021d725d528 174
makakensanba 0:b021d725d528 175 float x_acc=acc_buffx2*acc_buffx2;
makakensanba 0:b021d725d528 176 float y_acc=acc_buffy2*acc_buffy2;
makakensanba 0:b021d725d528 177 float z_acc=acc_buffz2*acc_buffz2;
makakensanba 0:b021d725d528 178
makakensanba 0:b021d725d528 179 float acc_sum = (x_acc+y_acc+z_acc)*1.0;
makakensanba 0:b021d725d528 180 alt_buff[col_open] = ms5607.getAltitude();
makakensanba 0:b021d725d528 181 if(acc_sum>ACC_LAUNCH||flightpin==1){
makakensanba 0:b021d725d528 182 if(cnt_judge++==3){
makakensanba 0:b021d725d528 183 Phase = RISE;
makakensanba 0:b021d725d528 184 alt_launch = _Median(alt_buff, RATE_OPEN);
makakensanba 0:b021d725d528 185 cnt_judge = 0;
makakensanba 0:b021d725d528 186 }
makakensanba 0:b021d725d528 187 device.printf("launch:%f",alt_launch);
makakensanba 0:b021d725d528 188 t_launch = timer.read();
makakensanba 0:b021d725d528 189 alt_max = alt_launch;
makakensanba 0:b021d725d528 190 }else{
makakensanba 0:b021d725d528 191 if(timer.read()>t_launch+1.0) cnt_judge = 0;
makakensanba 0:b021d725d528 192 }
makakensanba 0:b021d725d528 193 break;
makakensanba 0:b021d725d528 194 case RISE://上昇中（パラシュート開放判定）
makakensanba 0:b021d725d528 195 device.printf("Phase:RISE\r\n");
makakensanba 0:b021d725d528 196 float alt_rising = ms5607.getAltitude();
makakensanba 0:b021d725d528 197 if( alt_rising>alt_max && alt_rising-alt_max < 10.0 ) alt_max = alt_rising;
makakensanba 0:b021d725d528 198 if(alt_rising<alt_max-TOP_DROP_AMOUNT){
makakensanba 0:b021d725d528 199 if(cnt_judge++==3){
makakensanba 0:b021d725d528 200 twe.printf("Phase:RISE.ALT Open.\r\n");
makakensanba 0:b021d725d528 201 gps.attach(&_GPS,Serial::RxIrq);
makakensanba 0:b021d725d528 202
makakensanba 0:b021d725d528 203 _Motor(1,UNLOCK);
makakensanba 0:b021d725d528 204 Phase = DROP;
makakensanba 0:b021d725d528 205 cnt_judge = 0;
makakensanba 0:b021d725d528 206 }
makakensanba 0:b021d725d528 207 t_top = timer.read();
makakensanba 0:b021d725d528 208 }else{
makakensanba 0:b021d725d528 209 if(timer.read()>t_top+1.0) cnt_judge = 0;
makakensanba 0:b021d725d528 210 }
makakensanba 0:b021d725d528 211 if(timer.read()-t_launch>TIME_REACH_TOP){
makakensanba 0:b021d725d528 212 _Motor(1,UNLOCK);
makakensanba 0:b021d725d528 213 gps.attach(&_GPS,Serial::RxIrq);
makakensanba 0:b021d725d528 214
makakensanba 0:b021d725d528 215 Phase = DROP;
makakensanba 0:b021d725d528 216 cnt_judge = 0;
makakensanba 0:b021d725d528 217 }
makakensanba 0:b021d725d528 218 break;
makakensanba 0:b021d725d528 219 case DROP://降下中（缶サット開放判定)
makakensanba 0:b021d725d528 220
makakensanba 0:b021d725d528 221 //device.printf("%d %c %f %f %f %f \n\r",u,st,vel_norm, vel_head, alt_drop, alt_max);
makakensanba 0:b021d725d528 222 //device.printf("Phase:DROP. Input:%d /r/n",u);
makakensanba 0:b021d725d528 223
makakensanba 0:b021d725d528 224 //_Measure alt 現在の高度 + dt->60ms
makakensanba 0:b021d725d528 225
makakensanba 0:b021d725d528 226 break;
makakensanba 0:b021d725d528 227 }
makakensanba 0:b021d725d528 228 if(col_open++==RATE_OPEN) col_open = 0;
makakensanba 0:b021d725d528 229 }
makakensanba 0:b021d725d528 230 /*
makakensanba 0:b021d725d528 231 void _Log(){
makakensanba 0:b021d725d528 232 if(t[row][col] = timer.read()>=30.0*60.0){
makakensanba 0:b021d725d528 233 timer.reset();
makakensanba 0:b021d725d528 234 t[row][col] = timer.read();
makakensanba 0:b021d725d528 235 }
makakensanba 0:b021d725d528 236 pressure[row][col] = ms5607.getPressure()/100;
makakensanba 0:b021d725d528 237 temperature[row][col] = ms5607.getTemperature();
makakensanba 0:b021d725d528 238 fprintf(fp,"%d,%f,%f\r\n",t[row][col],pressure[row][col],temperature[row][col]);
makakensanba 0:b021d725d528 239 if(col++==RATE_LOG){
makakensanba 0:b021d725d528 240 col = 0;
makakensanba 0:b021d725d528 241 row =! row;
makakensanba 0:b021d725d528 242 fclose(fp);
makakensanba 0:b021d725d528 243 fp = fopen("/sd/log.txt", "a");
makakensanba 0:b021d725d528 244 }
makakensanba 0:b021d725d528 245 }
makakensanba 0:b021d725d528 246 */
makakensanba 0:b021d725d528 247 void _Motor(int8_t num, int8_t motion) {
makakensanba 0:b021d725d528 248 if(num==1) { //扉
makakensanba 0:b021d725d528 249 if(motion==UNLOCK) {
makakensanba 0:b021d725d528 250 Door_UP.pulsewidth(0.0015);
makakensanba 0:b021d725d528 251 Door_BOTTOM.pulsewidth(0.0023);
makakensanba 0:b021d725d528 252 wait(1.0);
makakensanba 0:b021d725d528 253 Door_UP.pulsewidth(0);
makakensanba 0:b021d725d528 254 Door_BOTTOM.pulsewidth(0);
makakensanba 0:b021d725d528 255 } else if(motion==LOCK) {
makakensanba 0:b021d725d528 256 Door_UP.pulsewidth(0.0023);
makakensanba 0:b021d725d528 257 Door_BOTTOM.pulsewidth(0.0015);
makakensanba 0:b021d725d528 258 wait(1.0);
makakensanba 0:b021d725d528 259 Door_UP.pulsewidth(0);
makakensanba 0:b021d725d528 260 Door_BOTTOM.pulsewidth(0);
makakensanba 0:b021d725d528 261 } else {
makakensanba 0:b021d725d528 262 device.printf("error%f\r\n",motion);
makakensanba 0:b021d725d528 263 }
makakensanba 0:b021d725d528 264 } else if(num==2) { //DC
makakensanba 0:b021d725d528 265 if(motion==FORW) {
makakensanba 0:b021d725d528 266 DC.pulsewidth(0.001);
makakensanba 0:b021d725d528 267 wait(0.2);
makakensanba 0:b021d725d528 268 } else if(motion==STOP) {
makakensanba 0:b021d725d528 269 DC.pulsewidth(0.0015);
makakensanba 0:b021d725d528 270 wait(0.2);
makakensanba 0:b021d725d528 271 } else if(motion==BACK) {
makakensanba 0:b021d725d528 272 DC.pulsewidth(0.002);
makakensanba 0:b021d725d528 273 wait(0.2);
makakensanba 0:b021d725d528 274 } else {
makakensanba 0:b021d725d528 275 device.printf("error%f\r\n",motion);
makakensanba 0:b021d725d528 276 }
makakensanba 0:b021d725d528 277 } else {
makakensanba 0:b021d725d528 278 device.printf("Motor error:%d\r\n",num);
makakensanba 0:b021d725d528 279 }
makakensanba 0:b021d725d528 280 }
makakensanba 0:b021d725d528 281 void _GPS() {
makakensanba 0:b021d725d528 282 static int cnt=0;
makakensanba 0:b021d725d528 283 cnt++;
makakensanba 0:b021d725d528 284 RED =1;
makakensanba 0:b021d725d528 285 c = gps.getc();
makakensanba 0:b021d725d528 286 if( c=='$' || i == 256){
makakensanba 0:b021d725d528 287 mode = 0;
makakensanba 0:b021d725d528 288 i = 0;
makakensanba 0:b021d725d528 289 for(int j=0; j<256; j++){
makakensanba 0:b021d725d528 290 gps_data[j]=NULL;
makakensanba 0:b021d725d528 291 }
makakensanba 0:b021d725d528 292 }
makakensanba 0:b021d725d528 293 if(mode==0){
makakensanba 0:b021d725d528 294 if((gps_data[i]=c) != '\r'){
makakensanba 0:b021d725d528 295 i++;
makakensanba 0:b021d725d528 296 }else{
makakensanba 0:b021d725d528 297 gps_data[i]='\0';
makakensanba 0:b021d725d528 298
makakensanba 0:b021d725d528 299 if( sscanf(gps_data, "$GNRMC,%f,%c,%f,%c,%f,%c,%f,%f",&w_time,&st,&hokui,&ns,&tokei,&ew,&vel_norm,&vel_head) >= 1){
makakensanba 0:b021d725d528 300 //logitude
makakensanba 0:b021d725d528 301
makakensanba 0:b021d725d528 302 d_tokei= int(tokei/100);
makakensanba 0:b021d725d528 303 m_tokei= (tokei-d_tokei*100)/60;
makakensanba 0:b021d725d528 304 g_tokei= d_tokei+m_tokei;
makakensanba 0:b021d725d528 305 //Latitude
makakensanba 0:b021d725d528 306 d_hokui=int(hokui/100);
makakensanba 0:b021d725d528 307 m_hokui=(hokui-d_hokui*100)/60;
makakensanba 0:b021d725d528 308 g_hokui=d_hokui+m_hokui;
makakensanba 0:b021d725d528 309
makakensanba 0:b021d725d528 310 dy = (L_HOKUI-g_hokui)/180*M_PI;
makakensanba 0:b021d725d528 311 dx = (L_TOUKEI-g_tokei)/180*M_PI;
makakensanba 0:b021d725d528 312 yAve = (g_hokui+L_HOKUI)/2/180*M_PI;
makakensanba 0:b021d725d528 313 w = sqrt(1-ECC2*sin(yAve)*sin(yAve));
makakensanba 0:b021d725d528 314 m = A_RADIUS*(1-ECC2)/(w*w*w);
makakensanba 0:b021d725d528 315 n = A_RADIUS/w;
makakensanba 0:b021d725d528 316 dist = sqrt((dy*m)*(dy*m)+(dx*n*cos(yAve))*(dx*n*cos(yAve))/1);
makakensanba 0:b021d725d528 317 xDist = dx*n*cos(yAve);
makakensanba 0:b021d725d528 318 yDist = dy*m;
makakensanba 0:b021d725d528 319
makakensanba 0:b021d725d528 320 float alt = ms5607.getAltitude();
makakensanba 0:b021d725d528 321 //device.printf("Lon:%.6f, Lat:%.6f,vel_norm:%f,vel_head:%f\r\n",g_tokei, g_hokui,vel_norm,vel_head);
makakensanba 0:b021d725d528 322 device.printf("Radio \r\n");
makakensanba 0:b021d725d528 323
makakensanba 0:b021d725d528 324 /*
makakensanba 0:b021d725d528 325 static int k=0;
makakensanba 0:b021d725d528 326 if (k++ % 6 == 0){
makakensanba 0:b021d725d528 327 float alt = ms5607.getAltitude();
makakensanba 0:b021d725d528 328 //device.printf("Lon:%.6f, Lat:%.6f,vel_norm:%f,vel_head:%f\r\n",g_tokei, g_hokui,vel_norm,vel_head);
makakensanba 0:b021d725d528 329 device.printf("Radio \r\n");
makakensanba 0:b021d725d528 330
makakensanba 0:b021d725d528 331 twe.printf("%c,%f %f %f %f %f %d \r\n",Phase,hokui,tokei,vel_norm,vel_head,alt,alt_launch,u);
makakensanba 0:b021d725d528 332 fprintf(fp,"%c,%f %f %f %f %f %d \r\n" ,Phase,hokui,tokei,vel_norm,vel_head,alt,alt_launch,u);
makakensanba 0:b021d725d528 333
makakensanba 0:b021d725d528 334 if(k % 30 == 0){
makakensanba 0:b021d725d528 335 k = 0;
makakensanba 0:b021d725d528 336 row =! row;
makakensanba 0:b021d725d528 337 fclose(fp);
makakensanba 0:b021d725d528 338 fp = fopen("/sd/log.txt", "a");
makakensanba 0:b021d725d528 339 }
makakensanba 0:b021d725d528 340 }
makakensanba 0:b021d725d528 341 */ static float alt_drop2=ms5607.getAltitude();
makakensanba 0:b021d725d528 342 alt_drop2=ms5607.getAltitude();
makakensanba 0:b021d725d528 343 alt_drop=alt_drop2-alt_launch;
makakensanba 0:b021d725d528 344
makakensanba 0:b021d725d528 345 float x_s[] = { xDist, yDist };
makakensanba 0:b021d725d528 346 device.printf("x_s %f %f drp:%f max:%f \r\n",x_s[0],x_s[1],alt_drop,alt_max - alt_launch);
makakensanba 0:b021d725d528 347 u = set_input(st == 'A', x_s, vel_norm, vel_head, alt_drop, alt_max - alt_launch);
makakensanba 0:b021d725d528 348 device.printf("%d",u);
makakensanba 0:b021d725d528 349 twe.printf("%d,%f %f %f %f %f %d \r\n",cnt,hokui,tokei,vel_norm,vel_head,alt_drop,alt_max - alt_launch,u);
makakensanba 0:b021d725d528 350
makakensanba 0:b021d725d528 351 static int ui = 0;
makakensanba 0:b021d725d528 352 const int ui_max = 30, ui_min = 10; // ui_min = 0 ?
makakensanba 0:b021d725d528 353 if (ui < ui_min) u = 1;
makakensanba 0:b021d725d528 354 else if (ui > ui_max) u = -1;
makakensanba 0:b021d725d528 355 ui += u;
makakensanba 0:b021d725d528 356
makakensanba 0:b021d725d528 357 if(u == -1) _Motor(2,FORW);//右旋回
makakensanba 0:b021d725d528 358 else if(u == 0) _Motor(2,STOP);
makakensanba 0:b021d725d528 359 else if(u == 1) _Motor(2,BACK);//左旋回
makakensanba 0:b021d725d528 360
makakensanba 0:b021d725d528 361
makakensanba 0:b021d725d528 362 sprintf(gps_data, "");
makakensanba 0:b021d725d528 363 }//if
makakensanba 0:b021d725d528 364 }
makakensanba 0:b021d725d528 365 }
makakensanba 0:b021d725d528 366 }
makakensanba 0:b021d725d528 367
makakensanba 0:b021d725d528 368 void _Input(){
makakensanba 0:b021d725d528 369 input_buff[input_cnt] = device.getc();
makakensanba 0:b021d725d528 370 device.printf("\r\n");
makakensanba 0:b021d725d528 371 switch (Mode) {
makakensanba 0:b021d725d528 372 case STANDBY:
makakensanba 0:b021d725d528 373 if(input_cnt==0){
makakensanba 0:b021d725d528 374 if(input_buff[0]=='S'){
makakensanba 0:b021d725d528 375 device.printf("U >> UNLOCK\r\n");
makakensanba 0:b021d725d528 376 device.printf("L >> LOCK\r\n");
makakensanba 0:b021d725d528 377 }else if(input_buff[0]=='M'){
makakensanba 0:b021d725d528 378 device.printf("S >> STANDBY\r\n");
makakensanba 0:b021d725d528 379 device.printf("F >> FLIGHT\r\n");
makakensanba 0:b021d725d528 380 }else{
makakensanba 0:b021d725d528 381 device.printf("This command is not found >> %c\r\n",input_buff[0]);
makakensanba 0:b021d725d528 382 device.printf(">>MAINMENU<<\r\n");
makakensanba 0:b021d725d528 383 device.printf("S >> Servo Operation\r\n");
makakensanba 0:b021d725d528 384 device.printf("M >> Mode Change\r\n");
makakensanba 0:b021d725d528 385 device.printf("-->>");
makakensanba 0:b021d725d528 386 return;
makakensanba 0:b021d725d528 387 }
makakensanba 0:b021d725d528 388 }else if(input_cnt==1){
makakensanba 0:b021d725d528 389 if(input_buff[0]=='S'){
makakensanba 0:b021d725d528 390 if(input_buff[1]=='U')_Motor(1,UNLOCK);
makakensanba 0:b021d725d528 391 else if(input_buff[1]=='L')_Motor(1,LOCK);
makakensanba 0:b021d725d528 392 else{
makakensanba 0:b021d725d528 393 device.printf("This command is not found >> %c\r\n",input_buff[1]);
makakensanba 0:b021d725d528 394 device.printf("U >> UNLOCK\r\n");
makakensanba 0:b021d725d528 395 device.printf("L >> LOCK\r\n");
makakensanba 0:b021d725d528 396 device.printf("-->>");
makakensanba 0:b021d725d528 397 return;
makakensanba 0:b021d725d528 398 }
makakensanba 0:b021d725d528 399 }else if(input_buff[0]=='M'){
makakensanba 0:b021d725d528 400 if(input_buff[1]=='S'){
makakensanba 0:b021d725d528 401 Mode = STANDBY;
makakensanba 0:b021d725d528 402 }else if(input_buff[1]=='F'){
makakensanba 0:b021d725d528 403 Mode = FLIGHT;
makakensanba 0:b021d725d528 404 Phase = LAUNCH;
makakensanba 0:b021d725d528 405
makakensanba 0:b021d725d528 406 device.printf("FLIGHT-Mode ON!\r\n");
makakensanba 0:b021d725d528 407 device.printf("***alert***\r\n");
makakensanba 0:b021d725d528 408 device.printf("You will be able to reset only!\r\n");
makakensanba 0:b021d725d528 409 return;
makakensanba 0:b021d725d528 410 }else{
makakensanba 0:b021d725d528 411 device.printf("This command is not found >> %c\r\n",input_buff[1]);
makakensanba 0:b021d725d528 412 device.printf("S >> STANDBY\r\n");
makakensanba 0:b021d725d528 413 device.printf("F >> FLIGHT\r\n");
makakensanba 0:b021d725d528 414 device.printf("-->>");
makakensanba 0:b021d725d528 415 return;
makakensanba 0:b021d725d528 416 }
makakensanba 0:b021d725d528 417 }
makakensanba 0:b021d725d528 418 input_cnt = 0;
makakensanba 0:b021d725d528 419 device.printf(">>MAINMENU<<\r\n");
makakensanba 0:b021d725d528 420 device.printf("S >> Servo Operation\r\n");
makakensanba 0:b021d725d528 421 device.printf("M >> Mode Change\r\n");
makakensanba 0:b021d725d528 422 device.printf("-->>");
makakensanba 0:b021d725d528 423 return;
makakensanba 0:b021d725d528 424 }
makakensanba 0:b021d725d528 425 device.printf("-->>");
makakensanba 0:b021d725d528 426 input_cnt++;
makakensanba 0:b021d725d528 427 break;
makakensanba 0:b021d725d528 428 case FLIGHT://reset only
makakensanba 0:b021d725d528 429 break;
makakensanba 0:b021d725d528 430 }
makakensanba 0:b021d725d528 431 }
makakensanba 0:b021d725d528 432
makakensanba 0:b021d725d528 433 /*その他雑関数*/
makakensanba 0:b021d725d528 434 float _Measure_Alt(float press/*[hPa]*/, float temp/*[℃]*/){
makakensanba 0:b021d725d528 435 return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
makakensanba 0:b021d725d528 436 }
makakensanba 0:b021d725d528 437 float _Median(float data[], int num){
makakensanba 0:b021d725d528 438 float median;
makakensanba 0:b021d725d528 439 float *sort = (float *)malloc(sizeof(float)*num);
makakensanba 0:b021d725d528 440 for(int i=0; i<num; i++) sort[i] = data[i];
makakensanba 0:b021d725d528 441 for(int i=0; i<num; i++){
makakensanba 0:b021d725d528 442 for(int j=0; j<num-i-1; j++){
makakensanba 0:b021d725d528 443 if(sort[j]>sort[j+1]){
makakensanba 0:b021d725d528 444 float buff = sort[j+1];
makakensanba 0:b021d725d528 445 sort[j+1] = sort[j];
makakensanba 0:b021d725d528 446 sort[j] = buff;
makakensanba 0:b021d725d528 447 }
makakensanba 0:b021d725d528 448 }
makakensanba 0:b021d725d528 449 }
makakensanba 0:b021d725d528 450 if(num%2!=0)median = sort[num/2];
makakensanba 0:b021d725d528 451 else median = (sort[num/2-1]+sort[num/2])/2.0;
makakensanba 0:b021d725d528 452 free(sort);
makakensanba 0:b021d725d528 453 return median;
makakensanba 0:b021d725d528 454 }
makakensanba 0:b021d725d528 455
makakensanba 0:b021d725d528 456 int set_input(bool gps_good, float x_s[], float vg_knot, float hdg_gps, float alt_s, float alt_max)
makakensanba 0:b021d725d528 457 {
makakensanba 0:b021d725d528 458 // グローバルにするかも？
makakensanba 0:b021d725d528 459 const int dn = 40; // windstarのベクトルを取得するステップ間隔
makakensanba 0:b021d725d528 460 const float vz_est = 4.62; // パラフォイルのsinkrate[m/s]
makakensanba 0:b021d725d528 461 const float dist_tgt = 30.0; // 目標旋回半径[m]
makakensanba 0:b021d725d528 462 const float alt_unctrl = 50.0; // 無制御で降下する高度[m]
makakensanba 0:b021d725d528 463
makakensanba 0:b021d725d528 464 static int gps_step = 1;
makakensanba 0:b021d725d528 465 float u = 0.0;
makakensanba 0:b021d725d528 466 if (gps_good)
makakensanba 0:b021d725d528 467 {
makakensanba 0:b021d725d528 468 enum Phase
makakensanba 0:b021d725d528 469 {
makakensanba 0:b021d725d528 470 WIND_EST,
makakensanba 0:b021d725d528 471 CTRL
makakensanba 0:b021d725d528 472 };
makakensanba 0:b021d725d528 473 static Phase phase = WIND_EST;
makakensanba 0:b021d725d528 474
makakensanba 0:b021d725d528 475 // sensor data
makakensanba 0:b021d725d528 476 float dt_ = dt*gps_step;
makakensanba 0:b021d725d528 477 gps_step = 1;
makakensanba 0:b021d725d528 478
makakensanba 0:b021d725d528 479 // state variable estimation
makakensanba 0:b021d725d528 480 float hdg_s = hdg_conv(hdg_gps);
makakensanba 0:b021d725d528 481 static float hdg0 = hdg_s;
makakensanba 0:b021d725d528 482 float omg_calc = hdg_s - hdg0;
makakensanba 0:b021d725d528 483 if (omg_calc >= M_PI) omg_calc -= 2 * M_PI;
makakensanba 0:b021d725d528 484 else if (omg_calc <= -M_PI) omg_calc += 2 * M_PI;
makakensanba 0:b021d725d528 485 omg_calc /= dt_;
makakensanba 0:b021d725d528 486 hdg0 = hdg_s;
makakensanba 0:b021d725d528 487
makakensanba 0:b021d725d528 488 // static float alt0 = alt_s;
makakensanba 0:b021d725d528 489 // float vz_calc = (alt_s - alt0) / dt_;
makakensanba 0:b021d725d528 490 // alt0 = alt_s;
makakensanba 0:b021d725d528 491
makakensanba 0:b021d725d528 492 float vg_s[] = { vg_knot*0.514444444*cos(hdg_s), vg_knot*0.514444444*sin(hdg_s) };
makakensanba 0:b021d725d528 493 // if want to use estimated position, uncomment below
makakensanba 0:b021d725d528 494 // float x_est[2] = {};
makakensanba 0:b021d725d528 495 // filter_x(dt_, 0.1, x_est, x_s, vg_s);
makakensanba 0:b021d725d528 496
makakensanba 0:b021d725d528 497 // wind estimation sequence
makakensanba 0:b021d725d528 498 static float vw_est[2] = {};
makakensanba 0:b021d725d528 499 switch (phase)
makakensanba 0:b021d725d528 500 {
makakensanba 0:b021d725d528 501 case WIND_EST:
makakensanba 0:b021d725d528 502 {
makakensanba 0:b021d725d528 503 const int dn2 = 2 * dn;
makakensanba 0:b021d725d528 504 static int n = 0;
makakensanba 0:b021d725d528 505 float vw_t[2] = {};
makakensanba 0:b021d725d528 506 static int cnt = 0;
makakensanba 0:b021d725d528 507 if (n >= dn2)
makakensanba 0:b021d725d528 508 {
makakensanba 0:b021d725d528 509 static int j = 0;
makakensanba 0:b021d725d528 510 static float vg_buf[dn2][2] = {};
makakensanba 0:b021d725d528 511 windstar(vw_t, vg_buf[n % dn2], vg_buf[(n + dn) % dn2], vg_s);
makakensanba 0:b021d725d528 512 vw_est[0] = (vw_est[0] * j + vw_t[0]) / (j + 1);
makakensanba 0:b021d725d528 513 vw_est[1] = (vw_est[1] * j + vw_t[1]) / (j + 1);
makakensanba 0:b021d725d528 514 j++;
makakensanba 0:b021d725d528 515 vg_buf[n % dn2][0] = vg_s[0];
makakensanba 0:b021d725d528 516 vg_buf[n % dn2][1] = vg_s[1];
makakensanba 0:b021d725d528 517 }
makakensanba 0:b021d725d528 518 n++;
makakensanba 0:b021d725d528 519
makakensanba 0:b021d725d528 520 if (alt_s < alt_max - alt_unctrl) //
makakensanba 0:b021d725d528 521 if (cnt++ >= 3) phase = CTRL;
makakensanba 0:b021d725d528 522 }
makakensanba 0:b021d725d528 523 break;
makakensanba 0:b021d725d528 524 case CTRL:
makakensanba 0:b021d725d528 525 {
makakensanba 0:b021d725d528 526 float t_flt_est = (alt_s - x_tgt[2]) / vz_est;
makakensanba 0:b021d725d528 527 if (abs(vz_est) <= epsilon) t_flt_est = 0.0;
makakensanba 0:b021d725d528 528 float windoffset[2] = { -vw_est[0] * t_flt_est, -vw_est[1] * t_flt_est };
makakensanba 0:b021d725d528 529 if (windoffset[1] > 200.0) windoffset[1] = 200.0;
makakensanba 0:b021d725d528 530 float x_virt[2] = { x_s[0] + windoffset[0], x_s[1] + windoffset[1] };
makakensanba 0:b021d725d528 531 // if want to use estimated position, comment out above and uncomment below
makakensanba 0:b021d725d528 532 // float x_virt[2] = { x_est[0] + windoffset[0], x_est[1] + windoffset[1] };
makakensanba 0:b021d725d528 533 float x_virt_rel[2] = { x_virt[0] - x_tgt[0], x_virt[1] - x_tgt[1] };
makakensanba 0:b021d725d528 534 u = control(x_virt_rel, vw_est, vg_s, hdg_s, omg_calc, dist_tgt);
makakensanba 0:b021d725d528 535 }
makakensanba 0:b021d725d528 536 break;
makakensanba 0:b021d725d528 537 }
makakensanba 0:b021d725d528 538
makakensanba 0:b021d725d528 539 }
makakensanba 0:b021d725d528 540 else
makakensanba 0:b021d725d528 541 gps_step++;
makakensanba 0:b021d725d528 542
makakensanba 0:b021d725d528 543 return u;
makakensanba 0:b021d725d528 544 }
makakensanba 0:b021d725d528 545
makakensanba 0:b021d725d528 546 int control(float r_rel[], float vw[], float v[], float hdg, float omg, float dist_tgt)
makakensanba 0:b021d725d528 547 {
makakensanba 0:b021d725d528 548 const float threshold = 0.20;
makakensanba 0:b021d725d528 549
makakensanba 0:b021d725d528 550 float theta = atan2(r_rel[1], r_rel[0]);
makakensanba 0:b021d725d528 551
makakensanba 0:b021d725d528 552 float dist = norm2(r_rel);
makakensanba 0:b021d725d528 553 float Vr = dot2(r_rel, v) / dist;
makakensanba 0:b021d725d528 554 float Vtheta = (r_rel[0] * v[1] - r_rel[1] * v[0]) / dist;
makakensanba 0:b021d725d528 555
makakensanba 0:b021d725d528 556 float hdg_tgt = 0.0;
makakensanba 0:b021d725d528 557 float alpha = atan(0.5*(dist - dist_tgt));
makakensanba 0:b021d725d528 558 float limitalpha = M_PI_2 - asin(dist_tgt / dist);
makakensanba 0:b021d725d528 559 if (alpha > limitalpha) alpha = limitalpha;
makakensanba 0:b021d725d528 560 if (Vtheta > 0)
makakensanba 0:b021d725d528 561 hdg_tgt = theta + M_PI_2 + alpha;
makakensanba 0:b021d725d528 562 else
makakensanba 0:b021d725d528 563 hdg_tgt = theta - M_PI_2 - alpha;
makakensanba 0:b021d725d528 564 periodize(hdg_tgt, M_PI, -M_PI);
makakensanba 0:b021d725d528 565
makakensanba 0:b021d725d528 566 float dhdg = hdg - hdg_tgt;
makakensanba 0:b021d725d528 567 periodize(dhdg, M_PI, -M_PI);
makakensanba 0:b021d725d528 568
makakensanba 0:b021d725d528 569 float domg = omg - Vtheta / dist;
makakensanba 0:b021d725d528 570 float Kp = 0.5, Kd = 3.0;
makakensanba 0:b021d725d528 571 float u = -Kp*dhdg - Kd*domg;
makakensanba 0:b021d725d528 572
makakensanba 0:b021d725d528 573 if (dist < 200.0)
makakensanba 0:b021d725d528 574 {
makakensanba 0:b021d725d528 575 if (Vtheta > 0) u += 0.05*Vr;
makakensanba 0:b021d725d528 576 else u -= 0.05*Vr;
makakensanba 0:b021d725d528 577 }
makakensanba 0:b021d725d528 578
makakensanba 0:b021d725d528 579 if (u > threshold) return 1;
makakensanba 0:b021d725d528 580 else if (u < -threshold) return -1;
makakensanba 0:b021d725d528 581 else return 0;
makakensanba 0:b021d725d528 582 }
makakensanba 0:b021d725d528 583
makakensanba 0:b021d725d528 584 void filter_x(float dt, float C, float x_est[], float x_gps[], float vg_gps[])
makakensanba 0:b021d725d528 585 {
makakensanba 0:b021d725d528 586 static float x0[] = { x_gps[0], x_gps[1], x_gps[2] };
makakensanba 0:b021d725d528 587 static int called = 0;
makakensanba 0:b021d725d528 588 if (!called)
makakensanba 0:b021d725d528 589 {
makakensanba 0:b021d725d528 590 for (int i = 0; i < 2; i++)
makakensanba 0:b021d725d528 591 x_est[i] = x0[i];
makakensanba 0:b021d725d528 592 called++;
makakensanba 0:b021d725d528 593 return;
makakensanba 0:b021d725d528 594 }
makakensanba 0:b021d725d528 595 float xt[2];
makakensanba 0:b021d725d528 596 for (int i = 0; i < 2; i++)
makakensanba 0:b021d725d528 597 {
makakensanba 0:b021d725d528 598 xt[i] = x0[i] + vg_gps[i] * dt;
makakensanba 0:b021d725d528 599 x_est[i] = C*x_gps[i] + (1 - C)*xt[i];
makakensanba 0:b021d725d528 600 x0[i] = x_est[i];
makakensanba 0:b021d725d528 601 }
makakensanba 0:b021d725d528 602 }
makakensanba 0:b021d725d528 603
makakensanba 0:b021d725d528 604 float hdg_conv(float hdgN0deg)
makakensanba 0:b021d725d528 605 {
makakensanba 0:b021d725d528 606 hdgN0deg *= -M_PI / 180.0;
makakensanba 0:b021d725d528 607 hdgN0deg += M_PI_2;
makakensanba 0:b021d725d528 608 periodize(hdgN0deg, M_PI, -M_PI);
makakensanba 0:b021d725d528 609 return hdgN0deg;
makakensanba 0:b021d725d528 610 }
makakensanba 0:b021d725d528 611 void periodize(float &x, float max, float min)
makakensanba 0:b021d725d528 612 {
makakensanba 0:b021d725d528 613 float range = max - min;
makakensanba 0:b021d725d528 614 while (x > max) x -= range;
makakensanba 0:b021d725d528 615 while (x < min) x += range;
makakensanba 0:b021d725d528 616 }
makakensanba 0:b021d725d528 617
makakensanba 0:b021d725d528 618 void windstar(float Vw[], float Vg0[], float Vg1[], float Vg2[])
makakensanba 0:b021d725d528 619 {
makakensanba 0:b021d725d528 620 float l[3][3] = {}; // line coeffs l[0]*x + l[1]*y + l[2] = 0
makakensanba 0:b021d725d528 621 segmentBisector(l[0], Vg0, Vg1);
makakensanba 0:b021d725d528 622 segmentBisector(l[1], Vg1, Vg2);
makakensanba 0:b021d725d528 623 segmentBisector(l[2], Vg2, Vg0);
makakensanba 0:b021d725d528 624 float r[3][3] = {}; // intersection point
makakensanba 0:b021d725d528 625 intersection(r[0], l[0], l[1]);
makakensanba 0:b021d725d528 626 intersection(r[1], l[1], l[2]);
makakensanba 0:b021d725d528 627 intersection(r[2], l[2], l[0]);
makakensanba 0:b021d725d528 628 Vw[0] = (r[0][0] + r[1][0] + r[2][0]) / 3;
makakensanba 0:b021d725d528 629 Vw[1] = (r[0][1] + r[1][1] + r[2][1]) / 3;
makakensanba 0:b021d725d528 630 }
makakensanba 0:b021d725d528 631 void segmentBisector(float a[], float v1[], float v2[])
makakensanba 0:b021d725d528 632 {
makakensanba 0:b021d725d528 633 a[0] = v2[0] - v1[0];
makakensanba 0:b021d725d528 634 a[1] = v2[1] - v1[1];
makakensanba 0:b021d725d528 635 a[2] = -0.5*(a[0] * (v1[0] + v2[0]) + a[1] * (v1[1] + v2[1]));
makakensanba 0:b021d725d528 636 }
makakensanba 0:b021d725d528 637 int intersection(float r[], float l[], float m[])
makakensanba 0:b021d725d528 638 {
makakensanba 0:b021d725d528 639 float det = l[0] * m[1] - l[1] * m[0];
makakensanba 0:b021d725d528 640 if (abs(det) < epsilon) return -1;
makakensanba 0:b021d725d528 641 r[0] = (-m[1] * l[2] + l[1] * m[2]) / det;
makakensanba 0:b021d725d528 642 r[1] = (m[0] * l[2] - l[0] * m[2]) / det;
makakensanba 0:b021d725d528 643 return 0;
makakensanba 0:b021d725d528 644 }
makakensanba 0:b021d725d528 645
makakensanba 0:b021d725d528 646 float dot2(float a[], float b[])
makakensanba 0:b021d725d528 647 {
makakensanba 0:b021d725d528 648 return a[0] * b[0] + a[1] * b[1];
makakensanba 0:b021d725d528 649 }
makakensanba 0:b021d725d528 650 float norm2(float v[])
makakensanba 0:b021d725d528 651 {
makakensanba 0:b021d725d528 652 return sqrt(dot2(v, v));
makakensanba 0:b021d725d528 653 }