PQ_Hybrid_Electrical_Equipment_Team
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
Diff: HYBRYD2018_IZU_ROCKET.cpp
- Revision:
- 0:9b6737b144fc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HYBRYD2018_IZU_ROCKET.cpp Wed May 02 18:20:57 2018 +0000
@@ -0,0 +1,1374 @@
+#include "mbed.h"
+#include "GPS_interrupt.h"
+#include "HDC1050.h"
+#include "mpu9250_i2c.h"
+#include "MadgwickFilter.hpp"
+#include "INA226.h"
+#include "pqLPS22HB_lib.h"
+#include "TSL2561.h"
+#include "SHT3x.h"
+#include "SDFileSystem.h"
+#include "ADXL375_I2C.h"
+#include "nicrom.hpp"
+#include "ES920LR.hpp"
+/*
+#include "PowerControl.h"
+#include "EthernetPowerControl.h"
+*/
+/*******************************************************************************
+#include "hoge.h"
+*******************************************************************************/
+
+
+
+/*******************************************************************************
+タイマー等の設定
+*******************************************************************************/
+
+Timer Inner_Time;
+Timer flight_time;
+Ticker timer_reset;
+
+int in_time;
+char timer_count=0;
+
+void inner_time_reset()
+{
+ timer_count ++;
+ in_time = 1800*timer_count;
+ Inner_Time.reset();
+}
+/*******************************************************************************
+フラグ等の設定
+*******************************************************************************/
+
+bool altitude_accept = false; //十分な高度(10m)に到達したフラグ
+bool vertex_arrival = false; //頂点検知のフラグ、速度が0.1m/s以下でtrue
+
+bool safety_flag = false; //安全装置解除のフラグ
+bool flight_detect_mode = false; //フライトピン待機状態のフラグ
+bool safety_device_reboot = false; //安全装置再起動のフラグ
+bool flight_mode = false; //飛行中のフラグ
+bool collect_mode = false; //回収状態のフラグ
+
+bool solenoid_complete = false; //電磁弁が10回押されたフラグ
+bool extrusion_accept = false; //電磁弁を押すことを許可するフラグ
+bool sepa_accept = false; //ニクロム線を熱することを許可するフラグ
+
+bool interrupt_prohibit_flag = false; //すべてのタイマー割り込みを禁止するフラグ
+
+/*******************************************************************************
+計算の中で使う変数の宣言
+*******************************************************************************/
+int iter = 0; //for文を回すときに使う
+float sum = 0; //平均を計算する際に使う
+
+const double DEG_TO_RAG = 0.01745329251994329576923690768489;//PI / 180.0
+/*******************************************************************************
+ダウンリンクするデータ
+*******************************************************************************/
+
+typedef struct HYBRID_ROCKET{
+
+ //unsigned int inner_time; //内部時間
+ bool flight_pin_state; //
+ float latitude; //緯度,1
+ float longitude; //経度,2
+ float w,x,y,z; //クォータニオン,3,4,5,6
+ float vol_external; //7
+ float current_external; //8
+ float vol_inner; //9
+ float current_inner; //10
+ float pressure; //11
+ float altitude; //高度,12
+ float velocity; //13
+ float luminosity; //照度,光度(明るさ),14
+ unsigned int TSL_time; //TSL2561の変換時間
+ //int camera_state; //0: 作動していない 1: 録画している
+ float temperature; //15
+ float humidity; //16
+ float adxl_acc[3]; //17,18,19
+ float press_pitot; //20
+ float gps_alt; //21
+ char Rocket_Phase; // ロケットのフェーズを設定するフラグ
+ // (1:接続確認、初期設定,2:準備,3:発射待機,4:飛行,5:回収)
+ //その他取得するデータを宣言する
+};
+
+static HYBRID_ROCKET hr;
+
+/*******************************************************************************
+PCシリアル通信
+*******************************************************************************/
+RawSerial pc(USBTX,USBRX, 115200);
+
+/*******************************************************************************
+I2C通信の設定
+*******************************************************************************/
+#define I2C_SDA p28
+#define I2C_SCL p27
+
+bool i2c_using_flag = false; //i2cを使ってないときはfalse、使っているときはtrue
+
+I2C i2c_Bus(I2C_SDA, I2C_SCL);
+
+/*******************************************************************************
+SDFileSystem
+*******************************************************************************/
+#define MOSI_SD p5
+#define MISO_SD p6
+#define SCLK_SD p7
+#define CS_SD p8
+
+SDFileSystem sd(MOSI_SD,MISO_SD,SCLK_SD,CS_SD,"sd");
+FILE *log_fp;
+
+const int DATA_NUMBER = 21; //取得するデータの数
+const int BUFF_SIZE = 8; //バッファの容量、8個
+int SD_BUFF_COUNT_FLAG[DATA_NUMBER] = {}; //バッファがたまったことを知らせるフラグ
+float sd_buff[BUFF_SIZE][DATA_NUMBER] = {}; //sdカードに書き込むバッファ
+float time_buff[BUFF_SIZE][DATA_NUMBER] = {};
+int buff_count[DATA_NUMBER] = {}; //配列の何個目に入れるかのカウント
+//bool sd_using_flag = false;
+
+Ticker sd_ticker;
+
+void write_buff(int number,float data)
+{
+ //pc.printf("number = %d\tcount = %d\tdata = %f\r\n", number,buff_count[number],data);
+ sd_buff[buff_count[number]][number] = data;
+ time_buff[buff_count[number]][number] = flight_time.read();
+ buff_count[number] ++;
+ //pc.printf("buff\r\n");
+ if(buff_count[number] >= 8)
+ {
+ SD_BUFF_COUNT_FLAG[number] = 1;
+ buff_count[number] = 0;
+ }
+}
+
+
+void write_sd(int number)
+{
+ //if(!sd_using_flag)
+ //{
+ //sd_using_flag = true;
+ fprintf(log_fp, "Number = %d\r\n",number);
+ for(iter=0;iter<8;iter++)
+ {
+ fprintf(log_fp,"%f\t",sd_buff[iter][number]);
+ //pc.printf("%f\t",sd_buff[iter][number]);
+ }
+ fprintf(log_fp, "\r\n");
+ for(iter=0;iter<8;iter++)
+ {
+ fprintf(log_fp,"%f\t",time_buff[iter][number]);
+ //pc.printf("%f\t", time_buff[iter][number]);
+ }
+ //pc.printf("\r\n");
+ buff_count[number] = 0;
+ SD_BUFF_COUNT_FLAG[number] = 0;
+ //sd_using_flag = false;
+ //pc.printf("sd write\r\n");
+ //}
+}
+
+void write_sd()
+{
+ for(iter=0;iter<DATA_NUMBER;iter++)if(SD_BUFF_COUNT_FLAG[iter]==1)write_sd(iter);
+}
+/*******************************************************************************
+新しいセンサの設定を書くこと！！！！！！！！
+*******************************************************************************/
+/*******************************************************************************
+GPSシリアル通信
+*******************************************************************************/
+#define GPS_TX p13
+#define GPS_RX p14
+
+Serial gps_bus(GPS_TX, GPS_RX);
+GPS_interrupt gps(&gps_bus);
+Ticker auto_GPS_Lat_Long;
+const int lat_number = 1;
+const int long_number = 2;
+const int gps_alt_number = 3;
+
+Ticker gps_read;
+
+//Mutex gps_mutex;
+
+/*
+void gps_thread()
+{
+ while(1)
+ {
+ gps_mutex.lock();
+ gps.getPosition(&hr.longitude, &hr.latitude);
+ write_buff(lat_number, hr.latitude);
+ write_buff(long_number, hr.longitude);
+ gps_mutex.unlock();
+ Thread::wait(1500);
+ }
+
+}*/
+
+void get_gps()
+{
+ if(!interrupt_prohibit_flag)
+ {
+ float xy[2];
+ gps.getPosition(xy);
+ hr.gps_alt = gps.Height();
+ hr.longitude = xy[0];
+ hr.latitude = xy[1];
+ write_buff(lat_number, hr.latitude);
+ write_buff(long_number, hr.longitude);
+ write_buff(gps_alt_number, hr.gps_alt);
+
+ }
+}
+
+
+/*******************************************************************************
+INA226
+*******************************************************************************/
+myINA226 external_ina(i2c_Bus, myINA226::A1_VDD, myINA226::A0_VDD);
+myINA226 inner_ina(i2c_Bus, myINA226::A1_GND, myINA226::A0_GND);
+const int volt_ex_number = 4;
+const int current_ex_number = 5;
+const int volt_in_number = 6;
+const int current_in_number = 7;
+
+Ticker ina_read;
+
+void get_ex_ina()
+{
+ if(!i2c_using_flag && !interrupt_prohibit_flag)
+ {
+ i2c_using_flag = true;
+ external_ina.get_Voltage_current(&hr.vol_external, &hr.current_external);
+ i2c_using_flag = false;
+ write_buff(volt_ex_number, hr.vol_external);
+ write_buff(current_ex_number, hr.current_external);
+ //pc.printf("ex_volt = %f\tex_current = %f\r\n",hr.vol_external,hr.current_external);
+ }
+}
+
+void get_in_ina()
+{
+ if(!i2c_using_flag && !interrupt_prohibit_flag)
+ {
+ i2c_using_flag = true;
+ inner_ina.get_Voltage_current(&hr.vol_inner, &hr.current_inner);
+ i2c_using_flag = false;
+ write_buff(volt_in_number, hr.vol_inner);
+ write_buff(current_in_number, hr.current_inner);
+ //pc.printf("in_volt = %f\tin_current = %\r\n",hr.vol_inner,hr.current_inner);
+ }
+}
+
+void get_ina()
+{
+ get_in_ina();
+ get_ex_ina();
+}
+/*******************************************************************************
+LPS22HB
+*******************************************************************************/
+
+pqLPS22HB_lib myLPS22HB(pqLPS22HB_lib::AD0_LOW,i2c_Bus);
+
+static int altitude_count = 0;
+float P_0,T_0; //高度計算に必要な地上での気圧、温度
+const int press_number = 8;
+const int alt_number = 9;
+const int vel_number = 10;
+bool alt_count = false;
+
+Ticker lps_read;
+
+char pres_count = 0;
+
+void get_lps_press()
+{
+ if(!i2c_using_flag && !interrupt_prohibit_flag)
+ {
+ i2c_using_flag = true;
+ hr.pressure = myLPS22HB.getPres();
+ i2c_using_flag = false;
+ write_buff(press_number, hr.pressure);
+ //pc.printf("press = %f\r\n",hr.pressure);
+ }
+}
+
+float alt_to_vel[5] = {};
+float delta_T;
+float delta_alt;
+float oldAtitude;
+float newAtitude;
+float oldTime = 0;
+
+/*
+void get_lps_vel()
+{
+ if(!interrupt_prohibit_flag && !i2c_using_flag)
+ {
+ sum = 0;
+ for(iter = 0; iter < 10; iter ++) sum += alt_to_vel[iter];
+
+ newAtitude = sum / 10;
+ delta_alt = newAtitude - oldAtitude;
+ delta_T = Inner_Time.read() - oldTime;
+ hr.velocity = sum / delta_T;
+ write_buff(vel_number, hr.velocity);
+ oldTime = Inner_Time.read();
+ oldAtitude;
+ ///*
+ i2c_using_flag = true;
+ delta_alt = myLPS22HB.getAlt(P_0,T_0);
+ i2c_using_flag = false;
+ delta_T = Inner_Time.read() - differential_time;
+ hr.velocity = delta_alt / delta_T;
+ //
+ write_buff(vel_number, hr.velocity);
+ if(hr.Rocket_Phase == 4 && hr.velocity < 1 && hr.velocity > -1 && altitude_accept)vertex_arrival = true;
+ //3m/sが多分本番時の頂点検知の閾値
+ }
+} */
+
+const float flight_min_altitude = 5;
+
+void get_lps_alt()
+{
+ if(!i2c_using_flag && !interrupt_prohibit_flag)
+ {
+ i2c_using_flag = true;
+ hr.altitude = myLPS22HB.getAlt(P_0,T_0);
+ i2c_using_flag = false;
+ alt_to_vel[altitude_count] = hr.altitude;
+ altitude_count ++;
+ if(alt_count && altitude_count >= 10)
+ {
+ altitude_count = 0;
+ sum=0;
+ for(iter=0;iter<10;iter++)sum += alt_to_vel[iter];
+ delta_T = (Inner_Time.read_us() - oldTime)/1000000.0;
+ newAtitude = sum/10;
+ delta_alt = newAtitude - oldAtitude;
+ hr.velocity = delta_alt / delta_T;
+ write_buff(vel_number, hr.velocity);
+ oldTime = Inner_Time.read_us();
+ oldAtitude = newAtitude;
+ alt_count = true;
+ //pc.printf("velocity = %f\r\n",hr.velocity);
+ }
+ write_buff(alt_number, hr.altitude);
+ //pc.printf("altitude = %f\r\n",hr.altitude);
+ if(!altitude_accept && hr.altitude > 10)altitude_accept=true;
+ if(hr.Rocket_Phase == 4 && hr.velocity < 1 && hr.velocity > -1 && altitude_accept)vertex_arrival = true;
+ }
+}
+
+void get_lps()
+{
+ //pc.printf("call ex\r\n");
+ if(!i2c_using_flag && !interrupt_prohibit_flag)
+ {
+ //pc.printf("call in\r\n");
+ i2c_using_flag = true;
+ hr.altitude = myLPS22HB.getAlt(P_0,T_0);
+ hr.pressure = myLPS22HB.getPres();
+ i2c_using_flag = false;
+ //pc.printf("pressure = %f\r\n",hr.pressure);
+ write_buff(alt_number, hr.altitude);
+ write_buff(press_number, hr.pressure);
+ alt_to_vel[altitude_count] = hr.altitude;
+ altitude_count ++;
+ if(alt_count && altitude_count >= 10)
+ {
+ altitude_count = 0;
+ sum=0;
+ for(iter=0;iter<10;iter++)sum += alt_to_vel[iter];
+ delta_T = (Inner_Time.read_us() - oldTime)/1000000.0;
+ newAtitude = sum/10;
+ delta_alt = newAtitude - oldAtitude;
+ hr.velocity = delta_alt / delta_T;
+ write_buff(vel_number, hr.velocity);
+ oldTime = Inner_Time.read_us();
+ oldAtitude = newAtitude;
+ alt_count = true;
+ //pc.printf("velocity = %f\r\n",hr.velocity);
+ }
+ //pc.printf("altitude = %f\r\n",hr.altitude);
+ if(!altitude_accept && hr.altitude > 10)altitude_accept=true;
+ if(hr.Rocket_Phase == 4 && hr.velocity < 1 && hr.velocity > -1 && altitude_accept)vertex_arrival = true;
+ }
+}
+
+/*******************************************************************************
+MPU9250
+*******************************************************************************/
+#define SET_ACC_LPF NO_USE
+#define SET_GYRO _1000DPS
+#define SET_ACC _16G
+
+mpu9250 mympu9250(i2c_Bus);
+//const int imu_number = 11;
+//const int mag_number = 12;
+
+const double mpu_offset[9] = {};
+
+static double imu[6];
+static double mag[3];
+
+/*******************************************************************************
+SHT-35
+*******************************************************************************/
+mySHT3x mySHT35(i2c_Bus, mySHT3x::AD0_LOW);
+
+const int SHT_temp_number = 11;
+const int SHT_hum_number = 12;
+
+Ticker sht_read;
+
+void get_sht()
+{
+ if(!i2c_using_flag && !interrupt_prohibit_flag)
+ {
+ i2c_using_flag = true;
+ mySHT35.get_temp_hum(&hr.temperature, &hr.humidity);
+ i2c_using_flag = false;
+ write_buff(SHT_temp_number, hr.temperature);
+ write_buff(SHT_hum_number, hr.humidity);
+ //pc.printf("temp = %f\r\nhum = %f\r\n",hr.temperature,hr.humidity);
+ }
+}
+
+/*******************************************************************************
+TSL2561
+*******************************************************************************/
+myTSL2561 myTSL2561(i2c_Bus, myTSL2561::AD0_OPEN);
+const int lux_number = 13;
+
+Ticker tsl_read;
+
+void get_tsl()
+{
+ if(!i2c_using_flag && !interrupt_prohibit_flag)
+ {
+ i2c_using_flag = true;
+ hr.luminosity = myTSL2561.get_luminosity(hr.TSL_time);
+ i2c_using_flag = false;
+ write_buff(lux_number, hr.luminosity);
+ //pc.printf("lux = %f\r\n",hr.luminosity);
+ }
+}
+
+/*******************************************************************************
+ADXL375
+*******************************************************************************/
+ADXL375_I2C three(I2C_SDA, I2C_SCL);
+
+const int three_x = 14;
+const int three_y = 15;
+const int three_z = 16;
+
+int three_acc[3];
+
+#define ADXL_DEVICE_ID 0xE5
+#define DATA_FORMAT_CONTORL 0x08
+
+Ticker adxl_read;
+
+void get_adxl()
+{
+ if(!i2c_using_flag && !interrupt_prohibit_flag)
+ {
+ i2c_using_flag = true;
+ three.getOutput(three_acc);
+ i2c_using_flag = false;
+ for(iter=0;iter<3;iter++)hr.adxl_acc[iter] = (float)three_acc[iter]*200/4096;
+ write_buff(three_x, hr.adxl_acc[0]);
+ write_buff(three_y, hr.adxl_acc[1]);
+ write_buff(three_z, hr.adxl_acc[2]);
+ }
+}
+
+/*******************************************************************************
+ピトー管
+*******************************************************************************/
+#define PITOT_PIN p20
+
+AnalogIn pitot(PITOT_PIN);
+const int pitot_number = 17;
+
+Ticker pitot_press;
+
+void get_pitot()
+{
+ if(!interrupt_prohibit_flag)
+ {
+ hr.press_pitot = pitot;
+ write_buff(pitot_number, hr.press_pitot);
+ }
+}
+/*******************************************************************************
+ひずみゲージ
+*******************************************************************************/
+//HIZUMI instance make
+#define STRAIN_PIN p29
+
+DigitalOut strain_state(STRAIN_PIN);
+
+/*******************************************************************************
+ES920LR
+*******************************************************************************/
+#define ES_TX p9
+#define ES_RX p10
+#define ES_PIN p11
+
+RawSerial es_bus(ES_TX,ES_RX);
+ES920LR es(es_bus, pc,115200);
+//DigitalIn busy(ES_PIN);
+/*******************************************************************************
+ニクロム線
+*******************************************************************************/
+#define NICROM_PIN p24
+
+Nicrom nicrom(NICROM_PIN);
+/*******************************************************************************
+フライトピン
+*******************************************************************************/
+#define FLIGHT_PIN p26
+InterruptIn flight_pin(FLIGHT_PIN);
+
+void flightPin_detect()
+{
+ interrupt_prohibit_flag = true;
+ hr.flight_pin_state = true;
+ //flight_detect_mode = true;
+ pc.printf("flight pin removed\r\n");
+ //flight_mode=true;
+ interrupt_prohibit_flag = false;
+}
+
+/*******************************************************************************
+発射待機、安全装置等
+*******************************************************************************/
+
+//safety device release
+
+void safety_device()
+{
+ while(!safety_flag)
+ {
+ //flight_detect_mode = true;
+ //pc.printf("don't accept standby\r\n");
+ //wait(1.0f);
+ //if(Inner_Time.read() > 30)break;
+ wait_ms(50);
+ hr.flight_pin_state = false;
+ }
+ //wait_ms(500);
+ safety_device_reboot = false;
+ es << (char)0x01 << (char) 9 << es.endl; //ぷラキューの9
+}
+
+//flight detect mode
+//int flightPin_detect_count=0;
+//Timeout flightPin_dummy;
+
+#define DETECT_TIME 50
+
+void flight_detect()
+{
+ //pc.printf("detect start\r\n");
+ if(flight_detect_mode)
+ {
+ //pc.printf("detect a\r\n");
+ while(!hr.flight_pin_state)
+ {
+ //pc.printf("detect b\r\n");
+
+ /*
+ if(flightPin_dummy.read() > DETECT_TIME)
+ {
+ // pc.printf("detect c\r\n");
+ hr.flight_pin_state = true;
+ flight_mode = true;
+ pc.printf("flight mode true\r\n");
+ break;
+ }
+ */
+ if(hr.flight_pin_state)break;
+ P_0 = hr.pressure;
+ T_0 = hr.temperature;
+ wait_ms(10);
+ //pc.printf("roop\r\n");
+ if(safety_device_reboot)safety_device();
+ }
+ }
+}
+
+
+/*******************************************************************************
+分離機構
+*******************************************************************************/
+#define SOLENOID_PIN p25
+#define EXTRUSION_FLIGHT_PIN p22
+
+DigitalOut solenoid(SOLENOID_PIN);
+
+
+//プロトタイプ宣言
+void separate_action();
+void extrusion_action();
+void collect();
+void nicrom_low();
+void solenoid_low();
+
+void header0x06();
+void header0x07();
+
+short solenoid_count = 0;
+
+#define OSHIDASHI_TIME 0.5
+
+//最低10回はプシュプシュさせる、回転数はなるはやで
+//0.1秒ごとにHIGH,LOWの切り替え?
+
+void oshidashi()
+{
+ pc.printf("oshidashi start\r\n");
+ while(!solenoid_complete)
+ {
+ solenoid = 1;
+ wait(OSHIDASHI_TIME);
+ solenoid = 0;
+ wait(OSHIDASHI_TIME);
+ pc.printf("iter = %d\r\n",solenoid_count);
+ solenoid_count ++;
+ if(solenoid_count >=10)solenoid_complete = true;
+ }
+ header0x07();
+}
+
+DigitalOut led_3(LED3);
+
+#define FIRE_TIME 5
+
+void separate()
+{
+ led_3 = 1;
+ nicrom.fire();
+ wait(FIRE_TIME);
+ nicrom.stop();
+ led_3 = 0;
+ header0x06();
+ extrusion_action();
+ header0x07();
+ //nicrom_stop.attach(&nicrom_low, 3.0);
+}
+
+
+
+// separate action start
+const float sepa_prohibit_time = 7.0;
+const float sepa_velocity = 0.5;
+Timer sepa_start_time;
+
+void separate_action()
+{
+ //pc.printf("sepa begin\r\n");
+ if(flight_mode)
+ {
+ //pc.printf("sepa a\r\n");
+ if(!sepa_accept)
+ {
+ //pc.printf("sepa b\r\n");
+ if(flight_time.read() > sepa_prohibit_time)
+ {
+ sepa_accept=true;
+ pc.printf("separate\r\n");
+ separate();
+ header0x06();
+ }
+
+
+
+ if(altitude_accept && hr.velocity < sepa_velocity)
+ {
+ sepa_accept=true;
+ altitude_accept = true;
+ pc.printf("separate\r\n");
+ separate();
+ header0x06();
+ }
+
+
+
+ //pc.printf("sepa e\r\n");
+ }
+ //pc.printf("sepa f\r\n");
+ }
+ //pc.printf("sepa finish\r\n");
+}
+
+// extrusion(oshidashi) action
+const float extrusion_prohibit_time = 7.0;
+InterruptIn extrusion_pin(EXTRUSION_FLIGHT_PIN);
+
+void extrusion_action() //最低10回はプシュプシュさせる、回転数はなるはやで
+{
+ //pc.printf("ex begin\r\n"); //0.5秒ごとにHIGH,LOWの切り替え?
+ if(sepa_accept)
+ {
+ //pc.printf("ex a\r\n");
+ if(!extrusion_accept)
+ {
+ //pc.printf("ex b\r\n");
+ if(flight_time.read() > extrusion_prohibit_time)
+ {
+ //pc.printf("ex c\r\n");
+ extrusion_accept = true;
+ pc.printf("oshidashi\r\n");
+ oshidashi();
+ header0x07();
+ //extrusion_start_time.start();
+ }
+ //pc.printf("ex e\r\n");
+ //if(cmd receive)oshidashi_kyoka
+ }
+ //pc.printf("ex f\r\n");
+ }
+ //pc.printf("ex finish\r\n");
+}
+
+// collect mode
+const float flight_min_time = 40;
+
+void collect()
+{
+ //pc.printf("re a\r\n");
+ if(flight_mode)
+ {
+ //pc.printf("re b\r\n");
+ if(!collect_mode)
+ {
+ //pc.printf("re c\r\n");
+ if(flight_time.read() > flight_min_time)
+ {
+ //pc.printf("re d\r\n");
+ collect_mode=true;
+ pc.printf("collect\r\n");
+ }
+ //if(高度が10m到達した_flag == true && hr.altitude < 30)kaisyuu mode
+ if(altitude_accept && hr.altitude < 30 && extrusion_accept)
+ {
+ collect_mode=true;
+ pc.printf("collect\r\n");
+ }
+ //pc.printf("re e\r\n");
+ }
+ //pc.printf("re f\r\n");
+ }
+ //pc.printf("re g\r\n");
+}
+
+
+/*******************************************************************************
+姿勢計算
+*******************************************************************************/
+
+static Quaternion myQ(1.0, 0.0, 0.0, 0.0);
+MadgwickFilter attitude(1.35);
+
+const int Q_W_number=18;
+const int Q_X_number=19;
+const int Q_Y_number=20;
+const int Q_Z_number=21;
+
+Ticker attitude_ticker;
+
+void get_attitude()
+{
+ if(!i2c_using_flag && !interrupt_prohibit_flag)
+ {
+ i2c_using_flag = 1;
+ mympu9250.getGyroAcc(imu);
+ mympu9250.getMag(mag);
+ i2c_using_flag = 0;
+ }
+ for(iter=0; iter<3; iter++) imu[iter] = imu[iter]*DEG_TO_RAG;
+ attitude.MadgwickAHRSupdate(imu[0],imu[1],imu[2],imu[3],imu[4],imu[5],mag[0],mag[1],mag[2]);
+ attitude.getAttitude(&myQ);
+ hr.w = myQ.w;
+ hr.x = myQ.x;
+ hr.y = myQ.y;
+ hr.z = myQ.z;
+ write_buff(Q_W_number, hr.w);
+ write_buff(Q_X_number, hr.x);
+ write_buff(Q_Y_number, hr.y);
+ write_buff(Q_Z_number, hr.z);
+}
+
+/*******************************************************************************
+ダウンリンク
+*******************************************************************************/
+#define COMPRES_Inner_Time 36.40833333 //32768/1800
+#define COMPRES_Q 305200 // ? pakuru
+#define COMPRES_VOLTAGE 0.9102222 // 65535/36 =
+#define COMPRES_CURRENT 0.8192 // 65535/40 =
+#define COMPRES_PRESSURE 26.0063 // 65535/1260 =
+#define COMPRES_ALTITUDE 65.536 // 65535/500 = //range = 0~500
+#define COMPRES_VELOCITY 327.68 // 65535/100 = //pakuru
+#define COMPRES_LUMINOSITY 3.2768 // 65535/10000 =
+#define COMPRES_TEMPERATURE 546.133 // 63353/60 = //range = -20 ~ 40
+#define COMPRES_HUMIDITY 327.68 // 65535/100 =
+#define COMPRES_ACC 81.92 // 65535/400 =
+#define COMPRES_PITOT 32768 // 65535/1 =
+
+Ticker rocket_state;
+
+void downlink0x03()
+{
+
+ if(!interrupt_prohibit_flag)
+ {
+
+ if(hr.Rocket_Phase==1 || hr.Rocket_Phase==2 || hr.Rocket_Phase==3)
+ {
+
+ char state=0;
+
+ int down_time = in_time + (int)Inner_Time.read();
+
+ short down_data[20];
+
+ if(safety_flag) state |= 0x01; //0bit
+ if(flight_detect_mode) state |= 0x02; //1bit
+ if(hr.flight_pin_state) state |= 0x04; //2bit
+ if(flight_mode) state |= 0x08; //3bit
+ if(sepa_accept) state |= 0x10; //4bit
+ if(extrusion_accept) state |= 0x20; //5bit
+ if(collect_mode) state |= 0x40; //6bit
+ if(vertex_arrival) state |= 0x80; //7bit
+
+ /*
+ down_data[17] = (short)(Inner_Time.read()*COMPRES_Inner_Time);
+ down_data[0] = (short)(hr.w/COMPRES_Q);
+ down_data[1] = (short)(hr.x/COMPRES_Q);
+ down_data[2] = (short)(hr.y/COMPRES_Q);
+ down_data[3] = (short)(hr.z/COMPRES_Q);
+ */
+ down_data[4] = (short)(hr.vol_external*COMPRES_VOLTAGE);
+ down_data[5] = (short)(hr.current_external*COMPRES_CURRENT);
+ down_data[6] = (short)(hr.vol_inner*COMPRES_VOLTAGE);
+ down_data[7] = (short)(hr.current_inner*COMPRES_CURRENT);
+ down_data[8] = (short)(hr.pressure*COMPRES_PRESSURE);
+ /*
+ down_data[9] = (short)(hr.altitude*COMPRES_ALTITUDE);
+ down_data[10] = (short)(hr.velocity*COMPRES_VELOCITY);
+ */
+ down_data[11] = (short)(hr.luminosity*COMPRES_LUMINOSITY);
+ down_data[12] = (short)(hr.temperature*COMPRES_TEMPERATURE);
+ down_data[13] = (short)(hr.humidity*COMPRES_HUMIDITY);
+ /*
+ down_data[14] = (short)(hr.adxl_acc[0]*COMPRES_ACC);
+ down_data[15] = (short)(hr.adxl_acc[1]*COMPRES_ACC);
+ down_data[16] = (short)(hr.adxl_acc[2]*COMPRES_ACC);
+ down_data[18] = (short)(hr.press_pitot*COMPRES_PITOT);
+ */
+ //down_data[19] = (short)(hr.gps_alt*COMPRES_ALTTITUDE);
+
+ //for(iter=0;iter<17;iter++)down_data[iter]=0xFFFF; //for debug
+ //for(iter=6;iter<13;iter++)pc.printf("data[%d] = %d\r\n",iter,down_data[iter]);
+
+ interrupt_prohibit_flag = true;
+
+ es<<(char)0x03<<hr.Rocket_Phase<<state<<down_time<< //1+1+4
+ hr.latitude<<hr.longitude<< //4+4
+ //hr.vol_external<<hr.current_external<<hr.vol_inner<<hr.current_inner
+ down_data[4]<<down_data[5]<<down_data[6]<<down_data[7]
+ <<down_data[8]<<down_data[11]<<
+ down_data[12]<<down_data[13]<<es.endl;
+ //hr.pressure<<hr.luminosity<<
+ //hr.temperature<<hr.humidity<<es.endl;
+ /*down_data[4]<<down_data[5]<<down_data[6]<<down_data[7]<< //2+2+2+2
+ down_data[8]<<down_data[11]<<down_data[12]<<down_data[13] //2+2+2+2
+ <<es.endl;*/
+
+ interrupt_prohibit_flag = false;
+ }
+ if(hr.Rocket_Phase==4)
+ {
+ pc.printf("call phase 4\r\n");
+ char state=0;
+ int down_time = in_time + (int)Inner_Time.read();
+
+ short down_data[20];
+
+ if(safety_flag) state |= 0x01; //0bit
+ if(flight_detect_mode) state |= 0x02; //1bit
+ if(hr.flight_pin_state) state |= 0x04; //2bit
+ if(flight_mode) state |= 0x08; //3bit
+ if(sepa_accept) state |= 0x10; //4bit
+ if(extrusion_accept) state |= 0x20; //5bit
+ if(collect_mode) state |= 0x40; //6bit
+ if(vertex_arrival) state |= 0x80; //7bit
+
+ /*
+ down_data[17] = (short)(Inner_Time.read()*COMPRES_Inner_Time);
+ down_data[0] = (short)(hr.w/COMPRES_Q);
+ down_data[1] = (short)(hr.x/COMPRES_Q);
+ down_data[2] = (short)(hr.y/COMPRES_Q);
+ down_data[3] = (short)(hr.z/COMPRES_Q);
+ */
+ //down_data[4] = (short)(hr.vol_external*COMPRES_VOLTAGE);
+ //down_data[5] = (short)(hr.current_external*COMPRES_CURRENT);
+ down_data[6] = (short)(hr.vol_inner*COMPRES_VOLTAGE);
+ down_data[7] = (short)(hr.current_inner*COMPRES_CURRENT);
+ //down_data[8] = (short)(hr.pressure*COMPRES_PRESSURE);
+ //down_data[9] = (short)(hr.altitude*COMPRES_ALTITUDE);
+ //down_data[10] = (short)(hr.velocity*COMPRES_VELOCITY);
+ down_data[11] = (short)(hr.luminosity*COMPRES_LUMINOSITY);
+ down_data[12] = (short)(hr.temperature*COMPRES_TEMPERATURE);
+ down_data[13] = (short)(hr.humidity*COMPRES_HUMIDITY);
+ /*
+ down_data[14] = (short)(hr.adxl_acc[0]*COMPRES_ACC);
+ down_data[15] = (short)(hr.adxl_acc[1]*COMPRES_ACC);
+ down_data[16] = (short)(hr.adxl_acc[2]*COMPRES_ACC);
+ */
+ down_data[18] = (short)(hr.press_pitot*COMPRES_PITOT);
+ down_data[19] = (short)(hr.gps_alt*COMPRES_ALTITUDE);
+
+ //for(iter=0;iter<17;iter++)down_data[iter]=0xFFFF; //for debug
+ //for(iter=6;iter<13;iter++)pc.printf("data[%d] = %d\r\n",iter,down_data[iter]);
+
+ interrupt_prohibit_flag=true;
+
+ es<<(char)0x03<<hr.Rocket_Phase<<state<<down_time<< //1+1+4
+ hr.latitude<<hr.longitude<< //4+4
+ //down_data[0]<<down_data[1]<<down_data[2]<<down_data[3]<< //2*4
+ down_data[6]<<down_data[7]<<hr.altitude<<hr.velocity<< //2+2+4+2
+ down_data[11]<<down_data[12]<<down_data[13]<< //2*3
+ down_data[18]<<down_data[19]<<es.endl; //2*2
+
+ interrupt_prohibit_flag = false;
+ }
+ if(hr.Rocket_Phase==5)
+ {
+
+ char state=0;
+ int down_time = in_time + (int)Inner_Time.read();
+
+ short down_data[20];
+
+ if(safety_flag) state |= 0x01; //0bit
+ if(flight_detect_mode) state |= 0x02; //1bit
+ if(hr.flight_pin_state) state |= 0x04; //2bit
+ if(flight_mode) state |= 0x08; //3bit
+ if(sepa_accept) state |= 0x10; //4bit
+ if(extrusion_accept) state |= 0x20; //5bit
+ if(collect_mode) state |= 0x40; //6bit
+ if(vertex_arrival) state |= 0x80; //7bit
+
+ /*
+ down_data[17] = (short)(Inner_Time.read()*COMPRES_Inner_Time);
+ down_data[0] = (short)(hr.w/COMPRES_Q);
+ down_data[1] = (short)(hr.x/COMPRES_Q);
+ down_data[2] = (short)(hr.y/COMPRES_Q);
+ down_data[3] = (short)(hr.z/COMPRES_Q);
+ */
+ //down_data[4] = (short)(hr.vol_external*COMPRES_VOLTAGE);
+ //down_data[5] = (short)(hr.current_external*COMPRES_CURRENT);
+ down_data[6] = (short)(hr.vol_inner*COMPRES_VOLTAGE);
+ down_data[7] = (short)(hr.current_inner*COMPRES_CURRENT);
+ down_data[8] = (short)(hr.pressure*COMPRES_PRESSURE);
+ //down_data[9] = (short)(hr.altitude*COMPRES_ALTITUDE);
+ //down_data[10] = (short)(hr.velocity*COMPRES_VELOCITY);
+ down_data[11] = (short)(hr.luminosity*COMPRES_LUMINOSITY);
+ down_data[12] = (short)(hr.temperature*COMPRES_TEMPERATURE);
+ down_data[13] = (short)(hr.humidity*COMPRES_HUMIDITY);
+ /*
+ down_data[14] = (short)(hr.adxl_acc[0]*COMPRES_ACC);
+ down_data[15] = (short)(hr.adxl_acc[1]*COMPRES_ACC);
+ down_data[16] = (short)(hr.adxl_acc[2]*COMPRES_ACC);
+ down_data[18] = (short)(hr.press_pitot*COMPRES_PITOT);
+ */
+ //down_data[19] = (short)(hr.gps_alt*COMPRES_ALTTITUDE);
+
+ //for(iter=0;iter<17;iter++)down_data[iter]=0xFFFF; //for debug
+ //for(iter=6;iter<13;iter++)pc.printf("data[%d] = %d\r\n",iter,down_data[iter]);
+
+
+ interrupt_prohibit_flag=true;
+
+ es<<(char)0x03<<hr.Rocket_Phase<<state<<down_time<<
+ hr.longitude<<hr.latitude<<
+ down_data[6]<<down_data[7]<<down_data[8]<<
+ down_data[11]<<down_data[12]<<down_data[13]<<
+ es.endl;
+
+ interrupt_prohibit_flag = false;
+ }
+ }
+}
+
+
+void header0x05()
+{
+ interrupt_prohibit_flag = true;
+ es << (char)0x05 << 'd' << es.endl; //flight pin detect
+ interrupt_prohibit_flag = false;
+}
+
+void header0x06()
+{
+ interrupt_prohibit_flag = true;
+ es << (char)0x06 << 'n' << es.endl; //nicrom fire finish
+ interrupt_prohibit_flag = false;
+}
+
+void header0x07()
+{
+ interrupt_prohibit_flag = true;
+ es << (char)0x07 << 's' << es.endl; //solenoid push finish
+ interrupt_prohibit_flag = false;
+}
+
+void header0x08()
+{
+ interrupt_prohibit_flag = true;
+ es << (char)0x08 << 'c' << es.endl; //collect mode
+ interrupt_prohibit_flag = false;
+}
+
+void header0x09(char phase)
+{
+ interrupt_prohibit_flag = true;
+ es << (char)0x09 << phase << es.endl;
+ interrupt_prohibit_flag = false;
+}
+
+
+/*******************************************************************************
+アップリンク
+*******************************************************************************/
+
+void header0x10()
+{
+ pc.printf("cmd receive\r\n");
+
+ char buff = es.data[0];
+
+ interrupt_prohibit_flag = true;
+ if(buff == 'r')
+ {
+ safety_flag = true;
+ flight_detect_mode = true;
+ strain_state = 1;
+ }
+ else if(buff == 'f') //forced transition fllight mode
+ {
+ hr.flight_pin_state = true;
+ flight_mode = true;
+ pc.printf("forced transmit flight mode\r\n");
+ }
+ else if(buff == 'n' && hr.Rocket_Phase==4)
+ {
+ nicrom.fire();
+ wait(FIRE_TIME);
+ nicrom.stop();
+ header0x06();
+ iter = 0;
+ while(1)
+ {
+ solenoid = 1;
+ wait(OSHIDASHI_TIME);
+ solenoid = 0;
+ wait(OSHIDASHI_TIME);
+ iter++;
+ pc.printf("iter = %d\r\n",iter);
+ if(iter > 10)break;
+ }
+ header0x07();
+ pc.printf("forced separate from nicrom\r\n");
+ }
+ else if(buff == 's' && hr.Rocket_Phase==4)
+ {
+ int i=0;
+ while(1)
+ {
+ solenoid = 1;
+ wait(OSHIDASHI_TIME);
+ solenoid = 0;
+ wait(OSHIDASHI_TIME);
+ i++;
+ if(i > 10)break;
+ }
+ header0x07();
+ pc.printf("forced separate from solenoid\r\n");
+ }
+ else if(buff == 'c' && hr.Rocket_Phase==4)
+ {
+ collect_mode = true;
+ pc.printf("forced transmit collect mode\r\n");
+ }
+ else if(buff == 'd')
+ {
+ pc.printf("debug cmd receive\r\n");
+ wait_ms(50);
+ es << (char)0x1A << 'd' << es.endl;
+ }
+ else if(buff == 'i')
+ {
+ Inner_Time.reset();
+ wait_ms(50);
+ es << (char)0x1A << 'i' << es.endl;
+ pc.printf("Inner time reset complete\r\n");
+ }
+ else if(buff == 'b')
+ {
+ safety_device_reboot = true;
+ safety_flag = false;
+ flight_detect_mode = false;
+ wait_ms(50);
+ es << (char)0x1A << 'b' << es.endl;
+ }
+ interrupt_prohibit_flag = false;
+}
+
+/*******************************************************************************
+main関数
+*******************************************************************************/
+int main()
+{
+ pc.baud(115200);
+ wait(1.0f);
+ rocket_state.attach(&downlink0x03, 5.0f);
+ wait(1.0f);
+ strain_state = 0;
+ /***************************************************************************
+ フェーズ1: 接続確認、初期設定
+ ***************************************************************************/
+
+ pc.printf("-------------------------------------------------------\r\n");
+ pc.printf("Phase 1:接続確認、初期設定\r\n");
+
+ hr.Rocket_Phase = 1;
+
+ header0x09(hr.Rocket_Phase);
+
+ short check = 0; //センサが通信できているかカウントする変数
+ //各ビットにOK,NGの情報を持たせた
+ Inner_Time.start();
+ timer_reset.attach(&inner_time_reset, 1800);
+
+ sd_ticker.attach(&write_sd, 10.0);
+
+
+ // センサの初期設定、接続確認
+ if(external_ina.Connection_check() == 0)
+ {
+ pc.printf("External INA226 OK\r\n");
+ external_ina.set_callibretion();
+ check |= 0x001;
+ }
+ if(inner_ina.Connection_check() == 0)
+ {
+ pc.printf("Inner INA226 OK\r\n");
+ inner_ina.set_callibretion();
+ inner_ina.get_Voltage_current(&hr.vol_inner, &hr.current_inner);
+ check |= 0x002;
+ }
+ if(mympu9250.sensorTest())
+ {
+ pc.printf("MPU9250 kansei OK\r\n");
+ check |= 0x004;
+ }
+ if(mympu9250.mag_sensorTest())
+ {
+ pc.printf("MPU9250 tiziki OK\r\n");
+ check |= 0x008;
+ }
+ if(myLPS22HB.whoAmI() == 0)
+ {
+ pc.printf("LPS22HB OK\r\n");
+ myLPS22HB.begin(75);
+ check |= 0x010;
+ }
+ if(0 < mySHT35.get_temp() && mySHT35.get_temp() < 28) //実際に温度を測ってみていい感じの範囲に収まってるかで接続確認としている
+ {
+ pc.printf("SHT-35 maybe OK\r\n");
+ check |= 0x020;
+ }
+ if(myTSL2561.connect_check() == 1)
+ {
+ pc.printf("TSL2561 OK\r\n");
+ myTSL2561.begin();
+ hr.TSL_time = myTSL2561.set_rate(0);
+ check |= 0x040;
+ }
+ if(three.getDeviceID() == (char)ADXL_DEVICE_ID)
+ {
+ three.setDataFormatControl(0x0B);
+ three.setDataRate(ADXL375_3200HZ);
+ three.setPowerControl(MeasurementMode);
+ pc.printf("ADXL375 OK\r\n");
+ check |= 0x080;
+ }
+ else pc.printf("ADXL device ID = 0x%X\r\n",three.getDeviceID());
+ if((log_fp = fopen("/sd/log.txt" ,"w")) != NULL)
+ {
+ pc.printf("SDFileSystem OK\r\n");
+ check |= 0x100;
+ }
+ if(gps.gps_readable)
+ {
+ pc.printf("GPS OK\r\n");
+ check |= 0x200;
+ }
+
+ //gps.debug(true);
+
+ //ほかのセンサの接続確認
+
+ es << char(0x02) << check << es.endl;
+
+ solenoid = 0;
+ i2c_Bus.frequency(400000);
+ hr.flight_pin_state = false;
+ extrusion_pin.mode(PullUp);
+ flight_pin.mode(PullUp);
+
+ es.attach(&header0x10, (char)0x10);
+
+ flight_time.start();
+
+ lps_read.attach(&get_lps_press, 1.0);
+ ina_read.attach(&get_ina, 0.2);
+ sht_read.attach(&get_sht, 0.3);
+ tsl_read.attach(&get_tsl, 0.6);
+ adxl_read.attach(&get_adxl, 0.8);
+ //attitude_ticker.attach(&get_attitude, 0.8f);
+ gps_read.attach(&get_gps, 1.5);
+
+ safety_device();
+
+ lps_read.detach();
+ ina_read.detach();
+ tsl_read.detach();
+ sht_read.detach();
+ adxl_read.detach();
+ gps_read.detach();
+
+ //header0x09(hr.Rocket_Phase);
+
+
+ /***************************************************************************
+ フェーズ2: 準備
+ ***************************************************************************/
+
+ pc.printf("-------------------------------------------------------\r\n");
+ pc.printf("Phase 2: 準備\r\n");
+
+ hr.Rocket_Phase = 2;
+
+ header0x09(hr.Rocket_Phase);
+
+ //Ticker
+
+ lps_read.attach(&get_lps, 1.0);
+ ina_read.attach(&get_in_ina, 0.0013);
+ tsl_read.attach(&get_tsl, 0.02);
+ sht_read.attach(&get_sht, 0.1);
+ //adxl_read.attach(&get_adxl, 2.5);
+ gps_read.attach(&get_gps, 0.1);
+ pitot_press.attach(&get_pitot, 0.8);
+ //attitude_ticker.attach(&get_attitude, 0.1);
+
+ pc.printf("timer prepare OK\r\n");
+
+ //各センサでデータ取得
+ //姿勢計算開始
+ //ダウンリンク開始
+
+ //rocket_state.attach(&downlink0x03, 0.8f);
+
+
+
+
+
+ /***************************************************************************
+ フェーズ3: 発射待機
+ ***************************************************************************/
+
+ pc.printf("-------------------------------------------------------\r\n");
+ pc.printf("Phase 3: 発射待機\r\n");
+
+ hr.Rocket_Phase = 3;
+
+ //header0x09(hr.Rocket_Phase);
+
+ flight_pin.rise(&flightPin_detect);
+
+ flight_detect_mode = true;
+ hr.flight_pin_state = false;
+
+ flight_detect();
+ strain_state = 0;
+
+ header0x05();
+ /***************************************************************************
+ フェーズ4: 飛行
+ ***************************************************************************/
+ pc.printf("-------------------------------------------------------\r\n");
+ pc.printf("Rocket Phase 4\r\n");
+
+ hr.Rocket_Phase = 4;
+
+ flight_time.reset();
+
+ rocket_state.detach();
+
+ rocket_state.attach(&downlink0x03, 1.0);
+
+ flight_mode = true;
+
+ while(!collect_mode)
+ {
+ separate_action();
+ extrusion_action();
+ collect();
+ for(iter = 0; iter < DATA_NUMBER; iter ++)if(SD_BUFF_COUNT_FLAG[iter] == 1)write_sd(iter);
+ }
+
+ pc.printf("flight while finish\r\n");
+ wait_ms(100);
+ header0x08();
+ flight_time.stop();
+
+ /***************************************************************************
+ フェーズ5: 回収
+ ***************************************************************************/
+ pc.printf("-------------------------------------------------------\r\n");
+ pc.printf("Rocket Phase 5\r\n");
+
+ hr.Rocket_Phase = 5;
+
+ header0x09(hr.Rocket_Phase);
+
+ pc.printf("遷音速超え太郎を回収\r\n");
+
+ fclose(log_fp);
+
+
+ while(1)
+ {
+
+ pc.printf("finish\r\n");
+ wait(2.0f);
+
+ }
+}