IZU2020 / Mbed 2 deprecated IZU2020_GROUND_STATION

ground station program

Dependencies:   mbed PQES920LR

main.cpp@1:408f149a1107, 2020-02-20 (annotated)

Committer:
tanahashi
Date:
Thu Feb 20 04:17:14 2020 +0000
Revision:
1:408f149a1107
Parent:
0:6bf7ee71fb68 
integration test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tanahashi 0:6bf7ee71fb68 1 #include "mbed.h"
tanahashi 0:6bf7ee71fb68 2
tanahashi 0:6bf7ee71fb68 3 #include "PQES920LR.h"
tanahashi 0:6bf7ee71fb68 4
tanahashi 1:408f149a1107 5 #define TIME_LSB 54.931640625
tanahashi 0:6bf7ee71fb68 6 #define VOLTAGE_LSB 1.25
tanahashi 1:408f149a1107 7 #define CURRENT_LSB 1.0986328125
tanahashi 1:408f149a1107 8 #define PRESS_LSB 0.0384521484375
tanahashi 1:408f149a1107 9 #define TEMP_LSB 0.002593994140625
tanahashi 1:408f149a1107 10 #define ACCEL_LSB 0.00048828125
tanahashi 1:408f149a1107 11 #define GYRO_LSB 0.06103515625
tanahashi 1:408f149a1107 12 #define MAG_LSB 0.146484375
tanahashi 0:6bf7ee71fb68 13
tanahashi 0:6bf7ee71fb68 14 Serial pc(USBTX, USBRX, 115200);
tanahashi 0:6bf7ee71fb68 15 Serial es_serial(D1, D0, 115200);
tanahashi 0:6bf7ee71fb68 16
tanahashi 0:6bf7ee71fb68 17 ES920LR es(es_serial);
tanahashi 0:6bf7ee71fb68 18
tanahashi 0:6bf7ee71fb68 19 void prompt();
tanahashi 0:6bf7ee71fb68 20
tanahashi 0:6bf7ee71fb68 21 void downlink_handler(char* data);
tanahashi 0:6bf7ee71fb68 22
tanahashi 1:408f149a1107 23 char *phase_names[] = {"SAFETY", "READY", "FLIGHT", "SEP", "EMERGENCY", "RECOVERY"};
tanahashi 0:6bf7ee71fb68 24
tanahashi 1:408f149a1107 25 char mission_timer_reset;
tanahashi 1:408f149a1107 26 int mission_time;
tanahashi 1:408f149a1107 27 int flight_time;
tanahashi 1:408f149a1107 28 char f_sd;
tanahashi 0:6bf7ee71fb68 29 char f_gps;
tanahashi 0:6bf7ee71fb68 30 char f_adxl;
tanahashi 0:6bf7ee71fb68 31 char f_ina_in;
tanahashi 0:6bf7ee71fb68 32 char f_ina_ex;
tanahashi 0:6bf7ee71fb68 33 char f_lps;
tanahashi 0:6bf7ee71fb68 34 char f_mpu;
tanahashi 0:6bf7ee71fb68 35 char f_mpu_ak;
tanahashi 1:408f149a1107 36 char phase;
tanahashi 0:6bf7ee71fb68 37 float lat;
tanahashi 0:6bf7ee71fb68 38 float lon;
tanahashi 0:6bf7ee71fb68 39 float alt;
tanahashi 0:6bf7ee71fb68 40 float voltage_in;
tanahashi 0:6bf7ee71fb68 41 float current_in;
tanahashi 0:6bf7ee71fb68 42 float voltage_ex;
tanahashi 0:6bf7ee71fb68 43 float current_ex;
tanahashi 0:6bf7ee71fb68 44 float press;
tanahashi 0:6bf7ee71fb68 45 float temp;
tanahashi 0:6bf7ee71fb68 46 float accel[3];
tanahashi 0:6bf7ee71fb68 47 float gyro[3];
tanahashi 0:6bf7ee71fb68 48 float mag[3];
tanahashi 0:6bf7ee71fb68 49
tanahashi 0:6bf7ee71fb68 50 short mission_time_bits;
tanahashi 0:6bf7ee71fb68 51 short flight_time_bits;
tanahashi 0:6bf7ee71fb68 52 short voltage_in_bits;
tanahashi 0:6bf7ee71fb68 53 short current_in_bits;
tanahashi 0:6bf7ee71fb68 54 short voltage_ex_bits;
tanahashi 0:6bf7ee71fb68 55 short current_ex_bits;
tanahashi 0:6bf7ee71fb68 56 short press_bits;
tanahashi 0:6bf7ee71fb68 57 short temp_bits;
tanahashi 0:6bf7ee71fb68 58 short accel_bits[3];
tanahashi 0:6bf7ee71fb68 59 short gyro_bits[3];
tanahashi 0:6bf7ee71fb68 60 short mag_bits[3];
tanahashi 0:6bf7ee71fb68 61
tanahashi 1:408f149a1107 62 int mission_time_s;
tanahashi 1:408f149a1107 63 int mission_hour, mission_min, mission_sec;
tanahashi 1:408f149a1107 64 float voltage_in_V, voltage_ex_V;
tanahashi 0:6bf7ee71fb68 65
tanahashi 0:6bf7ee71fb68 66 int main()
tanahashi 0:6bf7ee71fb68 67 {
tanahashi 0:6bf7ee71fb68 68 es.attach(downlink_handler);
tanahashi 1:408f149a1107 69 pc.printf("PLANET-Q GROUND STATION\r\n");
tanahashi 0:6bf7ee71fb68 70
tanahashi 0:6bf7ee71fb68 71 while(1) {
tanahashi 0:6bf7ee71fb68 72 if(pc.readable()) {
tanahashi 0:6bf7ee71fb68 73 char command[1];
tanahashi 1:408f149a1107 74 command[0] = pc.getc() + 0x80;
tanahashi 0:6bf7ee71fb68 75 es.send(command, 1);
tanahashi 1:408f149a1107 76 pc.printf("send:\t%02x\r\n", command[0]);
tanahashi 0:6bf7ee71fb68 77 }
tanahashi 0:6bf7ee71fb68 78 }
tanahashi 0:6bf7ee71fb68 79 }
tanahashi 0:6bf7ee71fb68 80
tanahashi 0:6bf7ee71fb68 81 void downlink_handler(char *data)
tanahashi 0:6bf7ee71fb68 82 {
tanahashi 0:6bf7ee71fb68 83 mission_timer_reset = data[1];
tanahashi 0:6bf7ee71fb68 84 mission_time_bits = (short)(data[2] | data[3] << 8);
tanahashi 0:6bf7ee71fb68 85 flight_time_bits = (short)(data[4] | data[5] << 8);
tanahashi 0:6bf7ee71fb68 86 f_sd = data[6] >> 7 & 1;
tanahashi 0:6bf7ee71fb68 87 f_gps = data[6] >> 6 & 1;
tanahashi 0:6bf7ee71fb68 88 f_adxl = data[6] >> 5 & 1;
tanahashi 0:6bf7ee71fb68 89 f_ina_in = data[6] >> 4 & 1;
tanahashi 0:6bf7ee71fb68 90 f_ina_ex = data[6] >> 3 & 1;
tanahashi 0:6bf7ee71fb68 91 f_lps = data[6] >> 2 & 1;
tanahashi 0:6bf7ee71fb68 92 f_mpu = data[6] >> 1 & 1;
tanahashi 0:6bf7ee71fb68 93 f_mpu_ak = data[6] & 1;
tanahashi 0:6bf7ee71fb68 94 phase = data[7];
tanahashi 1:408f149a1107 95 lat = *(float*)&data[8];
tanahashi 1:408f149a1107 96 lon = *(float*)&data[12];
tanahashi 1:408f149a1107 97 alt = *(float*)&data[16];
tanahashi 0:6bf7ee71fb68 98 voltage_in_bits = (short)(data[20] | data[21] << 8);
tanahashi 0:6bf7ee71fb68 99 current_in_bits = (short)(data[22] | data[23] << 8);
tanahashi 0:6bf7ee71fb68 100 voltage_ex_bits = (short)(data[24] | data[25] << 8);
tanahashi 0:6bf7ee71fb68 101 current_ex_bits = (short)(data[26] | data[27] << 8);
tanahashi 0:6bf7ee71fb68 102 press_bits = (short)(data[28] | data[29] << 8);
tanahashi 0:6bf7ee71fb68 103 temp_bits = (short)(data[30] | data[31] << 8);
tanahashi 0:6bf7ee71fb68 104 accel_bits[0] = (short)(data[32] | data[33] << 8);
tanahashi 0:6bf7ee71fb68 105 accel_bits[1] = (short)(data[34] | data[35] << 8);
tanahashi 0:6bf7ee71fb68 106 accel_bits[2] = (short)(data[36] | data[37] << 8);
tanahashi 0:6bf7ee71fb68 107 gyro_bits[0] = (short)(data[38] | data[39] << 8);
tanahashi 0:6bf7ee71fb68 108 gyro_bits[1] = (short)(data[40] | data[41] << 8);
tanahashi 0:6bf7ee71fb68 109 gyro_bits[2] = (short)(data[42] | data[43] << 8);
tanahashi 0:6bf7ee71fb68 110 mag_bits[0] = (short)(data[44] | data[45] << 8);
tanahashi 0:6bf7ee71fb68 111 mag_bits[1] = (short)(data[46] | data[47] << 8);
tanahashi 0:6bf7ee71fb68 112 mag_bits[2] = (short)(data[48] | data[49] << 8);
tanahashi 0:6bf7ee71fb68 113
tanahashi 0:6bf7ee71fb68 114 mission_time = mission_time_bits * TIME_LSB;
tanahashi 0:6bf7ee71fb68 115 flight_time = flight_time_bits * TIME_LSB;
tanahashi 0:6bf7ee71fb68 116 voltage_in = voltage_in_bits * VOLTAGE_LSB;
tanahashi 0:6bf7ee71fb68 117 current_in = current_in_bits * CURRENT_LSB;
tanahashi 0:6bf7ee71fb68 118 voltage_ex = voltage_ex_bits * VOLTAGE_LSB;
tanahashi 0:6bf7ee71fb68 119 current_ex = current_ex_bits * CURRENT_LSB;
tanahashi 0:6bf7ee71fb68 120 press = press_bits * PRESS_LSB;
tanahashi 0:6bf7ee71fb68 121 temp = temp_bits * TEMP_LSB;
tanahashi 0:6bf7ee71fb68 122 for(int i = 0; i < 3; i++) {
tanahashi 0:6bf7ee71fb68 123 accel[i] = accel_bits[i] * ACCEL_LSB;
tanahashi 0:6bf7ee71fb68 124 }
tanahashi 0:6bf7ee71fb68 125 for(int i = 0; i < 3; i++) {
tanahashi 0:6bf7ee71fb68 126 gyro[i] = gyro_bits[i] * GYRO_LSB;
tanahashi 0:6bf7ee71fb68 127 }
tanahashi 0:6bf7ee71fb68 128 for(int i = 0; i < 3; i++) {
tanahashi 0:6bf7ee71fb68 129 mag[i] = mag_bits[i] * MAG_LSB;
tanahashi 0:6bf7ee71fb68 130 }
tanahashi 1:408f149a1107 131
tanahashi 1:408f149a1107 132 mission_time_s = (mission_time / 1000) + (mission_timer_reset * 30*60);
tanahashi 1:408f149a1107 133 mission_hour = mission_time_s / 3600;
tanahashi 1:408f149a1107 134 mission_time_s %= 3600;
tanahashi 1:408f149a1107 135 mission_min = mission_time_s / 60;
tanahashi 1:408f149a1107 136 mission_time_s %= 60;
tanahashi 1:408f149a1107 137 mission_sec = mission_time_s;
tanahashi 1:408f149a1107 138
tanahashi 1:408f149a1107 139 voltage_in_V = voltage_in / 1000.0f;
tanahashi 1:408f149a1107 140 voltage_ex_V = voltage_ex / 1000.0f;
tanahashi 1:408f149a1107 141
tanahashi 1:408f149a1107 142 pc.printf(" PLANET-Q GROUND STATION\r\n");
tanahashi 1:408f149a1107 143 pc.printf(" commands:\r\n");
tanahashi 1:408f149a1107 144 pc.printf(" 0 : SAFETY\r\n");
tanahashi 1:408f149a1107 145 pc.printf(" 1 : WAIT\r\n");
tanahashi 1:408f149a1107 146 pc.printf(" 2 : FLIGHT\r\n");
tanahashi 1:408f149a1107 147 pc.printf(" 3 : SEP\r\n");
tanahashi 1:408f149a1107 148 pc.printf(" 4 : EMERGENCY\r\n");
tanahashi 1:408f149a1107 149 pc.printf(" 5 : RECOVERY\r\n");
tanahashi 1:408f149a1107 150 pc.printf(" DEL : SYSTEM RESET\r\n");
tanahashi 1:408f149a1107 151 pc.printf("\r\n");
tanahashi 1:408f149a1107 152 pc.printf(" mission time:\t%02d:%02d:%02d\r\n", mission_hour, mission_min, mission_sec);
tanahashi 1:408f149a1107 153 pc.printf(" flight time:\t%d[ms]\r\n", flight_time);
tanahashi 1:408f149a1107 154 pc.printf(" phase:\t%s\r\n", phase_names[phase]);
tanahashi 1:408f149a1107 155 pc.printf(" flags:\tSD\tGPS\tINA_in\tINA_ex\tADXL\tLPS\tMPU\tMPU_AK\r\n");
tanahashi 1:408f149a1107 156 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);
tanahashi 1:408f149a1107 157 pc.printf("\r\n");
tanahashi 1:408f149a1107 158 pc.printf(" lat:\t\t%.6f\r\n", lat);
tanahashi 1:408f149a1107 159 pc.printf(" lon:\t\t%.6f\r\n", lon);
tanahashi 1:408f149a1107 160 pc.printf(" alt:\t\t%.1f[m]\r\n", alt);
tanahashi 1:408f149a1107 161 pc.printf("\r\n");
tanahashi 1:408f149a1107 162 pc.printf(" internal:\tvoltage\t\tcurrent\r\n");
tanahashi 1:408f149a1107 163 pc.printf(" \t%.2f[V]\t\t%.0f[mA]\r\n", voltage_in_V, current_in);
tanahashi 1:408f149a1107 164 pc.printf(" external:\tvoltage\t\tcurrent\r\n");
tanahashi 1:408f149a1107 165 pc.printf(" \t%.2f[V]\t\t%.0f[mA]\r\n", voltage_ex_V, current_ex);
tanahashi 1:408f149a1107 166 pc.printf("\r\n");
tanahashi 1:408f149a1107 167 pc.printf(" press:\t\t%.2f[hPa]\r\n", press);
tanahashi 1:408f149a1107 168 pc.printf(" temp:\t\t%.2f[C]\r\n", temp);
tanahashi 1:408f149a1107 169 pc.printf("\r\n");
tanahashi 1:408f149a1107 170 pc.printf(" accel:\t\tx\t\ty\t\tz\r\n");
tanahashi 1:408f149a1107 171 pc.printf(" \t\t%.2f\t\t%.2f\t\t%.2f\t[G]\r\n", accel[0], accel[1], accel[2]);
tanahashi 1:408f149a1107 172 pc.printf(" gyro:\t\tx\t\ty\t\tz\r\n");
tanahashi 1:408f149a1107 173 pc.printf(" \t\t%.2f\t\t%.2f\t\t%.2f\t[rad/s]\r\n", gyro[0], gyro[1], gyro[2]);
tanahashi 1:408f149a1107 174 pc.printf(" mag:\t\tx\t\ty\t\tz\r\n");
tanahashi 1:408f149a1107 175 pc.printf(" \t\t%.2f\t\t%.2f\t\t%.2f\t[uT]\r\n", mag[0], mag[1], mag[2]);
tanahashi 0:6bf7ee71fb68 176 }