v1
Dependencies: Nichrome_lib mbed mpu9250_i2c IM920 INA226_lib PQLPS22HB EEPROM_lib GPS_interrupt
main.cpp@3:eca103d94b60, 2021-08-08 (annotated)
- Committer:
- imadaemi
- Date:
- Sun Aug 08 11:25:50 2021 +0000
- Revision:
- 3:eca103d94b60
- Parent:
- 2:980edad0eea2
- Child:
- 4:345e55d8e8a3
v4
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 | 3:eca103d94b60 | 10 | #define RESET_TIME 1800000 //1800000→30分 |
imadaemi | 3:eca103d94b60 | 11 | //#define RESET_TIME 30000000 // 30000000→30秒(実験用) |
imadaemi | 3:eca103d94b60 | 12 | #define FIRE_TIME 2.0 |
imadaemi | 3:eca103d94b60 | 13 | #define TO_SEPARATE_TIME 10.0 |
imadaemi | 3:eca103d94b60 | 14 | //#define TO_SEPARATE_TIME 300.0 |
imadaemi | 3:eca103d94b60 | 15 | #define TO_COLLECTION_TIME 30.0 |
imadaemi | 3:eca103d94b60 | 16 | //#define TO_COLLECTION_TIME 10.0 // 実験用 |
imadaemi | 3:eca103d94b60 | 17 | #define ALTITUDE_LIMIT 30.0 |
imadaemi | 3:eca103d94b60 | 18 | //#define ALTITUDE_LIMIT 10.0 |
imadaemi | 0:4e38f8b1c183 | 19 | #define ACC_RANGE _16G |
imadaemi | 0:4e38f8b1c183 | 20 | #define GYRO_RANGE _2000DPS |
imadaemi | 3:eca103d94b60 | 21 | //#define LPF_COEFFICIENT_ALT 0.01 |
imadaemi | 3:eca103d94b60 | 22 | //#define LPF_COEFFICIENT_ALT 0.001 |
imadaemi | 3:eca103d94b60 | 23 | #define LPF_COEFFICIENT_ALT 0.05 |
imadaemi | 3:eca103d94b60 | 24 | #define LPF_COEFFICIENT_VEL 0.2 |
imadaemi | 3:eca103d94b60 | 25 | |
imadaemi | 3:eca103d94b60 | 26 | #define TEMP_MULTIPLIER 100 |
imadaemi | 3:eca103d94b60 | 27 | #define LPF_ALT_MULTIPLIER 100 |
imadaemi | 3:eca103d94b60 | 28 | #define LPF_VEL_MULTIPLIER 100 |
imadaemi | 0:4e38f8b1c183 | 29 | |
imadaemi | 0:4e38f8b1c183 | 30 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 31 | // コンストラクタ |
imadaemi | 0:4e38f8b1c183 | 32 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 33 | Serial pc(USBTX, USBRX, 230400); |
imadaemi | 0:4e38f8b1c183 | 34 | Serial gps_serial(p9, p10, 115200); |
imadaemi | 0:4e38f8b1c183 | 35 | Serial im920_serial(p13, p14, 115200); |
imadaemi | 0:4e38f8b1c183 | 36 | |
imadaemi | 0:4e38f8b1c183 | 37 | I2C i2c(p28, p27); |
imadaemi | 0:4e38f8b1c183 | 38 | |
imadaemi | 0:4e38f8b1c183 | 39 | IM920 im920(im920_serial, pc, 115200); |
imadaemi | 0:4e38f8b1c183 | 40 | GPS_interrupt gps(&gps_serial); |
imadaemi | 0:4e38f8b1c183 | 41 | LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW); |
imadaemi | 0:4e38f8b1c183 | 42 | mpu9250 mpu9250(i2c,AD0_HIGH); |
imadaemi | 0:4e38f8b1c183 | 43 | myINA226 ina226_main(i2c, myINA226::A1_GND, myINA226::A0_GND); |
imadaemi | 0:4e38f8b1c183 | 44 | myINA226 ina226_sep(i2c, myINA226::A1_VDD, myINA226::A0_VDD); |
imadaemi | 0:4e38f8b1c183 | 45 | Nichrome_lib nich(p20); |
imadaemi | 3:eca103d94b60 | 46 | EEPROM_lib EEPROM(i2c, 4); |
imadaemi | 0:4e38f8b1c183 | 47 | |
imadaemi | 3:eca103d94b60 | 48 | InterruptIn flightpin(p18); |
imadaemi | 3:eca103d94b60 | 49 | DigitalOut nich_led(LED1); |
imadaemi | 3:eca103d94b60 | 50 | DigitalOut im920_busy_led(LED2); |
imadaemi | 0:4e38f8b1c183 | 51 | DigitalOut pinA(p21); |
imadaemi | 0:4e38f8b1c183 | 52 | DigitalOut pinB(p22); |
imadaemi | 0:4e38f8b1c183 | 53 | DigitalOut pinC(p23); |
imadaemi | 0:4e38f8b1c183 | 54 | |
imadaemi | 3:eca103d94b60 | 55 | DigitalIn im920_busy(p15); |
imadaemi | 3:eca103d94b60 | 56 | |
imadaemi | 1:3151936d9c55 | 57 | Ticker send_data; |
imadaemi | 3:eca103d94b60 | 58 | Ticker save_data; |
imadaemi | 3:eca103d94b60 | 59 | Ticker get_data; |
imadaemi | 3:eca103d94b60 | 60 | |
imadaemi | 3:eca103d94b60 | 61 | Timer main_timer; |
imadaemi | 3:eca103d94b60 | 62 | |
imadaemi | 3:eca103d94b60 | 63 | //Timeout to_separate_time; |
imadaemi | 3:eca103d94b60 | 64 | //Timeout to_collection_time; |
imadaemi | 3:eca103d94b60 | 65 | //Timeout fire_time; |
imadaemi | 3:eca103d94b60 | 66 | Timeout task_timeout; |
imadaemi | 0:4e38f8b1c183 | 67 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 68 | // 関数の宣言 |
imadaemi | 0:4e38f8b1c183 | 69 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 70 | void uplink(); |
imadaemi | 0:4e38f8b1c183 | 71 | void echoIM(); |
imadaemi | 3:eca103d94b60 | 72 | void PerformTask(); |
imadaemi | 3:eca103d94b60 | 73 | void FlightPinDetect(); |
imadaemi | 3:eca103d94b60 | 74 | void ChangeModeToSep(); |
imadaemi | 3:eca103d94b60 | 75 | void ChangeModeToCol(); |
imadaemi | 0:4e38f8b1c183 | 76 | void Separate(); |
imadaemi | 0:4e38f8b1c183 | 77 | void StopSeparate(); |
imadaemi | 0:4e38f8b1c183 | 78 | void SetSensor(); |
imadaemi | 0:4e38f8b1c183 | 79 | void GetData(); |
imadaemi | 3:eca103d94b60 | 80 | void SendLaunchTime(); |
imadaemi | 2:980edad0eea2 | 81 | void SendData(); |
imadaemi | 3:eca103d94b60 | 82 | void SaveData(); |
imadaemi | 0:4e38f8b1c183 | 83 | void setEEPROMGroup(int group_num); |
imadaemi | 0:4e38f8b1c183 | 84 | |
imadaemi | 0:4e38f8b1c183 | 85 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 86 | // 無線のヘッダまとめ |
imadaemi | 0:4e38f8b1c183 | 87 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 88 | const char HEADER_SETUP = 0x01; |
imadaemi | 3:eca103d94b60 | 89 | // 0x01, GPS, LPS22HB, MPU9250, MPU9250_MAG, INA226_MAIN, INA226_SEP |
imadaemi | 2:980edad0eea2 | 90 | // 1 1 1 1 1 1 1 |
imadaemi | 0:4e38f8b1c183 | 91 | |
imadaemi | 3:eca103d94b60 | 92 | const char HEADER_DATA = 0x02; |
imadaemi | 3:eca103d94b60 | 93 | // 0xA2, duration_main_time, duration_flight_time, mode, nich_status, flightpin_status, lat, lon, height, press, temp, altitude, voltage_main, voltage_sep, current_main, current_sep |
imadaemi | 3:eca103d94b60 | 94 | // 1 4 4 1 0.125 0.125 4 4 4 4 4 4 4 4 4 4 |
imadaemi | 3:eca103d94b60 | 95 | const char HEADER_LAUNCH_TIME = 0x03; |
imadaemi | 0:4e38f8b1c183 | 96 | |
imadaemi | 3:eca103d94b60 | 97 | const char HEADER_ECHO = 0x05; |
imadaemi | 0:4e38f8b1c183 | 98 | // 0xA5,コマンド |
imadaemi | 0:4e38f8b1c183 | 99 | // 1 1 |
imadaemi | 0:4e38f8b1c183 | 100 | |
imadaemi | 0:4e38f8b1c183 | 101 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 102 | // 変数の宣言 |
imadaemi | 0:4e38f8b1c183 | 103 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 104 | float launch_time = 0; |
imadaemi | 3:eca103d94b60 | 105 | float previous_main_time = 0; |
imadaemi | 3:eca103d94b60 | 106 | float flight_time = 0; |
imadaemi | 3:eca103d94b60 | 107 | float main_time = 0; |
imadaemi | 3:eca103d94b60 | 108 | int time_reset_counter = 0; |
imadaemi | 3:eca103d94b60 | 109 | |
imadaemi | 3:eca103d94b60 | 110 | bool send_launch_time_status = false; |
imadaemi | 3:eca103d94b60 | 111 | |
imadaemi | 3:eca103d94b60 | 112 | bool top_detect = false; |
imadaemi | 3:eca103d94b60 | 113 | char bitshift_top_detect; |
imadaemi | 3:eca103d94b60 | 114 | |
imadaemi | 3:eca103d94b60 | 115 | bool nich_status = false; |
imadaemi | 3:eca103d94b60 | 116 | char bitshift_nich_status; |
imadaemi | 3:eca103d94b60 | 117 | |
imadaemi | 3:eca103d94b60 | 118 | bool flightpin_status = false; |
imadaemi | 3:eca103d94b60 | 119 | char bitshift_flightpin_status; |
imadaemi | 3:eca103d94b60 | 120 | |
imadaemi | 3:eca103d94b60 | 121 | bool save_data_status = false; |
imadaemi | 3:eca103d94b60 | 122 | char bitshift_save_data_status; |
imadaemi | 3:eca103d94b60 | 123 | |
imadaemi | 3:eca103d94b60 | 124 | char bitshift_sum; |
imadaemi | 1:3151936d9c55 | 125 | |
imadaemi | 0:4e38f8b1c183 | 126 | bool header_set = false; |
imadaemi | 0:4e38f8b1c183 | 127 | char im_buf[55];//16なのって意味ある? |
imadaemi | 0:4e38f8b1c183 | 128 | int im_buf_n = 0; |
imadaemi | 0:4e38f8b1c183 | 129 | |
imadaemi | 0:4e38f8b1c183 | 130 | float lat, lon, height; |
imadaemi | 3:eca103d94b60 | 131 | int satellite_number; |
imadaemi | 3:eca103d94b60 | 132 | float press, temp; |
imadaemi | 3:eca103d94b60 | 133 | float altitude; |
imadaemi | 3:eca103d94b60 | 134 | //float ground_press = 1013.0,ground_temp = 25.0; |
imadaemi | 3:eca103d94b60 | 135 | float ground_press = -1.0,ground_temp = -1.0; |
imadaemi | 3:eca103d94b60 | 136 | |
imadaemi | 3:eca103d94b60 | 137 | float velocity; |
imadaemi | 3:eca103d94b60 | 138 | float previous_lpf_velocity; |
imadaemi | 3:eca103d94b60 | 139 | float lpf_velocity; |
imadaemi | 3:eca103d94b60 | 140 | |
imadaemi | 3:eca103d94b60 | 141 | float previous_lpf_altitude; |
imadaemi | 3:eca103d94b60 | 142 | float lpf_altitude; |
imadaemi | 0:4e38f8b1c183 | 143 | |
imadaemi | 0:4e38f8b1c183 | 144 | float imu[6], mag[3]; |
imadaemi | 0:4e38f8b1c183 | 145 | float mag_geo[3]; |
imadaemi | 0:4e38f8b1c183 | 146 | |
imadaemi | 0:4e38f8b1c183 | 147 | float voltage_main, voltage_sep; |
imadaemi | 0:4e38f8b1c183 | 148 | float current_main, current_sep; |
imadaemi | 0:4e38f8b1c183 | 149 | |
imadaemi | 3:eca103d94b60 | 150 | int ptr, n = 0; |
imadaemi | 3:eca103d94b60 | 151 | int eeprom_ptr = 0; |
imadaemi | 3:eca103d94b60 | 152 | int eeprom_group_counter = 0; |
imadaemi | 3:eca103d94b60 | 153 | int eeprom_number = 0; |
imadaemi | 3:eca103d94b60 | 154 | |
imadaemi | 3:eca103d94b60 | 155 | const char MODE_SETUP = 0x00; |
imadaemi | 3:eca103d94b60 | 156 | const char MODE_SAFE = 0x01; |
imadaemi | 3:eca103d94b60 | 157 | const char MODE_TO_ONCALL = 0x02; |
imadaemi | 3:eca103d94b60 | 158 | const char MODE_ONCALL = 0x03; |
imadaemi | 3:eca103d94b60 | 159 | const char MODE_TO_FLIGHT = 0x04; |
imadaemi | 3:eca103d94b60 | 160 | const char MODE_FLIGHT = 0x05; |
imadaemi | 3:eca103d94b60 | 161 | const char MODE_TO_SEPARATE = 0x06; |
imadaemi | 3:eca103d94b60 | 162 | const char MODE_SEPARATE = 0x07; |
imadaemi | 3:eca103d94b60 | 163 | const char MODE_TO_DESCEND = 0x08; |
imadaemi | 3:eca103d94b60 | 164 | const char MODE_DESCEND = 0x09; |
imadaemi | 3:eca103d94b60 | 165 | const char MODE_TO_COLLECTION = 0x0a; |
imadaemi | 3:eca103d94b60 | 166 | const char MODE_COLLECTION = 0x0b; |
imadaemi | 3:eca103d94b60 | 167 | |
imadaemi | 3:eca103d94b60 | 168 | char mode = MODE_SETUP; |
imadaemi | 3:eca103d94b60 | 169 | |
imadaemi | 0:4e38f8b1c183 | 170 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 171 | // メイン関数 |
imadaemi | 0:4e38f8b1c183 | 172 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 173 | int main() { |
imadaemi | 0:4e38f8b1c183 | 174 | pc.printf("Hello Main!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 175 | im920.attach(&uplink, 0xF0); |
imadaemi | 2:980edad0eea2 | 176 | send_data.attach(&SendData, 1.0); |
imadaemi | 3:eca103d94b60 | 177 | get_data.attach(&GetData, 0.02); |
imadaemi | 0:4e38f8b1c183 | 178 | |
imadaemi | 0:4e38f8b1c183 | 179 | while(1){ |
imadaemi | 3:eca103d94b60 | 180 | PerformTask(); |
imadaemi | 3:eca103d94b60 | 181 | /* |
imadaemi | 3:eca103d94b60 | 182 | if(im920_busy){ |
imadaemi | 3:eca103d94b60 | 183 | im920_busy_led = 1; |
imadaemi | 3:eca103d94b60 | 184 | }else{ |
imadaemi | 3:eca103d94b60 | 185 | im920_busy_led = 0; |
imadaemi | 3:eca103d94b60 | 186 | } |
imadaemi | 3:eca103d94b60 | 187 | */ |
imadaemi | 0:4e38f8b1c183 | 188 | } |
imadaemi | 0:4e38f8b1c183 | 189 | } |
imadaemi | 3:eca103d94b60 | 190 | |
imadaemi | 0:4e38f8b1c183 | 191 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 192 | // アップリンク(地上局から) |
imadaemi | 0:4e38f8b1c183 | 193 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 194 | void uplink(){ |
imadaemi | 0:4e38f8b1c183 | 195 | echoIM(); |
imadaemi | 0:4e38f8b1c183 | 196 | switch(im920.data[1]){ |
imadaemi | 3:eca103d94b60 | 197 | |
imadaemi | 3:eca103d94b60 | 198 | case 'U': |
imadaemi | 3:eca103d94b60 | 199 | if(mode == MODE_FLIGHT || mode == MODE_ONCALL){ |
imadaemi | 3:eca103d94b60 | 200 | mode = MODE_SETUP; |
imadaemi | 3:eca103d94b60 | 201 | save_data.detach(); |
imadaemi | 3:eca103d94b60 | 202 | save_data_status = false; |
imadaemi | 3:eca103d94b60 | 203 | pc.printf("********************\r\n"); |
imadaemi | 3:eca103d94b60 | 204 | pc.printf("Set : MODE_SETUP\r\n"); |
imadaemi | 3:eca103d94b60 | 205 | pc.printf("********************\r\n\r\n"); |
imadaemi | 3:eca103d94b60 | 206 | } |
imadaemi | 3:eca103d94b60 | 207 | break; |
imadaemi | 3:eca103d94b60 | 208 | |
imadaemi | 3:eca103d94b60 | 209 | case 'O': |
imadaemi | 3:eca103d94b60 | 210 | if(mode == MODE_SAFE){ |
imadaemi | 3:eca103d94b60 | 211 | mode = MODE_TO_ONCALL; |
imadaemi | 3:eca103d94b60 | 212 | pc.printf("********************\r\n"); |
imadaemi | 3:eca103d94b60 | 213 | pc.printf("Set : MODE_TO_ONCALL\r\n"); |
imadaemi | 3:eca103d94b60 | 214 | pc.printf("********************\r\n\r\n"); |
imadaemi | 3:eca103d94b60 | 215 | } |
imadaemi | 3:eca103d94b60 | 216 | break; |
imadaemi | 3:eca103d94b60 | 217 | |
imadaemi | 3:eca103d94b60 | 218 | case 'F': |
imadaemi | 3:eca103d94b60 | 219 | if(mode == MODE_ONCALL){ |
imadaemi | 3:eca103d94b60 | 220 | mode = MODE_TO_FLIGHT; |
imadaemi | 3:eca103d94b60 | 221 | pc.printf("********************\r\n"); |
imadaemi | 3:eca103d94b60 | 222 | pc.printf("Set : MODE_TO_FLIGHT\r\n"); |
imadaemi | 3:eca103d94b60 | 223 | pc.printf("********************\r\n\r\n"); |
imadaemi | 3:eca103d94b60 | 224 | } |
imadaemi | 3:eca103d94b60 | 225 | break; |
imadaemi | 3:eca103d94b60 | 226 | |
imadaemi | 3:eca103d94b60 | 227 | case 'S': |
imadaemi | 3:eca103d94b60 | 228 | if(mode == MODE_FLIGHT){ |
imadaemi | 3:eca103d94b60 | 229 | mode = MODE_TO_SEPARATE; |
imadaemi | 3:eca103d94b60 | 230 | pc.printf("********************\r\n"); |
imadaemi | 3:eca103d94b60 | 231 | pc.printf("Set : MODE_TO_SEPARATE\r\n"); |
imadaemi | 3:eca103d94b60 | 232 | pc.printf("********************\r\n\r\n"); |
imadaemi | 3:eca103d94b60 | 233 | } |
imadaemi | 3:eca103d94b60 | 234 | break; |
imadaemi | 3:eca103d94b60 | 235 | |
imadaemi | 0:4e38f8b1c183 | 236 | case 0x01: |
imadaemi | 0:4e38f8b1c183 | 237 | pc.printf("********************\r\n"); |
imadaemi | 0:4e38f8b1c183 | 238 | pc.printf("SEPARATE\r\n"); |
imadaemi | 0:4e38f8b1c183 | 239 | pc.printf("********************\r\n\r\n"); |
imadaemi | 0:4e38f8b1c183 | 240 | Separate(); |
imadaemi | 0:4e38f8b1c183 | 241 | break; |
imadaemi | 0:4e38f8b1c183 | 242 | |
imadaemi | 0:4e38f8b1c183 | 243 | case 0x00: |
imadaemi | 0:4e38f8b1c183 | 244 | pc.printf("********************\r\n"); |
imadaemi | 0:4e38f8b1c183 | 245 | pc.printf("STOP SEPARATE\r\n"); |
imadaemi | 0:4e38f8b1c183 | 246 | pc.printf("********************\r\n\r\n"); |
imadaemi | 0:4e38f8b1c183 | 247 | StopSeparate(); |
imadaemi | 0:4e38f8b1c183 | 248 | break; |
imadaemi | 0:4e38f8b1c183 | 249 | } |
imadaemi | 0:4e38f8b1c183 | 250 | } |
imadaemi | 0:4e38f8b1c183 | 251 | |
imadaemi | 0:4e38f8b1c183 | 252 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 253 | // 無線信号の送り返し |
imadaemi | 0:4e38f8b1c183 | 254 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 255 | void echoIM(){ |
imadaemi | 0:4e38f8b1c183 | 256 | im920.header(HEADER_ECHO); |
imadaemi | 0:4e38f8b1c183 | 257 | im920.write(im920.data[1]); |
imadaemi | 0:4e38f8b1c183 | 258 | im920.send(); |
imadaemi | 0:4e38f8b1c183 | 259 | } |
imadaemi | 0:4e38f8b1c183 | 260 | |
imadaemi | 0:4e38f8b1c183 | 261 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 262 | // タスクの実行 |
imadaemi | 3:eca103d94b60 | 263 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 264 | void PerformTask(){ |
imadaemi | 3:eca103d94b60 | 265 | switch(mode){ |
imadaemi | 3:eca103d94b60 | 266 | |
imadaemi | 3:eca103d94b60 | 267 | case(MODE_SETUP): |
imadaemi | 3:eca103d94b60 | 268 | SetSensor(); |
imadaemi | 3:eca103d94b60 | 269 | main_timer.start(); |
imadaemi | 3:eca103d94b60 | 270 | send_data.detach(); |
imadaemi | 3:eca103d94b60 | 271 | send_data.attach(&SendData, 1.0); |
imadaemi | 3:eca103d94b60 | 272 | mode = MODE_SAFE; |
imadaemi | 3:eca103d94b60 | 273 | break; |
imadaemi | 3:eca103d94b60 | 274 | |
imadaemi | 3:eca103d94b60 | 275 | case(MODE_SAFE): |
imadaemi | 3:eca103d94b60 | 276 | break; |
imadaemi | 3:eca103d94b60 | 277 | |
imadaemi | 3:eca103d94b60 | 278 | case(MODE_TO_ONCALL): |
imadaemi | 3:eca103d94b60 | 279 | ground_press = press; |
imadaemi | 3:eca103d94b60 | 280 | save_data.detach(); |
imadaemi | 3:eca103d94b60 | 281 | save_data.attach(&SaveData, 1.0); |
imadaemi | 3:eca103d94b60 | 282 | save_data_status = true; |
imadaemi | 3:eca103d94b60 | 283 | flightpin.rise(&FlightPinDetect); |
imadaemi | 3:eca103d94b60 | 284 | mode = MODE_ONCALL; |
imadaemi | 3:eca103d94b60 | 285 | break; |
imadaemi | 3:eca103d94b60 | 286 | |
imadaemi | 3:eca103d94b60 | 287 | case(MODE_ONCALL): |
imadaemi | 3:eca103d94b60 | 288 | break; |
imadaemi | 3:eca103d94b60 | 289 | |
imadaemi | 3:eca103d94b60 | 290 | case(MODE_TO_FLIGHT): |
imadaemi | 3:eca103d94b60 | 291 | save_data.detach(); |
imadaemi | 3:eca103d94b60 | 292 | save_data.attach(&SaveData, 0.02); |
imadaemi | 3:eca103d94b60 | 293 | launch_time = main_time; |
imadaemi | 3:eca103d94b60 | 294 | //to_separate_time.attach(&ChangeModeToSep,TO_SEPARATE_TIME); |
imadaemi | 3:eca103d94b60 | 295 | task_timeout.detach(); |
imadaemi | 3:eca103d94b60 | 296 | task_timeout.attach(&ChangeModeToSep,TO_SEPARATE_TIME); |
imadaemi | 3:eca103d94b60 | 297 | mode = MODE_FLIGHT; |
imadaemi | 3:eca103d94b60 | 298 | break; |
imadaemi | 3:eca103d94b60 | 299 | |
imadaemi | 3:eca103d94b60 | 300 | case(MODE_FLIGHT): |
imadaemi | 3:eca103d94b60 | 301 | //pc.printf("%f\t%f\t%f\t%f\t%f\r\n",press,altitude,lpf_altitude, ALTITUDE_LIMIT,velocity); |
imadaemi | 3:eca103d94b60 | 302 | if(!im920_busy && !send_launch_time_status){ |
imadaemi | 3:eca103d94b60 | 303 | SendLaunchTime(); |
imadaemi | 3:eca103d94b60 | 304 | } |
imadaemi | 3:eca103d94b60 | 305 | if((lpf_altitude >= ALTITUDE_LIMIT) && (lpf_velocity < 0.0)){//and 速度<0を書き込む |
imadaemi | 3:eca103d94b60 | 306 | mode = MODE_TO_SEPARATE; |
imadaemi | 3:eca103d94b60 | 307 | top_detect = true; |
imadaemi | 3:eca103d94b60 | 308 | } |
imadaemi | 3:eca103d94b60 | 309 | break; |
imadaemi | 3:eca103d94b60 | 310 | |
imadaemi | 3:eca103d94b60 | 311 | case(MODE_TO_SEPARATE): |
imadaemi | 3:eca103d94b60 | 312 | Separate(); |
imadaemi | 3:eca103d94b60 | 313 | mode = MODE_SEPARATE; |
imadaemi | 3:eca103d94b60 | 314 | break; |
imadaemi | 3:eca103d94b60 | 315 | |
imadaemi | 3:eca103d94b60 | 316 | case(MODE_SEPARATE): |
imadaemi | 3:eca103d94b60 | 317 | if(nich.status == false){ |
imadaemi | 3:eca103d94b60 | 318 | mode = MODE_TO_DESCEND; |
imadaemi | 3:eca103d94b60 | 319 | } |
imadaemi | 3:eca103d94b60 | 320 | break; |
imadaemi | 3:eca103d94b60 | 321 | |
imadaemi | 3:eca103d94b60 | 322 | case(MODE_TO_DESCEND): |
imadaemi | 3:eca103d94b60 | 323 | //to_collection_time.attach(&ChangeModeToCol,TO_COLLECTION_TIME); |
imadaemi | 3:eca103d94b60 | 324 | task_timeout.detach(); |
imadaemi | 3:eca103d94b60 | 325 | task_timeout.attach(&ChangeModeToCol,TO_COLLECTION_TIME); |
imadaemi | 3:eca103d94b60 | 326 | mode = MODE_DESCEND; |
imadaemi | 3:eca103d94b60 | 327 | break; |
imadaemi | 3:eca103d94b60 | 328 | |
imadaemi | 3:eca103d94b60 | 329 | case(MODE_DESCEND): |
imadaemi | 3:eca103d94b60 | 330 | break; |
imadaemi | 3:eca103d94b60 | 331 | |
imadaemi | 3:eca103d94b60 | 332 | case(MODE_TO_COLLECTION): |
imadaemi | 3:eca103d94b60 | 333 | save_data.detach(); |
imadaemi | 3:eca103d94b60 | 334 | save_data.attach(&SaveData, 1.0); |
imadaemi | 3:eca103d94b60 | 335 | mode = MODE_COLLECTION; |
imadaemi | 3:eca103d94b60 | 336 | break; |
imadaemi | 3:eca103d94b60 | 337 | |
imadaemi | 3:eca103d94b60 | 338 | case(MODE_COLLECTION): |
imadaemi | 3:eca103d94b60 | 339 | break; |
imadaemi | 3:eca103d94b60 | 340 | } |
imadaemi | 3:eca103d94b60 | 341 | } |
imadaemi | 3:eca103d94b60 | 342 | |
imadaemi | 3:eca103d94b60 | 343 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 344 | // フライトピン検知 |
imadaemi | 3:eca103d94b60 | 345 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 346 | void FlightPinDetect(){ |
imadaemi | 3:eca103d94b60 | 347 | if(mode == MODE_ONCALL){ |
imadaemi | 3:eca103d94b60 | 348 | mode = MODE_TO_FLIGHT; |
imadaemi | 3:eca103d94b60 | 349 | flightpin_status = true; |
imadaemi | 3:eca103d94b60 | 350 | /* |
imadaemi | 3:eca103d94b60 | 351 | pc.printf("********************\r\n"); |
imadaemi | 3:eca103d94b60 | 352 | pc.printf("Set : MODE_TO_FLIGHT\r\n"); |
imadaemi | 3:eca103d94b60 | 353 | pc.printf("********************\r\n\r\n"); |
imadaemi | 3:eca103d94b60 | 354 | */ |
imadaemi | 3:eca103d94b60 | 355 | } |
imadaemi | 3:eca103d94b60 | 356 | } |
imadaemi | 3:eca103d94b60 | 357 | |
imadaemi | 3:eca103d94b60 | 358 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 359 | // 1フライトモード→セパレートモード |
imadaemi | 3:eca103d94b60 | 360 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 361 | void ChangeModeToSep(){ |
imadaemi | 3:eca103d94b60 | 362 | if(mode == MODE_FLIGHT){ |
imadaemi | 3:eca103d94b60 | 363 | mode = MODE_TO_SEPARATE; |
imadaemi | 3:eca103d94b60 | 364 | /* |
imadaemi | 3:eca103d94b60 | 365 | pc.printf("********************\r\n"); |
imadaemi | 3:eca103d94b60 | 366 | pc.printf("Set : MODE_TO_SEPARATE\r\n"); |
imadaemi | 3:eca103d94b60 | 367 | pc.printf("********************\r\n\r\n"); |
imadaemi | 3:eca103d94b60 | 368 | */ |
imadaemi | 3:eca103d94b60 | 369 | } |
imadaemi | 3:eca103d94b60 | 370 | } |
imadaemi | 3:eca103d94b60 | 371 | |
imadaemi | 3:eca103d94b60 | 372 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 373 | // 下降モード→回収モード |
imadaemi | 3:eca103d94b60 | 374 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 375 | void ChangeModeToCol(){ |
imadaemi | 3:eca103d94b60 | 376 | if(mode == MODE_DESCEND){ |
imadaemi | 3:eca103d94b60 | 377 | mode = MODE_TO_COLLECTION; |
imadaemi | 3:eca103d94b60 | 378 | /* |
imadaemi | 3:eca103d94b60 | 379 | pc.printf("********************\r\n"); |
imadaemi | 3:eca103d94b60 | 380 | pc.printf("Set : MODE_TO_COLLECTION\r\n"); |
imadaemi | 3:eca103d94b60 | 381 | pc.printf("********************\r\n\r\n"); |
imadaemi | 3:eca103d94b60 | 382 | */ |
imadaemi | 3:eca103d94b60 | 383 | } |
imadaemi | 3:eca103d94b60 | 384 | } |
imadaemi | 3:eca103d94b60 | 385 | |
imadaemi | 3:eca103d94b60 | 386 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 387 | // 分離実行 |
imadaemi | 0:4e38f8b1c183 | 388 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 389 | void Separate(){ |
imadaemi | 1:3151936d9c55 | 390 | nich.fire_on(); |
imadaemi | 3:eca103d94b60 | 391 | nich_led = 1; |
imadaemi | 3:eca103d94b60 | 392 | //fire_time.attach(&StopSeparate,FIRE_TIME); |
imadaemi | 3:eca103d94b60 | 393 | task_timeout.detach(); |
imadaemi | 3:eca103d94b60 | 394 | task_timeout.attach(&StopSeparate,FIRE_TIME); |
imadaemi | 0:4e38f8b1c183 | 395 | } |
imadaemi | 0:4e38f8b1c183 | 396 | |
imadaemi | 0:4e38f8b1c183 | 397 | void StopSeparate(){ |
imadaemi | 0:4e38f8b1c183 | 398 | nich.fire_off(); |
imadaemi | 3:eca103d94b60 | 399 | nich_led = 0; |
imadaemi | 0:4e38f8b1c183 | 400 | } |
imadaemi | 0:4e38f8b1c183 | 401 | |
imadaemi | 0:4e38f8b1c183 | 402 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 403 | // センサーのセットアップ |
imadaemi | 0:4e38f8b1c183 | 404 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 405 | void SetSensor(){ |
imadaemi | 0:4e38f8b1c183 | 406 | pc.printf("\r\n"); |
imadaemi | 0:4e38f8b1c183 | 407 | for(int i = 0; i < 20; i++){ |
imadaemi | 0:4e38f8b1c183 | 408 | pc.printf("*"); |
imadaemi | 0:4e38f8b1c183 | 409 | } |
imadaemi | 0:4e38f8b1c183 | 410 | pc.printf("\r\n"); |
imadaemi | 0:4e38f8b1c183 | 411 | pc.printf("Start Setting\r\n"); |
imadaemi | 0:4e38f8b1c183 | 412 | |
imadaemi | 0:4e38f8b1c183 | 413 | if(!header_set){ |
imadaemi | 0:4e38f8b1c183 | 414 | im920.header((char)HEADER_SETUP); |
imadaemi | 0:4e38f8b1c183 | 415 | header_set = true; |
imadaemi | 0:4e38f8b1c183 | 416 | } |
imadaemi | 0:4e38f8b1c183 | 417 | |
imadaemi | 3:eca103d94b60 | 418 | //FlightPin |
imadaemi | 3:eca103d94b60 | 419 | flightpin.mode(PullUp); |
imadaemi | 3:eca103d94b60 | 420 | |
imadaemi | 0:4e38f8b1c183 | 421 | //GPS |
imadaemi | 0:4e38f8b1c183 | 422 | if(gps.gps_readable == true){ |
imadaemi | 0:4e38f8b1c183 | 423 | pc.printf("GPS : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 424 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 425 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 426 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 427 | }else{ |
imadaemi | 0:4e38f8b1c183 | 428 | pc.printf("GPS : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 429 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 430 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 431 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 432 | } |
imadaemi | 0:4e38f8b1c183 | 433 | |
imadaemi | 0:4e38f8b1c183 | 434 | //LPS22HB |
imadaemi | 0:4e38f8b1c183 | 435 | lps22hb.begin(); |
imadaemi | 0:4e38f8b1c183 | 436 | if(lps22hb.test() == true){ |
imadaemi | 0:4e38f8b1c183 | 437 | pc.printf("LPS22HB : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 438 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 439 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 440 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 441 | }else{ |
imadaemi | 0:4e38f8b1c183 | 442 | pc.printf("LPS22HB : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 443 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 444 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 445 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 446 | } |
imadaemi | 0:4e38f8b1c183 | 447 | |
imadaemi | 0:4e38f8b1c183 | 448 | //MPU9250 |
imadaemi | 0:4e38f8b1c183 | 449 | if(mpu9250.sensorTest() == true){ |
imadaemi | 0:4e38f8b1c183 | 450 | pc.printf("MPU9250 : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 451 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 452 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 453 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 454 | }else{ |
imadaemi | 0:4e38f8b1c183 | 455 | pc.printf("MPU9250 : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 456 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 457 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 458 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 459 | } |
imadaemi | 0:4e38f8b1c183 | 460 | if(mpu9250.mag_sensorTest() == true){ |
imadaemi | 0:4e38f8b1c183 | 461 | pc.printf("MPU9250 MAG : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 462 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 463 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 464 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 465 | }else{ |
imadaemi | 0:4e38f8b1c183 | 466 | pc.printf("MPU9250 MAG : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 467 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 468 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 469 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 470 | } |
imadaemi | 0:4e38f8b1c183 | 471 | mpu9250.setAcc(ACC_RANGE); |
imadaemi | 0:4e38f8b1c183 | 472 | mpu9250.setGyro(GYRO_RANGE); |
imadaemi | 0:4e38f8b1c183 | 473 | mpu9250.setOffset(0.528892327, -0.660206211, 0.757105891, -0.011691362, 0.025688783, 1.087885322, -159.750004, 121.425005, -392.700012); |
imadaemi | 0:4e38f8b1c183 | 474 | |
imadaemi | 0:4e38f8b1c183 | 475 | //INA226 |
imadaemi | 0:4e38f8b1c183 | 476 | ina226_main.set_callibretion(); |
imadaemi | 0:4e38f8b1c183 | 477 | ina226_sep.set_callibretion(); |
imadaemi | 1:3151936d9c55 | 478 | //ina226_main.setup(1); |
imadaemi | 1:3151936d9c55 | 479 | //ina226_sep.setup(1); |
imadaemi | 0:4e38f8b1c183 | 480 | |
imadaemi | 0:4e38f8b1c183 | 481 | if(ina226_main.Connection_check()==0){ |
imadaemi | 0:4e38f8b1c183 | 482 | pc.printf("INA226 Main : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 483 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 484 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 485 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 486 | }else{ |
imadaemi | 0:4e38f8b1c183 | 487 | pc.printf("INA226 Main : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 488 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 489 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 490 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 491 | } |
imadaemi | 0:4e38f8b1c183 | 492 | if(ina226_sep.Connection_check()==0){ |
imadaemi | 0:4e38f8b1c183 | 493 | pc.printf("INA226 Sep : OK!\r\n"); |
imadaemi | 0:4e38f8b1c183 | 494 | im920.write((char)0x01); |
imadaemi | 0:4e38f8b1c183 | 495 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:4e38f8b1c183 | 496 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 497 | }else{ |
imadaemi | 0:4e38f8b1c183 | 498 | pc.printf("INA226 Sep : NG...\r\n"); |
imadaemi | 0:4e38f8b1c183 | 499 | im920.write((char)0x00); |
imadaemi | 0:4e38f8b1c183 | 500 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:4e38f8b1c183 | 501 | im_buf_n ++; |
imadaemi | 0:4e38f8b1c183 | 502 | } |
imadaemi | 0:4e38f8b1c183 | 503 | |
imadaemi | 0:4e38f8b1c183 | 504 | if(header_set){ |
imadaemi | 0:4e38f8b1c183 | 505 | im920.send(); |
imadaemi | 0:4e38f8b1c183 | 506 | pc.printf("Send : %s\r\n", im_buf); |
imadaemi | 0:4e38f8b1c183 | 507 | header_set = false; |
imadaemi | 0:4e38f8b1c183 | 508 | for(int i = 0; i < im_buf_n; i ++){ |
imadaemi | 0:4e38f8b1c183 | 509 | im_buf[i] = '\0'; |
imadaemi | 0:4e38f8b1c183 | 510 | } |
imadaemi | 0:4e38f8b1c183 | 511 | im_buf_n = 0; |
imadaemi | 0:4e38f8b1c183 | 512 | } |
imadaemi | 0:4e38f8b1c183 | 513 | |
imadaemi | 3:eca103d94b60 | 514 | //EEPROM |
imadaemi | 3:eca103d94b60 | 515 | eeprom_group_counter = 0; |
imadaemi | 3:eca103d94b60 | 516 | setEEPROMGroup(eeprom_group_counter); |
imadaemi | 3:eca103d94b60 | 517 | EEPROM.setWriteAddr(1, 0, 0x00, 0x00); |
imadaemi | 3:eca103d94b60 | 518 | |
imadaemi | 0:4e38f8b1c183 | 519 | pc.printf("\r\n"); |
imadaemi | 0:4e38f8b1c183 | 520 | for(int i = 0; i < 20; i++){ |
imadaemi | 0:4e38f8b1c183 | 521 | pc.printf("*"); |
imadaemi | 0:4e38f8b1c183 | 522 | } |
imadaemi | 0:4e38f8b1c183 | 523 | pc.printf("\r\n"); |
imadaemi | 0:4e38f8b1c183 | 524 | } |
imadaemi | 0:4e38f8b1c183 | 525 | |
imadaemi | 0:4e38f8b1c183 | 526 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 527 | // センサーのデータ取得 |
imadaemi | 0:4e38f8b1c183 | 528 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 529 | void GetData(){ |
imadaemi | 3:eca103d94b60 | 530 | |
imadaemi | 3:eca103d94b60 | 531 | //Main_Time |
imadaemi | 3:eca103d94b60 | 532 | previous_main_time = main_time; |
imadaemi | 3:eca103d94b60 | 533 | main_time = main_timer.read_ms(); |
imadaemi | 3:eca103d94b60 | 534 | if(main_time >= RESET_TIME){ |
imadaemi | 3:eca103d94b60 | 535 | main_timer.reset(); |
imadaemi | 3:eca103d94b60 | 536 | time_reset_counter++; |
imadaemi | 3:eca103d94b60 | 537 | } |
imadaemi | 3:eca103d94b60 | 538 | |
imadaemi | 1:3151936d9c55 | 539 | //Nichrome |
imadaemi | 3:eca103d94b60 | 540 | nich_status = nich.status; |
imadaemi | 2:980edad0eea2 | 541 | |
imadaemi | 2:980edad0eea2 | 542 | //GPS |
imadaemi | 2:980edad0eea2 | 543 | lat = gps.Latitude(); |
imadaemi | 2:980edad0eea2 | 544 | lon = gps.Longitude(); |
imadaemi | 2:980edad0eea2 | 545 | height = gps.Height(); |
imadaemi | 3:eca103d94b60 | 546 | satellite_number = gps.Number(); |
imadaemi | 2:980edad0eea2 | 547 | //pc.printf("%f\t%f\t%f\t\r\n", lat, lon, height); |
imadaemi | 2:980edad0eea2 | 548 | |
imadaemi | 2:980edad0eea2 | 549 | //LPS22HB |
imadaemi | 3:eca103d94b60 | 550 | float press_tmp; |
imadaemi | 3:eca103d94b60 | 551 | //lps22hb.read_press(&press); |
imadaemi | 3:eca103d94b60 | 552 | lps22hb.read_press(&press_tmp); |
imadaemi | 3:eca103d94b60 | 553 | |
imadaemi | 3:eca103d94b60 | 554 | if(press_tmp > 500.0){ |
imadaemi | 3:eca103d94b60 | 555 | press = press_tmp; |
imadaemi | 3:eca103d94b60 | 556 | } |
imadaemi | 3:eca103d94b60 | 557 | |
imadaemi | 3:eca103d94b60 | 558 | if(ground_press == -1.0){ |
imadaemi | 3:eca103d94b60 | 559 | ground_press = press; |
imadaemi | 3:eca103d94b60 | 560 | } |
imadaemi | 3:eca103d94b60 | 561 | |
imadaemi | 2:980edad0eea2 | 562 | lps22hb.read_temp(&temp); |
imadaemi | 3:eca103d94b60 | 563 | |
imadaemi | 3:eca103d94b60 | 564 | if(ground_temp == -1.0){ |
imadaemi | 3:eca103d94b60 | 565 | ground_temp = temp; |
imadaemi | 3:eca103d94b60 | 566 | } |
imadaemi | 3:eca103d94b60 | 567 | |
imadaemi | 3:eca103d94b60 | 568 | altitude = (ground_temp + 273.15)/0.0065*(1 - powf(press/ground_press, 287 * 0.0065/9.80665)); |
imadaemi | 2:980edad0eea2 | 569 | //pc.printf("%f\t%f\t%f\r\n",press, temp, altitude); |
imadaemi | 2:980edad0eea2 | 570 | |
imadaemi | 3:eca103d94b60 | 571 | //lpf_altitude |
imadaemi | 3:eca103d94b60 | 572 | previous_lpf_altitude = lpf_altitude; |
imadaemi | 3:eca103d94b60 | 573 | lpf_altitude += LPF_COEFFICIENT_ALT*(altitude - previous_lpf_altitude); |
imadaemi | 3:eca103d94b60 | 574 | |
imadaemi | 3:eca103d94b60 | 575 | //pc.printf("%f\t%f\t\r\n",altitude,lpf_altitude); |
imadaemi | 3:eca103d94b60 | 576 | |
imadaemi | 3:eca103d94b60 | 577 | //velocity計算 |
imadaemi | 3:eca103d94b60 | 578 | velocity = (lpf_altitude - previous_lpf_altitude)*1000/(main_time - previous_main_time); |
imadaemi | 3:eca103d94b60 | 579 | //pc.printf("********************\r\n"); |
imadaemi | 3:eca103d94b60 | 580 | //pc.printf("lpf altitude : %f\r\n",lpf_altitude); |
imadaemi | 3:eca103d94b60 | 581 | //pc.printf("main time : %f\r\n",main_time/1000); |
imadaemi | 3:eca103d94b60 | 582 | //pc.printf("altitude def: %f\r\n",lpf_altitude - previous_lpf_altitude); |
imadaemi | 3:eca103d94b60 | 583 | //pc.printf("velocity : %f\r\n",velocity); |
imadaemi | 3:eca103d94b60 | 584 | //pc.printf("time def: %f\r\n",main_time - previous_main_time); |
imadaemi | 3:eca103d94b60 | 585 | //pc.printf("********************\r\n"); |
imadaemi | 3:eca103d94b60 | 586 | |
imadaemi | 3:eca103d94b60 | 587 | //lpf_velocity |
imadaemi | 3:eca103d94b60 | 588 | previous_lpf_velocity = lpf_velocity; |
imadaemi | 3:eca103d94b60 | 589 | lpf_velocity += LPF_COEFFICIENT_VEL*(velocity - previous_lpf_velocity); |
imadaemi | 3:eca103d94b60 | 590 | //pc.printf("%f\t%f\t%f\t%f\r\n",altitude,lpf_altitude,velocity,lpf_velocity); |
imadaemi | 3:eca103d94b60 | 591 | |
imadaemi | 2:980edad0eea2 | 592 | //MPU9250 |
imadaemi | 2:980edad0eea2 | 593 | mpu9250.getAll(imu, mag); |
imadaemi | 3:eca103d94b60 | 594 | //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 | 2:980edad0eea2 | 595 | |
imadaemi | 2:980edad0eea2 | 596 | //INA226 |
imadaemi | 2:980edad0eea2 | 597 | ina226_main.get_Voltage_current(&voltage_main, ¤t_main); |
imadaemi | 2:980edad0eea2 | 598 | ina226_sep.get_Voltage_current(&voltage_sep, ¤t_sep); |
imadaemi | 2:980edad0eea2 | 599 | //pc.printf("MainVol : %.2f, SepVol : %.2f, MainCur : %.2f, SepCur : %.2f\r\n", voltage_main, voltage_sep, current_main, current_sep); |
imadaemi | 2:980edad0eea2 | 600 | //pc.printf("MainVol : %f, SepVol : %f, MainCur : %f, SepCur : %f\r\n", voltage_main*(1/1000), voltage_sep, current_main, current_sep); |
imadaemi | 2:980edad0eea2 | 601 | } |
imadaemi | 2:980edad0eea2 | 602 | |
imadaemi | 2:980edad0eea2 | 603 | // *************************************************** |
imadaemi | 2:980edad0eea2 | 604 | // 取得データを地上に送信 |
imadaemi | 2:980edad0eea2 | 605 | // *************************************************** |
imadaemi | 2:980edad0eea2 | 606 | void SendData(){ |
imadaemi | 2:980edad0eea2 | 607 | if(!header_set){ |
imadaemi | 2:980edad0eea2 | 608 | im920.header((char)HEADER_DATA); |
imadaemi | 2:980edad0eea2 | 609 | header_set = true; |
imadaemi | 2:980edad0eea2 | 610 | } |
imadaemi | 2:980edad0eea2 | 611 | |
imadaemi | 3:eca103d94b60 | 612 | ///pc.printf("********************\r\n"); |
imadaemi | 3:eca103d94b60 | 613 | |
imadaemi | 3:eca103d94b60 | 614 | //time_reset_counter |
imadaemi | 3:eca103d94b60 | 615 | im920.write((short)time_reset_counter); |
imadaemi | 3:eca103d94b60 | 616 | im_buf_n += 2; |
imadaemi | 3:eca103d94b60 | 617 | //MainTime |
imadaemi | 3:eca103d94b60 | 618 | im920.write((float)main_time); |
imadaemi | 3:eca103d94b60 | 619 | im_buf_n += 4; |
imadaemi | 3:eca103d94b60 | 620 | |
imadaemi | 3:eca103d94b60 | 621 | //mode |
imadaemi | 3:eca103d94b60 | 622 | ///pc.printf("mode : %c\r\n",mode); |
imadaemi | 3:eca103d94b60 | 623 | im920.write((char)mode); |
imadaemi | 3:eca103d94b60 | 624 | im_buf_n ++; |
imadaemi | 3:eca103d94b60 | 625 | |
imadaemi | 3:eca103d94b60 | 626 | //flightpin |
imadaemi | 3:eca103d94b60 | 627 | bitshift_flightpin_status = flightpin_status << 0; |
imadaemi | 3:eca103d94b60 | 628 | //pc.printf("flightpin bool : %d\r\n",flightpin_status); |
imadaemi | 3:eca103d94b60 | 629 | //pc.printf("flightpin bool : %d\r\n",bitshift_flightpin_status); |
imadaemi | 2:980edad0eea2 | 630 | |
imadaemi | 2:980edad0eea2 | 631 | //Nichrome |
imadaemi | 3:eca103d94b60 | 632 | bitshift_nich_status = nich_status << 1; |
imadaemi | 3:eca103d94b60 | 633 | //pc.printf("nich bool : %d\r\n",nich_status); |
imadaemi | 3:eca103d94b60 | 634 | //pc.printf("nich bitshift : %d\r\n",bitshift_nich_status); |
imadaemi | 3:eca103d94b60 | 635 | |
imadaemi | 3:eca103d94b60 | 636 | //頂点検知 |
imadaemi | 3:eca103d94b60 | 637 | bitshift_top_detect = top_detect << 2; |
imadaemi | 3:eca103d94b60 | 638 | |
imadaemi | 3:eca103d94b60 | 639 | //保存データのステータス |
imadaemi | 3:eca103d94b60 | 640 | bitshift_save_data_status = save_data_status << 3; |
imadaemi | 3:eca103d94b60 | 641 | |
imadaemi | 3:eca103d94b60 | 642 | //bool8個を1つのcharで送るとき |
imadaemi | 3:eca103d94b60 | 643 | bitshift_sum = bitshift_flightpin_status | bitshift_nich_status | bitshift_top_detect | bitshift_save_data_status; |
imadaemi | 3:eca103d94b60 | 644 | im920.write((char)bitshift_sum); |
imadaemi | 3:eca103d94b60 | 645 | im_buf_n ++; |
imadaemi | 3:eca103d94b60 | 646 | |
imadaemi | 3:eca103d94b60 | 647 | //EEPROM Number |
imadaemi | 3:eca103d94b60 | 648 | im920.write((char)eeprom_number); |
imadaemi | 1:3151936d9c55 | 649 | im_buf_n ++; |
imadaemi | 1:3151936d9c55 | 650 | |
imadaemi | 0:4e38f8b1c183 | 651 | //GPS |
imadaemi | 3:eca103d94b60 | 652 | ///pc.printf("Latitude : %f\r\n",lat); |
imadaemi | 3:eca103d94b60 | 653 | ///pc.printf("Longitude : %f\r\n",lon); |
imadaemi | 3:eca103d94b60 | 654 | ///pc.printf("Height : %f\r\n",height); |
imadaemi | 0:4e38f8b1c183 | 655 | im920.write((float)lat); |
imadaemi | 3:eca103d94b60 | 656 | im_buf_n += 4; |
imadaemi | 0:4e38f8b1c183 | 657 | im920.write((float)lon); |
imadaemi | 3:eca103d94b60 | 658 | im_buf_n += 4; |
imadaemi | 3:eca103d94b60 | 659 | /* |
imadaemi | 0:4e38f8b1c183 | 660 | im920.write((float)height); |
imadaemi | 3:eca103d94b60 | 661 | im_buf_n += 4; |
imadaemi | 3:eca103d94b60 | 662 | */ |
imadaemi | 0:4e38f8b1c183 | 663 | |
imadaemi | 0:4e38f8b1c183 | 664 | //LPS22HB |
imadaemi | 3:eca103d94b60 | 665 | ///pc.printf("Pressure : %f\r\n",press); |
imadaemi | 3:eca103d94b60 | 666 | ///pc.printf("Temperarure : %f\r\n",temp); |
imadaemi | 3:eca103d94b60 | 667 | ///pc.printf("Altitude : %f\r\n",altitude); |
imadaemi | 0:4e38f8b1c183 | 668 | im920.write((float)press); |
imadaemi | 3:eca103d94b60 | 669 | im_buf_n += 4; |
imadaemi | 3:eca103d94b60 | 670 | im920.write((short)(temp*TEMP_MULTIPLIER)); |
imadaemi | 3:eca103d94b60 | 671 | im_buf_n += 2; |
imadaemi | 3:eca103d94b60 | 672 | im920.write((short)(lpf_altitude*LPF_ALT_MULTIPLIER)); |
imadaemi | 3:eca103d94b60 | 673 | im_buf_n += 2; |
imadaemi | 0:4e38f8b1c183 | 674 | |
imadaemi | 3:eca103d94b60 | 675 | im920.write((short)(lpf_velocity*LPF_VEL_MULTIPLIER)); |
imadaemi | 3:eca103d94b60 | 676 | im_buf_n += 2; |
imadaemi | 2:980edad0eea2 | 677 | |
imadaemi | 0:4e38f8b1c183 | 678 | //INA226 |
imadaemi | 3:eca103d94b60 | 679 | ///pc.printf("Vlotage Mian : %.2f\r\n",voltage_main); |
imadaemi | 3:eca103d94b60 | 680 | ///pc.printf("Current Main : %.2f\r\n",current_main); |
imadaemi | 3:eca103d94b60 | 681 | ///pc.printf("Voltage Sep : %.2f\r\n",voltage_sep); |
imadaemi | 3:eca103d94b60 | 682 | ///pc.printf("Current Sep : %.2f\r\n",current_sep); |
imadaemi | 3:eca103d94b60 | 683 | im920.write((short)voltage_main); |
imadaemi | 3:eca103d94b60 | 684 | im_buf_n += 2; |
imadaemi | 3:eca103d94b60 | 685 | im920.write((short)current_main); |
imadaemi | 3:eca103d94b60 | 686 | im_buf_n += 2; |
imadaemi | 3:eca103d94b60 | 687 | im920.write((short)voltage_sep); |
imadaemi | 3:eca103d94b60 | 688 | im_buf_n += 2; |
imadaemi | 3:eca103d94b60 | 689 | im920.write((short)current_sep); |
imadaemi | 3:eca103d94b60 | 690 | im_buf_n += 2; |
imadaemi | 1:3151936d9c55 | 691 | |
imadaemi | 3:eca103d94b60 | 692 | ///pc.printf("********************\r\n\r\n"); |
imadaemi | 1:3151936d9c55 | 693 | |
imadaemi | 0:4e38f8b1c183 | 694 | if(header_set){ |
imadaemi | 0:4e38f8b1c183 | 695 | im920.send(); |
imadaemi | 2:980edad0eea2 | 696 | //pc.printf("Send : %s\r\n", im_buf); |
imadaemi | 0:4e38f8b1c183 | 697 | header_set = false; |
imadaemi | 0:4e38f8b1c183 | 698 | for(int i = 0; i < im_buf_n; i ++){ |
imadaemi | 0:4e38f8b1c183 | 699 | im_buf[i] = '\0'; |
imadaemi | 0:4e38f8b1c183 | 700 | } |
imadaemi | 0:4e38f8b1c183 | 701 | im_buf_n = 0; |
imadaemi | 0:4e38f8b1c183 | 702 | } |
imadaemi | 0:4e38f8b1c183 | 703 | } |
imadaemi | 0:4e38f8b1c183 | 704 | |
imadaemi | 3:eca103d94b60 | 705 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 706 | // 打上げ時刻を送信 |
imadaemi | 3:eca103d94b60 | 707 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 708 | void SendLaunchTime(){ |
imadaemi | 3:eca103d94b60 | 709 | if(!header_set){ |
imadaemi | 3:eca103d94b60 | 710 | im920.header((char)HEADER_LAUNCH_TIME); |
imadaemi | 3:eca103d94b60 | 711 | header_set = true; |
imadaemi | 3:eca103d94b60 | 712 | } |
imadaemi | 3:eca103d94b60 | 713 | pc.printf("launch time : %f\r\n",launch_time); |
imadaemi | 3:eca103d94b60 | 714 | im920.write((float)launch_time); |
imadaemi | 3:eca103d94b60 | 715 | im_buf_n += 4; |
imadaemi | 3:eca103d94b60 | 716 | im920.write((short)time_reset_counter); |
imadaemi | 3:eca103d94b60 | 717 | im_buf_n += 2; |
imadaemi | 3:eca103d94b60 | 718 | send_launch_time_status = true; |
imadaemi | 3:eca103d94b60 | 719 | if(header_set){ |
imadaemi | 3:eca103d94b60 | 720 | im920.send(); |
imadaemi | 3:eca103d94b60 | 721 | pc.printf("Send launch time\r\n"); |
imadaemi | 3:eca103d94b60 | 722 | header_set = false; |
imadaemi | 3:eca103d94b60 | 723 | for(int i = 0; i < im_buf_n; i ++){ |
imadaemi | 3:eca103d94b60 | 724 | im_buf[i] = '\0'; |
imadaemi | 3:eca103d94b60 | 725 | } |
imadaemi | 3:eca103d94b60 | 726 | im_buf_n = 0; |
imadaemi | 3:eca103d94b60 | 727 | } |
imadaemi | 3:eca103d94b60 | 728 | } |
imadaemi | 2:980edad0eea2 | 729 | |
imadaemi | 0:4e38f8b1c183 | 730 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 731 | // EEPROMにデータを書き込む |
imadaemi | 0:4e38f8b1c183 | 732 | // *************************************************** |
imadaemi | 3:eca103d94b60 | 733 | void SaveData(){ |
imadaemi | 3:eca103d94b60 | 734 | if(eeprom_group_counter < 4){ |
imadaemi | 3:eca103d94b60 | 735 | //pc.printf("Save to EEPROM\r\n"); |
imadaemi | 3:eca103d94b60 | 736 | int eep_buf = 0; |
imadaemi | 0:4e38f8b1c183 | 737 | |
imadaemi | 3:eca103d94b60 | 738 | /* |
imadaemi | 3:eca103d94b60 | 739 | for(int i = eep_buf; i < 128; i++){ |
imadaemi | 3:eca103d94b60 | 740 | //ptr = EEPROM.chargeBuff((char)0x02); |
imadaemi | 0:4e38f8b1c183 | 741 | ptr = EEPROM.chargeBuff((int)n++); |
imadaemi | 3:eca103d94b60 | 742 | } |
imadaemi | 3:eca103d94b60 | 743 | */ |
imadaemi | 3:eca103d94b60 | 744 | ptr = EEPROM.chargeBuff((char)time_reset_counter); |
imadaemi | 3:eca103d94b60 | 745 | eep_buf += 1; |
imadaemi | 3:eca103d94b60 | 746 | ptr = EEPROM.chargeBuff((int)main_time); |
imadaemi | 3:eca103d94b60 | 747 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 748 | //ptr = EEPROM.chargeBuff((float)flight_time); |
imadaemi | 3:eca103d94b60 | 749 | //eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 750 | |
imadaemi | 3:eca103d94b60 | 751 | ptr = EEPROM.chargeBuff((float)lat); |
imadaemi | 3:eca103d94b60 | 752 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 753 | ptr = EEPROM.chargeBuff((float)lon); |
imadaemi | 3:eca103d94b60 | 754 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 755 | ptr = EEPROM.chargeBuff((float)height); |
imadaemi | 3:eca103d94b60 | 756 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 757 | ptr = EEPROM.chargeBuff((char)satellite_number); |
imadaemi | 3:eca103d94b60 | 758 | eep_buf += 1; |
imadaemi | 3:eca103d94b60 | 759 | |
imadaemi | 3:eca103d94b60 | 760 | ptr = EEPROM.chargeBuff((float)press); |
imadaemi | 3:eca103d94b60 | 761 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 762 | ptr = EEPROM.chargeBuff((float)temp); |
imadaemi | 3:eca103d94b60 | 763 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 764 | ptr = EEPROM.chargeBuff((float)altitude); |
imadaemi | 3:eca103d94b60 | 765 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 766 | ptr = EEPROM.chargeBuff((float)lpf_altitude); |
imadaemi | 3:eca103d94b60 | 767 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 768 | |
imadaemi | 3:eca103d94b60 | 769 | ptr = EEPROM.chargeBuff((float)velocity); |
imadaemi | 3:eca103d94b60 | 770 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 771 | ptr = EEPROM.chargeBuff((float)lpf_velocity); |
imadaemi | 3:eca103d94b60 | 772 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 773 | |
imadaemi | 3:eca103d94b60 | 774 | ptr = EEPROM.chargeBuff((float)imu[0]); |
imadaemi | 3:eca103d94b60 | 775 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 776 | ptr = EEPROM.chargeBuff((float)imu[1]); |
imadaemi | 3:eca103d94b60 | 777 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 778 | ptr = EEPROM.chargeBuff((float)imu[2]); |
imadaemi | 3:eca103d94b60 | 779 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 780 | ptr = EEPROM.chargeBuff((float)imu[3]); |
imadaemi | 3:eca103d94b60 | 781 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 782 | ptr = EEPROM.chargeBuff((float)imu[4]); |
imadaemi | 3:eca103d94b60 | 783 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 784 | ptr = EEPROM.chargeBuff((float)imu[5]); |
imadaemi | 3:eca103d94b60 | 785 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 786 | |
imadaemi | 3:eca103d94b60 | 787 | ptr = EEPROM.chargeBuff((float)mag[0]); |
imadaemi | 3:eca103d94b60 | 788 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 789 | ptr = EEPROM.chargeBuff((float)mag[1]); |
imadaemi | 3:eca103d94b60 | 790 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 791 | ptr = EEPROM.chargeBuff((float)mag[2]); |
imadaemi | 3:eca103d94b60 | 792 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 793 | |
imadaemi | 3:eca103d94b60 | 794 | ptr = EEPROM.chargeBuff((float)(voltage_main/1000)); |
imadaemi | 3:eca103d94b60 | 795 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 796 | ptr = EEPROM.chargeBuff((float)(current_main/1000)); |
imadaemi | 3:eca103d94b60 | 797 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 798 | ptr = EEPROM.chargeBuff((float)(voltage_sep/1000)); |
imadaemi | 3:eca103d94b60 | 799 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 800 | ptr = EEPROM.chargeBuff((float)(current_sep/1000)); |
imadaemi | 3:eca103d94b60 | 801 | eep_buf += 4; |
imadaemi | 3:eca103d94b60 | 802 | |
imadaemi | 3:eca103d94b60 | 803 | ptr = EEPROM.chargeBuff((char)mode); |
imadaemi | 3:eca103d94b60 | 804 | eep_buf ++; |
imadaemi | 3:eca103d94b60 | 805 | ptr = EEPROM.chargeBuff((char)flightpin_status); |
imadaemi | 3:eca103d94b60 | 806 | eep_buf ++; |
imadaemi | 3:eca103d94b60 | 807 | ptr = EEPROM.chargeBuff((char)nich_status); |
imadaemi | 3:eca103d94b60 | 808 | eep_buf ++; |
imadaemi | 3:eca103d94b60 | 809 | |
imadaemi | 3:eca103d94b60 | 810 | //ptr = EEPROM.chargeBuff((int)n++); |
imadaemi | 3:eca103d94b60 | 811 | //ptr = EEPROM.chargeBuff((char)0xff); |
imadaemi | 3:eca103d94b60 | 812 | //ptr = EEPROM.chargeBuff((int)0); |
imadaemi | 3:eca103d94b60 | 813 | |
imadaemi | 3:eca103d94b60 | 814 | //pc.printf("%d\r\n",eep_buf); |
imadaemi | 3:eca103d94b60 | 815 | |
imadaemi | 3:eca103d94b60 | 816 | for(int i = eep_buf; i < 128; i++){ |
imadaemi | 3:eca103d94b60 | 817 | ptr = EEPROM.chargeBuff((char)0x00); |
imadaemi | 3:eca103d94b60 | 818 | } |
imadaemi | 3:eca103d94b60 | 819 | |
imadaemi | 3:eca103d94b60 | 820 | //pc.printf("ptr : %d\r\n",ptr); |
imadaemi | 3:eca103d94b60 | 821 | EEPROM.writeBuff(); |
imadaemi | 3:eca103d94b60 | 822 | eeprom_ptr = EEPROM.setNextPage(); |
imadaemi | 3:eca103d94b60 | 823 | if(eeprom_ptr == 0x01000000 || eeprom_ptr == 0x02000000 || eeprom_ptr == 0x03000000 || eeprom_ptr == 0x04000000){ |
imadaemi | 3:eca103d94b60 | 824 | eeprom_number++; |
imadaemi | 3:eca103d94b60 | 825 | pc.printf("eeprom_number : %d\r\n",eeprom_number); |
imadaemi | 3:eca103d94b60 | 826 | } |
imadaemi | 3:eca103d94b60 | 827 | //pc.printf("eeprom_ptr: %x\r\n", eeprom_ptr); |
imadaemi | 3:eca103d94b60 | 828 | |
imadaemi | 3:eca103d94b60 | 829 | /* |
imadaemi | 3:eca103d94b60 | 830 | if(eeprom_ptr == 0x01000000){ |
imadaemi | 3:eca103d94b60 | 831 | ptr = 0; |
imadaemi | 3:eca103d94b60 | 832 | eeprom_ptr = 0; |
imadaemi | 3:eca103d94b60 | 833 | plexer_num++; |
imadaemi | 3:eca103d94b60 | 834 | setEEPROMGroup(plexer_num); |
imadaemi | 3:eca103d94b60 | 835 | EEPROM.setWriteAddr(1, 0, 0x00, 0x00); |
imadaemi | 3:eca103d94b60 | 836 | } |
imadaemi | 3:eca103d94b60 | 837 | */ |
imadaemi | 3:eca103d94b60 | 838 | |
imadaemi | 3:eca103d94b60 | 839 | if(eeprom_ptr == 0x04000000){ |
imadaemi | 3:eca103d94b60 | 840 | eeprom_ptr = 0; |
imadaemi | 3:eca103d94b60 | 841 | eeprom_group_counter++; |
imadaemi | 3:eca103d94b60 | 842 | setEEPROMGroup(eeprom_group_counter); |
imadaemi | 3:eca103d94b60 | 843 | EEPROM.setWriteAddr(1, 0, 0x00, 0x00); |
imadaemi | 3:eca103d94b60 | 844 | pc.printf("EEPROM_Group : %d\r\n",eeprom_group_counter); |
imadaemi | 3:eca103d94b60 | 845 | //pc.printf("SetWriteAddr\r\n"); |
imadaemi | 0:4e38f8b1c183 | 846 | } |
imadaemi | 0:4e38f8b1c183 | 847 | } |
imadaemi | 0:4e38f8b1c183 | 848 | } |
imadaemi | 0:4e38f8b1c183 | 849 | |
imadaemi | 0:4e38f8b1c183 | 850 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 851 | // マルチプレクサで使うEEPROMを変更する |
imadaemi | 0:4e38f8b1c183 | 852 | // *************************************************** |
imadaemi | 0:4e38f8b1c183 | 853 | void setEEPROMGroup(int group_num){ |
imadaemi | 0:4e38f8b1c183 | 854 | switch(group_num){ |
imadaemi | 0:4e38f8b1c183 | 855 | case 0: |
imadaemi | 0:4e38f8b1c183 | 856 | pinA = 0; |
imadaemi | 0:4e38f8b1c183 | 857 | pinB = 0; |
imadaemi | 0:4e38f8b1c183 | 858 | pinC = 0; |
imadaemi | 0:4e38f8b1c183 | 859 | break; |
imadaemi | 0:4e38f8b1c183 | 860 | |
imadaemi | 0:4e38f8b1c183 | 861 | case 1: |
imadaemi | 0:4e38f8b1c183 | 862 | pinA = 1; |
imadaemi | 0:4e38f8b1c183 | 863 | pinB = 0; |
imadaemi | 0:4e38f8b1c183 | 864 | pinC = 0; |
imadaemi | 0:4e38f8b1c183 | 865 | break; |
imadaemi | 0:4e38f8b1c183 | 866 | |
imadaemi | 0:4e38f8b1c183 | 867 | case 2: |
imadaemi | 0:4e38f8b1c183 | 868 | pinA = 0; |
imadaemi | 0:4e38f8b1c183 | 869 | pinB = 1; |
imadaemi | 0:4e38f8b1c183 | 870 | pinC = 0; |
imadaemi | 0:4e38f8b1c183 | 871 | break; |
imadaemi | 0:4e38f8b1c183 | 872 | |
imadaemi | 0:4e38f8b1c183 | 873 | case 3: |
imadaemi | 0:4e38f8b1c183 | 874 | pinA = 1; |
imadaemi | 0:4e38f8b1c183 | 875 | pinB = 1; |
imadaemi | 0:4e38f8b1c183 | 876 | pinC = 0; |
imadaemi | 0:4e38f8b1c183 | 877 | break; |
imadaemi | 0:4e38f8b1c183 | 878 | } |
imadaemi | 0:4e38f8b1c183 | 879 | } |