1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
|
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include "udp.h"
UDP_LISTENER::UDP_LISTENER() {
fd = -1;
}
UDP_LISTENER::~UDP_LISTENER() {
if( fd >= 0 )
close( fd );
}
STAT udp_open( UDP_LISTENER* udp, I32 port ) {
udp->fd = socket( AF_INET, SOCK_DGRAM, 0 );
if( udp->fd < 0 )
return STAT_ERR;
I32 one = 1;
setsockopt( udp->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one) );
struct sockaddr_in a;
memset( &a, 0, sizeof( a ) );
a.sin_family = AF_INET;
a.sin_addr.s_addr = htonl( INADDR_ANY );
a.sin_port = htons( (U16)port );
if( bind( udp->fd, (struct sockaddr*)&a, sizeof(a) ) < 0 ) {
close( udp->fd );
udp->fd = -1;
return STAT_ERR;
}
fcntl( udp->fd, F_SETFL, O_NONBLOCK );
return STAT_OK;
}
void udp_parse( UDP_LISTENER* udp, TELEMETRY* t ) {
if( udp->fd < 0 )
return;
U8 buf[2048];
for( ;; ) {
ssize_t n = recv( udp->fd, buf, sizeof( buf ), 0 );
if( n < 0 )
break; // EAGAIN
if( n < 20 )
continue; // not a forza packet
I32 race;
F32 fmax, fidle, fcur;
memcpy( &race, buf + 0, 4 );
memcpy( &fmax, buf + 8, 4 );
memcpy( &fidle, buf + 12, 4 );
memcpy( &fcur, buf + 16, 4 );
if( !( fmax >= 0.0f && fmax < 60000.0f ) )
continue;
if( !( fcur >= 0.0f && fcur < 60000.0f ) )
continue;
memcpy( &t->vel_x, buf + 28, 4 );
memcpy( &t->vel_y, buf + 32, 4 );
memcpy( &t->vel_z, buf + 40, 4 );
t->race_on = race;
t->rpm_max = fmax;
t->rpm_idle = fidle;
t->rpm = fcur;
t->extended = n >= 324;
if( t->extended ) {
I32 dt;
memcpy( &t->pos_x, buf + 244, 4 );
memcpy( &t->pos_y, buf + 248, 4 );
memcpy( &t->pos_z, buf + 252, 4 );
memcpy( &t->power, buf + 260, 4 );
memcpy( &t->speed_ms, buf + 256, 4 );
memcpy( &t->boost_psi, buf + 284, 4 );
memcpy( &dt, buf + 224, 4 );
memcpy( &t->laptime, buf + 304, 4 );
memcpy( &t->racetime, buf + 308, 4 );
t->lapnum = *(U16*)( &buf[312] );
t->racepos = buf[314];
t->drivetrain = dt;
t->accel = buf[315];
t->brake = buf[316];
t->clutch = buf[317];
t->handbrake = buf[318];
t->gear = buf[319];
t->norm_driving_line = buf[321];
t->norm_ai_brake_diff = buf[322];
memcpy( &t->car_ordinal, buf + 212, 4 );
for( I32 i = 0; i < 4; i++ ) {
memcpy( &t->slip_ratio[i], buf + 84 + 4* i, 4 );
memcpy( &t->combined_slip[i], buf + 180 + 4* i, 4 );
memcpy( &t->tire_temp[i], buf + 268 + 4* i, 4 );
}
}
t->stamp = t_nows();
}
}
|
