File content as of revision 3:0bd9ad37f319:

/**********************************************/
//更新情報

/**********************************************/

#include "mbed.h"
#include "XBee.h"
#include "MBed_Adafruit_GPS.h"
//#include "AigamozuControlPackets.h"
#include "agzIDLIST.h"
#include "aigamozuSetting.h"
#include "HMC5883L.h"
#include "VNH5019.h"
#include "cansat.h"
#include "math.h"

#define SIGMA_MIN 0.0001

/////////////////////////////////////////
//
//Pin Setting
//
/////////////////////////////////////////
VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);


/////////////////////////////////////////
//
//Connection Setting
//
/////////////////////////////////////////

//Serial Connect Setting: PC <--> mbed
Serial pc(USBTX, USBRX);    

//Serial Connect Setting: GPS <--> mbed
Serial * gps_Serial;

//Serial Connect Setting: XBEE <--> mbed
XBee xbee(p13,p14);
ZBRxResponse zbRx = ZBRxResponse();

//set up GPS module

//set up AigamozuControlPackets library
//AigamozuControlPackets agz(agz_motorShield);
CanSat cansat(agz_motorShield);


/////////////////////////////////////////
//
//For Kalman data
//
/////////////////////////////////////////
#define FIRST_S2_1 1.0e-8
#define FIRST_S2_2 1.0e-6
#define COUNTER_MAX 10000
#define ERROR_RANGE 0.001

double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
double s2x_cur=FIRST_S2_1,s2x_prev=FIRST_S2_1,s2y_cur=FIRST_S2_1,s2y_prev=FIRST_S2_1;//緯度経度のの時刻tと時刻t-1での共分散
double s2_R=FIRST_S2_2;//GPSセンサの分散
double s2_Q=FIRST_S2_2;
double Kx=0,Ky=0;//カルマンゲイン
double zx,zy;//観測値
void Kalman(double Latitude,double Longitude);
int change = 0;



/////////////////////////////////////////
//
//Kalman Processing
//
/////////////////////////////////////////
void calc_Kalman(){
  //calc Kalman gain
  Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q);
  Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
  //estimate
  x_cur = x_prev + Kx*(zx-x_prev);
  y_cur = y_prev + Ky*(zy-y_prev);
  //calc sigma
  s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
  s2y_cur = (1-Ky)*(s2y_prev+s2_Q);

}

void Kalman(double Latitude,double Longitude){

    zx = Latitude;
    zy = Longitude;

    calc_Kalman();
    
    //更新
    x_prev = x_cur;
    y_prev = y_cur;
    s2x_prev = s2x_cur;
    s2y_prev = s2y_cur;
    
    //robotK\x,robotK_yに格納する
    cansat.set_robotKalman_x(x_cur);
    cansat.set_robotKalman_y(y_cur);
}

/////////////////////////////////////////
//
//Get GPS function
//
/////////////////////////////////////////

void Get_GPS(Adafruit_GPS *myGPS){
    static int flag = 0;
    
    if (myGPS->fix) {
        
        cansat.nowStatus = GPS_AVAIL;
        cansat.set_robot_x((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 10000.0));
        cansat.set_robot_y((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 10000.0));
    
        
        if(flag < COUNTER_MAX){
            flag++; 
        }
        if(flag == 5){
            x_prev = cansat.get_robot_x();
            y_prev = cansat.get_robot_y();
        }
            
        if(flag >= 6){
            if(abs(x_prev - cansat.get_robot_x()) < ERROR_RANGE && abs(y_prev - cansat.get_robot_y()) < ERROR_RANGE){
                Kalman(cansat.get_robot_x(), cansat.get_robot_y());
                change = 1;
            }
            else{
                change = 0;
            }            
                //printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
                //agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
                //agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
                //agz.get_agzCov_lati(),agz.get_agzCov_longi());
        }
    }
    else cansat.nowStatus = GPS_UNAVAIL;
    
} 

/******************************
スタンバイモード
******************************/
void standby(){
    ;
}

/******************************
落下モード
******************************/
void falling(){
                
}

/******************************
走行モード
******************************/
void running(){
            
}

/******************************
停止モード
******************************/
void stopping(){
            
            
}

/////////////////////////////////////////
//
//Main Processing
//
/////////////////////////////////////////
int main() {
    //start up time
    wait(3);
    //set pc frequency to 57600bps 
    pc.baud(PC_BAUD_RATE); 
    //set xbee frequency to 57600bps
    xbee.begin(XBEE_BAUD_RATE);    
        

    //GPS setting
    gps_Serial = new Serial(p28,p27);
    Adafruit_GPS myGPS(gps_Serial); 
    Timer refresh_Timer;
    const int refresh_Time = 1000; //refresh time in ms
    int count = 0;

    myGPS.begin(GPS_BAUD_RATE); 
    
    //GPS Send Command
    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
    myGPS.sendCommand(PGCMD_ANTENNA);
    
    wait_ms(2000);
       
    //interrupt start
    refresh_Timer.start();

    printf("start\n");
    
    int mode = -1;
    
    while (true) {
        
        switch(mode){
            //スタートモード：パラシュートが開くまではこのモードを実行
            case -1:
                standby();
            break;
            //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
            case 1:
                falling();
            break;
            //走行モード：ターゲットにむかって走行を行う
            case 2:
                running();
            break;
            //停止モード：ターゲット
            case 100:
                stopping();
            break;
        }

        myGPS.read();
        //recive gps module
        //check if we recieved a new message from GPS, if so, attempt to parse it,
        if ( myGPS.newNMEAreceived() ) {
            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
                continue;   
            } 
            else{
                count++;
            }    
        }
        //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
        if (refresh_Timer.read_ms() >= refresh_Time) {
            refresh_Timer.reset();
            //print_gps(count);
            Get_GPS(&myGPS);

        }
    }
    
}