IZU2020
ground station program
Diff: main.cpp
- Revision:
- 1:408f149a1107
- Parent:
- 0:6bf7ee71fb68
diff -r 6bf7ee71fb68 -r 408f149a1107 main.cpp
--- a/main.cpp Sat Feb 15 03:35:39 2020 +0000
+++ b/main.cpp Thu Feb 20 04:17:14 2020 +0000
@@ -2,18 +2,14 @@
#include "PQES920LR.h"
-#define TIME_LSB 54.9
-#define LAT_LSB 0.00275
-#define LON_LSB 0.00549
-#define ALT_LSB 15.3
-#define HIGH_ACCEL_LSB 0.0061
+#define TIME_LSB 54.931640625
#define VOLTAGE_LSB 1.25
-#define CURRENT_LSB 1.1
-#define PRESS_LSB 0.0385
-#define TEMP_LSB 0.00259
-#define ACCEL_LSB 0.000488
-#define GYRO_LSB 0.061
-#define MAG_LSB 0.146
+#define CURRENT_LSB 1.0986328125
+#define PRESS_LSB 0.0384521484375
+#define TEMP_LSB 0.002593994140625
+#define ACCEL_LSB 0.00048828125
+#define GYRO_LSB 0.06103515625
+#define MAG_LSB 0.146484375
Serial pc(USBTX, USBRX, 115200);
Serial es_serial(D1, D0, 115200);
@@ -24,11 +20,12 @@
void downlink_handler(char* data);
-Timer timer;
+char *phase_names[] = {"SAFETY", "READY", "FLIGHT", "SEP", "EMERGENCY", "RECOVERY"};
-int phase;
-
-bool f_sd;
+char mission_timer_reset;
+int mission_time;
+int flight_time;
+char f_sd;
char f_gps;
char f_adxl;
char f_ina_in;
@@ -36,22 +33,10 @@
char f_lps;
char f_mpu;
char f_mpu_ak;
-
-char mission_timer_reset;
-int mission_time;
-char flight_timer_reset;
-int flight_time;
-int utc_hour;
-int utc_min;
-float utc_sec;
+char phase;
float lat;
float lon;
-int sat;
-int fix;
-float hdop;
float alt;
-float geoid;
-float high_accel[3];
float voltage_in;
float current_in;
float voltage_ex;
@@ -64,10 +49,6 @@
short mission_time_bits;
short flight_time_bits;
-short lat_bits;
-short lon_bits;
-short alt_bits;
-short high_accel_bits[3];
short voltage_in_bits;
short current_in_bits;
short voltage_ex_bits;
@@ -78,40 +59,25 @@
short gyro_bits[3];
short mag_bits[3];
+int mission_time_s;
+int mission_hour, mission_min, mission_sec;
+float voltage_in_V, voltage_ex_V;
int main()
{
- timer.start();
es.attach(downlink_handler);
- prompt();
+ pc.printf("PLANET-Q GROUND STATION\r\n");
while(1) {
if(pc.readable()) {
char command[1];
- char c = pc.getc();
- command[0] = c + 0x80;
+ command[0] = pc.getc() + 0x80;
es.send(command, 1);
- prompt();
+ pc.printf("send:\t%02x\r\n", command[0]);
}
}
}
-void prompt()
-{
- pc.printf("\r\n");
- pc.printf("PLANET-Q GROUND STATION\r\n");
- pc.printf("%.0f\r\n", timer.read());
- pc.printf("\r\n");
- pc.printf("0 : SAFETY\r\n");
- pc.printf("1 : WAIT\r\n");
- pc.printf("2 : FLIGHT\r\n");
- pc.printf("3 : SEP\r\n");
- pc.printf("4 : EMERGENCY\r\n")
- pc.printf("5 : RECOVERY\r\n");
- pc.printf("DEL : SYSTEM RESET\r\n");
- pc.printf(">>>");
-}
-
void downlink_handler(char *data)
{
mission_timer_reset = data[1];
@@ -126,12 +92,9 @@
f_mpu = data[6] >> 1 & 1;
f_mpu_ak = data[6] & 1;
phase = data[7];
- lat_bits = (short)(data[8] | data[9] << 8);
- lon_bits = (short)(data[10] | data[11] << 8);
- alt_bits = (short)(data[12] | data[13] << 8);
- high_accel_bits[0] = (short)(data[14] | data[15] << 8);
- high_accel_bits[1] = (short)(data[16] | data[17] << 8);
- high_accel_bits[2] = (short)(data[18] | data[19] << 8);
+ lat = *(float*)&data[8];
+ lon = *(float*)&data[12];
+ alt = *(float*)&data[16];
voltage_in_bits = (short)(data[20] | data[21] << 8);
current_in_bits = (short)(data[22] | data[23] << 8);
voltage_ex_bits = (short)(data[24] | data[25] << 8);
@@ -150,12 +113,6 @@
mission_time = mission_time_bits * TIME_LSB;
flight_time = flight_time_bits * TIME_LSB;
- lat = lat_bits * LAT_LSB;
- lon = lon_bits * LON_LSB;
- alt = alt_bits * ALT_LSB;
- for(int i = 0; i < 3; i++) {
- high_accel[i] = high_accel_bits[i] * HIGH_ACCEL_LSB;
- }
voltage_in = voltage_in_bits * VOLTAGE_LSB;
current_in = current_in_bits * CURRENT_LSB;
voltage_ex = voltage_ex_bits * VOLTAGE_LSB;
@@ -171,46 +128,49 @@
for(int i = 0; i < 3; i++) {
mag[i] = mag_bits[i] * MAG_LSB;
}
-
- {
- pc.printf("{\r\n");
- pc.printf(" ""phase"": %d,\r\n", phase);
- pc.printf(" ""mission_timer_reset"": %d,\r\n", mission_timer_reset);
- pc.printf(" ""mission_time"": %d,\r\n", mission_time);
- pc.printf(" ""flight_time"": %d,\r\n", flight_time);
- pc.printf(" ""f_sd"": %d,\r\n", f_sd);
- pc.printf(" ""f_gps"": %d,\r\n", f_gps);
- pc.printf(" ""f_adxl"": %d,\r\n", f_adxl);
- pc.printf(" ""f_ina_in"": %d,\r\n", f_ina_in);
- pc.printf(" ""f_ina_ex"": %d,\r\n", f_ina_ex);
- pc.printf(" ""f_lps"": %d,\r\n", f_lps);
- pc.printf(" ""f_mpu"": %d,\r\n", f_mpu);
- pc.printf(" ""f_mpu_ak"": %d,\r\n", f_mpu_ak);
- pc.printf(" ""lat"": %f,\r\n", lat);
- pc.printf(" ""lon"": %f,\r\n", lon);
- pc.printf(" ""sat"": %d,\r\n", sat);
- pc.printf(" ""fix"": %d,\r\n", fix);
- pc.printf(" ""hdop"": %f,\r\n", hdop);
- pc.printf(" ""alt"": %f,\r\n", alt);
- pc.printf(" ""geoid"": %f,\r\n", geoid);
- pc.printf(" ""high_accel[0]"": %f,\r\n", high_accel[0]);
- pc.printf(" ""high_accel[1]"": %f,\r\n", high_accel[1]);
- pc.printf(" ""high_accel[2]"": %f,\r\n", high_accel[2]);
- pc.printf(" ""voltage_in"": %f,\r\n", voltage_in);
- pc.printf(" ""current_in"": %f,\r\n", current_in);
- pc.printf(" ""voltage_ex"": %f,\r\n", voltage_ex);
- pc.printf(" ""current_ex"": %f,\r\n", current_ex);
- pc.printf(" ""press"": %f,\r\n", press);
- pc.printf(" ""temp"": %f,\r\n", temp);
- pc.printf(" ""accel[0]"": %f,\r\n", accel[0]);
- pc.printf(" ""accel[1]"": %f,\r\n", accel[1]);
- pc.printf(" ""accel[2]"": %f,\r\n", accel[2]);
- pc.printf(" ""gyro[0]"": %f,\r\n", gyro[0]);
- pc.printf(" ""gyro[1]"": %f,\r\n", gyro[1]);
- pc.printf(" ""gyro[2]"": %f,\r\n", gyro[2]);
- pc.printf(" ""mag[0]"": %f,\r\n", mag[0]);
- pc.printf(" ""mag[1]"": %f,\r\n", mag[1]);
- pc.printf(" ""mag[2]"": %f,\r\n", mag[2]);
- pc.printf("}\r\n");
- }
+
+ mission_time_s = (mission_time / 1000) + (mission_timer_reset * 30*60);
+ mission_hour = mission_time_s / 3600;
+ mission_time_s %= 3600;
+ mission_min = mission_time_s / 60;
+ mission_time_s %= 60;
+ mission_sec = mission_time_s;
+
+ voltage_in_V = voltage_in / 1000.0f;
+ voltage_ex_V = voltage_ex / 1000.0f;
+
+ pc.printf(" PLANET-Q GROUND STATION\r\n");
+ pc.printf(" commands:\r\n");
+ pc.printf(" 0 : SAFETY\r\n");
+ pc.printf(" 1 : WAIT\r\n");
+ pc.printf(" 2 : FLIGHT\r\n");
+ pc.printf(" 3 : SEP\r\n");
+ pc.printf(" 4 : EMERGENCY\r\n");
+ pc.printf(" 5 : RECOVERY\r\n");
+ pc.printf(" DEL : SYSTEM RESET\r\n");
+ pc.printf("\r\n");
+ pc.printf(" mission time:\t%02d:%02d:%02d\r\n", mission_hour, mission_min, mission_sec);
+ pc.printf(" flight time:\t%d[ms]\r\n", flight_time);
+ pc.printf(" phase:\t%s\r\n", phase_names[phase]);
+ pc.printf(" flags:\tSD\tGPS\tINA_in\tINA_ex\tADXL\tLPS\tMPU\tMPU_AK\r\n");
+ pc.printf(" \t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\r\n", f_sd, f_gps, f_adxl, f_ina_in, f_ina_ex, f_lps, f_mpu, f_mpu_ak);
+ pc.printf("\r\n");
+ pc.printf(" lat:\t\t%.6f\r\n", lat);
+ pc.printf(" lon:\t\t%.6f\r\n", lon);
+ pc.printf(" alt:\t\t%.1f[m]\r\n", alt);
+ pc.printf("\r\n");
+ pc.printf(" internal:\tvoltage\t\tcurrent\r\n");
+ pc.printf(" \t%.2f[V]\t\t%.0f[mA]\r\n", voltage_in_V, current_in);
+ pc.printf(" external:\tvoltage\t\tcurrent\r\n");
+ pc.printf(" \t%.2f[V]\t\t%.0f[mA]\r\n", voltage_ex_V, current_ex);
+ pc.printf("\r\n");
+ pc.printf(" press:\t\t%.2f[hPa]\r\n", press);
+ pc.printf(" temp:\t\t%.2f[C]\r\n", temp);
+ pc.printf("\r\n");
+ pc.printf(" accel:\t\tx\t\ty\t\tz\r\n");
+ pc.printf(" \t\t%.2f\t\t%.2f\t\t%.2f\t[G]\r\n", accel[0], accel[1], accel[2]);
+ pc.printf(" gyro:\t\tx\t\ty\t\tz\r\n");
+ pc.printf(" \t\t%.2f\t\t%.2f\t\t%.2f\t[rad/s]\r\n", gyro[0], gyro[1], gyro[2]);
+ pc.printf(" mag:\t\tx\t\ty\t\tz\r\n");
+ pc.printf(" \t\t%.2f\t\t%.2f\t\t%.2f\t[uT]\r\n", mag[0], mag[1], mag[2]);
}
\ No newline at end of file