1 files changed, 14 insertions, 12 deletions

diff --git a/src/survive_cal.c b/src/survive_cal.c
index 2fc1896..e094e7b 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -61,19 +61,20 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	//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] = tan(v6.fcal_0_tilt);
-	b->fcaltilt[1] = tan(v6.fcal_1_tilt);  //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
-	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->fcal.phase[0] = v6.fcal_0_phase;
+	b->fcal.phase[1] = v6.fcal_1_phase;
+	b->fcal.tilt[0] = (v6.fcal_0_tilt);
+	b->fcal.tilt[1] = (v6.fcal_1_tilt); // XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
+	b->fcal.curve[0] = v6.fcal_0_curve;
+	b->fcal.curve[1] = v6.fcal_1_curve;
+	b->fcal.gibpha[0] = v6.fcal_0_gibphase;
+	b->fcal.gibpha[1] = v6.fcal_1_gibphase;
+	b->fcal.gibmag[0] = v6.fcal_0_gibmag;
+	b->fcal.gibmag[1] = v6.fcal_1_gibmag;
 	b->accel[0] = v6.accel_dir_x;
 	b->accel[1] = v6.accel_dir_y;
 	b->accel[2] = v6.accel_dir_z;
+	b->mode = v6.mode_current;
 	b->OOTXSet = 1;
 
 	config_set_lighthouse(ctx->lh_config,b,id);
@@ -617,8 +618,9 @@ static void handle_calibration( struct SurviveCalData *cd )
 			}
 			fsd.lengths[i][j][0] = cd->avglens[dataindex+0];
 			fsd.lengths[i][j][1] = cd->avglens[dataindex+1];
-			fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0];
-			fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1];
+			// fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0];
+			// fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1];
+			survive_apply_bsd_calibration(ctx, lh, &cd->avgsweeps[dataindex], fsd.angles[i][j]);
 			fsd.synctimes[i][j] = temp_syncs[i][j];
 		}