PQ_Hybrid_Electrical_Equipment_Team / Mbed 2 deprecated HYBRYD2018_IZU_ROCKET

2018 IZU ROCKET

Dependencies:   ADXL375_I2C ES920LR GPS_interrupt HDC1050_lib INA226_lib MadgwickFilter Nicrom SDFileSystem SHT31_DIS_lib TSL_2561_lib mbed mpu9250_i2c pqLPS22HB_lib

HYBRYD2018_IZU_ROCKET.cpp@0:9b6737b144fc, 2018-05-02 (annotated)

Committer:
zebrin1422
Date:
Wed May 02 18:20:57 2018 +0000
Revision:
0:9b6737b144fc
2018 IZU ROCKET

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zebrin1422 0:9b6737b144fc 1 #include "mbed.h"
zebrin1422 0:9b6737b144fc 2 #include "GPS_interrupt.h"
zebrin1422 0:9b6737b144fc 3 #include "HDC1050.h"
zebrin1422 0:9b6737b144fc 4 #include "mpu9250_i2c.h"
zebrin1422 0:9b6737b144fc 5 #include "MadgwickFilter.hpp"
zebrin1422 0:9b6737b144fc 6 #include "INA226.h"
zebrin1422 0:9b6737b144fc 7 #include "pqLPS22HB_lib.h"
zebrin1422 0:9b6737b144fc 8 #include "TSL2561.h"
zebrin1422 0:9b6737b144fc 9 #include "SHT3x.h"
zebrin1422 0:9b6737b144fc 10 #include "SDFileSystem.h"
zebrin1422 0:9b6737b144fc 11 #include "ADXL375_I2C.h"
zebrin1422 0:9b6737b144fc 12 #include "nicrom.hpp"
zebrin1422 0:9b6737b144fc 13 #include "ES920LR.hpp"
zebrin1422 0:9b6737b144fc 14 /*
zebrin1422 0:9b6737b144fc 15 #include "PowerControl.h"
zebrin1422 0:9b6737b144fc 16 #include "EthernetPowerControl.h"
zebrin1422 0:9b6737b144fc 17 */
zebrin1422 0:9b6737b144fc 18 /*******************************************************************************
zebrin1422 0:9b6737b144fc 19 #include "hoge.h"
zebrin1422 0:9b6737b144fc 20 *******************************************************************************/
zebrin1422 0:9b6737b144fc 21
zebrin1422 0:9b6737b144fc 22
zebrin1422 0:9b6737b144fc 23
zebrin1422 0:9b6737b144fc 24 /*******************************************************************************
zebrin1422 0:9b6737b144fc 25 タイマー等の設定
zebrin1422 0:9b6737b144fc 26 *******************************************************************************/
zebrin1422 0:9b6737b144fc 27
zebrin1422 0:9b6737b144fc 28 Timer Inner_Time;
zebrin1422 0:9b6737b144fc 29 Timer flight_time;
zebrin1422 0:9b6737b144fc 30 Ticker timer_reset;
zebrin1422 0:9b6737b144fc 31
zebrin1422 0:9b6737b144fc 32 int in_time;
zebrin1422 0:9b6737b144fc 33 char timer_count=0;
zebrin1422 0:9b6737b144fc 34
zebrin1422 0:9b6737b144fc 35 void inner_time_reset()
zebrin1422 0:9b6737b144fc 36 {
zebrin1422 0:9b6737b144fc 37 timer_count ++;
zebrin1422 0:9b6737b144fc 38 in_time = 1800*timer_count;
zebrin1422 0:9b6737b144fc 39 Inner_Time.reset();
zebrin1422 0:9b6737b144fc 40 }
zebrin1422 0:9b6737b144fc 41 /*******************************************************************************
zebrin1422 0:9b6737b144fc 42 フラグ等の設定
zebrin1422 0:9b6737b144fc 43 *******************************************************************************/
zebrin1422 0:9b6737b144fc 44
zebrin1422 0:9b6737b144fc 45 bool altitude_accept = false; //十分な高度(10m)に到達したフラグ
zebrin1422 0:9b6737b144fc 46 bool vertex_arrival = false; //頂点検知のフラグ、速度が0.1m/s以下でtrue
zebrin1422 0:9b6737b144fc 47
zebrin1422 0:9b6737b144fc 48 bool safety_flag = false; //安全装置解除のフラグ
zebrin1422 0:9b6737b144fc 49 bool flight_detect_mode = false; //フライトピン待機状態のフラグ
zebrin1422 0:9b6737b144fc 50 bool safety_device_reboot = false; //安全装置再起動のフラグ
zebrin1422 0:9b6737b144fc 51 bool flight_mode = false; //飛行中のフラグ
zebrin1422 0:9b6737b144fc 52 bool collect_mode = false; //回収状態のフラグ
zebrin1422 0:9b6737b144fc 53
zebrin1422 0:9b6737b144fc 54 bool solenoid_complete = false; //電磁弁が10回押されたフラグ
zebrin1422 0:9b6737b144fc 55 bool extrusion_accept = false; //電磁弁を押すことを許可するフラグ
zebrin1422 0:9b6737b144fc 56 bool sepa_accept = false; //ニクロム線を熱することを許可するフラグ
zebrin1422 0:9b6737b144fc 57
zebrin1422 0:9b6737b144fc 58 bool interrupt_prohibit_flag = false; //すべてのタイマー割り込みを禁止するフラグ
zebrin1422 0:9b6737b144fc 59
zebrin1422 0:9b6737b144fc 60 /*******************************************************************************
zebrin1422 0:9b6737b144fc 61 計算の中で使う変数の宣言
zebrin1422 0:9b6737b144fc 62 *******************************************************************************/
zebrin1422 0:9b6737b144fc 63 int iter = 0; //for文を回すときに使う
zebrin1422 0:9b6737b144fc 64 float sum = 0; //平均を計算する際に使う
zebrin1422 0:9b6737b144fc 65
zebrin1422 0:9b6737b144fc 66 const double DEG_TO_RAG = 0.01745329251994329576923690768489;//PI / 180.0
zebrin1422 0:9b6737b144fc 67 /*******************************************************************************
zebrin1422 0:9b6737b144fc 68 ダウンリンクするデータ
zebrin1422 0:9b6737b144fc 69 *******************************************************************************/
zebrin1422 0:9b6737b144fc 70
zebrin1422 0:9b6737b144fc 71 typedef struct HYBRID_ROCKET{
zebrin1422 0:9b6737b144fc 72
zebrin1422 0:9b6737b144fc 73 //unsigned int inner_time; //内部時間
zebrin1422 0:9b6737b144fc 74 bool flight_pin_state; //
zebrin1422 0:9b6737b144fc 75 float latitude; //緯度,1
zebrin1422 0:9b6737b144fc 76 float longitude; //経度,2
zebrin1422 0:9b6737b144fc 77 float w,x,y,z; //クォータニオン,3,4,5,6
zebrin1422 0:9b6737b144fc 78 float vol_external; //7
zebrin1422 0:9b6737b144fc 79 float current_external; //8
zebrin1422 0:9b6737b144fc 80 float vol_inner; //9
zebrin1422 0:9b6737b144fc 81 float current_inner; //10
zebrin1422 0:9b6737b144fc 82 float pressure; //11
zebrin1422 0:9b6737b144fc 83 float altitude; //高度,12
zebrin1422 0:9b6737b144fc 84 float velocity; //13
zebrin1422 0:9b6737b144fc 85 float luminosity; //照度,光度(明るさ),14
zebrin1422 0:9b6737b144fc 86 unsigned int TSL_time; //TSL2561の変換時間
zebrin1422 0:9b6737b144fc 87 //int camera_state; //0:　作動していない　1:　録画している
zebrin1422 0:9b6737b144fc 88 float temperature; //15
zebrin1422 0:9b6737b144fc 89 float humidity; //16
zebrin1422 0:9b6737b144fc 90 float adxl_acc[3]; //17,18,19
zebrin1422 0:9b6737b144fc 91 float press_pitot; //20
zebrin1422 0:9b6737b144fc 92 float gps_alt; //21
zebrin1422 0:9b6737b144fc 93 char Rocket_Phase; // ロケットのフェーズを設定するフラグ
zebrin1422 0:9b6737b144fc 94 // (1:接続確認、初期設定,2:準備,3:発射待機,4:飛行,5:回収)
zebrin1422 0:9b6737b144fc 95 //その他取得するデータを宣言する
zebrin1422 0:9b6737b144fc 96 };
zebrin1422 0:9b6737b144fc 97
zebrin1422 0:9b6737b144fc 98 static HYBRID_ROCKET hr;
zebrin1422 0:9b6737b144fc 99
zebrin1422 0:9b6737b144fc 100 /*******************************************************************************
zebrin1422 0:9b6737b144fc 101 PCシリアル通信
zebrin1422 0:9b6737b144fc 102 *******************************************************************************/
zebrin1422 0:9b6737b144fc 103 RawSerial pc(USBTX,USBRX, 115200);
zebrin1422 0:9b6737b144fc 104
zebrin1422 0:9b6737b144fc 105 /*******************************************************************************
zebrin1422 0:9b6737b144fc 106 I2C通信の設定
zebrin1422 0:9b6737b144fc 107 *******************************************************************************/
zebrin1422 0:9b6737b144fc 108 #define I2C_SDA p28
zebrin1422 0:9b6737b144fc 109 #define I2C_SCL p27
zebrin1422 0:9b6737b144fc 110
zebrin1422 0:9b6737b144fc 111 bool i2c_using_flag = false; //i2cを使ってないときはfalse、使っているときはtrue
zebrin1422 0:9b6737b144fc 112
zebrin1422 0:9b6737b144fc 113 I2C i2c_Bus(I2C_SDA, I2C_SCL);
zebrin1422 0:9b6737b144fc 114
zebrin1422 0:9b6737b144fc 115 /*******************************************************************************
zebrin1422 0:9b6737b144fc 116 SDFileSystem
zebrin1422 0:9b6737b144fc 117 *******************************************************************************/
zebrin1422 0:9b6737b144fc 118 #define MOSI_SD p5
zebrin1422 0:9b6737b144fc 119 #define MISO_SD p6
zebrin1422 0:9b6737b144fc 120 #define SCLK_SD p7
zebrin1422 0:9b6737b144fc 121 #define CS_SD p8
zebrin1422 0:9b6737b144fc 122
zebrin1422 0:9b6737b144fc 123 SDFileSystem sd(MOSI_SD,MISO_SD,SCLK_SD,CS_SD,"sd");
zebrin1422 0:9b6737b144fc 124 FILE *log_fp;
zebrin1422 0:9b6737b144fc 125
zebrin1422 0:9b6737b144fc 126 const int DATA_NUMBER = 21; //取得するデータの数
zebrin1422 0:9b6737b144fc 127 const int BUFF_SIZE = 8; //バッファの容量、8個
zebrin1422 0:9b6737b144fc 128 int SD_BUFF_COUNT_FLAG[DATA_NUMBER] = {}; //バッファがたまったことを知らせるフラグ
zebrin1422 0:9b6737b144fc 129 float sd_buff[BUFF_SIZE][DATA_NUMBER] = {}; //sdカードに書き込むバッファ
zebrin1422 0:9b6737b144fc 130 float time_buff[BUFF_SIZE][DATA_NUMBER] = {};
zebrin1422 0:9b6737b144fc 131 int buff_count[DATA_NUMBER] = {}; //配列の何個目に入れるかのカウント
zebrin1422 0:9b6737b144fc 132 //bool sd_using_flag = false;
zebrin1422 0:9b6737b144fc 133
zebrin1422 0:9b6737b144fc 134 Ticker sd_ticker;
zebrin1422 0:9b6737b144fc 135
zebrin1422 0:9b6737b144fc 136 void write_buff(int number,float data)
zebrin1422 0:9b6737b144fc 137 {
zebrin1422 0:9b6737b144fc 138 //pc.printf("number = %d\tcount = %d\tdata = %f\r\n", number,buff_count[number],data);
zebrin1422 0:9b6737b144fc 139 sd_buff[buff_count[number]][number] = data;
zebrin1422 0:9b6737b144fc 140 time_buff[buff_count[number]][number] = flight_time.read();
zebrin1422 0:9b6737b144fc 141 buff_count[number] ++;
zebrin1422 0:9b6737b144fc 142 //pc.printf("buff\r\n");
zebrin1422 0:9b6737b144fc 143 if(buff_count[number] >= 8)
zebrin1422 0:9b6737b144fc 144 {
zebrin1422 0:9b6737b144fc 145 SD_BUFF_COUNT_FLAG[number] = 1;
zebrin1422 0:9b6737b144fc 146 buff_count[number] = 0;
zebrin1422 0:9b6737b144fc 147 }
zebrin1422 0:9b6737b144fc 148 }
zebrin1422 0:9b6737b144fc 149
zebrin1422 0:9b6737b144fc 150
zebrin1422 0:9b6737b144fc 151 void write_sd(int number)
zebrin1422 0:9b6737b144fc 152 {
zebrin1422 0:9b6737b144fc 153 //if(!sd_using_flag)
zebrin1422 0:9b6737b144fc 154 //{
zebrin1422 0:9b6737b144fc 155 //sd_using_flag = true;
zebrin1422 0:9b6737b144fc 156 fprintf(log_fp, "Number = %d\r\n",number);
zebrin1422 0:9b6737b144fc 157 for(iter=0;iter<8;iter++)
zebrin1422 0:9b6737b144fc 158 {
zebrin1422 0:9b6737b144fc 159 fprintf(log_fp,"%f\t",sd_buff[iter][number]);
zebrin1422 0:9b6737b144fc 160 //pc.printf("%f\t",sd_buff[iter][number]);
zebrin1422 0:9b6737b144fc 161 }
zebrin1422 0:9b6737b144fc 162 fprintf(log_fp, "\r\n");
zebrin1422 0:9b6737b144fc 163 for(iter=0;iter<8;iter++)
zebrin1422 0:9b6737b144fc 164 {
zebrin1422 0:9b6737b144fc 165 fprintf(log_fp,"%f\t",time_buff[iter][number]);
zebrin1422 0:9b6737b144fc 166 //pc.printf("%f\t", time_buff[iter][number]);
zebrin1422 0:9b6737b144fc 167 }
zebrin1422 0:9b6737b144fc 168 //pc.printf("\r\n");
zebrin1422 0:9b6737b144fc 169 buff_count[number] = 0;
zebrin1422 0:9b6737b144fc 170 SD_BUFF_COUNT_FLAG[number] = 0;
zebrin1422 0:9b6737b144fc 171 //sd_using_flag = false;
zebrin1422 0:9b6737b144fc 172 //pc.printf("sd write\r\n");
zebrin1422 0:9b6737b144fc 173 //}
zebrin1422 0:9b6737b144fc 174 }
zebrin1422 0:9b6737b144fc 175
zebrin1422 0:9b6737b144fc 176 void write_sd()
zebrin1422 0:9b6737b144fc 177 {
zebrin1422 0:9b6737b144fc 178 for(iter=0;iter<DATA_NUMBER;iter++)if(SD_BUFF_COUNT_FLAG[iter]==1)write_sd(iter);
zebrin1422 0:9b6737b144fc 179 }
zebrin1422 0:9b6737b144fc 180 /*******************************************************************************
zebrin1422 0:9b6737b144fc 181 新しいセンサの設定を書くこと！！！！！！！！
zebrin1422 0:9b6737b144fc 182 *******************************************************************************/
zebrin1422 0:9b6737b144fc 183 /*******************************************************************************
zebrin1422 0:9b6737b144fc 184 GPSシリアル通信
zebrin1422 0:9b6737b144fc 185 *******************************************************************************/
zebrin1422 0:9b6737b144fc 186 #define GPS_TX p13
zebrin1422 0:9b6737b144fc 187 #define GPS_RX p14
zebrin1422 0:9b6737b144fc 188
zebrin1422 0:9b6737b144fc 189 Serial gps_bus(GPS_TX, GPS_RX);
zebrin1422 0:9b6737b144fc 190 GPS_interrupt gps(&gps_bus);
zebrin1422 0:9b6737b144fc 191 Ticker auto_GPS_Lat_Long;
zebrin1422 0:9b6737b144fc 192 const int lat_number = 1;
zebrin1422 0:9b6737b144fc 193 const int long_number = 2;
zebrin1422 0:9b6737b144fc 194 const int gps_alt_number = 3;
zebrin1422 0:9b6737b144fc 195
zebrin1422 0:9b6737b144fc 196 Ticker gps_read;
zebrin1422 0:9b6737b144fc 197
zebrin1422 0:9b6737b144fc 198 //Mutex gps_mutex;
zebrin1422 0:9b6737b144fc 199
zebrin1422 0:9b6737b144fc 200 /*
zebrin1422 0:9b6737b144fc 201 void gps_thread()
zebrin1422 0:9b6737b144fc 202 {
zebrin1422 0:9b6737b144fc 203 while(1)
zebrin1422 0:9b6737b144fc 204 {
zebrin1422 0:9b6737b144fc 205 gps_mutex.lock();
zebrin1422 0:9b6737b144fc 206 gps.getPosition(&hr.longitude, &hr.latitude);
zebrin1422 0:9b6737b144fc 207 write_buff(lat_number, hr.latitude);
zebrin1422 0:9b6737b144fc 208 write_buff(long_number, hr.longitude);
zebrin1422 0:9b6737b144fc 209 gps_mutex.unlock();
zebrin1422 0:9b6737b144fc 210 Thread::wait(1500);
zebrin1422 0:9b6737b144fc 211 }
zebrin1422 0:9b6737b144fc 212
zebrin1422 0:9b6737b144fc 213 }*/
zebrin1422 0:9b6737b144fc 214
zebrin1422 0:9b6737b144fc 215 void get_gps()
zebrin1422 0:9b6737b144fc 216 {
zebrin1422 0:9b6737b144fc 217 if(!interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 218 {
zebrin1422 0:9b6737b144fc 219 float xy[2];
zebrin1422 0:9b6737b144fc 220 gps.getPosition(xy);
zebrin1422 0:9b6737b144fc 221 hr.gps_alt = gps.Height();
zebrin1422 0:9b6737b144fc 222 hr.longitude = xy[0];
zebrin1422 0:9b6737b144fc 223 hr.latitude = xy[1];
zebrin1422 0:9b6737b144fc 224 write_buff(lat_number, hr.latitude);
zebrin1422 0:9b6737b144fc 225 write_buff(long_number, hr.longitude);
zebrin1422 0:9b6737b144fc 226 write_buff(gps_alt_number, hr.gps_alt);
zebrin1422 0:9b6737b144fc 227
zebrin1422 0:9b6737b144fc 228 }
zebrin1422 0:9b6737b144fc 229 }
zebrin1422 0:9b6737b144fc 230
zebrin1422 0:9b6737b144fc 231
zebrin1422 0:9b6737b144fc 232 /*******************************************************************************
zebrin1422 0:9b6737b144fc 233 INA226
zebrin1422 0:9b6737b144fc 234 *******************************************************************************/
zebrin1422 0:9b6737b144fc 235 myINA226 external_ina(i2c_Bus, myINA226::A1_VDD, myINA226::A0_VDD);
zebrin1422 0:9b6737b144fc 236 myINA226 inner_ina(i2c_Bus, myINA226::A1_GND, myINA226::A0_GND);
zebrin1422 0:9b6737b144fc 237 const int volt_ex_number = 4;
zebrin1422 0:9b6737b144fc 238 const int current_ex_number = 5;
zebrin1422 0:9b6737b144fc 239 const int volt_in_number = 6;
zebrin1422 0:9b6737b144fc 240 const int current_in_number = 7;
zebrin1422 0:9b6737b144fc 241
zebrin1422 0:9b6737b144fc 242 Ticker ina_read;
zebrin1422 0:9b6737b144fc 243
zebrin1422 0:9b6737b144fc 244 void get_ex_ina()
zebrin1422 0:9b6737b144fc 245 {
zebrin1422 0:9b6737b144fc 246 if(!i2c_using_flag && !interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 247 {
zebrin1422 0:9b6737b144fc 248 i2c_using_flag = true;
zebrin1422 0:9b6737b144fc 249 external_ina.get_Voltage_current(&hr.vol_external, &hr.current_external);
zebrin1422 0:9b6737b144fc 250 i2c_using_flag = false;
zebrin1422 0:9b6737b144fc 251 write_buff(volt_ex_number, hr.vol_external);
zebrin1422 0:9b6737b144fc 252 write_buff(current_ex_number, hr.current_external);
zebrin1422 0:9b6737b144fc 253 //pc.printf("ex_volt = %f\tex_current = %f\r\n",hr.vol_external,hr.current_external);
zebrin1422 0:9b6737b144fc 254 }
zebrin1422 0:9b6737b144fc 255 }
zebrin1422 0:9b6737b144fc 256
zebrin1422 0:9b6737b144fc 257 void get_in_ina()
zebrin1422 0:9b6737b144fc 258 {
zebrin1422 0:9b6737b144fc 259 if(!i2c_using_flag && !interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 260 {
zebrin1422 0:9b6737b144fc 261 i2c_using_flag = true;
zebrin1422 0:9b6737b144fc 262 inner_ina.get_Voltage_current(&hr.vol_inner, &hr.current_inner);
zebrin1422 0:9b6737b144fc 263 i2c_using_flag = false;
zebrin1422 0:9b6737b144fc 264 write_buff(volt_in_number, hr.vol_inner);
zebrin1422 0:9b6737b144fc 265 write_buff(current_in_number, hr.current_inner);
zebrin1422 0:9b6737b144fc 266 //pc.printf("in_volt = %f\tin_current = %\r\n",hr.vol_inner,hr.current_inner);
zebrin1422 0:9b6737b144fc 267 }
zebrin1422 0:9b6737b144fc 268 }
zebrin1422 0:9b6737b144fc 269
zebrin1422 0:9b6737b144fc 270 void get_ina()
zebrin1422 0:9b6737b144fc 271 {
zebrin1422 0:9b6737b144fc 272 get_in_ina();
zebrin1422 0:9b6737b144fc 273 get_ex_ina();
zebrin1422 0:9b6737b144fc 274 }
zebrin1422 0:9b6737b144fc 275 /*******************************************************************************
zebrin1422 0:9b6737b144fc 276 LPS22HB
zebrin1422 0:9b6737b144fc 277 *******************************************************************************/
zebrin1422 0:9b6737b144fc 278
zebrin1422 0:9b6737b144fc 279 pqLPS22HB_lib myLPS22HB(pqLPS22HB_lib::AD0_LOW,i2c_Bus);
zebrin1422 0:9b6737b144fc 280
zebrin1422 0:9b6737b144fc 281 static int altitude_count = 0;
zebrin1422 0:9b6737b144fc 282 float P_0,T_0; //高度計算に必要な地上での気圧、温度
zebrin1422 0:9b6737b144fc 283 const int press_number = 8;
zebrin1422 0:9b6737b144fc 284 const int alt_number = 9;
zebrin1422 0:9b6737b144fc 285 const int vel_number = 10;
zebrin1422 0:9b6737b144fc 286 bool alt_count = false;
zebrin1422 0:9b6737b144fc 287
zebrin1422 0:9b6737b144fc 288 Ticker lps_read;
zebrin1422 0:9b6737b144fc 289
zebrin1422 0:9b6737b144fc 290 char pres_count = 0;
zebrin1422 0:9b6737b144fc 291
zebrin1422 0:9b6737b144fc 292 void get_lps_press()
zebrin1422 0:9b6737b144fc 293 {
zebrin1422 0:9b6737b144fc 294 if(!i2c_using_flag && !interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 295 {
zebrin1422 0:9b6737b144fc 296 i2c_using_flag = true;
zebrin1422 0:9b6737b144fc 297 hr.pressure = myLPS22HB.getPres();
zebrin1422 0:9b6737b144fc 298 i2c_using_flag = false;
zebrin1422 0:9b6737b144fc 299 write_buff(press_number, hr.pressure);
zebrin1422 0:9b6737b144fc 300 //pc.printf("press = %f\r\n",hr.pressure);
zebrin1422 0:9b6737b144fc 301 }
zebrin1422 0:9b6737b144fc 302 }
zebrin1422 0:9b6737b144fc 303
zebrin1422 0:9b6737b144fc 304 float alt_to_vel[5] = {};
zebrin1422 0:9b6737b144fc 305 float delta_T;
zebrin1422 0:9b6737b144fc 306 float delta_alt;
zebrin1422 0:9b6737b144fc 307 float oldAtitude;
zebrin1422 0:9b6737b144fc 308 float newAtitude;
zebrin1422 0:9b6737b144fc 309 float oldTime = 0;
zebrin1422 0:9b6737b144fc 310
zebrin1422 0:9b6737b144fc 311 /*
zebrin1422 0:9b6737b144fc 312 void get_lps_vel()
zebrin1422 0:9b6737b144fc 313 {
zebrin1422 0:9b6737b144fc 314 if(!interrupt_prohibit_flag && !i2c_using_flag)
zebrin1422 0:9b6737b144fc 315 {
zebrin1422 0:9b6737b144fc 316 sum = 0;
zebrin1422 0:9b6737b144fc 317 for(iter = 0; iter < 10; iter ++) sum += alt_to_vel[iter];
zebrin1422 0:9b6737b144fc 318
zebrin1422 0:9b6737b144fc 319 newAtitude = sum / 10;
zebrin1422 0:9b6737b144fc 320 delta_alt = newAtitude - oldAtitude;
zebrin1422 0:9b6737b144fc 321 delta_T = Inner_Time.read() - oldTime;
zebrin1422 0:9b6737b144fc 322 hr.velocity = sum / delta_T;
zebrin1422 0:9b6737b144fc 323 write_buff(vel_number, hr.velocity);
zebrin1422 0:9b6737b144fc 324 oldTime = Inner_Time.read();
zebrin1422 0:9b6737b144fc 325 oldAtitude;
zebrin1422 0:9b6737b144fc 326 ///*
zebrin1422 0:9b6737b144fc 327 i2c_using_flag = true;
zebrin1422 0:9b6737b144fc 328 delta_alt = myLPS22HB.getAlt(P_0,T_0);
zebrin1422 0:9b6737b144fc 329 i2c_using_flag = false;
zebrin1422 0:9b6737b144fc 330 delta_T = Inner_Time.read() - differential_time;
zebrin1422 0:9b6737b144fc 331 hr.velocity = delta_alt / delta_T;
zebrin1422 0:9b6737b144fc 332 //
zebrin1422 0:9b6737b144fc 333 write_buff(vel_number, hr.velocity);
zebrin1422 0:9b6737b144fc 334 if(hr.Rocket_Phase == 4 && hr.velocity < 1 && hr.velocity > -1 && altitude_accept)vertex_arrival = true;
zebrin1422 0:9b6737b144fc 335 //3m/sが多分本番時の頂点検知の閾値
zebrin1422 0:9b6737b144fc 336 }
zebrin1422 0:9b6737b144fc 337 } */
zebrin1422 0:9b6737b144fc 338
zebrin1422 0:9b6737b144fc 339 const float flight_min_altitude = 5;
zebrin1422 0:9b6737b144fc 340
zebrin1422 0:9b6737b144fc 341 void get_lps_alt()
zebrin1422 0:9b6737b144fc 342 {
zebrin1422 0:9b6737b144fc 343 if(!i2c_using_flag && !interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 344 {
zebrin1422 0:9b6737b144fc 345 i2c_using_flag = true;
zebrin1422 0:9b6737b144fc 346 hr.altitude = myLPS22HB.getAlt(P_0,T_0);
zebrin1422 0:9b6737b144fc 347 i2c_using_flag = false;
zebrin1422 0:9b6737b144fc 348 alt_to_vel[altitude_count] = hr.altitude;
zebrin1422 0:9b6737b144fc 349 altitude_count ++;
zebrin1422 0:9b6737b144fc 350 if(alt_count && altitude_count >= 10)
zebrin1422 0:9b6737b144fc 351 {
zebrin1422 0:9b6737b144fc 352 altitude_count = 0;
zebrin1422 0:9b6737b144fc 353 sum=0;
zebrin1422 0:9b6737b144fc 354 for(iter=0;iter<10;iter++)sum += alt_to_vel[iter];
zebrin1422 0:9b6737b144fc 355 delta_T = (Inner_Time.read_us() - oldTime)/1000000.0;
zebrin1422 0:9b6737b144fc 356 newAtitude = sum/10;
zebrin1422 0:9b6737b144fc 357 delta_alt = newAtitude - oldAtitude;
zebrin1422 0:9b6737b144fc 358 hr.velocity = delta_alt / delta_T;
zebrin1422 0:9b6737b144fc 359 write_buff(vel_number, hr.velocity);
zebrin1422 0:9b6737b144fc 360 oldTime = Inner_Time.read_us();
zebrin1422 0:9b6737b144fc 361 oldAtitude = newAtitude;
zebrin1422 0:9b6737b144fc 362 alt_count = true;
zebrin1422 0:9b6737b144fc 363 //pc.printf("velocity = %f\r\n",hr.velocity);
zebrin1422 0:9b6737b144fc 364 }
zebrin1422 0:9b6737b144fc 365 write_buff(alt_number, hr.altitude);
zebrin1422 0:9b6737b144fc 366 //pc.printf("altitude = %f\r\n",hr.altitude);
zebrin1422 0:9b6737b144fc 367 if(!altitude_accept && hr.altitude > 10)altitude_accept=true;
zebrin1422 0:9b6737b144fc 368 if(hr.Rocket_Phase == 4 && hr.velocity < 1 && hr.velocity > -1 && altitude_accept)vertex_arrival = true;
zebrin1422 0:9b6737b144fc 369 }
zebrin1422 0:9b6737b144fc 370 }
zebrin1422 0:9b6737b144fc 371
zebrin1422 0:9b6737b144fc 372 void get_lps()
zebrin1422 0:9b6737b144fc 373 {
zebrin1422 0:9b6737b144fc 374 //pc.printf("call ex\r\n");
zebrin1422 0:9b6737b144fc 375 if(!i2c_using_flag && !interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 376 {
zebrin1422 0:9b6737b144fc 377 //pc.printf("call in\r\n");
zebrin1422 0:9b6737b144fc 378 i2c_using_flag = true;
zebrin1422 0:9b6737b144fc 379 hr.altitude = myLPS22HB.getAlt(P_0,T_0);
zebrin1422 0:9b6737b144fc 380 hr.pressure = myLPS22HB.getPres();
zebrin1422 0:9b6737b144fc 381 i2c_using_flag = false;
zebrin1422 0:9b6737b144fc 382 //pc.printf("pressure = %f\r\n",hr.pressure);
zebrin1422 0:9b6737b144fc 383 write_buff(alt_number, hr.altitude);
zebrin1422 0:9b6737b144fc 384 write_buff(press_number, hr.pressure);
zebrin1422 0:9b6737b144fc 385 alt_to_vel[altitude_count] = hr.altitude;
zebrin1422 0:9b6737b144fc 386 altitude_count ++;
zebrin1422 0:9b6737b144fc 387 if(alt_count && altitude_count >= 10)
zebrin1422 0:9b6737b144fc 388 {
zebrin1422 0:9b6737b144fc 389 altitude_count = 0;
zebrin1422 0:9b6737b144fc 390 sum=0;
zebrin1422 0:9b6737b144fc 391 for(iter=0;iter<10;iter++)sum += alt_to_vel[iter];
zebrin1422 0:9b6737b144fc 392 delta_T = (Inner_Time.read_us() - oldTime)/1000000.0;
zebrin1422 0:9b6737b144fc 393 newAtitude = sum/10;
zebrin1422 0:9b6737b144fc 394 delta_alt = newAtitude - oldAtitude;
zebrin1422 0:9b6737b144fc 395 hr.velocity = delta_alt / delta_T;
zebrin1422 0:9b6737b144fc 396 write_buff(vel_number, hr.velocity);
zebrin1422 0:9b6737b144fc 397 oldTime = Inner_Time.read_us();
zebrin1422 0:9b6737b144fc 398 oldAtitude = newAtitude;
zebrin1422 0:9b6737b144fc 399 alt_count = true;
zebrin1422 0:9b6737b144fc 400 //pc.printf("velocity = %f\r\n",hr.velocity);
zebrin1422 0:9b6737b144fc 401 }
zebrin1422 0:9b6737b144fc 402 //pc.printf("altitude = %f\r\n",hr.altitude);
zebrin1422 0:9b6737b144fc 403 if(!altitude_accept && hr.altitude > 10)altitude_accept=true;
zebrin1422 0:9b6737b144fc 404 if(hr.Rocket_Phase == 4 && hr.velocity < 1 && hr.velocity > -1 && altitude_accept)vertex_arrival = true;
zebrin1422 0:9b6737b144fc 405 }
zebrin1422 0:9b6737b144fc 406 }
zebrin1422 0:9b6737b144fc 407
zebrin1422 0:9b6737b144fc 408 /*******************************************************************************
zebrin1422 0:9b6737b144fc 409 MPU9250
zebrin1422 0:9b6737b144fc 410 *******************************************************************************/
zebrin1422 0:9b6737b144fc 411 #define SET_ACC_LPF NO_USE
zebrin1422 0:9b6737b144fc 412 #define SET_GYRO _1000DPS
zebrin1422 0:9b6737b144fc 413 #define SET_ACC _16G
zebrin1422 0:9b6737b144fc 414
zebrin1422 0:9b6737b144fc 415 mpu9250 mympu9250(i2c_Bus);
zebrin1422 0:9b6737b144fc 416 //const int imu_number = 11;
zebrin1422 0:9b6737b144fc 417 //const int mag_number = 12;
zebrin1422 0:9b6737b144fc 418
zebrin1422 0:9b6737b144fc 419 const double mpu_offset[9] = {};
zebrin1422 0:9b6737b144fc 420
zebrin1422 0:9b6737b144fc 421 static double imu[6];
zebrin1422 0:9b6737b144fc 422 static double mag[3];
zebrin1422 0:9b6737b144fc 423
zebrin1422 0:9b6737b144fc 424 /*******************************************************************************
zebrin1422 0:9b6737b144fc 425 SHT-35
zebrin1422 0:9b6737b144fc 426 *******************************************************************************/
zebrin1422 0:9b6737b144fc 427 mySHT3x mySHT35(i2c_Bus, mySHT3x::AD0_LOW);
zebrin1422 0:9b6737b144fc 428
zebrin1422 0:9b6737b144fc 429 const int SHT_temp_number = 11;
zebrin1422 0:9b6737b144fc 430 const int SHT_hum_number = 12;
zebrin1422 0:9b6737b144fc 431
zebrin1422 0:9b6737b144fc 432 Ticker sht_read;
zebrin1422 0:9b6737b144fc 433
zebrin1422 0:9b6737b144fc 434 void get_sht()
zebrin1422 0:9b6737b144fc 435 {
zebrin1422 0:9b6737b144fc 436 if(!i2c_using_flag && !interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 437 {
zebrin1422 0:9b6737b144fc 438 i2c_using_flag = true;
zebrin1422 0:9b6737b144fc 439 mySHT35.get_temp_hum(&hr.temperature, &hr.humidity);
zebrin1422 0:9b6737b144fc 440 i2c_using_flag = false;
zebrin1422 0:9b6737b144fc 441 write_buff(SHT_temp_number, hr.temperature);
zebrin1422 0:9b6737b144fc 442 write_buff(SHT_hum_number, hr.humidity);
zebrin1422 0:9b6737b144fc 443 //pc.printf("temp = %f\r\nhum = %f\r\n",hr.temperature,hr.humidity);
zebrin1422 0:9b6737b144fc 444 }
zebrin1422 0:9b6737b144fc 445 }
zebrin1422 0:9b6737b144fc 446
zebrin1422 0:9b6737b144fc 447 /*******************************************************************************
zebrin1422 0:9b6737b144fc 448 TSL2561
zebrin1422 0:9b6737b144fc 449 *******************************************************************************/
zebrin1422 0:9b6737b144fc 450 myTSL2561 myTSL2561(i2c_Bus, myTSL2561::AD0_OPEN);
zebrin1422 0:9b6737b144fc 451 const int lux_number = 13;
zebrin1422 0:9b6737b144fc 452
zebrin1422 0:9b6737b144fc 453 Ticker tsl_read;
zebrin1422 0:9b6737b144fc 454
zebrin1422 0:9b6737b144fc 455 void get_tsl()
zebrin1422 0:9b6737b144fc 456 {
zebrin1422 0:9b6737b144fc 457 if(!i2c_using_flag && !interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 458 {
zebrin1422 0:9b6737b144fc 459 i2c_using_flag = true;
zebrin1422 0:9b6737b144fc 460 hr.luminosity = myTSL2561.get_luminosity(hr.TSL_time);
zebrin1422 0:9b6737b144fc 461 i2c_using_flag = false;
zebrin1422 0:9b6737b144fc 462 write_buff(lux_number, hr.luminosity);
zebrin1422 0:9b6737b144fc 463 //pc.printf("lux = %f\r\n",hr.luminosity);
zebrin1422 0:9b6737b144fc 464 }
zebrin1422 0:9b6737b144fc 465 }
zebrin1422 0:9b6737b144fc 466
zebrin1422 0:9b6737b144fc 467 /*******************************************************************************
zebrin1422 0:9b6737b144fc 468 ADXL375
zebrin1422 0:9b6737b144fc 469 *******************************************************************************/
zebrin1422 0:9b6737b144fc 470 ADXL375_I2C three(I2C_SDA, I2C_SCL);
zebrin1422 0:9b6737b144fc 471
zebrin1422 0:9b6737b144fc 472 const int three_x = 14;
zebrin1422 0:9b6737b144fc 473 const int three_y = 15;
zebrin1422 0:9b6737b144fc 474 const int three_z = 16;
zebrin1422 0:9b6737b144fc 475
zebrin1422 0:9b6737b144fc 476 int three_acc[3];
zebrin1422 0:9b6737b144fc 477
zebrin1422 0:9b6737b144fc 478 #define ADXL_DEVICE_ID 0xE5
zebrin1422 0:9b6737b144fc 479 #define DATA_FORMAT_CONTORL 0x08
zebrin1422 0:9b6737b144fc 480
zebrin1422 0:9b6737b144fc 481 Ticker adxl_read;
zebrin1422 0:9b6737b144fc 482
zebrin1422 0:9b6737b144fc 483 void get_adxl()
zebrin1422 0:9b6737b144fc 484 {
zebrin1422 0:9b6737b144fc 485 if(!i2c_using_flag && !interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 486 {
zebrin1422 0:9b6737b144fc 487 i2c_using_flag = true;
zebrin1422 0:9b6737b144fc 488 three.getOutput(three_acc);
zebrin1422 0:9b6737b144fc 489 i2c_using_flag = false;
zebrin1422 0:9b6737b144fc 490 for(iter=0;iter<3;iter++)hr.adxl_acc[iter] = (float)three_acc[iter]*200/4096;
zebrin1422 0:9b6737b144fc 491 write_buff(three_x, hr.adxl_acc[0]);
zebrin1422 0:9b6737b144fc 492 write_buff(three_y, hr.adxl_acc[1]);
zebrin1422 0:9b6737b144fc 493 write_buff(three_z, hr.adxl_acc[2]);
zebrin1422 0:9b6737b144fc 494 }
zebrin1422 0:9b6737b144fc 495 }
zebrin1422 0:9b6737b144fc 496
zebrin1422 0:9b6737b144fc 497 /*******************************************************************************
zebrin1422 0:9b6737b144fc 498 ピトー管
zebrin1422 0:9b6737b144fc 499 *******************************************************************************/
zebrin1422 0:9b6737b144fc 500 #define PITOT_PIN p20
zebrin1422 0:9b6737b144fc 501
zebrin1422 0:9b6737b144fc 502 AnalogIn pitot(PITOT_PIN);
zebrin1422 0:9b6737b144fc 503 const int pitot_number = 17;
zebrin1422 0:9b6737b144fc 504
zebrin1422 0:9b6737b144fc 505 Ticker pitot_press;
zebrin1422 0:9b6737b144fc 506
zebrin1422 0:9b6737b144fc 507 void get_pitot()
zebrin1422 0:9b6737b144fc 508 {
zebrin1422 0:9b6737b144fc 509 if(!interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 510 {
zebrin1422 0:9b6737b144fc 511 hr.press_pitot = pitot;
zebrin1422 0:9b6737b144fc 512 write_buff(pitot_number, hr.press_pitot);
zebrin1422 0:9b6737b144fc 513 }
zebrin1422 0:9b6737b144fc 514 }
zebrin1422 0:9b6737b144fc 515 /*******************************************************************************
zebrin1422 0:9b6737b144fc 516 ひずみゲージ
zebrin1422 0:9b6737b144fc 517 *******************************************************************************/
zebrin1422 0:9b6737b144fc 518 //HIZUMI instance make
zebrin1422 0:9b6737b144fc 519 #define STRAIN_PIN p29
zebrin1422 0:9b6737b144fc 520
zebrin1422 0:9b6737b144fc 521 DigitalOut strain_state(STRAIN_PIN);
zebrin1422 0:9b6737b144fc 522
zebrin1422 0:9b6737b144fc 523 /*******************************************************************************
zebrin1422 0:9b6737b144fc 524 ES920LR
zebrin1422 0:9b6737b144fc 525 *******************************************************************************/
zebrin1422 0:9b6737b144fc 526 #define ES_TX p9
zebrin1422 0:9b6737b144fc 527 #define ES_RX p10
zebrin1422 0:9b6737b144fc 528 #define ES_PIN p11
zebrin1422 0:9b6737b144fc 529
zebrin1422 0:9b6737b144fc 530 RawSerial es_bus(ES_TX,ES_RX);
zebrin1422 0:9b6737b144fc 531 ES920LR es(es_bus, pc,115200);
zebrin1422 0:9b6737b144fc 532 //DigitalIn busy(ES_PIN);
zebrin1422 0:9b6737b144fc 533 /*******************************************************************************
zebrin1422 0:9b6737b144fc 534 ニクロム線
zebrin1422 0:9b6737b144fc 535 *******************************************************************************/
zebrin1422 0:9b6737b144fc 536 #define NICROM_PIN p24
zebrin1422 0:9b6737b144fc 537
zebrin1422 0:9b6737b144fc 538 Nicrom nicrom(NICROM_PIN);
zebrin1422 0:9b6737b144fc 539 /*******************************************************************************
zebrin1422 0:9b6737b144fc 540 フライトピン
zebrin1422 0:9b6737b144fc 541 *******************************************************************************/
zebrin1422 0:9b6737b144fc 542 #define FLIGHT_PIN p26
zebrin1422 0:9b6737b144fc 543 InterruptIn flight_pin(FLIGHT_PIN);
zebrin1422 0:9b6737b144fc 544
zebrin1422 0:9b6737b144fc 545 void flightPin_detect()
zebrin1422 0:9b6737b144fc 546 {
zebrin1422 0:9b6737b144fc 547 interrupt_prohibit_flag = true;
zebrin1422 0:9b6737b144fc 548 hr.flight_pin_state = true;
zebrin1422 0:9b6737b144fc 549 //flight_detect_mode = true;
zebrin1422 0:9b6737b144fc 550 pc.printf("flight pin removed\r\n");
zebrin1422 0:9b6737b144fc 551 //flight_mode=true;
zebrin1422 0:9b6737b144fc 552 interrupt_prohibit_flag = false;
zebrin1422 0:9b6737b144fc 553 }
zebrin1422 0:9b6737b144fc 554
zebrin1422 0:9b6737b144fc 555 /*******************************************************************************
zebrin1422 0:9b6737b144fc 556 発射待機、安全装置等
zebrin1422 0:9b6737b144fc 557 *******************************************************************************/
zebrin1422 0:9b6737b144fc 558
zebrin1422 0:9b6737b144fc 559 //safety device release
zebrin1422 0:9b6737b144fc 560
zebrin1422 0:9b6737b144fc 561 void safety_device()
zebrin1422 0:9b6737b144fc 562 {
zebrin1422 0:9b6737b144fc 563 while(!safety_flag)
zebrin1422 0:9b6737b144fc 564 {
zebrin1422 0:9b6737b144fc 565 //flight_detect_mode = true;
zebrin1422 0:9b6737b144fc 566 //pc.printf("don't accept standby\r\n");
zebrin1422 0:9b6737b144fc 567 //wait(1.0f);
zebrin1422 0:9b6737b144fc 568 //if(Inner_Time.read() > 30)break;
zebrin1422 0:9b6737b144fc 569 wait_ms(50);
zebrin1422 0:9b6737b144fc 570 hr.flight_pin_state = false;
zebrin1422 0:9b6737b144fc 571 }
zebrin1422 0:9b6737b144fc 572 //wait_ms(500);
zebrin1422 0:9b6737b144fc 573 safety_device_reboot = false;
zebrin1422 0:9b6737b144fc 574 es << (char)0x01 << (char) 9 << es.endl; //ぷラキューの9
zebrin1422 0:9b6737b144fc 575 }
zebrin1422 0:9b6737b144fc 576
zebrin1422 0:9b6737b144fc 577 //flight detect mode
zebrin1422 0:9b6737b144fc 578 //int flightPin_detect_count=0;
zebrin1422 0:9b6737b144fc 579 //Timeout flightPin_dummy;
zebrin1422 0:9b6737b144fc 580
zebrin1422 0:9b6737b144fc 581 #define DETECT_TIME 50
zebrin1422 0:9b6737b144fc 582
zebrin1422 0:9b6737b144fc 583 void flight_detect()
zebrin1422 0:9b6737b144fc 584 {
zebrin1422 0:9b6737b144fc 585 //pc.printf("detect start\r\n");
zebrin1422 0:9b6737b144fc 586 if(flight_detect_mode)
zebrin1422 0:9b6737b144fc 587 {
zebrin1422 0:9b6737b144fc 588 //pc.printf("detect a\r\n");
zebrin1422 0:9b6737b144fc 589 while(!hr.flight_pin_state)
zebrin1422 0:9b6737b144fc 590 {
zebrin1422 0:9b6737b144fc 591 //pc.printf("detect b\r\n");
zebrin1422 0:9b6737b144fc 592
zebrin1422 0:9b6737b144fc 593 /*
zebrin1422 0:9b6737b144fc 594 if(flightPin_dummy.read() > DETECT_TIME)
zebrin1422 0:9b6737b144fc 595 {
zebrin1422 0:9b6737b144fc 596 // pc.printf("detect c\r\n");
zebrin1422 0:9b6737b144fc 597 hr.flight_pin_state = true;
zebrin1422 0:9b6737b144fc 598 flight_mode = true;
zebrin1422 0:9b6737b144fc 599 pc.printf("flight mode true\r\n");
zebrin1422 0:9b6737b144fc 600 break;
zebrin1422 0:9b6737b144fc 601 }
zebrin1422 0:9b6737b144fc 602 */
zebrin1422 0:9b6737b144fc 603 if(hr.flight_pin_state)break;
zebrin1422 0:9b6737b144fc 604 P_0 = hr.pressure;
zebrin1422 0:9b6737b144fc 605 T_0 = hr.temperature;
zebrin1422 0:9b6737b144fc 606 wait_ms(10);
zebrin1422 0:9b6737b144fc 607 //pc.printf("roop\r\n");
zebrin1422 0:9b6737b144fc 608 if(safety_device_reboot)safety_device();
zebrin1422 0:9b6737b144fc 609 }
zebrin1422 0:9b6737b144fc 610 }
zebrin1422 0:9b6737b144fc 611 }
zebrin1422 0:9b6737b144fc 612
zebrin1422 0:9b6737b144fc 613
zebrin1422 0:9b6737b144fc 614 /*******************************************************************************
zebrin1422 0:9b6737b144fc 615 分離機構
zebrin1422 0:9b6737b144fc 616 *******************************************************************************/
zebrin1422 0:9b6737b144fc 617 #define SOLENOID_PIN p25
zebrin1422 0:9b6737b144fc 618 #define EXTRUSION_FLIGHT_PIN p22
zebrin1422 0:9b6737b144fc 619
zebrin1422 0:9b6737b144fc 620 DigitalOut solenoid(SOLENOID_PIN);
zebrin1422 0:9b6737b144fc 621
zebrin1422 0:9b6737b144fc 622
zebrin1422 0:9b6737b144fc 623 //プロトタイプ宣言
zebrin1422 0:9b6737b144fc 624 void separate_action();
zebrin1422 0:9b6737b144fc 625 void extrusion_action();
zebrin1422 0:9b6737b144fc 626 void collect();
zebrin1422 0:9b6737b144fc 627 void nicrom_low();
zebrin1422 0:9b6737b144fc 628 void solenoid_low();
zebrin1422 0:9b6737b144fc 629
zebrin1422 0:9b6737b144fc 630 void header0x06();
zebrin1422 0:9b6737b144fc 631 void header0x07();
zebrin1422 0:9b6737b144fc 632
zebrin1422 0:9b6737b144fc 633 short solenoid_count = 0;
zebrin1422 0:9b6737b144fc 634
zebrin1422 0:9b6737b144fc 635 #define OSHIDASHI_TIME 0.5
zebrin1422 0:9b6737b144fc 636
zebrin1422 0:9b6737b144fc 637 //最低10回はプシュプシュさせる、回転数はなるはやで
zebrin1422 0:9b6737b144fc 638 //0.1秒ごとにHIGH,LOWの切り替え?
zebrin1422 0:9b6737b144fc 639
zebrin1422 0:9b6737b144fc 640 void oshidashi()
zebrin1422 0:9b6737b144fc 641 {
zebrin1422 0:9b6737b144fc 642 pc.printf("oshidashi start\r\n");
zebrin1422 0:9b6737b144fc 643 while(!solenoid_complete)
zebrin1422 0:9b6737b144fc 644 {
zebrin1422 0:9b6737b144fc 645 solenoid = 1;
zebrin1422 0:9b6737b144fc 646 wait(OSHIDASHI_TIME);
zebrin1422 0:9b6737b144fc 647 solenoid = 0;
zebrin1422 0:9b6737b144fc 648 wait(OSHIDASHI_TIME);
zebrin1422 0:9b6737b144fc 649 pc.printf("iter = %d\r\n",solenoid_count);
zebrin1422 0:9b6737b144fc 650 solenoid_count ++;
zebrin1422 0:9b6737b144fc 651 if(solenoid_count >=10)solenoid_complete = true;
zebrin1422 0:9b6737b144fc 652 }
zebrin1422 0:9b6737b144fc 653 header0x07();
zebrin1422 0:9b6737b144fc 654 }
zebrin1422 0:9b6737b144fc 655
zebrin1422 0:9b6737b144fc 656 DigitalOut led_3(LED3);
zebrin1422 0:9b6737b144fc 657
zebrin1422 0:9b6737b144fc 658 #define FIRE_TIME 5
zebrin1422 0:9b6737b144fc 659
zebrin1422 0:9b6737b144fc 660 void separate()
zebrin1422 0:9b6737b144fc 661 {
zebrin1422 0:9b6737b144fc 662 led_3 = 1;
zebrin1422 0:9b6737b144fc 663 nicrom.fire();
zebrin1422 0:9b6737b144fc 664 wait(FIRE_TIME);
zebrin1422 0:9b6737b144fc 665 nicrom.stop();
zebrin1422 0:9b6737b144fc 666 led_3 = 0;
zebrin1422 0:9b6737b144fc 667 header0x06();
zebrin1422 0:9b6737b144fc 668 extrusion_action();
zebrin1422 0:9b6737b144fc 669 header0x07();
zebrin1422 0:9b6737b144fc 670 //nicrom_stop.attach(&nicrom_low, 3.0);
zebrin1422 0:9b6737b144fc 671 }
zebrin1422 0:9b6737b144fc 672
zebrin1422 0:9b6737b144fc 673
zebrin1422 0:9b6737b144fc 674
zebrin1422 0:9b6737b144fc 675 // separate action start
zebrin1422 0:9b6737b144fc 676 const float sepa_prohibit_time = 7.0;
zebrin1422 0:9b6737b144fc 677 const float sepa_velocity = 0.5;
zebrin1422 0:9b6737b144fc 678 Timer sepa_start_time;
zebrin1422 0:9b6737b144fc 679
zebrin1422 0:9b6737b144fc 680 void separate_action()
zebrin1422 0:9b6737b144fc 681 {
zebrin1422 0:9b6737b144fc 682 //pc.printf("sepa begin\r\n");
zebrin1422 0:9b6737b144fc 683 if(flight_mode)
zebrin1422 0:9b6737b144fc 684 {
zebrin1422 0:9b6737b144fc 685 //pc.printf("sepa a\r\n");
zebrin1422 0:9b6737b144fc 686 if(!sepa_accept)
zebrin1422 0:9b6737b144fc 687 {
zebrin1422 0:9b6737b144fc 688 //pc.printf("sepa b\r\n");
zebrin1422 0:9b6737b144fc 689 if(flight_time.read() > sepa_prohibit_time)
zebrin1422 0:9b6737b144fc 690 {
zebrin1422 0:9b6737b144fc 691 sepa_accept=true;
zebrin1422 0:9b6737b144fc 692 pc.printf("separate\r\n");
zebrin1422 0:9b6737b144fc 693 separate();
zebrin1422 0:9b6737b144fc 694 header0x06();
zebrin1422 0:9b6737b144fc 695 }
zebrin1422 0:9b6737b144fc 696
zebrin1422 0:9b6737b144fc 697
zebrin1422 0:9b6737b144fc 698
zebrin1422 0:9b6737b144fc 699 if(altitude_accept && hr.velocity < sepa_velocity)
zebrin1422 0:9b6737b144fc 700 {
zebrin1422 0:9b6737b144fc 701 sepa_accept=true;
zebrin1422 0:9b6737b144fc 702 altitude_accept = true;
zebrin1422 0:9b6737b144fc 703 pc.printf("separate\r\n");
zebrin1422 0:9b6737b144fc 704 separate();
zebrin1422 0:9b6737b144fc 705 header0x06();
zebrin1422 0:9b6737b144fc 706 }
zebrin1422 0:9b6737b144fc 707
zebrin1422 0:9b6737b144fc 708
zebrin1422 0:9b6737b144fc 709
zebrin1422 0:9b6737b144fc 710 //pc.printf("sepa e\r\n");
zebrin1422 0:9b6737b144fc 711 }
zebrin1422 0:9b6737b144fc 712 //pc.printf("sepa f\r\n");
zebrin1422 0:9b6737b144fc 713 }
zebrin1422 0:9b6737b144fc 714 //pc.printf("sepa finish\r\n");
zebrin1422 0:9b6737b144fc 715 }
zebrin1422 0:9b6737b144fc 716
zebrin1422 0:9b6737b144fc 717 // extrusion(oshidashi) action
zebrin1422 0:9b6737b144fc 718 const float extrusion_prohibit_time = 7.0;
zebrin1422 0:9b6737b144fc 719 InterruptIn extrusion_pin(EXTRUSION_FLIGHT_PIN);
zebrin1422 0:9b6737b144fc 720
zebrin1422 0:9b6737b144fc 721 void extrusion_action() //最低10回はプシュプシュさせる、回転数はなるはやで
zebrin1422 0:9b6737b144fc 722 {
zebrin1422 0:9b6737b144fc 723 //pc.printf("ex begin\r\n"); //0.5秒ごとにHIGH,LOWの切り替え?
zebrin1422 0:9b6737b144fc 724 if(sepa_accept)
zebrin1422 0:9b6737b144fc 725 {
zebrin1422 0:9b6737b144fc 726 //pc.printf("ex a\r\n");
zebrin1422 0:9b6737b144fc 727 if(!extrusion_accept)
zebrin1422 0:9b6737b144fc 728 {
zebrin1422 0:9b6737b144fc 729 //pc.printf("ex b\r\n");
zebrin1422 0:9b6737b144fc 730 if(flight_time.read() > extrusion_prohibit_time)
zebrin1422 0:9b6737b144fc 731 {
zebrin1422 0:9b6737b144fc 732 //pc.printf("ex c\r\n");
zebrin1422 0:9b6737b144fc 733 extrusion_accept = true;
zebrin1422 0:9b6737b144fc 734 pc.printf("oshidashi\r\n");
zebrin1422 0:9b6737b144fc 735 oshidashi();
zebrin1422 0:9b6737b144fc 736 header0x07();
zebrin1422 0:9b6737b144fc 737 //extrusion_start_time.start();
zebrin1422 0:9b6737b144fc 738 }
zebrin1422 0:9b6737b144fc 739 //pc.printf("ex e\r\n");
zebrin1422 0:9b6737b144fc 740 //if(cmd receive)oshidashi_kyoka
zebrin1422 0:9b6737b144fc 741 }
zebrin1422 0:9b6737b144fc 742 //pc.printf("ex f\r\n");
zebrin1422 0:9b6737b144fc 743 }
zebrin1422 0:9b6737b144fc 744 //pc.printf("ex finish\r\n");
zebrin1422 0:9b6737b144fc 745 }
zebrin1422 0:9b6737b144fc 746
zebrin1422 0:9b6737b144fc 747 // collect mode
zebrin1422 0:9b6737b144fc 748 const float flight_min_time = 40;
zebrin1422 0:9b6737b144fc 749
zebrin1422 0:9b6737b144fc 750 void collect()
zebrin1422 0:9b6737b144fc 751 {
zebrin1422 0:9b6737b144fc 752 //pc.printf("re a\r\n");
zebrin1422 0:9b6737b144fc 753 if(flight_mode)
zebrin1422 0:9b6737b144fc 754 {
zebrin1422 0:9b6737b144fc 755 //pc.printf("re b\r\n");
zebrin1422 0:9b6737b144fc 756 if(!collect_mode)
zebrin1422 0:9b6737b144fc 757 {
zebrin1422 0:9b6737b144fc 758 //pc.printf("re c\r\n");
zebrin1422 0:9b6737b144fc 759 if(flight_time.read() > flight_min_time)
zebrin1422 0:9b6737b144fc 760 {
zebrin1422 0:9b6737b144fc 761 //pc.printf("re d\r\n");
zebrin1422 0:9b6737b144fc 762 collect_mode=true;
zebrin1422 0:9b6737b144fc 763 pc.printf("collect\r\n");
zebrin1422 0:9b6737b144fc 764 }
zebrin1422 0:9b6737b144fc 765 //if(高度が10m到達した_flag == true && hr.altitude < 30)kaisyuu mode
zebrin1422 0:9b6737b144fc 766 if(altitude_accept && hr.altitude < 30 && extrusion_accept)
zebrin1422 0:9b6737b144fc 767 {
zebrin1422 0:9b6737b144fc 768 collect_mode=true;
zebrin1422 0:9b6737b144fc 769 pc.printf("collect\r\n");
zebrin1422 0:9b6737b144fc 770 }
zebrin1422 0:9b6737b144fc 771 //pc.printf("re e\r\n");
zebrin1422 0:9b6737b144fc 772 }
zebrin1422 0:9b6737b144fc 773 //pc.printf("re f\r\n");
zebrin1422 0:9b6737b144fc 774 }
zebrin1422 0:9b6737b144fc 775 //pc.printf("re g\r\n");
zebrin1422 0:9b6737b144fc 776 }
zebrin1422 0:9b6737b144fc 777
zebrin1422 0:9b6737b144fc 778
zebrin1422 0:9b6737b144fc 779 /*******************************************************************************
zebrin1422 0:9b6737b144fc 780 姿勢計算
zebrin1422 0:9b6737b144fc 781 *******************************************************************************/
zebrin1422 0:9b6737b144fc 782
zebrin1422 0:9b6737b144fc 783 static Quaternion myQ(1.0, 0.0, 0.0, 0.0);
zebrin1422 0:9b6737b144fc 784 MadgwickFilter attitude(1.35);
zebrin1422 0:9b6737b144fc 785
zebrin1422 0:9b6737b144fc 786 const int Q_W_number=18;
zebrin1422 0:9b6737b144fc 787 const int Q_X_number=19;
zebrin1422 0:9b6737b144fc 788 const int Q_Y_number=20;
zebrin1422 0:9b6737b144fc 789 const int Q_Z_number=21;
zebrin1422 0:9b6737b144fc 790
zebrin1422 0:9b6737b144fc 791 Ticker attitude_ticker;
zebrin1422 0:9b6737b144fc 792
zebrin1422 0:9b6737b144fc 793 void get_attitude()
zebrin1422 0:9b6737b144fc 794 {
zebrin1422 0:9b6737b144fc 795 if(!i2c_using_flag && !interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 796 {
zebrin1422 0:9b6737b144fc 797 i2c_using_flag = 1;
zebrin1422 0:9b6737b144fc 798 mympu9250.getGyroAcc(imu);
zebrin1422 0:9b6737b144fc 799 mympu9250.getMag(mag);
zebrin1422 0:9b6737b144fc 800 i2c_using_flag = 0;
zebrin1422 0:9b6737b144fc 801 }
zebrin1422 0:9b6737b144fc 802 for(iter=0; iter<3; iter++) imu[iter] = imu[iter]*DEG_TO_RAG;
zebrin1422 0:9b6737b144fc 803 attitude.MadgwickAHRSupdate(imu[0],imu[1],imu[2],imu[3],imu[4],imu[5],mag[0],mag[1],mag[2]);
zebrin1422 0:9b6737b144fc 804 attitude.getAttitude(&myQ);
zebrin1422 0:9b6737b144fc 805 hr.w = myQ.w;
zebrin1422 0:9b6737b144fc 806 hr.x = myQ.x;
zebrin1422 0:9b6737b144fc 807 hr.y = myQ.y;
zebrin1422 0:9b6737b144fc 808 hr.z = myQ.z;
zebrin1422 0:9b6737b144fc 809 write_buff(Q_W_number, hr.w);
zebrin1422 0:9b6737b144fc 810 write_buff(Q_X_number, hr.x);
zebrin1422 0:9b6737b144fc 811 write_buff(Q_Y_number, hr.y);
zebrin1422 0:9b6737b144fc 812 write_buff(Q_Z_number, hr.z);
zebrin1422 0:9b6737b144fc 813 }
zebrin1422 0:9b6737b144fc 814
zebrin1422 0:9b6737b144fc 815 /*******************************************************************************
zebrin1422 0:9b6737b144fc 816 ダウンリンク
zebrin1422 0:9b6737b144fc 817 *******************************************************************************/
zebrin1422 0:9b6737b144fc 818 #define COMPRES_Inner_Time 36.40833333 //32768/1800
zebrin1422 0:9b6737b144fc 819 #define COMPRES_Q 305200 // ? pakuru
zebrin1422 0:9b6737b144fc 820 #define COMPRES_VOLTAGE 0.9102222 // 65535/36 =
zebrin1422 0:9b6737b144fc 821 #define COMPRES_CURRENT 0.8192 // 65535/40 =
zebrin1422 0:9b6737b144fc 822 #define COMPRES_PRESSURE 26.0063 // 65535/1260 =
zebrin1422 0:9b6737b144fc 823 #define COMPRES_ALTITUDE 65.536 // 65535/500 = //range = 0~500
zebrin1422 0:9b6737b144fc 824 #define COMPRES_VELOCITY 327.68 // 65535/100 = //pakuru
zebrin1422 0:9b6737b144fc 825 #define COMPRES_LUMINOSITY 3.2768 // 65535/10000 =
zebrin1422 0:9b6737b144fc 826 #define COMPRES_TEMPERATURE 546.133 // 63353/60 = //range = -20 ~ 40
zebrin1422 0:9b6737b144fc 827 #define COMPRES_HUMIDITY 327.68 // 65535/100 =
zebrin1422 0:9b6737b144fc 828 #define COMPRES_ACC 81.92 // 65535/400 =
zebrin1422 0:9b6737b144fc 829 #define COMPRES_PITOT 32768 // 65535/1 =
zebrin1422 0:9b6737b144fc 830
zebrin1422 0:9b6737b144fc 831 Ticker rocket_state;
zebrin1422 0:9b6737b144fc 832
zebrin1422 0:9b6737b144fc 833 void downlink0x03()
zebrin1422 0:9b6737b144fc 834 {
zebrin1422 0:9b6737b144fc 835
zebrin1422 0:9b6737b144fc 836 if(!interrupt_prohibit_flag)
zebrin1422 0:9b6737b144fc 837 {
zebrin1422 0:9b6737b144fc 838
zebrin1422 0:9b6737b144fc 839 if(hr.Rocket_Phase==1 || hr.Rocket_Phase==2 || hr.Rocket_Phase==3)
zebrin1422 0:9b6737b144fc 840 {
zebrin1422 0:9b6737b144fc 841
zebrin1422 0:9b6737b144fc 842 char state=0;
zebrin1422 0:9b6737b144fc 843
zebrin1422 0:9b6737b144fc 844 int down_time = in_time + (int)Inner_Time.read();
zebrin1422 0:9b6737b144fc 845
zebrin1422 0:9b6737b144fc 846 short down_data[20];
zebrin1422 0:9b6737b144fc 847
zebrin1422 0:9b6737b144fc 848 if(safety_flag) state |= 0x01; //0bit
zebrin1422 0:9b6737b144fc 849 if(flight_detect_mode) state |= 0x02; //1bit
zebrin1422 0:9b6737b144fc 850 if(hr.flight_pin_state) state |= 0x04; //2bit
zebrin1422 0:9b6737b144fc 851 if(flight_mode) state |= 0x08; //3bit
zebrin1422 0:9b6737b144fc 852 if(sepa_accept) state |= 0x10; //4bit
zebrin1422 0:9b6737b144fc 853 if(extrusion_accept) state |= 0x20; //5bit
zebrin1422 0:9b6737b144fc 854 if(collect_mode) state |= 0x40; //6bit
zebrin1422 0:9b6737b144fc 855 if(vertex_arrival) state |= 0x80; //7bit
zebrin1422 0:9b6737b144fc 856
zebrin1422 0:9b6737b144fc 857 /*
zebrin1422 0:9b6737b144fc 858 down_data[17] = (short)(Inner_Time.read()*COMPRES_Inner_Time);
zebrin1422 0:9b6737b144fc 859 down_data[0] = (short)(hr.w/COMPRES_Q);
zebrin1422 0:9b6737b144fc 860 down_data[1] = (short)(hr.x/COMPRES_Q);
zebrin1422 0:9b6737b144fc 861 down_data[2] = (short)(hr.y/COMPRES_Q);
zebrin1422 0:9b6737b144fc 862 down_data[3] = (short)(hr.z/COMPRES_Q);
zebrin1422 0:9b6737b144fc 863 */
zebrin1422 0:9b6737b144fc 864 down_data[4] = (short)(hr.vol_external*COMPRES_VOLTAGE);
zebrin1422 0:9b6737b144fc 865 down_data[5] = (short)(hr.current_external*COMPRES_CURRENT);
zebrin1422 0:9b6737b144fc 866 down_data[6] = (short)(hr.vol_inner*COMPRES_VOLTAGE);
zebrin1422 0:9b6737b144fc 867 down_data[7] = (short)(hr.current_inner*COMPRES_CURRENT);
zebrin1422 0:9b6737b144fc 868 down_data[8] = (short)(hr.pressure*COMPRES_PRESSURE);
zebrin1422 0:9b6737b144fc 869 /*
zebrin1422 0:9b6737b144fc 870 down_data[9] = (short)(hr.altitude*COMPRES_ALTITUDE);
zebrin1422 0:9b6737b144fc 871 down_data[10] = (short)(hr.velocity*COMPRES_VELOCITY);
zebrin1422 0:9b6737b144fc 872 */
zebrin1422 0:9b6737b144fc 873 down_data[11] = (short)(hr.luminosity*COMPRES_LUMINOSITY);
zebrin1422 0:9b6737b144fc 874 down_data[12] = (short)(hr.temperature*COMPRES_TEMPERATURE);
zebrin1422 0:9b6737b144fc 875 down_data[13] = (short)(hr.humidity*COMPRES_HUMIDITY);
zebrin1422 0:9b6737b144fc 876 /*
zebrin1422 0:9b6737b144fc 877 down_data[14] = (short)(hr.adxl_acc[0]*COMPRES_ACC);
zebrin1422 0:9b6737b144fc 878 down_data[15] = (short)(hr.adxl_acc[1]*COMPRES_ACC);
zebrin1422 0:9b6737b144fc 879 down_data[16] = (short)(hr.adxl_acc[2]*COMPRES_ACC);
zebrin1422 0:9b6737b144fc 880 down_data[18] = (short)(hr.press_pitot*COMPRES_PITOT);
zebrin1422 0:9b6737b144fc 881 */
zebrin1422 0:9b6737b144fc 882 //down_data[19] = (short)(hr.gps_alt*COMPRES_ALTTITUDE);
zebrin1422 0:9b6737b144fc 883
zebrin1422 0:9b6737b144fc 884 //for(iter=0;iter<17;iter++)down_data[iter]=0xFFFF; //for debug
zebrin1422 0:9b6737b144fc 885 //for(iter=6;iter<13;iter++)pc.printf("data[%d] = %d\r\n",iter,down_data[iter]);
zebrin1422 0:9b6737b144fc 886
zebrin1422 0:9b6737b144fc 887 interrupt_prohibit_flag = true;
zebrin1422 0:9b6737b144fc 888
zebrin1422 0:9b6737b144fc 889 es<<(char)0x03<<hr.Rocket_Phase<<state<<down_time<< //1+1+4
zebrin1422 0:9b6737b144fc 890 hr.latitude<<hr.longitude<< //4+4
zebrin1422 0:9b6737b144fc 891 //hr.vol_external<<hr.current_external<<hr.vol_inner<<hr.current_inner
zebrin1422 0:9b6737b144fc 892 down_data[4]<<down_data[5]<<down_data[6]<<down_data[7]
zebrin1422 0:9b6737b144fc 893 <<down_data[8]<<down_data[11]<<
zebrin1422 0:9b6737b144fc 894 down_data[12]<<down_data[13]<<es.endl;
zebrin1422 0:9b6737b144fc 895 //hr.pressure<<hr.luminosity<<
zebrin1422 0:9b6737b144fc 896 //hr.temperature<<hr.humidity<<es.endl;
zebrin1422 0:9b6737b144fc 897 /*down_data[4]<<down_data[5]<<down_data[6]<<down_data[7]<< //2+2+2+2
zebrin1422 0:9b6737b144fc 898 down_data[8]<<down_data[11]<<down_data[12]<<down_data[13] //2+2+2+2
zebrin1422 0:9b6737b144fc 899 <<es.endl;*/
zebrin1422 0:9b6737b144fc 900
zebrin1422 0:9b6737b144fc 901 interrupt_prohibit_flag = false;
zebrin1422 0:9b6737b144fc 902 }
zebrin1422 0:9b6737b144fc 903 if(hr.Rocket_Phase==4)
zebrin1422 0:9b6737b144fc 904 {
zebrin1422 0:9b6737b144fc 905 pc.printf("call phase 4\r\n");
zebrin1422 0:9b6737b144fc 906 char state=0;
zebrin1422 0:9b6737b144fc 907 int down_time = in_time + (int)Inner_Time.read();
zebrin1422 0:9b6737b144fc 908
zebrin1422 0:9b6737b144fc 909 short down_data[20];
zebrin1422 0:9b6737b144fc 910
zebrin1422 0:9b6737b144fc 911 if(safety_flag) state |= 0x01; //0bit
zebrin1422 0:9b6737b144fc 912 if(flight_detect_mode) state |= 0x02; //1bit
zebrin1422 0:9b6737b144fc 913 if(hr.flight_pin_state) state |= 0x04; //2bit
zebrin1422 0:9b6737b144fc 914 if(flight_mode) state |= 0x08; //3bit
zebrin1422 0:9b6737b144fc 915 if(sepa_accept) state |= 0x10; //4bit
zebrin1422 0:9b6737b144fc 916 if(extrusion_accept) state |= 0x20; //5bit
zebrin1422 0:9b6737b144fc 917 if(collect_mode) state |= 0x40; //6bit
zebrin1422 0:9b6737b144fc 918 if(vertex_arrival) state |= 0x80; //7bit
zebrin1422 0:9b6737b144fc 919
zebrin1422 0:9b6737b144fc 920 /*
zebrin1422 0:9b6737b144fc 921 down_data[17] = (short)(Inner_Time.read()*COMPRES_Inner_Time);
zebrin1422 0:9b6737b144fc 922 down_data[0] = (short)(hr.w/COMPRES_Q);
zebrin1422 0:9b6737b144fc 923 down_data[1] = (short)(hr.x/COMPRES_Q);
zebrin1422 0:9b6737b144fc 924 down_data[2] = (short)(hr.y/COMPRES_Q);
zebrin1422 0:9b6737b144fc 925 down_data[3] = (short)(hr.z/COMPRES_Q);
zebrin1422 0:9b6737b144fc 926 */
zebrin1422 0:9b6737b144fc 927 //down_data[4] = (short)(hr.vol_external*COMPRES_VOLTAGE);
zebrin1422 0:9b6737b144fc 928 //down_data[5] = (short)(hr.current_external*COMPRES_CURRENT);
zebrin1422 0:9b6737b144fc 929 down_data[6] = (short)(hr.vol_inner*COMPRES_VOLTAGE);
zebrin1422 0:9b6737b144fc 930 down_data[7] = (short)(hr.current_inner*COMPRES_CURRENT);
zebrin1422 0:9b6737b144fc 931 //down_data[8] = (short)(hr.pressure*COMPRES_PRESSURE);
zebrin1422 0:9b6737b144fc 932 //down_data[9] = (short)(hr.altitude*COMPRES_ALTITUDE);
zebrin1422 0:9b6737b144fc 933 //down_data[10] = (short)(hr.velocity*COMPRES_VELOCITY);
zebrin1422 0:9b6737b144fc 934 down_data[11] = (short)(hr.luminosity*COMPRES_LUMINOSITY);
zebrin1422 0:9b6737b144fc 935 down_data[12] = (short)(hr.temperature*COMPRES_TEMPERATURE);
zebrin1422 0:9b6737b144fc 936 down_data[13] = (short)(hr.humidity*COMPRES_HUMIDITY);
zebrin1422 0:9b6737b144fc 937 /*
zebrin1422 0:9b6737b144fc 938 down_data[14] = (short)(hr.adxl_acc[0]*COMPRES_ACC);
zebrin1422 0:9b6737b144fc 939 down_data[15] = (short)(hr.adxl_acc[1]*COMPRES_ACC);
zebrin1422 0:9b6737b144fc 940 down_data[16] = (short)(hr.adxl_acc[2]*COMPRES_ACC);
zebrin1422 0:9b6737b144fc 941 */
zebrin1422 0:9b6737b144fc 942 down_data[18] = (short)(hr.press_pitot*COMPRES_PITOT);
zebrin1422 0:9b6737b144fc 943 down_data[19] = (short)(hr.gps_alt*COMPRES_ALTITUDE);
zebrin1422 0:9b6737b144fc 944
zebrin1422 0:9b6737b144fc 945 //for(iter=0;iter<17;iter++)down_data[iter]=0xFFFF; //for debug
zebrin1422 0:9b6737b144fc 946 //for(iter=6;iter<13;iter++)pc.printf("data[%d] = %d\r\n",iter,down_data[iter]);
zebrin1422 0:9b6737b144fc 947
zebrin1422 0:9b6737b144fc 948 interrupt_prohibit_flag=true;
zebrin1422 0:9b6737b144fc 949
zebrin1422 0:9b6737b144fc 950 es<<(char)0x03<<hr.Rocket_Phase<<state<<down_time<< //1+1+4
zebrin1422 0:9b6737b144fc 951 hr.latitude<<hr.longitude<< //4+4
zebrin1422 0:9b6737b144fc 952 //down_data[0]<<down_data[1]<<down_data[2]<<down_data[3]<< //2*4
zebrin1422 0:9b6737b144fc 953 down_data[6]<<down_data[7]<<hr.altitude<<hr.velocity<< //2+2+4+2
zebrin1422 0:9b6737b144fc 954 down_data[11]<<down_data[12]<<down_data[13]<< //2*3
zebrin1422 0:9b6737b144fc 955 down_data[18]<<down_data[19]<<es.endl; //2*2
zebrin1422 0:9b6737b144fc 956
zebrin1422 0:9b6737b144fc 957 interrupt_prohibit_flag = false;
zebrin1422 0:9b6737b144fc 958 }
zebrin1422 0:9b6737b144fc 959 if(hr.Rocket_Phase==5)
zebrin1422 0:9b6737b144fc 960 {
zebrin1422 0:9b6737b144fc 961
zebrin1422 0:9b6737b144fc 962 char state=0;
zebrin1422 0:9b6737b144fc 963 int down_time = in_time + (int)Inner_Time.read();
zebrin1422 0:9b6737b144fc 964
zebrin1422 0:9b6737b144fc 965 short down_data[20];
zebrin1422 0:9b6737b144fc 966
zebrin1422 0:9b6737b144fc 967 if(safety_flag) state |= 0x01; //0bit
zebrin1422 0:9b6737b144fc 968 if(flight_detect_mode) state |= 0x02; //1bit
zebrin1422 0:9b6737b144fc 969 if(hr.flight_pin_state) state |= 0x04; //2bit
zebrin1422 0:9b6737b144fc 970 if(flight_mode) state |= 0x08; //3bit
zebrin1422 0:9b6737b144fc 971 if(sepa_accept) state |= 0x10; //4bit
zebrin1422 0:9b6737b144fc 972 if(extrusion_accept) state |= 0x20; //5bit
zebrin1422 0:9b6737b144fc 973 if(collect_mode) state |= 0x40; //6bit
zebrin1422 0:9b6737b144fc 974 if(vertex_arrival) state |= 0x80; //7bit
zebrin1422 0:9b6737b144fc 975
zebrin1422 0:9b6737b144fc 976 /*
zebrin1422 0:9b6737b144fc 977 down_data[17] = (short)(Inner_Time.read()*COMPRES_Inner_Time);
zebrin1422 0:9b6737b144fc 978 down_data[0] = (short)(hr.w/COMPRES_Q);
zebrin1422 0:9b6737b144fc 979 down_data[1] = (short)(hr.x/COMPRES_Q);
zebrin1422 0:9b6737b144fc 980 down_data[2] = (short)(hr.y/COMPRES_Q);
zebrin1422 0:9b6737b144fc 981 down_data[3] = (short)(hr.z/COMPRES_Q);
zebrin1422 0:9b6737b144fc 982 */
zebrin1422 0:9b6737b144fc 983 //down_data[4] = (short)(hr.vol_external*COMPRES_VOLTAGE);
zebrin1422 0:9b6737b144fc 984 //down_data[5] = (short)(hr.current_external*COMPRES_CURRENT);
zebrin1422 0:9b6737b144fc 985 down_data[6] = (short)(hr.vol_inner*COMPRES_VOLTAGE);
zebrin1422 0:9b6737b144fc 986 down_data[7] = (short)(hr.current_inner*COMPRES_CURRENT);
zebrin1422 0:9b6737b144fc 987 down_data[8] = (short)(hr.pressure*COMPRES_PRESSURE);
zebrin1422 0:9b6737b144fc 988 //down_data[9] = (short)(hr.altitude*COMPRES_ALTITUDE);
zebrin1422 0:9b6737b144fc 989 //down_data[10] = (short)(hr.velocity*COMPRES_VELOCITY);
zebrin1422 0:9b6737b144fc 990 down_data[11] = (short)(hr.luminosity*COMPRES_LUMINOSITY);
zebrin1422 0:9b6737b144fc 991 down_data[12] = (short)(hr.temperature*COMPRES_TEMPERATURE);
zebrin1422 0:9b6737b144fc 992 down_data[13] = (short)(hr.humidity*COMPRES_HUMIDITY);
zebrin1422 0:9b6737b144fc 993 /*
zebrin1422 0:9b6737b144fc 994 down_data[14] = (short)(hr.adxl_acc[0]*COMPRES_ACC);
zebrin1422 0:9b6737b144fc 995 down_data[15] = (short)(hr.adxl_acc[1]*COMPRES_ACC);
zebrin1422 0:9b6737b144fc 996 down_data[16] = (short)(hr.adxl_acc[2]*COMPRES_ACC);
zebrin1422 0:9b6737b144fc 997 down_data[18] = (short)(hr.press_pitot*COMPRES_PITOT);
zebrin1422 0:9b6737b144fc 998 */
zebrin1422 0:9b6737b144fc 999 //down_data[19] = (short)(hr.gps_alt*COMPRES_ALTTITUDE);
zebrin1422 0:9b6737b144fc 1000
zebrin1422 0:9b6737b144fc 1001 //for(iter=0;iter<17;iter++)down_data[iter]=0xFFFF; //for debug
zebrin1422 0:9b6737b144fc 1002 //for(iter=6;iter<13;iter++)pc.printf("data[%d] = %d\r\n",iter,down_data[iter]);
zebrin1422 0:9b6737b144fc 1003
zebrin1422 0:9b6737b144fc 1004
zebrin1422 0:9b6737b144fc 1005 interrupt_prohibit_flag=true;
zebrin1422 0:9b6737b144fc 1006
zebrin1422 0:9b6737b144fc 1007 es<<(char)0x03<<hr.Rocket_Phase<<state<<down_time<<
zebrin1422 0:9b6737b144fc 1008 hr.longitude<<hr.latitude<<
zebrin1422 0:9b6737b144fc 1009 down_data[6]<<down_data[7]<<down_data[8]<<
zebrin1422 0:9b6737b144fc 1010 down_data[11]<<down_data[12]<<down_data[13]<<
zebrin1422 0:9b6737b144fc 1011 es.endl;
zebrin1422 0:9b6737b144fc 1012
zebrin1422 0:9b6737b144fc 1013 interrupt_prohibit_flag = false;
zebrin1422 0:9b6737b144fc 1014 }
zebrin1422 0:9b6737b144fc 1015 }
zebrin1422 0:9b6737b144fc 1016 }
zebrin1422 0:9b6737b144fc 1017
zebrin1422 0:9b6737b144fc 1018
zebrin1422 0:9b6737b144fc 1019 void header0x05()
zebrin1422 0:9b6737b144fc 1020 {
zebrin1422 0:9b6737b144fc 1021 interrupt_prohibit_flag = true;
zebrin1422 0:9b6737b144fc 1022 es << (char)0x05 << 'd' << es.endl; //flight pin detect
zebrin1422 0:9b6737b144fc 1023 interrupt_prohibit_flag = false;
zebrin1422 0:9b6737b144fc 1024 }
zebrin1422 0:9b6737b144fc 1025
zebrin1422 0:9b6737b144fc 1026 void header0x06()
zebrin1422 0:9b6737b144fc 1027 {
zebrin1422 0:9b6737b144fc 1028 interrupt_prohibit_flag = true;
zebrin1422 0:9b6737b144fc 1029 es << (char)0x06 << 'n' << es.endl; //nicrom fire finish
zebrin1422 0:9b6737b144fc 1030 interrupt_prohibit_flag = false;
zebrin1422 0:9b6737b144fc 1031 }
zebrin1422 0:9b6737b144fc 1032
zebrin1422 0:9b6737b144fc 1033 void header0x07()
zebrin1422 0:9b6737b144fc 1034 {
zebrin1422 0:9b6737b144fc 1035 interrupt_prohibit_flag = true;
zebrin1422 0:9b6737b144fc 1036 es << (char)0x07 << 's' << es.endl; //solenoid push finish
zebrin1422 0:9b6737b144fc 1037 interrupt_prohibit_flag = false;
zebrin1422 0:9b6737b144fc 1038 }
zebrin1422 0:9b6737b144fc 1039
zebrin1422 0:9b6737b144fc 1040 void header0x08()
zebrin1422 0:9b6737b144fc 1041 {
zebrin1422 0:9b6737b144fc 1042 interrupt_prohibit_flag = true;
zebrin1422 0:9b6737b144fc 1043 es << (char)0x08 << 'c' << es.endl; //collect mode
zebrin1422 0:9b6737b144fc 1044 interrupt_prohibit_flag = false;
zebrin1422 0:9b6737b144fc 1045 }
zebrin1422 0:9b6737b144fc 1046
zebrin1422 0:9b6737b144fc 1047 void header0x09(char phase)
zebrin1422 0:9b6737b144fc 1048 {
zebrin1422 0:9b6737b144fc 1049 interrupt_prohibit_flag = true;
zebrin1422 0:9b6737b144fc 1050 es << (char)0x09 << phase << es.endl;
zebrin1422 0:9b6737b144fc 1051 interrupt_prohibit_flag = false;
zebrin1422 0:9b6737b144fc 1052 }
zebrin1422 0:9b6737b144fc 1053
zebrin1422 0:9b6737b144fc 1054
zebrin1422 0:9b6737b144fc 1055 /*******************************************************************************
zebrin1422 0:9b6737b144fc 1056 アップリンク
zebrin1422 0:9b6737b144fc 1057 *******************************************************************************/
zebrin1422 0:9b6737b144fc 1058
zebrin1422 0:9b6737b144fc 1059 void header0x10()
zebrin1422 0:9b6737b144fc 1060 {
zebrin1422 0:9b6737b144fc 1061 pc.printf("cmd receive\r\n");
zebrin1422 0:9b6737b144fc 1062
zebrin1422 0:9b6737b144fc 1063 char buff = es.data[0];
zebrin1422 0:9b6737b144fc 1064
zebrin1422 0:9b6737b144fc 1065 interrupt_prohibit_flag = true;
zebrin1422 0:9b6737b144fc 1066 if(buff == 'r')
zebrin1422 0:9b6737b144fc 1067 {
zebrin1422 0:9b6737b144fc 1068 safety_flag = true;
zebrin1422 0:9b6737b144fc 1069 flight_detect_mode = true;
zebrin1422 0:9b6737b144fc 1070 strain_state = 1;
zebrin1422 0:9b6737b144fc 1071 }
zebrin1422 0:9b6737b144fc 1072 else if(buff == 'f') //forced transition fllight mode
zebrin1422 0:9b6737b144fc 1073 {
zebrin1422 0:9b6737b144fc 1074 hr.flight_pin_state = true;
zebrin1422 0:9b6737b144fc 1075 flight_mode = true;
zebrin1422 0:9b6737b144fc 1076 pc.printf("forced transmit flight mode\r\n");
zebrin1422 0:9b6737b144fc 1077 }
zebrin1422 0:9b6737b144fc 1078 else if(buff == 'n' && hr.Rocket_Phase==4)
zebrin1422 0:9b6737b144fc 1079 {
zebrin1422 0:9b6737b144fc 1080 nicrom.fire();
zebrin1422 0:9b6737b144fc 1081 wait(FIRE_TIME);
zebrin1422 0:9b6737b144fc 1082 nicrom.stop();
zebrin1422 0:9b6737b144fc 1083 header0x06();
zebrin1422 0:9b6737b144fc 1084 iter = 0;
zebrin1422 0:9b6737b144fc 1085 while(1)
zebrin1422 0:9b6737b144fc 1086 {
zebrin1422 0:9b6737b144fc 1087 solenoid = 1;
zebrin1422 0:9b6737b144fc 1088 wait(OSHIDASHI_TIME);
zebrin1422 0:9b6737b144fc 1089 solenoid = 0;
zebrin1422 0:9b6737b144fc 1090 wait(OSHIDASHI_TIME);
zebrin1422 0:9b6737b144fc 1091 iter++;
zebrin1422 0:9b6737b144fc 1092 pc.printf("iter = %d\r\n",iter);
zebrin1422 0:9b6737b144fc 1093 if(iter > 10)break;
zebrin1422 0:9b6737b144fc 1094 }
zebrin1422 0:9b6737b144fc 1095 header0x07();
zebrin1422 0:9b6737b144fc 1096 pc.printf("forced separate from nicrom\r\n");
zebrin1422 0:9b6737b144fc 1097 }
zebrin1422 0:9b6737b144fc 1098 else if(buff == 's' && hr.Rocket_Phase==4)
zebrin1422 0:9b6737b144fc 1099 {
zebrin1422 0:9b6737b144fc 1100 int i=0;
zebrin1422 0:9b6737b144fc 1101 while(1)
zebrin1422 0:9b6737b144fc 1102 {
zebrin1422 0:9b6737b144fc 1103 solenoid = 1;
zebrin1422 0:9b6737b144fc 1104 wait(OSHIDASHI_TIME);
zebrin1422 0:9b6737b144fc 1105 solenoid = 0;
zebrin1422 0:9b6737b144fc 1106 wait(OSHIDASHI_TIME);
zebrin1422 0:9b6737b144fc 1107 i++;
zebrin1422 0:9b6737b144fc 1108 if(i > 10)break;
zebrin1422 0:9b6737b144fc 1109 }
zebrin1422 0:9b6737b144fc 1110 header0x07();
zebrin1422 0:9b6737b144fc 1111 pc.printf("forced separate from solenoid\r\n");
zebrin1422 0:9b6737b144fc 1112 }
zebrin1422 0:9b6737b144fc 1113 else if(buff == 'c' && hr.Rocket_Phase==4)
zebrin1422 0:9b6737b144fc 1114 {
zebrin1422 0:9b6737b144fc 1115 collect_mode = true;
zebrin1422 0:9b6737b144fc 1116 pc.printf("forced transmit collect mode\r\n");
zebrin1422 0:9b6737b144fc 1117 }
zebrin1422 0:9b6737b144fc 1118 else if(buff == 'd')
zebrin1422 0:9b6737b144fc 1119 {
zebrin1422 0:9b6737b144fc 1120 pc.printf("debug cmd receive\r\n");
zebrin1422 0:9b6737b144fc 1121 wait_ms(50);
zebrin1422 0:9b6737b144fc 1122 es << (char)0x1A << 'd' << es.endl;
zebrin1422 0:9b6737b144fc 1123 }
zebrin1422 0:9b6737b144fc 1124 else if(buff == 'i')
zebrin1422 0:9b6737b144fc 1125 {
zebrin1422 0:9b6737b144fc 1126 Inner_Time.reset();
zebrin1422 0:9b6737b144fc 1127 wait_ms(50);
zebrin1422 0:9b6737b144fc 1128 es << (char)0x1A << 'i' << es.endl;
zebrin1422 0:9b6737b144fc 1129 pc.printf("Inner time reset complete\r\n");
zebrin1422 0:9b6737b144fc 1130 }
zebrin1422 0:9b6737b144fc 1131 else if(buff == 'b')
zebrin1422 0:9b6737b144fc 1132 {
zebrin1422 0:9b6737b144fc 1133 safety_device_reboot = true;
zebrin1422 0:9b6737b144fc 1134 safety_flag = false;
zebrin1422 0:9b6737b144fc 1135 flight_detect_mode = false;
zebrin1422 0:9b6737b144fc 1136 wait_ms(50);
zebrin1422 0:9b6737b144fc 1137 es << (char)0x1A << 'b' << es.endl;
zebrin1422 0:9b6737b144fc 1138 }
zebrin1422 0:9b6737b144fc 1139 interrupt_prohibit_flag = false;
zebrin1422 0:9b6737b144fc 1140 }
zebrin1422 0:9b6737b144fc 1141
zebrin1422 0:9b6737b144fc 1142 /*******************************************************************************
zebrin1422 0:9b6737b144fc 1143 main関数
zebrin1422 0:9b6737b144fc 1144 *******************************************************************************/
zebrin1422 0:9b6737b144fc 1145 int main()
zebrin1422 0:9b6737b144fc 1146 {
zebrin1422 0:9b6737b144fc 1147 pc.baud(115200);
zebrin1422 0:9b6737b144fc 1148 wait(1.0f);
zebrin1422 0:9b6737b144fc 1149 rocket_state.attach(&downlink0x03, 5.0f);
zebrin1422 0:9b6737b144fc 1150 wait(1.0f);
zebrin1422 0:9b6737b144fc 1151 strain_state = 0;
zebrin1422 0:9b6737b144fc 1152 /***************************************************************************
zebrin1422 0:9b6737b144fc 1153 フェーズ1:　接続確認、初期設定
zebrin1422 0:9b6737b144fc 1154 ***************************************************************************/
zebrin1422 0:9b6737b144fc 1155
zebrin1422 0:9b6737b144fc 1156 pc.printf("-------------------------------------------------------\r\n");
zebrin1422 0:9b6737b144fc 1157 pc.printf("Phase 1:接続確認、初期設定\r\n");
zebrin1422 0:9b6737b144fc 1158
zebrin1422 0:9b6737b144fc 1159 hr.Rocket_Phase = 1;
zebrin1422 0:9b6737b144fc 1160
zebrin1422 0:9b6737b144fc 1161 header0x09(hr.Rocket_Phase);
zebrin1422 0:9b6737b144fc 1162
zebrin1422 0:9b6737b144fc 1163 short check = 0; //センサが通信できているかカウントする変数
zebrin1422 0:9b6737b144fc 1164 //各ビットにOK,NGの情報を持たせた
zebrin1422 0:9b6737b144fc 1165 Inner_Time.start();
zebrin1422 0:9b6737b144fc 1166 timer_reset.attach(&inner_time_reset, 1800);
zebrin1422 0:9b6737b144fc 1167
zebrin1422 0:9b6737b144fc 1168 sd_ticker.attach(&write_sd, 10.0);
zebrin1422 0:9b6737b144fc 1169
zebrin1422 0:9b6737b144fc 1170
zebrin1422 0:9b6737b144fc 1171 // センサの初期設定、接続確認
zebrin1422 0:9b6737b144fc 1172 if(external_ina.Connection_check() == 0)
zebrin1422 0:9b6737b144fc 1173 {
zebrin1422 0:9b6737b144fc 1174 pc.printf("External INA226 OK\r\n");
zebrin1422 0:9b6737b144fc 1175 external_ina.set_callibretion();
zebrin1422 0:9b6737b144fc 1176 check |= 0x001;
zebrin1422 0:9b6737b144fc 1177 }
zebrin1422 0:9b6737b144fc 1178 if(inner_ina.Connection_check() == 0)
zebrin1422 0:9b6737b144fc 1179 {
zebrin1422 0:9b6737b144fc 1180 pc.printf("Inner INA226 OK\r\n");
zebrin1422 0:9b6737b144fc 1181 inner_ina.set_callibretion();
zebrin1422 0:9b6737b144fc 1182 inner_ina.get_Voltage_current(&hr.vol_inner, &hr.current_inner);
zebrin1422 0:9b6737b144fc 1183 check |= 0x002;
zebrin1422 0:9b6737b144fc 1184 }
zebrin1422 0:9b6737b144fc 1185 if(mympu9250.sensorTest())
zebrin1422 0:9b6737b144fc 1186 {
zebrin1422 0:9b6737b144fc 1187 pc.printf("MPU9250 kansei OK\r\n");
zebrin1422 0:9b6737b144fc 1188 check |= 0x004;
zebrin1422 0:9b6737b144fc 1189 }
zebrin1422 0:9b6737b144fc 1190 if(mympu9250.mag_sensorTest())
zebrin1422 0:9b6737b144fc 1191 {
zebrin1422 0:9b6737b144fc 1192 pc.printf("MPU9250 tiziki OK\r\n");
zebrin1422 0:9b6737b144fc 1193 check |= 0x008;
zebrin1422 0:9b6737b144fc 1194 }
zebrin1422 0:9b6737b144fc 1195 if(myLPS22HB.whoAmI() == 0)
zebrin1422 0:9b6737b144fc 1196 {
zebrin1422 0:9b6737b144fc 1197 pc.printf("LPS22HB OK\r\n");
zebrin1422 0:9b6737b144fc 1198 myLPS22HB.begin(75);
zebrin1422 0:9b6737b144fc 1199 check |= 0x010;
zebrin1422 0:9b6737b144fc 1200 }
zebrin1422 0:9b6737b144fc 1201 if(0 < mySHT35.get_temp() && mySHT35.get_temp() < 28) //実際に温度を測ってみていい感じの範囲に収まってるかで接続確認としている
zebrin1422 0:9b6737b144fc 1202 {
zebrin1422 0:9b6737b144fc 1203 pc.printf("SHT-35 maybe OK\r\n");
zebrin1422 0:9b6737b144fc 1204 check |= 0x020;
zebrin1422 0:9b6737b144fc 1205 }
zebrin1422 0:9b6737b144fc 1206 if(myTSL2561.connect_check() == 1)
zebrin1422 0:9b6737b144fc 1207 {
zebrin1422 0:9b6737b144fc 1208 pc.printf("TSL2561 OK\r\n");
zebrin1422 0:9b6737b144fc 1209 myTSL2561.begin();
zebrin1422 0:9b6737b144fc 1210 hr.TSL_time = myTSL2561.set_rate(0);
zebrin1422 0:9b6737b144fc 1211 check |= 0x040;
zebrin1422 0:9b6737b144fc 1212 }
zebrin1422 0:9b6737b144fc 1213 if(three.getDeviceID() == (char)ADXL_DEVICE_ID)
zebrin1422 0:9b6737b144fc 1214 {
zebrin1422 0:9b6737b144fc 1215 three.setDataFormatControl(0x0B);
zebrin1422 0:9b6737b144fc 1216 three.setDataRate(ADXL375_3200HZ);
zebrin1422 0:9b6737b144fc 1217 three.setPowerControl(MeasurementMode);
zebrin1422 0:9b6737b144fc 1218 pc.printf("ADXL375 OK\r\n");
zebrin1422 0:9b6737b144fc 1219 check |= 0x080;
zebrin1422 0:9b6737b144fc 1220 }
zebrin1422 0:9b6737b144fc 1221 else pc.printf("ADXL device ID = 0x%X\r\n",three.getDeviceID());
zebrin1422 0:9b6737b144fc 1222 if((log_fp = fopen("/sd/log.txt" ,"w")) != NULL)
zebrin1422 0:9b6737b144fc 1223 {
zebrin1422 0:9b6737b144fc 1224 pc.printf("SDFileSystem OK\r\n");
zebrin1422 0:9b6737b144fc 1225 check |= 0x100;
zebrin1422 0:9b6737b144fc 1226 }
zebrin1422 0:9b6737b144fc 1227 if(gps.gps_readable)
zebrin1422 0:9b6737b144fc 1228 {
zebrin1422 0:9b6737b144fc 1229 pc.printf("GPS OK\r\n");
zebrin1422 0:9b6737b144fc 1230 check |= 0x200;
zebrin1422 0:9b6737b144fc 1231 }
zebrin1422 0:9b6737b144fc 1232
zebrin1422 0:9b6737b144fc 1233 //gps.debug(true);
zebrin1422 0:9b6737b144fc 1234
zebrin1422 0:9b6737b144fc 1235 //ほかのセンサの接続確認
zebrin1422 0:9b6737b144fc 1236
zebrin1422 0:9b6737b144fc 1237 es << char(0x02) << check << es.endl;
zebrin1422 0:9b6737b144fc 1238
zebrin1422 0:9b6737b144fc 1239 solenoid = 0;
zebrin1422 0:9b6737b144fc 1240 i2c_Bus.frequency(400000);
zebrin1422 0:9b6737b144fc 1241 hr.flight_pin_state = false;
zebrin1422 0:9b6737b144fc 1242 extrusion_pin.mode(PullUp);
zebrin1422 0:9b6737b144fc 1243 flight_pin.mode(PullUp);
zebrin1422 0:9b6737b144fc 1244
zebrin1422 0:9b6737b144fc 1245 es.attach(&header0x10, (char)0x10);
zebrin1422 0:9b6737b144fc 1246
zebrin1422 0:9b6737b144fc 1247 flight_time.start();
zebrin1422 0:9b6737b144fc 1248
zebrin1422 0:9b6737b144fc 1249 lps_read.attach(&get_lps_press, 1.0);
zebrin1422 0:9b6737b144fc 1250 ina_read.attach(&get_ina, 0.2);
zebrin1422 0:9b6737b144fc 1251 sht_read.attach(&get_sht, 0.3);
zebrin1422 0:9b6737b144fc 1252 tsl_read.attach(&get_tsl, 0.6);
zebrin1422 0:9b6737b144fc 1253 adxl_read.attach(&get_adxl, 0.8);
zebrin1422 0:9b6737b144fc 1254 //attitude_ticker.attach(&get_attitude, 0.8f);
zebrin1422 0:9b6737b144fc 1255 gps_read.attach(&get_gps, 1.5);
zebrin1422 0:9b6737b144fc 1256
zebrin1422 0:9b6737b144fc 1257 safety_device();
zebrin1422 0:9b6737b144fc 1258
zebrin1422 0:9b6737b144fc 1259 lps_read.detach();
zebrin1422 0:9b6737b144fc 1260 ina_read.detach();
zebrin1422 0:9b6737b144fc 1261 tsl_read.detach();
zebrin1422 0:9b6737b144fc 1262 sht_read.detach();
zebrin1422 0:9b6737b144fc 1263 adxl_read.detach();
zebrin1422 0:9b6737b144fc 1264 gps_read.detach();
zebrin1422 0:9b6737b144fc 1265
zebrin1422 0:9b6737b144fc 1266 //header0x09(hr.Rocket_Phase);
zebrin1422 0:9b6737b144fc 1267
zebrin1422 0:9b6737b144fc 1268
zebrin1422 0:9b6737b144fc 1269 /***************************************************************************
zebrin1422 0:9b6737b144fc 1270 フェーズ2:　準備
zebrin1422 0:9b6737b144fc 1271 ***************************************************************************/
zebrin1422 0:9b6737b144fc 1272
zebrin1422 0:9b6737b144fc 1273 pc.printf("-------------------------------------------------------\r\n");
zebrin1422 0:9b6737b144fc 1274 pc.printf("Phase 2: 準備\r\n");
zebrin1422 0:9b6737b144fc 1275
zebrin1422 0:9b6737b144fc 1276 hr.Rocket_Phase = 2;
zebrin1422 0:9b6737b144fc 1277
zebrin1422 0:9b6737b144fc 1278 header0x09(hr.Rocket_Phase);
zebrin1422 0:9b6737b144fc 1279
zebrin1422 0:9b6737b144fc 1280 //Ticker
zebrin1422 0:9b6737b144fc 1281
zebrin1422 0:9b6737b144fc 1282 lps_read.attach(&get_lps, 1.0);
zebrin1422 0:9b6737b144fc 1283 ina_read.attach(&get_in_ina, 0.0013);
zebrin1422 0:9b6737b144fc 1284 tsl_read.attach(&get_tsl, 0.02);
zebrin1422 0:9b6737b144fc 1285 sht_read.attach(&get_sht, 0.1);
zebrin1422 0:9b6737b144fc 1286 //adxl_read.attach(&get_adxl, 2.5);
zebrin1422 0:9b6737b144fc 1287 gps_read.attach(&get_gps, 0.1);
zebrin1422 0:9b6737b144fc 1288 pitot_press.attach(&get_pitot, 0.8);
zebrin1422 0:9b6737b144fc 1289 //attitude_ticker.attach(&get_attitude, 0.1);
zebrin1422 0:9b6737b144fc 1290
zebrin1422 0:9b6737b144fc 1291 pc.printf("timer prepare OK\r\n");
zebrin1422 0:9b6737b144fc 1292
zebrin1422 0:9b6737b144fc 1293 //各センサでデータ取得
zebrin1422 0:9b6737b144fc 1294 //姿勢計算開始
zebrin1422 0:9b6737b144fc 1295 //ダウンリンク開始
zebrin1422 0:9b6737b144fc 1296
zebrin1422 0:9b6737b144fc 1297 //rocket_state.attach(&downlink0x03, 0.8f);
zebrin1422 0:9b6737b144fc 1298
zebrin1422 0:9b6737b144fc 1299
zebrin1422 0:9b6737b144fc 1300
zebrin1422 0:9b6737b144fc 1301
zebrin1422 0:9b6737b144fc 1302
zebrin1422 0:9b6737b144fc 1303 /***************************************************************************
zebrin1422 0:9b6737b144fc 1304 フェーズ3:　発射待機
zebrin1422 0:9b6737b144fc 1305 ***************************************************************************/
zebrin1422 0:9b6737b144fc 1306
zebrin1422 0:9b6737b144fc 1307 pc.printf("-------------------------------------------------------\r\n");
zebrin1422 0:9b6737b144fc 1308 pc.printf("Phase 3: 発射待機\r\n");
zebrin1422 0:9b6737b144fc 1309
zebrin1422 0:9b6737b144fc 1310 hr.Rocket_Phase = 3;
zebrin1422 0:9b6737b144fc 1311
zebrin1422 0:9b6737b144fc 1312 //header0x09(hr.Rocket_Phase);
zebrin1422 0:9b6737b144fc 1313
zebrin1422 0:9b6737b144fc 1314 flight_pin.rise(&flightPin_detect);
zebrin1422 0:9b6737b144fc 1315
zebrin1422 0:9b6737b144fc 1316 flight_detect_mode = true;
zebrin1422 0:9b6737b144fc 1317 hr.flight_pin_state = false;
zebrin1422 0:9b6737b144fc 1318
zebrin1422 0:9b6737b144fc 1319 flight_detect();
zebrin1422 0:9b6737b144fc 1320 strain_state = 0;
zebrin1422 0:9b6737b144fc 1321
zebrin1422 0:9b6737b144fc 1322 header0x05();
zebrin1422 0:9b6737b144fc 1323 /***************************************************************************
zebrin1422 0:9b6737b144fc 1324 フェーズ4:　飛行
zebrin1422 0:9b6737b144fc 1325 ***************************************************************************/
zebrin1422 0:9b6737b144fc 1326 pc.printf("-------------------------------------------------------\r\n");
zebrin1422 0:9b6737b144fc 1327 pc.printf("Rocket Phase 4\r\n");
zebrin1422 0:9b6737b144fc 1328
zebrin1422 0:9b6737b144fc 1329 hr.Rocket_Phase = 4;
zebrin1422 0:9b6737b144fc 1330
zebrin1422 0:9b6737b144fc 1331 flight_time.reset();
zebrin1422 0:9b6737b144fc 1332
zebrin1422 0:9b6737b144fc 1333 rocket_state.detach();
zebrin1422 0:9b6737b144fc 1334
zebrin1422 0:9b6737b144fc 1335 rocket_state.attach(&downlink0x03, 1.0);
zebrin1422 0:9b6737b144fc 1336
zebrin1422 0:9b6737b144fc 1337 flight_mode = true;
zebrin1422 0:9b6737b144fc 1338
zebrin1422 0:9b6737b144fc 1339 while(!collect_mode)
zebrin1422 0:9b6737b144fc 1340 {
zebrin1422 0:9b6737b144fc 1341 separate_action();
zebrin1422 0:9b6737b144fc 1342 extrusion_action();
zebrin1422 0:9b6737b144fc 1343 collect();
zebrin1422 0:9b6737b144fc 1344 for(iter = 0; iter < DATA_NUMBER; iter ++)if(SD_BUFF_COUNT_FLAG[iter] == 1)write_sd(iter);
zebrin1422 0:9b6737b144fc 1345 }
zebrin1422 0:9b6737b144fc 1346
zebrin1422 0:9b6737b144fc 1347 pc.printf("flight while finish\r\n");
zebrin1422 0:9b6737b144fc 1348 wait_ms(100);
zebrin1422 0:9b6737b144fc 1349 header0x08();
zebrin1422 0:9b6737b144fc 1350 flight_time.stop();
zebrin1422 0:9b6737b144fc 1351
zebrin1422 0:9b6737b144fc 1352 /***************************************************************************
zebrin1422 0:9b6737b144fc 1353 フェーズ5:　回収
zebrin1422 0:9b6737b144fc 1354 ***************************************************************************/
zebrin1422 0:9b6737b144fc 1355 pc.printf("-------------------------------------------------------\r\n");
zebrin1422 0:9b6737b144fc 1356 pc.printf("Rocket Phase 5\r\n");
zebrin1422 0:9b6737b144fc 1357
zebrin1422 0:9b6737b144fc 1358 hr.Rocket_Phase = 5;
zebrin1422 0:9b6737b144fc 1359
zebrin1422 0:9b6737b144fc 1360 header0x09(hr.Rocket_Phase);
zebrin1422 0:9b6737b144fc 1361
zebrin1422 0:9b6737b144fc 1362 pc.printf("遷音速超え太郎を回収\r\n");
zebrin1422 0:9b6737b144fc 1363
zebrin1422 0:9b6737b144fc 1364 fclose(log_fp);
zebrin1422 0:9b6737b144fc 1365
zebrin1422 0:9b6737b144fc 1366
zebrin1422 0:9b6737b144fc 1367 while(1)
zebrin1422 0:9b6737b144fc 1368 {
zebrin1422 0:9b6737b144fc 1369
zebrin1422 0:9b6737b144fc 1370 pc.printf("finish\r\n");
zebrin1422 0:9b6737b144fc 1371 wait(2.0f);
zebrin1422 0:9b6737b144fc 1372
zebrin1422 0:9b6737b144fc 1373 }
zebrin1422 0:9b6737b144fc 1374 }