SSLM1 / Mbed 2 deprecated 5-3_thruster_depressmode

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   2_MPU9250 mbed 2_GPS_GMS6-CR6 2_LPS33HW 2_EEPROM 2_P7100

main.cpp@0:af4866697da4, 2020-07-17 (annotated)

Committer:
rary
Date:
Fri Jul 17 12:13:59 2020 +0000
Revision:
0:af4866697da4
add depress mode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rary 0:af4866697da4 1 #include "mbed.h"
rary 0:af4866697da4 2 #include "math.h"
rary 0:af4866697da4 3 #include "EEPROM.h"
rary 0:af4866697da4 4 #include "LPS33HW.h"
rary 0:af4866697da4 5 #include "P7100.h"
rary 0:af4866697da4 6 #include "MPU9250.h"
rary 0:af4866697da4 7 #include "GMS6_CR6.h"
rary 0:af4866697da4 8
rary 0:af4866697da4 9
rary 0:af4866697da4 10 Timer t; //時間計測
rary 0:af4866697da4 11 Serial pc(USBTX, USBRX);
rary 0:af4866697da4 12 LPS33HW lps(p9, p10);
rary 0:af4866697da4 13 EEPROM rom(p9, p10);
rary 0:af4866697da4 14 MPU9250 mpu(p9, p10);
rary 0:af4866697da4 15 P7100 p7100(p5, p6, p7, p8);
rary 0:af4866697da4 16 Serial twe(p13, p14);
rary 0:af4866697da4 17 GMS6_CR6 gps(p28, p27);
rary 0:af4866697da4 18
rary 0:af4866697da4 19 DigitalIn Flight_Pin(p30);
rary 0:af4866697da4 20 DigitalOut ThrustValve(p21);
rary 0:af4866697da4 21 DigitalOut DepressValve(p22);
rary 0:af4866697da4 22
rary 0:af4866697da4 23 void get_save_low(); //自作関数プロトタイプ宣言
rary 0:af4866697da4 24 float get_save_high(); //自作関数プロトタイプ宣言
rary 0:af4866697da4 25
rary 0:af4866697da4 26 int pointer_high = 0; //EEPROM保存アドレス(high)
rary 0:af4866697da4 27 int pointer_low = 0; //EEPROM保存アドレス(low)
rary 0:af4866697da4 28
rary 0:af4866697da4 29
rary 0:af4866697da4 30
rary 0:af4866697da4 31 //main
rary 0:af4866697da4 32 int main()
rary 0:af4866697da4 33 {
rary 0:af4866697da4 34 pc.baud(460800); //ボーレート変更
rary 0:af4866697da4 35 twe.baud(230400); //tweボーレート変更
rary 0:af4866697da4 36
rary 0:af4866697da4 37 float lat_north; //GPS緯度
rary 0:af4866697da4 38 float lon_east; //GPS経度
rary 0:af4866697da4 39 float GPS[2]; //GPSデータ格納配列
rary 0:af4866697da4 40
rary 0:af4866697da4 41 double groundP = 0; //地上の気圧データ
rary 0:af4866697da4 42 double heightP = 0; //高度TBDmの気圧
rary 0:af4866697da4 43 double judgeP[5]; //判定用気圧データ
rary 0:af4866697da4 44 double pastP = 0; //一つ前の気圧データ
rary 0:af4866697da4 45 double dP[5]; //気圧データの差異
rary 0:af4866697da4 46 int judge = 0; //判定結果
rary 0:af4866697da4 47 int i = 0; //差異配列の番号
rary 0:af4866697da4 48 memset(judgeP, 0, 5);
rary 0:af4866697da4 49 memset(dP, 1, 5);
rary 0:af4866697da4 50
rary 0:af4866697da4 51 int n_dvalve = 0; //脱圧バルブの開閉回数
rary 0:af4866697da4 52
rary 0:af4866697da4 53 t.start(); //timer Start
rary 0:af4866697da4 54
rary 0:af4866697da4 55 //sensor Setup
rary 0:af4866697da4 56 lps.start(1); //気圧センサスタート
rary 0:af4866697da4 57 lps.start(1); //気圧センサスタート
rary 0:af4866697da4 58 wait(0.1);
rary 0:af4866697da4 59 mpu.start(); //MPU9250 start&setup
rary 0:af4866697da4 60 mpu.accelsetup(3);
rary 0:af4866697da4 61 mpu.gyrosetup(3);
rary 0:af4866697da4 62 mpu.AKsetup(1);
rary 0:af4866697da4 63 wait(0.2);
rary 0:af4866697da4 64
rary 0:af4866697da4 65 pc.printf("Power ON!\r\n");
rary 0:af4866697da4 66
rary 0:af4866697da4 67 //地上の気圧取得&計算
rary 0:af4866697da4 68 for(int a = 0; a < 1000; a++) {
rary 0:af4866697da4 69 groundP = groundP + lps.data_read();
rary 0:af4866697da4 70 wait_ms(50);
rary 0:af4866697da4 71 }
rary 0:af4866697da4 72 groundP = groundP/1000; //地上の気圧データ
rary 0:af4866697da4 73 heightP = 0.998862 * groundP; //高度10mの気圧を計算
rary 0:af4866697da4 74
rary 0:af4866697da4 75
rary 0:af4866697da4 76
rary 0:af4866697da4 77 //Wait mode
rary 0:af4866697da4 78 pc.printf("Wait mode Start!\r\n");
rary 0:af4866697da4 79 while(Flight_Pin == 0) {
rary 0:af4866697da4 80 get_save_low(); //data get&save
rary 0:af4866697da4 81 wait_ms(1000);
rary 0:af4866697da4 82 }
rary 0:af4866697da4 83 pc.printf("Wait mode Finish!\r\n");
rary 0:af4866697da4 84
rary 0:af4866697da4 85
rary 0:af4866697da4 86
rary 0:af4866697da4 87 //Flight mode
rary 0:af4866697da4 88 pc.printf("Flight mode Start!\r\n");
rary 0:af4866697da4 89
rary 0:af4866697da4 90 while(1) {
rary 0:af4866697da4 91 for(int k = 0; k < 200; k++) {
rary 0:af4866697da4 92 judgeP[i] = 0; //初期化
rary 0:af4866697da4 93 for(int n = 0; n < 2; n++) { //2回に一回送信
rary 0:af4866697da4 94 judgeP[i] = judgeP[i] + get_save_high(); //data get&save
rary 0:af4866697da4 95 }
rary 0:af4866697da4 96 twe.printf("^"); //送信トリガー
rary 0:af4866697da4 97
rary 0:af4866697da4 98 judgeP[i] = judgeP[i]/2; //判定用気圧を計算
rary 0:af4866697da4 99 dP[i] = judgeP[i] - pastP; //差異を取得
rary 0:af4866697da4 100 pastP = judgeP[i]; //判定用気圧値を過去データに置き換える
rary 0:af4866697da4 101 i++;
rary 0:af4866697da4 102 if(i == 5) {
rary 0:af4866697da4 103 i = 0;
rary 0:af4866697da4 104 }
rary 0:af4866697da4 105
rary 0:af4866697da4 106 if(dP[0] > 0.05 && dP[1] > 0.05 && dP[2] > 0.05 && dP[3] > 0.05 && dP[4] > 0.05) { //空中判定
rary 0:af4866697da4 107 if(judgeP[0] > heightP && judgeP[1] > heightP && judgeP[2] > heightP && judgeP[3] > heightP && judgeP[4] > heightP) { //高度判定
rary 0:af4866697da4 108 judge = 1;
rary 0:af4866697da4 109 break;
rary 0:af4866697da4 110 }
rary 0:af4866697da4 111 }
rary 0:af4866697da4 112 else if(dP[0] < 0.001 && dP[1] < 0.001 && dP[2] < 0.001 && dP[3] < 0.001 && dP[4] < 0.001) {
rary 0:af4866697da4 113 judge = 2;
rary 0:af4866697da4 114 break;
rary 0:af4866697da4 115 }
rary 0:af4866697da4 116 }
rary 0:af4866697da4 117
rary 0:af4866697da4 118 if(judge == 1 || judge == 2) {
rary 0:af4866697da4 119 break; //判定を通過したらループを抜ける
rary 0:af4866697da4 120 }
rary 0:af4866697da4 121
rary 0:af4866697da4 122 gps.read(GPS); //測位データ取得
rary 0:af4866697da4 123 lat_north = GPS[0];
rary 0:af4866697da4 124 lon_east = GPS[1];
rary 0:af4866697da4 125
rary 0:af4866697da4 126 if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) { //緯度が正常な値のときのみ送信
rary 0:af4866697da4 127 twe.printf("%f,%f\r\n^", lat_north, lon_east); //送信トリガー
rary 0:af4866697da4 128 pc.printf("%f,%f\r\n", lat_north, lon_east); //check用
rary 0:af4866697da4 129 }
rary 0:af4866697da4 130 memset(GPS,'\0', 2); //送信トリガー
rary 0:af4866697da4 131 }
rary 0:af4866697da4 132
rary 0:af4866697da4 133 pc.printf("Flight mode Finish!\r\n");
rary 0:af4866697da4 134
rary 0:af4866697da4 135
rary 0:af4866697da4 136 //thruster mode
rary 0:af4866697da4 137 if(judge == 1) {
rary 0:af4866697da4 138 pc.printf("Thruster mode Start!\r\n");
rary 0:af4866697da4 139
rary 0:af4866697da4 140 ThrustValve = 1; //スラスタ電磁弁開
rary 0:af4866697da4 141 for(int k = 0; k < 250; k++)
rary 0:af4866697da4 142 {
rary 0:af4866697da4 143 for(int n = 0; n < 2; n++) //2回に一回送信
rary 0:af4866697da4 144 {
rary 0:af4866697da4 145 get_save_high(); //data get&save
rary 0:af4866697da4 146 }
rary 0:af4866697da4 147 twe.printf("^"); //送信トリガー
rary 0:af4866697da4 148 }
rary 0:af4866697da4 149 ThrustValve = 0; //スラスタ電磁弁閉
rary 0:af4866697da4 150
rary 0:af4866697da4 151 pc.printf("Thruster mode Finish!\r\n");
rary 0:af4866697da4 152 }
rary 0:af4866697da4 153
rary 0:af4866697da4 154 //depress mode
rary 0:af4866697da4 155 pc.printf("Depress mode Start!\r\n");
rary 0:af4866697da4 156 memset(judgeP, 0, 5);
rary 0:af4866697da4 157 memset(dP, 1, 5);
rary 0:af4866697da4 158 pastP = 0;
rary 0:af4866697da4 159
rary 0:af4866697da4 160 while(1) {
rary 0:af4866697da4 161 DepressValve = 1;
rary 0:af4866697da4 162
rary 0:af4866697da4 163 for(int k = 0; k < 100; k++) {
rary 0:af4866697da4 164 judgeP[i] = 0; //初期化
rary 0:af4866697da4 165
rary 0:af4866697da4 166 for(int n = 0; n < 2; n++) { //2回に一回送信
rary 0:af4866697da4 167 judgeP[i] = judgeP[i] + get_save_high(); //data get&save
rary 0:af4866697da4 168 }
rary 0:af4866697da4 169 twe.printf("^"); //送信トリガー
rary 0:af4866697da4 170
rary 0:af4866697da4 171 judgeP[i] = judgeP[i]/2; //判定用気圧を計算
rary 0:af4866697da4 172 dP[i] = judgeP[i] - pastP; //差異を取得
rary 0:af4866697da4 173 pastP = judgeP[i]; //判定用気圧値を過去データに置き換える
rary 0:af4866697da4 174
rary 0:af4866697da4 175 if(dP[0] < 0.001 && dP[1] < 0.001 && dP[2] < 0.001 && dP[3] < 0.001 && dP[4] < 0.001 && n_dvalve > 2 || n_dvalve == 10) //着地判定
rary 0:af4866697da4 176 {
rary 0:af4866697da4 177 judge = 3;
rary 0:af4866697da4 178 break;
rary 0:af4866697da4 179 }
rary 0:af4866697da4 180 i++;
rary 0:af4866697da4 181 if(i == 5) {i = 0;}
rary 0:af4866697da4 182 }
rary 0:af4866697da4 183
rary 0:af4866697da4 184 DepressValve = 0;
rary 0:af4866697da4 185 n_dvalve++;
rary 0:af4866697da4 186
rary 0:af4866697da4 187 if(judge == 3) {
rary 0:af4866697da4 188 break; //地上判定を通過したらループを抜ける
rary 0:af4866697da4 189 }
rary 0:af4866697da4 190
rary 0:af4866697da4 191 gps.read_low(GPS); //測位データ取得
rary 0:af4866697da4 192 lat_north = GPS[0];
rary 0:af4866697da4 193 lon_east = GPS[1];
rary 0:af4866697da4 194
rary 0:af4866697da4 195 if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) { //緯度が正常な値のときのみ送信
rary 0:af4866697da4 196 twe.printf("%f,%f\r\n^", lat_north, lon_east); //送信トリガー
rary 0:af4866697da4 197 pc.printf("%f,%f\r\n", lat_north, lon_east); //check用
rary 0:af4866697da4 198 }
rary 0:af4866697da4 199 memset(GPS,'\0', 2); //送信トリガー
rary 0:af4866697da4 200 }
rary 0:af4866697da4 201 pc.printf("Depress mode Finish!\r\n");
rary 0:af4866697da4 202
rary 0:af4866697da4 203 pc.printf("Recovery mode Start!\r\n");
rary 0:af4866697da4 204 pc.printf("Recovery mode Finish!\r\n");
rary 0:af4866697da4 205
rary 0:af4866697da4 206
rary 0:af4866697da4 207
rary 0:af4866697da4 208 }
rary 0:af4866697da4 209
rary 0:af4866697da4 210
rary 0:af4866697da4 211
rary 0:af4866697da4 212 void get_save_low()
rary 0:af4866697da4 213 {
rary 0:af4866697da4 214 char s[64]; //データ取得配列
rary 0:af4866697da4 215
rary 0:af4866697da4 216 float accel[3], gyro[3]; //MPU9250data
rary 0:af4866697da4 217 float AX, AY, AZ, GX, GY, GZ; //MPU9250data
rary 0:af4866697da4 218
rary 0:af4866697da4 219 float T = t.read(); //時刻データ取得
rary 0:af4866697da4 220 float P = lps.data_read(); //気圧データ取得
rary 0:af4866697da4 221 float V = p7100.v_read(); //電圧取得
rary 0:af4866697da4 222 float TP = 0.25 * V - 0.125; //タンク圧力換算
rary 0:af4866697da4 223
rary 0:af4866697da4 224 mpu.accel_read(3, accel);
rary 0:af4866697da4 225 mpu.gyro_read(3, gyro);
rary 0:af4866697da4 226 AX = accel[0];
rary 0:af4866697da4 227 AY = accel[1];
rary 0:af4866697da4 228 AZ = accel[2];
rary 0:af4866697da4 229 GX = gyro[0];
rary 0:af4866697da4 230 GY = gyro[1];
rary 0:af4866697da4 231 GZ = gyro[2];
rary 0:af4866697da4 232
rary 0:af4866697da4 233 sprintf(s, "%8.3f %8.3f %6.3f %5.3f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換
rary 0:af4866697da4 234
rary 0:af4866697da4 235 rom.write_low(pointer_low, s, 64); // write tha data
rary 0:af4866697da4 236 pointer_low = pointer_low + 64; //アドレスずらし
rary 0:af4866697da4 237
rary 0:af4866697da4 238 pc.printf("%s\r\n", s); //check用
rary 0:af4866697da4 239
rary 0:af4866697da4 240 memset(s, '\0', 64 ); //初期化
rary 0:af4866697da4 241 }
rary 0:af4866697da4 242
rary 0:af4866697da4 243 float get_save_high()
rary 0:af4866697da4 244 {
rary 0:af4866697da4 245 char s[64]; //データ取得配列
rary 0:af4866697da4 246
rary 0:af4866697da4 247 float accel[3], gyro[3]; //MPU9250data
rary 0:af4866697da4 248 float AX, AY, AZ, GX, GY, GZ; //MPU9250data
rary 0:af4866697da4 249
rary 0:af4866697da4 250 float T = t.read(); //時刻データ取得
rary 0:af4866697da4 251 float P = lps.data_read(); //気圧データ取得
rary 0:af4866697da4 252 float V = p7100.v_read(); //電圧取得
rary 0:af4866697da4 253 float TP = 0.25 * V - 0.125; //タンク圧力換算
rary 0:af4866697da4 254
rary 0:af4866697da4 255 mpu.accel_read(3, accel);
rary 0:af4866697da4 256 mpu.gyro_read(3, gyro);
rary 0:af4866697da4 257 AX = accel[0];
rary 0:af4866697da4 258 AY = accel[1];
rary 0:af4866697da4 259 AZ = accel[2];
rary 0:af4866697da4 260 GX = gyro[0];
rary 0:af4866697da4 261 GY = gyro[1];
rary 0:af4866697da4 262 GZ = gyro[2];
rary 0:af4866697da4 263
rary 0:af4866697da4 264 sprintf(s, "%8.3f %8.3f %6.3f %5.3f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換
rary 0:af4866697da4 265
rary 0:af4866697da4 266 rom.write_high(pointer_high, s, 64); // write tha data
rary 0:af4866697da4 267
rary 0:af4866697da4 268 twe.printf("%s", s); //無線機送信
rary 0:af4866697da4 269
rary 0:af4866697da4 270 pointer_high = pointer_high + 64; //アドレスずらし
rary 0:af4866697da4 271
rary 0:af4866697da4 272 pc.printf("%s\r\n", s); //check用
rary 0:af4866697da4 273
rary 0:af4866697da4 274 memset(s, '\0', 64 ); //初期化
rary 0:af4866697da4 275
rary 0:af4866697da4 276 return P;
rary 0:af4866697da4 277 }