diff --git a/src/deck/drivers/src/vl53l0x.c b/src/deck/drivers/src/vl53l0x.c index cc48080780..7205227656 100644 --- a/src/deck/drivers/src/vl53l0x.c +++ b/src/deck/drivers/src/vl53l0x.c @@ -35,9 +35,26 @@ #include "log.h" #include "i2cdev.h" - #include "vl53l0x.h" +#include "stabilizer_types.h" +#ifdef ESTIMATOR_TYPE_kalman + +#include "estimator_kalman.h" +#include "arm_math.h" + +//#define UPDATE_KALMAN_WITH_RANGING // uncomment to push into the kalman +#ifdef UPDATE_KALMAN_WITH_RANGING +#define RANGE_OUTLIER_LIMIT 1500 // the measured range is in [mm] +// Measurement noise model +static float expPointA = 1.0f; +static float expStdA = 0.0025f; // STD at elevation expPointA [m] +static float expPointB = 1.3f; +static float expStdB = 0.2f; // STD at elevation expPointB [m] +static float expCoeff; +#endif // UPDATE_KALMAN_WITH_RANGING +#endif // ESTIMATOR_TYPE_kalman + static uint8_t devAddr; static I2C_Dev *I2Cx; static bool isInit; @@ -120,6 +137,7 @@ static bool vl53l0xWriteReg32Bit(uint8_t reg, uint32_t val); /** Default constructor, uses default I2C address. * @see VL53L0X_DEFAULT_ADDRESS */ + void vl53l0xInit(DeckInfo* info) { if (isInit) @@ -128,9 +146,13 @@ void vl53l0xInit(DeckInfo* info) i2cdevInit(I2C1_DEV); I2Cx = I2C1_DEV; devAddr = VL53L0X_DEFAULT_ADDRESS; - xTaskCreate(vl53l0xTask, "vl53l0x", 2*configMINIMAL_STACK_SIZE, NULL, 3, NULL); - + +#if defined(ESTIMATOR_TYPE_kalman) && defined(UPDATE_KALMAN_WITH_RANGING) + // pre-compute constant in the measurement noise mdoel + expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA); +#endif + isInit = true; } @@ -140,7 +162,7 @@ bool vl53l0xTest(void) if (!isInit) return false; - + // Measurement noise model testStatus = vl53l0xTestConnection(); testStatus &= vl53l0xInitSensor(true); @@ -154,10 +176,24 @@ void vl53l0xTask(void* arg) vl53l0xSetVcselPulsePeriod(VcselPeriodPreRange, 18); vl53l0xSetVcselPulsePeriod(VcselPeriodFinalRange, 14); - vl53l0xStartContinuous(0); + vl53l0xStartContinuous(100); while (1) { xLastWakeTime = xTaskGetTickCount(); range_last = vl53l0xReadRangeContinuousMillimeters(); +#if defined(ESTIMATOR_TYPE_kalman) && defined(UPDATE_KALMAN_WITH_RANGING) + // check if range is feasible and push into the kalman filter + // the sensor should not be able to measure >3 [m], and outliers typically + // occur as >8 [m] measurements + if (range_last < RANGE_OUTLIER_LIMIT){ + + // Form measurement + tofMeasurement_t tofData; + tofData.timestamp = xTaskGetTickCount(); + tofData.distance = (float)range_last * 0.001f; // Scale from [mm] to [m] + tofData.stdDev = expStdA * (1.0f + expf( expCoeff * ( tofData.distance - expPointA))); + stateEstimatorEnqueueTOF(&tofData); + } +#endif vTaskDelayUntil(&xLastWakeTime, M2T(measurement_timing_budget_ms)); } } @@ -1105,8 +1141,8 @@ bool vl53l0xWriteReg32Bit(uint8_t reg, uint32_t val) // TODO: Decide on vid:pid and set the used pins static const DeckDriver vl53l0x_deck = { - .vid = 0, - .pid = 0, + .vid = 0, // Changed this from 0 + .pid = 0, // Changed this from 0 .name = "vl53l0x", .usedGpio = 0, diff --git a/src/modules/interface/estimator_kalman.h b/src/modules/interface/estimator_kalman.h index 8f975c5b10..aa4a2511b0 100644 --- a/src/modules/interface/estimator_kalman.h +++ b/src/modules/interface/estimator_kalman.h @@ -67,3 +67,4 @@ bool stateEstimatorTest(void); bool stateEstimatorEnqueueTDOA(tdoaMeasurement_t *uwb); bool stateEstimatorEnqueuePosition(positionMeasurement_t *pos); bool stateEstimatorEnqueueDistance(distanceMeasurement_t *dist); +bool stateEstimatorEnqueueTOF(tofMeasurement_t *tof); diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 9bea719bde..3c0f9e8acf 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -173,6 +173,13 @@ typedef struct setpointZ_s { bool isUpdate; // True = small update of setpoint, false = completely new } setpointZ_t; +/** TOF measurement**/ +typedef struct tofMeasurement_s { + uint32_t timestamp; + float distance; + float stdDev; +} tofMeasurement_t; + // Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number. #define RATE_1000_HZ 1000 #define RATE_500_HZ 500 diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c index d3a148f241..2767e5d518 100644 --- a/src/modules/src/estimator_kalman.c +++ b/src/modules/src/estimator_kalman.c @@ -141,6 +141,15 @@ static inline bool stateEstimatorHasTDOAPacket(tdoaMeasurement_t *uwb) { return (pdTRUE == xQueueReceive(tdoaDataQueue, uwb, 0)); } +// Measurements of TOF from laser sensor +static xQueueHandle tofDataQueue; +#define TOF_QUEUE_LENGTH (10) + +static void stateEstimatorUpdateWithTof(tofMeasurement_t *tof); + +static inline bool stateEstimatorHasTOFPacket(tofMeasurement_t *tof) { + return (pdTRUE == xQueueReceive(tofDataQueue, tof, 0)); +} /** * Constants used in the estimator @@ -261,8 +270,6 @@ static uint32_t lastFlightCmd; static uint32_t takeoffTime; static uint32_t tdoaCount; - - /** * Supporting and utility functions */ @@ -421,6 +428,13 @@ void stateEstimatorUpdate(state_t *state, sensorData_t *sensors, control_t *cont * we therefore consume all measurements since the last loop, rather than accumulating */ + tofMeasurement_t tof; + while (stateEstimatorHasTOFPacket(&tof)) + { + stateEstimatorUpdateWithTof(&tof); + doneUpdate = true; + } + distanceMeasurement_t dist; while (stateEstimatorHasDistanceMeasurement(&dist)) { @@ -442,7 +456,6 @@ void stateEstimatorUpdate(state_t *state, sensorData_t *sensors, control_t *cont doneUpdate = true; } - /** * If an update has been made, the state is finalized: * - the attitude error is moved into the body attitude quaternion, @@ -923,6 +936,32 @@ static void stateEstimatorUpdateWithTDOA(tdoaMeasurement_t *tdoa) tdoaCount++; } +static void stateEstimatorUpdateWithTof(tofMeasurement_t *tof) +{ + // Updates the filter with a measured distance in the zb direction using the + float h[STATE_DIM] = {0}; + arm_matrix_instance_f32 H = {1, STATE_DIM, h}; + + // Only update the filter if the measurement is reliable (\hat{h} -> infty when R[2][2] -> 0) + if (fabs(R[2][2]) > 0.1 && R[2][2] > 0){ + float angleOfApterure = 10 * DEG_TO_RAD; // Half aperture angle radians + float alpha = acosf(R[2][2]) - angleOfApterure; + if (alpha < 0.0f){ + alpha = 0.0f; + } + float predictedDistance = S[STATE_Z] / cosf(alpha); + float measuredDistance = tof->distance; // [m] + + //Measurement equation + // + // h = z/((R*z_b)\dot z_b) = z/cos(alpha) + h[STATE_Z] = 1 / cosf(alpha); + + // Scalar update + stateEstimatorScalarUpdate(&H, measuredDistance-predictedDistance, tof->stdDev); + } +} + static void stateEstimatorFinalize(sensorData_t *sensors, uint32_t tick) { // Matrix to rotate the attitude covariances once updated @@ -1107,12 +1146,14 @@ void stateEstimatorInit(void) { distDataQueue = xQueueCreate(DIST_QUEUE_LENGTH, sizeof(distanceMeasurement_t)); posDataQueue = xQueueCreate(POS_QUEUE_LENGTH, sizeof(positionMeasurement_t)); tdoaDataQueue = xQueueCreate(UWB_QUEUE_LENGTH, sizeof(tdoaMeasurement_t)); + tofDataQueue = xQueueCreate(TOF_QUEUE_LENGTH, sizeof(tofMeasurement_t)); } else { xQueueReset(distDataQueue); xQueueReset(posDataQueue); xQueueReset(tdoaDataQueue); + xQueueReset(tofDataQueue); } lastPrediction = xTaskGetTickCount(); @@ -1207,6 +1248,12 @@ bool stateEstimatorEnqueueDistance(distanceMeasurement_t *dist) return stateEstimatorEnqueueExternalMeasurement(distDataQueue, (void *)dist); } +bool stateEstimatorEnqueueTOF(tofMeasurement_t *tof) +{ + // A distance (distance) [m] to the ground along the z_B axis. + return stateEstimatorEnqueueExternalMeasurement(tofDataQueue, (void *)tof); +} + bool stateEstimatorTest(void) { // TODO: Figure out what we could test?