File content as of revision 0:f3a52599183f:

#include "mbed.h"
#include "math.h"

#include "PQLPS22HB.h"
#include "mpu9250_i2c.h"
#include "GPS_interrupt.h"

#define ACC_RANGE _16G
#define GYRO_RANGE _2000DPS
#define SEA_LEVEL_PRESS 1029.3//投下前に設定
#define CURRENT_LOCATION_PRESS 1023.8//投下前に設定
#define CURRENT_LOCATION_TEMP 16.6//投下前に設定
#define LAT_GOAL 33.594909//投下前に計測
#define LON_GOAL 130.217779//投下前に計測
#define PI 3.14159265358979323846
#define R 6378137

Serial pc(USBTX,USBRX, 115200);
Serial xbee(p28, p27, 115200);
Serial openlog(p13, p14, 115200);
Serial gps_serial(p13, p14, 115200);
Serial xbee_raspi(p28, p27, 115200);
I2C i2c(p9, p10);
DigitalIn flightpin(p20);
DigitalOut pin(p22);

PwmOut motor_left26(p26);//左IN2
PwmOut motor_left25(p25);//左IN1
PwmOut motor_right24(p24);//右IN1
PwmOut motor_right23(p23);//右IN2

LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW);
mpu9250 mpu9250(i2c,AD0_HIGH);
GPS_interrupt gps(&gps_serial);

Timer timer;
Timeout nich_timeout;
Timeout nich_wait_timeout;
Timeout motor_timeout;
Ticker tick_mpu9250;
Ticker tick_lps22hb;
Ticker tick_gps;
Ticker tick_time;
Ticker tick_data;

void Phase1();
void Phase2();
void Phase3();
void Phase4();
void on_data_received();
void GetData();
void SetSensor();
void SetMpu9250();
void GetMpu9250();
void SetLps22hb();
void GetLps22hb();
void SetGPS();
void GetGPS();
void CalculateAltitude();
void FlightPin();
void CutNichrome(float nich_wait);
void NichromeOn();
void NichromeOff();
void CalcDirection(float lat, float lon, float lat_f, float lon_f);
void CalcDistance(float lat, float lon);
void MotorFront(float f_left,float f_right,float f_wait);
void MotorBack(float f_left,float f_right,float f_wait);
void MotorStop();

int cansat_phase = 1;
bool mpu9250_test;
bool mag_mpu9250_test;
float imu[6], mag[3];
float press, temp;
float altitude_ground, altitude;
float lat, lon, height;
bool nich_status = false;
float cansat_direction;
float cansat_distance;
float lat_0, lon_0;
int count_loop = 1;

int main(){
    Phase4();
}

void Phase1(){
    pc.printf("Phase_%d\r\n", cansat_phase);
    xbee.printf("Phase_%d\r\n", cansat_phase);
    SetSensor();
    tick_data.attach(&GetData, 1.0);
    cansat_phase = 2;
    pc.printf("SetSensor finished!\r\n");
    xbee.printf("SetSensor finished!\r\n");
}

void Phase2(){//フライトピンの検知
    pc.printf("Phese_%d\r\n", cansat_phase);
    xbee.printf("Phese_%d\r\n", cansat_phase);
    timer.start();
    while(cansat_phase == 2){
        if(flightpin == 1){
        break;
        }
    }
    cansat_phase = 3;
    pc.printf("FlightPin finished!\r\n");
    xbee.printf("FlightPin finished!\r\n");
}

void Phase3(){//ニクロム線による分離
    pc.printf("Phase_%d\r\n", cansat_phase);
    xbee.printf("Phase_%d\r\n", cansat_phase);
    while(cansat_phase == 3){
        CutNichrome(2.0f);
        break;
    }
    //MotorFront(0.95, 1.0, 5.0);//パラシュートから離れる
    //MotorStop();
    cansat_phase = 4;
    pc.printf("CutNichrome finished!\r\n");
    xbee.printf("CutNichrome finished!\r\n");
}

void Phase4(){//走行中
    wait(3.0f);
    while(1){
        GetGPS();
        if(lat == 0){
            pc.printf("GPS Data NG...\r\n");
            xbee.printf("GPS Data NG...\r\n");
        }else{
            break;
        }
    }

    GetGPS();
    lat_0 = lat;
    lon_0 = lon;

    MotorFront(1.0, 1.0, 3.0);
    while(1){
    GetGPS();

    xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0);   
                
    CalcDirection(lat, lon, lat_0, lon_0);
    float direc_n = cansat_direction;

    xbee.printf("%f\t\r\n", direc_n);
            
    if(-90 < direc_n  && direc_n <= 0){//右へ
        pc.printf("Go to the Right\r\n");
        xbee.printf("Go to the Right-90\r\n");
        MotorFront(1.0, 0, 0.25);

    }else if(-180 < direc_n && direc_n<= -90){//右へ
        pc.printf("Go to the Right\r\n");
        xbee.printf("Go to the Right-180\r\n");
        MotorFront(1.0, 0, 0.5);

    }else if(-270 < direc_n && direc_n <= -180){//左へ
        pc.printf("Go to the Left\r\n");
        xbee.printf("Go to the Left-270\r\n");
        MotorFront(0, 1.0, 0.5);

    }else if(-360 <= direc_n && direc_n <= -270){//左へ
        pc.printf("Go to the Left\r\n");
        xbee.printf("Go to the Left-360\r\n");
        MotorFront(0, 1.0, 0.25);
                 
    }else if(0 < direc_n && direc_n<= 90){//左へ
        pc.printf("Go to the Left\r\n");
        xbee.printf("Go to the Left90\r\n");
        MotorFront(0, 1.0, 0.25);

    }else if(90 < direc_n && direc_n <= 180){//左へ
        pc.printf("Go to the Left\r\n");
        xbee.printf("Go to the Left180\r\n");
        MotorFront(0, 1.0, 0.5);
                
    }else if(180 < direc_n && direc_n<= 270){//右へ
        pc.printf("Go to the Right\r\n");
        xbee.printf("Go to the Right270\r\n");
        MotorFront(1.0, 0, 0.5);

    }else if(270 < direc_n && direc_n <=360){//右へ
        pc.printf("Go to the Right\r\n");
        xbee.printf("Go to the Right360\r\n");
        MotorFront(1.0, 0, 0.25);
    }

    lat_0 = lat;
    lon_0 = lon;
    MotorFront(1.0, 1.0, 3.0);
    MotorStop();
    wait(5.0);
    
    count_loop = count_loop + 1;
    xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0); 
    }  
    
}
      
/*
void Phase5(){
    while(cansat_phase = 5){
        void on_data_received();
    }
}
*/
void on_data_received(){
    char data = xbee_raspi.getc();
    xbee_raspi.printf("%c", data);
    switch(data) {
        case 0x81://ゴール判定
            xbee_raspi.printf("red rate > 0.1\r\n");
            break;
        case 0x82://ゴール発見
            xbee_raspi.printf("red rate > 0.01\r\n");
            break;
        case 0x83://ゴールが見えない
            xbee_raspi.printf("cannot find red\r\n");
            break;
    }
}
/***************************************************
センサのセットアップ
***************************************************/
void SetSensor(){
    SetMpu9250();
    SetLps22hb();
    SetGPS();
}
    
void SetMpu9250(){//MPU9250のセットアップ
    mpu9250_test = mpu9250.sensorTest();
    mag_mpu9250_test = mpu9250.mag_sensorTest();
    if(mpu9250_test == true){
        pc.printf("MPU9250 OK!\r\n");
        xbee.printf("MPU9250 OK!\r\n");
    }else{
        pc.printf("MPU9250 NG...\r\n");
        xbee.printf("MPU9250 NG...\r\n");
    }
    if(mag_mpu9250_test == true){
        pc.printf("MPU9250 MAG OK!\r\n");
        xbee.printf("MPU9250 MAG OK!\r\n");
    }else{
        pc.printf("MPU9250 MAG NG...\r\n");
        xbee.printf("MPU9250 MAG NG...\r\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);
}

void SetLps22hb(){//LPS22HBのセットアップ
    lps22hb.begin();
    if(lps22hb.test() == true){
        pc.printf("LPS22HB OK!\r\n");
        xbee.printf("LPS22HB OK!\r\n");
    }else{
        pc.printf("LPS22HB NG...\r\n");
        xbee.printf("LPS22HB NG...\r\n");
    }
}

void SetGPS(){//GPSのセットアップ
//    gps.changeGPSFreq(10); 入れるとエラーが出る
    pc.printf("GPS OK!\r\n");
    xbee.printf("GPS OK!\r\n");
}

/***************************************************
データの取得
***************************************************/
void GetData(){
    GetMpu9250();
    GetLps22hb();
    GetGPS();
    pc.printf("%f\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%.7f\t%.7f\t%f\t%f\t%f\t\r\n", timer.read(), cansat_phase, imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2], press, temp, altitude, lat, lon, height, cansat_direction, cansat_distance);
    openlog.printf("%f\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%.7f\t%.7f\t%f\t%f\t%f\t\r\n", timer.read(), cansat_phase, imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2], press, temp, altitude, lat, lon, height, cansat_direction, cansat_distance);
}

void GetMpu9250(){
    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]);
    //xbee.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]);
    //openlog.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]);
}

void GetLps22hb(){
    lps22hb.read_press(&press);
    lps22hb.read_temp(&temp);
    //pc.printf("%f\t%f\t",press, temp);
}

void GetGPS(){
    lat = gps.Latitude();
    lon = gps.Longitude();
    height = gps.Height();
    //pc.printf("%f\t%f\t%f\t", lat, lon, height);
    //xbee.printf("%f\t%f\t%f\t", lat, lon, height);
    //openlog.printf("%f\t%f\t%f\t", lat, lon, height);
}

/***************************************************
制御計算
***************************************************/
void CalculateAltitude(){
    lps22hb.read_press(&press);
    lps22hb.read_temp(&temp);    
    altitude_ground = (powf(SEA_LEVEL_PRESS / CURRENT_LOCATION_PRESS, 1/5.257) - 1) * (CURRENT_LOCATION_TEMP + 273.15) / 0.0065;
    altitude = (powf(SEA_LEVEL_PRESS / press, 1/5.257) - 1) * (temp + 273.15) / 0.0065 - altitude_ground;
    pc.printf("%f\t\r\n",altitude);
}

void FlightPin(){
    //pc.printf("Hello FlightPin!\r\n");
    while(cansat_phase == 2){
        if(flightpin == 1){
        break;
        }
    }
    //pc.printf("Goodbye FlightPin!\r\n"); 
}

void CutNichrome(float nich_wait){
    NichromeOn();
    wait(nich_wait);
    NichromeOff();
}

void NichromeOn(){
    pin = 1;
    nich_status = true;
}

void NichromeOff(){
    pin = 0;
    nich_status = false;
}    

void CalcDirection(float lat, float lon, float lat_f, float lon_f){ //ヒュベニの公式を使いたい
    float dlat_g = (LAT_GOAL - lat) * (PI / 180);
    float dlon_g = (LON_GOAL - lon) * (PI / 180);
    float direc_g = atan2(dlat_g, dlon_g) * (180 / PI);

    float dlat = (lat - lat_f) * (PI / 180);
    float dlon = (lon - lon_f) * (PI / 180);
    float direc = atan2(dlat, dlon) * (180 / PI);

    cansat_direction = direc_g - direc;
}

void CalcDistance(float lat, float lon){
    float dlat_distance = R * (LAT_GOAL - lat) * (PI / 180);
    float dlon_distance = R * (LON_GOAL - lon) * (PI / 180) * cos(lat * (PI / 180));
    float distance_squared = dlat_distance * dlat_distance + dlon_distance * dlon_distance;
    cansat_distance = sqrt(distance_squared);
}

/***************************************************
モーター制御
***************************************************/
void MotorFront(float f_left,float f_right,float f_wait){
    motor_left26 = 0;//正転
    motor_left25 = f_left;
    motor_right24 = f_right;
    motor_right23 = 0;
    wait(f_wait);
}

void MotorBack(float b_left,float b_right,float b_wait){
    motor_left26 = b_left;//逆転
    motor_left25 = 0;
    motor_right24 = 0;
    motor_right23 = b_right;
    wait(b_wait);
}

void MotorStop(){
    motor_left26 = 0;//ストップ
    motor_left25 = 0;
    motor_right24 = 0;
    motor_right23 = 0;
}