From d1199e02739f4099fea773836ea6ed4fb3e4f337 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 10 Nov 2020 13:17:47 +0100 Subject: [PATCH] #641 Refactoring, renamed types to work better with both LH1 and LH2 --- examples/app_api/src/app_lighthouse.c | 4 +- examples/demos/swarm_demo/app.c | 4 +- src/modules/interface/stabilizer_types.h | 2 +- src/modules/src/lighthouse/lighthouse_core.c | 10 ++--- .../src/lighthouse/lighthouse_position_est.c | 8 ++-- .../lighthouse/lighthouse_calibration.h | 8 ++-- .../interface/lighthouse/pulse_processor.h | 8 ++-- .../src/lighthouse/lighthouse_calibration.c | 42 +++++++++---------- src/utils/src/lighthouse/pulse_processor_v1.c | 10 ++--- src/utils/src/lighthouse/pulse_processor_v2.c | 2 +- 10 files changed, 49 insertions(+), 49 deletions(-) diff --git a/examples/app_api/src/app_lighthouse.c b/examples/app_api/src/app_lighthouse.c index c8730df8b5..8508be1a05 100644 --- a/examples/app_api/src/app_lighthouse.c +++ b/examples/app_api/src/app_lighthouse.c @@ -37,14 +37,14 @@ baseStationGeometry_t sampleGeoData[PULSE_PROCESSOR_N_BASE_STATIONS] = { lighthouseCalibration_t sampleCalibrationData[PULSE_PROCESSOR_N_BASE_STATIONS] = { { // Base station 0 .valid = true, - .axis = { + .sweep = { {.tilt = -0.047058, .phase = 0.0, .curve = 0.052215, .gibphase = 2.087890, .gibmag = -0.003913, .ogeephase = 0.433105, .ogeemag = -0.049285}, {.tilt = 0.048065, .phase = -0.005336, .curve = 0.122375, .gibphase = 2.097656, .gibmag = -0.003883, .ogeephase = 0.631835, .ogeemag = -0.034851}, }, }, { // Base station 1 .valid = true, - .axis = { + .sweep = { {.tilt = -0.051208, .phase = 0.0, .curve = 0.011756, .gibphase = 2.136718, .gibmag = -0.006057, .ogeephase = 2.705078,}, {.tilt = 0.045623, .phase = -0.004142, .curve = 0.104736, .gibphase = 2.349609, .gibmag = -0.003332, .ogeephase = 0.380859, .ogeemag = -0.240112,}, }, diff --git a/examples/demos/swarm_demo/app.c b/examples/demos/swarm_demo/app.c index f46862af6b..479021da02 100644 --- a/examples/demos/swarm_demo/app.c +++ b/examples/demos/swarm_demo/app.c @@ -181,14 +181,14 @@ const baseStationGeometry_t lighthouseGeoData[PULSE_PROCESSOR_N_BASE_STATIONS] lighthouseCalibration_t lighthouseCalibrationData[PULSE_PROCESSOR_N_BASE_STATIONS] = { { // Base station 0 .valid = true, - .axis = { + .sweep = { {.tilt = -0.047058, .phase = 0.0, .curve = 0.052215, .gibphase = 2.087890, .gibmag = -0.003913, .ogeephase = 0.433105, .ogeemag = -0.049285}, {.tilt = 0.048065, .phase = -0.005336, .curve = 0.122375, .gibphase = 2.097656, .gibmag = -0.003883, .ogeephase = 0.631835, .ogeemag = -0.034851}, }, }, { // Base station 1 .valid = true, - .axis = { + .sweep = { {.tilt = -0.051208, .phase = 0.0, .curve = 0.011756, .gibphase = 2.136718, .gibmag = -0.006057, .ogeephase = 2.705078,}, {.tilt = 0.045623, .phase = -0.004142, .curve = 0.104736, .gibphase = 2.349609, .gibmag = -0.003332, .ogeephase = 0.380859, .ogeemag = -0.240112,}, }, diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 9f17b681fd..9b730d63fe 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -256,7 +256,7 @@ typedef struct { float measuredSweepAngle; float stdDev; int baseStationType; // Cast to lighthouseBaseStationType_t enum. - const lighthouseCalibrationAxis_t* calib; + const lighthouseCalibrationSweep_t* calib; } sweepAngleMeasurement_t; // Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number. diff --git a/src/modules/src/lighthouse/lighthouse_core.c b/src/modules/src/lighthouse/lighthouse_core.c index 744b00c8e6..5ca59fca47 100644 --- a/src/modules/src/lighthouse/lighthouse_core.c +++ b/src/modules/src/lighthouse/lighthouse_core.c @@ -190,8 +190,8 @@ static void convertV2AnglesToV1Angles(pulseProcessorResult_t* angles) { } } -static void usePulseResult(pulseProcessor_t *appState, pulseProcessorResult_t* angles, int basestation, int axis) { - if (axis == sweepDirection_y) { +static void usePulseResult(pulseProcessor_t *appState, pulseProcessorResult_t* angles, int basestation, int sweepId) { + if (sweepId == sweepIdSecond) { pulseProcessorApplyCalibration(appState, angles, basestation); if (lighthouseBsTypeV2 == angles->measurementType) { // Emulate V1 base stations for now, convert to V1 angles @@ -262,13 +262,13 @@ static pulseProcessorProcessPulse_t identifySystem(const lighthouseUartFrame_t* static void processFrame(pulseProcessor_t *appState, pulseProcessorResult_t* angles, const lighthouseUartFrame_t* frame) { int basestation; - int axis; + int sweepId; pulseWidth[frame->data.sensor] = frame->data.width; - if (pulseProcessorProcessPulse(&ppState, &frame->data, angles, &basestation, &axis)) { + if (pulseProcessorProcessPulse(&ppState, &frame->data, angles, &basestation, &sweepId)) { STATS_CNT_RATE_EVENT(bsRates[basestation]); - usePulseResult(appState, angles, basestation, axis); + usePulseResult(appState, angles, basestation, sweepId); } } diff --git a/src/modules/src/lighthouse/lighthouse_position_est.c b/src/modules/src/lighthouse/lighthouse_position_est.c index 291eb520bd..faaedbc8c1 100644 --- a/src/modules/src/lighthouse/lighthouse_position_est.c +++ b/src/modules/src/lighthouse/lighthouse_position_est.c @@ -217,7 +217,7 @@ static void estimatePositionSweepsLh1(pulseProcessorResult_t* angles, int baseSt if (sweepInfo.measuredSweepAngle != 0) { sweepInfo.rotorRot = &lighthouseBaseStationsGeometry[baseStation].mat; sweepInfo.rotorRotInv = &baseStationInvertedRotationMatrixes[baseStation]; - sweepInfo.calib = &bsCalib->axis[0]; + sweepInfo.calib = &bsCalib->sweep[0]; estimatorEnqueueSweepAngles(&sweepInfo); STATS_CNT_RATE_EVENT(bsEstRates[baseStation]); @@ -227,7 +227,7 @@ static void estimatePositionSweepsLh1(pulseProcessorResult_t* angles, int baseSt if (sweepInfo.measuredSweepAngle != 0) { sweepInfo.rotorRot = &lh1Rotor2RotationMatrixes[baseStation]; sweepInfo.rotorRotInv = &lh1Rotor2InvertedRotationMatrixes[baseStation]; - sweepInfo.calib = &bsCalib->axis[1]; + sweepInfo.calib = &bsCalib->sweep[1]; estimatorEnqueueSweepAngles(&sweepInfo); STATS_CNT_RATE_EVENT(bsEstRates[baseStation]); @@ -252,7 +252,7 @@ static void estimatePositionSweepsLh2(pulseProcessorResult_t* angles, int baseSt sweepInfo.measuredSweepAngle = bsMeasurement->angles[0]; if (sweepInfo.measuredSweepAngle != 0) { sweepInfo.t = -t30; - sweepInfo.calib = &bsCalib->axis[0]; + sweepInfo.calib = &bsCalib->sweep[0]; estimatorEnqueueSweepAngles(&sweepInfo); STATS_CNT_RATE_EVENT(bsEstRates[baseStation]); } @@ -260,7 +260,7 @@ static void estimatePositionSweepsLh2(pulseProcessorResult_t* angles, int baseSt sweepInfo.measuredSweepAngle = bsMeasurement->angles[1]; if (sweepInfo.measuredSweepAngle != 0) { sweepInfo.t = t30; - sweepInfo.calib = &bsCalib->axis[1]; + sweepInfo.calib = &bsCalib->sweep[1]; estimatorEnqueueSweepAngles(&sweepInfo); STATS_CNT_RATE_EVENT(bsEstRates[baseStation]); } diff --git a/src/utils/interface/lighthouse/lighthouse_calibration.h b/src/utils/interface/lighthouse/lighthouse_calibration.h index 0b88e02265..4c255236b1 100644 --- a/src/utils/interface/lighthouse/lighthouse_calibration.h +++ b/src/utils/interface/lighthouse/lighthouse_calibration.h @@ -11,11 +11,11 @@ typedef struct { // Lh2 extra params float ogeemag; float ogeephase; -} lighthouseCalibrationAxis_t; +} lighthouseCalibrationSweep_t; typedef struct { bool valid; - lighthouseCalibrationAxis_t axis[2]; + lighthouseCalibrationSweep_t sweep[2]; } lighthouseCalibration_t; /** @@ -60,7 +60,7 @@ void lighthouseCalibrationApplyNothing(const float rawAngles[2], float corrected * @param calib Calibration data for the rotor * @return float The predicted uncompensated sweep angle of the rotor */ -float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, const lighthouseCalibrationAxis_t* calib); +float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, const lighthouseCalibrationSweep_t* calib); /** * @brief Predict the measured sweep angle based on a position for a lighthouse 2 rotor. The position is relative to the rotor reference frame. @@ -71,4 +71,4 @@ float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, con * @param calib Calibration data for the rotor * @return float The predicted uncompensated sweep angle of the rotor */ -float lighthouseCalibrationMeasurementModelLh2(const float x, const float y, const float z, const float t, const lighthouseCalibrationAxis_t* calib); +float lighthouseCalibrationMeasurementModelLh2(const float x, const float y, const float z, const float t, const lighthouseCalibrationSweep_t* calib); diff --git a/src/utils/interface/lighthouse/pulse_processor.h b/src/utils/interface/lighthouse/pulse_processor.h index 9758c82cce..b57baf770e 100644 --- a/src/utils/interface/lighthouse/pulse_processor.h +++ b/src/utils/interface/lighthouse/pulse_processor.h @@ -107,9 +107,9 @@ typedef struct { } pulseProcessorPulse_t; typedef enum { - sweepDirection_x = 0, - sweepDirection_y = 1 -} SweepDirection; + sweepIdFirst = 0, // The X sweep in LH 1 + sweepIdSecond = 1 // The Y sweep in LH 1 +} SweepId_t; typedef enum { sweepStorageStateWaiting = 0, @@ -150,7 +150,7 @@ typedef struct { // Base station and axis of the current frame int currentBaseStation; - SweepDirection currentAxis; + SweepId_t currentAxis; // Sweep timestamps struct { diff --git a/src/utils/src/lighthouse/lighthouse_calibration.c b/src/utils/src/lighthouse/lighthouse_calibration.c index 47c47623f3..c994603066 100644 --- a/src/utils/src/lighthouse/lighthouse_calibration.c +++ b/src/utils/src/lighthouse/lighthouse_calibration.c @@ -38,21 +38,21 @@ void lighthouseCalibrationInitFromFrame(lighthouseCalibration_t *calib, struct ootxDataFrame_s *frame) { - calib->axis[0].phase = frame->phase0; - calib->axis[0].tilt = frame->tilt0; - calib->axis[0].curve = frame->curve0; - calib->axis[0].gibmag = frame->gibmag0; - calib->axis[0].gibphase = frame->gibphase0; - calib->axis[0].ogeemag = frame->ogeemag0; - calib->axis[0].ogeephase = frame->ogeephase0; - - calib->axis[1].phase = frame->phase1; - calib->axis[1].tilt = frame->tilt1; - calib->axis[1].curve = frame->curve1; - calib->axis[1].gibmag = frame->gibmag1; - calib->axis[1].gibphase = frame->gibphase1; - calib->axis[1].ogeemag = frame->ogeemag1; - calib->axis[1].ogeephase = frame->ogeephase1; + calib->sweep[0].phase = frame->phase0; + calib->sweep[0].tilt = frame->tilt0; + calib->sweep[0].curve = frame->curve0; + calib->sweep[0].gibmag = frame->gibmag0; + calib->sweep[0].gibphase = frame->gibphase0; + calib->sweep[0].ogeemag = frame->ogeemag0; + calib->sweep[0].ogeephase = frame->ogeephase0; + + calib->sweep[1].phase = frame->phase1; + calib->sweep[1].tilt = frame->tilt1; + calib->sweep[1].curve = frame->curve1; + calib->sweep[1].gibmag = frame->gibmag1; + calib->sweep[1].gibphase = frame->gibphase1; + calib->sweep[1].ogeemag = frame->ogeemag1; + calib->sweep[1].ogeephase = frame->ogeephase1; calib->valid = true; } @@ -65,8 +65,8 @@ static void idealToDistortedV1(const lighthouseCalibration_t* calib, const float const float y = tanf(ax); const float z = tanf(ay); - distorted[0] = lighthouseCalibrationMeasurementModelLh1(x, y, z, &calib->axis[0]); - distorted[1] = lighthouseCalibrationMeasurementModelLh1(x, z, -y, &calib->axis[1]); + distorted[0] = lighthouseCalibrationMeasurementModelLh1(x, y, z, &calib->sweep[0]); + distorted[1] = lighthouseCalibrationMeasurementModelLh1(x, z, -y, &calib->sweep[1]); } static void idealToDistortedV2(const lighthouseCalibration_t* calib, const float* ideal, float* distorted) { @@ -80,8 +80,8 @@ static void idealToDistortedV2(const lighthouseCalibration_t* calib, const float const float y = tanf((a2 + a1) / 2.0f); const float z = sinf(a2 - a1) / (tan30 * (cosf(a2) + cosf(a1))); - distorted[0] = lighthouseCalibrationMeasurementModelLh2(x, y, z, -t30, &calib->axis[0]); - distorted[1] = lighthouseCalibrationMeasurementModelLh2(x, y, z, t30, &calib->axis[1]); + distorted[0] = lighthouseCalibrationMeasurementModelLh2(x, y, z, -t30, &calib->sweep[0]); + distorted[1] = lighthouseCalibrationMeasurementModelLh2(x, y, z, t30, &calib->sweep[1]); } typedef void (* idealToDistortedFcn_t)(const lighthouseCalibration_t* calib, const float* ideal, float* distorted); @@ -123,7 +123,7 @@ void lighthouseCalibrationApplyNothing(const float rawAngles[2], float corrected correctedAngles[1] = rawAngles[1]; } -float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, const lighthouseCalibrationAxis_t* calib) { +float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, const lighthouseCalibrationSweep_t* calib) { const float ax = atan2f(y, x); const float ay = atan2f(z, x); const float r = arm_sqrt(x * x + y * y); @@ -135,7 +135,7 @@ float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, con return ax - (compTilt + calib->phase + compGib + compCurve); } -float lighthouseCalibrationMeasurementModelLh2(const float x, const float y, const float z, const float t, const lighthouseCalibrationAxis_t* calib) { +float lighthouseCalibrationMeasurementModelLh2(const float x, const float y, const float z, const float t, const lighthouseCalibrationSweep_t* calib) { const float ax = atan2f(y, x); // const float ay = atan2f(z, x); const float r = arm_sqrt(x * x + y * y); diff --git a/src/utils/src/lighthouse/pulse_processor_v1.c b/src/utils/src/lighthouse/pulse_processor_v1.c index 72285c6098..14c7324cfb 100644 --- a/src/utils/src/lighthouse/pulse_processor_v1.c +++ b/src/utils/src/lighthouse/pulse_processor_v1.c @@ -145,11 +145,11 @@ TESTABLE_STATIC int getBaseStationId(pulseProcessorV1_t *stateV1, unsigned int t return baseStation; } -static SweepDirection getAxis(int width) { - SweepDirection result = sweepDirection_x; +static SweepId_t getAxis(int width) { + SweepId_t result = sweepIdFirst; if ((((width-SYNC_BASE_WIDTH)/SYNC_DIVIDER)&0x01) != 0) { - result = sweepDirection_y; + result = sweepIdSecond; } return result; @@ -226,7 +226,7 @@ static void storeSyncData(pulseProcessorV1_t *stateV1, int baseStation, unsigned if (0 == baseStation) { stateV1->currentSync0 = timestamp; stateV1->currentSync0Width = width; - if (getAxis(width) == sweepDirection_x) { + if (getAxis(width) == sweepIdFirst) { uint32_t prevSync0X = stateV1->currentSync0X; stateV1->currentSync0X = timestamp; stateV1->frameWidth[0][0] = TS_DIFF(stateV1->currentSync0X, prevSync0X); @@ -237,7 +237,7 @@ static void storeSyncData(pulseProcessorV1_t *stateV1, int baseStation, unsigned } } else { stateV1->currentSync1Width = width; - if (getAxis(width) == sweepDirection_x) { + if (getAxis(width) == sweepIdFirst) { uint32_t prevSync1X = stateV1->currentSync1X; stateV1->currentSync1X = timestamp; stateV1->frameWidth[1][0] = TS_DIFF(stateV1->currentSync1X, prevSync1X); diff --git a/src/utils/src/lighthouse/pulse_processor_v2.c b/src/utils/src/lighthouse/pulse_processor_v2.c index c1962e5489..2dce2fee7a 100644 --- a/src/utils/src/lighthouse/pulse_processor_v2.c +++ b/src/utils/src/lighthouse/pulse_processor_v2.c @@ -306,7 +306,7 @@ bool handleAngles(pulseProcessor_t *state, const pulseProcessorFrame_t* frameDat calculateAngles(block, previousBlock, angles); *baseStation = channel; - *axis = sweepDirection_y; + *axis = sweepIdSecond; angles->measurementType = lighthouseBsTypeV2; anglesMeasured = true;