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
|
// (C) 2016, 2017 Joshua Allen, MIT/x11 License.
// (C) 2016, 2017 <>< C. N. Lohr, Under MIT/x11 License.
// All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
#include "survive_cal.h"
#include "survive_internal.h"
void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
{
struct SurviveContext * ctx = (struct SurviveContext*)(ct->user);
struct SurviveCalData * cd = ctx->calptr;
int id = ct->user1;
SV_INFO( "Got OOTX packet %d %p\n", id, cd );
lighthouse_info_v6 v6;
init_lighthouse_info_v6(&v6, packet->data);
struct BaseStationData * b = &ctx->bsd[id];
//print_lighthouse_info_v6(&v6);
b->BaseStationID = v6.id;
b->fcalphase[0] = v6.fcal_0_phase;
b->fcalphase[1] = v6.fcal_1_phase;
b->fcaltilt[0] = v6.fcal_0_tilt;
b->fcaltilt[1] = v6.fcal_1_tilt;
b->fcalcurve[0] = v6.fcal_0_curve;
b->fcalcurve[1] = v6.fcal_1_curve;
b->fcalgibpha[0] = v6.fcal_0_gibphase;
b->fcalgibpha[1] = v6.fcal_1_gibphase;
b->fcalgibmag[0] = v6.fcal_0_gibmag;
b->fcalgibmag[1] = v6.fcal_1_gibmag;
b->OOTXSet = 1;
}
void survive_cal_install( struct SurviveContext * ctx )
{
int i;
struct SurviveCalData * cd = ctx->calptr = calloc( 1, sizeof( struct SurviveCalData ) );
for( i = 0; i < NUM_LIGHTHOUSES; i++ )
{
ootx_init_decoder_context(&cd->ootx_decoders[i]);
cd->ootx_decoders[i].user = ctx;
cd->ootx_decoders[i].user1 = i;
}
cd->stage = 1;
ootx_packet_clbk = ootx_packet_clbk_d;
ctx->calptr = cd;
}
void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length )
{
struct SurviveContext * ctx = so->ctx;
struct SurviveCalData * cd = ctx->calptr;
if( !cd ) return;
switch( cd->stage )
{
default:
case 0: //Default, inactive.
break;
case 1:
//Collecting OOTX data.
if( sensor_id < 0 )
{
int lhid = -sensor_id-1;
if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' )
{
uint8_t dbit = (acode & 2)>>1;
//printf( "%s %d %d %d\n", so->codename, lhid, acode, dbit );
ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
}
int i;
for( i = 0; i < NUM_LIGHTHOUSES; i++ )
if( ctx->bsd[i].OOTXSet == 0 ) break;
if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on.
}
break;
case 2:
break;
}
}
|
