GPS to Pulga

Dependencies:   Si1133 BME280

gps.txt

Committer:
pancotinho
Date:
2020-10-23
Revision:
25:ecdee9ef9939
Child:
26:1e1776201716

File content as of revision 25:ecdee9ef9939:


#define  SYNC1       0xB5
#define  SYNC2       0x62

//GPS DEclaration
SPI spi_2(P0_5, P0_7, P0_11); // mosi, miso, sclk
DigitalOut cs(P0_30);
DigitalOut gps_reset(P1_2);
DigitalOut gps_int (P0_23);
DigitalOut gps_DSEL (P0_27);





typedef struct
{
    uint8_t cls;
    uint8_t id;
    uint16_t len;          //Length of the payload. Does not include cls, id, or checksum bytes
    uint8_t *payload;
    uint8_t checksumA; //Given to us from module. Checked against the rolling calculated A/B checksums.
    uint8_t checksumB;
} gps_ubxPacket;

typedef struct
{
    uint32_t iTOW=0;
    uint16_t year=0;
    uint8_t month=0;
    uint8_t day=0;
    uint8_t hour=0;
    uint8_t min=0;
    uint8_t sec=0;
    int8_t valid=0;
    uint32_t tAcc=0;
    int32_t nano=0;
    uint8_t fixtype=0;
    int8_t flags=0;
    int8_t flags2=0;
    uint8_t numSV=0;
    int32_t lon=0;
    int32_t lat=0;
    int32_t height=0;
    int32_t hMSL=0;
    uint32_t hAcc=0;
    uint32_t vAcc=0;
    int32_t velN=0;
    int32_t velE=0;
    int32_t velD=0;
    int32_t gSpeed=0;
    int32_t headMot=0;
    uint32_t sAcc=0;
    uint32_t headAcc=0;
    uint16_t pDOP=0;
    int32_t headVeh;
    int16_t magDec;
    uint16_t magAcc;

    
} gps_navPVT;


void gps_leBootMsg(){

    #define MAXIMUM_PACKET_SIZE 60
    int packet_size;
    uint8_t packet[MAXIMUM_PACKET_SIZE];
    uint8_t value;
    int cont;
    value = spi_2.write(0x00);

    while (value  != '$' ){ //wait start boot msg
        value = spi_2.write(0x00);
        wait_ms(5);
        cont++;
        if (cont > 100){
            printf("\n no response \n"); 
            return;
            }
    }
    packet[0] = value;
        value = spi_2.write(0x00);
        for (packet_size = 1 ; packet_size <= MAXIMUM_PACKET_SIZE ;packet_size++) {
            if (value  != '\n' ){
                packet [packet_size]= value;
                value = spi_2.write(0x00);
            }
            else {
              //lora_send_packet (packet , (uint8_t) packet_size+1); // manda atraves do lora a mensagem de boot do gps
              printf("Boot msg: %s \n", packet);
              return;
            } 
        
        }

}

void gps_le_envia_linha(){
    
    uint8_t packet[150];
    uint8_t value;

    for (int i=0;i < 149; i++){
        if (value == '\n'){
            //lora_send_packet (packet , (uint8_t) i);
            return;
        }
        else packet [i] =value;
    }
    //lora_send_packet (packet , (uint8_t) 99);
    
}
                               

gps_ubxPacket gps_calcula_check(gps_ubxPacket Packet) {
 
    uint8_t Buffer[Packet.len + 4];
    uint8_t CK_A=0;
    uint8_t CK_B=0;

    Buffer[0]= Packet.cls;
    Buffer[1]= Packet.id;
    Buffer[2]= Packet.len & 0xFF;
    Buffer[3]= (Packet.len >> 8)& 0xFF;;

    //send_packet (Buffer, (uint8_t) 4 );
        
    for (uint16_t i = 0; i < Packet.len; i++) {
        Buffer [i+4] = Packet.payload[i] ;
    }
    
    //send_packet (Buffer, (uint8_t) Packet.len + 4 );

    for(int i=0 ; i < Packet.len + 4 ; i++) {
        CK_A = CK_A + Buffer[i];
        CK_B = CK_B + CK_A;
        CK_A = CK_A & 0xFF;
        CK_B = CK_B & 0xFF;
        //uint8_t packet_check[5] ={Buffer[i],(uint8_t) (i+1),CK_A,CK_B,'#'};
//        send_packet (packet_check , (uint8_t) 5);
    }
    
    
//    uint8_t packet_check[2] ={CK_A, CK_B};
//    send_packet (packet_check , (uint8_t) 2);

    Packet.checksumA = CK_A;
    Packet.checksumB = CK_B;
    
    return Packet;
    
}

void send_gps_packet(gps_ubxPacket packet){
    
    spi_2.write(SYNC1);
    spi_2.write(SYNC2);
    spi_2.write(packet.cls);
    spi_2.write(packet.id);
    spi_2.write(packet.len & 0xFF);
    spi_2.write((packet.len >> 8)& 0xFF);
    
    for (uint16_t i = 0; i < packet.len; i++) {
        spi_2.write(packet.payload[i]);
    }
    spi_2.write(packet.checksumA);
    spi_2.write(packet.checksumB);
    
    //=============imprime resposta
    wait_ms(50);
    gps_le_envia_linha();
    gps_le_envia_linha();

}

gps_navPVT le_nav_pvt () {
    
    gps_navPVT Pac;
    
    char state = 0;
    int cont =0;
    int numb=0;
    
    while (1){ //começou mensagem
        if (cont > 100) {
           // BMX160_read_acc();
            return Pac;
            }
        if (state == 0 ){
            
            if ( spi_2.write(0x00) == 0xB5){
                state =1;
                }
            else {
                cont++;
                wait_ms(40);
            }
                    
        }
        else if (state == 1){ // read 0xb5  
            if (spi_2.write(0x00) == 0x62){
                state =2;
                //wait_ms(30);
            }
            else state =0;
        }
        else if (state == 2) {// read 0xb5 0x62
            if (spi_2.write(0x00) == 0x01){
                printf("le_nav_pvt going to state 3");
                state =3;
                //wait_ms(30);
            }
            else state =0;
        }
        else if (state == 3) {// read 0xb5 0x62 0x01
            if (spi_2.write(0x00) == 0x07){
                state =4;
                printf("le_nav_pvt going to state 4");
                //wait_ms(30);
            }
            else state =0;
        }
        else if (state == 4) {// read 0xb5 0x62 0x01 0x07
            if (spi_2.write(0x00) == 0x5c){
                state =5;
                printf("le_nav_pvt going to state 5");
                //wait_ms(30);
            }
            else state =0;
        }
        else if (state == 5) {// read 0xb5 0x62 0x01 0x07 0x5c
            if (spi_2.write(0x00) == 0x00){
                state =6;
                printf("le_nav_pvt going to state 6");
                //wait_ms(30);
            }
            else state =0;
        }
        else if (state == 6){ // read 0xb5 0x62 0x01 0x07 0x5c 0x00
            uint8_t value1,value2,value3,value4;
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.iTOW =  (value4 << 24) + (value3 <<16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            Pac.year = (value2 << 8) + value1;
            
            Pac.month = 0xff & spi_2.write(0x00);
            Pac.day = 0xff & spi_2.write(0x00);
            Pac.hour = 0xff & spi_2.write(0x00);
            Pac.min = 0xff & spi_2.write(0x00);
            Pac.sec = 0xff & spi_2.write(0x00);
            Pac.valid = 0xff & spi_2.write(0x00);
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.tAcc =  (value4 << 24) + (value3 <<16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.nano =  (value4 << 24) + (value3 <<16) + (value2 << 8) + value1;
            
            Pac.fixtype = 0xff & spi_2.write(0x00);
            Pac.flags = 0xff & spi_2.write(0x00);
            Pac.flags2 = 0xff & spi_2.write(0x00);
            Pac.numSV = 0xff & spi_2.write(0x00);
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.lon =  (value4 << 24) + (value3 <<16) + (value2 << 8) + value1;
            lon=  (value4 << 24) + (value3 <<16) + (value2 << 8) + value1;
            printf("Long Data %d \n ", lon);
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.lat =  (value4 << 24) + (value3 <<16) + (value2 << 8) + value1;
            lat =  (value4 << 24) + (value3 <<16) + (value2 << 8) + value1;
            printf("Lat Data %d", lat);
            
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.height =  (value4 << 24) + (value3 <<16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.hMSL  =  (value4 << 24) + (value3 <<16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.hAcc  =  (value4 << 24) + (value3 <<16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.vAcc  =  (value4 << 24) + (value3 << 16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.velN  =  (value4 << 24) + (value3 << 16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.velE  =  (value4 << 24) + (value3 << 16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.velD  =  (value4 << 24) + (value3 << 16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.gSpeed  =  (value4 << 24) + (value3 << 16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.headMot  =  (value4 << 24) + (value3 << 16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.sAcc  =  (value4 << 24) + (value3 << 16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.headAcc =  (value4 << 24) + (value3 << 16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);                
            Pac.pDOP  = (value2 << 8) + value1;
            
            for (int i=0; i < 6; i++)spi_2.write(0x00); 
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.headAcc  =  (value4 << 24) + (value3 << 16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);
            value3 = 0xff & spi_2.write(0x00);
            value4 = 0xff & spi_2.write(0x00);                   
            Pac.headAcc =  (value4 << 24) + (value3 << 16) + (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);                
            Pac.magDec = (value2 << 8) + value1;
            
            value1 = 0xff & spi_2.write(0x00);
            value2 = 0xff & spi_2.write(0x00);                
            Pac.magAcc = (value2 << 8) + value1;

                              
            return Pac;
        }
    }
        wait_ms(100);  
}

void send_nav_pvt (){
    uint8_t packet_nav_pvt[] = {    0xB5,   0x62,   0x01,   0x07,   0x00,   0x00,   0x08,   0x19};
    
    //=============envia pacote nav pvt
    for ( int i=0; i< sizeof(packet_nav_pvt) ; i++){
        spi_2.write(packet_nav_pvt[i]);
        wait_ms(5);
    }

    gps_navPVT Data = le_nav_pvt();

    uint8_t packet [100];
    
    packet [0]= (Data.lon >> 24)& 0xff;
    packet [1]= (Data.lon >> 16) & 0xff ;
    packet [2]= (Data.lon >> 8) & 0xff;
    packet [3]= Data.lon  & 0xff;
    packet [4]= (Data.lat >> 24)& 0xff;
    packet [5]= (Data.lat >> 16) & 0xff ;
    packet [6]= (Data.lat >> 8) & 0xff;
    packet [7]= Data.lat  & 0xff;
    packet [8]= (Data.hMSL >> 24)& 0xff;
    packet [9]= (Data.hMSL >> 16) & 0xff ;
    packet [10]= (Data.hMSL >> 8) & 0xff;
    packet [11]= Data.hMSL  & 0xff;
    
    if (Data.lon !=0 || Data.lat !=0  ){
        //lora_send_packet (packet , (uint8_t) 12);
    }
}

void send_gps_data(uint8_t *packet, uint8_t size){
    uint8_t packet_rec[size]; 
    for ( int i=0; i< size  ; i++){
        spi_2.write(packet[i]);

        //wait_ms(5);
    }
    //send_packet (packet_rec ,size);
}

/*
void wait_packet (uint8_t *header) {
    
    char state = 0;
    int cont =0;
    
    while (1){ //começou mensagem
        if (cont > 250) {
            //led2=!led2;
            return;
        }
        if (state == 0 ){
            if (spi_2.write(0x00) == header[0])
                state =1;
            else {
                cont++;
                wait_ms(10);
            }
        
            
        }
        else if (state == 1){ // read 0xb5  
            if (spi_2.write(0x00) == header[1]){
                state =2;
            }
            else state =0;
        }
        else if (state == 2) {// read 0xb5 0x62
            if (spi_2.write(0x00) == header[2]){
                state =3;
            }
            else state =0;
        }
        else if (state == 3) {// read 0xb5 0x62 0x06
            if (spi_2.write(0x00) == header[3]){
                state =4;
            }
            else state =0;
        }
        else if (state == 4) {// read 0xb5 0x62 0x06 0x013
        
            uint8_t packet[100];
            uint8_t value = spi_2.write(0x00);
            uint8_t packet_size;
             
            for (packet_size = 0 ; packet_size < 100 ;packet_size++) {
                if (value  != '\n' ){
                    packet [packet_size]= value;
                    value = spi_2.write(0x00);
                }
                else {
                  lora_send_packet (packet , (uint8_t) packet_size+1);
                  //packet_size = MAXIMUM_PACKET_SIZE +1;
                  return;
                }             
            }
        
        }
    }
}*/

void wait_packet_byte (uint8_t *header, uint8_t byte) {
    
    char state = 0;
    int cont =0;
    
    while (1){ //começou mensagem
        if (cont > 250) {
            //led2=!led2;
            return;
        }
        if (state == 0 ){
            if (spi_2.write(0x00) == header[0])
                state =1;
            else {
                cont++;
                wait_ms(10);
            }
        
            
        }
        else if (state == 1){ // read 0xb5  
            if (spi_2.write(0x00) == header[1]){
                state =2;
            }
            else state =0;
        }
        else if (state == 2) {// read 0xb5 0x62
            if (spi_2.write(0x00) == header[2]){
                state =3;
            }
            else state =0;
        }
        else if (state == 3) {// read 0xb5 0x62 0x06
            if (spi_2.write(0x00) == header[3]){
                state =4;
            }
            else state =0;
        }
        else if (state == 4) {// read 0xb5 0x62 0x06 0x013
            
            if (byte == 0){//mostrar todo o pacote
                uint8_t packet[100];
                uint8_t value = spi_2.write(0x00);
                uint8_t packet_size;
                 
                for (packet_size = 0 ; packet_size < 100 ;packet_size++) {
                    if (value  != '\n' ){
                        packet [packet_size]= value;
                        value = spi_2.write(0x00);
                    }
                    else {
                      //lora_send_packet (packet , (uint8_t) packet_size+1);
                      //packet_size = MAXIMUM_PACKET_SIZE +1;
                      return;
                    }             
                }//fim for
            }//fim if byte
            
            else {//mostrar apenas um byte
            
             
                for (int i = 0 ; i < byte-1 ;i++)
                    spi_2.write(0x00);
                    
                uint8_t packet[]={(uint8_t)99,(uint8_t)spi_2.write(0x00)};            
                //lora_send_packet (packet , (uint8_t) 2);
                   
                    
                for (int i = 0 ; i < 100 ;i++)
                    if (spi_2.write(0x00) == '\n') 
                        return;
    
                return;
                
            }//fim else byte
         
         }
        
      }
    
}


void gps_wait_same_packet () {
    
    char state = 0;
    int cont =0;
    
    while (1){ //começou mensagem
        if (cont > 250) {
            //led2=!led2;
            return;
        }
        if (state == 0 ){
            if (spi_2.write(0x00) == 0xb5)
                state =1;
            else {
                cont++;
                wait_ms(10);
            }
        
            
        }
        else if (state == 1){ // read 0xb5  
            if (spi_2.write(0x00) == 0x62){
                state =2;
            }
            else state =0;
        }
        else if (state == 2){
                uint8_t packet[200];
                uint8_t value = spi_2.write(0x00);
                uint8_t packet_size;
                 
                for (packet_size = 0 ; packet_size < 200 ;packet_size++) {
                    if (value  != '\n' ){
                        packet [packet_size]= value;
                        value = spi_2.write(0x00);
                    }
                    else {
                      //lora_send_packet (packet , (uint8_t) packet_size+1);
                      //packet_size = MAXIMUM_PACKET_SIZE +1;
                      return;
                    }
                             
                }//fim for   
               // lora_send_packet (packet , 200);       
        }
    }
}

void gps_config_gnss (){
    
    uint8_t packet_cfg_gnss[] = {
      0xB5, 0x62, // Header
      0x03, 0x3E, // Class ID = CFG, Msg ID = UBX-CFG-GNSS
      0x3C, 0x00, // Payload Length = 60 bytes
      0x00, 0x00, 0x20, 0x07, // msgVer=0, numTrkChHw=32, numTrkChUse=32, numConfigBlocks=7
      0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // gnssId=0 (GPS), resTrkCh=8, maxTrkCh=12, ENABLE
      0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x00, // gnssId=1 (SBAS), resTrkCh=1, maxTrkCh=2, ENABLE
      0x02, 0x04, 0x0A, 0x00, 0x01, 0x00, 0x01, 0x00, // gnssId=2 (Galileo), resTrkCh=4, maxTrkCh=10, ENABLE 
      0x03, 0x04, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, // gnssId=3 (BeiDou), resTrkCh=4, maxTrkCh=8, DISABLE 
      0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, // gnssId=4 (IMES), resTrkCh=0, maxTrkCh=8, DISABLE
      0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x00, // gnssId=5 (QZSS), resTrkCh=0, maxTrkCh=3, DISABLE
      0x06, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x00, // gnssId=6 (GLONASS), resTrkCh=4, maxTrkCh=8, ENABLE
      0x00, 0x00 //checksums A &amp; B
    };
    uint8_t pos_ck = sizeof(packet_cfg_gnss)-2;
    printf("gnss config finish");
    uint8_t ubxi;
    //calcula checksum
    for (ubxi=2; ubxi<pos_ck ; ubxi++) {
        packet_cfg_gnss[pos_ck] = packet_cfg_gnss[pos_ck] + packet_cfg_gnss[ubxi];
        packet_cfg_gnss[pos_ck+1] = packet_cfg_gnss[pos_ck+1] + packet_cfg_gnss[pos_ck];
    }
    
}


void gps_print_local (){
    uint8_t packet_nav_pvt[] = {    0xB5,   0x62,   0x01,   0x07,   0x00,   0x00,   0x08,   0x19};
    
    //=============envia pacote nav pvt
    for ( int i=0; i< sizeof(packet_nav_pvt) ; i++){
        spi_2.write(packet_nav_pvt[i]);
        wait_ms(20);
    }
    
    gps_navPVT Data = le_nav_pvt();  
   
    //printf ("gps lat=%d lon=%d \n",Data.lat ,Data.lon );
    //lat=Data.lat;
    //lon=Data.lon;
}

void gps_config (){
    gps_DSEL = 0;
    //spi gps configuration
    spi_2.format(8,0);
    spi_2.frequency(1000000); //1MHz
    cs=1;
    wait(0.1);
    cs=0;
    wait(0.1);
    
    gps_int = 0;
    
    //gps reset
    gps_reset = 1;    
    wait_ms(50);
    //gps_reset = 0;    
    //wait(1.5);
}