Hybrid_OB2021 / Mbed 2 deprecated Hybrid_OB2021_MAIN

v1

Dependencies:   Nichrome_lib mbed mpu9250_i2c IM920 INA226_lib PQLPS22HB EEPROM_lib GPS_interrupt

main.cpp@4:345e55d8e8a3, 2021-10-19 (annotated)

Committer:
imadaemi
Date:
Tue Oct 19 08:23:02 2021 +0000
Revision:
4:345e55d8e8a3
Parent:
3:eca103d94b60 
20211019

Who changed what in which revision?

UserRevisionLine numberNew contents of line
imadaemi 0:4e38f8b1c183 1 #include "mbed.h"
imadaemi 0:4e38f8b1c183 2 #include "IM920.h"
imadaemi 0:4e38f8b1c183 3 #include "GPS_interrupt.h"
imadaemi 0:4e38f8b1c183 4 #include "PQLPS22HB.h"
imadaemi 0:4e38f8b1c183 5 #include "mpu9250_i2c.h"
imadaemi 0:4e38f8b1c183 6 #include "INA226.h"
imadaemi 0:4e38f8b1c183 7 #include "Nichrome_lib.h"
imadaemi 0:4e38f8b1c183 8 #include "EEPROM_lib.h"
imadaemi 0:4e38f8b1c183 9
imadaemi 4:345e55d8e8a3 10 #define RESET_TIME 1800000 //1800000ms→30分
imadaemi 3:eca103d94b60 11 //#define RESET_TIME 30000000 // 30000000→30秒(実験用)
imadaemi 3:eca103d94b60 12 #define FIRE_TIME 2.0
imadaemi 4:345e55d8e8a3 13 #define TO_SEPARATE_TIME 8.0
imadaemi 3:eca103d94b60 14 //#define TO_SEPARATE_TIME 300.0
imadaemi 3:eca103d94b60 15 #define TO_COLLECTION_TIME 30.0
imadaemi 3:eca103d94b60 16 //#define TO_COLLECTION_TIME 10.0 // 実験用
imadaemi 4:345e55d8e8a3 17 #define ALTITUDE_LIMIT 50.0
imadaemi 3:eca103d94b60 18 //#define ALTITUDE_LIMIT 10.0
imadaemi 0:4e38f8b1c183 19 #define ACC_RANGE _16G
imadaemi 0:4e38f8b1c183 20 #define GYRO_RANGE _2000DPS
imadaemi 3:eca103d94b60 21 //#define LPF_COEFFICIENT_ALT 0.01
imadaemi 3:eca103d94b60 22 //#define LPF_COEFFICIENT_ALT 0.001
imadaemi 3:eca103d94b60 23 #define LPF_COEFFICIENT_ALT 0.05
imadaemi 3:eca103d94b60 24 #define LPF_COEFFICIENT_VEL 0.2
imadaemi 3:eca103d94b60 25
imadaemi 3:eca103d94b60 26 #define TEMP_MULTIPLIER 100
imadaemi 3:eca103d94b60 27 #define LPF_ALT_MULTIPLIER 100
imadaemi 3:eca103d94b60 28 #define LPF_VEL_MULTIPLIER 100
imadaemi 0:4e38f8b1c183 29
imadaemi 0:4e38f8b1c183 30 // ***************************************************
imadaemi 0:4e38f8b1c183 31 // コンストラクタ
imadaemi 0:4e38f8b1c183 32 // ***************************************************
imadaemi 3:eca103d94b60 33 Serial pc(USBTX, USBRX, 230400);
imadaemi 0:4e38f8b1c183 34 Serial gps_serial(p9, p10, 115200);
imadaemi 0:4e38f8b1c183 35 Serial im920_serial(p13, p14, 115200);
imadaemi 0:4e38f8b1c183 36
imadaemi 0:4e38f8b1c183 37 I2C i2c(p28, p27);
imadaemi 0:4e38f8b1c183 38
imadaemi 0:4e38f8b1c183 39 IM920 im920(im920_serial, pc, 115200);
imadaemi 0:4e38f8b1c183 40 GPS_interrupt gps(&gps_serial);
imadaemi 0:4e38f8b1c183 41 LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW);
imadaemi 0:4e38f8b1c183 42 mpu9250 mpu9250(i2c,AD0_HIGH);
imadaemi 0:4e38f8b1c183 43 myINA226 ina226_main(i2c, myINA226::A1_GND, myINA226::A0_GND);
imadaemi 0:4e38f8b1c183 44 myINA226 ina226_sep(i2c, myINA226::A1_VDD, myINA226::A0_VDD);
imadaemi 0:4e38f8b1c183 45 Nichrome_lib nich(p20);
imadaemi 3:eca103d94b60 46 EEPROM_lib EEPROM(i2c, 4);
imadaemi 0:4e38f8b1c183 47
imadaemi 3:eca103d94b60 48 InterruptIn flightpin(p18);
imadaemi 3:eca103d94b60 49 DigitalOut nich_led(LED1);
imadaemi 3:eca103d94b60 50 DigitalOut im920_busy_led(LED2);
imadaemi 0:4e38f8b1c183 51 DigitalOut pinA(p21);
imadaemi 0:4e38f8b1c183 52 DigitalOut pinB(p22);
imadaemi 0:4e38f8b1c183 53 DigitalOut pinC(p23);
imadaemi 0:4e38f8b1c183 54
imadaemi 3:eca103d94b60 55 DigitalIn im920_busy(p15);
imadaemi 3:eca103d94b60 56
imadaemi 1:3151936d9c55 57 Ticker send_data;
imadaemi 3:eca103d94b60 58 Ticker save_data;
imadaemi 3:eca103d94b60 59 Ticker get_data;
imadaemi 3:eca103d94b60 60
imadaemi 3:eca103d94b60 61 Timer main_timer;
imadaemi 3:eca103d94b60 62
imadaemi 3:eca103d94b60 63 //Timeout to_separate_time;
imadaemi 3:eca103d94b60 64 //Timeout to_collection_time;
imadaemi 3:eca103d94b60 65 //Timeout fire_time;
imadaemi 3:eca103d94b60 66 Timeout task_timeout;
imadaemi 0:4e38f8b1c183 67 // ***************************************************
imadaemi 0:4e38f8b1c183 68 // 関数の宣言
imadaemi 0:4e38f8b1c183 69 // ***************************************************
imadaemi 0:4e38f8b1c183 70 void uplink();
imadaemi 0:4e38f8b1c183 71 void echoIM();
imadaemi 3:eca103d94b60 72 void PerformTask();
imadaemi 3:eca103d94b60 73 void FlightPinDetect();
imadaemi 3:eca103d94b60 74 void ChangeModeToSep();
imadaemi 3:eca103d94b60 75 void ChangeModeToCol();
imadaemi 0:4e38f8b1c183 76 void Separate();
imadaemi 0:4e38f8b1c183 77 void StopSeparate();
imadaemi 0:4e38f8b1c183 78 void SetSensor();
imadaemi 0:4e38f8b1c183 79 void GetData();
imadaemi 3:eca103d94b60 80 void SendLaunchTime();
imadaemi 2:980edad0eea2 81 void SendData();
imadaemi 3:eca103d94b60 82 void SaveData();
imadaemi 0:4e38f8b1c183 83 void setEEPROMGroup(int group_num);
imadaemi 0:4e38f8b1c183 84
imadaemi 0:4e38f8b1c183 85 // ***************************************************
imadaemi 0:4e38f8b1c183 86 // 無線のヘッダまとめ
imadaemi 0:4e38f8b1c183 87 // ***************************************************
imadaemi 0:4e38f8b1c183 88 const char HEADER_SETUP = 0x01;
imadaemi 3:eca103d94b60 89 // 0x01, GPS, LPS22HB, MPU9250, MPU9250_MAG, INA226_MAIN, INA226_SEP
imadaemi 2:980edad0eea2 90 // 1 1 1 1 1 1 1
imadaemi 0:4e38f8b1c183 91
imadaemi 3:eca103d94b60 92 const char HEADER_DATA = 0x02;
imadaemi 3:eca103d94b60 93 // 0xA2, duration_main_time, duration_flight_time, mode, nich_status, flightpin_status, lat, lon, height, press, temp, altitude, voltage_main, voltage_sep, current_main, current_sep
imadaemi 3:eca103d94b60 94 // 1 4 4 1 0.125 0.125 4 4 4 4 4 4 4 4 4 4
imadaemi 3:eca103d94b60 95 const char HEADER_LAUNCH_TIME = 0x03;
imadaemi 0:4e38f8b1c183 96
imadaemi 3:eca103d94b60 97 const char HEADER_ECHO = 0x05;
imadaemi 0:4e38f8b1c183 98 // 0xA5,コマンド
imadaemi 0:4e38f8b1c183 99 // 1 1
imadaemi 0:4e38f8b1c183 100
imadaemi 0:4e38f8b1c183 101 // ***************************************************
imadaemi 0:4e38f8b1c183 102 // 変数の宣言
imadaemi 0:4e38f8b1c183 103 // ***************************************************
imadaemi 3:eca103d94b60 104 float launch_time = 0;
imadaemi 3:eca103d94b60 105 float previous_main_time = 0;
imadaemi 3:eca103d94b60 106 float flight_time = 0;
imadaemi 3:eca103d94b60 107 float main_time = 0;
imadaemi 3:eca103d94b60 108 int time_reset_counter = 0;
imadaemi 3:eca103d94b60 109
imadaemi 3:eca103d94b60 110 bool send_launch_time_status = false;
imadaemi 3:eca103d94b60 111
imadaemi 3:eca103d94b60 112 bool top_detect = false;
imadaemi 3:eca103d94b60 113 char bitshift_top_detect;
imadaemi 3:eca103d94b60 114
imadaemi 3:eca103d94b60 115 bool nich_status = false;
imadaemi 3:eca103d94b60 116 char bitshift_nich_status;
imadaemi 3:eca103d94b60 117
imadaemi 3:eca103d94b60 118 bool flightpin_status = false;
imadaemi 3:eca103d94b60 119 char bitshift_flightpin_status;
imadaemi 3:eca103d94b60 120
imadaemi 3:eca103d94b60 121 bool save_data_status = false;
imadaemi 3:eca103d94b60 122 char bitshift_save_data_status;
imadaemi 3:eca103d94b60 123
imadaemi 3:eca103d94b60 124 char bitshift_sum;
imadaemi 1:3151936d9c55 125
imadaemi 0:4e38f8b1c183 126 bool header_set = false;
imadaemi 0:4e38f8b1c183 127 char im_buf[55];//16なのって意味ある？
imadaemi 0:4e38f8b1c183 128 int im_buf_n = 0;
imadaemi 0:4e38f8b1c183 129
imadaemi 0:4e38f8b1c183 130 float lat, lon, height;
imadaemi 3:eca103d94b60 131 int satellite_number;
imadaemi 3:eca103d94b60 132 float press, temp;
imadaemi 3:eca103d94b60 133 float altitude;
imadaemi 3:eca103d94b60 134 //float ground_press = 1013.0,ground_temp = 25.0;
imadaemi 3:eca103d94b60 135 float ground_press = -1.0,ground_temp = -1.0;
imadaemi 3:eca103d94b60 136
imadaemi 3:eca103d94b60 137 float velocity;
imadaemi 3:eca103d94b60 138 float previous_lpf_velocity;
imadaemi 3:eca103d94b60 139 float lpf_velocity;
imadaemi 3:eca103d94b60 140
imadaemi 3:eca103d94b60 141 float previous_lpf_altitude;
imadaemi 3:eca103d94b60 142 float lpf_altitude;
imadaemi 0:4e38f8b1c183 143
imadaemi 0:4e38f8b1c183 144 float imu[6], mag[3];
imadaemi 0:4e38f8b1c183 145 float mag_geo[3];
imadaemi 0:4e38f8b1c183 146
imadaemi 0:4e38f8b1c183 147 float voltage_main, voltage_sep;
imadaemi 0:4e38f8b1c183 148 float current_main, current_sep;
imadaemi 0:4e38f8b1c183 149
imadaemi 3:eca103d94b60 150 int ptr, n = 0;
imadaemi 3:eca103d94b60 151 int eeprom_ptr = 0;
imadaemi 3:eca103d94b60 152 int eeprom_group_counter = 0;
imadaemi 3:eca103d94b60 153 int eeprom_number = 0;
imadaemi 3:eca103d94b60 154
imadaemi 3:eca103d94b60 155 const char MODE_SETUP = 0x00;
imadaemi 3:eca103d94b60 156 const char MODE_SAFE = 0x01;
imadaemi 3:eca103d94b60 157 const char MODE_TO_ONCALL = 0x02;
imadaemi 3:eca103d94b60 158 const char MODE_ONCALL = 0x03;
imadaemi 3:eca103d94b60 159 const char MODE_TO_FLIGHT = 0x04;
imadaemi 3:eca103d94b60 160 const char MODE_FLIGHT = 0x05;
imadaemi 3:eca103d94b60 161 const char MODE_TO_SEPARATE = 0x06;
imadaemi 3:eca103d94b60 162 const char MODE_SEPARATE = 0x07;
imadaemi 3:eca103d94b60 163 const char MODE_TO_DESCEND = 0x08;
imadaemi 3:eca103d94b60 164 const char MODE_DESCEND = 0x09;
imadaemi 3:eca103d94b60 165 const char MODE_TO_COLLECTION = 0x0a;
imadaemi 3:eca103d94b60 166 const char MODE_COLLECTION = 0x0b;
imadaemi 3:eca103d94b60 167
imadaemi 3:eca103d94b60 168 char mode = MODE_SETUP;
imadaemi 3:eca103d94b60 169
imadaemi 0:4e38f8b1c183 170 // ***************************************************
imadaemi 0:4e38f8b1c183 171 // メイン関数
imadaemi 0:4e38f8b1c183 172 // ***************************************************
imadaemi 0:4e38f8b1c183 173 int main() {
imadaemi 0:4e38f8b1c183 174 pc.printf("Hello Main!\r\n");
imadaemi 0:4e38f8b1c183 175 im920.attach(&uplink, 0xF0);
imadaemi 2:980edad0eea2 176 send_data.attach(&SendData, 1.0);
imadaemi 3:eca103d94b60 177 get_data.attach(&GetData, 0.02);
imadaemi 0:4e38f8b1c183 178
imadaemi 0:4e38f8b1c183 179 while(1){
imadaemi 3:eca103d94b60 180 PerformTask();
imadaemi 3:eca103d94b60 181 /*
imadaemi 3:eca103d94b60 182 if(im920_busy){
imadaemi 3:eca103d94b60 183 im920_busy_led = 1;
imadaemi 3:eca103d94b60 184 }else{
imadaemi 3:eca103d94b60 185 im920_busy_led = 0;
imadaemi 3:eca103d94b60 186 }
imadaemi 3:eca103d94b60 187 */
imadaemi 0:4e38f8b1c183 188 }
imadaemi 0:4e38f8b1c183 189 }
imadaemi 3:eca103d94b60 190
imadaemi 0:4e38f8b1c183 191 // ***************************************************
imadaemi 0:4e38f8b1c183 192 // アップリンク（地上局から）
imadaemi 0:4e38f8b1c183 193 // ***************************************************
imadaemi 0:4e38f8b1c183 194 void uplink(){
imadaemi 0:4e38f8b1c183 195 echoIM();
imadaemi 0:4e38f8b1c183 196 switch(im920.data[1]){
imadaemi 3:eca103d94b60 197
imadaemi 3:eca103d94b60 198 case 'U':
imadaemi 3:eca103d94b60 199 if(mode == MODE_FLIGHT || mode == MODE_ONCALL){
imadaemi 3:eca103d94b60 200 mode = MODE_SETUP;
imadaemi 3:eca103d94b60 201 save_data.detach();
imadaemi 3:eca103d94b60 202 save_data_status = false;
imadaemi 3:eca103d94b60 203 pc.printf("********************\r\n");
imadaemi 3:eca103d94b60 204 pc.printf("Set : MODE_SETUP\r\n");
imadaemi 3:eca103d94b60 205 pc.printf("********************\r\n\r\n");
imadaemi 3:eca103d94b60 206 }
imadaemi 3:eca103d94b60 207 break;
imadaemi 3:eca103d94b60 208
imadaemi 3:eca103d94b60 209 case 'O':
imadaemi 3:eca103d94b60 210 if(mode == MODE_SAFE){
imadaemi 3:eca103d94b60 211 mode = MODE_TO_ONCALL;
imadaemi 3:eca103d94b60 212 pc.printf("********************\r\n");
imadaemi 3:eca103d94b60 213 pc.printf("Set : MODE_TO_ONCALL\r\n");
imadaemi 3:eca103d94b60 214 pc.printf("********************\r\n\r\n");
imadaemi 3:eca103d94b60 215 }
imadaemi 3:eca103d94b60 216 break;
imadaemi 3:eca103d94b60 217
imadaemi 3:eca103d94b60 218 case 'F':
imadaemi 3:eca103d94b60 219 if(mode == MODE_ONCALL){
imadaemi 3:eca103d94b60 220 mode = MODE_TO_FLIGHT;
imadaemi 3:eca103d94b60 221 pc.printf("********************\r\n");
imadaemi 3:eca103d94b60 222 pc.printf("Set : MODE_TO_FLIGHT\r\n");
imadaemi 3:eca103d94b60 223 pc.printf("********************\r\n\r\n");
imadaemi 3:eca103d94b60 224 }
imadaemi 3:eca103d94b60 225 break;
imadaemi 3:eca103d94b60 226
imadaemi 3:eca103d94b60 227 case 'S':
imadaemi 3:eca103d94b60 228 if(mode == MODE_FLIGHT){
imadaemi 3:eca103d94b60 229 mode = MODE_TO_SEPARATE;
imadaemi 3:eca103d94b60 230 pc.printf("********************\r\n");
imadaemi 3:eca103d94b60 231 pc.printf("Set : MODE_TO_SEPARATE\r\n");
imadaemi 3:eca103d94b60 232 pc.printf("********************\r\n\r\n");
imadaemi 3:eca103d94b60 233 }
imadaemi 3:eca103d94b60 234 break;
imadaemi 3:eca103d94b60 235
imadaemi 0:4e38f8b1c183 236 case 0x01:
imadaemi 0:4e38f8b1c183 237 pc.printf("********************\r\n");
imadaemi 0:4e38f8b1c183 238 pc.printf("SEPARATE\r\n");
imadaemi 0:4e38f8b1c183 239 pc.printf("********************\r\n\r\n");
imadaemi 0:4e38f8b1c183 240 Separate();
imadaemi 0:4e38f8b1c183 241 break;
imadaemi 0:4e38f8b1c183 242
imadaemi 0:4e38f8b1c183 243 case 0x00:
imadaemi 0:4e38f8b1c183 244 pc.printf("********************\r\n");
imadaemi 0:4e38f8b1c183 245 pc.printf("STOP SEPARATE\r\n");
imadaemi 0:4e38f8b1c183 246 pc.printf("********************\r\n\r\n");
imadaemi 0:4e38f8b1c183 247 StopSeparate();
imadaemi 0:4e38f8b1c183 248 break;
imadaemi 0:4e38f8b1c183 249 }
imadaemi 0:4e38f8b1c183 250 }
imadaemi 0:4e38f8b1c183 251
imadaemi 0:4e38f8b1c183 252 // ***************************************************
imadaemi 0:4e38f8b1c183 253 // 無線信号の送り返し
imadaemi 0:4e38f8b1c183 254 // ***************************************************
imadaemi 0:4e38f8b1c183 255 void echoIM(){
imadaemi 0:4e38f8b1c183 256 im920.header(HEADER_ECHO);
imadaemi 0:4e38f8b1c183 257 im920.write(im920.data[1]);
imadaemi 0:4e38f8b1c183 258 im920.send();
imadaemi 0:4e38f8b1c183 259 }
imadaemi 0:4e38f8b1c183 260
imadaemi 0:4e38f8b1c183 261 // ***************************************************
imadaemi 3:eca103d94b60 262 // タスクの実行
imadaemi 3:eca103d94b60 263 // ***************************************************
imadaemi 3:eca103d94b60 264 void PerformTask(){
imadaemi 3:eca103d94b60 265 switch(mode){
imadaemi 3:eca103d94b60 266
imadaemi 3:eca103d94b60 267 case(MODE_SETUP):
imadaemi 3:eca103d94b60 268 SetSensor();
imadaemi 3:eca103d94b60 269 main_timer.start();
imadaemi 3:eca103d94b60 270 send_data.detach();
imadaemi 3:eca103d94b60 271 send_data.attach(&SendData, 1.0);
imadaemi 3:eca103d94b60 272 mode = MODE_SAFE;
imadaemi 3:eca103d94b60 273 break;
imadaemi 3:eca103d94b60 274
imadaemi 3:eca103d94b60 275 case(MODE_SAFE):
imadaemi 3:eca103d94b60 276 break;
imadaemi 3:eca103d94b60 277
imadaemi 3:eca103d94b60 278 case(MODE_TO_ONCALL):
imadaemi 3:eca103d94b60 279 ground_press = press;
imadaemi 3:eca103d94b60 280 save_data.detach();
imadaemi 3:eca103d94b60 281 save_data.attach(&SaveData, 1.0);
imadaemi 3:eca103d94b60 282 save_data_status = true;
imadaemi 3:eca103d94b60 283 flightpin.rise(&FlightPinDetect);
imadaemi 3:eca103d94b60 284 mode = MODE_ONCALL;
imadaemi 3:eca103d94b60 285 break;
imadaemi 3:eca103d94b60 286
imadaemi 3:eca103d94b60 287 case(MODE_ONCALL):
imadaemi 3:eca103d94b60 288 break;
imadaemi 3:eca103d94b60 289
imadaemi 3:eca103d94b60 290 case(MODE_TO_FLIGHT):
imadaemi 3:eca103d94b60 291 save_data.detach();
imadaemi 3:eca103d94b60 292 save_data.attach(&SaveData, 0.02);
imadaemi 3:eca103d94b60 293 launch_time = main_time;
imadaemi 3:eca103d94b60 294 //to_separate_time.attach(&ChangeModeToSep,TO_SEPARATE_TIME);
imadaemi 3:eca103d94b60 295 task_timeout.detach();
imadaemi 3:eca103d94b60 296 task_timeout.attach(&ChangeModeToSep,TO_SEPARATE_TIME);
imadaemi 3:eca103d94b60 297 mode = MODE_FLIGHT;
imadaemi 3:eca103d94b60 298 break;
imadaemi 3:eca103d94b60 299
imadaemi 3:eca103d94b60 300 case(MODE_FLIGHT):
imadaemi 3:eca103d94b60 301 //pc.printf("%f\t%f\t%f\t%f\t%f\r\n",press,altitude,lpf_altitude, ALTITUDE_LIMIT,velocity);
imadaemi 3:eca103d94b60 302 if(!im920_busy && !send_launch_time_status){
imadaemi 3:eca103d94b60 303 SendLaunchTime();
imadaemi 3:eca103d94b60 304 }
imadaemi 3:eca103d94b60 305 if((lpf_altitude >= ALTITUDE_LIMIT) && (lpf_velocity < 0.0)){//and 速度＜0を書き込む
imadaemi 3:eca103d94b60 306 mode = MODE_TO_SEPARATE;
imadaemi 3:eca103d94b60 307 top_detect = true;
imadaemi 3:eca103d94b60 308 }
imadaemi 3:eca103d94b60 309 break;
imadaemi 3:eca103d94b60 310
imadaemi 3:eca103d94b60 311 case(MODE_TO_SEPARATE):
imadaemi 3:eca103d94b60 312 Separate();
imadaemi 3:eca103d94b60 313 mode = MODE_SEPARATE;
imadaemi 3:eca103d94b60 314 break;
imadaemi 3:eca103d94b60 315
imadaemi 3:eca103d94b60 316 case(MODE_SEPARATE):
imadaemi 3:eca103d94b60 317 if(nich.status == false){
imadaemi 3:eca103d94b60 318 mode = MODE_TO_DESCEND;
imadaemi 3:eca103d94b60 319 }
imadaemi 3:eca103d94b60 320 break;
imadaemi 3:eca103d94b60 321
imadaemi 3:eca103d94b60 322 case(MODE_TO_DESCEND):
imadaemi 3:eca103d94b60 323 //to_collection_time.attach(&ChangeModeToCol,TO_COLLECTION_TIME);
imadaemi 3:eca103d94b60 324 task_timeout.detach();
imadaemi 3:eca103d94b60 325 task_timeout.attach(&ChangeModeToCol,TO_COLLECTION_TIME);
imadaemi 3:eca103d94b60 326 mode = MODE_DESCEND;
imadaemi 3:eca103d94b60 327 break;
imadaemi 3:eca103d94b60 328
imadaemi 3:eca103d94b60 329 case(MODE_DESCEND):
imadaemi 3:eca103d94b60 330 break;
imadaemi 3:eca103d94b60 331
imadaemi 3:eca103d94b60 332 case(MODE_TO_COLLECTION):
imadaemi 3:eca103d94b60 333 save_data.detach();
imadaemi 3:eca103d94b60 334 save_data.attach(&SaveData, 1.0);
imadaemi 3:eca103d94b60 335 mode = MODE_COLLECTION;
imadaemi 3:eca103d94b60 336 break;
imadaemi 3:eca103d94b60 337
imadaemi 3:eca103d94b60 338 case(MODE_COLLECTION):
imadaemi 3:eca103d94b60 339 break;
imadaemi 3:eca103d94b60 340 }
imadaemi 3:eca103d94b60 341 }
imadaemi 3:eca103d94b60 342
imadaemi 3:eca103d94b60 343 // ***************************************************
imadaemi 3:eca103d94b60 344 // フライトピン検知
imadaemi 3:eca103d94b60 345 // ***************************************************
imadaemi 3:eca103d94b60 346 void FlightPinDetect(){
imadaemi 3:eca103d94b60 347 if(mode == MODE_ONCALL){
imadaemi 3:eca103d94b60 348 mode = MODE_TO_FLIGHT;
imadaemi 3:eca103d94b60 349 flightpin_status = true;
imadaemi 3:eca103d94b60 350 /*
imadaemi 3:eca103d94b60 351 pc.printf("********************\r\n");
imadaemi 3:eca103d94b60 352 pc.printf("Set : MODE_TO_FLIGHT\r\n");
imadaemi 3:eca103d94b60 353 pc.printf("********************\r\n\r\n");
imadaemi 3:eca103d94b60 354 */
imadaemi 3:eca103d94b60 355 }
imadaemi 3:eca103d94b60 356 }
imadaemi 3:eca103d94b60 357
imadaemi 3:eca103d94b60 358 // ***************************************************
imadaemi 3:eca103d94b60 359 // 1フライトモード→セパレートモード
imadaemi 3:eca103d94b60 360 // ***************************************************
imadaemi 3:eca103d94b60 361 void ChangeModeToSep(){
imadaemi 3:eca103d94b60 362 if(mode == MODE_FLIGHT){
imadaemi 3:eca103d94b60 363 mode = MODE_TO_SEPARATE;
imadaemi 3:eca103d94b60 364 /*
imadaemi 3:eca103d94b60 365 pc.printf("********************\r\n");
imadaemi 3:eca103d94b60 366 pc.printf("Set : MODE_TO_SEPARATE\r\n");
imadaemi 3:eca103d94b60 367 pc.printf("********************\r\n\r\n");
imadaemi 3:eca103d94b60 368 */
imadaemi 3:eca103d94b60 369 }
imadaemi 3:eca103d94b60 370 }
imadaemi 3:eca103d94b60 371
imadaemi 3:eca103d94b60 372 // ***************************************************
imadaemi 3:eca103d94b60 373 // 下降モード→回収モード
imadaemi 3:eca103d94b60 374 // ***************************************************
imadaemi 3:eca103d94b60 375 void ChangeModeToCol(){
imadaemi 3:eca103d94b60 376 if(mode == MODE_DESCEND){
imadaemi 3:eca103d94b60 377 mode = MODE_TO_COLLECTION;
imadaemi 3:eca103d94b60 378 /*
imadaemi 3:eca103d94b60 379 pc.printf("********************\r\n");
imadaemi 3:eca103d94b60 380 pc.printf("Set : MODE_TO_COLLECTION\r\n");
imadaemi 3:eca103d94b60 381 pc.printf("********************\r\n\r\n");
imadaemi 3:eca103d94b60 382 */
imadaemi 3:eca103d94b60 383 }
imadaemi 3:eca103d94b60 384 }
imadaemi 3:eca103d94b60 385
imadaemi 3:eca103d94b60 386 // ***************************************************
imadaemi 0:4e38f8b1c183 387 // 分離実行
imadaemi 0:4e38f8b1c183 388 // ***************************************************
imadaemi 0:4e38f8b1c183 389 void Separate(){
imadaemi 1:3151936d9c55 390 nich.fire_on();
imadaemi 3:eca103d94b60 391 nich_led = 1;
imadaemi 3:eca103d94b60 392 //fire_time.attach(&StopSeparate,FIRE_TIME);
imadaemi 3:eca103d94b60 393 task_timeout.detach();
imadaemi 3:eca103d94b60 394 task_timeout.attach(&StopSeparate,FIRE_TIME);
imadaemi 0:4e38f8b1c183 395 }
imadaemi 0:4e38f8b1c183 396
imadaemi 0:4e38f8b1c183 397 void StopSeparate(){
imadaemi 0:4e38f8b1c183 398 nich.fire_off();
imadaemi 3:eca103d94b60 399 nich_led = 0;
imadaemi 0:4e38f8b1c183 400 }
imadaemi 0:4e38f8b1c183 401
imadaemi 0:4e38f8b1c183 402 // ***************************************************
imadaemi 0:4e38f8b1c183 403 // センサーのセットアップ
imadaemi 0:4e38f8b1c183 404 // ***************************************************
imadaemi 0:4e38f8b1c183 405 void SetSensor(){
imadaemi 0:4e38f8b1c183 406 pc.printf("\r\n");
imadaemi 0:4e38f8b1c183 407 for(int i = 0; i < 20; i++){
imadaemi 0:4e38f8b1c183 408 pc.printf("*");
imadaemi 0:4e38f8b1c183 409 }
imadaemi 0:4e38f8b1c183 410 pc.printf("\r\n");
imadaemi 0:4e38f8b1c183 411 pc.printf("Start Setting\r\n");
imadaemi 0:4e38f8b1c183 412
imadaemi 0:4e38f8b1c183 413 if(!header_set){
imadaemi 0:4e38f8b1c183 414 im920.header((char)HEADER_SETUP);
imadaemi 0:4e38f8b1c183 415 header_set = true;
imadaemi 0:4e38f8b1c183 416 }
imadaemi 0:4e38f8b1c183 417
imadaemi 3:eca103d94b60 418 //FlightPin
imadaemi 3:eca103d94b60 419 flightpin.mode(PullUp);
imadaemi 3:eca103d94b60 420
imadaemi 0:4e38f8b1c183 421 //GPS
imadaemi 0:4e38f8b1c183 422 if(gps.gps_readable == true){
imadaemi 0:4e38f8b1c183 423 pc.printf("GPS : OK!\r\n");
imadaemi 0:4e38f8b1c183 424 im920.write((char)0x01);
imadaemi 0:4e38f8b1c183 425 im_buf[im_buf_n] = '1';
imadaemi 0:4e38f8b1c183 426 im_buf_n ++;
imadaemi 0:4e38f8b1c183 427 }else{
imadaemi 0:4e38f8b1c183 428 pc.printf("GPS : NG...\r\n");
imadaemi 0:4e38f8b1c183 429 im920.write((char)0x00);
imadaemi 0:4e38f8b1c183 430 im_buf[im_buf_n] = '0';
imadaemi 0:4e38f8b1c183 431 im_buf_n ++;
imadaemi 0:4e38f8b1c183 432 }
imadaemi 0:4e38f8b1c183 433
imadaemi 0:4e38f8b1c183 434 //LPS22HB
imadaemi 0:4e38f8b1c183 435 lps22hb.begin();
imadaemi 0:4e38f8b1c183 436 if(lps22hb.test() == true){
imadaemi 0:4e38f8b1c183 437 pc.printf("LPS22HB : OK!\r\n");
imadaemi 0:4e38f8b1c183 438 im920.write((char)0x01);
imadaemi 0:4e38f8b1c183 439 im_buf[im_buf_n] = '1';
imadaemi 0:4e38f8b1c183 440 im_buf_n ++;
imadaemi 0:4e38f8b1c183 441 }else{
imadaemi 0:4e38f8b1c183 442 pc.printf("LPS22HB : NG...\r\n");
imadaemi 0:4e38f8b1c183 443 im920.write((char)0x00);
imadaemi 0:4e38f8b1c183 444 im_buf[im_buf_n] = '0';
imadaemi 0:4e38f8b1c183 445 im_buf_n ++;
imadaemi 0:4e38f8b1c183 446 }
imadaemi 0:4e38f8b1c183 447
imadaemi 0:4e38f8b1c183 448 //MPU9250
imadaemi 0:4e38f8b1c183 449 if(mpu9250.sensorTest() == true){
imadaemi 0:4e38f8b1c183 450 pc.printf("MPU9250 : OK!\r\n");
imadaemi 0:4e38f8b1c183 451 im920.write((char)0x01);
imadaemi 0:4e38f8b1c183 452 im_buf[im_buf_n] = '1';
imadaemi 0:4e38f8b1c183 453 im_buf_n ++;
imadaemi 0:4e38f8b1c183 454 }else{
imadaemi 0:4e38f8b1c183 455 pc.printf("MPU9250 : NG...\r\n");
imadaemi 0:4e38f8b1c183 456 im920.write((char)0x00);
imadaemi 0:4e38f8b1c183 457 im_buf[im_buf_n] = '0';
imadaemi 0:4e38f8b1c183 458 im_buf_n ++;
imadaemi 0:4e38f8b1c183 459 }
imadaemi 0:4e38f8b1c183 460 if(mpu9250.mag_sensorTest() == true){
imadaemi 0:4e38f8b1c183 461 pc.printf("MPU9250 MAG : OK!\r\n");
imadaemi 0:4e38f8b1c183 462 im920.write((char)0x01);
imadaemi 0:4e38f8b1c183 463 im_buf[im_buf_n] = '1';
imadaemi 0:4e38f8b1c183 464 im_buf_n ++;
imadaemi 0:4e38f8b1c183 465 }else{
imadaemi 0:4e38f8b1c183 466 pc.printf("MPU9250 MAG : NG...\r\n");
imadaemi 0:4e38f8b1c183 467 im920.write((char)0x00);
imadaemi 0:4e38f8b1c183 468 im_buf[im_buf_n] = '0';
imadaemi 0:4e38f8b1c183 469 im_buf_n ++;
imadaemi 0:4e38f8b1c183 470 }
imadaemi 0:4e38f8b1c183 471 mpu9250.setAcc(ACC_RANGE);
imadaemi 0:4e38f8b1c183 472 mpu9250.setGyro(GYRO_RANGE);
imadaemi 0:4e38f8b1c183 473 mpu9250.setOffset(0.528892327, -0.660206211, 0.757105891, -0.011691362, 0.025688783, 1.087885322, -159.750004, 121.425005, -392.700012);
imadaemi 0:4e38f8b1c183 474
imadaemi 0:4e38f8b1c183 475 //INA226
imadaemi 0:4e38f8b1c183 476 ina226_main.set_callibretion();
imadaemi 0:4e38f8b1c183 477 ina226_sep.set_callibretion();
imadaemi 1:3151936d9c55 478 //ina226_main.setup(1);
imadaemi 1:3151936d9c55 479 //ina226_sep.setup(1);
imadaemi 0:4e38f8b1c183 480
imadaemi 0:4e38f8b1c183 481 if(ina226_main.Connection_check()==0){
imadaemi 0:4e38f8b1c183 482 pc.printf("INA226 Main : OK!\r\n");
imadaemi 0:4e38f8b1c183 483 im920.write((char)0x01);
imadaemi 0:4e38f8b1c183 484 im_buf[im_buf_n] = '1';
imadaemi 0:4e38f8b1c183 485 im_buf_n ++;
imadaemi 0:4e38f8b1c183 486 }else{
imadaemi 0:4e38f8b1c183 487 pc.printf("INA226 Main : NG...\r\n");
imadaemi 0:4e38f8b1c183 488 im920.write((char)0x00);
imadaemi 0:4e38f8b1c183 489 im_buf[im_buf_n] = '0';
imadaemi 0:4e38f8b1c183 490 im_buf_n ++;
imadaemi 0:4e38f8b1c183 491 }
imadaemi 0:4e38f8b1c183 492 if(ina226_sep.Connection_check()==0){
imadaemi 0:4e38f8b1c183 493 pc.printf("INA226 Sep : OK!\r\n");
imadaemi 0:4e38f8b1c183 494 im920.write((char)0x01);
imadaemi 0:4e38f8b1c183 495 im_buf[im_buf_n] = '1';
imadaemi 0:4e38f8b1c183 496 im_buf_n ++;
imadaemi 0:4e38f8b1c183 497 }else{
imadaemi 0:4e38f8b1c183 498 pc.printf("INA226 Sep : NG...\r\n");
imadaemi 0:4e38f8b1c183 499 im920.write((char)0x00);
imadaemi 0:4e38f8b1c183 500 im_buf[im_buf_n] = '0';
imadaemi 0:4e38f8b1c183 501 im_buf_n ++;
imadaemi 0:4e38f8b1c183 502 }
imadaemi 0:4e38f8b1c183 503
imadaemi 0:4e38f8b1c183 504 if(header_set){
imadaemi 0:4e38f8b1c183 505 im920.send();
imadaemi 0:4e38f8b1c183 506 pc.printf("Send : %s\r\n", im_buf);
imadaemi 0:4e38f8b1c183 507 header_set = false;
imadaemi 0:4e38f8b1c183 508 for(int i = 0; i < im_buf_n; i ++){
imadaemi 0:4e38f8b1c183 509 im_buf[i] = '\0';
imadaemi 0:4e38f8b1c183 510 }
imadaemi 0:4e38f8b1c183 511 im_buf_n = 0;
imadaemi 0:4e38f8b1c183 512 }
imadaemi 0:4e38f8b1c183 513
imadaemi 3:eca103d94b60 514 //EEPROM
imadaemi 3:eca103d94b60 515 eeprom_group_counter = 0;
imadaemi 3:eca103d94b60 516 setEEPROMGroup(eeprom_group_counter);
imadaemi 3:eca103d94b60 517 EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
imadaemi 3:eca103d94b60 518
imadaemi 0:4e38f8b1c183 519 pc.printf("\r\n");
imadaemi 0:4e38f8b1c183 520 for(int i = 0; i < 20; i++){
imadaemi 0:4e38f8b1c183 521 pc.printf("*");
imadaemi 0:4e38f8b1c183 522 }
imadaemi 0:4e38f8b1c183 523 pc.printf("\r\n");
imadaemi 0:4e38f8b1c183 524 }
imadaemi 0:4e38f8b1c183 525
imadaemi 0:4e38f8b1c183 526 // ***************************************************
imadaemi 0:4e38f8b1c183 527 // センサーのデータ取得
imadaemi 0:4e38f8b1c183 528 // ***************************************************
imadaemi 0:4e38f8b1c183 529 void GetData(){
imadaemi 3:eca103d94b60 530
imadaemi 3:eca103d94b60 531 //Main_Time
imadaemi 3:eca103d94b60 532 previous_main_time = main_time;
imadaemi 3:eca103d94b60 533 main_time = main_timer.read_ms();
imadaemi 3:eca103d94b60 534 if(main_time >= RESET_TIME){
imadaemi 3:eca103d94b60 535 main_timer.reset();
imadaemi 3:eca103d94b60 536 time_reset_counter++;
imadaemi 3:eca103d94b60 537 }
imadaemi 3:eca103d94b60 538
imadaemi 1:3151936d9c55 539 //Nichrome
imadaemi 3:eca103d94b60 540 nich_status = nich.status;
imadaemi 2:980edad0eea2 541
imadaemi 2:980edad0eea2 542 //GPS
imadaemi 2:980edad0eea2 543 lat = gps.Latitude();
imadaemi 2:980edad0eea2 544 lon = gps.Longitude();
imadaemi 2:980edad0eea2 545 height = gps.Height();
imadaemi 3:eca103d94b60 546 satellite_number = gps.Number();
imadaemi 2:980edad0eea2 547 //pc.printf("%f\t%f\t%f\t\r\n", lat, lon, height);
imadaemi 2:980edad0eea2 548
imadaemi 2:980edad0eea2 549 //LPS22HB
imadaemi 3:eca103d94b60 550 float press_tmp;
imadaemi 3:eca103d94b60 551 //lps22hb.read_press(&press);
imadaemi 3:eca103d94b60 552 lps22hb.read_press(&press_tmp);
imadaemi 3:eca103d94b60 553
imadaemi 3:eca103d94b60 554 if(press_tmp > 500.0){
imadaemi 3:eca103d94b60 555 press = press_tmp;
imadaemi 3:eca103d94b60 556 }
imadaemi 3:eca103d94b60 557
imadaemi 3:eca103d94b60 558 if(ground_press == -1.0){
imadaemi 3:eca103d94b60 559 ground_press = press;
imadaemi 3:eca103d94b60 560 }
imadaemi 3:eca103d94b60 561
imadaemi 2:980edad0eea2 562 lps22hb.read_temp(&temp);
imadaemi 3:eca103d94b60 563
imadaemi 3:eca103d94b60 564 if(ground_temp == -1.0){
imadaemi 3:eca103d94b60 565 ground_temp = temp;
imadaemi 3:eca103d94b60 566 }
imadaemi 3:eca103d94b60 567
imadaemi 3:eca103d94b60 568 altitude = (ground_temp + 273.15)/0.0065*(1 - powf(press/ground_press, 287 * 0.0065/9.80665));
imadaemi 2:980edad0eea2 569 //pc.printf("%f\t%f\t%f\r\n",press, temp, altitude);
imadaemi 2:980edad0eea2 570
imadaemi 3:eca103d94b60 571 //lpf_altitude
imadaemi 3:eca103d94b60 572 previous_lpf_altitude = lpf_altitude;
imadaemi 3:eca103d94b60 573 lpf_altitude += LPF_COEFFICIENT_ALT*(altitude - previous_lpf_altitude);
imadaemi 3:eca103d94b60 574
imadaemi 3:eca103d94b60 575 //pc.printf("%f\t%f\t\r\n",altitude,lpf_altitude);
imadaemi 3:eca103d94b60 576
imadaemi 3:eca103d94b60 577 //velocity計算
imadaemi 3:eca103d94b60 578 velocity = (lpf_altitude - previous_lpf_altitude)*1000/(main_time - previous_main_time);
imadaemi 3:eca103d94b60 579 //pc.printf("********************\r\n");
imadaemi 3:eca103d94b60 580 //pc.printf("lpf altitude : %f\r\n",lpf_altitude);
imadaemi 3:eca103d94b60 581 //pc.printf("main time : %f\r\n",main_time/1000);
imadaemi 3:eca103d94b60 582 //pc.printf("altitude def: %f\r\n",lpf_altitude - previous_lpf_altitude);
imadaemi 3:eca103d94b60 583 //pc.printf("velocity : %f\r\n",velocity);
imadaemi 3:eca103d94b60 584 //pc.printf("time def: %f\r\n",main_time - previous_main_time);
imadaemi 3:eca103d94b60 585 //pc.printf("********************\r\n");
imadaemi 3:eca103d94b60 586
imadaemi 3:eca103d94b60 587 //lpf_velocity
imadaemi 3:eca103d94b60 588 previous_lpf_velocity = lpf_velocity;
imadaemi 3:eca103d94b60 589 lpf_velocity += LPF_COEFFICIENT_VEL*(velocity - previous_lpf_velocity);
imadaemi 3:eca103d94b60 590 //pc.printf("%f\t%f\t%f\t%f\r\n",altitude,lpf_altitude,velocity,lpf_velocity);
imadaemi 3:eca103d94b60 591
imadaemi 2:980edad0eea2 592 //MPU9250
imadaemi 2:980edad0eea2 593 mpu9250.getAll(imu, mag);
imadaemi 3:eca103d94b60 594 //pc.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]);
imadaemi 2:980edad0eea2 595
imadaemi 2:980edad0eea2 596 //INA226
imadaemi 2:980edad0eea2 597 ina226_main.get_Voltage_current(&voltage_main, &current_main);
imadaemi 2:980edad0eea2 598 ina226_sep.get_Voltage_current(&voltage_sep, &current_sep);
imadaemi 2:980edad0eea2 599 //pc.printf("MainVol : %.2f, SepVol : %.2f, MainCur : %.2f, SepCur : %.2f\r\n", voltage_main, voltage_sep, current_main, current_sep);
imadaemi 2:980edad0eea2 600 //pc.printf("MainVol : %f, SepVol : %f, MainCur : %f, SepCur : %f\r\n", voltage_main*(1/1000), voltage_sep, current_main, current_sep);
imadaemi 2:980edad0eea2 601 }
imadaemi 2:980edad0eea2 602
imadaemi 2:980edad0eea2 603 // ***************************************************
imadaemi 2:980edad0eea2 604 // 取得データを地上に送信
imadaemi 2:980edad0eea2 605 // ***************************************************
imadaemi 2:980edad0eea2 606 void SendData(){
imadaemi 2:980edad0eea2 607 if(!header_set){
imadaemi 2:980edad0eea2 608 im920.header((char)HEADER_DATA);
imadaemi 2:980edad0eea2 609 header_set = true;
imadaemi 2:980edad0eea2 610 }
imadaemi 2:980edad0eea2 611
imadaemi 3:eca103d94b60 612 ///pc.printf("********************\r\n");
imadaemi 3:eca103d94b60 613
imadaemi 3:eca103d94b60 614 //time_reset_counter
imadaemi 3:eca103d94b60 615 im920.write((short)time_reset_counter);
imadaemi 3:eca103d94b60 616 im_buf_n += 2;
imadaemi 3:eca103d94b60 617 //MainTime
imadaemi 3:eca103d94b60 618 im920.write((float)main_time);
imadaemi 3:eca103d94b60 619 im_buf_n += 4;
imadaemi 3:eca103d94b60 620
imadaemi 3:eca103d94b60 621 //mode
imadaemi 3:eca103d94b60 622 ///pc.printf("mode : %c\r\n",mode);
imadaemi 3:eca103d94b60 623 im920.write((char)mode);
imadaemi 3:eca103d94b60 624 im_buf_n ++;
imadaemi 3:eca103d94b60 625
imadaemi 3:eca103d94b60 626 //flightpin
imadaemi 3:eca103d94b60 627 bitshift_flightpin_status = flightpin_status << 0;
imadaemi 3:eca103d94b60 628 //pc.printf("flightpin bool : %d\r\n",flightpin_status);
imadaemi 3:eca103d94b60 629 //pc.printf("flightpin bool : %d\r\n",bitshift_flightpin_status);
imadaemi 2:980edad0eea2 630
imadaemi 2:980edad0eea2 631 //Nichrome
imadaemi 3:eca103d94b60 632 bitshift_nich_status = nich_status << 1;
imadaemi 3:eca103d94b60 633 //pc.printf("nich bool : %d\r\n",nich_status);
imadaemi 3:eca103d94b60 634 //pc.printf("nich bitshift : %d\r\n",bitshift_nich_status);
imadaemi 3:eca103d94b60 635
imadaemi 3:eca103d94b60 636 //頂点検知
imadaemi 3:eca103d94b60 637 bitshift_top_detect = top_detect << 2;
imadaemi 3:eca103d94b60 638
imadaemi 3:eca103d94b60 639 //保存データのステータス
imadaemi 3:eca103d94b60 640 bitshift_save_data_status = save_data_status << 3;
imadaemi 3:eca103d94b60 641
imadaemi 3:eca103d94b60 642 //bool8個を1つのcharで送るとき
imadaemi 3:eca103d94b60 643 bitshift_sum = bitshift_flightpin_status | bitshift_nich_status | bitshift_top_detect | bitshift_save_data_status;
imadaemi 3:eca103d94b60 644 im920.write((char)bitshift_sum);
imadaemi 3:eca103d94b60 645 im_buf_n ++;
imadaemi 3:eca103d94b60 646
imadaemi 3:eca103d94b60 647 //EEPROM Number
imadaemi 3:eca103d94b60 648 im920.write((char)eeprom_number);
imadaemi 1:3151936d9c55 649 im_buf_n ++;
imadaemi 1:3151936d9c55 650
imadaemi 0:4e38f8b1c183 651 //GPS
imadaemi 3:eca103d94b60 652 ///pc.printf("Latitude : %f\r\n",lat);
imadaemi 3:eca103d94b60 653 ///pc.printf("Longitude : %f\r\n",lon);
imadaemi 3:eca103d94b60 654 ///pc.printf("Height : %f\r\n",height);
imadaemi 0:4e38f8b1c183 655 im920.write((float)lat);
imadaemi 3:eca103d94b60 656 im_buf_n += 4;
imadaemi 0:4e38f8b1c183 657 im920.write((float)lon);
imadaemi 3:eca103d94b60 658 im_buf_n += 4;
imadaemi 3:eca103d94b60 659 /*
imadaemi 0:4e38f8b1c183 660 im920.write((float)height);
imadaemi 3:eca103d94b60 661 im_buf_n += 4;
imadaemi 3:eca103d94b60 662 */
imadaemi 0:4e38f8b1c183 663
imadaemi 0:4e38f8b1c183 664 //LPS22HB
imadaemi 3:eca103d94b60 665 ///pc.printf("Pressure : %f\r\n",press);
imadaemi 3:eca103d94b60 666 ///pc.printf("Temperarure : %f\r\n",temp);
imadaemi 3:eca103d94b60 667 ///pc.printf("Altitude : %f\r\n",altitude);
imadaemi 0:4e38f8b1c183 668 im920.write((float)press);
imadaemi 3:eca103d94b60 669 im_buf_n += 4;
imadaemi 3:eca103d94b60 670 im920.write((short)(temp*TEMP_MULTIPLIER));
imadaemi 3:eca103d94b60 671 im_buf_n += 2;
imadaemi 3:eca103d94b60 672 im920.write((short)(lpf_altitude*LPF_ALT_MULTIPLIER));
imadaemi 3:eca103d94b60 673 im_buf_n += 2;
imadaemi 0:4e38f8b1c183 674
imadaemi 3:eca103d94b60 675 im920.write((short)(lpf_velocity*LPF_VEL_MULTIPLIER));
imadaemi 3:eca103d94b60 676 im_buf_n += 2;
imadaemi 2:980edad0eea2 677
imadaemi 0:4e38f8b1c183 678 //INA226
imadaemi 3:eca103d94b60 679 ///pc.printf("Vlotage Mian : %.2f\r\n",voltage_main);
imadaemi 3:eca103d94b60 680 ///pc.printf("Current Main : %.2f\r\n",current_main);
imadaemi 3:eca103d94b60 681 ///pc.printf("Voltage Sep : %.2f\r\n",voltage_sep);
imadaemi 3:eca103d94b60 682 ///pc.printf("Current Sep : %.2f\r\n",current_sep);
imadaemi 3:eca103d94b60 683 im920.write((short)voltage_main);
imadaemi 3:eca103d94b60 684 im_buf_n += 2;
imadaemi 3:eca103d94b60 685 im920.write((short)current_main);
imadaemi 3:eca103d94b60 686 im_buf_n += 2;
imadaemi 3:eca103d94b60 687 im920.write((short)voltage_sep);
imadaemi 3:eca103d94b60 688 im_buf_n += 2;
imadaemi 3:eca103d94b60 689 im920.write((short)current_sep);
imadaemi 3:eca103d94b60 690 im_buf_n += 2;
imadaemi 1:3151936d9c55 691
imadaemi 3:eca103d94b60 692 ///pc.printf("********************\r\n\r\n");
imadaemi 1:3151936d9c55 693
imadaemi 0:4e38f8b1c183 694 if(header_set){
imadaemi 0:4e38f8b1c183 695 im920.send();
imadaemi 2:980edad0eea2 696 //pc.printf("Send : %s\r\n", im_buf);
imadaemi 0:4e38f8b1c183 697 header_set = false;
imadaemi 0:4e38f8b1c183 698 for(int i = 0; i < im_buf_n; i ++){
imadaemi 0:4e38f8b1c183 699 im_buf[i] = '\0';
imadaemi 0:4e38f8b1c183 700 }
imadaemi 0:4e38f8b1c183 701 im_buf_n = 0;
imadaemi 0:4e38f8b1c183 702 }
imadaemi 0:4e38f8b1c183 703 }
imadaemi 0:4e38f8b1c183 704
imadaemi 3:eca103d94b60 705 // ***************************************************
imadaemi 3:eca103d94b60 706 // 打上げ時刻を送信
imadaemi 3:eca103d94b60 707 // ***************************************************
imadaemi 3:eca103d94b60 708 void SendLaunchTime(){
imadaemi 3:eca103d94b60 709 if(!header_set){
imadaemi 3:eca103d94b60 710 im920.header((char)HEADER_LAUNCH_TIME);
imadaemi 3:eca103d94b60 711 header_set = true;
imadaemi 3:eca103d94b60 712 }
imadaemi 3:eca103d94b60 713 pc.printf("launch time : %f\r\n",launch_time);
imadaemi 3:eca103d94b60 714 im920.write((float)launch_time);
imadaemi 3:eca103d94b60 715 im_buf_n += 4;
imadaemi 3:eca103d94b60 716 im920.write((short)time_reset_counter);
imadaemi 3:eca103d94b60 717 im_buf_n += 2;
imadaemi 3:eca103d94b60 718 send_launch_time_status = true;
imadaemi 3:eca103d94b60 719 if(header_set){
imadaemi 3:eca103d94b60 720 im920.send();
imadaemi 3:eca103d94b60 721 pc.printf("Send launch time\r\n");
imadaemi 3:eca103d94b60 722 header_set = false;
imadaemi 3:eca103d94b60 723 for(int i = 0; i < im_buf_n; i ++){
imadaemi 3:eca103d94b60 724 im_buf[i] = '\0';
imadaemi 3:eca103d94b60 725 }
imadaemi 3:eca103d94b60 726 im_buf_n = 0;
imadaemi 3:eca103d94b60 727 }
imadaemi 3:eca103d94b60 728 }
imadaemi 2:980edad0eea2 729
imadaemi 0:4e38f8b1c183 730 // ***************************************************
imadaemi 3:eca103d94b60 731 // EEPROMにデータを書き込む
imadaemi 0:4e38f8b1c183 732 // ***************************************************
imadaemi 3:eca103d94b60 733 void SaveData(){
imadaemi 3:eca103d94b60 734 if(eeprom_group_counter < 4){
imadaemi 3:eca103d94b60 735 //pc.printf("Save to EEPROM\r\n");
imadaemi 3:eca103d94b60 736 int eep_buf = 0;
imadaemi 0:4e38f8b1c183 737
imadaemi 3:eca103d94b60 738 /*
imadaemi 3:eca103d94b60 739 for(int i = eep_buf; i < 128; i++){
imadaemi 3:eca103d94b60 740 //ptr = EEPROM.chargeBuff((char)0x02);
imadaemi 0:4e38f8b1c183 741 ptr = EEPROM.chargeBuff((int)n++);
imadaemi 3:eca103d94b60 742 }
imadaemi 3:eca103d94b60 743 */
imadaemi 3:eca103d94b60 744 ptr = EEPROM.chargeBuff((char)time_reset_counter);
imadaemi 3:eca103d94b60 745 eep_buf += 1;
imadaemi 3:eca103d94b60 746 ptr = EEPROM.chargeBuff((int)main_time);
imadaemi 3:eca103d94b60 747 eep_buf += 4;
imadaemi 3:eca103d94b60 748 //ptr = EEPROM.chargeBuff((float)flight_time);
imadaemi 3:eca103d94b60 749 //eep_buf += 4;
imadaemi 3:eca103d94b60 750
imadaemi 3:eca103d94b60 751 ptr = EEPROM.chargeBuff((float)lat);
imadaemi 3:eca103d94b60 752 eep_buf += 4;
imadaemi 3:eca103d94b60 753 ptr = EEPROM.chargeBuff((float)lon);
imadaemi 3:eca103d94b60 754 eep_buf += 4;
imadaemi 3:eca103d94b60 755 ptr = EEPROM.chargeBuff((float)height);
imadaemi 3:eca103d94b60 756 eep_buf += 4;
imadaemi 3:eca103d94b60 757 ptr = EEPROM.chargeBuff((char)satellite_number);
imadaemi 3:eca103d94b60 758 eep_buf += 1;
imadaemi 3:eca103d94b60 759
imadaemi 3:eca103d94b60 760 ptr = EEPROM.chargeBuff((float)press);
imadaemi 3:eca103d94b60 761 eep_buf += 4;
imadaemi 3:eca103d94b60 762 ptr = EEPROM.chargeBuff((float)temp);
imadaemi 3:eca103d94b60 763 eep_buf += 4;
imadaemi 3:eca103d94b60 764 ptr = EEPROM.chargeBuff((float)altitude);
imadaemi 3:eca103d94b60 765 eep_buf += 4;
imadaemi 3:eca103d94b60 766 ptr = EEPROM.chargeBuff((float)lpf_altitude);
imadaemi 3:eca103d94b60 767 eep_buf += 4;
imadaemi 3:eca103d94b60 768
imadaemi 3:eca103d94b60 769 ptr = EEPROM.chargeBuff((float)velocity);
imadaemi 3:eca103d94b60 770 eep_buf += 4;
imadaemi 3:eca103d94b60 771 ptr = EEPROM.chargeBuff((float)lpf_velocity);
imadaemi 3:eca103d94b60 772 eep_buf += 4;
imadaemi 3:eca103d94b60 773
imadaemi 3:eca103d94b60 774 ptr = EEPROM.chargeBuff((float)imu[0]);
imadaemi 3:eca103d94b60 775 eep_buf += 4;
imadaemi 3:eca103d94b60 776 ptr = EEPROM.chargeBuff((float)imu[1]);
imadaemi 3:eca103d94b60 777 eep_buf += 4;
imadaemi 3:eca103d94b60 778 ptr = EEPROM.chargeBuff((float)imu[2]);
imadaemi 3:eca103d94b60 779 eep_buf += 4;
imadaemi 3:eca103d94b60 780 ptr = EEPROM.chargeBuff((float)imu[3]);
imadaemi 3:eca103d94b60 781 eep_buf += 4;
imadaemi 3:eca103d94b60 782 ptr = EEPROM.chargeBuff((float)imu[4]);
imadaemi 3:eca103d94b60 783 eep_buf += 4;
imadaemi 3:eca103d94b60 784 ptr = EEPROM.chargeBuff((float)imu[5]);
imadaemi 3:eca103d94b60 785 eep_buf += 4;
imadaemi 3:eca103d94b60 786
imadaemi 3:eca103d94b60 787 ptr = EEPROM.chargeBuff((float)mag[0]);
imadaemi 3:eca103d94b60 788 eep_buf += 4;
imadaemi 3:eca103d94b60 789 ptr = EEPROM.chargeBuff((float)mag[1]);
imadaemi 3:eca103d94b60 790 eep_buf += 4;
imadaemi 3:eca103d94b60 791 ptr = EEPROM.chargeBuff((float)mag[2]);
imadaemi 3:eca103d94b60 792 eep_buf += 4;
imadaemi 3:eca103d94b60 793
imadaemi 3:eca103d94b60 794 ptr = EEPROM.chargeBuff((float)(voltage_main/1000));
imadaemi 3:eca103d94b60 795 eep_buf += 4;
imadaemi 3:eca103d94b60 796 ptr = EEPROM.chargeBuff((float)(current_main/1000));
imadaemi 3:eca103d94b60 797 eep_buf += 4;
imadaemi 3:eca103d94b60 798 ptr = EEPROM.chargeBuff((float)(voltage_sep/1000));
imadaemi 3:eca103d94b60 799 eep_buf += 4;
imadaemi 3:eca103d94b60 800 ptr = EEPROM.chargeBuff((float)(current_sep/1000));
imadaemi 3:eca103d94b60 801 eep_buf += 4;
imadaemi 3:eca103d94b60 802
imadaemi 3:eca103d94b60 803 ptr = EEPROM.chargeBuff((char)mode);
imadaemi 3:eca103d94b60 804 eep_buf ++;
imadaemi 3:eca103d94b60 805 ptr = EEPROM.chargeBuff((char)flightpin_status);
imadaemi 3:eca103d94b60 806 eep_buf ++;
imadaemi 3:eca103d94b60 807 ptr = EEPROM.chargeBuff((char)nich_status);
imadaemi 3:eca103d94b60 808 eep_buf ++;
imadaemi 3:eca103d94b60 809
imadaemi 3:eca103d94b60 810 //ptr = EEPROM.chargeBuff((int)n++);
imadaemi 3:eca103d94b60 811 //ptr = EEPROM.chargeBuff((char)0xff);
imadaemi 3:eca103d94b60 812 //ptr = EEPROM.chargeBuff((int)0);
imadaemi 3:eca103d94b60 813
imadaemi 3:eca103d94b60 814 //pc.printf("%d\r\n",eep_buf);
imadaemi 3:eca103d94b60 815
imadaemi 3:eca103d94b60 816 for(int i = eep_buf; i < 128; i++){
imadaemi 3:eca103d94b60 817 ptr = EEPROM.chargeBuff((char)0x00);
imadaemi 3:eca103d94b60 818 }
imadaemi 3:eca103d94b60 819
imadaemi 3:eca103d94b60 820 //pc.printf("ptr : %d\r\n",ptr);
imadaemi 3:eca103d94b60 821 EEPROM.writeBuff();
imadaemi 3:eca103d94b60 822 eeprom_ptr = EEPROM.setNextPage();
imadaemi 3:eca103d94b60 823 if(eeprom_ptr == 0x01000000 || eeprom_ptr == 0x02000000 || eeprom_ptr == 0x03000000 || eeprom_ptr == 0x04000000){
imadaemi 3:eca103d94b60 824 eeprom_number++;
imadaemi 3:eca103d94b60 825 pc.printf("eeprom_number : %d\r\n",eeprom_number);
imadaemi 3:eca103d94b60 826 }
imadaemi 3:eca103d94b60 827 //pc.printf("eeprom_ptr: %x\r\n", eeprom_ptr);
imadaemi 3:eca103d94b60 828
imadaemi 3:eca103d94b60 829 /*
imadaemi 3:eca103d94b60 830 if(eeprom_ptr == 0x01000000){
imadaemi 3:eca103d94b60 831 ptr = 0;
imadaemi 3:eca103d94b60 832 eeprom_ptr = 0;
imadaemi 3:eca103d94b60 833 plexer_num++;
imadaemi 3:eca103d94b60 834 setEEPROMGroup(plexer_num);
imadaemi 3:eca103d94b60 835 EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
imadaemi 3:eca103d94b60 836 }
imadaemi 3:eca103d94b60 837 */
imadaemi 3:eca103d94b60 838
imadaemi 3:eca103d94b60 839 if(eeprom_ptr == 0x04000000){
imadaemi 3:eca103d94b60 840 eeprom_ptr = 0;
imadaemi 3:eca103d94b60 841 eeprom_group_counter++;
imadaemi 3:eca103d94b60 842 setEEPROMGroup(eeprom_group_counter);
imadaemi 3:eca103d94b60 843 EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
imadaemi 3:eca103d94b60 844 pc.printf("EEPROM_Group : %d\r\n",eeprom_group_counter);
imadaemi 3:eca103d94b60 845 //pc.printf("SetWriteAddr\r\n");
imadaemi 0:4e38f8b1c183 846 }
imadaemi 0:4e38f8b1c183 847 }
imadaemi 0:4e38f8b1c183 848 }
imadaemi 0:4e38f8b1c183 849
imadaemi 0:4e38f8b1c183 850 // ***************************************************
imadaemi 0:4e38f8b1c183 851 // マルチプレクサで使うEEPROMを変更する
imadaemi 0:4e38f8b1c183 852 // ***************************************************
imadaemi 0:4e38f8b1c183 853 void setEEPROMGroup(int group_num){
imadaemi 0:4e38f8b1c183 854 switch(group_num){
imadaemi 0:4e38f8b1c183 855 case 0:
imadaemi 0:4e38f8b1c183 856 pinA = 0;
imadaemi 0:4e38f8b1c183 857 pinB = 0;
imadaemi 0:4e38f8b1c183 858 pinC = 0;
imadaemi 0:4e38f8b1c183 859 break;
imadaemi 0:4e38f8b1c183 860
imadaemi 0:4e38f8b1c183 861 case 1:
imadaemi 0:4e38f8b1c183 862 pinA = 1;
imadaemi 0:4e38f8b1c183 863 pinB = 0;
imadaemi 0:4e38f8b1c183 864 pinC = 0;
imadaemi 0:4e38f8b1c183 865 break;
imadaemi 0:4e38f8b1c183 866
imadaemi 0:4e38f8b1c183 867 case 2:
imadaemi 0:4e38f8b1c183 868 pinA = 0;
imadaemi 0:4e38f8b1c183 869 pinB = 1;
imadaemi 0:4e38f8b1c183 870 pinC = 0;
imadaemi 0:4e38f8b1c183 871 break;
imadaemi 0:4e38f8b1c183 872
imadaemi 0:4e38f8b1c183 873 case 3:
imadaemi 0:4e38f8b1c183 874 pinA = 1;
imadaemi 0:4e38f8b1c183 875 pinB = 1;
imadaemi 0:4e38f8b1c183 876 pinC = 0;
imadaemi 0:4e38f8b1c183 877 break;
imadaemi 0:4e38f8b1c183 878 }
imadaemi 0:4e38f8b1c183 879 }