Hybrid_OB2021 / Mbed 2 deprecated Hybrid_OB2021_GROUND

v1

Dependencies:   mbed IM920

main.cpp@0:f4fb5a0f6981, 2021-07-29 (annotated)

Committer:
imadaemi
Date:
Thu Jul 29 17:03:31 2021 +0000
Revision:
0:f4fb5a0f6981
Child:
1:69472741bd36
v1

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