File content as of revision 3:60bad3679b4b:

/**********************************************/
//  
//    
//
//  Program name: Aigamozu BASE
//  Author: Mineta Kizuku
//  
//
/**********************************************/

/**********************************************/
//更新情報
//2015/05/11
//ベースプログラムの作成
//
//2015/05/13
//カルマンフィルタの共分散の値を0.0001以下にならないようにした
//共分散の値を10進数に変換するようにした
/**********************************************/

#include "mbed.h"
#include "XBee.h"
#include "MBed_Adafruit_GPS.h"
#include "AigamozuControlPackets.h"
#include "agzIDLIST.h"
#include "aigamozuSetting.h"
#include "agz_common.h"
#include "Kalman.h"


#define SIGMA_MIN 0.0001

//************ID Number*****************
const char MyID = 'D';
//************ID Number*****************

/////////////////////////////////////////
//
//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);


/////////////////////////////////////////
//
//For Kalman data
//
/////////////////////////////////////////
double sigmaGPS[2][2] = {{250,0},{0,250}};
double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
double y[2],x[2][2]={0};
void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);

/////////////////////////////////////////
//
//Address List
//
/////////////////////////////////////////
XBeeAddress64 base_Address[BaseNumber] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
                                 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
                                 XBeeAddress64(BASE5_32H,BASE5_32L)};
XBeeAddress64 robot_Address[RobotNumber] = {XBeeAddress64(ROBOT1_32H,ROBOT1_32L), XBeeAddress64(ROBOT1_32H,ROBOT1_32L)};
XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L);


/////////////////////////////////////////
//
//Send_Status
//
//リクエストがきたとき、自分の位置情報などを返信する
/////////////////////////////////////////
void Send_Status(char SenderIDc){
    XBeeAddress64 send_Address;
    if(SenderIDc == '0'){
        send_Address = manager_Address;
    }
    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
        send_Address = robot_Address[SenderIDc - 'A'];
    }
    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
        send_Address = base_Address[SenderIDc - 'a'];
    }
    //send normal data
    //Create GPS Infomation Packet
    agz.createReceiveStatusCommand(MyID,SenderIDc,
                                    agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
                                    agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
                                    agz.get_agzCov_lati(),agz.get_agzCov_longi());
    
    //debug***************************************************
    printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\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()
            );
    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
        printf("\n");
    //debug end***************************************************
    
    //Select Destination
    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
    //Send -> Base
    xbee.send(tx64request);
}

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

void Get_GPS(Adafruit_GPS *myGPS){
    static bool flag = true;
    
    if (myGPS->fix) {
        agz.nowStatus = GPS_AVAIL;
        
        if(flag){//初期値設定
            if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
                flag = false;
                x[0][0]=(double)myGPS->latitudeL;                        
                x[0][1]=(double)myGPS->longitudeL;   
            } 
        }
        
        if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
            return;
        }
        //Kalman Filter
        Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
                
        agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
        agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
    }
    else agz.nowStatus = GPS_UNAVAIL;
    
} 


/////////////////////////////////////////
//
//Kalman Processing
//
/////////////////////////////////////////
    
void get_K(){
  double temp[2][2]={
    {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
    {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
  };
  double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
  K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
  K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
}


void get_x(){
  x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
  x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
}


void get_sigma(){
    double temp[2][2];
    for(int i=0;i<2;i++) {
        for(int j=0;j<2;j++) {
            for(int k=0;k<2;k++) {
                temp[i][j]+=K[1][i][k]*sigma[0][k][j];
            }
        }
   }
    for(int i = 0;i < 2;i++){
        for(int j = 0;j < 2;j++){
            sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
        }
    }   
}

void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
    y[0] = Latitude;
    y[1] = Longitude;
    //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
    get_K();
    //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
    get_x();
    //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
    get_sigma();
    
    
    //kousinn
    for(int i = 0;i < 2;i++){
        for(int j = 0;j < 2;j++){
            K[0][i][j]=K[1][i][j];
            x[0][i]=x[1][i];
            sigma[0][i][j]=sigma[1][i][j];
        }
    }
    
    if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN;
    if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN;
            
    myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
    myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
    myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
    myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
    
    agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
}


/////////////////////////////////////////
//
//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
    myGPS.begin(GPS_BAUD_RATE); 
    
    char SenderIDc;
    //GPS Send Command
    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
    myGPS.sendCommand(PGCMD_ANTENNA);
    
    wait(2);
       
    //interrupt start
    refresh_Timer.start();

    printf("start\n");
    
    while (true) {
        
        //Check Xbee Buffer Available
        xbee.readPacket();
            
        if (xbee.getResponse().isAvailable()) {
            xbee.getResponse().getZBRxResponse(zbRx);
            uint8_t *buf = zbRx.getFrameData();
                
            if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                xbee.getResponse().getZBRxResponse(zbRx);
                uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
                uint8_t *buf1 = &buf[11];//データの部分のみを格納する
                SenderIDc = buf1[5];//送信元のIDを取得する
                char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
                 
                //Check Command Type 
                switch(Command_type){
                    //Get Request command
                    case STATUS_REQUEST:{
                        Send_Status(SenderIDc);                     
                        break;          
                    }
                    default:{
                        break;
                    }
                }//endswitch
            }//endifZB_RX_RESPONSE
        }//endifisAvailable
                       
        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;   
            }       
        }
        
        
        if (refresh_Timer.read_ms() >= refresh_Time) {
            refresh_Timer.reset();
            Get_GPS(&myGPS);

        }
    }
}