From 2e5d0355da2376f27dcbe0cc6d04b737145ac853 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 16 Feb 2017 15:47:46 -0500 Subject: update with more integration stuff. --- src/survive_cal.c | 27 ++++++++++++++++++++++++--- src/survive_process.c | 20 ++++++++++++++++++-- 2 files changed, 42 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/survive_cal.c b/src/survive_cal.c index adcb7bc..cdc7783 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -12,11 +12,26 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) struct SurviveCalData * cd = ctx->calptr; int id = ct->user1; - printf( "Got OOTX packet %d %p\n", id, cd ); + SV_INFO( "Got OOTX packet %d %p\n", id, cd ); lighthouse_info_v6 v6; init_lighthouse_info_v6(&v6, packet->data); - print_lighthouse_info_v6(&v6); + + 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; } @@ -64,8 +79,14 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //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; } } diff --git a/src/survive_process.c b/src/survive_process.c index 184532b..cb53588 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -8,13 +8,29 @@ 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( so->ctx->calptr ) + if( ctx->calptr ) { survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); } - //TODO: Writeme! + if( base_station > NUM_LIGHTHOUSES ) return; + + //No loner need sync information past this point. + if( sensor_id < 0 ) return; + + float angle = (timeinsweep - 200000) * (1./200000. * 3.14159265359/2.0); + + //Need to now do angle correction. +#if 1 + struct BaseStationData * bsd = &ctx->bsd[base_station]; + + //TODO!!! +#endif + } void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ) -- cgit v1.2.3