Hybrid_OB2021 / Mbed 2 deprecated Hybrid_OB2021_GROUND

v1

Dependencies:   mbed IM920

main.cpp@1:69472741bd36, 2021-07-29 (annotated)

Committer:
imadaemi
Date:
Thu Jul 29 18:37:48 2021 +0000
Revision:
1:69472741bd36
Parent:
0:f4fb5a0f6981
Child:
2:41238982b498
v2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
imadaemi 0:f4fb5a0f6981 1 #include "mbed.h"
imadaemi 0:f4fb5a0f6981 2 #include "IM920.h"
imadaemi 0:f4fb5a0f6981 3
imadaemi 0:f4fb5a0f6981 4 // ***************************************************
imadaemi 0:f4fb5a0f6981 5 // コンストラクタ
imadaemi 0:f4fb5a0f6981 6 // ***************************************************
imadaemi 0:f4fb5a0f6981 7 Serial pc(USBTX, USBRX, 115200);
imadaemi 0:f4fb5a0f6981 8 Serial im920_serial(D1, D0, 115200);
imadaemi 0:f4fb5a0f6981 9
imadaemi 0:f4fb5a0f6981 10 IM920 im920(im920_serial, pc, 115200);
imadaemi 0:f4fb5a0f6981 11
imadaemi 0:f4fb5a0f6981 12 // ***************************************************
imadaemi 0:f4fb5a0f6981 13 // 関数の宣言
imadaemi 0:f4fb5a0f6981 14 // ***************************************************
imadaemi 0:f4fb5a0f6981 15 void ReadPC();
imadaemi 0:f4fb5a0f6981 16 void ReadSensorSetup();
imadaemi 0:f4fb5a0f6981 17 void ReadData();
imadaemi 0:f4fb5a0f6981 18 void Read0xA5();
imadaemi 0:f4fb5a0f6981 19 void Help();
imadaemi 0:f4fb5a0f6981 20
imadaemi 0:f4fb5a0f6981 21 // ***************************************************
imadaemi 0:f4fb5a0f6981 22 // 変数の宣言
imadaemi 0:f4fb5a0f6981 23 // ***************************************************
imadaemi 1:69472741bd36 24 char nich_status;
imadaemi 1:69472741bd36 25
imadaemi 0:f4fb5a0f6981 26 bool header_set = false;
imadaemi 0:f4fb5a0f6981 27 char im_buf[16];//16なのって意味ある？
imadaemi 0:f4fb5a0f6981 28 int im_buf_n = 0;
imadaemi 0:f4fb5a0f6981 29
imadaemi 0:f4fb5a0f6981 30 float lat, lon, height;
imadaemi 0:f4fb5a0f6981 31 float press, temp, altitude;
imadaemi 0:f4fb5a0f6981 32
imadaemi 0:f4fb5a0f6981 33 float imu[6], mag[3];
imadaemi 0:f4fb5a0f6981 34 float mag_geo[3];
imadaemi 0:f4fb5a0f6981 35
imadaemi 0:f4fb5a0f6981 36 float voltage_main, voltage_sep;
imadaemi 0:f4fb5a0f6981 37 float current_main, current_sep;
imadaemi 0:f4fb5a0f6981 38
imadaemi 0:f4fb5a0f6981 39 // ***************************************************
imadaemi 0:f4fb5a0f6981 40 // 無線のヘッダまとめ
imadaemi 0:f4fb5a0f6981 41 // ***************************************************
imadaemi 0:f4fb5a0f6981 42 const char HEADER_SETUP = 0x01;
imadaemi 0:f4fb5a0f6981 43 // 0xA1
imadaemi 0:f4fb5a0f6981 44
imadaemi 0:f4fb5a0f6981 45 const char HEADER_DATA = 0xA2;
imadaemi 1:69472741bd36 46 // 0xA2,lat,lon,height,press,temp,altitude
imadaemi 1:69472741bd36 47 // 1
imadaemi 0:f4fb5a0f6981 48
imadaemi 0:f4fb5a0f6981 49 const char HEADER_ECHO = 0xA5;
imadaemi 0:f4fb5a0f6981 50 // 0xA5,コマンド
imadaemi 0:f4fb5a0f6981 51 // 1 1
imadaemi 0:f4fb5a0f6981 52
imadaemi 0:f4fb5a0f6981 53 // ***************************************************
imadaemi 0:f4fb5a0f6981 54 // メイン関数
imadaemi 0:f4fb5a0f6981 55 // ***************************************************
imadaemi 0:f4fb5a0f6981 56 int main() {
imadaemi 0:f4fb5a0f6981 57 wait(0.5f);
imadaemi 0:f4fb5a0f6981 58 pc.printf("Hello Ground!\r\n");
imadaemi 0:f4fb5a0f6981 59 pc.attach(&ReadPC, Serial::RxIrq);
imadaemi 0:f4fb5a0f6981 60 im920.attach(&Read0xA5, HEADER_ECHO);
imadaemi 0:f4fb5a0f6981 61 im920.attach(&ReadSensorSetup, HEADER_SETUP);
imadaemi 0:f4fb5a0f6981 62 im920.attach(&ReadData, HEADER_DATA);
imadaemi 0:f4fb5a0f6981 63 }
imadaemi 0:f4fb5a0f6981 64
imadaemi 0:f4fb5a0f6981 65 void ReadPC(){
imadaemi 0:f4fb5a0f6981 66 char c = pc.getc();
imadaemi 0:f4fb5a0f6981 67 if(!header_set){
imadaemi 0:f4fb5a0f6981 68 im920.header((char)0xF0);
imadaemi 0:f4fb5a0f6981 69 header_set = true;
imadaemi 0:f4fb5a0f6981 70 }
imadaemi 0:f4fb5a0f6981 71
imadaemi 0:f4fb5a0f6981 72 switch(c){
imadaemi 0:f4fb5a0f6981 73 case '0':
imadaemi 0:f4fb5a0f6981 74 im920.write((char)0x00);
imadaemi 0:f4fb5a0f6981 75 im_buf[im_buf_n] = '0';
imadaemi 0:f4fb5a0f6981 76 im_buf_n ++;
imadaemi 0:f4fb5a0f6981 77 pc.printf("INPUT : 0\r\n");
imadaemi 0:f4fb5a0f6981 78 break;
imadaemi 0:f4fb5a0f6981 79
imadaemi 0:f4fb5a0f6981 80 case '1':
imadaemi 0:f4fb5a0f6981 81 im920.write((char)0x01);
imadaemi 0:f4fb5a0f6981 82 im_buf[im_buf_n] = '1';
imadaemi 0:f4fb5a0f6981 83 im_buf_n ++;
imadaemi 0:f4fb5a0f6981 84 pc.printf("INPUT : 1\r\n");
imadaemi 0:f4fb5a0f6981 85 break;
imadaemi 0:f4fb5a0f6981 86
imadaemi 0:f4fb5a0f6981 87 case '?':
imadaemi 0:f4fb5a0f6981 88 Help();
imadaemi 0:f4fb5a0f6981 89 break;
imadaemi 0:f4fb5a0f6981 90
imadaemi 0:f4fb5a0f6981 91 case '\n':
imadaemi 0:f4fb5a0f6981 92 if(header_set){
imadaemi 0:f4fb5a0f6981 93 im920.send();
imadaemi 0:f4fb5a0f6981 94 pc.printf("Send : %s\r\n", im_buf);
imadaemi 0:f4fb5a0f6981 95 header_set = false;
imadaemi 0:f4fb5a0f6981 96 for(int i = 0; i < im_buf_n; i ++){
imadaemi 0:f4fb5a0f6981 97 im_buf[i] = '\0';
imadaemi 0:f4fb5a0f6981 98 }
imadaemi 0:f4fb5a0f6981 99 im_buf_n = 0;
imadaemi 0:f4fb5a0f6981 100 }
imadaemi 0:f4fb5a0f6981 101 break;
imadaemi 0:f4fb5a0f6981 102 }
imadaemi 0:f4fb5a0f6981 103 }
imadaemi 0:f4fb5a0f6981 104
imadaemi 0:f4fb5a0f6981 105 void ReadSensorSetup(){
imadaemi 0:f4fb5a0f6981 106 pc.printf("\r\n");
imadaemi 0:f4fb5a0f6981 107 pc.printf("****Sensor Setup****\r\n");
imadaemi 0:f4fb5a0f6981 108
imadaemi 0:f4fb5a0f6981 109 if(im920.data[1] == 1){
imadaemi 0:f4fb5a0f6981 110 pc.printf("GPS : OK\r\n");
imadaemi 0:f4fb5a0f6981 111 }else{
imadaemi 0:f4fb5a0f6981 112 pc.printf("GPS : NG\r\n");
imadaemi 0:f4fb5a0f6981 113 }
imadaemi 0:f4fb5a0f6981 114 if(im920.data[2] == 1){
imadaemi 0:f4fb5a0f6981 115 pc.printf("LPS22HB : OK\r\n");
imadaemi 0:f4fb5a0f6981 116 }else{
imadaemi 0:f4fb5a0f6981 117 pc.printf("LPS22HB : NG\r\n");
imadaemi 0:f4fb5a0f6981 118 }
imadaemi 0:f4fb5a0f6981 119 if(im920.data[3] == 1){
imadaemi 0:f4fb5a0f6981 120 pc.printf("MPU9250 : OK\r\n");
imadaemi 0:f4fb5a0f6981 121 }else{
imadaemi 0:f4fb5a0f6981 122 pc.printf("MPU9250 : NG\r\n");
imadaemi 0:f4fb5a0f6981 123 }
imadaemi 0:f4fb5a0f6981 124 if(im920.data[4] == 1){
imadaemi 0:f4fb5a0f6981 125 pc.printf("MPU9250_MAG : OK\r\n");
imadaemi 0:f4fb5a0f6981 126 }else{
imadaemi 0:f4fb5a0f6981 127 pc.printf("MPU9250_MAG : NG\r\n");
imadaemi 0:f4fb5a0f6981 128 }
imadaemi 0:f4fb5a0f6981 129 if(im920.data[5] == 1){
imadaemi 0:f4fb5a0f6981 130 pc.printf("INA226_MAIN : OK\r\n");
imadaemi 0:f4fb5a0f6981 131 }else{
imadaemi 0:f4fb5a0f6981 132 pc.printf("INA226_MAIN : NG\r\n");
imadaemi 0:f4fb5a0f6981 133 }
imadaemi 0:f4fb5a0f6981 134 if(im920.data[6] == 1){
imadaemi 0:f4fb5a0f6981 135 pc.printf("INA226_SEP : OK\r\n");
imadaemi 0:f4fb5a0f6981 136 }else{
imadaemi 0:f4fb5a0f6981 137 pc.printf("INA226_SEP : NG\r\n");
imadaemi 0:f4fb5a0f6981 138 }
imadaemi 0:f4fb5a0f6981 139
imadaemi 0:f4fb5a0f6981 140 for(int i = 0; i < 20; i++){
imadaemi 0:f4fb5a0f6981 141 pc.printf("*");
imadaemi 0:f4fb5a0f6981 142 }
imadaemi 0:f4fb5a0f6981 143 pc.printf("\r\n");
imadaemi 0:f4fb5a0f6981 144 }
imadaemi 0:f4fb5a0f6981 145
imadaemi 0:f4fb5a0f6981 146 void ReadData(){
imadaemi 1:69472741bd36 147 //pc.printf("***Acquired Data****\r\n");
imadaemi 1:69472741bd36 148 pc.printf("***Recieved Data****\r\n");
imadaemi 0:f4fb5a0f6981 149 int bit = 1;
imadaemi 1:69472741bd36 150 nich_status = im920.data[bit];
imadaemi 1:69472741bd36 151 bit += 1;
imadaemi 1:69472741bd36 152 pc.printf("Nichrome Status : %c\r\n",nich_status);
imadaemi 0:f4fb5a0f6981 153 lat = (float)im920.toFloat(bit);
imadaemi 0:f4fb5a0f6981 154 bit += 4;
imadaemi 1:69472741bd36 155 pc.printf("Latitude : %.7f\r\n",lat);
imadaemi 0:f4fb5a0f6981 156 lon = (float)im920.toFloat(bit);
imadaemi 0:f4fb5a0f6981 157 bit += 4;
imadaemi 1:69472741bd36 158 pc.printf("Longitude : %.7f\r\n",lon);
imadaemi 1:69472741bd36 159 height = (float)im920.toFloat(bit);
imadaemi 0:f4fb5a0f6981 160 bit += 4;
imadaemi 1:69472741bd36 161 pc.printf("Height : %.7f\r\n",height);
imadaemi 0:f4fb5a0f6981 162
imadaemi 0:f4fb5a0f6981 163 press = (float)im920.toFloat(bit);
imadaemi 0:f4fb5a0f6981 164 bit += 4;
imadaemi 1:69472741bd36 165 pc.printf("Pressure : %.4f[hPa]\r\n", press);
imadaemi 0:f4fb5a0f6981 166 temp = (float)im920.toFloat(bit);
imadaemi 0:f4fb5a0f6981 167 bit += 4;
imadaemi 1:69472741bd36 168 pc.printf("Temperarure : %.2f[℃]\r\n", temp);
imadaemi 0:f4fb5a0f6981 169 altitude = (float)im920.toFloat(bit);
imadaemi 0:f4fb5a0f6981 170 bit += 4;
imadaemi 1:69472741bd36 171 pc.printf("Altitude : %.2f[m]\r\n", altitude);
imadaemi 0:f4fb5a0f6981 172
imadaemi 0:f4fb5a0f6981 173 voltage_main = (float)im920.toFloat(bit);
imadaemi 0:f4fb5a0f6981 174 bit += 4;
imadaemi 1:69472741bd36 175 pc.printf("Voltage Main : %.2f\r\n",voltage_main);
imadaemi 0:f4fb5a0f6981 176 current_main = (float)im920.toFloat(bit);
imadaemi 0:f4fb5a0f6981 177 bit += 4;
imadaemi 1:69472741bd36 178 pc.printf("Crrent Main : %.2f\r\n",current_main);
imadaemi 0:f4fb5a0f6981 179 voltage_sep = (float)im920.toFloat(bit);
imadaemi 0:f4fb5a0f6981 180 bit += 4;
imadaemi 1:69472741bd36 181 pc.printf("Voltage Sep : %.2f\r\n",voltage_sep);
imadaemi 0:f4fb5a0f6981 182 current_sep = (float)im920.toFloat(bit);
imadaemi 0:f4fb5a0f6981 183 bit += 4;
imadaemi 1:69472741bd36 184 pc.printf("Current Sep : %.2f\r\n",current_sep);
imadaemi 1:69472741bd36 185
imadaemi 1:69472741bd36 186
imadaemi 0:f4fb5a0f6981 187
imadaemi 0:f4fb5a0f6981 188 for(int i = 0; i < 20; i++){
imadaemi 0:f4fb5a0f6981 189 pc.printf("*");
imadaemi 0:f4fb5a0f6981 190 }
imadaemi 0:f4fb5a0f6981 191 pc.printf("\r\n\r\n");
imadaemi 0:f4fb5a0f6981 192 }
imadaemi 0:f4fb5a0f6981 193
imadaemi 0:f4fb5a0f6981 194 void Read0xA5(){
imadaemi 0:f4fb5a0f6981 195 for(int i = 0; i < 20; i++){
imadaemi 0:f4fb5a0f6981 196 pc.printf("*");
imadaemi 0:f4fb5a0f6981 197 }
imadaemi 0:f4fb5a0f6981 198 pc.printf("\r\nIM920 ECHO : ");
imadaemi 0:f4fb5a0f6981 199
imadaemi 0:f4fb5a0f6981 200 switch(im920.data[1]){
imadaemi 0:f4fb5a0f6981 201 case 0x01:
imadaemi 0:f4fb5a0f6981 202 pc.printf("SEPARATE\r\n");
imadaemi 0:f4fb5a0f6981 203 break;
imadaemi 0:f4fb5a0f6981 204
imadaemi 0:f4fb5a0f6981 205 case 0x00:
imadaemi 0:f4fb5a0f6981 206 pc.printf("STOP SEPARATE\r\n");
imadaemi 0:f4fb5a0f6981 207 break;
imadaemi 0:f4fb5a0f6981 208 }
imadaemi 0:f4fb5a0f6981 209
imadaemi 0:f4fb5a0f6981 210 for(int i = 0; i < 20; i++){
imadaemi 0:f4fb5a0f6981 211 pc.printf("*");
imadaemi 0:f4fb5a0f6981 212 }
imadaemi 0:f4fb5a0f6981 213 pc.printf("\r\n");
imadaemi 0:f4fb5a0f6981 214 }
imadaemi 0:f4fb5a0f6981 215
imadaemi 0:f4fb5a0f6981 216 void Help(){
imadaemi 0:f4fb5a0f6981 217 pc.printf("\r\n");
imadaemi 0:f4fb5a0f6981 218 pc.printf("**I can help you!***\r\n");
imadaemi 0:f4fb5a0f6981 219 pc.printf("0 : Stop Separate\r\n");
imadaemi 0:f4fb5a0f6981 220 pc.printf("1 : Separate\r\n");
imadaemi 0:f4fb5a0f6981 221 pc.printf("? : Help\r\n");
imadaemi 0:f4fb5a0f6981 222 pc.printf("********************\r\n\r\n");
imadaemi 0:f4fb5a0f6981 223 }