liumin/GPS-Navigator-
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

增加CRC校验

Browse Source
This commit is contained in:
liumin
2025-10-14 21:46:03 +08:00
parent f0106c0f17
commit 0346a5e662
1 changed files with 74 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

135
ETH/UdpServer/app/app_data_processing.c
View File

@@ -2,27 +2,27 @@
#include <stdint.h> #include <stdint.h>
#include <ctype.h> #include <ctype.h>
// <EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD> // ??????
Timer data_processing_timer; Timer data_processing_timer;
Timer iim6234_timer; Timer iim6234_timer;
Timer rm3100_timer; Timer rm3100_timer;
Timer rs232_output_timer; Timer rs232_output_timer;
StrInfNavigationOutput nav_data = {0}; // <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>Ϊȫ0 StrInfNavigationOutput nav_data = {0}; // ?????0
UnInfProtocolFrame yis321_data = {0}; UnInfProtocolFrame yis321_data = {0};
GGA_Data gga_data1 = {0}; GGA_Data gga_data1 = {0};
VTG_Data vtg_data1 = {0}; VTG_Data vtg_data1 = {0};
THS_Data ths_data1 = {0}; THS_Data ths_data1 = {0};
// <EFBFBD><EFBFBD><EFBFBD>尴ť<EFBFBD>ṹ<EFBFBD><EFBFBD> // ???????
uint8_t remote_tcp_ip[4] = {192,168,10,7}; uint8_t remote_tcp_ip[4] = {192,168,10,7};
/* <EFBFBD><EFBFBD><EFBFBD>ȷָ<EFBFBD>ʽת<EFBFBD><EFBFBD>Ϊʮ<EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD><EFBFBD><EFBFBD> */ /* ????????????? */
double nmea_to_decimal(double dm, char direction) double nmea_to_decimal(double dm, char direction)
{ {
double degrees = (int)(dm / 100); double degrees = (int)(dm / 100);
@@ -31,7 +31,7 @@ double nmea_to_decimal(double dm, char direction)
return (direction == 'S' || direction == 'W') ? -decimal : decimal; return (direction == 'S' || direction == 'W') ? -decimal : decimal;
} }
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GNGGA<EFBFBD><EFBFBD><EFBFBD><EFBFBD> // ??GNGGA??
uint8_t parse_GGA(const char* data, GGA_Data* gga) uint8_t parse_GGA(const char* data, GGA_Data* gga)
{ {
char buffer[256]; char buffer[256];
@@ -55,8 +55,8 @@ uint8_t parse_GGA(const char* data, GGA_Data* gga)
field_index++; field_index++;
} }
// <EFBFBD>ȷ<EFBFBD>תʮ<EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD><EFBFBD>ڽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɺ<EFBFBD>ͳһ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> // ??????????????????
if (field_index > 5) { // ȷ<EFBFBD><EFBFBD><EFBFBD>ѽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֶ<EFBFBD> if (field_index > 5) { // ??????????
gga->latitude = nmea_to_decimal(gga->latitude, gga->lat_dir); gga->latitude = nmea_to_decimal(gga->latitude, gga->lat_dir);
gga->longitude = nmea_to_decimal(gga->longitude, gga->lon_dir); gga->longitude = nmea_to_decimal(gga->longitude, gga->lon_dir);
} }
@@ -64,22 +64,22 @@ uint8_t parse_GGA(const char* data, GGA_Data* gga)
return 0; return 0;
} }
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GNVTG<EFBFBD><EFBFBD><EFBFBD><EFBFBD> // ??GNVTG??
uint8_t parse_VTG(const char* data, VTG_Data* vtg) uint8_t parse_VTG(const char* data, VTG_Data* vtg)
{ {
char buffer[256]; char buffer[256];
strncpy(buffer, data, sizeof(buffer)); strncpy(buffer, data, sizeof(buffer));
buffer[sizeof(buffer)-1] = '\0'; // ȷ<EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ buffer[sizeof(buffer)-1] = '\0'; // ???????
char* token = strtok(buffer, ","); char* token = strtok(buffer, ",");
int field_index = 0; int field_index = 0;
while (token != NULL) { while (token != NULL) {
switch (field_index) { switch (field_index) {
case 1: vtg->course_true = atof(token); break; // <EFBFBD>汱<EFBFBD><EFBFBD><EFBFBD><EFBFBD> case 1: vtg->course_true = atof(token); break; // ????
case 3: vtg->course_magnetic = atof(token); break; // <EFBFBD>ű<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> case 3: vtg->course_magnetic = atof(token); break; // ????
case 5: vtg->speed_knots = atof(token); break; // <EFBFBD>ٶȣ<EFBFBD><EFBFBD>ڣ<EFBFBD> case 5: vtg->speed_knots = atof(token); break; // ?????
case 7: vtg->speed_kmh = atof(token); break; // <EFBFBD>ٶȣ<EFBFBD>km/h<EFBFBD><EFBFBD> case 7: vtg->speed_kmh = atof(token); break; // ???km/h?
case 9: vtg->mode = token[0]; break; // ģʽָʾ case 9: vtg->mode = token[0]; break; // ????
} }
token = strtok(NULL, ","); token = strtok(NULL, ",");
field_index++; field_index++;
@@ -87,7 +87,7 @@ uint8_t parse_VTG(const char* data, VTG_Data* vtg)
return 0; return 0;
} }
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GNTHS<EFBFBD><EFBFBD><EFBFBD><EFBFBD> // ??GNTHS??
uint8_t parse_THS(const char* data, THS_Data* ths) uint8_t parse_THS(const char* data, THS_Data* ths)
{ {
char buffer[128]; char buffer[128];
@@ -98,13 +98,13 @@ uint8_t parse_THS(const char* data, THS_Data* ths)
while (token != NULL) { while (token != NULL) {
switch (field_index) { switch (field_index) {
case 1: // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֶΣ<EFBFBD>ʾ<EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>341.3344<EFBFBD><EFBFBD> case 1: // ?????????341.3344?
ths->heading = atof(token); ths->heading = atof(token);
break; break;
case 2: // ģʽ<EFBFBD>ֶΣ<EFBFBD>ʾ<EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>A<EFBFBD><EFBFBD> case 2: // ?????????A?
ths->mode = token[0]; ths->mode = token[0];
break; break;
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֶΣ<EFBFBD>У<EFBFBD><EFBFBD><EFBFBD>͵ȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>չ // ????????????????
} }
token = strtok(NULL, ","); token = strtok(NULL, ",");
field_index++; field_index++;
@@ -118,18 +118,18 @@ uint8_t parse_THS(const char* data, THS_Data* ths)
void nmeaProcess(void *signal_id) void nmeaProcess(void *signal_id)
{ {
(void)signal_id; // <EFBFBD><EFBFBD><EFBFBD>DZ<EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD>ʹ<EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (void)signal_id; // ????????????????
// printf("AccX:%f,AccY:%f,AccZ:%f,",Acc[0],Acc[1],Acc[2]); // printf("AccX:%f,AccY:%f,AccZ:%f,",Acc[0],Acc[1],Acc[2]);
// printf("GyroX:%f,GyroY:%f,GyroZ:%f\r\n",Gyro[0],Gyro[1],Gyro[2]);//ʹ<EFBFBD>ô<EFBFBD><EFBFBD>ڴ<EFBFBD>ӡ<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> // printf("GyroX:%f,GyroY:%f,GyroZ:%f\r\n",Gyro[0],Gyro[1],Gyro[2]);//???????????????
// if (!nmea_validate_checksum(&nema_output1.buffer[nema_output1.active_buf^1][0])) //У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> // if (!nmea_validate_checksum(&nema_output1.buffer[nema_output1.active_buf^1][0])) //????
// { // {
// printf("NMEA CRC Failed"); // printf("NMEA CRC Failed");
// return; // return;
// } // }
// <EFBFBD>жϺ<EFBFBD>3<EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><EFBFBD>Ƿ<EFBFBD>Ϊ"GGA"/"VTG"/"THS" // ???3??????"GGA"/"VTG"/"THS"
if(strncmp(&nema_output1.buffer[3], "GGA", 3) == 0) { if(strncmp(&nema_output1.buffer[3], "GGA", 3) == 0) {
nema_output1.frame_ready = 1; nema_output1.frame_ready = 1;
} }
@@ -140,11 +140,11 @@ void nmeaProcess(void *signal_id)
nema_output1.frame_ready = 3; nema_output1.frame_ready = 3;
} }
else else
{ // <EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD>Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> { // ????????
nema_output1.frame_ready = 0; nema_output1.frame_ready = 0;
} }
// <EFBFBD>޸ĺ<EFBFBD><EFBFBD><EFBFBD>switch<EFBFBD><EFBFBD><EFBFBD><EFBFBD> // ????switch??
switch(nema_output1.frame_ready) switch(nema_output1.frame_ready)
{ {
case NMEA_TYPE_GGA: case NMEA_TYPE_GGA:
@@ -152,9 +152,9 @@ void nmeaProcess(void *signal_id)
if (0 == parse_GGA(&nema_output1.buffer[0], &gga_data1)) if (0 == parse_GGA(&nema_output1.buffer[0], &gga_data1))
{ {
navigation_output1.bit_data.lon = (int32_t) (gga_data1.longitude * 10000000); //20250426<EFBFBD>ȷ<EFBFBD>ת<EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD>10<EFBFBD><EFBFBD><EFBFBD><EFBFBD> navigation_output1.bit_data.lon = (int32_t) (gga_data1.longitude * 10000000); //20250426??????10??
navigation_output1.bit_data.lat = (int32_t) (gga_data1.latitude * 10000000); navigation_output1.bit_data.lat = (int32_t) (gga_data1.latitude * 10000000);
navigation_output1.bit_data.high = (int32_t)(gga_data1.altitude * 1000);//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>1000 navigation_output1.bit_data.high = (int32_t)(gga_data1.altitude * 1000);//??1000
navigation_output1.bit_data.rtk_state = gga_data1.fix_quality;// navigation_output1.bit_data.rtk_state = gga_data1.fix_quality;//
@@ -173,8 +173,8 @@ void nmeaProcess(void *signal_id)
if (0 == parse_VTG(&nema_output1.buffer[0], &vtg_data1)) if (0 == parse_VTG(&nema_output1.buffer[0], &vtg_data1))
{ {
navigation_output1.bit_data.gps_speed = (int16_t)(vtg_data1.speed_kmh*1000/3600*100);//ת<EFBFBD><EFBFBD>Ϊm/s <20><><EFBFBD><EFBFBD>100<EFBFBD><EFBFBD> navigation_output1.bit_data.gps_speed = (int16_t)(vtg_data1.speed_kmh*1000/3600*100);//???m/s ??100?
// printf("[VTG] Course:%.1f<EFBFBD><EFBFBD> Speed:%.1fkm/h\n", // printf("[VTG] Course:%.1f<EFBFBD> Speed:%.1fkm/h\n",
// vtg_data1.course_true, vtg_data1.speed_kmh); // vtg_data1.course_true, vtg_data1.speed_kmh);
} }
else else
@@ -187,8 +187,8 @@ void nmeaProcess(void *signal_id)
if (0 == parse_THS(&nema_output1.buffer[0], &ths_data1)) if (0 == parse_THS(&nema_output1.buffer[0], &ths_data1))
{ {
navigation_output1.bit_data.dual_antenna_heading = (int16_t)(ths_data1.heading*100);// <EFBFBD><EFBFBD><EFBFBD><EFBFBD>100<EFBFBD><EFBFBD> navigation_output1.bit_data.dual_antenna_heading = (int16_t)(ths_data1.heading*100);// ??100?
// printf("[THS] Heading:%.1f<EFBFBD><EFBFBD> Mode:%c\n", // printf("[THS] Heading:%.1f<EFBFBD> Mode:%c\n",
// ths_data1.heading, ths_data1.mode); // ths_data1.heading, ths_data1.mode);
} }
else else
@@ -197,13 +197,13 @@ void nmeaProcess(void *signal_id)
} }
break; break;
default: // <EFBFBD><EFBFBD><EFBFBD><EFBFBD>δ֪<EFBFBD><EFBFBD><EFBFBD><EFBFBD> default: // ??????
// printf("Unknown NMEA frame type\n"); // printf("Unknown NMEA frame type\n");
break; break;
} }
} }
unsigned short NumCheak(unsigned char *data,unsigned char len)//<EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> unsigned short NumCheak(unsigned char *data,unsigned char len)//?????
{ {
unsigned short NUM=0; unsigned short NUM=0;
unsigned char i; unsigned char i;
@@ -214,9 +214,16 @@ unsigned short NumCheak(unsigned char *data,unsigned char len)//
return NUM; return NUM;
} }
// ?????????0-56?
uint8_t calculate_checksum(const uint8_t* data, uint8_t len) {
uint8_t checksum = 0;
for (size_t i = 0; i < len; ++i) {
checksum ^= data[i];
}
return checksum;
}
uint16_t yis321Cheak(unsigned char *data,unsigned char len)//?????
uint16_t yis321Cheak(unsigned char *data,unsigned char len)//<2F><>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{ {
uint8_t CK1 = 0; uint8_t CK1 = 0;
uint8_t CK2 = 0; uint8_t CK2 = 0;
@@ -301,7 +308,7 @@ void imuInput(void *signal_id)
{ {
memcpy(&yis321_data.arr[0],&imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][0],sizeof(UnInfProtocolFrame)); memcpy(&yis321_data.arr[0],&imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][0],sizeof(UnInfProtocolFrame));
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD> //????
Acc[0] = (float)((int32_t)yis321_data.bit_data.acc_x)*0.000001f; Acc[0] = (float)((int32_t)yis321_data.bit_data.acc_x)*0.000001f;
Acc[1] = (float)((int32_t)yis321_data.bit_data.acc_y)*0.000001f; Acc[1] = (float)((int32_t)yis321_data.bit_data.acc_y)*0.000001f;
Acc[2] = (float)((int32_t)yis321_data.bit_data.acc_z)*0.000001f; Acc[2] = (float)((int32_t)yis321_data.bit_data.acc_z)*0.000001f;
@@ -368,25 +375,25 @@ void iim6234Init(void *signal_id)
void nmeaPrintf(void *signal_id) void nmeaPrintf(void *signal_id)
{ {
printf("[THS] Heading:%.1f<EFBFBD><EFBFBD> Mode:%c\n", printf("[THS] Heading:%.1f<EFBFBD> Mode:%c\n",
ths_data1.heading, ths_data1.mode); ths_data1.heading, ths_data1.mode);
printf("[VTG] Course:%.1f<EFBFBD><EFBFBD> Speed:%.1fkm/h\n", printf("[VTG] Course:%.1f<EFBFBD> Speed:%.1fkm/h\n",
vtg_data1.course_true, vtg_data1.speed_kmh); vtg_data1.course_true, vtg_data1.speed_kmh);
printf("[GGA] Time:%.2f Lat:%.5f%c Lon:%.5f%c Alt:%.2f rtk_state:%d satellite:%d hdop:%2f\n", printf("[GGA] Time:%.2f Lat:%.5f%c Lon:%.5f%c Alt:%.2f rtk_state:%d satellite:%d hdop:%2f\n",
gga_data1.utc_time, gga_data1.utc_time,
gga_data1.latitude, gga_data1.lat_dir, gga_data1.latitude, gga_data1.lat_dir,
gga_data1.longitude, gga_data1.lon_dir, gga_data1.longitude, gga_data1.lon_dir,
gga_data1.altitude, // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶ȵ<EFBFBD>λΪM(<28><>) gga_data1.altitude, // ???????M(?)
gga_data1.fix_quality, gga_data1.fix_quality,
gga_data1.satellites, gga_data1.satellites,
gga_data1.hdop gga_data1.hdop
); );
printf("<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>: (%.6f, %.6f, %.6f, %.6f)\n", printf("???: (%.6f, %.6f, %.6f, %.6f)\n",
yis321_data.bit_data.q0*0.000001, yis321_data.bit_data.q1*0.000001, yis321_data.bit_data.q2*0.000001, (int32_t)yis321_data.bit_data.q3*0.000001); yis321_data.bit_data.q0*0.000001, yis321_data.bit_data.q1*0.000001, yis321_data.bit_data.q2*0.000001, (int32_t)yis321_data.bit_data.q3*0.000001);
printf("ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: (%.6f, %.6f, %.6f)\n", printf("???: (%.6f, %.6f, %.6f)\n",
yis321_data.bit_data.pitch*0.000001, yis321_data.bit_data.roll*0.000001, yis321_data.bit_data.yaw*0.000001); yis321_data.bit_data.pitch*0.000001, yis321_data.bit_data.roll*0.000001, yis321_data.bit_data.yaw*0.000001);
timerStart(&data_processing_timer, 1000,1);//100ms timerStart(&data_processing_timer, 1000,1);//100ms
@@ -394,25 +401,25 @@ void nmeaPrintf(void *signal_id)
/** /**
* @brief <EFBFBD><EFBFBD>GGAʱ<EFBFBD>䣨HHMMSS.SSS<EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD>double<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>ΪЭ<EFBFBD><EFBFBD>ʱ<EFBFBD>䣨U32<EFBFBD><EFBFBD><EFBFBD>Ŵ<EFBFBD>4000<EFBFBD><EFBFBD><EFBFBD><EFBFBD> * @brief ?GGA???HHMMSS.SSS???double?????????U32???4000??
* @param utc_time GGAʱ<EFBFBD>䣨<EFBFBD><EFBFBD>75049.00<EFBFBD><EFBFBD>ʾ07:50:49.00<EFBFBD><EFBFBD> * @param utc_time GGA????75049.00??07:50:49.00?
* @return uint32_t Э<EFBFBD><EFBFBD>ʱ<EFBFBD>䣨ʵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> = <20><><EFBFBD><EFBFBD>ֵ / 4000<EFBFBD><EFBFBD> * @return uint32_t ????????? = ??? / 4000?
*/ */
uint32_t GgaTimeToProtocol(double utc_time) uint32_t GgaTimeToProtocol(double utc_time)
{ {
// 1. <EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֺ<EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֣<EFBFBD><EFBFBD><EFBFBD><EFBFBD>⸡<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣩 // 1. ???????????????????
uint32_t time_int = (uint32_t)utc_time; // HHMMSS uint32_t time_int = (uint32_t)utc_time; // HHMMSS
uint32_t time_frac = (uint32_t)((utc_time - time_int) * 1000 + 0.5); // .SSS <EFBFBD><EFBFBD> <20><><EFBFBD>루<EFBFBD><EBA3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>룩 uint32_t time_frac = (uint32_t)((utc_time - time_int) * 1000 + 0.5); // .SSS ? ????????
// 2. <EFBFBD>ֽ<EFBFBD>HH<EFBFBD><EFBFBD>MM<EFBFBD><EFBFBD>SS // 2. ??HH?MM?SS
uint32_t hh = time_int / 10000; // Сʱ uint32_t hh = time_int / 10000; // ??
uint32_t mm = (time_int % 10000) / 100; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD> uint32_t mm = (time_int % 10000) / 100; // ??
uint32_t ss = time_int % 100; // <EFBFBD><EFBFBD> uint32_t ss = time_int % 100; // ?
// 3. <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܺ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>HH*3600 + MM*60 + SS<EFBFBD><EFBFBD>* 1000 + SSS // 3. ???????HH*3600 + MM*60 + SS?* 1000 + SSS
uint32_t total_ms = hh * 3600000 + mm * 60000 + ss * 1000 + time_frac; uint32_t total_ms = hh * 3600000 + mm * 60000 + ss * 1000 + time_frac;
// 4. ת<EFBFBD><EFBFBD>ΪЭ<EFBFBD><EFBFBD>ʱ<EFBFBD>䣨<EFBFBD>Ŵ<EFBFBD>4000<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><EFBFBD><EFBFBD> total_ms * 4<EFBFBD><EFBFBD> // 4. ??????????4000????? total_ms * 4?
return total_ms * 4; return total_ms * 4;
} }
@@ -429,7 +436,7 @@ void rs232Process(void *signal_id)
navigation_rs232_output.bit_data.header3 = 0x0B;//0B navigation_rs232_output.bit_data.header3 = 0x0B;//0B
//ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> //???
float_temp = (float) (yis321_data.bit_data.roll)*0.000001; float_temp = (float) (yis321_data.bit_data.roll)*0.000001;
navigation_rs232_output.bit_data.roll = (int16_t) (float_temp*32768/360.0f); navigation_rs232_output.bit_data.roll = (int16_t) (float_temp*32768/360.0f);
@@ -439,7 +446,7 @@ void rs232Process(void *signal_id)
float_temp = (float) (yis321_data.bit_data.yaw)*0.000001; float_temp = (float) (yis321_data.bit_data.yaw)*0.000001;
navigation_rs232_output.bit_data.yaw = (int16_t) (float_temp*32768/360.0f); navigation_rs232_output.bit_data.yaw = (int16_t) (float_temp*32768/360.0f);
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD> //??
float_temp = (float) (yis321_data.bit_data.gyro_x)*0.000001; float_temp = (float) (yis321_data.bit_data.gyro_x)*0.000001;
navigation_rs232_output.bit_data.gyro_x = (int16_t) (float_temp*32768/300.0f); navigation_rs232_output.bit_data.gyro_x = (int16_t) (float_temp*32768/300.0f);
@@ -449,7 +456,7 @@ void rs232Process(void *signal_id)
float_temp = (float) (yis321_data.bit_data.gyro_z)*0.000001f; float_temp = (float) (yis321_data.bit_data.gyro_z)*0.000001f;
navigation_rs232_output.bit_data.gyro_z = (int16_t) (float_temp*32768/300.0f); navigation_rs232_output.bit_data.gyro_z = (int16_t) (float_temp*32768/300.0f);
//<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> //???
float_temp = (float) (yis321_data.bit_data.acc_x)*0.000001f; float_temp = (float) (yis321_data.bit_data.acc_x)*0.000001f;
navigation_rs232_output.bit_data.accel_x = (int16_t) (float_temp*32768/12.0f); navigation_rs232_output.bit_data.accel_x = (int16_t) (float_temp*32768/12.0f);
@@ -459,34 +466,38 @@ void rs232Process(void *signal_id)
float_temp = (float) (yis321_data.bit_data.acc_z)*0.000001f; float_temp = (float) (yis321_data.bit_data.acc_z)*0.000001f;
navigation_rs232_output.bit_data.accel_z = (int16_t) (float_temp*32768/12.0f); navigation_rs232_output.bit_data.accel_z = (int16_t) (float_temp*32768/12.0f);
//gps<EFBFBD><EFBFBD>Ϣ //gps??
navigation_rs232_output.bit_data.latitude = (int32_t) (gga_data1.latitude * 10000000); navigation_rs232_output.bit_data.latitude = (int32_t) (gga_data1.latitude * 10000000);
navigation_rs232_output.bit_data.longitude = (int32_t) (gga_data1.longitude * 10000000); navigation_rs232_output.bit_data.longitude = (int32_t) (gga_data1.longitude * 10000000);
navigation_rs232_output.bit_data.altitude = (int32_t) (gga_data1.altitude * 1000); navigation_rs232_output.bit_data.altitude = (int32_t) (gga_data1.altitude * 1000);
navigation_rs232_output.bit_data.init_status = 0xF;
navigation_rs232_output.bit_data.rtk_status = gga_data1.fix_quality; navigation_rs232_output.bit_data.rtk_status = gga_data1.fix_quality;
float_temp = (float)vtg_data1.speed_kmh/3.6f*100.0f;//ϵ<EFBFBD><EFBFBD>100 float_temp = (float)vtg_data1.speed_kmh/3.6f*100.0f;//??100
navigation_rs232_output.bit_data.gps_speed = (int16_t)float_temp; navigation_rs232_output.bit_data.gps_speed = (int16_t)float_temp;
navigation_rs232_output.bit_data.dual_heading = (int16_t)(ths_data1.heading*100);// <EFBFBD><EFBFBD><EFBFBD><EFBFBD>100<EFBFBD><EFBFBD>; navigation_rs232_output.bit_data.dual_heading = (int16_t)(ths_data1.heading*100);// ??100?;
navigation_rs232_output.bit_data.timestamp = GgaTimeToProtocol(gga_data1.utc_time); navigation_rs232_output.bit_data.timestamp = GgaTimeToProtocol(gga_data1.utc_time);
navigation_rs232_output.bit_data.checksum = calculate_checksum(&navigation_rs232_output.arr[0],57);
publishMessage(&navigation_rs232_output, 1); publishMessage(&navigation_rs232_output, 1);
timerStart(&rs232_output_timer, 50,1);//50ms timerStart(&rs232_output_timer, 10,1);//10ms
} }
// APPģ<EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD>ʼ<EFBFBD><EFBFBD> // APP??????
void dataProcessingAppInit(void) void dataProcessingAppInit(void)
{ {
// <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD> // ??????
timerInit(&iim6234_timer); timerInit(&iim6234_timer);
timerInit(&rm3100_timer); timerInit(&rm3100_timer);
timerInit(&data_processing_timer); timerInit(&data_processing_timer);
timerInit(&rs232_output_timer); timerInit(&rs232_output_timer);
rm3100Init();//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>RM3100 rm3100Init();//???RM3100
// <EFBFBD><EFBFBD><EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>źţ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڽ<EFBFBD><EFBFBD><EFBFBD>GNGGA<EFBFBD><EFBFBD><EFBFBD><EFBFBD> // ????????????GNGGA??
subscribe(&iim6234_timer, iim6234Init); subscribe(&iim6234_timer, iim6234Init);
subscribe(&rm3100_timer, rm3100Process); subscribe(&rm3100_timer, rm3100Process);
subscribe(&nema_output1, nmeaProcess); subscribe(&nema_output1, nmeaProcess);