main.cpp

00001 #include "mbed.h"
00002 
00003 #include "PQES920LR.h"
00004 
00005 #define TIME_LSB 54.931640625
00006 #define VOLTAGE_LSB 1.25
00007 #define CURRENT_LSB 1.0986328125
00008 #define PRESS_LSB 0.0384521484375
00009 #define TEMP_LSB 0.002593994140625
00010 #define ACCEL_LSB 0.00048828125
00011 #define GYRO_LSB 0.06103515625
00012 #define MAG_LSB 0.146484375
00013 
00014 Serial pc(USBTX, USBRX, 115200);
00015 Serial es_serial(D1, D0, 115200);
00016 
00017 ES920LR es(es_serial);
00018 
00019 void prompt();
00020 
00021 void downlink_handler(char* data);
00022 
00023 char *phase_names[] =  {"SAFETY", "READY", "FLIGHT", "SEP", "EMERGENCY", "RECOVERY"};
00024 
00025 char mission_timer_reset;
00026 int mission_time;
00027 int flight_time;
00028 char f_sd;
00029 char f_gps;
00030 char f_adxl;
00031 char f_ina_in;
00032 char f_ina_ex;
00033 char f_lps;
00034 char f_mpu;
00035 char f_mpu_ak;
00036 char phase;
00037 float lat;
00038 float lon;
00039 float alt;
00040 float voltage_in;
00041 float current_in;
00042 float voltage_ex;
00043 float current_ex;
00044 float press;
00045 float temp;
00046 float accel[3];
00047 float gyro[3];
00048 float mag[3];
00049 
00050 short mission_time_bits;
00051 short flight_time_bits;
00052 short voltage_in_bits;
00053 short current_in_bits;
00054 short voltage_ex_bits;
00055 short current_ex_bits;
00056 short press_bits;
00057 short temp_bits;
00058 short accel_bits[3];
00059 short gyro_bits[3];
00060 short mag_bits[3];
00061 
00062 int mission_time_s;
00063 int mission_hour, mission_min, mission_sec;
00064 float voltage_in_V, voltage_ex_V;
00065 
00066 int main()
00067 {
00068     es.attach(downlink_handler);
00069     pc.printf("PLANET-Q GROUND STATION\r\n");
00070 
00071     while(1) {
00072         if(pc.readable()) {
00073             char command[1];
00074             command[0] = pc.getc() + 0x80;
00075             es.send(command, 1);
00076             pc.printf("send:\t%02x\r\n", command[0]);
00077         }
00078     }
00079 }
00080 
00081 void downlink_handler(char *data)
00082 {
00083     mission_timer_reset = data[1];
00084     mission_time_bits = (short)(data[2] | data[3] << 8);
00085     flight_time_bits = (short)(data[4] | data[5] << 8);
00086     f_sd = data[6]     >> 7 & 1;
00087     f_gps = data[6]    >> 6 & 1;
00088     f_adxl = data[6]   >> 5 & 1;
00089     f_ina_in = data[6] >> 4 & 1;
00090     f_ina_ex = data[6] >> 3 & 1;
00091     f_lps = data[6]    >> 2 & 1;
00092     f_mpu = data[6]    >> 1 & 1;
00093     f_mpu_ak = data[6]      & 1;
00094     phase = data[7];
00095     lat = *(float*)&data[8];
00096     lon = *(float*)&data[12];
00097     alt = *(float*)&data[16];
00098     voltage_in_bits = (short)(data[20] | data[21] << 8);
00099     current_in_bits = (short)(data[22] | data[23] << 8);
00100     voltage_ex_bits = (short)(data[24] | data[25] << 8);
00101     current_ex_bits = (short)(data[26] | data[27] << 8);
00102     press_bits = (short)(data[28] | data[29] << 8);
00103     temp_bits = (short)(data[30] | data[31] << 8);
00104     accel_bits[0] = (short)(data[32] | data[33] << 8);
00105     accel_bits[1] = (short)(data[34] | data[35] << 8);
00106     accel_bits[2] = (short)(data[36] | data[37] << 8);
00107     gyro_bits[0] = (short)(data[38] | data[39] << 8);
00108     gyro_bits[1] = (short)(data[40] | data[41] << 8);
00109     gyro_bits[2] = (short)(data[42] | data[43] << 8);
00110     mag_bits[0] = (short)(data[44] | data[45] << 8);
00111     mag_bits[1] = (short)(data[46] | data[47] << 8);
00112     mag_bits[2] = (short)(data[48] | data[49] << 8);
00113 
00114     mission_time = mission_time_bits * TIME_LSB;
00115     flight_time = flight_time_bits * TIME_LSB;
00116     voltage_in = voltage_in_bits * VOLTAGE_LSB;
00117     current_in = current_in_bits * CURRENT_LSB;
00118     voltage_ex = voltage_ex_bits * VOLTAGE_LSB;
00119     current_ex = current_ex_bits * CURRENT_LSB;
00120     press = press_bits * PRESS_LSB;
00121     temp = temp_bits * TEMP_LSB;
00122     for(int i = 0; i < 3; i++) {
00123         accel[i] = accel_bits[i] * ACCEL_LSB;
00124     }
00125     for(int i = 0; i < 3; i++) {
00126         gyro[i] = gyro_bits[i] * GYRO_LSB;
00127     }
00128     for(int i = 0; i < 3; i++) {
00129         mag[i] = mag_bits[i] * MAG_LSB;
00130     }
00131     
00132     mission_time_s = (mission_time / 1000) + (mission_timer_reset * 30*60);
00133     mission_hour = mission_time_s / 3600;
00134     mission_time_s %= 3600;
00135     mission_min = mission_time_s / 60;
00136     mission_time_s %= 60;
00137     mission_sec = mission_time_s;
00138     
00139     voltage_in_V = voltage_in / 1000.0f;
00140     voltage_ex_V = voltage_ex / 1000.0f;
00141     
00142     pc.printf(" PLANET-Q GROUND STATION\r\n");
00143     pc.printf(" commands:\r\n"); 
00144     pc.printf(" 0   : SAFETY\r\n");
00145     pc.printf(" 1   : WAIT\r\n");
00146     pc.printf(" 2   : FLIGHT\r\n");
00147     pc.printf(" 3   : SEP\r\n");
00148     pc.printf(" 4   : EMERGENCY\r\n");
00149     pc.printf(" 5   : RECOVERY\r\n");
00150     pc.printf(" DEL : SYSTEM RESET\r\n");
00151     pc.printf("\r\n");
00152     pc.printf(" mission time:\t%02d:%02d:%02d\r\n", mission_hour, mission_min, mission_sec);
00153     pc.printf(" flight time:\t%d[ms]\r\n", flight_time);
00154     pc.printf(" phase:\t%s\r\n", phase_names[phase]);
00155     pc.printf(" flags:\tSD\tGPS\tINA_in\tINA_ex\tADXL\tLPS\tMPU\tMPU_AK\r\n");
00156     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);
00157     pc.printf("\r\n");
00158     pc.printf(" lat:\t\t%.6f\r\n", lat);
00159     pc.printf(" lon:\t\t%.6f\r\n", lon);
00160     pc.printf(" alt:\t\t%.1f[m]\r\n", alt);
00161     pc.printf("\r\n");
00162     pc.printf(" internal:\tvoltage\t\tcurrent\r\n");
00163     pc.printf("          \t%.2f[V]\t\t%.0f[mA]\r\n", voltage_in_V, current_in);
00164     pc.printf(" external:\tvoltage\t\tcurrent\r\n");
00165     pc.printf("          \t%.2f[V]\t\t%.0f[mA]\r\n", voltage_ex_V, current_ex);
00166     pc.printf("\r\n");
00167     pc.printf(" press:\t\t%.2f[hPa]\r\n", press);
00168     pc.printf(" temp:\t\t%.2f[C]\r\n", temp);
00169     pc.printf("\r\n");
00170     pc.printf(" accel:\t\tx\t\ty\t\tz\r\n");
00171     pc.printf("       \t\t%.2f\t\t%.2f\t\t%.2f\t[G]\r\n", accel[0], accel[1], accel[2]);
00172     pc.printf(" gyro:\t\tx\t\ty\t\tz\r\n");
00173     pc.printf("       \t\t%.2f\t\t%.2f\t\t%.2f\t[rad/s]\r\n", gyro[0], gyro[1], gyro[2]);
00174     pc.printf(" mag:\t\tx\t\ty\t\tz\r\n");
00175     pc.printf("       \t\t%.2f\t\t%.2f\t\t%.2f\t[uT]\r\n", mag[0], mag[1], mag[2]);
00176 }