diff --git a/ETH/UdpServer/app/app_data_processing.c b/ETH/UdpServer/app/app_data_processing.c index 76d6981..608a962 100644 --- a/ETH/UdpServer/app/app_data_processing.c +++ b/ETH/UdpServer/app/app_data_processing.c @@ -2,27 +2,27 @@ #include #include -// ¶¨Ê±Æ÷½á¹¹Ìå +// ?????? Timer data_processing_timer; Timer iim6234_timer; Timer rm3100_timer; Timer rs232_output_timer; -StrInfNavigationOutput nav_data = {0}; // ³õʼ»¯ÎªÈ«0 +StrInfNavigationOutput nav_data = {0}; // ?????0 UnInfProtocolFrame yis321_data = {0}; GGA_Data gga_data1 = {0}; VTG_Data vtg_data1 = {0}; THS_Data ths_data1 = {0}; -// ¶¨Òå°´Å¥½á¹¹Ìå +// ??????? uint8_t remote_tcp_ip[4] = {192,168,10,7}; -/* ½«¶È·Ö¸ñʽת»»ÎªÊ®½øÖƶÈÊý */ +/* ????????????? */ double nmea_to_decimal(double dm, char direction) { double degrees = (int)(dm / 100); @@ -31,7 +31,7 @@ double nmea_to_decimal(double dm, char direction) return (direction == 'S' || direction == 'W') ? -decimal : decimal; } -// ½âÎöGNGGAº¯Êý +// ??GNGGA?? uint8_t parse_GGA(const char* data, GGA_Data* gga) { char buffer[256]; @@ -55,8 +55,8 @@ uint8_t parse_GGA(const char* data, GGA_Data* gga) field_index++; } - // ¶È·Öתʮ½øÖÆ£¨ÔÚ½âÎöÍê³Éºóͳһ´¦Àí£© - if (field_index > 5) { // È·±£ÒѽâÎöµ½¾­¶È×Ö¶Î + // ?????????????????? + if (field_index > 5) { // ?????????? gga->latitude = nmea_to_decimal(gga->latitude, gga->lat_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; } -// ½âÎöGNVTGÓï¾ä +// ??GNVTG?? uint8_t parse_VTG(const char* data, VTG_Data* vtg) { char buffer[256]; strncpy(buffer, data, sizeof(buffer)); - buffer[sizeof(buffer)-1] = '\0'; // È·±£×Ö·û´®ÖÕÖ¹ + buffer[sizeof(buffer)-1] = '\0'; // ??????? char* token = strtok(buffer, ","); int field_index = 0; while (token != NULL) { switch (field_index) { - case 1: vtg->course_true = atof(token); break; // Õæ±±º½Ïò - case 3: vtg->course_magnetic = atof(token); break; // ´Å±±º½Ïò - case 5: vtg->speed_knots = atof(token); break; // ËÙ¶È£¨½Ú£© - case 7: vtg->speed_kmh = atof(token); break; // ËÙ¶È£¨km/h£© - case 9: vtg->mode = token[0]; break; // ģʽָʾ + case 1: vtg->course_true = atof(token); break; // ???? + case 3: vtg->course_magnetic = atof(token); break; // ???? + case 5: vtg->speed_knots = atof(token); break; // ????? + case 7: vtg->speed_kmh = atof(token); break; // ???km/h? + case 9: vtg->mode = token[0]; break; // ???? } token = strtok(NULL, ","); field_index++; @@ -87,7 +87,7 @@ uint8_t parse_VTG(const char* data, VTG_Data* vtg) return 0; } -// ½âÎöGNTHSÓï¾ä +// ??GNTHS?? uint8_t parse_THS(const char* data, THS_Data* ths) { char buffer[128]; @@ -98,13 +98,13 @@ uint8_t parse_THS(const char* data, THS_Data* ths) while (token != NULL) { switch (field_index) { - case 1: // º½Ïò×ֶΣ¨Ê¾ÀýÖµ£º341.3344£© + case 1: // ?????????341.3344? ths->heading = atof(token); break; - case 2: // ģʽ×ֶΣ¨Ê¾ÀýÖµ£ºA£© + case 2: // ?????????A? ths->mode = token[0]; break; - // ÆäËû×ֶΣ¨Ð£ÑéºÍµÈ£©¸ù¾ÝÐèÇóÀ©Õ¹ + // ???????????????? } token = strtok(NULL, ","); field_index++; @@ -118,18 +118,18 @@ uint8_t parse_THS(const char* data, THS_Data* ths) void nmeaProcess(void *signal_id) { - (void)signal_id; // ±ê¼Ç±äÁ¿ÎªÒÑʹÓ㬱ÜÃâ±àÒëÆ÷¾¯¸æ + (void)signal_id; // ???????????????? // 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]);//ʹÓô®¿Ú´òÓ¡¼ÓËÙ¶È£¬½ÇËÙ¶ÈÊý¾Ý +// 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])) //УÑé´úÂë + // if (!nmea_validate_checksum(&nema_output1.buffer[nema_output1.active_buf^1][0])) //???? // { // printf("NMEA CRC Failed"); // return; // } - // ÅжϺó3¸ö×Ö·ûÊÇ·ñΪ"GGA"/"VTG"/"THS" + // ???3??????"GGA"/"VTG"/"THS" if(strncmp(&nema_output1.buffer[3], "GGA", 3) == 0) { nema_output1.frame_ready = 1; } @@ -140,11 +140,11 @@ void nmeaProcess(void *signal_id) nema_output1.frame_ready = 3; } else - { // ·ÇÄ¿±êЭÒéÔòÖØÖà + { // ???????? nema_output1.frame_ready = 0; } - // Ð޸ĺóµÄswitchÓï¾ä + // ????switch?? switch(nema_output1.frame_ready) { case NMEA_TYPE_GGA: @@ -152,9 +152,9 @@ void nmeaProcess(void *signal_id) if (0 == parse_GGA(&nema_output1.buffer[0], &gga_data1)) { - navigation_output1.bit_data.lon = (int32_t) (gga_data1.longitude * 10000000); //20250426¶È·Öת»»Îª¶È10½øÖÆ + 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.high = (int32_t)(gga_data1.altitude * 1000);//À©´ó1000 + navigation_output1.bit_data.high = (int32_t)(gga_data1.altitude * 1000);//??1000 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)) { - navigation_output1.bit_data.gps_speed = (int16_t)(vtg_data1.speed_kmh*1000/3600*100);//ת»»Îªm/s À©´ó100±¶ - // printf("[VTG] Course:%.1f¡ã Speed:%.1fkm/h\n", + navigation_output1.bit_data.gps_speed = (int16_t)(vtg_data1.speed_kmh*1000/3600*100);//???m/s ??100? + // printf("[VTG] Course:%.1f° Speed:%.1fkm/h\n", // vtg_data1.course_true, vtg_data1.speed_kmh); } else @@ -187,8 +187,8 @@ void nmeaProcess(void *signal_id) if (0 == parse_THS(&nema_output1.buffer[0], &ths_data1)) { - navigation_output1.bit_data.dual_antenna_heading = (int16_t)(ths_data1.heading*100);// À©´ó100±¶ - // printf("[THS] Heading:%.1f¡ã Mode:%c\n", + navigation_output1.bit_data.dual_antenna_heading = (int16_t)(ths_data1.heading*100);// ??100? + // printf("[THS] Heading:%.1f° Mode:%c\n", // ths_data1.heading, ths_data1.mode); } else @@ -197,13 +197,13 @@ void nmeaProcess(void *signal_id) } break; - default: // ´¦Àíδ֪ÀàÐÍ + default: // ?????? // printf("Unknown NMEA frame type\n"); break; } } -unsigned short NumCheak(unsigned char *data,unsigned char len)//ºÍУÑ鼯Ëã +unsigned short NumCheak(unsigned char *data,unsigned char len)//????? { unsigned short NUM=0; unsigned char i; @@ -214,9 +214,16 @@ unsigned short NumCheak(unsigned char *data,unsigned char len)// 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)//????? { uint8_t CK1 = 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)); - //¾«¶Èת»» + //???? 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[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) { - printf("[THS] Heading:%.1f¡ã Mode:%c\n", + printf("[THS] Heading:%.1f° Mode:%c\n", ths_data1.heading, ths_data1.mode); - printf("[VTG] Course:%.1f¡ã Speed:%.1fkm/h\n", + printf("[VTG] Course:%.1f° Speed:%.1fkm/h\n", 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", gga_data1.utc_time, gga_data1.latitude, gga_data1.lat_dir, gga_data1.longitude, gga_data1.lon_dir, - gga_data1.altitude, // ¼ÙÉè¸ß¶Èµ¥Î»ÎªM(Ã×) + gga_data1.altitude, // ???????M(?) gga_data1.fix_quality, gga_data1.satellites, gga_data1.hdop ); - printf("ËÄÔªÊý: (%.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); - printf("Å·À­½Ç: (%.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); timerStart(&data_processing_timer, 1000,1);//100ms @@ -394,25 +401,25 @@ void nmeaPrintf(void *signal_id) /** - * @brief ½«GGAʱ¼ä£¨HHMMSS.SSS¸ñʽµÄdouble£©×ª»»ÎªÐ­Òéʱ¼ä£¨U32£¬·Å´ó4000±¶£© - * @param utc_time GGAʱ¼ä£¨Èç75049.00±íʾ07:50:49.00£© - * @return uint32_t ЭÒéʱ¼ä£¨Êµ¼ÊÃëÊý = ·µ»ØÖµ / 4000£© + * @brief ?GGA???HHMMSS.SSS???double?????????U32???4000?? + * @param utc_time GGA????75049.00??07:50:49.00? + * @return uint32_t ????????? = ??? / 4000? */ uint32_t GgaTimeToProtocol(double utc_time) { - // 1. ÌáÈ¡ÕûÊý²¿·ÖºÍСÊý²¿·Ö£¨±ÜÃ⸡µãÔËË㣩 + // 1. ??????????????????? uint32_t time_int = (uint32_t)utc_time; // HHMMSS - uint32_t time_frac = (uint32_t)((utc_time - time_int) * 1000 + 0.5); // .SSS ¡ú ºÁÃ루ËÄÉáÎåÈ룩 + uint32_t time_frac = (uint32_t)((utc_time - time_int) * 1000 + 0.5); // .SSS ? ???????? - // 2. ·Ö½âHH¡¢MM¡¢SS - uint32_t hh = time_int / 10000; // Сʱ - uint32_t mm = (time_int % 10000) / 100; // ·ÖÖÓ - uint32_t ss = time_int % 100; // Ãë + // 2. ??HH?MM?SS + uint32_t hh = time_int / 10000; // ?? + uint32_t mm = (time_int % 10000) / 100; // ?? + uint32_t ss = time_int % 100; // ? - // 3. ¼ÆËã×ܺÁÃëÊý£¨HH*3600 + MM*60 + SS£©* 1000 + SSS + // 3. ???????HH*3600 + MM*60 + SS?* 1000 + SSS uint32_t total_ms = hh * 3600000 + mm * 60000 + ss * 1000 + time_frac; - // 4. ת»»ÎªÐ­Òéʱ¼ä£¨·Å´ó4000±¶£¬µÈ¼ÛÓÚ total_ms * 4£© + // 4. ??????????4000????? total_ms * 4? return total_ms * 4; } @@ -429,7 +436,7 @@ void rs232Process(void *signal_id) navigation_rs232_output.bit_data.header3 = 0x0B;//0B -//Å·À­½Ç +//??? float_temp = (float) (yis321_data.bit_data.roll)*0.000001; 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; navigation_rs232_output.bit_data.yaw = (int16_t) (float_temp*32768/360.0f); -//ÍÓÂÝ +//?? 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); @@ -449,7 +456,7 @@ void rs232Process(void *signal_id) 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); -//¼ÓËÙ¶È +//??? 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); @@ -459,34 +466,38 @@ void rs232Process(void *signal_id) 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); -//gpsÐÅÏ¢ +//gps?? 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.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; - float_temp = (float)vtg_data1.speed_kmh/3.6f*100.0f;//ϵÊý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.dual_heading = (int16_t)(ths_data1.heading*100);// À©´ó100±¶; + 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.checksum = calculate_checksum(&navigation_rs232_output.arr[0],57); - navigation_rs232_output.bit_data.timestamp = GgaTimeToProtocol(gga_data1.utc_time); publishMessage(&navigation_rs232_output, 1); - timerStart(&rs232_output_timer, 50,1);//50ms + timerStart(&rs232_output_timer, 10,1);//10ms } -// APPÄ£¿éµÄ³õʼ»¯ +// APP?????? void dataProcessingAppInit(void) { - // ³õʼ»¯¶¨Ê±Æ÷ + // ?????? timerInit(&iim6234_timer); timerInit(&rm3100_timer); timerInit(&data_processing_timer); timerInit(&rs232_output_timer); - rm3100Init();//³õʼ»¯RM3100 - // ¶©ÔĶ¨Ê±Æ÷Ðźţ¬ÓÃÓÚ½âÎöGNGGAÊý¾Ý + rm3100Init();//???RM3100 + // ????????????GNGGA?? subscribe(&iim6234_timer, iim6234Init); subscribe(&rm3100_timer, rm3100Process); subscribe(&nema_output1, nmeaProcess);