From b55d0ecb501bf6d9b1507dcf87cfa68c3c5fecfc Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Fri, 21 Apr 2017 10:37:07 +0200 Subject: [PATCH] #211 Fixed vl53 deck driver hijacking i2c buss. The vl53 deck driver used interrupt polling and a periodic timer to read out sensor data. The periodic timer was running faster then the vl53 sensor could provide values leaving the driver in a constant polling state. This had the consequence that the mem subsystem, having a lower priority, never got to read the eeprom on the same i2c buss. A relaxation delay was added to fix this. Also the sensor reading was changed from every 100ms to as fast as possible. --- src/config/config.h | 3 +++ src/deck/drivers/src/vl53l0x.c | 9 +++++++-- src/modules/src/position_estimator_altitude.c | 2 +- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/config/config.h b/src/config/config.h index da08a79df4..a7a0e9554b 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -82,6 +82,7 @@ #define CRTP_TX_TASK_PRI 2 #define CRTP_RX_TASK_PRI 2 #define EXTRX_TASK_PRI 2 +#define VL53_TASK_PRI 2 #define LOG_TASK_PRI 1 #define MEM_TASK_PRI 1 #define PARAM_TASK_PRI 1 @@ -125,6 +126,7 @@ #define PROXIMITY_TASK_NAME "PROXIMITY" #define EXTRX_TASK_NAME "EXTRX" #define UART_RX_TASK_NAME "UART" +#define VL53_TASK_NAME "VL53" //Task stack sizes #define SYSTEM_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE) @@ -145,6 +147,7 @@ #define PROXIMITY_TASK_STACKSIZE configMINIMAL_STACK_SIZE #define EXTRX_TASK_STACKSIZE configMINIMAL_STACK_SIZE #define UART_RX_TASK_STACKSIZE configMINIMAL_STACK_SIZE +#define VL53_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE) //The radio channel. From 0 to 125 #define RADIO_CHANNEL 80 diff --git a/src/deck/drivers/src/vl53l0x.c b/src/deck/drivers/src/vl53l0x.c index f551edcdf7..5814ede6b3 100644 --- a/src/deck/drivers/src/vl53l0x.c +++ b/src/deck/drivers/src/vl53l0x.c @@ -147,7 +147,7 @@ void vl53l0xInit(DeckInfo* info) i2cdevInit(I2C1_DEV); I2Cx = I2C1_DEV; devAddr = VL53L0X_DEFAULT_ADDRESS; - xTaskCreate(vl53l0xTask, "vl53l0x", 2*configMINIMAL_STACK_SIZE, NULL, 3, NULL); + xTaskCreate(vl53l0xTask, VL53_TASK_NAME, VL53_TASK_STACKSIZE, NULL, VL53_TASK_PRI, NULL); #if defined(ESTIMATOR_TYPE_kalman) && defined(UPDATE_KALMAN_WITH_RANGING) // pre-compute constant in the measurement noise mdoel @@ -177,7 +177,7 @@ void vl53l0xTask(void* arg) vl53l0xSetVcselPulsePeriod(VcselPeriodPreRange, 18); vl53l0xSetVcselPulsePeriod(VcselPeriodFinalRange, 14); - vl53l0xStartContinuous(100); + vl53l0xStartContinuous(0); while (1) { xLastWakeTime = xTaskGetTickCount(); range_last = vl53l0xReadRangeContinuousMillimeters(); @@ -912,6 +912,11 @@ uint16_t vl53l0xReadRangeContinuousMillimeters(void) while ((val & 0x07) == 0) { i2cdevReadByte(I2Cx, devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &val); + if ((val & 0x07) == 0) + { + // Relaxation delay when polling interrupt + vTaskDelay(M2T(1)); + } if (checkTimeoutExpired()) { did_timeout = true; diff --git a/src/modules/src/position_estimator_altitude.c b/src/modules/src/position_estimator_altitude.c index b905a70ec5..aabf12b8a8 100644 --- a/src/modules/src/position_estimator_altitude.c +++ b/src/modules/src/position_estimator_altitude.c @@ -45,7 +45,7 @@ struct selfState_s { static struct selfState_s state = { .estimatedZ = 0.0f, .velocityZ = 0.0f, - .estAlphaZrange = 0.93f, + .estAlphaZrange = 0.90f, .estAlphaAsl = 0.997f, .velocityFactor = 1.0f, .vAccDeadband = 0.04f,