Diff: main.cpp

Revision:

0:4e38f8b1c183

Child:

1:3151936d9c55

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 29 16:38:36 2021 +0000
@@ -0,0 +1,393 @@
+#include "mbed.h"
+#include "IM920.h"
+#include "GPS_interrupt.h"
+#include "PQLPS22HB.h"
+#include "mpu9250_i2c.h"
+#include "INA226.h"
+#include "Nichrome_lib.h"
+#include "EEPROM_lib.h"
+
+#define CURRENT_LOCATION_PRESS 1022.624268//投下前に設定
+#define CURRENT_LOCATION_TEMP 10.650000//投下前に設定
+#define ACC_RANGE _16G
+#define GYRO_RANGE _2000DPS
+
+// ***************************************************
+// コンストラクタ
+// ***************************************************
+Serial pc(USBTX, USBRX, 115200);
+Serial gps_serial(p9, p10, 115200);
+Serial im920_serial(p13, p14, 115200);
+
+I2C i2c(p28, p27);
+
+IM920 im920(im920_serial, pc, 115200);
+GPS_interrupt gps(&gps_serial);
+LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW);
+mpu9250 mpu9250(i2c,AD0_HIGH);
+myINA226 ina226_main(i2c, myINA226::A1_GND, myINA226::A0_GND);
+myINA226 ina226_sep(i2c, myINA226::A1_VDD, myINA226::A0_VDD);
+Nichrome_lib nich(p20);
+EEPROM_lib EEPROM(i2c, 1);
+
+DigitalOut pinA(p21);
+DigitalOut pinB(p22);
+DigitalOut pinC(p23);
+
+// ***************************************************
+// 関数の宣言
+// ***************************************************
+void uplink();
+void echoIM();
+void Separate();
+void StopSeparate();
+void SetSensor();
+void GetData();
+void WriteEEPROM();
+void setEEPROMGroup(int group_num);
+
+// ***************************************************
+// 無線のヘッダまとめ
+// ***************************************************
+const char HEADER_SETUP = 0x01;
+// 0xA1
+
+const char HEADER_DATA = 0xA2;
+// 0xA2
+//
+
+const char HEADER_ECHO = 0xA5;
+// 0xA5,コマンド
+//   1     1
+
+// ***************************************************
+// 変数の宣言
+// ***************************************************
+bool header_set = false;
+char im_buf[55];//16なのって意味ある？
+int im_buf_n = 0;
+
+float lat, lon, height;
+float press, temp, altitude;
+
+float imu[6], mag[3];
+float mag_geo[3];
+
+float voltage_main, voltage_sep;
+float current_main, current_sep;
+
+// ***************************************************
+// メイン関数
+// ***************************************************
+int main() {
+    pc.printf("Hello Main!\r\n");
+    SetSensor();
+    im920.attach(&uplink, 0xF0);
+
+    while(1){
+    GetData();
+    wait(0.5f);
+    }
+}
+// ***************************************************
+// アップリンク（地上局から）
+// ***************************************************
+void uplink(){
+    echoIM();
+    switch(im920.data[1]){
+        case 0x01:
+        pc.printf("********************\r\n");
+        pc.printf("SEPARATE\r\n");
+        pc.printf("********************\r\n\r\n");
+        Separate();
+        break;
+        
+        case 0x00:
+        pc.printf("********************\r\n");
+        pc.printf("STOP SEPARATE\r\n");
+        pc.printf("********************\r\n\r\n");
+        StopSeparate();
+        break;
+    }
+}
+
+// ***************************************************
+// 無線信号の送り返し
+// ***************************************************
+void echoIM(){
+    im920.header(HEADER_ECHO);
+    im920.write(im920.data[1]);
+    im920.send();
+}
+
+// ***************************************************
+// 分離実行
+// ***************************************************
+void Separate(){
+    nich.fire(3.0f);
+}
+
+void StopSeparate(){
+     nich.fire_off();
+}
+
+// ***************************************************
+// センサーのセットアップ
+// ***************************************************
+void SetSensor(){
+    pc.printf("\r\n");
+    for(int i = 0; i < 20; i++){
+    pc.printf("*");
+    }
+    pc.printf("\r\n");
+    pc.printf("Start Setting\r\n");
+    
+    if(!header_set){
+        im920.header((char)HEADER_SETUP);
+        header_set = true;
+    }
+    
+    //GPS
+    if(gps.gps_readable == true){
+        pc.printf("GPS         : OK!\r\n");
+        im920.write((char)0x01);
+        im_buf[im_buf_n] = '1';
+        im_buf_n ++;
+    }else{
+        pc.printf("GPS         : NG...\r\n");
+        im920.write((char)0x00);
+        im_buf[im_buf_n] = '0';
+        im_buf_n ++;
+    }
+    
+    //LPS22HB
+    lps22hb.begin();
+    if(lps22hb.test() == true){
+        pc.printf("LPS22HB     : OK!\r\n");
+        im920.write((char)0x01);
+        im_buf[im_buf_n] = '1';
+        im_buf_n ++;
+    }else{
+        pc.printf("LPS22HB     : NG...\r\n");
+        im920.write((char)0x00);
+        im_buf[im_buf_n] = '0';
+        im_buf_n ++;
+    }
+    
+    //MPU9250
+    if(mpu9250.sensorTest() == true){
+        pc.printf("MPU9250     : OK!\r\n");
+        im920.write((char)0x01);
+        im_buf[im_buf_n] = '1';
+        im_buf_n ++;
+    }else{
+        pc.printf("MPU9250     : NG...\r\n");
+        im920.write((char)0x00);
+        im_buf[im_buf_n] = '0';
+        im_buf_n ++;
+    }
+    if(mpu9250.mag_sensorTest() == true){
+        pc.printf("MPU9250 MAG : OK!\r\n");
+        im920.write((char)0x01);
+        im_buf[im_buf_n] = '1';
+        im_buf_n ++;
+    }else{
+        pc.printf("MPU9250 MAG : NG...\r\n");
+        im920.write((char)0x00);
+        im_buf[im_buf_n] = '0';
+        im_buf_n ++;
+    }
+    mpu9250.setAcc(ACC_RANGE);
+    mpu9250.setGyro(GYRO_RANGE);
+    mpu9250.setOffset(0.528892327, -0.660206211, 0.757105891, -0.011691362, 0.025688783, 1.087885322, -159.750004, 121.425005, -392.700012);
+
+    //INA226
+    ina226_main.set_callibretion();
+    ina226_sep.set_callibretion();
+    ina226_main.setup(1);
+    ina226_sep.setup(1);
+    
+    if(ina226_main.Connection_check()==0){
+        pc.printf("INA226 Main : OK!\r\n");
+        im920.write((char)0x01);
+        im_buf[im_buf_n] = '1';
+        im_buf_n ++;
+    }else{
+        pc.printf("INA226 Main : NG...\r\n");
+        im920.write((char)0x00);
+        im_buf[im_buf_n] = '0';
+        im_buf_n ++;
+        }
+    if(ina226_sep.Connection_check()==0){
+        pc.printf("INA226 Sep  : OK!\r\n");
+        im920.write((char)0x01);
+        im_buf[im_buf_n] = '1';
+        im_buf_n ++;
+    }else{
+        pc.printf("INA226 Sep  : NG...\r\n");
+        im920.write((char)0x00);
+        im_buf[im_buf_n] = '0';
+        im_buf_n ++;
+    }
+    
+    if(header_set){
+        im920.send();
+        pc.printf("Send : %s\r\n", im_buf);
+        header_set = false;
+        for(int i = 0; i < im_buf_n; i ++){
+            im_buf[i] = '\0';
+        }
+        im_buf_n = 0;
+    }
+    
+    pc.printf("\r\n");
+    for(int i = 0; i < 20; i++){
+    pc.printf("*");
+    }
+    pc.printf("\r\n");
+}
+
+// ***************************************************
+// センサーのデータ取得
+// ***************************************************
+void GetData(){
+    if(!header_set){
+        im920.header((char)HEADER_DATA);
+        header_set = true;
+    }
+    
+    //GPS
+    lat = gps.Latitude();
+    lon = gps.Longitude();
+    height = gps.Height();
+    pc.printf("%f\t%f\t%f\t\r\n", lat, lon, height);
+    im920.write((float)lat);
+    im_buf_n ++;
+    im920.write((float)lon);
+    im_buf_n ++;
+    im920.write((float)height);
+    im_buf_n ++;
+    
+    //LPS22HB
+    lps22hb.read_press(&press);
+    lps22hb.read_temp(&temp);
+    altitude =  (CURRENT_LOCATION_TEMP + 273.15)/0.0065*(1 - powf(press/CURRENT_LOCATION_PRESS, 287 * 0.0065/9.80665));
+    pc.printf("%f\t%f\t%f\r\n",press, temp, altitude);
+    im920.write((float)press);
+    im_buf_n ++;
+    im920.write((float)temp);
+    im_buf_n ++;
+    im920.write((float)altitude);
+    im_buf_n ++;
+    
+    //MPU9250
+    /*
+    mpu9250.getAll(imu, mag);
+    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]);
+    
+    im920.write((float)imu[0]);
+    im_buf_n ++;
+    im920.write((float)imu[1]);
+    im_buf_n ++;
+    im920.write((float)imu[2]);
+    im_buf_n ++;
+    im920.write((float)imu[3]);
+    im_buf_n ++;
+    im920.write((float)imu[4]);
+    im_buf_n ++;
+    im920.write((float)imu[5]);
+    im_buf_n ++;
+    im920.write((float)mag[0]);
+    im_buf_n ++;
+    im920.write((float)mag[1]);
+    im_buf_n ++;
+    im920.write((float)mag[2]);
+    im_buf_n ++;  
+    */
+    //INA226
+    ina226_main.get_Voltage_current(&voltage_main, &current_main);
+    ina226_sep.get_Voltage_current(&voltage_sep, &current_sep);
+    pc.printf("MainVol : %.2f, SepVol : %.2f, MainCur : %.2f, SepCur : %.2f\r\n", voltage_main*(1/1000), voltage_sep, current_main, current_sep);
+    //pc.printf("MainVol : %f, SepVol : %f, MainCur : %f, SepCur : %f\r\n", voltage_main*(1/1000), voltage_sep, current_main, current_sep);
+    im920.write((float)voltage_main);
+    im_buf_n ++;
+    im920.write((float)current_main);
+    im_buf_n ++;
+    im920.write((float)voltage_sep);
+    im_buf_n ++;
+    im920.write((float)current_sep);
+    im_buf_n ++;
+
+    if(header_set){
+        im920.send();
+        pc.printf("Send : %s\r\n", im_buf);
+        header_set = false;
+        for(int i = 0; i < im_buf_n; i ++){
+            im_buf[i] = '\0';
+        }
+        im_buf_n = 0;
+    }
+}
+
+// ***************************************************
+// EEPROMにデータを書き込むプログラム
+// ***************************************************
+void WriteEEPROM(){
+    int ptr, n = 0;
+    int eeprom_ptr = 0;
+    
+    for(int i = 0; i < 4; i++){
+        pc.printf("Start to write %d EEPROM\r\n", i);
+        setEEPROMGroup(i);
+        EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
+        
+        while(1){
+            ptr = EEPROM.chargeBuff((int)n++);
+            //ptr = EEPROM.chargeBuff((char)0xff);
+            //ptr = EEPROM.chargeBuff((int)0);
+            if(ptr == 128){
+                EEPROM.writeBuff();
+                //ptr = EEPROM.setNextPage();
+                eeprom_ptr = EEPROM.setNextPage();
+                //pc.printf("eeprom_ptr: %08x\r\n", eeprom_ptr);
+            }
+            
+            if(eeprom_ptr == 0x01000000){
+                ptr = 0;
+                eeprom_ptr = 0;
+                break;
+            }
+        }
+    }
+}
+
+// ***************************************************
+// マルチプレクサで使うEEPROMを変更する
+// ***************************************************
+void setEEPROMGroup(int group_num){
+    switch(group_num){
+        case 0:
+        pinA = 0;
+        pinB = 0;
+        pinC = 0;
+        break;
+        
+        case 1:
+        pinA = 1;
+        pinB = 0;
+        pinC = 0;
+        break;
+        
+        case 2:
+        pinA = 0;
+        pinB = 1;
+        pinC = 0;
+        break;
+        
+        case 3:
+        pinA = 1;
+        pinB = 1;
+        pinC = 0;
+        break;
+    }
+}