pulga+gps
Dependencies: pulga-lorawan-drv SPI_MX25R Si1133 BME280
gps.txt
- Committer:
- brunnobbco
- Date:
- 2021-01-08
- Revision:
- 61:65744bc8ab55
File content as of revision 61:65744bc8ab55:
#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 & 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); }