Hybrid_OB2021
v1
Diff: main.cpp
- Revision:
- 2:41238982b498
- Parent:
- 1:69472741bd36
- Child:
- 3:8e2d7ebdafb7
diff -r 69472741bd36 -r 41238982b498 main.cpp
--- a/main.cpp Thu Jul 29 18:37:48 2021 +0000
+++ b/main.cpp Sun Aug 08 11:26:29 2021 +0000
@@ -1,6 +1,12 @@
#include "mbed.h"
#include "IM920.h"
+#define RESET_TIME 1800
+
+#define TEMP_MULTIPLIER 100
+#define LPF_ALT_MULTIPLIER 100
+#define LPF_VEL_MULTIPLIER 100
+
// ***************************************************
// コンストラクタ
// ***************************************************
@@ -15,20 +21,44 @@
void ReadPC();
void ReadSensorSetup();
void ReadData();
+void ReadLaunchTime();
void Read0xA5();
void Help();
// ***************************************************
// 変数の宣言
// ***************************************************
-char nich_status;
+int minute;
+int second;
+int launch_minute;
+int launch_second;
+int flight_minute = 0;
+int flight_second = 0;
+float flight_time = 0;
+float duration_flight_time = 0;
+float main_time;
+float duration_main_time = 0;
+float launch_time = 0;
+int time_reset_counter = 0;
+int time_reset_counter_launch = 0;
+
+char mode;
+
+bool top_detect;
+bool flightpin_status;
+bool nich_status;
+bool save_data_status;
+char bitshift_sum;
bool header_set = false;
char im_buf[16];//16なのって意味ある？
int im_buf_n = 0;
+char eeprom_number;
+
float lat, lon, height;
-float press, temp, altitude;
+float press, temp, lpf_altitude;
+float lpf_velocity;
float imu[6], mag[3];
float mag_geo[3];
@@ -40,13 +70,16 @@
// 無線のヘッダまとめ
// ***************************************************
const char HEADER_SETUP = 0x01;
-// 0xA1
+// 0x01, GPS, LPS22HB, MPU9250, MPU9250_MAG, INA226_MAIN, INA226_SEP
+// 1 1 1 1 1 1 1
-const char HEADER_DATA = 0xA2;
-// 0xA2,lat,lon,height,press,temp,altitude
-// 1
+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_ECHO = 0xA5;
+const char HEADER_LAUNCH_TIME = 0x03;
+
+const char HEADER_ECHO = 0x05;
// 0xA5,コマンド
// 1 1
@@ -54,9 +87,10 @@
// メイン関数
// ***************************************************
int main() {
- wait(0.5f);
+ //wait(0.5f);
pc.printf("Hello Ground!\r\n");
pc.attach(&ReadPC, Serial::RxIrq);
+ im920.attach(&ReadLaunchTime, HEADER_LAUNCH_TIME);
im920.attach(&Read0xA5, HEADER_ECHO);
im920.attach(&ReadSensorSetup, HEADER_SETUP);
im920.attach(&ReadData, HEADER_DATA);
@@ -70,6 +104,63 @@
}
switch(c){
+ case 'U':
+ im920.write((char)c);
+ im_buf[im_buf_n] = 'U';
+ im_buf_n ++;
+ pc.printf("INPUT : U\r\n");
+ if(header_set){
+ im920.send();
+ pc.printf("Send : %s\r\n", im_buf);
+ header_set = false;
+ for(int i = 0; i < im_buf_n; i ++){
+ im_buf[i] = '\0';
+ }
+ im_buf_n = 0;
+ }
+ break;
+
+ case 'O':
+ im920.write((char)c);
+ im_buf[im_buf_n] = 'O';
+ im_buf_n ++;
+ pc.printf("INPUT : O\r\n");
+ break;
+
+ case 'F':
+ im920.write((char)c);
+ im_buf[im_buf_n] = 'F';
+ im_buf_n ++;
+ pc.printf("INPUT : F\r\n");
+
+ if(header_set){
+ im920.send();
+ pc.printf("Send : %s\r\n", im_buf);
+ header_set = false;
+ for(int i = 0; i < im_buf_n; i ++){
+ im_buf[i] = '\0';
+ }
+ im_buf_n = 0;
+ }
+ break;
+
+ case 'S':
+ im920.write((char)c);
+ im_buf[im_buf_n] = 'S';
+ im_buf_n ++;
+ pc.printf("INPUT : S\r\n");
+
+ if(header_set){
+ im920.send();
+ pc.printf("Send : %s\r\n", im_buf);
+ header_set = false;
+ for(int i = 0; i < im_buf_n; i ++){
+ im_buf[i] = '\0';
+ }
+ im_buf_n = 0;
+ }
+ break;
+
case '0':
im920.write((char)0x00);
im_buf[im_buf_n] = '0';
@@ -146,44 +237,167 @@
void ReadData(){
//pc.printf("***Acquired Data****\r\n");
pc.printf("***Recieved Data****\r\n");
+
int bit = 1;
+
+ time_reset_counter = (short)im920.toShort(bit);
+ bit += 2;
+
+ main_time = (float)im920.toFloat(bit);
+ main_time /= 1000;
+ bit += 4;
+
+ minute = main_time / 60 + time_reset_counter*30;
+ //pc.printf("%d\t%f\r\n",minute,minute);
+ second = (int)main_time % 60;
+
+ //duration_main_time = main_time + RESET_TIME*time_reset_counter;
+ pc.printf("Main Time : %d : %02d\r\n",minute,second);
+ //pc.printf("Main Time : %f\r\n",duration_main_time);
+
+ pc.printf("Launch Time : %d : %02d\r\n",launch_minute,launch_second);
+
+ if(mode >= 0x04){
+ if(second >= launch_second){
+ flight_minute = minute - launch_minute;
+ flight_second = second - launch_second;
+ }else{
+ flight_minute = minute - launch_minute - 1;
+ flight_second = 60 + second - launch_second;
+ }
+ pc.printf("Flight Time : %d : %02d\r\n",flight_minute,flight_second);
+ }else{
+ pc.printf("Flight Time : %d : %02d\r\n",flight_minute,flight_second);
+ }
+
+ mode = im920.data[bit];
+ bit += 1;
+ switch(mode){
+ case 0x00:
+ pc.printf("Mode : SETUP\r\n");
+ break;
+
+ case 0x01:
+ pc.printf("Mode : SAFETY\r\n");
+ break;
+
+ case 0x02:
+ pc.printf("Mode : TO_ONCALL\r\n");
+ break;
+
+ case 0x03:
+ pc.printf("Mode : ONCALL\r\n");
+ break;
+
+ case 0x04:
+ pc.printf("Mode : TO_FLIGHT\r\n");
+ break;
+
+ case 0x05:
+ pc.printf("Mode : FLIGHT\r\n");
+ break;
+
+ case 0x06:
+ pc.printf("Mode : TO_SEPARATE\r\n");
+ break;
+
+ case 0x07:
+ pc.printf("Mode : SEPARATE\r\n");
+ break;
+
+ case 0x08:
+ pc.printf("Mode : TO_DESCEND\r\n");
+ break;
+
+ case 0x09:
+ pc.printf("Mode : DESCEND\r\n");
+ break;
+
+ case 0x0a:
+ pc.printf("Mode : TO_COLLECTION\r\n");
+ break;
+
+ case 0x0b:
+ pc.printf("Mode : COLLECTION\r\n");
+ break;
+ }
+
+ /*bool1個を1バイトで送る
+ flightpin_status = im920.data[bit];
+ bit += 1;
+ pc.printf("FlightPin Status: %d\r\n",flightpin_status);
+
nich_status = im920.data[bit];
bit += 1;
- pc.printf("Nichrome Status : %c\r\n",nich_status);
+ pc.printf("Nichrome Status : %d\r\n",nich_status);
+ */
+
+ //bool8個を1バイトで送る
+ bitshift_sum = im920.data[bit];
+ bit += 1;
+ //pc.printf("Bitshift Sum : %d\r\n",bitshift_sum);
+
+ flightpin_status = 0x01 & (bitshift_sum);
+ pc.printf("FlightPin Status: %d\r\n",flightpin_status);
+
+ //nich_status = bitshift_sum >> 1;
+ nich_status = 0x02 & (bitshift_sum);
+ pc.printf("Nichrome Status : %d\r\n",nich_status);
+
+ top_detect = 0x04 & (bitshift_sum);
+ pc.printf("Top Detect : %d\r\n",top_detect);
+
+ save_data_status = 0x08 & (bitshift_sum);
+ pc.printf("Save Data Status: %d\r\n",save_data_status);
+
+ eeprom_number = im920.data[bit];
+ bit += 1;
+ pc.printf("eeprom_number : %d\r\n",eeprom_number);
+
lat = (float)im920.toFloat(bit);
bit += 4;
pc.printf("Latitude : %.7f\r\n",lat);
lon = (float)im920.toFloat(bit);
bit += 4;
pc.printf("Longitude : %.7f\r\n",lon);
+ /*
height = (float)im920.toFloat(bit);
bit += 4;
pc.printf("Height : %.7f\r\n",height);
-
+ */
press = (float)im920.toFloat(bit);
bit += 4;
- pc.printf("Pressure : %.4f[hPa]\r\n", press);
- temp = (float)im920.toFloat(bit);
- bit += 4;
- pc.printf("Temperarure : %.2f[℃]\r\n", temp);
- altitude = (float)im920.toFloat(bit);
- bit += 4;
- pc.printf("Altitude : %.2f[m]\r\n", altitude);
+ pc.printf("Pressure : %.2f [hPa]\r\n", press);
+ temp = (short)im920.toShort(bit);
+ temp /= TEMP_MULTIPLIER;
+ bit += 2;
+ pc.printf("Temperarure : %.2f [degC]\r\n", temp);
+ lpf_altitude = (short)im920.toShort(bit);
+ lpf_altitude /= LPF_ALT_MULTIPLIER;
+ bit += 2;
+ pc.printf("LPF Altitude : %.2f [m]\r\n", lpf_altitude);
+
+ lpf_velocity = (short)im920.toShort(bit);
+ lpf_velocity /= LPF_VEL_MULTIPLIER;
+ bit += 2;
+ pc.printf("Lpf Velocity : %f [m/s]\r\n", lpf_velocity);
- voltage_main = (float)im920.toFloat(bit);
+ voltage_main = (short)im920.toShort(bit);
+ voltage_main /= 1000;
+ bit += 2;
+ pc.printf("Voltage Main : %.2f [V]\r\n",voltage_main);
+ current_main = (short)im920.toShort(bit);
+ current_main /= 1000;
+ bit += 2;
+ pc.printf("Crrent Main : %.2f [A]\r\n",current_main);
+ voltage_sep = (short)im920.toShort(bit);
+ voltage_sep /= 1000;
+ bit += 2;
+ pc.printf("Voltage Sep : %.2f [V]\r\n",voltage_sep);
+ current_sep = (short)im920.toShort(bit);
+ current_sep /= 1000;
bit += 4;
- pc.printf("Voltage Main : %.2f\r\n",voltage_main);
- current_main = (float)im920.toFloat(bit);
- bit += 4;
- pc.printf("Crrent Main : %.2f\r\n",current_main);
- voltage_sep = (float)im920.toFloat(bit);
- bit += 4;
- pc.printf("Voltage Sep : %.2f\r\n",voltage_sep);
- current_sep = (float)im920.toFloat(bit);
- bit += 4;
- pc.printf("Current Sep : %.2f\r\n",current_sep);
-
-
+ pc.printf("Current Sep : %.2f [A]\r\n",current_sep);
for(int i = 0; i < 20; i++){
pc.printf("*");
@@ -191,6 +405,19 @@
pc.printf("\r\n\r\n");
}
+void ReadLaunchTime(){
+ int bit = 1;
+ launch_time = (float)im920.toFloat(bit);
+ bit += 4;
+ launch_time /= 1000;
+ time_reset_counter_launch = (short)im920.toShort(bit);
+ bit += 2;
+ launch_minute = launch_time / 60;
+ launch_second = (int)launch_time % 60;
+ pc.printf("Get launch time\r\n");
+ pc.printf("launch time : %f\r\n",launch_time);
+}
+
void Read0xA5(){
for(int i = 0; i < 20; i++){
pc.printf("*");
@@ -198,6 +425,30 @@
pc.printf("\r\nIM920 ECHO : ");
switch(im920.data[1]){
+ case 'U':
+ pc.printf("MODE SETUP\r\n");
+ break;
+
+ case 'O':
+ pc.printf("MODE ONCALL\r\n");
+ break;
+
+ case 'F':
+ pc.printf("MODE FLIGHT\r\n");
+ break;
+
+ case 'S':
+ pc.printf("MODE SEPARATE\r\n");
+ break;
+
+ case 'D':
+ pc.printf("MODE DESCEND\r\n");
+ break;
+
+ case 'C':
+ pc.printf("MODE COLLECTION\r\n");
+ break;
+
case 0x01:
pc.printf("SEPARATE\r\n");
break;
@@ -216,6 +467,12 @@
void Help(){
pc.printf("\r\n");
pc.printf("**I can help you!***\r\n");
+ pc.printf("U : Mode Setup\r\n");
+ pc.printf("O : Mode Oncall\r\n");
+ pc.printf("F : Mode Flight\r\n");
+ pc.printf("S : Mode Separate\r\n");
+ pc.printf("D : Mode Descend\r\n");
+ pc.printf("C : Mode Collection\r\n");
pc.printf("0 : Stop Separate\r\n");
pc.printf("1 : Separate\r\n");
pc.printf("? : Help\r\n");