Hybrid_OB2021
v1
Dependencies: Nichrome_lib mbed mpu9250_i2c IM920 INA226_lib PQLPS22HB EEPROM_lib GPS_interrupt
Diff: main.cpp
- Revision:
- 3:eca103d94b60
- Parent:
- 2:980edad0eea2
- Child:
- 4:345e55d8e8a3
--- a/main.cpp Fri Jul 30 06:03:31 2021 +0000
+++ b/main.cpp Sun Aug 08 11:25:50 2021 +0000
@@ -7,16 +7,30 @@
#include "Nichrome_lib.h"
#include "EEPROM_lib.h"
-#define FIRE_TIME 3.0
-#define CURRENT_LOCATION_PRESS 1022.624268//投下前に設定
-#define CURRENT_LOCATION_TEMP 10.650000//投下前に設定
+#define RESET_TIME 1800000 //1800000→30分
+//#define RESET_TIME 30000000 // 30000000→30秒(実験用)
+#define FIRE_TIME 2.0
+#define TO_SEPARATE_TIME 10.0
+//#define TO_SEPARATE_TIME 300.0
+#define TO_COLLECTION_TIME 30.0
+//#define TO_COLLECTION_TIME 10.0 // 実験用
+#define ALTITUDE_LIMIT 30.0
+//#define ALTITUDE_LIMIT 10.0
#define ACC_RANGE _16G
#define GYRO_RANGE _2000DPS
+//#define LPF_COEFFICIENT_ALT 0.01
+//#define LPF_COEFFICIENT_ALT 0.001
+#define LPF_COEFFICIENT_ALT 0.05
+#define LPF_COEFFICIENT_VEL 0.2
+
+#define TEMP_MULTIPLIER 100
+#define LPF_ALT_MULTIPLIER 100
+#define LPF_VEL_MULTIPLIER 100
// ***************************************************
// コンストラクタ
// ***************************************************
-Serial pc(USBTX, USBRX, 115200);
+Serial pc(USBTX, USBRX, 230400);
Serial gps_serial(p9, p10, 115200);
Serial im920_serial(p13, p14, 115200);
@@ -29,53 +43,103 @@
myINA226 ina226_main(i2c, myINA226::A1_GND, myINA226::A0_GND);
myINA226 ina226_sep(i2c, myINA226::A1_VDD, myINA226::A0_VDD);
Nichrome_lib nich(p20);
-EEPROM_lib EEPROM(i2c, 1);
+EEPROM_lib EEPROM(i2c, 4);
+InterruptIn flightpin(p18);
+DigitalOut nich_led(LED1);
+DigitalOut im920_busy_led(LED2);
DigitalOut pinA(p21);
DigitalOut pinB(p22);
DigitalOut pinC(p23);
+DigitalIn im920_busy(p15);
+
Ticker send_data;
-Timeout fire_time;
+Ticker save_data;
+Ticker get_data;
+
+Timer main_timer;
+
+//Timeout to_separate_time;
+//Timeout to_collection_time;
+//Timeout fire_time;
+Timeout task_timeout;
// ***************************************************
// 関数の宣言
// ***************************************************
void uplink();
void echoIM();
+void PerformTask();
+void FlightPinDetect();
+void ChangeModeToSep();
+void ChangeModeToCol();
void Separate();
void StopSeparate();
void SetSensor();
void GetData();
+void SendLaunchTime();
void SendData();
-void WriteEEPROM();
+void SaveData();
void setEEPROMGroup(int group_num);
// ***************************************************
// 無線のヘッダまとめ
// ***************************************************
const char HEADER_SETUP = 0x01;
-// 0xA1, GPS, LPS22HB, MPU9250, MPU9250_MAG, INA226_MAIN, INA226_SEP
+// 0x01, GPS, LPS22HB, MPU9250, MPU9250_MAG, INA226_MAIN, INA226_SEP
// 1 1 1 1 1 1 1
-const char HEADER_DATA = 0xA2;
-// 0xA2, nich_status, lat, lon, height, press, temp, altitude, voltage_main, voltage_sep, current_main, current_sep
-// 1 1 4 4 4 4 4 4 4 4 4 4
+const char HEADER_DATA = 0x02;
+// 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
+// 1 4 4 1 0.125 0.125 4 4 4 4 4 4 4 4 4 4
+const char HEADER_LAUNCH_TIME = 0x03;
-const char HEADER_ECHO = 0xA5;
+const char HEADER_ECHO = 0x05;
// 0xA5,コマンド
// 1 1
// ***************************************************
// 変数の宣言
// ***************************************************
-char nich_status;
+float launch_time = 0;
+float previous_main_time = 0;
+float flight_time = 0;
+float main_time = 0;
+int time_reset_counter = 0;
+
+bool send_launch_time_status = false;
+
+bool top_detect = false;
+char bitshift_top_detect;
+
+bool nich_status = false;
+char bitshift_nich_status;
+
+bool flightpin_status = false;
+char bitshift_flightpin_status;
+
+bool save_data_status = false;
+char bitshift_save_data_status;
+
+char bitshift_sum;
bool header_set = false;
char im_buf[55];//16なのって意味ある？
int im_buf_n = 0;
float lat, lon, height;
-float press, temp, altitude;
+int satellite_number;
+float press, temp;
+float altitude;
+//float ground_press = 1013.0,ground_temp = 25.0;
+float ground_press = -1.0,ground_temp = -1.0;
+
+float velocity;
+float previous_lpf_velocity;
+float lpf_velocity;
+
+float previous_lpf_altitude;
+float lpf_altitude;
float imu[6], mag[3];
float mag_geo[3];
@@ -83,25 +147,92 @@
float voltage_main, voltage_sep;
float current_main, current_sep;
+int ptr, n = 0;
+int eeprom_ptr = 0;
+int eeprom_group_counter = 0;
+int eeprom_number = 0;
+
+const char MODE_SETUP = 0x00;
+const char MODE_SAFE = 0x01;
+const char MODE_TO_ONCALL = 0x02;
+const char MODE_ONCALL = 0x03;
+const char MODE_TO_FLIGHT = 0x04;
+const char MODE_FLIGHT = 0x05;
+const char MODE_TO_SEPARATE = 0x06;
+const char MODE_SEPARATE = 0x07;
+const char MODE_TO_DESCEND = 0x08;
+const char MODE_DESCEND = 0x09;
+const char MODE_TO_COLLECTION = 0x0a;
+const char MODE_COLLECTION = 0x0b;
+
+char mode = MODE_SETUP;
+
// ***************************************************
// メイン関数
// ***************************************************
int main() {
pc.printf("Hello Main!\r\n");
- SetSensor();
im920.attach(&uplink, 0xF0);
send_data.attach(&SendData, 1.0);
+ get_data.attach(&GetData, 0.02);
while(1){
- GetData();
+ PerformTask();
+ /*
+ if(im920_busy){
+ im920_busy_led = 1;
+ }else{
+ im920_busy_led = 0;
+ }
+ */
}
}
+
// ***************************************************
// アップリンク（地上局から）
// ***************************************************
void uplink(){
echoIM();
switch(im920.data[1]){
+
+ case 'U':
+ if(mode == MODE_FLIGHT || mode == MODE_ONCALL){
+ mode = MODE_SETUP;
+ save_data.detach();
+ save_data_status = false;
+ pc.printf("********************\r\n");
+ pc.printf("Set : MODE_SETUP\r\n");
+ pc.printf("********************\r\n\r\n");
+ }
+ break;
+
+ case 'O':
+ if(mode == MODE_SAFE){
+ mode = MODE_TO_ONCALL;
+ pc.printf("********************\r\n");
+ pc.printf("Set : MODE_TO_ONCALL\r\n");
+ pc.printf("********************\r\n\r\n");
+ }
+ break;
+
+ case 'F':
+ if(mode == MODE_ONCALL){
+ mode = MODE_TO_FLIGHT;
+ pc.printf("********************\r\n");
+ pc.printf("Set : MODE_TO_FLIGHT\r\n");
+ pc.printf("********************\r\n\r\n");
+ }
+ break;
+
+ case 'S':
+ if(mode == MODE_FLIGHT){
+ mode = MODE_TO_SEPARATE;
+ pc.printf("********************\r\n");
+ pc.printf("Set : MODE_TO_SEPARATE\r\n");
+ pc.printf("********************\r\n\r\n");
+ }
+ break;
+
case 0x01:
pc.printf("********************\r\n");
pc.printf("SEPARATE\r\n");
@@ -128,15 +259,144 @@
}
// ***************************************************
+// タスクの実行
+// ***************************************************
+void PerformTask(){
+ switch(mode){
+
+ case(MODE_SETUP):
+ SetSensor();
+ main_timer.start();
+ send_data.detach();
+ send_data.attach(&SendData, 1.0);
+ mode = MODE_SAFE;
+ break;
+
+ case(MODE_SAFE):
+ break;
+
+ case(MODE_TO_ONCALL):
+ ground_press = press;
+ save_data.detach();
+ save_data.attach(&SaveData, 1.0);
+ save_data_status = true;
+ flightpin.rise(&FlightPinDetect);
+ mode = MODE_ONCALL;
+ break;
+
+ case(MODE_ONCALL):
+ break;
+
+ case(MODE_TO_FLIGHT):
+ save_data.detach();
+ save_data.attach(&SaveData, 0.02);
+ launch_time = main_time;
+ //to_separate_time.attach(&ChangeModeToSep,TO_SEPARATE_TIME);
+ task_timeout.detach();
+ task_timeout.attach(&ChangeModeToSep,TO_SEPARATE_TIME);
+ mode = MODE_FLIGHT;
+ break;
+
+ case(MODE_FLIGHT):
+ //pc.printf("%f\t%f\t%f\t%f\t%f\r\n",press,altitude,lpf_altitude, ALTITUDE_LIMIT,velocity);
+ if(!im920_busy && !send_launch_time_status){
+ SendLaunchTime();
+ }
+ if((lpf_altitude >= ALTITUDE_LIMIT) && (lpf_velocity < 0.0)){//and 速度＜0を書き込む
+ mode = MODE_TO_SEPARATE;
+ top_detect = true;
+ }
+ break;
+
+ case(MODE_TO_SEPARATE):
+ Separate();
+ mode = MODE_SEPARATE;
+ break;
+
+ case(MODE_SEPARATE):
+ if(nich.status == false){
+ mode = MODE_TO_DESCEND;
+ }
+ break;
+
+ case(MODE_TO_DESCEND):
+ //to_collection_time.attach(&ChangeModeToCol,TO_COLLECTION_TIME);
+ task_timeout.detach();
+ task_timeout.attach(&ChangeModeToCol,TO_COLLECTION_TIME);
+ mode = MODE_DESCEND;
+ break;
+
+ case(MODE_DESCEND):
+ break;
+
+ case(MODE_TO_COLLECTION):
+ save_data.detach();
+ save_data.attach(&SaveData, 1.0);
+ mode = MODE_COLLECTION;
+ break;
+
+ case(MODE_COLLECTION):
+ break;
+ }
+}
+
+// ***************************************************
+// フライトピン検知
+// ***************************************************
+void FlightPinDetect(){
+ if(mode == MODE_ONCALL){
+ mode = MODE_TO_FLIGHT;
+ flightpin_status = true;
+ /*
+ pc.printf("********************\r\n");
+ pc.printf("Set : MODE_TO_FLIGHT\r\n");
+ pc.printf("********************\r\n\r\n");
+ */
+ }
+}
+
+// ***************************************************
+// 1フライトモード→セパレートモード
+// ***************************************************
+void ChangeModeToSep(){
+ if(mode == MODE_FLIGHT){
+ mode = MODE_TO_SEPARATE;
+ /*
+ pc.printf("********************\r\n");
+ pc.printf("Set : MODE_TO_SEPARATE\r\n");
+ pc.printf("********************\r\n\r\n");
+ */
+ }
+}
+
+// ***************************************************
+// 下降モード→回収モード
+// ***************************************************
+void ChangeModeToCol(){
+ if(mode == MODE_DESCEND){
+ mode = MODE_TO_COLLECTION;
+ /*
+ pc.printf("********************\r\n");
+ pc.printf("Set : MODE_TO_COLLECTION\r\n");
+ pc.printf("********************\r\n\r\n");
+ */
+ }
+}
+
+// ***************************************************
// 分離実行
// ***************************************************
void Separate(){
nich.fire_on();
- fire_time.attach(&StopSeparate,FIRE_TIME);
+ nich_led = 1;
+ //fire_time.attach(&StopSeparate,FIRE_TIME);
+ task_timeout.detach();
+ task_timeout.attach(&StopSeparate,FIRE_TIME);
}
void StopSeparate(){
nich.fire_off();
+ nich_led = 0;
}
// ***************************************************
@@ -155,6 +415,9 @@
header_set = true;
}
+ //FlightPin
+ flightpin.mode(PullUp);
+
//GPS
if(gps.gps_readable == true){
pc.printf("GPS : OK!\r\n");
@@ -248,6 +511,11 @@
im_buf_n = 0;
}
+ //EEPROM
+ eeprom_group_counter = 0;
+ setEEPROMGroup(eeprom_group_counter);
+ EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
+
pc.printf("\r\n");
for(int i = 0; i < 20; i++){
pc.printf("*");
@@ -259,38 +527,75 @@
// センサーのデータ取得
// ***************************************************
void GetData(){
+
+ //Main_Time
+ previous_main_time = main_time;
+ main_time = main_timer.read_ms();
+ if(main_time >= RESET_TIME){
+ main_timer.reset();
+ time_reset_counter++;
+ }
+
//Nichrome
- if(nich.status){
- nich_status = '1';
- }else{
- nich_status = '0';
- }
+ nich_status = nich.status;
//GPS
lat = gps.Latitude();
lon = gps.Longitude();
height = gps.Height();
+ satellite_number = gps.Number();
//pc.printf("%f\t%f\t%f\t\r\n", lat, lon, height);
//LPS22HB
- lps22hb.read_press(&press);
+ float press_tmp;
+ //lps22hb.read_press(&press);
+ lps22hb.read_press(&press_tmp);
+
+ if(press_tmp > 500.0){
+ press = press_tmp;
+ }
+
+ if(ground_press == -1.0){
+ ground_press = press;
+ }
+
lps22hb.read_temp(&temp);
- altitude = (CURRENT_LOCATION_TEMP + 273.15)/0.0065*(1 - powf(press/CURRENT_LOCATION_PRESS, 287 * 0.0065/9.80665));
+
+ if(ground_temp == -1.0){
+ ground_temp = temp;
+ }
+
+ altitude = (ground_temp + 273.15)/0.0065*(1 - powf(press/ground_press, 287 * 0.0065/9.80665));
//pc.printf("%f\t%f\t%f\r\n",press, temp, altitude);
+ //lpf_altitude
+ previous_lpf_altitude = lpf_altitude;
+ lpf_altitude += LPF_COEFFICIENT_ALT*(altitude - previous_lpf_altitude);
+
+ //pc.printf("%f\t%f\t\r\n",altitude,lpf_altitude);
+
+ //velocity計算
+ velocity = (lpf_altitude - previous_lpf_altitude)*1000/(main_time - previous_main_time);
+ //pc.printf("********************\r\n");
+ //pc.printf("lpf altitude : %f\r\n",lpf_altitude);
+ //pc.printf("main time : %f\r\n",main_time/1000);
+ //pc.printf("altitude def: %f\r\n",lpf_altitude - previous_lpf_altitude);
+ //pc.printf("velocity : %f\r\n",velocity);
+ //pc.printf("time def: %f\r\n",main_time - previous_main_time);
+ //pc.printf("********************\r\n");
+
+ //lpf_velocity
+ previous_lpf_velocity = lpf_velocity;
+ lpf_velocity += LPF_COEFFICIENT_VEL*(velocity - previous_lpf_velocity);
+ //pc.printf("%f\t%f\t%f\t%f\r\n",altitude,lpf_altitude,velocity,lpf_velocity);
+
//MPU9250
- /*
mpu9250.getAll(imu, mag);
- 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]);
- */
+ //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]);
//INA226
ina226_main.get_Voltage_current(&voltage_main, ¤t_main);
ina226_sep.get_Voltage_current(&voltage_sep, ¤t_sep);
- voltage_main /= 1000;
- current_main /= 1000;
- voltage_sep /= 1000;
- current_sep /= 1000;
//pc.printf("MainVol : %.2f, SepVol : %.2f, MainCur : %.2f, SepCur : %.2f\r\n", voltage_main, voltage_sep, current_main, current_sep);
//pc.printf("MainVol : %f, SepVol : %f, MainCur : %f, SepCur : %f\r\n", voltage_main*(1/1000), voltage_sep, current_main, current_sep);
}
@@ -304,72 +609,87 @@
header_set = true;
}
- pc.printf("********************\r\n\r\n");
+ ///pc.printf("********************\r\n");
+
+ //time_reset_counter
+ im920.write((short)time_reset_counter);
+ im_buf_n += 2;
+ //MainTime
+ im920.write((float)main_time);
+ im_buf_n += 4;
+
+ //mode
+ ///pc.printf("mode : %c\r\n",mode);
+ im920.write((char)mode);
+ im_buf_n ++;
+
+ //flightpin
+ bitshift_flightpin_status = flightpin_status << 0;
+ //pc.printf("flightpin bool : %d\r\n",flightpin_status);
+ //pc.printf("flightpin bool : %d\r\n",bitshift_flightpin_status);
//Nichrome
- pc.printf("Nichrome_status : %c\r\n",nich_status);
- im920.write((char)nich_status);
+ bitshift_nich_status = nich_status << 1;
+ //pc.printf("nich bool : %d\r\n",nich_status);
+ //pc.printf("nich bitshift : %d\r\n",bitshift_nich_status);
+
+ //頂点検知
+ bitshift_top_detect = top_detect << 2;
+
+ //保存データのステータス
+ bitshift_save_data_status = save_data_status << 3;
+
+ //bool8個を1つのcharで送るとき
+ bitshift_sum = bitshift_flightpin_status | bitshift_nich_status | bitshift_top_detect | bitshift_save_data_status;
+ im920.write((char)bitshift_sum);
+ im_buf_n ++;
+
+ //EEPROM Number
+ im920.write((char)eeprom_number);
im_buf_n ++;
//GPS
- pc.printf("Latitude : %f\r\n",lat);
- pc.printf("Longitude : %f\r\n",lon);
- pc.printf("Height : %f\r\n",height);
+ ///pc.printf("Latitude : %f\r\n",lat);
+ ///pc.printf("Longitude : %f\r\n",lon);
+ ///pc.printf("Height : %f\r\n",height);
im920.write((float)lat);
- im_buf_n ++;
+ im_buf_n += 4;
im920.write((float)lon);
- im_buf_n ++;
+ im_buf_n += 4;
+ /*
im920.write((float)height);
- im_buf_n ++;
+ im_buf_n += 4;
+ */
//LPS22HB
- pc.printf("Pressure : %f\r\n",press);
- pc.printf("Temperarure : %f\r\n",temp);
- pc.printf("Altitude : %f\r\n",altitude);
+ ///pc.printf("Pressure : %f\r\n",press);
+ ///pc.printf("Temperarure : %f\r\n",temp);
+ ///pc.printf("Altitude : %f\r\n",altitude);
im920.write((float)press);
- im_buf_n ++;
- im920.write((float)temp);
- im_buf_n ++;
- im920.write((float)altitude);
- im_buf_n ++;
+ im_buf_n += 4;
+ im920.write((short)(temp*TEMP_MULTIPLIER));
+ im_buf_n += 2;
+ im920.write((short)(lpf_altitude*LPF_ALT_MULTIPLIER));
+ im_buf_n += 2;
- //MPU9250
- /*
- im920.write((float)imu[0]);
- im_buf_n ++;
- im920.write((float)imu[1]);
- im_buf_n ++;
- im920.write((float)imu[2]);
- im_buf_n ++;
- im920.write((float)imu[3]);
- im_buf_n ++;
- im920.write((float)imu[4]);
- im_buf_n ++;
- im920.write((float)imu[5]);
- im_buf_n ++;
- im920.write((float)mag[0]);
- im_buf_n ++;
- im920.write((float)mag[1]);
- im_buf_n ++;
- im920.write((float)mag[2]);
- im_buf_n ++;
- */
+ im920.write((short)(lpf_velocity*LPF_VEL_MULTIPLIER));
+ im_buf_n += 2;
//INA226
- pc.printf("Vlotage Mian : %.2f\r\n",voltage_main);
- pc.printf("Current Main : %.2f\r\n",current_main);
- pc.printf("Voltage Sep : %.2f\r\n",voltage_sep);
- pc.printf("Current Sep : %.2f\r\n",current_sep);
- im920.write((float)voltage_main);
- im_buf_n ++;
- im920.write((float)current_main);
- im_buf_n ++;
- im920.write((float)voltage_sep);
- im_buf_n ++;
- im920.write((float)current_sep);
- im_buf_n ++;
+ ///pc.printf("Vlotage Mian : %.2f\r\n",voltage_main);
+ ///pc.printf("Current Main : %.2f\r\n",current_main);
+ ///pc.printf("Voltage Sep : %.2f\r\n",voltage_sep);
+ ///pc.printf("Current Sep : %.2f\r\n",current_sep);
+ im920.write((short)voltage_main);
+ im_buf_n += 2;
+ im920.write((short)current_main);
+ im_buf_n += 2;
+ im920.write((short)voltage_sep);
+ im_buf_n += 2;
+ im920.write((short)current_sep);
+ im_buf_n += 2;
- pc.printf("********************\r\n\r\n");
+ ///pc.printf("********************\r\n\r\n");
if(header_set){
im920.send();
@@ -382,35 +702,147 @@
}
}
+// ***************************************************
+// 打上げ時刻を送信
+// ***************************************************
+void SendLaunchTime(){
+ if(!header_set){
+ im920.header((char)HEADER_LAUNCH_TIME);
+ header_set = true;
+ }
+ pc.printf("launch time : %f\r\n",launch_time);
+ im920.write((float)launch_time);
+ im_buf_n += 4;
+ im920.write((short)time_reset_counter);
+ im_buf_n += 2;
+ send_launch_time_status = true;
+ if(header_set){
+ im920.send();
+ pc.printf("Send launch time\r\n");
+ header_set = false;
+ for(int i = 0; i < im_buf_n; i ++){
+ im_buf[i] = '\0';
+ }
+ im_buf_n = 0;
+ }
+}
// ***************************************************
-// EEPROMにデータを書き込むプログラム
+// EEPROMにデータを書き込む
// ***************************************************
-void WriteEEPROM(){
- int ptr, n = 0;
- int eeprom_ptr = 0;
-
- for(int i = 0; i < 4; i++){
- pc.printf("Start to write %d EEPROM\r\n", i);
- setEEPROMGroup(i);
- EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
+void SaveData(){
+ if(eeprom_group_counter < 4){
+ //pc.printf("Save to EEPROM\r\n");
+ int eep_buf = 0;
- while(1){
+ /*
+ for(int i = eep_buf; i < 128; i++){
+ //ptr = EEPROM.chargeBuff((char)0x02);
ptr = EEPROM.chargeBuff((int)n++);
- //ptr = EEPROM.chargeBuff((char)0xff);
- //ptr = EEPROM.chargeBuff((int)0);
- if(ptr == 128){
- EEPROM.writeBuff();
- //ptr = EEPROM.setNextPage();
- eeprom_ptr = EEPROM.setNextPage();
- //pc.printf("eeprom_ptr: %08x\r\n", eeprom_ptr);
- }
-
- if(eeprom_ptr == 0x01000000){
- ptr = 0;
- eeprom_ptr = 0;
- break;
- }
+ }
+ */
+ ptr = EEPROM.chargeBuff((char)time_reset_counter);
+ eep_buf += 1;
+ ptr = EEPROM.chargeBuff((int)main_time);
+ eep_buf += 4;
+ //ptr = EEPROM.chargeBuff((float)flight_time);
+ //eep_buf += 4;
+
+ ptr = EEPROM.chargeBuff((float)lat);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)lon);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)height);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((char)satellite_number);
+ eep_buf += 1;
+
+ ptr = EEPROM.chargeBuff((float)press);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)temp);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)altitude);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)lpf_altitude);
+ eep_buf += 4;
+
+ ptr = EEPROM.chargeBuff((float)velocity);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)lpf_velocity);
+ eep_buf += 4;
+
+ ptr = EEPROM.chargeBuff((float)imu[0]);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)imu[1]);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)imu[2]);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)imu[3]);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)imu[4]);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)imu[5]);
+ eep_buf += 4;
+
+ ptr = EEPROM.chargeBuff((float)mag[0]);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)mag[1]);
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)mag[2]);
+ eep_buf += 4;
+
+ ptr = EEPROM.chargeBuff((float)(voltage_main/1000));
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)(current_main/1000));
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)(voltage_sep/1000));
+ eep_buf += 4;
+ ptr = EEPROM.chargeBuff((float)(current_sep/1000));
+ eep_buf += 4;
+
+ ptr = EEPROM.chargeBuff((char)mode);
+ eep_buf ++;
+ ptr = EEPROM.chargeBuff((char)flightpin_status);
+ eep_buf ++;
+ ptr = EEPROM.chargeBuff((char)nich_status);
+ eep_buf ++;
+
+ //ptr = EEPROM.chargeBuff((int)n++);
+ //ptr = EEPROM.chargeBuff((char)0xff);
+ //ptr = EEPROM.chargeBuff((int)0);
+
+ //pc.printf("%d\r\n",eep_buf);
+
+ for(int i = eep_buf; i < 128; i++){
+ ptr = EEPROM.chargeBuff((char)0x00);
+ }
+
+ //pc.printf("ptr : %d\r\n",ptr);
+ EEPROM.writeBuff();
+ eeprom_ptr = EEPROM.setNextPage();
+ if(eeprom_ptr == 0x01000000 || eeprom_ptr == 0x02000000 || eeprom_ptr == 0x03000000 || eeprom_ptr == 0x04000000){
+ eeprom_number++;
+ pc.printf("eeprom_number : %d\r\n",eeprom_number);
+ }
+ //pc.printf("eeprom_ptr: %x\r\n", eeprom_ptr);
+
+ /*
+ if(eeprom_ptr == 0x01000000){
+ ptr = 0;
+ eeprom_ptr = 0;
+ plexer_num++;
+ setEEPROMGroup(plexer_num);
+ EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
+ }
+ */
+
+ if(eeprom_ptr == 0x04000000){
+ eeprom_ptr = 0;
+ eeprom_group_counter++;
+ setEEPROMGroup(eeprom_group_counter);
+ EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
+ pc.printf("EEPROM_Group : %d\r\n",eeprom_group_counter);
+ //pc.printf("SetWriteAddr\r\n");
}
}
}