v1
Dependencies: Nichrome_lib mbed mpu9250_i2c IM920 INA226_lib PQLPS22HB EEPROM_lib GPS_interrupt
main.cpp@1:3151936d9c55, 2021-07-29 (annotated)
- Committer:
- imadaemi
- Date:
- Thu Jul 29 18:37:34 2021 +0000
- Revision:
- 1:3151936d9c55
- Parent:
- 0:4e38f8b1c183
- Child:
- 2:980edad0eea2
v2
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
imadaemi | 0:4e38f8b1c183 | 1 | #include "mbed.h" |
imadaemi | 0:4e38f8b1c183 | 2 | #include "IM920.h" |
imadaemi | 0:4e38f8b1c183 | 3 | #include "GPS_interrupt.h" |
imadaemi | 0:4e38f8b1c183 | 4 | #include "PQLPS22HB.h" |
imadaemi | 0:4e38f8b1c183 | 5 | #include "mpu9250_i2c.h" |
imadaemi | 0:4e38f8b1c183 | 6 | #include "INA226.h" |
imadaemi | 0:4e38f8b1c183 | 7 | #include "Nichrome_lib.h" |
imadaemi | 0:4e38f8b1c183 | 8 | #include "EEPROM_lib.h" |
imadaemi | 0:4e38f8b1c183 | 9 | |
imadaemi | 1:3151936d9c55 | 10 | #define FIRE_TIME 3.0 |
imadaemi | 0:4e38f8b1c183 | 11 | #define CURRENT_LOCATION_PRESS 1022.624268//投下前に設定 |
imadaemi | 0:4e38f8b1c183 | 12 | #define CURRENT_LOCATION_TEMP 10.650000//投下前に設定 |
imadaemi | 0:4e38f8b1c183 | 13 | #define ACC_RANGE _16G |
imadaemi | 0:4e38f8b1c183 | 14 | #define GYRO_RANGE _2000DPS |
imadaemi | 0:4e38f8b1c183 | 15 | |
imadaemi | 0:4e38f8b1c183 | 16 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 17 | // コンストラクタ |
imadaemi | 0:4e38f8b1c183 | 18 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 19 | Serial pc(USBTX, USBRX, 115200); |
imadaemi | 0:4e38f8b1c183 | 20 | Serial gps_serial(p9, p10, 115200); |
imadaemi | 0:4e38f8b1c183 | 21 | Serial im920_serial(p13, p14, 115200); |
imadaemi | 0:4e38f8b1c183 | 22 | |
imadaemi | 0:4e38f8b1c183 | 23 | I2C i2c(p28, p27); |
imadaemi | 0:4e38f8b1c183 | 24 | |
imadaemi | 0:4e38f8b1c183 | 25 | IM920 im920(im920_serial, pc, 115200); |
imadaemi | 0:4e38f8b1c183 | 26 | GPS_interrupt gps(&gps_serial); |
imadaemi | 0:4e38f8b1c183 | 27 | LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW); |
imadaemi | 0:4e38f8b1c183 | 28 | mpu9250 mpu9250(i2c,AD0_HIGH); |
imadaemi | 0:4e38f8b1c183 | 29 | myINA226 ina226_main(i2c, myINA226::A1_GND, myINA226::A0_GND); |
imadaemi | 0:4e38f8b1c183 | 30 | myINA226 ina226_sep(i2c, myINA226::A1_VDD, myINA226::A0_VDD); |
imadaemi | 0:4e38f8b1c183 | 31 | Nichrome_lib nich(p20); |
imadaemi | 0:4e38f8b1c183 | 32 | EEPROM_lib EEPROM(i2c, 1); |
imadaemi | 0:4e38f8b1c183 | 33 | |
imadaemi | 0:4e38f8b1c183 | 34 | DigitalOut pinA(p21); |
imadaemi | 0:4e38f8b1c183 | 35 | DigitalOut pinB(p22); |
imadaemi | 0:4e38f8b1c183 | 36 | DigitalOut pinC(p23); |
imadaemi | 0:4e38f8b1c183 | 37 | |
imadaemi | 1:3151936d9c55 | 38 | Ticker send_data; |
imadaemi | 1:3151936d9c55 | 39 | Timeout fire_time; |
imadaemi | 0:4e38f8b1c183 | 40 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 41 | // 関数の宣言 |
imadaemi | 0:4e38f8b1c183 | 42 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 43 | void uplink(); |
imadaemi | 0:4e38f8b1c183 | 44 | void echoIM(); |
imadaemi | 0:4e38f8b1c183 | 45 | void Separate(); |
imadaemi | 0:4e38f8b1c183 | 46 | void StopSeparate(); |
imadaemi | 0:4e38f8b1c183 | 47 | void SetSensor(); |
imadaemi | 0:4e38f8b1c183 | 48 | void GetData(); |
imadaemi | 0:4e38f8b1c183 | 49 | void WriteEEPROM(); |
imadaemi | 0:4e38f8b1c183 | 50 | void setEEPROMGroup(int group_num); |
imadaemi | 0:4e38f8b1c183 | 51 | |
imadaemi | 0:4e38f8b1c183 | 52 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 53 | // 無線のヘッダまとめ |
imadaemi | 0:4e38f8b1c183 | 54 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 55 | const char HEADER_SETUP = 0x01; |
imadaemi | 0:4e38f8b1c183 | 56 | // 0xA1 |
imadaemi | 0:4e38f8b1c183 | 57 | |
imadaemi | 0:4e38f8b1c183 | 58 | const char HEADER_DATA = 0xA2; |
imadaemi | 0:4e38f8b1c183 | 59 | // 0xA2 |
imadaemi | 0:4e38f8b1c183 | 60 | // |
imadaemi | 0:4e38f8b1c183 | 61 | |
imadaemi | 0:4e38f8b1c183 | 62 | const char HEADER_ECHO = 0xA5; |
imadaemi | 0:4e38f8b1c183 | 63 | // 0xA5,コマンド |
imadaemi | 0:4e38f8b1c183 | 64 | // 1 1 |
imadaemi | 0:4e38f8b1c183 | 65 | |
imadaemi | 0:4e38f8b1c183 | 66 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 67 | // 変数の宣言 |
imadaemi | 0:4e38f8b1c183 | 68 | // *************************************************** |
imadaemi | 1:3151936d9c55 | 69 | char nich_status; |
imadaemi | 1:3151936d9c55 | 70 | |
imadaemi | 0:4e38f8b1c183 | 71 | bool header_set = false; |
imadaemi | 0:4e38f8b1c183 | 72 | char im_buf[55];//16なのって意味ある? |
imadaemi | 0:4e38f8b1c183 | 73 | int im_buf_n = 0; |
imadaemi | 0:4e38f8b1c183 | 74 | |
imadaemi | 0:4e38f8b1c183 | 75 | float lat, lon, height; |
imadaemi | 0:4e38f8b1c183 | 76 | float press, temp, altitude; |
imadaemi | 0:4e38f8b1c183 | 77 | |
imadaemi | 0:4e38f8b1c183 | 78 | float imu[6], mag[3]; |
imadaemi | 0:4e38f8b1c183 | 79 | float mag_geo[3]; |
imadaemi | 0:4e38f8b1c183 | 80 | |
imadaemi | 0:4e38f8b1c183 | 81 | float voltage_main, voltage_sep; |
imadaemi | 0:4e38f8b1c183 | 82 | float current_main, current_sep; |
imadaemi | 0:4e38f8b1c183 | 83 | |
imadaemi | 0:4e38f8b1c183 | 84 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 85 | // メイン関数 |
imadaemi | 0:4e38f8b1c183 | 86 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 87 | int main() { |
imadaemi | 0:4e38f8b1c183 | 88 | pc.printf("Hello Main!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 89 | SetSensor(); |
imadaemi | 0:4e38f8b1c183 | 90 | im920.attach(&uplink, 0xF0); |
imadaemi | 1:3151936d9c55 | 91 | send_data.attach(&GetData, 1.0); |
imadaemi | 0:4e38f8b1c183 | 92 | |
imadaemi | 0:4e38f8b1c183 | 93 | while(1){ |
imadaemi | 0:4e38f8b1c183 | 94 | } |
imadaemi | 0:4e38f8b1c183 | 95 | } |
imadaemi | 0:4e38f8b1c183 | 96 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 97 | // アップリンク(地上局から) |
imadaemi | 0:4e38f8b1c183 | 98 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 99 | void uplink(){ |
imadaemi | 0:4e38f8b1c183 | 100 | echoIM(); |
imadaemi | 0:4e38f8b1c183 | 101 | switch(im920.data[1]){ |
imadaemi | 0:4e38f8b1c183 | 102 | case 0x01: |
imadaemi | 0:4e38f8b1c183 | 103 | pc.printf("********************\r\n"); |
imadaemi | 0:4e38f8b1c183 | 104 | pc.printf("SEPARATE\r\n"); |
imadaemi | 0:4e38f8b1c183 | 105 | pc.printf("********************\r\n\r\n"); |
imadaemi | 0:4e38f8b1c183 | 106 | Separate(); |
imadaemi | 0:4e38f8b1c183 | 107 | break; |
imadaemi | 0:4e38f8b1c183 | 108 | |
imadaemi | 0:4e38f8b1c183 | 109 | case 0x00: |
imadaemi | 0:4e38f8b1c183 | 110 | pc.printf("********************\r\n"); |
imadaemi | 0:4e38f8b1c183 | 111 | pc.printf("STOP SEPARATE\r\n"); |
imadaemi | 0:4e38f8b1c183 | 112 | pc.printf("********************\r\n\r\n"); |
imadaemi | 0:4e38f8b1c183 | 113 | StopSeparate(); |
imadaemi | 0:4e38f8b1c183 | 114 | break; |
imadaemi | 0:4e38f8b1c183 | 115 | } |
imadaemi | 0:4e38f8b1c183 | 116 | } |
imadaemi | 0:4e38f8b1c183 | 117 | |
imadaemi | 0:4e38f8b1c183 | 118 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 119 | // 無線信号の送り返し |
imadaemi | 0:4e38f8b1c183 | 120 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 121 | void echoIM(){ |
imadaemi | 0:4e38f8b1c183 | 122 | im920.header(HEADER_ECHO); |
imadaemi | 0:4e38f8b1c183 | 123 | im920.write(im920.data[1]); |
imadaemi | 0:4e38f8b1c183 | 124 | im920.send(); |
imadaemi | 0:4e38f8b1c183 | 125 | } |
imadaemi | 0:4e38f8b1c183 | 126 | |
imadaemi | 0:4e38f8b1c183 | 127 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 128 | // 分離実行 |
imadaemi | 0:4e38f8b1c183 | 129 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 130 | void Separate(){ |
imadaemi | 1:3151936d9c55 | 131 | nich.fire_on(); |
imadaemi | 1:3151936d9c55 | 132 | fire_time.attach(&StopSeparate,FIRE_TIME); |
imadaemi | 0:4e38f8b1c183 | 133 | } |
imadaemi | 0:4e38f8b1c183 | 134 | |
imadaemi | 0:4e38f8b1c183 | 135 | void StopSeparate(){ |
imadaemi | 0:4e38f8b1c183 | 136 | nich.fire_off(); |
imadaemi | 0:4e38f8b1c183 | 137 | } |
imadaemi | 0:4e38f8b1c183 | 138 | |
imadaemi | 0:4e38f8b1c183 | 139 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 140 | // センサーのセットアップ |
imadaemi | 0:4e38f8b1c183 | 141 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 142 | void SetSensor(){ |
imadaemi | 0:4e38f8b1c183 | 143 | pc.printf("\r\n"); |
imadaemi | 0:4e38f8b1c183 | 144 | for(int i = 0; i < 20; i++){ |
imadaemi | 0:4e38f8b1c183 | 145 | pc.printf("*"); |
imadaemi | 0:4e38f8b1c183 | 146 | } |
imadaemi | 0:4e38f8b1c183 | 147 | pc.printf("\r\n"); |
imadaemi | 0:4e38f8b1c183 | 148 | pc.printf("Start Setting\r\n"); |
imadaemi | 0:4e38f8b1c183 | 149 | |
imadaemi | 0:4e38f8b1c183 | 150 | if(!header_set){ |
imadaemi | 0:4e38f8b1c183 | 151 | im920.header((char)HEADER_SETUP); |
imadaemi | 0:4e38f8b1c183 | 152 | header_set = true; |
imadaemi | 0:4e38f8b1c183 | 153 | } |
imadaemi | 0:4e38f8b1c183 | 154 | |
imadaemi | 0:4e38f8b1c183 | 155 | //GPS |
imadaemi | 0:4e38f8b1c183 | 156 | if(gps.gps_readable == true){ |
imadaemi | 0:4e38f8b1c183 | 157 | pc.printf("GPS : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 158 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 159 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 160 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 161 | }else{ |
imadaemi | 0:4e38f8b1c183 | 162 | pc.printf("GPS : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 163 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 164 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 165 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 166 | } |
imadaemi | 0:4e38f8b1c183 | 167 | |
imadaemi | 0:4e38f8b1c183 | 168 | //LPS22HB |
imadaemi | 0:4e38f8b1c183 | 169 | lps22hb.begin(); |
imadaemi | 0:4e38f8b1c183 | 170 | if(lps22hb.test() == true){ |
imadaemi | 0:4e38f8b1c183 | 171 | pc.printf("LPS22HB : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 172 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 173 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 174 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 175 | }else{ |
imadaemi | 0:4e38f8b1c183 | 176 | pc.printf("LPS22HB : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 177 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 178 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 179 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 180 | } |
imadaemi | 0:4e38f8b1c183 | 181 | |
imadaemi | 0:4e38f8b1c183 | 182 | //MPU9250 |
imadaemi | 0:4e38f8b1c183 | 183 | if(mpu9250.sensorTest() == true){ |
imadaemi | 0:4e38f8b1c183 | 184 | pc.printf("MPU9250 : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 185 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 186 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 187 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 188 | }else{ |
imadaemi | 0:4e38f8b1c183 | 189 | pc.printf("MPU9250 : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 190 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 191 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 192 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 193 | } |
imadaemi | 0:4e38f8b1c183 | 194 | if(mpu9250.mag_sensorTest() == true){ |
imadaemi | 0:4e38f8b1c183 | 195 | pc.printf("MPU9250 MAG : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 196 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 197 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 198 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 199 | }else{ |
imadaemi | 0:4e38f8b1c183 | 200 | pc.printf("MPU9250 MAG : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 201 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 202 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 203 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 204 | } |
imadaemi | 0:4e38f8b1c183 | 205 | mpu9250.setAcc(ACC_RANGE); |
imadaemi | 0:4e38f8b1c183 | 206 | mpu9250.setGyro(GYRO_RANGE); |
imadaemi | 0:4e38f8b1c183 | 207 | mpu9250.setOffset(0.528892327, -0.660206211, 0.757105891, -0.011691362, 0.025688783, 1.087885322, -159.750004, 121.425005, -392.700012); |
imadaemi | 0:4e38f8b1c183 | 208 | |
imadaemi | 0:4e38f8b1c183 | 209 | //INA226 |
imadaemi | 0:4e38f8b1c183 | 210 | ina226_main.set_callibretion(); |
imadaemi | 0:4e38f8b1c183 | 211 | ina226_sep.set_callibretion(); |
imadaemi | 1:3151936d9c55 | 212 | //ina226_main.setup(1); |
imadaemi | 1:3151936d9c55 | 213 | //ina226_sep.setup(1); |
imadaemi | 0:4e38f8b1c183 | 214 | |
imadaemi | 0:4e38f8b1c183 | 215 | if(ina226_main.Connection_check()==0){ |
imadaemi | 0:4e38f8b1c183 | 216 | pc.printf("INA226 Main : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 217 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 218 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 219 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 220 | }else{ |
imadaemi | 0:4e38f8b1c183 | 221 | pc.printf("INA226 Main : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 222 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 223 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 224 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 225 | } |
imadaemi | 0:4e38f8b1c183 | 226 | if(ina226_sep.Connection_check()==0){ |
imadaemi | 0:4e38f8b1c183 | 227 | pc.printf("INA226 Sep : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 228 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 229 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 230 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 231 | }else{ |
imadaemi | 0:4e38f8b1c183 | 232 | pc.printf("INA226 Sep : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 233 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 234 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 235 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 236 | } |
imadaemi | 0:4e38f8b1c183 | 237 | |
imadaemi | 0:4e38f8b1c183 | 238 | if(header_set){ |
imadaemi | 0:4e38f8b1c183 | 239 | im920.send(); |
imadaemi | 0:4e38f8b1c183 | 240 | pc.printf("Send : %s\r\n", im_buf); |
imadaemi | 0:4e38f8b1c183 | 241 | header_set = false; |
imadaemi | 0:4e38f8b1c183 | 242 | for(int i = 0; i < im_buf_n; i ++){ |
imadaemi | 0:4e38f8b1c183 | 243 | im_buf[i] = '\0'; |
imadaemi | 0:4e38f8b1c183 | 244 | } |
imadaemi | 0:4e38f8b1c183 | 245 | im_buf_n = 0; |
imadaemi | 0:4e38f8b1c183 | 246 | } |
imadaemi | 0:4e38f8b1c183 | 247 | |
imadaemi | 0:4e38f8b1c183 | 248 | pc.printf("\r\n"); |
imadaemi | 0:4e38f8b1c183 | 249 | for(int i = 0; i < 20; i++){ |
imadaemi | 0:4e38f8b1c183 | 250 | pc.printf("*"); |
imadaemi | 0:4e38f8b1c183 | 251 | } |
imadaemi | 0:4e38f8b1c183 | 252 | pc.printf("\r\n"); |
imadaemi | 0:4e38f8b1c183 | 253 | } |
imadaemi | 0:4e38f8b1c183 | 254 | |
imadaemi | 0:4e38f8b1c183 | 255 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 256 | // センサーのデータ取得 |
imadaemi | 0:4e38f8b1c183 | 257 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 258 | void GetData(){ |
imadaemi | 0:4e38f8b1c183 | 259 | if(!header_set){ |
imadaemi | 0:4e38f8b1c183 | 260 | im920.header((char)HEADER_DATA); |
imadaemi | 0:4e38f8b1c183 | 261 | header_set = true; |
imadaemi | 0:4e38f8b1c183 | 262 | } |
imadaemi | 0:4e38f8b1c183 | 263 | |
imadaemi | 1:3151936d9c55 | 264 | pc.printf("********************\r\n\r\n"); |
imadaemi | 1:3151936d9c55 | 265 | //Nichrome |
imadaemi | 1:3151936d9c55 | 266 | if(nich.status){ |
imadaemi | 1:3151936d9c55 | 267 | nich_status = '1'; |
imadaemi | 1:3151936d9c55 | 268 | }else{ |
imadaemi | 1:3151936d9c55 | 269 | nich_status = '0'; |
imadaemi | 1:3151936d9c55 | 270 | } |
imadaemi | 1:3151936d9c55 | 271 | pc.printf("Nichrome_status : %c\r\n",nich_status); |
imadaemi | 1:3151936d9c55 | 272 | im920.write((char)nich_status); |
imadaemi | 1:3151936d9c55 | 273 | im_buf_n ++; |
imadaemi | 1:3151936d9c55 | 274 | |
imadaemi | 0:4e38f8b1c183 | 275 | //GPS |
imadaemi | 0:4e38f8b1c183 | 276 | lat = gps.Latitude(); |
imadaemi | 0:4e38f8b1c183 | 277 | lon = gps.Longitude(); |
imadaemi | 0:4e38f8b1c183 | 278 | height = gps.Height(); |
imadaemi | 1:3151936d9c55 | 279 | pc.printf("Latitude : %f\r\n",lat); |
imadaemi | 1:3151936d9c55 | 280 | pc.printf("Longitude : %f\r\n",lon); |
imadaemi | 1:3151936d9c55 | 281 | pc.printf("Height : %f\r\n",height); |
imadaemi | 1:3151936d9c55 | 282 | //pc.printf("%f\t%f\t%f\t\r\n", lat, lon, height); |
imadaemi | 0:4e38f8b1c183 | 283 | im920.write((float)lat); |
imadaemi | 0:4e38f8b1c183 | 284 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 285 | im920.write((float)lon); |
imadaemi | 0:4e38f8b1c183 | 286 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 287 | im920.write((float)height); |
imadaemi | 0:4e38f8b1c183 | 288 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 289 | |
imadaemi | 0:4e38f8b1c183 | 290 | //LPS22HB |
imadaemi | 0:4e38f8b1c183 | 291 | lps22hb.read_press(&press); |
imadaemi | 0:4e38f8b1c183 | 292 | lps22hb.read_temp(&temp); |
imadaemi | 0:4e38f8b1c183 | 293 | altitude = (CURRENT_LOCATION_TEMP + 273.15)/0.0065*(1 - powf(press/CURRENT_LOCATION_PRESS, 287 * 0.0065/9.80665)); |
imadaemi | 1:3151936d9c55 | 294 | pc.printf("Pressure : %f\r\n",press); |
imadaemi | 1:3151936d9c55 | 295 | pc.printf("Temperarure : %f\r\n",temp); |
imadaemi | 1:3151936d9c55 | 296 | pc.printf("Altitude : %f\r\n",altitude); |
imadaemi | 1:3151936d9c55 | 297 | //pc.printf("%f\t%f\t%f\r\n",press, temp, altitude); |
imadaemi | 0:4e38f8b1c183 | 298 | im920.write((float)press); |
imadaemi | 0:4e38f8b1c183 | 299 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 300 | im920.write((float)temp); |
imadaemi | 0:4e38f8b1c183 | 301 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 302 | im920.write((float)altitude); |
imadaemi | 0:4e38f8b1c183 | 303 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 304 | |
imadaemi | 0:4e38f8b1c183 | 305 | //MPU9250 |
imadaemi | 0:4e38f8b1c183 | 306 | /* |
imadaemi | 0:4e38f8b1c183 | 307 | mpu9250.getAll(imu, mag); |
imadaemi | 0:4e38f8b1c183 | 308 | pc.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]); |
imadaemi | 0:4e38f8b1c183 | 309 | |
imadaemi | 0:4e38f8b1c183 | 310 | im920.write((float)imu[0]); |
imadaemi | 0:4e38f8b1c183 | 311 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 312 | im920.write((float)imu[1]); |
imadaemi | 0:4e38f8b1c183 | 313 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 314 | im920.write((float)imu[2]); |
imadaemi | 0:4e38f8b1c183 | 315 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 316 | im920.write((float)imu[3]); |
imadaemi | 0:4e38f8b1c183 | 317 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 318 | im920.write((float)imu[4]); |
imadaemi | 0:4e38f8b1c183 | 319 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 320 | im920.write((float)imu[5]); |
imadaemi | 0:4e38f8b1c183 | 321 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 322 | im920.write((float)mag[0]); |
imadaemi | 0:4e38f8b1c183 | 323 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 324 | im920.write((float)mag[1]); |
imadaemi | 0:4e38f8b1c183 | 325 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 326 | im920.write((float)mag[2]); |
imadaemi | 0:4e38f8b1c183 | 327 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 328 | */ |
imadaemi | 0:4e38f8b1c183 | 329 | //INA226 |
imadaemi | 0:4e38f8b1c183 | 330 | ina226_main.get_Voltage_current(&voltage_main, ¤t_main); |
imadaemi | 0:4e38f8b1c183 | 331 | ina226_sep.get_Voltage_current(&voltage_sep, ¤t_sep); |
imadaemi | 1:3151936d9c55 | 332 | voltage_main /= 1000; |
imadaemi | 1:3151936d9c55 | 333 | current_main /= 1000; |
imadaemi | 1:3151936d9c55 | 334 | voltage_sep /= 1000; |
imadaemi | 1:3151936d9c55 | 335 | current_sep /= 1000; |
imadaemi | 1:3151936d9c55 | 336 | pc.printf("Vlotage Mian : %.2f\r\n",voltage_main); |
imadaemi | 1:3151936d9c55 | 337 | pc.printf("Current Main : %.2f\r\n",current_main); |
imadaemi | 1:3151936d9c55 | 338 | pc.printf("Voltage Sep : %.2f\r\n",voltage_sep); |
imadaemi | 1:3151936d9c55 | 339 | pc.printf("Current Sep : %.2f\r\n",current_sep); |
imadaemi | 1:3151936d9c55 | 340 | //pc.printf("MainVol : %.2f, SepVol : %.2f, MainCur : %.2f, SepCur : %.2f\r\n", voltage_main, voltage_sep, current_main, current_sep); |
imadaemi | 0:4e38f8b1c183 | 341 | //pc.printf("MainVol : %f, SepVol : %f, MainCur : %f, SepCur : %f\r\n", voltage_main*(1/1000), voltage_sep, current_main, current_sep); |
imadaemi | 0:4e38f8b1c183 | 342 | im920.write((float)voltage_main); |
imadaemi | 0:4e38f8b1c183 | 343 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 344 | im920.write((float)current_main); |
imadaemi | 0:4e38f8b1c183 | 345 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 346 | im920.write((float)voltage_sep); |
imadaemi | 0:4e38f8b1c183 | 347 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 348 | im920.write((float)current_sep); |
imadaemi | 0:4e38f8b1c183 | 349 | im_buf_n ++; |
imadaemi | 1:3151936d9c55 | 350 | |
imadaemi | 1:3151936d9c55 | 351 | pc.printf("********************\r\n\r\n"); |
imadaemi | 1:3151936d9c55 | 352 | |
imadaemi | 0:4e38f8b1c183 | 353 | |
imadaemi | 0:4e38f8b1c183 | 354 | if(header_set){ |
imadaemi | 0:4e38f8b1c183 | 355 | im920.send(); |
imadaemi | 0:4e38f8b1c183 | 356 | pc.printf("Send : %s\r\n", im_buf); |
imadaemi | 0:4e38f8b1c183 | 357 | header_set = false; |
imadaemi | 0:4e38f8b1c183 | 358 | for(int i = 0; i < im_buf_n; i ++){ |
imadaemi | 0:4e38f8b1c183 | 359 | im_buf[i] = '\0'; |
imadaemi | 0:4e38f8b1c183 | 360 | } |
imadaemi | 0:4e38f8b1c183 | 361 | im_buf_n = 0; |
imadaemi | 0:4e38f8b1c183 | 362 | } |
imadaemi | 0:4e38f8b1c183 | 363 | } |
imadaemi | 0:4e38f8b1c183 | 364 | |
imadaemi | 0:4e38f8b1c183 | 365 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 366 | // EEPROMにデータを書き込むプログラム |
imadaemi | 0:4e38f8b1c183 | 367 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 368 | void WriteEEPROM(){ |
imadaemi | 0:4e38f8b1c183 | 369 | int ptr, n = 0; |
imadaemi | 0:4e38f8b1c183 | 370 | int eeprom_ptr = 0; |
imadaemi | 0:4e38f8b1c183 | 371 | |
imadaemi | 0:4e38f8b1c183 | 372 | for(int i = 0; i < 4; i++){ |
imadaemi | 0:4e38f8b1c183 | 373 | pc.printf("Start to write %d EEPROM\r\n", i); |
imadaemi | 0:4e38f8b1c183 | 374 | setEEPROMGroup(i); |
imadaemi | 0:4e38f8b1c183 | 375 | EEPROM.setWriteAddr(1, 0, 0x00, 0x00); |
imadaemi | 0:4e38f8b1c183 | 376 | |
imadaemi | 0:4e38f8b1c183 | 377 | while(1){ |
imadaemi | 0:4e38f8b1c183 | 378 | ptr = EEPROM.chargeBuff((int)n++); |
imadaemi | 0:4e38f8b1c183 | 379 | //ptr = EEPROM.chargeBuff((char)0xff); |
imadaemi | 0:4e38f8b1c183 | 380 | //ptr = EEPROM.chargeBuff((int)0); |
imadaemi | 0:4e38f8b1c183 | 381 | if(ptr == 128){ |
imadaemi | 0:4e38f8b1c183 | 382 | EEPROM.writeBuff(); |
imadaemi | 0:4e38f8b1c183 | 383 | //ptr = EEPROM.setNextPage(); |
imadaemi | 0:4e38f8b1c183 | 384 | eeprom_ptr = EEPROM.setNextPage(); |
imadaemi | 0:4e38f8b1c183 | 385 | //pc.printf("eeprom_ptr: %08x\r\n", eeprom_ptr); |
imadaemi | 0:4e38f8b1c183 | 386 | } |
imadaemi | 0:4e38f8b1c183 | 387 | |
imadaemi | 0:4e38f8b1c183 | 388 | if(eeprom_ptr == 0x01000000){ |
imadaemi | 0:4e38f8b1c183 | 389 | ptr = 0; |
imadaemi | 0:4e38f8b1c183 | 390 | eeprom_ptr = 0; |
imadaemi | 0:4e38f8b1c183 | 391 | break; |
imadaemi | 0:4e38f8b1c183 | 392 | } |
imadaemi | 0:4e38f8b1c183 | 393 | } |
imadaemi | 0:4e38f8b1c183 | 394 | } |
imadaemi | 0:4e38f8b1c183 | 395 | } |
imadaemi | 0:4e38f8b1c183 | 396 | |
imadaemi | 0:4e38f8b1c183 | 397 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 398 | // マルチプレクサで使うEEPROMを変更する |
imadaemi | 0:4e38f8b1c183 | 399 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 400 | void setEEPROMGroup(int group_num){ |
imadaemi | 0:4e38f8b1c183 | 401 | switch(group_num){ |
imadaemi | 0:4e38f8b1c183 | 402 | case 0: |
imadaemi | 0:4e38f8b1c183 | 403 | pinA = 0; |
imadaemi | 0:4e38f8b1c183 | 404 | pinB = 0; |
imadaemi | 0:4e38f8b1c183 | 405 | pinC = 0; |
imadaemi | 0:4e38f8b1c183 | 406 | break; |
imadaemi | 0:4e38f8b1c183 | 407 | |
imadaemi | 0:4e38f8b1c183 | 408 | case 1: |
imadaemi | 0:4e38f8b1c183 | 409 | pinA = 1; |
imadaemi | 0:4e38f8b1c183 | 410 | pinB = 0; |
imadaemi | 0:4e38f8b1c183 | 411 | pinC = 0; |
imadaemi | 0:4e38f8b1c183 | 412 | break; |
imadaemi | 0:4e38f8b1c183 | 413 | |
imadaemi | 0:4e38f8b1c183 | 414 | case 2: |
imadaemi | 0:4e38f8b1c183 | 415 | pinA = 0; |
imadaemi | 0:4e38f8b1c183 | 416 | pinB = 1; |
imadaemi | 0:4e38f8b1c183 | 417 | pinC = 0; |
imadaemi | 0:4e38f8b1c183 | 418 | break; |
imadaemi | 0:4e38f8b1c183 | 419 | |
imadaemi | 0:4e38f8b1c183 | 420 | case 3: |
imadaemi | 0:4e38f8b1c183 | 421 | pinA = 1; |
imadaemi | 0:4e38f8b1c183 | 422 | pinB = 1; |
imadaemi | 0:4e38f8b1c183 | 423 | pinC = 0; |
imadaemi | 0:4e38f8b1c183 | 424 | break; |
imadaemi | 0:4e38f8b1c183 | 425 | } |
imadaemi | 0:4e38f8b1c183 | 426 | } |