第一次提交
This commit is contained in:
505
ETH/UdpServer/app/app_data_processing.c
Normal file
505
ETH/UdpServer/app/app_data_processing.c
Normal file
@@ -0,0 +1,505 @@
|
||||
#include "interface_config.h"
|
||||
#include <stdint.h>
|
||||
#include <ctype.h>
|
||||
|
||||
// <20><>ʱ<EFBFBD><CAB1><EFBFBD>ṹ<EFBFBD><E1B9B9>
|
||||
Timer data_processing_timer;
|
||||
Timer iim6234_timer;
|
||||
Timer rm3100_timer;
|
||||
Timer rs232_output_timer;
|
||||
|
||||
StrInfNavigationOutput nav_data = {0}; // <20><>ʼ<EFBFBD><CABC>Ϊȫ0
|
||||
UnInfProtocolFrame yis321_data = {0};
|
||||
|
||||
GGA_Data gga_data1 = {0};
|
||||
VTG_Data vtg_data1 = {0};
|
||||
THS_Data ths_data1 = {0};
|
||||
|
||||
// <20><><EFBFBD>尴ť<E5B0B4>ṹ<EFBFBD><E1B9B9>
|
||||
|
||||
|
||||
uint8_t remote_tcp_ip[4] = {192,168,10,7};
|
||||
|
||||
|
||||
|
||||
/* <20><><EFBFBD>ȷָ<C8B7>ʽת<CABD><D7AA>Ϊʮ<CEAA><CAAE><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD> */
|
||||
double nmea_to_decimal(double dm, char direction)
|
||||
{
|
||||
double degrees = (int)(dm / 100);
|
||||
double minutes = dm - degrees * 100;
|
||||
double decimal = degrees + minutes / 60.0;
|
||||
return (direction == 'S' || direction == 'W') ? -decimal : decimal;
|
||||
}
|
||||
|
||||
// <20><><EFBFBD><EFBFBD>GNGGA<47><41><EFBFBD><EFBFBD>
|
||||
uint8_t parse_GGA(const char* data, GGA_Data* gga)
|
||||
{
|
||||
char buffer[256];
|
||||
strncpy(buffer, data, sizeof(buffer));
|
||||
char* token = strtok(buffer, ",");
|
||||
int field_index = 0;
|
||||
|
||||
while (token != NULL) {
|
||||
switch (field_index) {
|
||||
case 1: gga->utc_time = atof(token); break;
|
||||
case 2: gga->latitude = atof(token); break;
|
||||
case 3: gga->lat_dir = token[0]; break;
|
||||
case 4: gga->longitude = atof(token); break;
|
||||
case 5: gga->lon_dir = token[0]; break;
|
||||
case 6: gga->fix_quality = atoi(token); break;
|
||||
case 7: gga->satellites = atoi(token); break;
|
||||
case 8: gga->hdop = atof(token); break;
|
||||
case 9: gga->altitude = atof(token); break;
|
||||
}
|
||||
token = strtok(NULL, ",");
|
||||
field_index++;
|
||||
}
|
||||
|
||||
// <20>ȷ<EFBFBD>תʮ<D7AA><CAAE><EFBFBD>ƣ<EFBFBD><C6A3>ڽ<EFBFBD><DABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɺ<EFBFBD>ͳһ<CDB3><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
if (field_index > 5) { // ȷ<><C8B7><EFBFBD>ѽ<EFBFBD><D1BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֶ<EFBFBD>
|
||||
gga->latitude = nmea_to_decimal(gga->latitude, gga->lat_dir);
|
||||
gga->longitude = nmea_to_decimal(gga->longitude, gga->lon_dir);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// <20><><EFBFBD><EFBFBD>GNVTG<54><47><EFBFBD><EFBFBD>
|
||||
uint8_t parse_VTG(const char* data, VTG_Data* vtg)
|
||||
{
|
||||
char buffer[256];
|
||||
strncpy(buffer, data, sizeof(buffer));
|
||||
buffer[sizeof(buffer)-1] = '\0'; // ȷ<><C8B7><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD>ֹ
|
||||
char* token = strtok(buffer, ",");
|
||||
int field_index = 0;
|
||||
|
||||
while (token != NULL) {
|
||||
switch (field_index) {
|
||||
case 1: vtg->course_true = atof(token); break; // <20>汱<EFBFBD><E6B1B1><EFBFBD><EFBFBD>
|
||||
case 3: vtg->course_magnetic = atof(token); break; // <20>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>
|
||||
case 5: vtg->speed_knots = atof(token); break; // <20>ٶȣ<D9B6><C8A3>ڣ<EFBFBD>
|
||||
case 7: vtg->speed_kmh = atof(token); break; // <20>ٶȣ<D9B6>km/h<><68>
|
||||
case 9: vtg->mode = token[0]; break; // ģʽָʾ
|
||||
}
|
||||
token = strtok(NULL, ",");
|
||||
field_index++;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
// <20><><EFBFBD><EFBFBD>GNTHS<48><53><EFBFBD><EFBFBD>
|
||||
uint8_t parse_THS(const char* data, THS_Data* ths)
|
||||
{
|
||||
char buffer[128];
|
||||
strncpy(buffer, data, sizeof(buffer));
|
||||
buffer[sizeof(buffer)-1] = '\0';
|
||||
char* token = strtok(buffer, ",");
|
||||
int field_index = 0;
|
||||
|
||||
while (token != NULL) {
|
||||
switch (field_index) {
|
||||
case 1: // <20><><EFBFBD><EFBFBD><EFBFBD>ֶΣ<D6B6>ʾ<EFBFBD><CABE>ֵ<EFBFBD><D6B5>341.3344<EFBFBD><EFBFBD>
|
||||
ths->heading = atof(token);
|
||||
break;
|
||||
case 2: // ģʽ<C4A3>ֶΣ<D6B6>ʾ<EFBFBD><CABE>ֵ<EFBFBD><D6B5>A<EFBFBD><41>
|
||||
ths->mode = token[0];
|
||||
break;
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD>ֶΣ<D6B6>У<EFBFBD><D0A3><EFBFBD>͵ȣ<CDB5><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>չ
|
||||
}
|
||||
token = strtok(NULL, ",");
|
||||
field_index++;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void nmeaProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
// 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>
|
||||
|
||||
// if (!nmea_validate_checksum(&nema_output1.buffer[nema_output1.active_buf^1][0])) //У<><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// {
|
||||
// printf("NMEA CRC Failed");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// <20>жϺ<D0B6>3<EFBFBD><33><EFBFBD>ַ<EFBFBD><D6B7>Ƿ<EFBFBD>Ϊ"GGA"/"VTG"/"THS"
|
||||
if(strncmp(&nema_output1.buffer[3], "GGA", 3) == 0) {
|
||||
nema_output1.frame_ready = 1;
|
||||
}
|
||||
else if(strncmp(&nema_output1.buffer[3], "VTG", 3) == 0) {
|
||||
nema_output1.frame_ready = 2;
|
||||
}
|
||||
else if(strncmp(&nema_output1.buffer[3], "THS", 3) == 0) {
|
||||
nema_output1.frame_ready = 3;
|
||||
}
|
||||
else
|
||||
{ // <20><>Ŀ<EFBFBD><C4BF>Э<EFBFBD><D0AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
nema_output1.frame_ready = 0;
|
||||
}
|
||||
|
||||
// <20>ĺ<DEB8><C4BA><EFBFBD>switch<63><68><EFBFBD><EFBFBD>
|
||||
switch(nema_output1.frame_ready)
|
||||
{
|
||||
case NMEA_TYPE_GGA:
|
||||
|
||||
if (0 == parse_GGA(&nema_output1.buffer[0], &gga_data1))
|
||||
{
|
||||
|
||||
navigation_output1.bit_data.lon = (int32_t) (gga_data1.longitude * 10000000); //20250426<32>ȷ<EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA>10<31><30><EFBFBD><EFBFBD>
|
||||
navigation_output1.bit_data.lat = (int32_t) (gga_data1.latitude * 10000000);
|
||||
navigation_output1.bit_data.high = (int32_t)(gga_data1.altitude * 1000);//<2F><><EFBFBD><EFBFBD>1000
|
||||
|
||||
navigation_output1.bit_data.rtk_state = gga_data1.fix_quality;//
|
||||
|
||||
// printf("[GGA] Time:%.2f Lat:%.5f%c Lon:%.5f%c rtk_state:%d\n",
|
||||
// gga_data1.utc_time, gga_data1.latitude, gga_data1.lat_dir,
|
||||
// gga_data1.longitude, gga_data1.lon_dir,gga_data1.fix_quality);
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("gga parsing failed" );
|
||||
}
|
||||
break;
|
||||
|
||||
case NMEA_TYPE_VTG:
|
||||
|
||||
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);//ת<><D7AA>Ϊm/s <20><><EFBFBD><EFBFBD>100<30><30>
|
||||
// printf("[VTG] Course:%.1f<EFBFBD><EFBFBD> Speed:%.1fkm/h\n",
|
||||
// vtg_data1.course_true, vtg_data1.speed_kmh);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("vtg parsing failed" );
|
||||
}
|
||||
break;
|
||||
|
||||
case NMEA_TYPE_THS:
|
||||
|
||||
if (0 == parse_THS(&nema_output1.buffer[0], &ths_data1))
|
||||
{
|
||||
navigation_output1.bit_data.dual_antenna_heading = (int16_t)(ths_data1.heading*100);// <20><><EFBFBD><EFBFBD>100<30><30>
|
||||
// printf("[THS] Heading:%.1f<EFBFBD><EFBFBD> Mode:%c\n",
|
||||
// ths_data1.heading, ths_data1.mode);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("ths parsing failed" );
|
||||
}
|
||||
break;
|
||||
|
||||
default: // <20><><EFBFBD><EFBFBD>δ֪<CEB4><D6AA><EFBFBD><EFBFBD>
|
||||
// printf("Unknown NMEA frame type\n");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned short NumCheak(unsigned char *data,unsigned char len)//<2F><>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
{
|
||||
unsigned short NUM=0;
|
||||
unsigned char i;
|
||||
for(i = 0;i < len ; i++)
|
||||
{
|
||||
NUM += data[i];
|
||||
}
|
||||
return NUM;
|
||||
}
|
||||
|
||||
|
||||
|
||||
uint16_t yis321Cheak(unsigned char *data,unsigned char len)//<2F><>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
{
|
||||
uint8_t CK1 = 0;
|
||||
uint8_t CK2 = 0;
|
||||
uint16_t num;
|
||||
for(uint8_t i=0; i<len; i++)
|
||||
{
|
||||
CK1 = (uint8_t) (CK1 + data[i]);
|
||||
CK2 = (uint8_t) (CK2 + CK1);
|
||||
}
|
||||
num = CK2 << 8 | CK1;
|
||||
return num;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
float Acc[3],Gyro[3];
|
||||
void iim6234Process()
|
||||
{
|
||||
int lAcc[3],lGyro[3];
|
||||
float fAcc[3],fGyro[3];
|
||||
|
||||
if((imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][38]<< 8 | imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][39]) == NumCheak(&imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][3],35))
|
||||
{
|
||||
lAcc[0]=(( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][14]<<24 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][15]<<16 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][16]<<8 |imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][17]);
|
||||
lAcc[1]=(( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][18]<<24 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][19]<<16 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][20]<<8 |imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][21]);
|
||||
lAcc[2]=(( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][22]<<24 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][23]<<16 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][24]<<8 |imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][25]);
|
||||
lGyro[0]=(( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][26]<<24 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][27]<<16 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][28]<<8 |imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][29]);
|
||||
lGyro[1]=(( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][30]<<24 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][31]<<16 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][32]<<8 |imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][33]);
|
||||
lGyro[2]=(( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][34]<<24 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][35]<<16 |( int)imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][36]<<8 |imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][37]);
|
||||
memcpy(fAcc,lAcc,12);
|
||||
memcpy(fGyro,lGyro,12);
|
||||
|
||||
navigation_output1.bit_data.gyroscope_x_axis = (int16_t) (fGyro[0]*32768/300.0f);
|
||||
navigation_output1.bit_data.gyroscope_y_axis = (int16_t) (fGyro[1]*32768/300.0f);
|
||||
navigation_output1.bit_data.gyroscope_z_axis = (int16_t) (fGyro[2]*32768/300.0f);
|
||||
navigation_output1.bit_data.acceleration_x_axis = (int16_t) (fAcc[0]*32768/12.0f);
|
||||
navigation_output1.bit_data.acceleration_y_axis = (int16_t) (fAcc[1]*32768/12.0f);
|
||||
navigation_output1.bit_data.acceleration_z_axis = (int16_t) (fAcc[2]*32768/12.0f);
|
||||
|
||||
publishMessage(&navigation_output1, 1);
|
||||
|
||||
Acc[0]=fAcc[0];
|
||||
Acc[1]=fAcc[1];
|
||||
Acc[2]=fAcc[2];
|
||||
Gyro[0]=fGyro[0];
|
||||
Gyro[1]=fGyro[1];
|
||||
Gyro[2]=fGyro[2];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void rm3100Process(void *signal_id)
|
||||
{
|
||||
short hx,hy,hz=0;
|
||||
RM3100_GetData(&hx,&hy,&hz);
|
||||
|
||||
navigation_output1.bit_data.magnetic_x_axis = (int16_t)(hx/75);
|
||||
navigation_output1.bit_data.magnetic_y_axis = (int16_t)(hy/75);
|
||||
navigation_output1.bit_data.magnetic_z_axis = (int16_t)(hz/75);
|
||||
|
||||
timerStart(&rm3100_timer, RM3100_TIME,1);//10ms
|
||||
}
|
||||
|
||||
void imuInput(void *signal_id)
|
||||
{
|
||||
switch(imu_buffer1.type)
|
||||
{
|
||||
case IIM46234:
|
||||
iim6234Process();
|
||||
break;
|
||||
|
||||
case YIS321:
|
||||
|
||||
if((imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][82]<< 8 | imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][81]) == yis321Cheak(&imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][2],79))
|
||||
{
|
||||
memcpy(&yis321_data.arr[0],&imu_buffer1.dma_buffer[imu_buffer1.active_buf^1][0],sizeof(UnInfProtocolFrame));
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
||||
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;
|
||||
Gyro[0] = (float)((int32_t)yis321_data.bit_data.gyro_x)*0.000001f;
|
||||
Gyro[1] = (float)((int32_t)yis321_data.bit_data.gyro_y)*0.000001f;
|
||||
Gyro[2] = (float)((int32_t)yis321_data.bit_data.gyro_z)*0.000001f;
|
||||
|
||||
navigation_output1.bit_data.gyroscope_x_axis = (int16_t) (Gyro[0]*32768/300.0f);
|
||||
navigation_output1.bit_data.gyroscope_y_axis = (int16_t) (Gyro[1]*32768/300.0f);
|
||||
navigation_output1.bit_data.gyroscope_z_axis = (int16_t) (Gyro[2]*32768/300.0f);
|
||||
navigation_output1.bit_data.acceleration_x_axis = (int16_t) (Acc[0]*32768/12.0f);
|
||||
navigation_output1.bit_data.acceleration_y_axis = (int16_t) (Acc[1]*32768/12.0f);
|
||||
navigation_output1.bit_data.acceleration_z_axis = (int16_t) (Acc[2]*32768/12.0f);
|
||||
|
||||
navigation_outputC1.bit_data.q0 = SWAP_ENDIAN_32(yis321_data.bit_data.q0);
|
||||
navigation_outputC1.bit_data.q1 = SWAP_ENDIAN_32(yis321_data.bit_data.q1);
|
||||
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);
|
||||
|
||||
publishMessage(&navigation_output1, 1);
|
||||
}
|
||||
break;
|
||||
default:;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void iim6234Init(void *signal_id)
|
||||
{
|
||||
static uint8_t iim6234_state = 0;
|
||||
switch (iim6234_state)
|
||||
{
|
||||
case 0:
|
||||
publishMessage(set_output_data, 1);
|
||||
iim6234_state = 1;
|
||||
timerStart(&iim6234_timer, 100,1);
|
||||
break;
|
||||
case 1:
|
||||
publishMessage(set_rate, 1);
|
||||
iim6234_state = 2;
|
||||
timerStart(&iim6234_timer, 100,1);
|
||||
break;
|
||||
case 2:
|
||||
publishMessage(save_cmd, 1);
|
||||
iim6234_state = 3;
|
||||
timerStart(&iim6234_timer, 100,1);
|
||||
break;
|
||||
case 3:
|
||||
publishMessage(stream_cmd, 1);
|
||||
iim6234_state = 4;
|
||||
break;
|
||||
case 4:
|
||||
iim6234_state = 4;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
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("[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)\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
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @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>
|
||||
void dataProcessingAppInit(void)
|
||||
{
|
||||
// <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
|
||||
timerInit(&iim6234_timer);
|
||||
timerInit(&rm3100_timer);
|
||||
timerInit(&data_processing_timer);
|
||||
timerInit(&rs232_output_timer);
|
||||
|
||||
rm3100Init();//<2F><>ʼ<EFBFBD><CABC>RM3100
|
||||
// <20><><EFBFBD>Ķ<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>źţ<C5BA><C5A3><EFBFBD><EFBFBD>ڽ<EFBFBD><DABD><EFBFBD>GNGGA<47><41><EFBFBD><EFBFBD>
|
||||
subscribe(&iim6234_timer, iim6234Init);
|
||||
subscribe(&rm3100_timer, rm3100Process);
|
||||
subscribe(&nema_output1, nmeaProcess);
|
||||
subscribe(&data_processing_timer, nmeaPrintf);
|
||||
subscribe(&imu_buffer1, imuInput);
|
||||
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
|
||||
|
||||
printf("APP: initial OK %d\n",getCurrentTime());
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user