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

Compare commits

base: liumin:f0106c0f17ab3921292d374a6e1dc44cdff0c566
Branches Tags
liumin:main
liumin:20251024-IMU
..
compare: liumin:20251024-IMU
Branches Tags
liumin:main
liumin:20251024-IMU

2 Commits
f0106c0f17 ... 20251024-I

Author SHA1 Message Date
liumin
84221794b6 解决无符号处理以及定时问题 2025-10-24 21:56:55 +08:00
liumin
0346a5e662 增加CRC校验 2025-10-14 21:46:03 +08:00
3 changed files with 147 additions and 109 deletions
Expand all files Collapse all files

2
ETH/UdpServer/User/main.c
View File

@@ -133,7 +133,7 @@ int main(void)
bootInterfaceInit();
appMonitorInit();
testAppInit();
// testAppInit();
feedWatchdog();
dataProcessingAppInit();
ethernetInterfaceInit();

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

@@ -214,7 +214,14 @@ unsigned short NumCheak(unsigned char *data,unsigned char len)//
return NUM;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD>飨<EFBFBD>ֽ<EFBFBD>0-56<35><36>
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)//<2F><>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
@@ -287,8 +294,104 @@ void rm3100Process(void *signal_id)
timerStart(&rm3100_timer, RM3100_TIME,1);//10ms
}
/**
* @brief <20><>GGAʱ<41>䣨HHMMSS.SSS<53><53>ʽ<EFBFBD><CABD>double<6C><65>ת<EFBFBD><D7AA>ΪЭ<CEAA><D0AD>ʱ<EFBFBD>䣨U32<33><32><EFBFBD>Ŵ<EFBFBD>4000<30><30><EFBFBD><EFBFBD>
* @param utc_time GGAʱ<41>䣨<EFBFBD><E4A3A8>75049.00<EFBFBD><EFBFBD>ʾ07:50:49.00<EFBFBD><EFBFBD>
* @return uint32_t Э<><D0AD>ʱ<EFBFBD>䣨ʵ<E4A3A8><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD> = <20><><EFBFBD><EFBFBD>ֵ / 4000<30><30>
*/
uint32_t GgaTimeToProtocol(double utc_time)
{
// 1. <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֺ<EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>֣<EFBFBD><D6A3><EFBFBD><EFBFBD>⸡<EFBFBD><E2B8A1><EFBFBD><EFBFBD><EFBFBD>㣩
uint32_t time_int = (uint32_t)utc_time; // HHMMSS
uint32_t time_frac = (uint32_t)((utc_time - time_int) * 1000 + 0.5); // .SSS <20><> <20><><EFBFBD>루<EFBFBD><EBA3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>룩
// 2. <20>ֽ<EFBFBD>HH<48><48>MM<4D><4D>SS
uint32_t hh = time_int / 10000; // Сʱ
uint32_t mm = (time_int % 10000) / 100; // <20><><EFBFBD><EFBFBD>
uint32_t ss = time_int % 100; // <20><>
// 3. <20><><EFBFBD><EFBFBD><EFBFBD>ܺ<EFBFBD><DCBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>HH*3600 + MM*60 + SS<53><53>* 1000 + SSS
uint32_t total_ms = hh * 3600000 + mm * 60000 + ss * 1000 + time_frac;
// 4. ת<><D7AA>ΪЭ<CEAA><D0AD>ʱ<EFBFBD>䣨<EFBFBD>Ŵ<EFBFBD>4000<30><30><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD> total_ms * 4<><34>
return total_ms * 4;
}
void rs232Process(void)
{
float float_temp = 0.0f;
// navigation_rs232_output.bit_data.header[0] = 0xBD;//BD
// navigation_rs232_output.bit_data.header[1] = 0xDB;//DB
// navigation_rs232_output.bit_data.header[2] = 0x0B;//0B
navigation_rs232_output.bit_data.header1 = 0xBD;//BD
navigation_rs232_output.bit_data.header2 = 0xDB;//DB
navigation_rs232_output.bit_data.header3 = 0x0B;//0B
//ŷ<><C5B7><EFBFBD><EFBFBD>
float_temp = (float) (yis321_data.bit_data.roll)*0.000001;
navigation_rs232_output.bit_data.roll = (int16_t) (float_temp*32768/360.0f);
float_temp = (float) (yis321_data.bit_data.pitch)*0.000001;
navigation_rs232_output.bit_data.pitch = (int16_t) (float_temp*32768/360.0f);
float_temp = (float) (yis321_data.bit_data.yaw)*0.000001;
navigation_rs232_output.bit_data.yaw = (int16_t) (float_temp*32768/360.0f);
//<2F><><EFBFBD><EFBFBD>
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);
float_temp = (float) (yis321_data.bit_data.gyro_y)*0.000001;
navigation_rs232_output.bit_data.gyro_y = (int16_t) (float_temp*32768/300.0f);
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);
//<2F><><EFBFBD>ٶ<EFBFBD>
float_temp = (float) (yis321_data.bit_data.acc_x)*0.000001f/9.8;//<2F>޸ĵ<DEB8>λΪg
navigation_rs232_output.bit_data.accel_x = (int16_t) (float_temp*32768/12.0f);
float_temp = (float) (yis321_data.bit_data.acc_y)*0.000001f/9.8;//<2F>޸ĵ<DEB8>λΪg
navigation_rs232_output.bit_data.accel_y = (int16_t) (float_temp*32768/12.0f);
float_temp = (float) (yis321_data.bit_data.acc_z)*0.000001f/9.8;//<2F>޸ĵ<DEB8>λΪg
navigation_rs232_output.bit_data.accel_z = (int16_t) (float_temp*32768/12.0f);
//gps<70><73>Ϣ
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;//ϵ<><CFB5>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);// <20><><EFBFBD><EFBFBD>100<30><30>;
navigation_rs232_output.bit_data.nav_status = 0x32;
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);
// uint32_t debubg_len = 58;
// uint8_t debug_ip[4] = {192,168,17,77};
// UdpSendToData(SocketId2, (uint8_t *)&navigation_output2, &debubg_len,debug_ip,8011);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// timerStart(&rs232_output_timer, 20,1);//10ms
}
void imuInput(void *signal_id)
{
static uint8_t navigationProcessCnt = 0;
switch(imu_buffer1.type)
{
case IIM46234:
@@ -321,6 +424,17 @@ void imuInput(void *signal_id)
navigation_outputC1.bit_data.q2 = SWAP_ENDIAN_32(yis321_data.bit_data.q2);
navigation_outputC1.bit_data.q3 = SWAP_ENDIAN_32(yis321_data.bit_data.q3);
navigationProcessCnt ++;//<2F><><EFBFBD>ⶨʱ<E2B6A8><CAB1>׼<EFBFBD><D7BC><EFBFBD>·<EFBFBD><C2B7><EFBFBD>Ƶ<EFBFBD>ʽ<EFBFBD><CABD>͵<EFBFBD><CDB5><EFBFBD><EFBFBD><EFBFBD>
if(navigationProcessCnt >= 5)
{
navigationProcessCnt = 0;
rs232Process();
}
publishMessage(&navigation_output1, 1);
}
break;
@@ -368,112 +482,36 @@ void iim6234Init(void *signal_id)
void nmeaPrintf(void *signal_id)
{
printf("[THS] Heading:%.1f<EFBFBD><EFBFBD> Mode:%c\n",
ths_data1.heading, ths_data1.mode);
printf("[VTG] Course:%.1f<EFBFBD><EFBFBD> Speed:%.1fkm/h\n",
vtg_data1.course_true, vtg_data1.speed_kmh);
// printf("[THS] Heading:%.1f<EFBFBD><EFBFBD> Mode:%c\n",
// ths_data1.heading, ths_data1.mode);
// printf("[VTG] Course:%.1f<EFBFBD><EFBFBD> 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, // <20><><EFBFBD><EFBFBD><EFBFBD>߶ȵ<DFB6>λΪM(<28><>)
gga_data1.fix_quality,
gga_data1.satellites,
gga_data1.hdop
);
// 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, // <20><><EFBFBD><EFBFBD><EFBFBD>߶ȵ<DFB6>λΪM(<28><>)
// gga_data1.fix_quality,
// gga_data1.satellites,
// gga_data1.hdop
// );
printf("<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>: (%.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("<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>: (%.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("ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: (%.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);
// printf("ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: (%.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
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><C3B4>ڴ<EFBFBD>ӡ<EFBFBD><D3A1><EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
timerStart(&data_processing_timer, 1000,1);//100ms
}
/**
* @brief <20><>GGAʱ<41>䣨HHMMSS.SSS<53><53>ʽ<EFBFBD><CABD>double<6C><65>ת<EFBFBD><D7AA>ΪЭ<CEAA><D0AD>ʱ<EFBFBD>䣨U32<33><32><EFBFBD>Ŵ<EFBFBD>4000<30><30><EFBFBD><EFBFBD>
* @param utc_time GGAʱ<41>䣨<EFBFBD><E4A3A8>75049.00<EFBFBD><EFBFBD>ʾ07:50:49.00<EFBFBD><EFBFBD>
* @return uint32_t Э<><D0AD>ʱ<EFBFBD>䣨ʵ<E4A3A8><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD> = <20><><EFBFBD><EFBFBD>ֵ / 4000<30><30>
*/
uint32_t GgaTimeToProtocol(double utc_time)
{
// 1. <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֺ<EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>֣<EFBFBD><D6A3><EFBFBD><EFBFBD>⸡<EFBFBD><E2B8A1><EFBFBD><EFBFBD><EFBFBD>㣩
uint32_t time_int = (uint32_t)utc_time; // HHMMSS
uint32_t time_frac = (uint32_t)((utc_time - time_int) * 1000 + 0.5); // .SSS <20><> <20><><EFBFBD>루<EFBFBD><EBA3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>룩
// 2. <20>ֽ<EFBFBD>HH<48><48>MM<4D><4D>SS
uint32_t hh = time_int / 10000; // Сʱ
uint32_t mm = (time_int % 10000) / 100; // <20><><EFBFBD><EFBFBD>
uint32_t ss = time_int % 100; // <20><>
// 3. <20><><EFBFBD><EFBFBD><EFBFBD>ܺ<EFBFBD><DCBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>HH*3600 + MM*60 + SS<53><53>* 1000 + SSS
uint32_t total_ms = hh * 3600000 + mm * 60000 + ss * 1000 + time_frac;
// 4. ת<><D7AA>ΪЭ<CEAA><D0AD>ʱ<EFBFBD>䣨<EFBFBD>Ŵ<EFBFBD>4000<30><30><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD> total_ms * 4<><34>
return total_ms * 4;
}
void rs232Process(void *signal_id)
{
float float_temp = 0.0f;
// navigation_rs232_output.bit_data.header[0] = 0xBD;//BD
// navigation_rs232_output.bit_data.header[1] = 0xDB;//DB
// navigation_rs232_output.bit_data.header[2] = 0x0B;//0B
navigation_rs232_output.bit_data.header1 = 0xBD;//BD
navigation_rs232_output.bit_data.header2 = 0xDB;//DB
navigation_rs232_output.bit_data.header3 = 0x0B;//0B
//ŷ<><C5B7><EFBFBD><EFBFBD>
float_temp = (float) (yis321_data.bit_data.roll)*0.000001;
navigation_rs232_output.bit_data.roll = (int16_t) (float_temp*32768/360.0f);
float_temp = (float) (yis321_data.bit_data.pitch)*0.000001;
navigation_rs232_output.bit_data.pitch = (int16_t) (float_temp*32768/360.0f);
float_temp = (float) (yis321_data.bit_data.yaw)*0.000001;
navigation_rs232_output.bit_data.yaw = (int16_t) (float_temp*32768/360.0f);
//<2F><><EFBFBD><EFBFBD>
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);
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);
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);
//<2F><><EFBFBD>ٶ<EFBFBD>
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);
float_temp = (float) (yis321_data.bit_data.acc_y)*0.000001f;
navigation_rs232_output.bit_data.accel_y = (int16_t) (float_temp*32768/12.0f);
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<70><73>Ϣ
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.rtk_status = gga_data1.fix_quality;
float_temp = (float)vtg_data1.speed_kmh/3.6f*100.0f;//ϵ<><CFB5>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);// <20><><EFBFBD><EFBFBD>100<30><30>;
navigation_rs232_output.bit_data.timestamp = GgaTimeToProtocol(gga_data1.utc_time);
publishMessage(&navigation_rs232_output, 1);
timerStart(&rs232_output_timer, 50,1);//50ms
}
// APPģ<50><C4A3><EFBFBD>ij<EFBFBD>ʼ<EFBFBD><CABC>
@@ -492,13 +530,13 @@ void dataProcessingAppInit(void)
subscribe(&nema_output1, nmeaProcess);
subscribe(&data_processing_timer, nmeaPrintf);
subscribe(&imu_buffer1, imuInput);
subscribe(&rs232_output_timer,rs232Process);
// subscribe(&rs232_output_timer,rs232Process);
timerStart(&rm3100_timer, RM3100_TIME,1);//
timerStart(&iim6234_timer, 100,1);//100ms
timerStart(&data_processing_timer, 1000,1);//100ms
timerStart(&rs232_output_timer, 50,1);//50ms
// timerStart(&rs232_output_timer, 50,1);//50ms
printf("APP: initial OK %d\n",getCurrentTime());
}

12
ETH/UdpServer/app/app_data_processing.h
View File

@@ -47,16 +47,16 @@ typedef struct {
// <20><><EFBFBD>ٶȼƣ<C8BC><C6A3><EFBFBD><EFBFBD><EFBFBD>ID 0x10<31><30>
uint8_t acc_id; ///< <20><>ʶ<EFBFBD><CAB6>0x10<31><30>Э<EFBFBD>鸽¼A.1<EFBFBD><EFBFBD>
uint8_t acc_len; ///< <20><><EFBFBD>ݳ<EFBFBD><DDB3>ȣ<EFBFBD>12<31>ֽ<EFBFBD>
uint32_t acc_x; ///< X<><58><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD><EFBFBD> 0.000001 <20><>λ m/s2
uint32_t acc_y; ///< Y<><59><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><><EFBFBD><EFBFBD> 0.000001 <20><>λ m/s2
uint32_t acc_z; ///< Z<><5A><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><><EFBFBD><EFBFBD> 0.000001 <20><>λ m/s2
int32_t acc_x; ///< X<><58><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD><EFBFBD> 0.000001 <20><>λ m/s2
int32_t acc_y; ///< Y<><59><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><><EFBFBD><EFBFBD> 0.000001 <20><>λ m/s2
int32_t acc_z; ///< Z<><5A><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><><EFBFBD><EFBFBD> 0.000001 <20><>λ m/s2
// <20><><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD>ID 0x20<32><30>
uint8_t gyro_id; ///< <20><>ʶ<EFBFBD><CAB6>0x20
uint8_t gyro_len; ///< <20><><EFBFBD>ݳ<EFBFBD><DDB3>ȣ<EFBFBD>12<31>ֽ<EFBFBD>
uint32_t gyro_x; ///< X<><58><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><><EFBFBD><EFBFBD> 0.000001 <20><>λ deg/s
uint32_t gyro_y; ///< Y<><59><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><><EFBFBD><EFBFBD> 0.000001 <20><>λ deg/s
uint32_t gyro_z; ///< Z<><5A><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><><EFBFBD><EFBFBD> 0.000001 <20><>λ deg/s
int32_t gyro_x; ///< X<><58><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><><EFBFBD><EFBFBD> 0.000001 <20><>λ deg/s
int32_t gyro_y; ///< Y<><59><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><><EFBFBD><EFBFBD> 0.000001 <20><>λ deg/s
int32_t gyro_z; ///< Z<><5A><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><><EFBFBD><EFBFBD> 0.000001 <20><>λ deg/s
// ŷ<><C5B7><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD>ID 0x40<34><30>[1,2](@ref)
uint8_t euler_id; ///< <20><>ʶ<EFBFBD><CAB6><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD>ֵ0x40<34><30>Э<EFBFBD>鸽¼A.4<EFBFBD><EFBFBD>Tait-Bryan<61><6E>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD>