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
|
//<>< (C) 2016 C. N. Lohr, FULLY Under MIT/x11 License.
//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
#include "survive_cal.h"
//XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to
//proper angles, then from there perform the rest of the solution.
void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length )
{
struct SurviveContext * ctx = so->ctx;
int base_station = acode >> 2;
int axis = acode & 1;
if( ctx->calptr )
{
survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length );
}
if( base_station > NUM_LIGHTHOUSES ) return;
//No loner need sync information past this point.
if( sensor_id < 0 ) return;
FLT angle = (timeinsweep - so->timecenter_ticks) * (1./so->timecenter_ticks * 3.14159265359/2.0);
//Need to now do angle correction.
#if 1
struct BaseStationData * bsd = &ctx->bsd[base_station];
//XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18
angle += bsd->fcalphase[axis];
// angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
//TODO!!!
#endif
FLT length_sec = length / (FLT)so->timebase_hz;
ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle );
}
void survive_default_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
{
struct SurviveContext * ctx = so->ctx;
if( ctx->calptr )
{
survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
}
//TODO: Writeme!
}
void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
{
//TODO: Writeme!
}
|
