From 76a92b22ad17600223b49f88c91adc1c6a4314d9 Mon Sep 17 00:00:00 2001
From: 488_MP-4 <488_MP-4@iastate.edu>
Date: Sun, 24 Mar 2024 22:27:35 +0100
Subject: [PATCH] commenting rest of files
---
.../src/drivers/src/nvic.c | 172 +-
.../src/drivers/src/pca9685.c | 75 +-
.../src/drivers/src/piezo.c | 118 +-
.../src/drivers/src/pmw3901.c | 306 ++--
.../src/drivers/src/swd.c | 179 +--
.../src/drivers/src/uart1.c | 165 +-
.../src/drivers/src/uart2.c | 325 ++--
.../src/drivers/src/uart_syslink.c | 635 ++++----
.../src/drivers/src/vl53l0x.c | 1386 +++++++++--------
.../src/drivers/src/vl53l1x.c | 281 ++--
.../src/drivers/src/watchdog.c | 63 +-
.../src/drivers/src/ws2812_cf2.c | 327 ++--
12 files changed, 2055 insertions(+), 1977 deletions(-)
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nvic.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nvic.c
index a5c777c0e..9cc83b178 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nvic.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nvic.c
@@ -36,7 +36,8 @@
void nvicInit(void)
{
- NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
+ //COMMENTED FIRMWARE
+ //NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
}
/**
@@ -85,81 +86,84 @@ void DONT_DISCARD HardFault_Handler(void)
// asm (" MOVS r0, #1 \n"
// " LDM r0,{r1-r2} \n"
// " BX LR; \n");
- asm( "TST LR, #4 \n"
- "ITE EQ \n"
- "MRSEQ R0, MSP \n"
- "MRSNE R0, PSP \n"
- "B printHardFault");
+ //COMMENTED FIRMWARE
+ // asm( "TST LR, #4 \n"
+ // "ITE EQ \n"
+ // "MRSEQ R0, MSP \n"
+ // "MRSNE R0, PSP \n"
+ // "B printHardFault");
}
void DONT_DISCARD printHardFault(uint32_t* hardfaultArgs)
{
- unsigned int stacked_r0;
- unsigned int stacked_r1;
- unsigned int stacked_r2;
- unsigned int stacked_r3;
- unsigned int stacked_r12;
- unsigned int stacked_lr;
- unsigned int stacked_pc;
- unsigned int stacked_psr;
-
- stacked_r0 = ((unsigned long) hardfaultArgs[0]);
- stacked_r1 = ((unsigned long) hardfaultArgs[1]);
- stacked_r2 = ((unsigned long) hardfaultArgs[2]);
- stacked_r3 = ((unsigned long) hardfaultArgs[3]);
-
- stacked_r12 = ((unsigned long) hardfaultArgs[4]);
- stacked_lr = ((unsigned long) hardfaultArgs[5]);
- stacked_pc = ((unsigned long) hardfaultArgs[6]);
- stacked_psr = ((unsigned long) hardfaultArgs[7]);
-
-
- UART_PRINT("[Hard fault handler]\n");
- UART_PRINT("R0 = %x\n", stacked_r0);
- UART_PRINT("R1 = %x\n", stacked_r1);
- UART_PRINT("R2 = %x\n", stacked_r2);
- UART_PRINT("R3 = %x\n", stacked_r3);
- UART_PRINT("R12 = %x\n", stacked_r12);
- UART_PRINT("LR = %x\n", stacked_lr);
- UART_PRINT("PC = %x\n", stacked_pc);
- UART_PRINT("PSR = %x\n", stacked_psr);
- UART_PRINT("BFAR = %x\n", (*((volatile unsigned int *)(0xE000ED38))));
- UART_PRINT("CFSR = %x\n", (*((volatile unsigned int *)(0xE000ED28))));
- UART_PRINT("HFSR = %x\n", (*((volatile unsigned int *)(0xE000ED2C))));
- UART_PRINT("DFSR = %x\n", (*((volatile unsigned int *)(0xE000ED30))));
- UART_PRINT("AFSR = %x\n", (*((volatile unsigned int *)(0xE000ED3C))));
-
- ledClearAll();
- ledSet(ERR_LED1, 1);
- ledSet(ERR_LED2, 1);
- powerStop();
-
- storeAssertHardfaultData(
- stacked_r0,
- stacked_r1,
- stacked_r2,
- stacked_r3,
- stacked_r12,
- stacked_lr,
- stacked_pc,
- stacked_psr);
- while (1)
- {}
+ //COMMENTED FIRMWARE
+ // unsigned int stacked_r0;
+ // unsigned int stacked_r1;
+ // unsigned int stacked_r2;
+ // unsigned int stacked_r3;
+ // unsigned int stacked_r12;
+ // unsigned int stacked_lr;
+ // unsigned int stacked_pc;
+ // unsigned int stacked_psr;
+
+ // stacked_r0 = ((unsigned long) hardfaultArgs[0]);
+ // stacked_r1 = ((unsigned long) hardfaultArgs[1]);
+ // stacked_r2 = ((unsigned long) hardfaultArgs[2]);
+ // stacked_r3 = ((unsigned long) hardfaultArgs[3]);
+
+ // stacked_r12 = ((unsigned long) hardfaultArgs[4]);
+ // stacked_lr = ((unsigned long) hardfaultArgs[5]);
+ // stacked_pc = ((unsigned long) hardfaultArgs[6]);
+ // stacked_psr = ((unsigned long) hardfaultArgs[7]);
+
+
+ // UART_PRINT("[Hard fault handler]\n");
+ // UART_PRINT("R0 = %x\n", stacked_r0);
+ // UART_PRINT("R1 = %x\n", stacked_r1);
+ // UART_PRINT("R2 = %x\n", stacked_r2);
+ // UART_PRINT("R3 = %x\n", stacked_r3);
+ // UART_PRINT("R12 = %x\n", stacked_r12);
+ // UART_PRINT("LR = %x\n", stacked_lr);
+ // UART_PRINT("PC = %x\n", stacked_pc);
+ // UART_PRINT("PSR = %x\n", stacked_psr);
+ // UART_PRINT("BFAR = %x\n", (*((volatile unsigned int *)(0xE000ED38))));
+ // UART_PRINT("CFSR = %x\n", (*((volatile unsigned int *)(0xE000ED28))));
+ // UART_PRINT("HFSR = %x\n", (*((volatile unsigned int *)(0xE000ED2C))));
+ // UART_PRINT("DFSR = %x\n", (*((volatile unsigned int *)(0xE000ED30))));
+ // UART_PRINT("AFSR = %x\n", (*((volatile unsigned int *)(0xE000ED3C))));
+
+ // ledClearAll();
+ // ledSet(ERR_LED1, 1);
+ // ledSet(ERR_LED2, 1);
+ // powerStop();
+
+ // storeAssertHardfaultData(
+ // stacked_r0,
+ // stacked_r1,
+ // stacked_r2,
+ // stacked_r3,
+ // stacked_r12,
+ // stacked_lr,
+ // stacked_pc,
+ // stacked_psr);
+ // while (1)
+ // {}
}
/**
* @brief This function handles Memory Manage exception.
*/
void DONT_DISCARD MemManage_Handler(void)
{
- /* Go to infinite loop when Memory Manage exception occurs */
- ledClearAll();
- ledSet(ERR_LED1, 1);
- ledSet(ERR_LED2, 1);
- powerStop();
-
- storeAssertTextData("MemManage");
- while (1)
- {}
+ //COMMENTED FIRMWARE
+ // /* Go to infinite loop when Memory Manage exception occurs */
+ // ledClearAll();
+ // ledSet(ERR_LED1, 1);
+ // ledSet(ERR_LED2, 1);
+ // powerStop();
+
+ // storeAssertTextData("MemManage");
+ // while (1)
+ // {}
}
/**
@@ -167,15 +171,16 @@ void DONT_DISCARD MemManage_Handler(void)
*/
void DONT_DISCARD BusFault_Handler(void)
{
+ //COMMENTED FIRMWARE
/* Go to infinite loop when Bus Fault exception occurs */
- ledClearAll();
- ledSet(ERR_LED1, 1);
- ledSet(ERR_LED2, 1);
- powerStop();
-
- storeAssertTextData("BusFault");
- while (1)
- {}
+ // ledClearAll();
+ // ledSet(ERR_LED1, 1);
+ // ledSet(ERR_LED2, 1);
+ // powerStop();
+
+ // storeAssertTextData("BusFault");
+ // while (1)
+ // {}
}
/**
@@ -183,15 +188,16 @@ void DONT_DISCARD BusFault_Handler(void)
*/
void DONT_DISCARD UsageFault_Handler(void)
{
- /* Go to infinite loop when Usage Fault exception occurs */
- ledClearAll();
- ledSet(ERR_LED1, 1);
- ledSet(ERR_LED2, 1);
- powerStop();
-
- storeAssertTextData("UsageFault");
- while (1)
- {}
+ //COMMENTED FIRMWARE
+ // /* Go to infinite loop when Usage Fault exception occurs */
+ // ledClearAll();
+ // ledSet(ERR_LED1, 1);
+ // ledSet(ERR_LED2, 1);
+ // powerStop();
+
+ // storeAssertTextData("UsageFault");
+ // while (1)
+ // {}
}
/**
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pca9685.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pca9685.c
index abab44e8d..92a19eeee 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pca9685.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pca9685.c
@@ -92,8 +92,9 @@ static void durationToBytes(uint16_t duration, uint8_t *bytes)
static uint16_t dutyToDuration(float duty)
{
- duty = fmax(duty, 0.0f);
- duty = fmin(duty, 1.0f);
+ //COMMENTED FIRMWARE
+ // duty = fmax(duty, 0.0f);
+ // duty = fmin(duty, 1.0f);
return duty * 4096.0f;
}
@@ -177,55 +178,59 @@ STATIC_MEM_QUEUE_ALLOC(queue, 1, sizeof(struct asyncRequest));
static void asyncTask(__attribute__((unused)) void *param)
{
- while (true) {
- BaseType_t ok = xQueueReceive(queue, &reqPop, portMAX_DELAY);
- if (ok == pdTRUE) {
- // blocking message send.
- pca9685setDurations(
- reqPop.addr, reqPop.chanBegin, reqPop.nChan, reqPop.durations);
- }
- }
+
+ // while (true) {
+ // BaseType_t ok = xQueueReceive(queue, &reqPop, portMAX_DELAY);
+ // if (ok == pdTRUE) {
+ // // blocking message send.
+ // pca9685setDurations(
+ // reqPop.addr, reqPop.chanBegin, reqPop.nChan, reqPop.durations);
+ // }
+ // }
}
bool pca9685startAsyncTask()
{
- queue = STATIC_MEM_QUEUE_CREATE(queue);
- if (queue == 0) {
- return false;
- }
+ //COMMENTED FIRMWARE
+ // queue = STATIC_MEM_QUEUE_CREATE(queue);
+ // if (queue == 0) {
+ // return false;
+ // }
- task = STATIC_MEM_TASK_CREATE(task, asyncTask, PCA9685_TASK_NAME, NULL, PCA9685_TASK_PRI);
+ // task = STATIC_MEM_TASK_CREATE(task, asyncTask, PCA9685_TASK_NAME, NULL, PCA9685_TASK_PRI);
return true;
}
bool pca9685setDutiesAsync(
int addr, int chanBegin, int nChan, float const *duties)
{
- struct asyncRequest reqPush = {
- .addr = addr,
- .chanBegin = chanBegin,
- .nChan = nChan,
- };
- for (int i = 0; i < nChan; ++i) {
- reqPush.durations[i] = dutyToDuration(duties[i]);
- }
- // drop message unless queue is empty!
- xQueueSend(queue, &reqPush, 0);
+ //COMMENTED FIRMWARE
+ // struct asyncRequest reqPush = {
+ // .addr = addr,
+ // .chanBegin = chanBegin,
+ // .nChan = nChan,
+ // };
+ // for (int i = 0; i < nChan; ++i) {
+ // reqPush.durations[i] = dutyToDuration(duties[i]);
+ // }
+ // // drop message unless queue is empty!
+ // xQueueSend(queue, &reqPush, 0);
return true;
}
bool pca9685setDurationsAsync(
int addr, int chanBegin, int nChan, uint16_t const *durations)
{
- struct asyncRequest reqPush = {
- .addr = addr,
- .chanBegin = chanBegin,
- .nChan = nChan,
- };
- for (int i = 0; i < nChan; ++i) {
- reqPush.durations[i] = durations[i];
- }
- // drop message unless queue is empty!
- xQueueSend(queue, &reqPush, 0);
+ //COMMENTED FIRMWARE
+ // struct asyncRequest reqPush = {
+ // .addr = addr,
+ // .chanBegin = chanBegin,
+ // .nChan = nChan,
+ // };
+ // for (int i = 0; i < nChan; ++i) {
+ // reqPush.durations[i] = durations[i];
+ // }
+ // // drop message unless queue is empty!
+ // xQueueSend(queue, &reqPush, 0);
return true;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/piezo.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/piezo.c
index e28ef7129..78acc40f6 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/piezo.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/piezo.c
@@ -71,61 +71,61 @@ void piezoInit()
{
if (isInit)
return;
-
- //Init structures
- GPIO_InitTypeDef GPIO_InitStructure;
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_OCInitTypeDef TIM_OCInitStructure;
-
- //Clock the gpio and the timers
- RCC_AHB1PeriphClockCmd(PIEZO_GPIO_POS_PERIF | PIEZO_GPIO_NEG_PERIF, ENABLE);
- RCC_APB1PeriphClockCmd(PIEZO_TIM_PERIF, ENABLE);
-
- // Configure the GPIO for the timer output
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
- GPIO_InitStructure.GPIO_Pin = PIEZO_GPIO_POS_PIN;
- GPIO_Init(PIEZO_GPIO_POS_PORT, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = PIEZO_GPIO_NEG_PIN;
- GPIO_Init(PIEZO_GPIO_NEG_PORT, &GPIO_InitStructure);
-
- //Map timers to alternate functions
- GPIO_PinAFConfig(PIEZO_GPIO_POS_PORT, PIEZO_GPIO_AF_POS_PIN, PIEZO_GPIO_AF_POS);
- GPIO_PinAFConfig(PIEZO_GPIO_NEG_PORT, PIEZO_GPIO_AF_NEG_PIN, PIEZO_GPIO_AF_NEG);
-
- //Timer configuration
- TIM_TimeBaseStructure.TIM_Period = PIEZO_PWM_PERIOD;
- TIM_TimeBaseStructure.TIM_Prescaler = PIEZO_PWM_PRESCALE;
- TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
- TIM_TimeBaseInit(PIEZO_TIM, &TIM_TimeBaseStructure);
-
- // PWM channels configuration
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
- TIM_OCInitStructure.TIM_Pulse = 0;
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
- TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
-
- // Configure OC3
- TIM_OC3Init(PIEZO_TIM, &TIM_OCInitStructure);
- TIM_OC3PreloadConfig(PIEZO_TIM, TIM_OCPreload_Enable);
-
- // Configure OC4 inverted
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
- TIM_OC4Init(PIEZO_TIM, &TIM_OCInitStructure);
- TIM_OC4PreloadConfig(PIEZO_TIM, TIM_OCPreload_Enable);
-
- //Enable the timer PWM outputs
- TIM_CtrlPWMOutputs(PIEZO_TIM, ENABLE);
- TIM_SetCompare3(PIEZO_TIM, 0x00);
- TIM_SetCompare4(PIEZO_TIM, 0x00);
-
- //Enable the timer
- TIM_Cmd(PIEZO_TIM, ENABLE);
+ //COMMENTED FIRMWARE
+ // //Init structures
+ // GPIO_InitTypeDef GPIO_InitStructure;
+ // TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ // TIM_OCInitTypeDef TIM_OCInitStructure;
+
+ // //Clock the gpio and the timers
+ // RCC_AHB1PeriphClockCmd(PIEZO_GPIO_POS_PERIF | PIEZO_GPIO_NEG_PERIF, ENABLE);
+ // RCC_APB1PeriphClockCmd(PIEZO_TIM_PERIF, ENABLE);
+
+ // // Configure the GPIO for the timer output
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+ // GPIO_InitStructure.GPIO_Pin = PIEZO_GPIO_POS_PIN;
+ // GPIO_Init(PIEZO_GPIO_POS_PORT, &GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Pin = PIEZO_GPIO_NEG_PIN;
+ // GPIO_Init(PIEZO_GPIO_NEG_PORT, &GPIO_InitStructure);
+
+ // //Map timers to alternate functions
+ // GPIO_PinAFConfig(PIEZO_GPIO_POS_PORT, PIEZO_GPIO_AF_POS_PIN, PIEZO_GPIO_AF_POS);
+ // GPIO_PinAFConfig(PIEZO_GPIO_NEG_PORT, PIEZO_GPIO_AF_NEG_PIN, PIEZO_GPIO_AF_NEG);
+
+ // //Timer configuration
+ // TIM_TimeBaseStructure.TIM_Period = PIEZO_PWM_PERIOD;
+ // TIM_TimeBaseStructure.TIM_Prescaler = PIEZO_PWM_PRESCALE;
+ // TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ // TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ // TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+ // TIM_TimeBaseInit(PIEZO_TIM, &TIM_TimeBaseStructure);
+
+ // // PWM channels configuration
+ // TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+ // TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+ // TIM_OCInitStructure.TIM_Pulse = 0;
+ // TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
+ // TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
+
+ // // Configure OC3
+ // TIM_OC3Init(PIEZO_TIM, &TIM_OCInitStructure);
+ // TIM_OC3PreloadConfig(PIEZO_TIM, TIM_OCPreload_Enable);
+
+ // // Configure OC4 inverted
+ // TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+ // TIM_OC4Init(PIEZO_TIM, &TIM_OCInitStructure);
+ // TIM_OC4PreloadConfig(PIEZO_TIM, TIM_OCPreload_Enable);
+
+ // //Enable the timer PWM outputs
+ // TIM_CtrlPWMOutputs(PIEZO_TIM, ENABLE);
+ // TIM_SetCompare3(PIEZO_TIM, 0x00);
+ // TIM_SetCompare4(PIEZO_TIM, 0x00);
+
+ // //Enable the timer
+ // TIM_Cmd(PIEZO_TIM, ENABLE);
isInit = true;
}
@@ -137,11 +137,13 @@ bool piezoTest(void)
void piezoSetRatio(uint8_t ratio)
{
- TIM_SetCompare3(PIEZO_TIM, ratio);
- TIM_SetCompare4(PIEZO_TIM, ratio);
+ //COMMENTED FIRMWARE
+ // TIM_SetCompare3(PIEZO_TIM, ratio);
+ // TIM_SetCompare4(PIEZO_TIM, ratio);
}
void piezoSetFreq(uint16_t freq)
{
- TIM_PrescalerConfig(PIEZO_TIM, (PIEZO_BASE_FREQ/freq), TIM_PSCReloadMode_Update);
+ //COMMENTED FIRMWARE
+ //TIM_PrescalerConfig(PIEZO_TIM, (PIEZO_BASE_FREQ/freq), TIM_PSCReloadMode_Update);
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pmw3901.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pmw3901.c
index 111af0eca..4777db247 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pmw3901.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pmw3901.c
@@ -41,133 +41,136 @@ static bool isInit = false;
static void registerWrite(const deckPin_t csPin, uint8_t reg, uint8_t value)
{
- // Set MSB to 1 for write
- reg |= 0x80u;
+ //COMMENTED FIRMWARE
+ // // Set MSB to 1 for write
+ // reg |= 0x80u;
- spiBeginTransaction(SPI_BAUDRATE_2MHZ);
- digitalWrite(csPin, LOW);
+ // spiBeginTransaction(SPI_BAUDRATE_2MHZ);
+ // digitalWrite(csPin, LOW);
- sleepus(50);
+ // sleepus(50);
- spiExchange(1, ®, ®);
- sleepus(50);
- spiExchange(1, &value, &value);
+ // spiExchange(1, ®, ®);
+ // sleepus(50);
+ // spiExchange(1, &value, &value);
- sleepus(50);
+ // sleepus(50);
- digitalWrite(csPin, HIGH);
- spiEndTransaction();
- sleepus(200);
+ // digitalWrite(csPin, HIGH);
+ // spiEndTransaction();
+ // sleepus(200);
}
static uint8_t registerRead(const deckPin_t csPin, uint8_t reg)
{
uint8_t data = 0;
- uint8_t dummy = 0;
+ //COMMENTED FIRMWARE
+ // uint8_t dummy = 0;
- // Set MSB to 0 for read
- reg &= ~0x80u;
+ // // Set MSB to 0 for read
+ // reg &= ~0x80u;
- spiBeginTransaction(SPI_BAUDRATE_2MHZ);
- digitalWrite(csPin, LOW);
+ // spiBeginTransaction(SPI_BAUDRATE_2MHZ);
+ // digitalWrite(csPin, LOW);
- sleepus(50);
+ // sleepus(50);
- spiExchange(1, ®, ®);
- sleepus(500);
- spiExchange(1, &dummy, &data);
+ // spiExchange(1, ®, ®);
+ // sleepus(500);
+ // spiExchange(1, &dummy, &data);
- sleepus(50);
+ // sleepus(50);
- digitalWrite(csPin, HIGH);
- spiEndTransaction();
- sleepus(200);
+ // digitalWrite(csPin, HIGH);
+ // spiEndTransaction();
+ // sleepus(200);
return data;
}
static void InitRegisters(const deckPin_t csPin)
{
- registerWrite(csPin, 0x7F, 0x00);
- registerWrite(csPin, 0x61, 0xAD);
- registerWrite(csPin, 0x7F, 0x03);
- registerWrite(csPin, 0x40, 0x00);
- registerWrite(csPin, 0x7F, 0x05);
- registerWrite(csPin, 0x41, 0xB3);
- registerWrite(csPin, 0x43, 0xF1);
- registerWrite(csPin, 0x45, 0x14);
- registerWrite(csPin, 0x5B, 0x32);
- registerWrite(csPin, 0x5F, 0x34);
- registerWrite(csPin, 0x7B, 0x08);
- registerWrite(csPin, 0x7F, 0x06);
- registerWrite(csPin, 0x44, 0x1B);
- registerWrite(csPin, 0x40, 0xBF);
- registerWrite(csPin, 0x4E, 0x3F);
- registerWrite(csPin, 0x7F, 0x08);
- registerWrite(csPin, 0x65, 0x20);
- registerWrite(csPin, 0x6A, 0x18);
- registerWrite(csPin, 0x7F, 0x09);
- registerWrite(csPin, 0x4F, 0xAF);
- registerWrite(csPin, 0x5F, 0x40);
- registerWrite(csPin, 0x48, 0x80);
- registerWrite(csPin, 0x49, 0x80);
- registerWrite(csPin, 0x57, 0x77);
- registerWrite(csPin, 0x60, 0x78);
- registerWrite(csPin, 0x61, 0x78);
- registerWrite(csPin, 0x62, 0x08);
- registerWrite(csPin, 0x63, 0x50);
- registerWrite(csPin, 0x7F, 0x0A);
- registerWrite(csPin, 0x45, 0x60);
- registerWrite(csPin, 0x7F, 0x00);
- registerWrite(csPin, 0x4D, 0x11);
- registerWrite(csPin, 0x55, 0x80);
- registerWrite(csPin, 0x74, 0x1F);
- registerWrite(csPin, 0x75, 0x1F);
- registerWrite(csPin, 0x4A, 0x78);
- registerWrite(csPin, 0x4B, 0x78);
- registerWrite(csPin, 0x44, 0x08);
- registerWrite(csPin, 0x45, 0x50);
- registerWrite(csPin, 0x64, 0xFF);
- registerWrite(csPin, 0x65, 0x1F);
- registerWrite(csPin, 0x7F, 0x14);
- registerWrite(csPin, 0x65, 0x67);
- registerWrite(csPin, 0x66, 0x08);
- registerWrite(csPin, 0x63, 0x70);
- registerWrite(csPin, 0x7F, 0x15);
- registerWrite(csPin, 0x48, 0x48);
- registerWrite(csPin, 0x7F, 0x07);
- registerWrite(csPin, 0x41, 0x0D);
- registerWrite(csPin, 0x43, 0x14);
- registerWrite(csPin, 0x4B, 0x0E);
- registerWrite(csPin, 0x45, 0x0F);
- registerWrite(csPin, 0x44, 0x42);
- registerWrite(csPin, 0x4C, 0x80);
- registerWrite(csPin, 0x7F, 0x10);
- registerWrite(csPin, 0x5B, 0x02);
- registerWrite(csPin, 0x7F, 0x07);
- registerWrite(csPin, 0x40, 0x41);
- registerWrite(csPin, 0x70, 0x00);
-
- vTaskDelay(M2T(10)); // delay 10ms
-
- registerWrite(csPin, 0x32, 0x44);
- registerWrite(csPin, 0x7F, 0x07);
- registerWrite(csPin, 0x40, 0x40);
- registerWrite(csPin, 0x7F, 0x06);
- registerWrite(csPin, 0x62, 0xF0);
- registerWrite(csPin, 0x63, 0x00);
- registerWrite(csPin, 0x7F, 0x0D);
- registerWrite(csPin, 0x48, 0xC0);
- registerWrite(csPin, 0x6F, 0xD5);
- registerWrite(csPin, 0x7F, 0x00);
- registerWrite(csPin, 0x5B, 0xA0);
- registerWrite(csPin, 0x4E, 0xA8);
- registerWrite(csPin, 0x5A, 0x50);
- registerWrite(csPin, 0x40, 0x80);
-
- registerWrite(csPin, 0x7F, 0x00);
- registerWrite(csPin, 0x5A, 0x10);
- registerWrite(csPin, 0x54, 0x00);
+ //COMMENTED FIRMWARE
+ // registerWrite(csPin, 0x7F, 0x00);
+ // registerWrite(csPin, 0x61, 0xAD);
+ // registerWrite(csPin, 0x7F, 0x03);
+ // registerWrite(csPin, 0x40, 0x00);
+ // registerWrite(csPin, 0x7F, 0x05);
+ // registerWrite(csPin, 0x41, 0xB3);
+ // registerWrite(csPin, 0x43, 0xF1);
+ // registerWrite(csPin, 0x45, 0x14);
+ // registerWrite(csPin, 0x5B, 0x32);
+ // registerWrite(csPin, 0x5F, 0x34);
+ // registerWrite(csPin, 0x7B, 0x08);
+ // registerWrite(csPin, 0x7F, 0x06);
+ // registerWrite(csPin, 0x44, 0x1B);
+ // registerWrite(csPin, 0x40, 0xBF);
+ // registerWrite(csPin, 0x4E, 0x3F);
+ // registerWrite(csPin, 0x7F, 0x08);
+ // registerWrite(csPin, 0x65, 0x20);
+ // registerWrite(csPin, 0x6A, 0x18);
+ // registerWrite(csPin, 0x7F, 0x09);
+ // registerWrite(csPin, 0x4F, 0xAF);
+ // registerWrite(csPin, 0x5F, 0x40);
+ // registerWrite(csPin, 0x48, 0x80);
+ // registerWrite(csPin, 0x49, 0x80);
+ // registerWrite(csPin, 0x57, 0x77);
+ // registerWrite(csPin, 0x60, 0x78);
+ // registerWrite(csPin, 0x61, 0x78);
+ // registerWrite(csPin, 0x62, 0x08);
+ // registerWrite(csPin, 0x63, 0x50);
+ // registerWrite(csPin, 0x7F, 0x0A);
+ // registerWrite(csPin, 0x45, 0x60);
+ // registerWrite(csPin, 0x7F, 0x00);
+ // registerWrite(csPin, 0x4D, 0x11);
+ // registerWrite(csPin, 0x55, 0x80);
+ // registerWrite(csPin, 0x74, 0x1F);
+ // registerWrite(csPin, 0x75, 0x1F);
+ // registerWrite(csPin, 0x4A, 0x78);
+ // registerWrite(csPin, 0x4B, 0x78);
+ // registerWrite(csPin, 0x44, 0x08);
+ // registerWrite(csPin, 0x45, 0x50);
+ // registerWrite(csPin, 0x64, 0xFF);
+ // registerWrite(csPin, 0x65, 0x1F);
+ // registerWrite(csPin, 0x7F, 0x14);
+ // registerWrite(csPin, 0x65, 0x67);
+ // registerWrite(csPin, 0x66, 0x08);
+ // registerWrite(csPin, 0x63, 0x70);
+ // registerWrite(csPin, 0x7F, 0x15);
+ // registerWrite(csPin, 0x48, 0x48);
+ // registerWrite(csPin, 0x7F, 0x07);
+ // registerWrite(csPin, 0x41, 0x0D);
+ // registerWrite(csPin, 0x43, 0x14);
+ // registerWrite(csPin, 0x4B, 0x0E);
+ // registerWrite(csPin, 0x45, 0x0F);
+ // registerWrite(csPin, 0x44, 0x42);
+ // registerWrite(csPin, 0x4C, 0x80);
+ // registerWrite(csPin, 0x7F, 0x10);
+ // registerWrite(csPin, 0x5B, 0x02);
+ // registerWrite(csPin, 0x7F, 0x07);
+ // registerWrite(csPin, 0x40, 0x41);
+ // registerWrite(csPin, 0x70, 0x00);
+
+ // vTaskDelay(M2T(10)); // delay 10ms
+
+ // registerWrite(csPin, 0x32, 0x44);
+ // registerWrite(csPin, 0x7F, 0x07);
+ // registerWrite(csPin, 0x40, 0x40);
+ // registerWrite(csPin, 0x7F, 0x06);
+ // registerWrite(csPin, 0x62, 0xF0);
+ // registerWrite(csPin, 0x63, 0x00);
+ // registerWrite(csPin, 0x7F, 0x0D);
+ // registerWrite(csPin, 0x48, 0xC0);
+ // registerWrite(csPin, 0x6F, 0xD5);
+ // registerWrite(csPin, 0x7F, 0x00);
+ // registerWrite(csPin, 0x5B, 0xA0);
+ // registerWrite(csPin, 0x4E, 0xA8);
+ // registerWrite(csPin, 0x5A, 0x50);
+ // registerWrite(csPin, 0x40, 0x80);
+
+ // registerWrite(csPin, 0x7F, 0x00);
+ // registerWrite(csPin, 0x5A, 0x10);
+ // registerWrite(csPin, 0x54, 0x00);
}
bool pmw3901Init(const deckPin_t csPin)
@@ -175,64 +178,65 @@ bool pmw3901Init(const deckPin_t csPin)
if (isInit) {
return true;
}
+ //COMMENTED FIRMWARE
+ // // Initialize CS Pin
+ // pinMode(csPin, OUTPUT);
+ // digitalWrite(csPin, HIGH);
- // Initialize CS Pin
- pinMode(csPin, OUTPUT);
- digitalWrite(csPin, HIGH);
+ // spiBegin();
+ // vTaskDelay(M2T(40));
- spiBegin();
- vTaskDelay(M2T(40));
+ // digitalWrite(csPin, HIGH);
+ // vTaskDelay(M2T(2));
+ // digitalWrite(csPin, LOW);
+ // vTaskDelay(M2T(2));
+ // digitalWrite(csPin, HIGH);
+ // vTaskDelay(M2T(2));
- digitalWrite(csPin, HIGH);
- vTaskDelay(M2T(2));
- digitalWrite(csPin, LOW);
- vTaskDelay(M2T(2));
- digitalWrite(csPin, HIGH);
- vTaskDelay(M2T(2));
+ // uint8_t chipId = registerRead(csPin, 0x00);
+ // uint8_t invChipId = registerRead(csPin, 0x5f);
- uint8_t chipId = registerRead(csPin, 0x00);
- uint8_t invChipId = registerRead(csPin, 0x5f);
+ // DEBUG_PRINT("Motion chip id: 0x%x:0x%x\n", chipId, invChipId);
- DEBUG_PRINT("Motion chip id: 0x%x:0x%x\n", chipId, invChipId);
+ // if (chipId == 0x49 && invChipId == 0xB6)
+ // {
+ // // Power on reset
+ // registerWrite(csPin, 0x3a, 0x5a);
+ // vTaskDelay(M2T(5));
- if (chipId == 0x49 && invChipId == 0xB6)
- {
- // Power on reset
- registerWrite(csPin, 0x3a, 0x5a);
- vTaskDelay(M2T(5));
+ // // Reading the motion registers one time
+ // registerRead(csPin, 0x02);
+ // registerRead(csPin, 0x03);
+ // registerRead(csPin, 0x04);
+ // registerRead(csPin, 0x05);
+ // registerRead(csPin, 0x06);
+ // vTaskDelay(M2T(1));
- // Reading the motion registers one time
- registerRead(csPin, 0x02);
- registerRead(csPin, 0x03);
- registerRead(csPin, 0x04);
- registerRead(csPin, 0x05);
- registerRead(csPin, 0x06);
- vTaskDelay(M2T(1));
+ // InitRegisters(csPin);
- InitRegisters(csPin);
-
- isInit = true;
- }
+ // isInit = true;
+ // }
return isInit;
}
void pmw3901ReadMotion(const deckPin_t csPin, motionBurst_t * motion)
{
- uint8_t address = 0x16;
-
- spiBeginTransaction(SPI_BAUDRATE_2MHZ);
- digitalWrite(csPin,LOW);
- sleepus(50);
- spiExchange(1, &address, &address);
- sleepus(50);
- spiExchange(sizeof(motionBurst_t), (uint8_t*)motion, (uint8_t*)motion);
- sleepus(50);
- digitalWrite(csPin, HIGH);
- spiEndTransaction();
- sleepus(50);
-
- uint16_t realShutter = (motion->shutter >> 8) & 0x0FF;
- realShutter |= (motion->shutter & 0x0ff) << 8;
- motion->shutter = realShutter;
+ //COMMENTED FIRMWARE
+ // uint8_t address = 0x16;
+
+ // spiBeginTransaction(SPI_BAUDRATE_2MHZ);
+ // digitalWrite(csPin,LOW);
+ // sleepus(50);
+ // spiExchange(1, &address, &address);
+ // sleepus(50);
+ // spiExchange(sizeof(motionBurst_t), (uint8_t*)motion, (uint8_t*)motion);
+ // sleepus(50);
+ // digitalWrite(csPin, HIGH);
+ // spiEndTransaction();
+ // sleepus(50);
+
+ // uint16_t realShutter = (motion->shutter >> 8) & 0x0FF;
+ // realShutter |= (motion->shutter & 0x0ff) << 8;
+ // motion->shutter = realShutter;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/swd.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/swd.c
index bccba0424..a553065d7 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/swd.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/swd.c
@@ -53,49 +53,50 @@ static bool isInit = false;
static void initGPIO(void) {
- GPIO_InitTypeDef GPIO_InitStructure;
+ //COMMENTED FIRMWARE
+ // GPIO_InitTypeDef GPIO_InitStructure;
- SWD_SPI_CLK_INIT(SWD_SPI_CLK, ENABLE);
+ // SWD_SPI_CLK_INIT(SWD_SPI_CLK, ENABLE);
- RCC_AHB1PeriphClockCmd(SWD_SPI_SCK_GPIO_CLK | SWD_SPI_MISO_GPIO_CLK, ENABLE);
+ // RCC_AHB1PeriphClockCmd(SWD_SPI_SCK_GPIO_CLK | SWD_SPI_MISO_GPIO_CLK, ENABLE);
- GPIO_PinAFConfig(SWD_SPI_SCK_GPIO_PORT, SWD_SPI_SCK_SOURCE, SWD_SPI_SCK_AF);
- GPIO_PinAFConfig(SWD_SPI_MISO_GPIO_PORT, SWD_SPI_MISO_SOURCE, SWD_SPI_MISO_AF);
+ // GPIO_PinAFConfig(SWD_SPI_SCK_GPIO_PORT, SWD_SPI_SCK_SOURCE, SWD_SPI_SCK_AF);
+ // GPIO_PinAFConfig(SWD_SPI_MISO_GPIO_PORT, SWD_SPI_MISO_SOURCE, SWD_SPI_MISO_AF);
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
- GPIO_InitStructure.GPIO_Pin = SWD_SPI_SCK_PIN;
- GPIO_Init(SWD_SPI_SCK_GPIO_PORT, &GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Pin = SWD_SPI_SCK_PIN;
+ // GPIO_Init(SWD_SPI_SCK_GPIO_PORT, &GPIO_InitStructure);
- /* In bi-directional mode only MISO is used */
- GPIO_InitStructure.GPIO_Pin = SWD_SPI_MISO_PIN;
- GPIO_Init(SWD_SPI_MISO_GPIO_PORT, &GPIO_InitStructure);
+ // /* In bi-directional mode only MISO is used */
+ // GPIO_InitStructure.GPIO_Pin = SWD_SPI_MISO_PIN;
+ // GPIO_Init(SWD_SPI_MISO_GPIO_PORT, &GPIO_InitStructure);
}
static void initSPI(void) {
+ //COMMENTED FIRMWARE
+ // SPI_InitTypeDef SPI_InitStructure;
- SPI_InitTypeDef SPI_InitStructure;
+ // initGPIO();
- initGPIO();
+ // /* Set up SPI in bi-directional mode */
+ // SPI_InitStructure.SPI_Direction = SPI_Direction_1Line_Tx;
+ // SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
+ // SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
+ // SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
+ // SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
+ // SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
+ // SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256;
- /* Set up SPI in bi-directional mode */
- SPI_InitStructure.SPI_Direction = SPI_Direction_1Line_Tx;
- SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
- SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
- SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
- SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
- SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
- SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256;
+ // SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
+ // SPI_InitStructure.SPI_CRCPolynomial = 7;
+ // SPI_Init(SWD_SPI, &SPI_InitStructure);
- SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
- SPI_InitStructure.SPI_CRCPolynomial = 7;
- SPI_Init(SWD_SPI, &SPI_InitStructure);
-
- SPI_Cmd(SWD_SPI, ENABLE);
+ // SPI_Cmd(SWD_SPI, ENABLE);
}
//Initialize the green led pin as output
@@ -103,8 +104,8 @@ void swdInit()
{
if(isInit)
return;
-
- initSPI();
+ //COMMENTED FIRMWARE
+ // initSPI();
isInit = true;
}
@@ -141,64 +142,64 @@ bool swdTest(void) {
// Should read out (and does):0x0BB11477 (according to OpenOCD)
//T ACK |------------DATA--------------|P
//? 100 111011100010100010001101110100001
-
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
-#if 0
- tx_data[idx++] = 0x9E;
- tx_data[idx++] = 0xE7;
-#else
- tx_data[idx++] = 0x79;
- tx_data[idx++] = 0xE7;
-#endif
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFF;
- tx_data[idx++] = 0xFC;
- tx_data[idx++] = header;
- // Rest is 0x00
-
- /* As long as the SPI is enabled in bi-di mode the clock is enabled. So
- * we write data as normal, but when reading there's no need to output any
- * data to trigger the clock. Direction needs to be set accordingly.
- */
-
- while(number_of_txd_bytes < transfer_size)
- {
- if (number_of_txd_bytes < idx)
- {
- SPI_BiDirectionalLineConfig(SWD_SPI, SPI_Direction_Tx);
-
- /*!< Loop while DR register in not emplty */
- while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_TXE) == RESET);
-
- SPI_I2S_SendData(SWD_SPI, tx_data[number_of_txd_bytes]);
-
- // Make sure that we have sent data in case next loop will be RX, the
- // mode is switched before we manage to send the data!
- while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_BSY) == SET);
- }
- else
- {
- SPI_BiDirectionalLineConfig(SWD_SPI, SPI_Direction_Rx);
- while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_RXNE) == RESET);
-
- /*!< Return the byte read from the SPI bus */
- rx_data[number_of_txd_bytes] = (uint8_t) SPI_I2S_ReceiveData(SWD_SPI);
- }
- number_of_txd_bytes++;
- }
- SPI_Cmd(SWD_SPI, DISABLE);
+//COMMENTED FIRMWARE
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// #if 0
+// tx_data[idx++] = 0x9E;
+// tx_data[idx++] = 0xE7;
+// #else
+// tx_data[idx++] = 0x79;
+// tx_data[idx++] = 0xE7;
+// #endif
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFF;
+// tx_data[idx++] = 0xFC;
+// tx_data[idx++] = header;
+// // Rest is 0x00
+
+// /* As long as the SPI is enabled in bi-di mode the clock is enabled. So
+// * we write data as normal, but when reading there's no need to output any
+// * data to trigger the clock. Direction needs to be set accordingly.
+// */
+
+// while(number_of_txd_bytes < transfer_size)
+// {
+// if (number_of_txd_bytes < idx)
+// {
+// SPI_BiDirectionalLineConfig(SWD_SPI, SPI_Direction_Tx);
+
+// /*!< Loop while DR register in not emplty */
+// while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_TXE) == RESET);
+
+// SPI_I2S_SendData(SWD_SPI, tx_data[number_of_txd_bytes]);
+
+// // Make sure that we have sent data in case next loop will be RX, the
+// // mode is switched before we manage to send the data!
+// while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_BSY) == SET);
+// }
+// else
+// {
+// SPI_BiDirectionalLineConfig(SWD_SPI, SPI_Direction_Rx);
+// while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_RXNE) == RESET);
+
+// /*!< Return the byte read from the SPI bus */
+// rx_data[number_of_txd_bytes] = (uint8_t) SPI_I2S_ReceiveData(SWD_SPI);
+// }
+// number_of_txd_bytes++;
+// }
+// SPI_Cmd(SWD_SPI, DISABLE);
return isInit;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart1.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart1.c
index 213cce235..dabca42bb 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart1.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart1.c
@@ -109,74 +109,76 @@ static void uart1DmaInit(void)
}
void uart1Init(const uint32_t baudrate) {
- uart1InitWithParity(baudrate, uart1ParityNone);
+ //COMMENTED FIRMWARE
+ //uart1InitWithParity(baudrate, uart1ParityNone);
}
void uart1InitWithParity(const uint32_t baudrate, const uart1Parity_t parity)
{
+ //COMMENTED FIRMWARE
- USART_InitTypeDef USART_InitStructure;
- GPIO_InitTypeDef GPIO_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
+ // USART_InitTypeDef USART_InitStructure;
+ // GPIO_InitTypeDef GPIO_InitStructure;
+ // NVIC_InitTypeDef NVIC_InitStructure;
- /* Enable GPIO and USART clock */
- RCC_AHB1PeriphClockCmd(UART1_GPIO_PERIF, ENABLE);
- ENABLE_UART1_RCC(UART1_PERIF, ENABLE);
-
- /* Configure USART Rx as input floating */
- GPIO_InitStructure.GPIO_Pin = UART1_GPIO_RX_PIN;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
- GPIO_Init(UART1_GPIO_PORT, &GPIO_InitStructure);
-
- /* Configure USART Tx as alternate function */
- GPIO_InitStructure.GPIO_Pin = UART1_GPIO_TX_PIN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_Init(UART1_GPIO_PORT, &GPIO_InitStructure);
-
- //Map uart to alternate functions
- GPIO_PinAFConfig(UART1_GPIO_PORT, UART1_GPIO_AF_TX_PIN, UART1_GPIO_AF_TX);
- GPIO_PinAFConfig(UART1_GPIO_PORT, UART1_GPIO_AF_RX_PIN, UART1_GPIO_AF_RX);
-
- USART_InitStructure.USART_BaudRate = baudrate;
- USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
- if (parity == uart1ParityEven || parity == uart1ParityOdd) {
- USART_InitStructure.USART_WordLength = USART_WordLength_9b;
- } else {
- USART_InitStructure.USART_WordLength = USART_WordLength_8b;
- }
+ // /* Enable GPIO and USART clock */
+ // RCC_AHB1PeriphClockCmd(UART1_GPIO_PERIF, ENABLE);
+ // ENABLE_UART1_RCC(UART1_PERIF, ENABLE);
- USART_InitStructure.USART_StopBits = USART_StopBits_1;
+ // /* Configure USART Rx as input floating */
+ // GPIO_InitStructure.GPIO_Pin = UART1_GPIO_RX_PIN;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+ // GPIO_Init(UART1_GPIO_PORT, &GPIO_InitStructure);
- if (parity == uart1ParityEven) {
- USART_InitStructure.USART_Parity = USART_Parity_Even;
- } else if (parity == uart1ParityOdd) {
- USART_InitStructure.USART_Parity = USART_Parity_Odd;
- } else {
- USART_InitStructure.USART_Parity = USART_Parity_No;
- }
+ // /* Configure USART Tx as alternate function */
+ // GPIO_InitStructure.GPIO_Pin = UART1_GPIO_TX_PIN;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ // GPIO_Init(UART1_GPIO_PORT, &GPIO_InitStructure);
- USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
- USART_Init(UART1_TYPE, &USART_InitStructure);
+ // //Map uart to alternate functions
+ // GPIO_PinAFConfig(UART1_GPIO_PORT, UART1_GPIO_AF_TX_PIN, UART1_GPIO_AF_TX);
+ // GPIO_PinAFConfig(UART1_GPIO_PORT, UART1_GPIO_AF_RX_PIN, UART1_GPIO_AF_RX);
- uart1DmaInit();
+ // USART_InitStructure.USART_BaudRate = baudrate;
+ // USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+ // if (parity == uart1ParityEven || parity == uart1ParityOdd) {
+ // USART_InitStructure.USART_WordLength = USART_WordLength_9b;
+ // } else {
+ // USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+ // }
- NVIC_InitStructure.NVIC_IRQChannel = UART1_IRQ;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
+ // USART_InitStructure.USART_StopBits = USART_StopBits_1;
+
+ // if (parity == uart1ParityEven) {
+ // USART_InitStructure.USART_Parity = USART_Parity_Even;
+ // } else if (parity == uart1ParityOdd) {
+ // USART_InitStructure.USART_Parity = USART_Parity_Odd;
+ // } else {
+ // USART_InitStructure.USART_Parity = USART_Parity_No;
+ // }
+
+ // USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+ // USART_Init(UART1_TYPE, &USART_InitStructure);
- uart1queue = STATIC_MEM_QUEUE_CREATE(uart1queue);
+ // uart1DmaInit();
- USART_ITConfig(UART1_TYPE, USART_IT_RXNE, ENABLE);
+ // NVIC_InitStructure.NVIC_IRQChannel = UART1_IRQ;
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ // NVIC_Init(&NVIC_InitStructure);
- //Enable UART
- USART_Cmd(UART1_TYPE, ENABLE);
+ // uart1queue = STATIC_MEM_QUEUE_CREATE(uart1queue);
- USART_ITConfig(UART1_TYPE, USART_IT_RXNE, ENABLE);
+ // USART_ITConfig(UART1_TYPE, USART_IT_RXNE, ENABLE);
+
+ // //Enable UART
+ // USART_Cmd(UART1_TYPE, ENABLE);
+
+ // USART_ITConfig(UART1_TYPE, USART_IT_RXNE, ENABLE);
isInit = true;
}
@@ -204,16 +206,17 @@ bool uart1GetDataWithDefaultTimeout(uint8_t *c)
void uart1SendData(uint32_t size, uint8_t* data)
{
- uint32_t i;
+ //COMMENTED FIRMWARE
+ // uint32_t i;
- if (!isInit)
- return;
+ // if (!isInit)
+ // return;
- for(i = 0; i < size; i++)
- {
- while (!(UART1_TYPE->SR & USART_FLAG_TXE));
- UART1_TYPE->DR = (data[i] & 0x00FF);
- }
+ // for(i = 0; i < size; i++)
+ // {
+ // while (!(UART1_TYPE->SR & USART_FLAG_TXE));
+ // UART1_TYPE->DR = (data[i] & 0x00FF);
+ // }
}
#ifdef ENABLE_UART1_DMA
@@ -246,7 +249,8 @@ void uart1SendDataDmaBlocking(uint32_t size, uint8_t* data)
int uart1Putchar(int ch)
{
- uart1SendData(1, (uint8_t *)&ch);
+ //COMMENTED FIRMWARE
+ //uart1SendData(1, (uint8_t *)&ch);
return (unsigned char)ch;
}
@@ -281,22 +285,23 @@ void __attribute__((used)) DMA1_Stream3_IRQHandler(void)
void __attribute__((used)) USART3_IRQHandler(void)
{
- uint8_t rxData;
- portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-
- if (USART_GetITStatus(UART1_TYPE, USART_IT_RXNE))
- {
- rxData = USART_ReceiveData(UART1_TYPE) & 0x00FF;
- xQueueSendFromISR(uart1queue, &rxData, &xHigherPriorityTaskWoken);
- } else {
- /** if we get here, the error is most likely caused by an overrun!
- * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
- * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
- * - reading USART_SR register followed reading the USART_DR register.
- */
- asm volatile ("" : "=m" (UART1_TYPE->SR) : "r" (UART1_TYPE->SR)); // force non-optimizable reads
- asm volatile ("" : "=m" (UART1_TYPE->DR) : "r" (UART1_TYPE->DR)); // of these two registers
-
- hasOverrun = true;
- }
+ //COMMENTED FIRMWARE
+ // uint8_t rxData;
+ // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+
+ // if (USART_GetITStatus(UART1_TYPE, USART_IT_RXNE))
+ // {
+ // rxData = USART_ReceiveData(UART1_TYPE) & 0x00FF;
+ // xQueueSendFromISR(uart1queue, &rxData, &xHigherPriorityTaskWoken);
+ // } else {
+ // /** if we get here, the error is most likely caused by an overrun!
+ // * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
+ // * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
+ // * - reading USART_SR register followed reading the USART_DR register.
+ // */
+ // asm volatile ("" : "=m" (UART1_TYPE->SR) : "r" (UART1_TYPE->SR)); // force non-optimizable reads
+ // asm volatile ("" : "=m" (UART1_TYPE->DR) : "r" (UART1_TYPE->DR)); // of these two registers
+
+ // hasOverrun = true;
+ // }
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart2.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart2.c
index 53419a3ae..3eca14c49 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart2.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart2.c
@@ -83,111 +83,113 @@ static bool hasOverrun = false;
*/
static void uart2DmaInit(void)
{
- NVIC_InitTypeDef NVIC_InitStructure;
-
- RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
-
- // USART TX DMA Channel Config
- DMA_InitStructureShare.DMA_PeripheralBaseAddr = (uint32_t)&UART2_TYPE->DR;
- DMA_InitStructureShare.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
- DMA_InitStructureShare.DMA_MemoryInc = DMA_MemoryInc_Enable;
- DMA_InitStructureShare.DMA_MemoryBurst = DMA_MemoryBurst_Single;
- DMA_InitStructureShare.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
- DMA_InitStructureShare.DMA_BufferSize = 0;
- DMA_InitStructureShare.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- DMA_InitStructureShare.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
- DMA_InitStructureShare.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
- DMA_InitStructureShare.DMA_DIR = DMA_DIR_MemoryToPeripheral;
- DMA_InitStructureShare.DMA_Mode = DMA_Mode_Normal;
- DMA_InitStructureShare.DMA_FIFOMode = DMA_FIFOMode_Disable;
- DMA_InitStructureShare.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
- DMA_InitStructureShare.DMA_Channel = UART2_DMA_CH;
- #ifdef UART2_LINK_COMM
- DMA_InitStructureShare.DMA_Priority = DMA_Priority_High;
- #else
- DMA_InitStructureShare.DMA_Priority = DMA_Priority_Low;
- #endif
-
- NVIC_InitStructure.NVIC_IRQChannel = UART2_DMA_IRQ;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- #ifdef UART2_LINK_COMM
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
- #else
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
- #endif
- NVIC_Init(&NVIC_InitStructure);
-
- isUartDmaInitialized = true;
+ //COMMENTED FIRMWARE
+ // NVIC_InitTypeDef NVIC_InitStructure;
+
+ // RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
+
+ // // USART TX DMA Channel Config
+ // DMA_InitStructureShare.DMA_PeripheralBaseAddr = (uint32_t)&UART2_TYPE->DR;
+ // DMA_InitStructureShare.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
+ // DMA_InitStructureShare.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ // DMA_InitStructureShare.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+ // DMA_InitStructureShare.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ // DMA_InitStructureShare.DMA_BufferSize = 0;
+ // DMA_InitStructureShare.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ // DMA_InitStructureShare.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+ // DMA_InitStructureShare.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+ // DMA_InitStructureShare.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+ // DMA_InitStructureShare.DMA_Mode = DMA_Mode_Normal;
+ // DMA_InitStructureShare.DMA_FIFOMode = DMA_FIFOMode_Disable;
+ // DMA_InitStructureShare.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
+ // DMA_InitStructureShare.DMA_Channel = UART2_DMA_CH;
+ // #ifdef UART2_LINK_COMM
+ // DMA_InitStructureShare.DMA_Priority = DMA_Priority_High;
+ // #else
+ // DMA_InitStructureShare.DMA_Priority = DMA_Priority_Low;
+ // #endif
+
+ // NVIC_InitStructure.NVIC_IRQChannel = UART2_DMA_IRQ;
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ // #ifdef UART2_LINK_COMM
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
+ // #else
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
+ // #endif
+ // NVIC_Init(&NVIC_InitStructure);
+
+ // isUartDmaInitialized = true;
}
void uart2Init(const uint32_t baudrate)
{
-
- USART_InitTypeDef USART_InitStructure;
- GPIO_InitTypeDef GPIO_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
-
- // initialize the FreeRTOS structures first, to prevent null pointers in interrupts
- waitUntilSendDone = xSemaphoreCreateBinaryStatic(&waitUntilSendDoneBuffer); // initialized as blocking
- uartBusy = xSemaphoreCreateBinaryStatic(&uartBusyBuffer); // initialized as blocking
- xSemaphoreGive(uartBusy); // but we give it because the uart isn't busy at initialization
-
- /* Enable GPIO and USART clock */
- RCC_AHB1PeriphClockCmd(UART2_GPIO_PERIF, ENABLE);
- ENABLE_UART2_RCC(UART2_PERIF, ENABLE);
-
- /* Configure USART Rx as input floating */
- GPIO_InitStructure.GPIO_Pin = UART2_GPIO_RX_PIN;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
- GPIO_Init(UART2_GPIO_PORT, &GPIO_InitStructure);
-
- /* Configure USART Tx as alternate function */
- GPIO_InitStructure.GPIO_Pin = UART2_GPIO_TX_PIN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_Init(UART2_GPIO_PORT, &GPIO_InitStructure);
-
- //Map uart to alternate functions
- GPIO_PinAFConfig(UART2_GPIO_PORT, UART2_GPIO_AF_TX_PIN, UART2_GPIO_AF_TX);
- GPIO_PinAFConfig(UART2_GPIO_PORT, UART2_GPIO_AF_RX_PIN, UART2_GPIO_AF_RX);
-
- USART_InitStructure.USART_BaudRate = baudrate;
- USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
- USART_InitStructure.USART_WordLength = USART_WordLength_8b;
- USART_InitStructure.USART_StopBits = USART_StopBits_1;
- USART_InitStructure.USART_Parity = USART_Parity_No ;
- USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
- USART_Init(UART2_TYPE, &USART_InitStructure);
-
- uart2DmaInit();
-
- // Configure Rx buffer not empty interrupt
- NVIC_InitStructure.NVIC_IRQChannel = UART2_IRQ;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- #ifdef UART2_LINK_COMM
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SYSLINK_PRI;
- #else
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
- #endif
- NVIC_Init(&NVIC_InitStructure);
-
- #ifdef UART2_LINK_COMM
- uart2PacketDelivery = STATIC_MEM_QUEUE_CREATE(uart2PacketDelivery);
- DEBUG_QUEUE_MONITOR_REGISTER(uart2PacketDelivery);
- #else
- uart2queue = STATIC_MEM_QUEUE_CREATE(uart2queue);
- #endif
-
- USART_ITConfig(UART2_TYPE, USART_IT_RXNE, ENABLE);
-
- //Enable UART
- USART_Cmd(UART2_TYPE, ENABLE);
-
- USART_ITConfig(UART2_TYPE, USART_IT_RXNE, ENABLE);
+ //COMMENTED FIRMWARE
+
+ // USART_InitTypeDef USART_InitStructure;
+ // GPIO_InitTypeDef GPIO_InitStructure;
+ // NVIC_InitTypeDef NVIC_InitStructure;
+
+ // // initialize the FreeRTOS structures first, to prevent null pointers in interrupts
+ // waitUntilSendDone = xSemaphoreCreateBinaryStatic(&waitUntilSendDoneBuffer); // initialized as blocking
+ // uartBusy = xSemaphoreCreateBinaryStatic(&uartBusyBuffer); // initialized as blocking
+ // xSemaphoreGive(uartBusy); // but we give it because the uart isn't busy at initialization
+
+ // /* Enable GPIO and USART clock */
+ // RCC_AHB1PeriphClockCmd(UART2_GPIO_PERIF, ENABLE);
+ // ENABLE_UART2_RCC(UART2_PERIF, ENABLE);
+
+ // /* Configure USART Rx as input floating */
+ // GPIO_InitStructure.GPIO_Pin = UART2_GPIO_RX_PIN;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+ // GPIO_Init(UART2_GPIO_PORT, &GPIO_InitStructure);
+
+ // /* Configure USART Tx as alternate function */
+ // GPIO_InitStructure.GPIO_Pin = UART2_GPIO_TX_PIN;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ // GPIO_Init(UART2_GPIO_PORT, &GPIO_InitStructure);
+
+ // //Map uart to alternate functions
+ // GPIO_PinAFConfig(UART2_GPIO_PORT, UART2_GPIO_AF_TX_PIN, UART2_GPIO_AF_TX);
+ // GPIO_PinAFConfig(UART2_GPIO_PORT, UART2_GPIO_AF_RX_PIN, UART2_GPIO_AF_RX);
+
+ // USART_InitStructure.USART_BaudRate = baudrate;
+ // USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+ // USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+ // USART_InitStructure.USART_StopBits = USART_StopBits_1;
+ // USART_InitStructure.USART_Parity = USART_Parity_No ;
+ // USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+ // USART_Init(UART2_TYPE, &USART_InitStructure);
+
+ // uart2DmaInit();
+
+ // // Configure Rx buffer not empty interrupt
+ // NVIC_InitStructure.NVIC_IRQChannel = UART2_IRQ;
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ // #ifdef UART2_LINK_COMM
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SYSLINK_PRI;
+ // #else
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
+ // #endif
+ // NVIC_Init(&NVIC_InitStructure);
+
+ // #ifdef UART2_LINK_COMM
+ // uart2PacketDelivery = STATIC_MEM_QUEUE_CREATE(uart2PacketDelivery);
+ // DEBUG_QUEUE_MONITOR_REGISTER(uart2PacketDelivery);
+ // #else
+ // uart2queue = STATIC_MEM_QUEUE_CREATE(uart2queue);
+ // #endif
+
+ // USART_ITConfig(UART2_TYPE, USART_IT_RXNE, ENABLE);
+
+ // //Enable UART
+ // USART_Cmd(UART2_TYPE, ENABLE);
+
+ // USART_ITConfig(UART2_TYPE, USART_IT_RXNE, ENABLE);
isInit = true;
}
@@ -199,47 +201,50 @@ bool uart2Test(void)
void uart2SendData(uint32_t size, uint8_t* data)
{
- uint32_t i;
+ //COMMENTED FIRMWARE
+ // uint32_t i;
- if (!isInit)
- return;
+ // if (!isInit)
+ // return;
- for(i = 0; i < size; i++)
- {
- while (!(UART2_TYPE->SR & USART_FLAG_TXE));
- UART2_TYPE->DR = (data[i] & 0x00FF);
- }
+ // for(i = 0; i < size; i++)
+ // {
+ // while (!(UART2_TYPE->SR & USART_FLAG_TXE));
+ // UART2_TYPE->DR = (data[i] & 0x00FF);
+ // }
}
void uart2SendDataDmaBlocking(uint32_t size, uint8_t* data)
{
- if (isUartDmaInitialized)
- {
- xSemaphoreTake(uartBusy, portMAX_DELAY);
- // Wait for DMA to be free
- while(DMA_GetCmdStatus(UART2_DMA_STREAM) != DISABLE);
- //Copy data in DMA buffer
- memcpy(dmaBuffer, data, size);
- DMA_InitStructureShare.DMA_BufferSize = size;
- initialDMACount = size;
- // Init new DMA stream
- DMA_Init(UART2_DMA_STREAM, &DMA_InitStructureShare);
- // Enable the Transfer Complete interrupt
- DMA_ITConfig(UART2_DMA_STREAM, DMA_IT_TC, ENABLE);
- /* Enable USART DMA TX Requests */
- USART_DMACmd(UART2_TYPE, USART_DMAReq_Tx, ENABLE);
- /* Clear transfer complete */
- USART_ClearFlag(UART2_TYPE, USART_FLAG_TC);
- /* Enable DMA USART TX Stream */
- DMA_Cmd(UART2_DMA_STREAM, ENABLE);
- xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
- xSemaphoreGive(uartBusy);
- }
+ //COMMENTED FIRMWARE
+ // if (isUartDmaInitialized)
+ // {
+ // xSemaphoreTake(uartBusy, portMAX_DELAY);
+ // // Wait for DMA to be free
+ // while(DMA_GetCmdStatus(UART2_DMA_STREAM) != DISABLE);
+ // //Copy data in DMA buffer
+ // memcpy(dmaBuffer, data, size);
+ // DMA_InitStructureShare.DMA_BufferSize = size;
+ // initialDMACount = size;
+ // // Init new DMA stream
+ // DMA_Init(UART2_DMA_STREAM, &DMA_InitStructureShare);
+ // // Enable the Transfer Complete interrupt
+ // DMA_ITConfig(UART2_DMA_STREAM, DMA_IT_TC, ENABLE);
+ // /* Enable USART DMA TX Requests */
+ // USART_DMACmd(UART2_TYPE, USART_DMAReq_Tx, ENABLE);
+ // /* Clear transfer complete */
+ // USART_ClearFlag(UART2_TYPE, USART_FLAG_TC);
+ // /* Enable DMA USART TX Stream */
+ // DMA_Cmd(UART2_DMA_STREAM, ENABLE);
+ // xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
+ // xSemaphoreGive(uartBusy);
+ // }
}
int uart2Putchar(int ch)
{
- uart2SendData(1, (uint8_t *)&ch);
+ //COMMENTED FIRMWARE
+ //uart2SendData(1, (uint8_t *)&ch);
return (unsigned char)ch;
}
@@ -363,15 +368,16 @@ bool uart2DidOverrun()
void __attribute__((used)) DMA1_Stream6_IRQHandler(void)
{
- portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+ //COMMENTED FIRMWARE
+ // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
- // Stop and cleanup DMA stream
- DMA_ITConfig(UART2_DMA_STREAM, DMA_IT_TC, DISABLE);
- DMA_ClearITPendingBit(UART2_DMA_STREAM, UART2_DMA_FLAG_TCIF);
- USART_DMACmd(UART2_TYPE, USART_DMAReq_Tx, DISABLE);
- DMA_Cmd(UART2_DMA_STREAM, DISABLE);
+ // // Stop and cleanup DMA stream
+ // DMA_ITConfig(UART2_DMA_STREAM, DMA_IT_TC, DISABLE);
+ // DMA_ClearITPendingBit(UART2_DMA_STREAM, UART2_DMA_FLAG_TCIF);
+ // USART_DMACmd(UART2_TYPE, USART_DMAReq_Tx, DISABLE);
+ // DMA_Cmd(UART2_DMA_STREAM, DISABLE);
- xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
+ // xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
}
#ifdef UART2_LINK_COMM
@@ -403,26 +409,27 @@ void __attribute__((used)) USART2_IRQHandler(void)
void __attribute__((used)) USART2_IRQHandler(void)
{
- uint8_t rxData;
- portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-
- if ((UART2_TYPE->SR & (1<<5)) != 0) // fast check if the RXNE interrupt has occurred
- {
- rxData = USART_ReceiveData(UART2_TYPE) & 0x00FF;
- xQueueSendFromISR(uart2queue, &rxData, &xHigherPriorityTaskWoken);
- }
- else
- {
- /** if we get here, the error is most likely caused by an overrun!
- * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
- * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
- * - reading USART_SR register followed reading the USART_DR register.
- */
- asm volatile ("" : "=m" (UART2_TYPE->SR) : "r" (UART2_TYPE->SR)); // force non-optimizable reads
- asm volatile ("" : "=m" (UART2_TYPE->DR) : "r" (UART2_TYPE->DR)); // of these two registers
-
- hasOverrun = true;
- }
+ //COMMENTED FIRMWARE
+ // uint8_t rxData;
+ // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+
+ // if ((UART2_TYPE->SR & (1<<5)) != 0) // fast check if the RXNE interrupt has occurred
+ // {
+ // rxData = USART_ReceiveData(UART2_TYPE) & 0x00FF;
+ // xQueueSendFromISR(uart2queue, &rxData, &xHigherPriorityTaskWoken);
+ // }
+ // else
+ // {
+ // /** if we get here, the error is most likely caused by an overrun!
+ // * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
+ // * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
+ // * - reading USART_SR register followed reading the USART_DR register.
+ // */
+ // asm volatile ("" : "=m" (UART2_TYPE->SR) : "r" (UART2_TYPE->SR)); // force non-optimizable reads
+ // asm volatile ("" : "=m" (UART2_TYPE->DR) : "r" (UART2_TYPE->DR)); // of these two registers
+
+ // hasOverrun = true;
+ // }
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart_syslink.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart_syslink.c
index fd5df3e31..c960da880 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart_syslink.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart_syslink.c
@@ -83,116 +83,118 @@ static void uartslkResumeDma();
*/
void uartslkDmaInit(void)
{
- NVIC_InitTypeDef NVIC_InitStructure;
-
- RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
-
- // USART TX DMA Channel Config
- DMA_InitStructureShare.DMA_PeripheralBaseAddr = (uint32_t)&UARTSLK_TYPE->DR;
- DMA_InitStructureShare.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
- DMA_InitStructureShare.DMA_MemoryInc = DMA_MemoryInc_Enable;
- DMA_InitStructureShare.DMA_MemoryBurst = DMA_MemoryBurst_Single;
- DMA_InitStructureShare.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
- DMA_InitStructureShare.DMA_BufferSize = 0;
- DMA_InitStructureShare.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- DMA_InitStructureShare.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
- DMA_InitStructureShare.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
- DMA_InitStructureShare.DMA_DIR = DMA_DIR_MemoryToPeripheral;
- DMA_InitStructureShare.DMA_Mode = DMA_Mode_Normal;
- DMA_InitStructureShare.DMA_Priority = DMA_Priority_High;
- DMA_InitStructureShare.DMA_FIFOMode = DMA_FIFOMode_Disable;
- DMA_InitStructureShare.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
- DMA_InitStructureShare.DMA_Channel = UARTSLK_DMA_CH;
-
- NVIC_InitStructure.NVIC_IRQChannel = UARTSLK_DMA_IRQ;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- isUartDmaInitialized = true;
+ //COMMENTED FIRMWARE
+ // NVIC_InitTypeDef NVIC_InitStructure;
+
+ // RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
+
+ // // USART TX DMA Channel Config
+ // DMA_InitStructureShare.DMA_PeripheralBaseAddr = (uint32_t)&UARTSLK_TYPE->DR;
+ // DMA_InitStructureShare.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
+ // DMA_InitStructureShare.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ // DMA_InitStructureShare.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+ // DMA_InitStructureShare.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ // DMA_InitStructureShare.DMA_BufferSize = 0;
+ // DMA_InitStructureShare.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ // DMA_InitStructureShare.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+ // DMA_InitStructureShare.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+ // DMA_InitStructureShare.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+ // DMA_InitStructureShare.DMA_Mode = DMA_Mode_Normal;
+ // DMA_InitStructureShare.DMA_Priority = DMA_Priority_High;
+ // DMA_InitStructureShare.DMA_FIFOMode = DMA_FIFOMode_Disable;
+ // DMA_InitStructureShare.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
+ // DMA_InitStructureShare.DMA_Channel = UARTSLK_DMA_CH;
+
+ // NVIC_InitStructure.NVIC_IRQChannel = UARTSLK_DMA_IRQ;
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ // NVIC_Init(&NVIC_InitStructure);
+
+ // isUartDmaInitialized = true;
}
void uartslkInit(void)
{
- // initialize the FreeRTOS structures first, to prevent null pointers in interrupts
- waitUntilSendDone = xSemaphoreCreateBinaryStatic(&waitUntilSendDoneBuffer); // initialized as blocking
- uartBusy = xSemaphoreCreateBinaryStatic(&uartBusyBuffer); // initialized as blocking
- xSemaphoreGive(uartBusy); // but we give it because the uart isn't busy at initialization
-
- syslinkPacketDelivery = STATIC_MEM_QUEUE_CREATE(syslinkPacketDelivery);
- DEBUG_QUEUE_MONITOR_REGISTER(syslinkPacketDelivery);
-
- USART_InitTypeDef USART_InitStructure;
- GPIO_InitTypeDef GPIO_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
- EXTI_InitTypeDef extiInit;
-
- /* Enable GPIO and USART clock */
- RCC_AHB1PeriphClockCmd(UARTSLK_GPIO_PERIF, ENABLE);
- ENABLE_UARTSLK_RCC(UARTSLK_PERIF, ENABLE);
-
- /* Configure USART Rx as input floating */
- GPIO_InitStructure.GPIO_Pin = UARTSLK_GPIO_RX_PIN;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
- GPIO_Init(UARTSLK_GPIO_PORT, &GPIO_InitStructure);
-
- /* Configure USART Tx as alternate function */
- GPIO_InitStructure.GPIO_Pin = UARTSLK_GPIO_TX_PIN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_Init(UARTSLK_GPIO_PORT, &GPIO_InitStructure);
-
- //Map uartslk to alternate functions
- GPIO_PinAFConfig(UARTSLK_GPIO_PORT, UARTSLK_GPIO_AF_TX_PIN, UARTSLK_GPIO_AF_TX);
- GPIO_PinAFConfig(UARTSLK_GPIO_PORT, UARTSLK_GPIO_AF_RX_PIN, UARTSLK_GPIO_AF_RX);
-
-#if defined(UARTSLK_OUTPUT_TRACE_DATA) || defined(ADC_OUTPUT_RAW_DATA) || defined(IMU_OUTPUT_RAW_DATA_ON_UART)
- USART_InitStructure.USART_BaudRate = 2000000;
- USART_InitStructure.USART_Mode = USART_Mode_Tx;
-#else
- USART_InitStructure.USART_BaudRate = 1000000;
- USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
-#endif
- USART_InitStructure.USART_WordLength = USART_WordLength_8b;
- USART_InitStructure.USART_StopBits = USART_StopBits_1;
- USART_InitStructure.USART_Parity = USART_Parity_No ;
- USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
- USART_Init(UARTSLK_TYPE, &USART_InitStructure);
-
- uartslkDmaInit();
-
- // Configure Rx buffer not empty interrupt
- NVIC_InitStructure.NVIC_IRQChannel = UARTSLK_IRQ;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SYSLINK_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- USART_ITConfig(UARTSLK_TYPE, USART_IT_RXNE, ENABLE);
-
- //Setting up TXEN pin (NRF flow control)
- RCC_AHB1PeriphClockCmd(UARTSLK_TXEN_PERIF, ENABLE);
-
- bzero(&GPIO_InitStructure, sizeof(GPIO_InitStructure));
- GPIO_InitStructure.GPIO_Pin = UARTSLK_TXEN_PIN;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
- GPIO_Init(UARTSLK_TXEN_PORT, &GPIO_InitStructure);
-
- extiInit.EXTI_Line = UARTSLK_TXEN_EXTI;
- extiInit.EXTI_Mode = EXTI_Mode_Interrupt;
- extiInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
- extiInit.EXTI_LineCmd = ENABLE;
- EXTI_Init(&extiInit);
- EXTI_ClearITPendingBit(UARTSLK_TXEN_EXTI);
-
- NVIC_EnableIRQ(EXTI4_IRQn);
-
- //Enable UART
- USART_Cmd(UARTSLK_TYPE, ENABLE);
+ //COMMENTED FIRMWARE
+// // initialize the FreeRTOS structures first, to prevent null pointers in interrupts
+// waitUntilSendDone = xSemaphoreCreateBinaryStatic(&waitUntilSendDoneBuffer); // initialized as blocking
+// uartBusy = xSemaphoreCreateBinaryStatic(&uartBusyBuffer); // initialized as blocking
+// xSemaphoreGive(uartBusy); // but we give it because the uart isn't busy at initialization
+
+// syslinkPacketDelivery = STATIC_MEM_QUEUE_CREATE(syslinkPacketDelivery);
+// DEBUG_QUEUE_MONITOR_REGISTER(syslinkPacketDelivery);
+
+// USART_InitTypeDef USART_InitStructure;
+// GPIO_InitTypeDef GPIO_InitStructure;
+// NVIC_InitTypeDef NVIC_InitStructure;
+// EXTI_InitTypeDef extiInit;
+
+// /* Enable GPIO and USART clock */
+// RCC_AHB1PeriphClockCmd(UARTSLK_GPIO_PERIF, ENABLE);
+// ENABLE_UARTSLK_RCC(UARTSLK_PERIF, ENABLE);
+
+// /* Configure USART Rx as input floating */
+// GPIO_InitStructure.GPIO_Pin = UARTSLK_GPIO_RX_PIN;
+// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+// GPIO_Init(UARTSLK_GPIO_PORT, &GPIO_InitStructure);
+
+// /* Configure USART Tx as alternate function */
+// GPIO_InitStructure.GPIO_Pin = UARTSLK_GPIO_TX_PIN;
+// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+// GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+// GPIO_Init(UARTSLK_GPIO_PORT, &GPIO_InitStructure);
+
+// //Map uartslk to alternate functions
+// GPIO_PinAFConfig(UARTSLK_GPIO_PORT, UARTSLK_GPIO_AF_TX_PIN, UARTSLK_GPIO_AF_TX);
+// GPIO_PinAFConfig(UARTSLK_GPIO_PORT, UARTSLK_GPIO_AF_RX_PIN, UARTSLK_GPIO_AF_RX);
+
+// #if defined(UARTSLK_OUTPUT_TRACE_DATA) || defined(ADC_OUTPUT_RAW_DATA) || defined(IMU_OUTPUT_RAW_DATA_ON_UART)
+// USART_InitStructure.USART_BaudRate = 2000000;
+// USART_InitStructure.USART_Mode = USART_Mode_Tx;
+// #else
+// USART_InitStructure.USART_BaudRate = 1000000;
+// USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+// #endif
+// USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+// USART_InitStructure.USART_StopBits = USART_StopBits_1;
+// USART_InitStructure.USART_Parity = USART_Parity_No ;
+// USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+// USART_Init(UARTSLK_TYPE, &USART_InitStructure);
+
+// uartslkDmaInit();
+
+// // Configure Rx buffer not empty interrupt
+// NVIC_InitStructure.NVIC_IRQChannel = UARTSLK_IRQ;
+// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SYSLINK_PRI;
+// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+// NVIC_Init(&NVIC_InitStructure);
+
+// USART_ITConfig(UARTSLK_TYPE, USART_IT_RXNE, ENABLE);
+
+// //Setting up TXEN pin (NRF flow control)
+// RCC_AHB1PeriphClockCmd(UARTSLK_TXEN_PERIF, ENABLE);
+
+// bzero(&GPIO_InitStructure, sizeof(GPIO_InitStructure));
+// GPIO_InitStructure.GPIO_Pin = UARTSLK_TXEN_PIN;
+// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+// GPIO_Init(UARTSLK_TXEN_PORT, &GPIO_InitStructure);
+
+// extiInit.EXTI_Line = UARTSLK_TXEN_EXTI;
+// extiInit.EXTI_Mode = EXTI_Mode_Interrupt;
+// extiInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
+// extiInit.EXTI_LineCmd = ENABLE;
+// EXTI_Init(&extiInit);
+// EXTI_ClearITPendingBit(UARTSLK_TXEN_EXTI);
+
+// NVIC_EnableIRQ(EXTI4_IRQn);
+
+// //Enable UART
+// USART_Cmd(UARTSLK_TYPE, ENABLE);
isInit = true;
}
@@ -208,260 +210,273 @@ void uartslkGetPacketBlocking(SyslinkPacket* packet)
void uartslkSendData(uint32_t size, uint8_t* data)
{
- uint32_t i;
-
- if (!isInit)
- return;
-
- for(i = 0; i < size; i++)
- {
-#ifdef UARTSLK_SPINLOOP_FLOWCTRL
- while(GPIO_ReadInputDataBit(UARTSLK_TXEN_PORT, UARTSLK_TXEN_PIN) == Bit_SET);
-#endif
- while (!(UARTSLK_TYPE->SR & USART_FLAG_TXE));
- UARTSLK_TYPE->DR = (data[i] & 0x00FF);
- }
+ //COMMENTED FIRMWARE
+// uint32_t i;
+
+// if (!isInit)
+// return;
+
+// for(i = 0; i < size; i++)
+// {
+// #ifdef UARTSLK_SPINLOOP_FLOWCTRL
+// while(GPIO_ReadInputDataBit(UARTSLK_TXEN_PORT, UARTSLK_TXEN_PIN) == Bit_SET);
+// #endif
+// while (!(UARTSLK_TYPE->SR & USART_FLAG_TXE));
+// UARTSLK_TYPE->DR = (data[i] & 0x00FF);
+// }
}
void uartslkSendDataIsrBlocking(uint32_t size, uint8_t* data)
{
- xSemaphoreTake(uartBusy, portMAX_DELAY);
- outDataIsr = data;
- dataSizeIsr = size;
- dataIndexIsr = 1;
- uartslkSendData(1, &data[0]);
- USART_ITConfig(UARTSLK_TYPE, USART_IT_TXE, ENABLE);
- xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
- outDataIsr = 0;
- xSemaphoreGive(uartBusy);
+ //COMMENTED FIRMWARE
+ // xSemaphoreTake(uartBusy, portMAX_DELAY);
+ // outDataIsr = data;
+ // dataSizeIsr = size;
+ // dataIndexIsr = 1;
+ // uartslkSendData(1, &data[0]);
+ // USART_ITConfig(UARTSLK_TYPE, USART_IT_TXE, ENABLE);
+ // xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
+ // outDataIsr = 0;
+ // xSemaphoreGive(uartBusy);
}
int uartslkPutchar(int ch)
{
- uartslkSendData(1, (uint8_t *)&ch);
+ //COMMENTED FIRMWARE
+ //uartslkSendData(1, (uint8_t *)&ch);
return (unsigned char)ch;
}
void uartslkSendDataDmaBlocking(uint32_t size, uint8_t* data)
{
- if (isUartDmaInitialized)
- {
- xSemaphoreTake(uartBusy, portMAX_DELAY);
- // Wait for DMA to be free
- while(DMA_GetCmdStatus(UARTSLK_DMA_STREAM) != DISABLE);
- //Copy data in DMA buffer
- memcpy(dmaBuffer, data, size);
- DMA_InitStructureShare.DMA_BufferSize = size;
- initialDMACount = size;
- // Init new DMA stream
- DMA_Init(UARTSLK_DMA_STREAM, &DMA_InitStructureShare);
- // Enable the Transfer Complete interrupt
- DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, ENABLE);
- /* Enable USART DMA TX Requests */
- USART_DMACmd(UARTSLK_TYPE, USART_DMAReq_Tx, ENABLE);
- /* Clear transfer complete */
- USART_ClearFlag(UARTSLK_TYPE, USART_FLAG_TC);
- /* Enable DMA USART TX Stream */
- DMA_Cmd(UARTSLK_DMA_STREAM, ENABLE);
- xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
- xSemaphoreGive(uartBusy);
- }
+ //COMMENTED FIRMWARE
+ // if (isUartDmaInitialized)
+ // {
+ // xSemaphoreTake(uartBusy, portMAX_DELAY);
+ // // Wait for DMA to be free
+ // while(DMA_GetCmdStatus(UARTSLK_DMA_STREAM) != DISABLE);
+ // //Copy data in DMA buffer
+ // memcpy(dmaBuffer, data, size);
+ // DMA_InitStructureShare.DMA_BufferSize = size;
+ // initialDMACount = size;
+ // // Init new DMA stream
+ // DMA_Init(UARTSLK_DMA_STREAM, &DMA_InitStructureShare);
+ // // Enable the Transfer Complete interrupt
+ // DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, ENABLE);
+ // /* Enable USART DMA TX Requests */
+ // USART_DMACmd(UARTSLK_TYPE, USART_DMAReq_Tx, ENABLE);
+ // /* Clear transfer complete */
+ // USART_ClearFlag(UARTSLK_TYPE, USART_FLAG_TC);
+ // /* Enable DMA USART TX Stream */
+ // DMA_Cmd(UARTSLK_DMA_STREAM, ENABLE);
+ // xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
+ // xSemaphoreGive(uartBusy);
+ // }
}
static void uartslkPauseDma()
{
- if (DMA_GetCmdStatus(UARTSLK_DMA_STREAM) == ENABLE)
- {
- // Disable transfer complete interrupt
- DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, DISABLE);
- // Disable stream to pause it
- DMA_Cmd(UARTSLK_DMA_STREAM, DISABLE);
- // Wait for it to be disabled
- while(DMA_GetCmdStatus(UARTSLK_DMA_STREAM) != DISABLE);
- // Disable transfer complete
- DMA_ClearITPendingBit(UARTSLK_DMA_STREAM, UARTSLK_DMA_FLAG_TCIF);
- // Read remaining data count
- remainingDMACount = DMA_GetCurrDataCounter(UARTSLK_DMA_STREAM);
- dmaIsPaused = true;
- }
+ //COMMENTED FIRMWARE
+ // if (DMA_GetCmdStatus(UARTSLK_DMA_STREAM) == ENABLE)
+ // {
+ // // Disable transfer complete interrupt
+ // DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, DISABLE);
+ // // Disable stream to pause it
+ // DMA_Cmd(UARTSLK_DMA_STREAM, DISABLE);
+ // // Wait for it to be disabled
+ // while(DMA_GetCmdStatus(UARTSLK_DMA_STREAM) != DISABLE);
+ // // Disable transfer complete
+ // DMA_ClearITPendingBit(UARTSLK_DMA_STREAM, UARTSLK_DMA_FLAG_TCIF);
+ // // Read remaining data count
+ // remainingDMACount = DMA_GetCurrDataCounter(UARTSLK_DMA_STREAM);
+ // dmaIsPaused = true;
+ // }
}
static void uartslkResumeDma()
{
- if (dmaIsPaused)
- {
- // Update DMA counter
- DMA_SetCurrDataCounter(UARTSLK_DMA_STREAM, remainingDMACount);
- // Update memory read address
- UARTSLK_DMA_STREAM->M0AR = (uint32_t)&dmaBuffer[initialDMACount - remainingDMACount];
- // Enable the Transfer Complete interrupt
- DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, ENABLE);
- /* Clear transfer complete */
- USART_ClearFlag(UARTSLK_TYPE, USART_FLAG_TC);
- /* Enable DMA USART TX Stream */
- DMA_Cmd(UARTSLK_DMA_STREAM, ENABLE);
- dmaIsPaused = false;
- }
+ //COMMENTED FIRMWARE
+ // if (dmaIsPaused)
+ // {
+ // // Update DMA counter
+ // DMA_SetCurrDataCounter(UARTSLK_DMA_STREAM, remainingDMACount);
+ // // Update memory read address
+ // UARTSLK_DMA_STREAM->M0AR = (uint32_t)&dmaBuffer[initialDMACount - remainingDMACount];
+ // // Enable the Transfer Complete interrupt
+ // DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, ENABLE);
+ // /* Clear transfer complete */
+ // USART_ClearFlag(UARTSLK_TYPE, USART_FLAG_TC);
+ // /* Enable DMA USART TX Stream */
+ // DMA_Cmd(UARTSLK_DMA_STREAM, ENABLE);
+ // dmaIsPaused = false;
+ // }
}
void uartslkDmaIsr(void)
{
- portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+ //COMMENTED FIRMWARE
+ // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
- // Stop and cleanup DMA stream
- DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, DISABLE);
- DMA_ClearITPendingBit(UARTSLK_DMA_STREAM, UARTSLK_DMA_FLAG_TCIF);
- USART_DMACmd(UARTSLK_TYPE, USART_DMAReq_Tx, DISABLE);
- DMA_Cmd(UARTSLK_DMA_STREAM, DISABLE);
+ // // Stop and cleanup DMA stream
+ // DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, DISABLE);
+ // DMA_ClearITPendingBit(UARTSLK_DMA_STREAM, UARTSLK_DMA_FLAG_TCIF);
+ // USART_DMACmd(UARTSLK_TYPE, USART_DMAReq_Tx, DISABLE);
+ // DMA_Cmd(UARTSLK_DMA_STREAM, DISABLE);
- remainingDMACount = 0;
- xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
+ // remainingDMACount = 0;
+ // xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
}
void uartslkHandleDataFromISR(uint8_t c, BaseType_t * const pxHigherPriorityTaskWoken)
{
- switch (rxState)
- {
- case waitForFirstStart:
- rxState = (c == SYSLINK_START_BYTE1) ? waitForSecondStart : waitForFirstStart;
- break;
- case waitForSecondStart:
- rxState = (c == SYSLINK_START_BYTE2) ? waitForType : waitForFirstStart;
- break;
- case waitForType:
- cksum[0] = c;
- cksum[1] = c;
- slp.type = c;
- rxState = waitForLength;
- break;
- case waitForLength:
- if (c <= SYSLINK_MTU)
- {
- slp.length = c;
- cksum[0] += c;
- cksum[1] += cksum[0];
- dataIndex = 0;
- rxState = (c > 0) ? waitForData : waitForChksum1;
- }
- else
- {
- rxState = waitForFirstStart;
- }
- break;
- case waitForData:
- slp.data[dataIndex] = c;
- cksum[0] += c;
- cksum[1] += cksum[0];
- dataIndex++;
- if (dataIndex == slp.length)
- {
- rxState = waitForChksum1;
- }
- break;
- case waitForChksum1:
- if (cksum[0] == c)
- {
- rxState = waitForChksum2;
- }
- else
- {
- rxState = waitForFirstStart; //Checksum error
- ASSERT(0);
- }
- break;
- case waitForChksum2:
- if (cksum[1] == c)
- {
- // Post the packet to the queue if there's room
- if (!xQueueIsQueueFullFromISR(syslinkPacketDelivery))
- {
- xQueueSendFromISR(syslinkPacketDelivery, (void *)&slp, pxHigherPriorityTaskWoken);
- }
- else
- {
- ASSERT(0); // Queue overflow
- }
- }
- else
- {
- rxState = waitForFirstStart; //Checksum error
- ASSERT(0);
- }
- rxState = waitForFirstStart;
- break;
- default:
- ASSERT(0);
- break;
- }
+ //COMMENTED FIRMWARE
+ // switch (rxState)
+ // {
+ // case waitForFirstStart:
+ // rxState = (c == SYSLINK_START_BYTE1) ? waitForSecondStart : waitForFirstStart;
+ // break;
+ // case waitForSecondStart:
+ // rxState = (c == SYSLINK_START_BYTE2) ? waitForType : waitForFirstStart;
+ // break;
+ // case waitForType:
+ // cksum[0] = c;
+ // cksum[1] = c;
+ // slp.type = c;
+ // rxState = waitForLength;
+ // break;
+ // case waitForLength:
+ // if (c <= SYSLINK_MTU)
+ // {
+ // slp.length = c;
+ // cksum[0] += c;
+ // cksum[1] += cksum[0];
+ // dataIndex = 0;
+ // rxState = (c > 0) ? waitForData : waitForChksum1;
+ // }
+ // else
+ // {
+ // rxState = waitForFirstStart;
+ // }
+ // break;
+ // case waitForData:
+ // slp.data[dataIndex] = c;
+ // cksum[0] += c;
+ // cksum[1] += cksum[0];
+ // dataIndex++;
+ // if (dataIndex == slp.length)
+ // {
+ // rxState = waitForChksum1;
+ // }
+ // break;
+ // case waitForChksum1:
+ // if (cksum[0] == c)
+ // {
+ // rxState = waitForChksum2;
+ // }
+ // else
+ // {
+ // rxState = waitForFirstStart; //Checksum error
+ // ASSERT(0);
+ // }
+ // break;
+ // case waitForChksum2:
+ // if (cksum[1] == c)
+ // {
+ // // Post the packet to the queue if there's room
+ // if (!xQueueIsQueueFullFromISR(syslinkPacketDelivery))
+ // {
+ // xQueueSendFromISR(syslinkPacketDelivery, (void *)&slp, pxHigherPriorityTaskWoken);
+ // }
+ // else
+ // {
+ // ASSERT(0); // Queue overflow
+ // }
+ // }
+ // else
+ // {
+ // rxState = waitForFirstStart; //Checksum error
+ // ASSERT(0);
+ // }
+ // rxState = waitForFirstStart;
+ // break;
+ // default:
+ // ASSERT(0);
+ // break;
+ // }
}
void uartslkIsr(void)
{
- portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-
- // the following if statement replaces:
- // if (USART_GetITStatus(UARTSLK_TYPE, USART_IT_RXNE) == SET)
- // we do this check as fast as possible to minimize the chance of an overrun,
- // which occasionally cause problems and cause packet loss at high CPU usage
- if ((UARTSLK_TYPE->SR & (1<<5)) != 0) // if the RXNE interrupt has occurred
- {
- uint8_t rxDataInterrupt = (uint8_t)(UARTSLK_TYPE->DR & 0xFF);
- uartslkHandleDataFromISR(rxDataInterrupt, &xHigherPriorityTaskWoken);
- }
- else if (USART_GetITStatus(UARTSLK_TYPE, USART_IT_TXE) == SET)
- {
- if (outDataIsr && (dataIndexIsr < dataSizeIsr))
- {
- USART_SendData(UARTSLK_TYPE, outDataIsr[dataIndexIsr] & 0x00FF);
- dataIndexIsr++;
- }
- else
- {
- USART_ITConfig(UARTSLK_TYPE, USART_IT_TXE, DISABLE);
- xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
- }
- }
- else
- {
- /** if we get here, the error is most likely caused by an overrun!
- * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
- * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
- * - reading USART_SR register followed reading the USART_DR register.
- */
- asm volatile ("" : "=m" (UARTSLK_TYPE->SR) : "r" (UARTSLK_TYPE->SR)); // force non-optimizable reads
- asm volatile ("" : "=m" (UARTSLK_TYPE->DR) : "r" (UARTSLK_TYPE->DR)); // of these two registers
- }
-
- portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+ //COMMENTED FIRMWARE
+ // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+
+ // // the following if statement replaces:
+ // // if (USART_GetITStatus(UARTSLK_TYPE, USART_IT_RXNE) == SET)
+ // // we do this check as fast as possible to minimize the chance of an overrun,
+ // // which occasionally cause problems and cause packet loss at high CPU usage
+ // if ((UARTSLK_TYPE->SR & (1<<5)) != 0) // if the RXNE interrupt has occurred
+ // {
+ // uint8_t rxDataInterrupt = (uint8_t)(UARTSLK_TYPE->DR & 0xFF);
+ // uartslkHandleDataFromISR(rxDataInterrupt, &xHigherPriorityTaskWoken);
+ // }
+ // else if (USART_GetITStatus(UARTSLK_TYPE, USART_IT_TXE) == SET)
+ // {
+ // if (outDataIsr && (dataIndexIsr < dataSizeIsr))
+ // {
+ // USART_SendData(UARTSLK_TYPE, outDataIsr[dataIndexIsr] & 0x00FF);
+ // dataIndexIsr++;
+ // }
+ // else
+ // {
+ // USART_ITConfig(UARTSLK_TYPE, USART_IT_TXE, DISABLE);
+ // xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
+ // }
+ // }
+ // else
+ // {
+ // /** if we get here, the error is most likely caused by an overrun!
+ // * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
+ // * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
+ // * - reading USART_SR register followed reading the USART_DR register.
+ // */
+ // asm volatile ("" : "=m" (UARTSLK_TYPE->SR) : "r" (UARTSLK_TYPE->SR)); // force non-optimizable reads
+ // asm volatile ("" : "=m" (UARTSLK_TYPE->DR) : "r" (UARTSLK_TYPE->DR)); // of these two registers
+ // }
+
+ // portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
void uartslkTxenFlowctrlIsr()
{
- EXTI_ClearFlag(UARTSLK_TXEN_EXTI);
- if (GPIO_ReadInputDataBit(UARTSLK_TXEN_PORT, UARTSLK_TXEN_PIN) == Bit_SET)
- {
- uartslkPauseDma();
- //ledSet(LED_GREEN_R, 1);
- }
- else
- {
- uartslkResumeDma();
- //ledSet(LED_GREEN_R, 0);
- }
+ //COMMENTED FIRMWARE
+ // EXTI_ClearFlag(UARTSLK_TXEN_EXTI);
+ // if (GPIO_ReadInputDataBit(UARTSLK_TXEN_PORT, UARTSLK_TXEN_PIN) == Bit_SET)
+ // {
+ // uartslkPauseDma();
+ // //ledSet(LED_GREEN_R, 1);
+ // }
+ // else
+ // {
+ // uartslkResumeDma();
+ // //ledSet(LED_GREEN_R, 0);
+ // }
}
void __attribute__((used)) EXTI4_Callback(void)
{
- uartslkTxenFlowctrlIsr();
+ //COMMENTED FIRMWARE
+ //uartslkTxenFlowctrlIsr();
}
void __attribute__((used)) USART6_IRQHandler(void)
{
- uartslkIsr();
+ //COMMENTED FIRMWARE
+ //uartslkIsr();
}
void __attribute__((used)) DMA2_Stream7_IRQHandler(void)
{
- uartslkDmaIsr();
+ //COMMENTED FIRMWARE
+ //uartslkDmaIsr();
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l0x.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l0x.c
index b28be495c..49b006c39 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l0x.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l0x.c
@@ -104,26 +104,27 @@ static int nextI2CAddress = VL53L0X_DEFAULT_ADDRESS+1;
bool vl53l0xInit(VL53L0xDev* dev, I2C_Dev *I2Cx, bool io_2V8)
{
- dev->I2Cx = I2Cx;
- dev->devAddr = VL53L0X_DEFAULT_ADDRESS;
-
- dev->io_timeout = 0;
- dev->did_timeout = 0;
- dev->timeout_start_ms = 0;
- dev->stop_variable = 0;
- dev->measurement_timing_budget_us = 0;
- dev->measurement_timing_budget_ms = 0;
-
- if (!vl53l0xInitSensor(dev, io_2V8)) {
- return false;
- }
+ //COMMENTED FIRMWARE
+ // dev->I2Cx = I2Cx;
+ // dev->devAddr = VL53L0X_DEFAULT_ADDRESS;
+
+ // dev->io_timeout = 0;
+ // dev->did_timeout = 0;
+ // dev->timeout_start_ms = 0;
+ // dev->stop_variable = 0;
+ // dev->measurement_timing_budget_us = 0;
+ // dev->measurement_timing_budget_ms = 0;
+
+ // if (!vl53l0xInitSensor(dev, io_2V8)) {
+ // return false;
+ // }
- /* Move initialized sensor to a new I2C address */
- int newAddress;
+ // /* Move initialized sensor to a new I2C address */
+ // int newAddress;
- taskENTER_CRITICAL();
- newAddress = nextI2CAddress++;
- taskEXIT_CRITICAL();
+ // taskENTER_CRITICAL();
+ // newAddress = nextI2CAddress++;
+ // taskEXIT_CRITICAL();
return vl53l0xSetI2CAddress(dev, newAddress);
}
@@ -135,8 +136,9 @@ bool vl53l0xInit(VL53L0xDev* dev, I2C_Dev *I2Cx, bool io_2V8)
bool vl53l0xTestConnection(VL53L0xDev* dev)
{
bool ret = true;
- ret &= vl53l0xGetModelID(dev) == VL53L0X_IDENTIFICATION_MODEL_ID;
- ret &= vl53l0xGetRevisionID(dev) == VL53L0X_IDENTIFICATION_REVISION_ID;
+ //COMMENTED FIRMWARE
+ // ret &= vl53l0xGetModelID(dev) == VL53L0X_IDENTIFICATION_MODEL_ID;
+ // ret &= vl53l0xGetRevisionID(dev) == VL53L0X_IDENTIFICATION_REVISION_ID;
return ret;
}
@@ -162,7 +164,8 @@ uint16_t vl53l0xGetModelID(VL53L0xDev* dev)
uint8_t vl53l0xGetRevisionID(VL53L0xDev* dev)
{
uint8_t output = 0;
- i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_IDENTIFICATION_REVISION_ID, &output);
+ //COMMENTED FIRMWARE
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_IDENTIFICATION_REVISION_ID, &output);
return output;
}
@@ -173,7 +176,8 @@ uint8_t vl53l0xGetRevisionID(VL53L0xDev* dev)
*/
bool vl53l0xSetI2CAddress(VL53L0xDev* dev, uint8_t address) {
bool pass = i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS, address);
- dev->devAddr = address;
+ //COMMENTED FIRMWARE
+ //dev->devAddr = address;
return pass;
}
@@ -187,225 +191,226 @@ bool vl53l0xSetI2CAddress(VL53L0xDev* dev, uint8_t address) {
// mode.
bool vl53l0xInitSensor(VL53L0xDev* dev, bool io_2v8)
{
- uint8_t temp;
- // VL53L0X_DataInit() begin
-
- // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
- if (io_2v8)
- {
- i2cdevWriteBit(dev->I2Cx, dev->devAddr, VL53L0X_RA_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0, 0x01);
- }
-
- // "Set I2C standard mode"
- if (!i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x88, 0x00)) {
- return false;
- }
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
- i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x91, &dev->stop_variable);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
-
- // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
- i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_CONTROL, &temp);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_CONTROL, temp | 0x12);
-
- // set final range signal rate limit to 0.25 MCPS (million counts per second)
- vl53l0xSetSignalRateLimit(dev, 0.25);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xFF);
-
- // VL53L0X_DataInit() end
-
- // VL53L0X_StaticInit() begin
-
- uint8_t spad_count;
- bool spad_type_is_aperture;
- if (!vl53l0xGetSpadInfo(dev, &spad_count, &spad_type_is_aperture)) { return false; }
-
- // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
- // the API, but the same data seems to be more easily readable from
- // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
- uint8_t ref_spad_map[6];
- i2cdevReadReg8(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6, ref_spad_map);
-
- // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
-
- uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad
- uint8_t spads_enabled = 0;
-
- for (uint8_t i = 0; i < 48; i++)
- {
- if (i < first_spad_to_enable || spads_enabled == spad_count)
- {
- // This bit is lower than the first one that should be enabled, or
- // (reference_spad_count) bits have already been enabled, so zero this bit
- ref_spad_map[i / 8] &= ~(1 << (i % 8));
- }
- else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1)
- {
- spads_enabled++;
- }
- }
-
- i2cdevWriteReg8(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6, ref_spad_map);
-
- // -- VL53L0X_set_reference_spads() end
-
- // -- VL53L0X_load_tuning_settings() begin
- // DefaultTuningSettings from vl53l0x_tuning.h
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x09, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x10, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x11, 0x00);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x24, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x25, 0xFF);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x75, 0x00);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4E, 0x2C);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x48, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x30, 0x20);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x30, 0x09);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x54, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x31, 0x04);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x32, 0x03);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x40, 0x83);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x46, 0x25);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x60, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x27, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x50, 0x06);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x51, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x52, 0x96);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x56, 0x08);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x57, 0x30);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x61, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x62, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x64, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x65, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x66, 0xA0);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x22, 0x32);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x47, 0x14);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x49, 0xFF);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4A, 0x00);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x7A, 0x0A);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x7B, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x78, 0x21);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x23, 0x34);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x42, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x44, 0xFF);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x45, 0x26);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x46, 0x05);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x40, 0x40);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x0E, 0x06);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x20, 0x1A);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x43, 0x40);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x34, 0x03);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x35, 0x44);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x31, 0x04);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4B, 0x09);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4C, 0x05);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4D, 0x04);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x44, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x45, 0x20);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x47, 0x08);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x48, 0x28);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x67, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x70, 0x04);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x71, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x72, 0xFE);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x76, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x77, 0x00);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x0D, 0x01);
+ //COMMENTED FIRMWARE
+ // uint8_t temp;
+ // // VL53L0X_DataInit() begin
+
+ // // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
+ // if (io_2v8)
+ // {
+ // i2cdevWriteBit(dev->I2Cx, dev->devAddr, VL53L0X_RA_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0, 0x01);
+ // }
+
+ // // "Set I2C standard mode"
+ // if (!i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x88, 0x00)) {
+ // return false;
+ // }
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x91, &dev->stop_variable);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
+
+ // // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_CONTROL, &temp);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_CONTROL, temp | 0x12);
+
+ // // set final range signal rate limit to 0.25 MCPS (million counts per second)
+ // vl53l0xSetSignalRateLimit(dev, 0.25);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xFF);
+
+ // // VL53L0X_DataInit() end
+
+ // // VL53L0X_StaticInit() begin
+
+ // uint8_t spad_count;
+ // bool spad_type_is_aperture;
+ // if (!vl53l0xGetSpadInfo(dev, &spad_count, &spad_type_is_aperture)) { return false; }
+
+ // // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
+ // // the API, but the same data seems to be more easily readable from
+ // // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
+ // uint8_t ref_spad_map[6];
+ // i2cdevReadReg8(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6, ref_spad_map);
+
+ // // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
+
+ // uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad
+ // uint8_t spads_enabled = 0;
+
+ // for (uint8_t i = 0; i < 48; i++)
+ // {
+ // if (i < first_spad_to_enable || spads_enabled == spad_count)
+ // {
+ // // This bit is lower than the first one that should be enabled, or
+ // // (reference_spad_count) bits have already been enabled, so zero this bit
+ // ref_spad_map[i / 8] &= ~(1 << (i % 8));
+ // }
+ // else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1)
+ // {
+ // spads_enabled++;
+ // }
+ // }
+
+ // i2cdevWriteReg8(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6, ref_spad_map);
+
+ // // -- VL53L0X_set_reference_spads() end
+
+ // // -- VL53L0X_load_tuning_settings() begin
+ // // DefaultTuningSettings from vl53l0x_tuning.h
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x09, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x10, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x11, 0x00);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x24, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x25, 0xFF);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x75, 0x00);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4E, 0x2C);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x48, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x30, 0x20);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x30, 0x09);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x54, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x31, 0x04);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x32, 0x03);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x40, 0x83);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x46, 0x25);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x60, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x27, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x50, 0x06);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x51, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x52, 0x96);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x56, 0x08);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x57, 0x30);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x61, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x62, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x64, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x65, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x66, 0xA0);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x22, 0x32);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x47, 0x14);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x49, 0xFF);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4A, 0x00);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x7A, 0x0A);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x7B, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x78, 0x21);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x23, 0x34);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x42, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x44, 0xFF);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x45, 0x26);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x46, 0x05);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x40, 0x40);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x0E, 0x06);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x20, 0x1A);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x43, 0x40);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x34, 0x03);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x35, 0x44);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x31, 0x04);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4B, 0x09);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4C, 0x05);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4D, 0x04);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x44, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x45, 0x20);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x47, 0x08);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x48, 0x28);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x67, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x70, 0x04);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x71, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x72, 0xFE);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x76, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x77, 0x00);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x0D, 0x01);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x01, 0xF8);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x8E, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
+
+ // // -- VL53L0X_load_tuning_settings() end
+
+ // // "Set interrupt config to new sample ready"
+ // // -- VL53L0X_SetGpioConfig() begin
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
+ // i2cdevWriteBit(dev->I2Cx, dev->devAddr, VL53L0X_RA_GPIO_HV_MUX_ACTIVE_HIGH, 4, 0); // active low
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
+
+ // // -- VL53L0X_SetGpioConfig() end
+
+ // dev->measurement_timing_budget_us = vl53l0xGetMeasurementTimingBudget(dev);
+ // dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
+
+ // // "Disable MSRC and TCC by default"
+ // // MSRC = Minimum Signal Rate Check
+ // // TCC = Target CentreCheck
+ // // -- VL53L0X_SetSequenceStepEnable() begin
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xE8);
+
+ // // -- VL53L0X_SetSequenceStepEnable() end
+
+ // // "Recalculate timing budget"
+ // vl53l0xSetMeasurementTimingBudget(dev, dev->measurement_timing_budget_us);
+
+ // // VL53L0X_StaticInit() end
+
+ // // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration())
+
+ // // -- VL53L0X_perform_vhv_calibration() begin
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x01, 0xF8);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x01);
+ // if (!vl53l0xPerformSingleRefCalibration(dev, 0x40)) { DEBUG_PRINT("Failed VHV calibration\n"); return false; }
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x8E, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
-
- // -- VL53L0X_load_tuning_settings() end
-
- // "Set interrupt config to new sample ready"
- // -- VL53L0X_SetGpioConfig() begin
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
- i2cdevWriteBit(dev->I2Cx, dev->devAddr, VL53L0X_RA_GPIO_HV_MUX_ACTIVE_HIGH, 4, 0); // active low
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
-
- // -- VL53L0X_SetGpioConfig() end
-
- dev->measurement_timing_budget_us = vl53l0xGetMeasurementTimingBudget(dev);
- dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
-
- // "Disable MSRC and TCC by default"
- // MSRC = Minimum Signal Rate Check
- // TCC = Target CentreCheck
- // -- VL53L0X_SetSequenceStepEnable() begin
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xE8);
-
- // -- VL53L0X_SetSequenceStepEnable() end
-
- // "Recalculate timing budget"
- vl53l0xSetMeasurementTimingBudget(dev, dev->measurement_timing_budget_us);
-
- // VL53L0X_StaticInit() end
-
- // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration())
-
- // -- VL53L0X_perform_vhv_calibration() begin
+ // // -- VL53L0X_perform_vhv_calibration() end
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x01);
- if (!vl53l0xPerformSingleRefCalibration(dev, 0x40)) { DEBUG_PRINT("Failed VHV calibration\n"); return false; }
+ // // -- VL53L0X_perform_phase_calibration() begin
- // -- VL53L0X_perform_vhv_calibration() end
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x02);
+ // if (!vl53l0xPerformSingleRefCalibration(dev, 0x00)) { DEBUG_PRINT("Failed phase calibration\n"); return false; }
- // -- VL53L0X_perform_phase_calibration() begin
+ // // -- VL53L0X_perform_phase_calibration() end
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x02);
- if (!vl53l0xPerformSingleRefCalibration(dev, 0x00)) { DEBUG_PRINT("Failed phase calibration\n"); return false; }
+ // // "restore the previous Sequence Config"
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xE8);
- // -- VL53L0X_perform_phase_calibration() end
-
- // "restore the previous Sequence Config"
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xE8);
-
- // VL53L0X_PerformRefCalibration() end
+ // // VL53L0X_PerformRefCalibration() end
return true;
}
@@ -420,10 +425,11 @@ bool vl53l0xInitSensor(VL53L0xDev* dev, bool io_2v8)
// Defaults to 0.25 MCPS as initialized by the ST API and this library.
bool vl53l0xSetSignalRateLimit(VL53L0xDev* dev, float limit_Mcps)
{
- if (limit_Mcps < 0 || limit_Mcps > 511.99f) { return false; }
+ //COMMENTED FIRMWARE
+ // if (limit_Mcps < 0 || limit_Mcps > 511.99f) { return false; }
- // Q9.7 fixed point format (9 integer bits, 7 fractional bits)
- uint16_t fixed_pt = limit_Mcps * (1 << 7);
+ // // Q9.7 fixed point format (9 integer bits, 7 fractional bits)
+ // uint16_t fixed_pt = limit_Mcps * (1 << 7);
return vl53l0xWriteReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, fixed_pt);
}
@@ -436,88 +442,89 @@ bool vl53l0xSetSignalRateLimit(VL53L0xDev* dev, float limit_Mcps)
// based on VL53L0X_set_measurement_timing_budget_micro_seconds()
bool vl53l0xSetMeasurementTimingBudget(VL53L0xDev* dev, uint32_t budget_us)
{
- SequenceStepEnables enables;
- SequenceStepTimeouts timeouts;
-
- uint16_t const StartOverhead = 1320; // note that this is different than the value in get_
- uint16_t const EndOverhead = 960;
- uint16_t const MsrcOverhead = 660;
- uint16_t const TccOverhead = 590;
- uint16_t const DssOverhead = 690;
- uint16_t const PreRangeOverhead = 660;
- uint16_t const FinalRangeOverhead = 550;
-
- uint32_t const MinTimingBudget = 20000;
-
- if (budget_us < MinTimingBudget) { return false; }
-
- uint32_t used_budget_us = StartOverhead + EndOverhead;
-
- vl53l0xGetSequenceStepEnables(dev, &enables);
- vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
-
- if (enables.tcc)
- {
- used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
- }
-
- if (enables.dss)
- {
- used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
- }
- else if (enables.msrc)
- {
- used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
- }
-
- if (enables.pre_range)
- {
- used_budget_us += (timeouts.pre_range_us + PreRangeOverhead);
- }
-
- if (enables.final_range)
- {
- used_budget_us += FinalRangeOverhead;
-
- // "Note that the final range timeout is determined by the timing
- // budget and the sum of all other timeouts within the sequence.
- // If there is no room for the final range timeout, then an error
- // will be set. Otherwise the remaining time will be applied to
- // the final range."
-
- if (used_budget_us > budget_us)
- {
- // "Requested timeout too big."
- return false;
- }
-
- uint32_t final_range_timeout_us = budget_us - used_budget_us;
-
- // set_sequence_step_timeout() begin
- // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
-
- // "For the final range timeout, the pre-range timeout
- // must be added. To do this both final and pre-range
- // timeouts must be expressed in macro periods MClks
- // because they have different vcsel periods."
-
- uint16_t final_range_timeout_mclks =
- vl53l0xTimeoutMicrosecondsToMclks(final_range_timeout_us,
- timeouts.final_range_vcsel_period_pclks);
-
- if (enables.pre_range)
- {
- final_range_timeout_mclks += timeouts.pre_range_mclks;
- }
-
- uint16_t temp = vl53l0xEncodeTimeout(final_range_timeout_mclks);
- vl53l0xWriteReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, temp);
-
- // set_sequence_step_timeout() end
-
- dev->measurement_timing_budget_us = budget_us; // store for internal reuse
- dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
- }
+ //COMMENTED FIRMWARE
+ // SequenceStepEnables enables;
+ // SequenceStepTimeouts timeouts;
+
+ // uint16_t const StartOverhead = 1320; // note that this is different than the value in get_
+ // uint16_t const EndOverhead = 960;
+ // uint16_t const MsrcOverhead = 660;
+ // uint16_t const TccOverhead = 590;
+ // uint16_t const DssOverhead = 690;
+ // uint16_t const PreRangeOverhead = 660;
+ // uint16_t const FinalRangeOverhead = 550;
+
+ // uint32_t const MinTimingBudget = 20000;
+
+ // if (budget_us < MinTimingBudget) { return false; }
+
+ // uint32_t used_budget_us = StartOverhead + EndOverhead;
+
+ // vl53l0xGetSequenceStepEnables(dev, &enables);
+ // vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
+
+ // if (enables.tcc)
+ // {
+ // used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
+ // }
+
+ // if (enables.dss)
+ // {
+ // used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
+ // }
+ // else if (enables.msrc)
+ // {
+ // used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
+ // }
+
+ // if (enables.pre_range)
+ // {
+ // used_budget_us += (timeouts.pre_range_us + PreRangeOverhead);
+ // }
+
+ // if (enables.final_range)
+ // {
+ // used_budget_us += FinalRangeOverhead;
+
+ // // "Note that the final range timeout is determined by the timing
+ // // budget and the sum of all other timeouts within the sequence.
+ // // If there is no room for the final range timeout, then an error
+ // // will be set. Otherwise the remaining time will be applied to
+ // // the final range."
+
+ // if (used_budget_us > budget_us)
+ // {
+ // // "Requested timeout too big."
+ // return false;
+ // }
+
+ // uint32_t final_range_timeout_us = budget_us - used_budget_us;
+
+ // // set_sequence_step_timeout() begin
+ // // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
+
+ // // "For the final range timeout, the pre-range timeout
+ // // must be added. To do this both final and pre-range
+ // // timeouts must be expressed in macro periods MClks
+ // // because they have different vcsel periods."
+
+ // uint16_t final_range_timeout_mclks =
+ // vl53l0xTimeoutMicrosecondsToMclks(final_range_timeout_us,
+ // timeouts.final_range_vcsel_period_pclks);
+
+ // if (enables.pre_range)
+ // {
+ // final_range_timeout_mclks += timeouts.pre_range_mclks;
+ // }
+
+ // uint16_t temp = vl53l0xEncodeTimeout(final_range_timeout_mclks);
+ // vl53l0xWriteReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, temp);
+
+ // // set_sequence_step_timeout() end
+
+ // dev->measurement_timing_budget_us = budget_us; // store for internal reuse
+ // dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
+ // }
return true;
}
@@ -539,36 +546,36 @@ uint32_t vl53l0xGetMeasurementTimingBudget(VL53L0xDev* dev)
// "Start and end overhead times always present"
uint32_t budget_us = StartOverhead + EndOverhead;
-
- vl53l0xGetSequenceStepEnables(dev, &enables);
- vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
-
- if (enables.tcc)
- {
- budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
- }
-
- if (enables.dss)
- {
- budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
- }
- else if (enables.msrc)
- {
- budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
- }
-
- if (enables.pre_range)
- {
- budget_us += (timeouts.pre_range_us + PreRangeOverhead);
- }
-
- if (enables.final_range)
- {
- budget_us += (timeouts.final_range_us + FinalRangeOverhead);
- }
-
- dev->measurement_timing_budget_us = budget_us; // store for internal reuse
- dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
+ //COMMENTED FIRMWARE
+ // vl53l0xGetSequenceStepEnables(dev, &enables);
+ // vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
+
+ // if (enables.tcc)
+ // {
+ // budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
+ // }
+
+ // if (enables.dss)
+ // {
+ // budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
+ // }
+ // else if (enables.msrc)
+ // {
+ // budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
+ // }
+
+ // if (enables.pre_range)
+ // {
+ // budget_us += (timeouts.pre_range_us + PreRangeOverhead);
+ // }
+
+ // if (enables.final_range)
+ // {
+ // budget_us += (timeouts.final_range_us + FinalRangeOverhead);
+ // }
+
+ // dev->measurement_timing_budget_us = budget_us; // store for internal reuse
+ // dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
return budget_us;
}
@@ -582,176 +589,177 @@ uint32_t vl53l0xGetMeasurementTimingBudget(VL53L0xDev* dev)
// based on VL53L0X_set_vcsel_pulse_period()
bool vl53l0xSetVcselPulsePeriod(VL53L0xDev* dev, vcselPeriodType type, uint8_t period_pclks)
{
- uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks);
-
- SequenceStepEnables enables;
- SequenceStepTimeouts timeouts;
-
- vl53l0xGetSequenceStepEnables(dev, &enables);
- vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
-
- // "Apply specific settings for the requested clock period"
- // "Re-calculate and apply timeouts, in macro periods"
-
- // "When the VCSEL period for the pre or final range is changed,
- // the corresponding timeout must be read from the device using
- // the current VCSEL period, then the new VCSEL period can be
- // applied. The timeout then must be written back to the device
- // using the new VCSEL period.
- //
- // For the MSRC timeout, the same applies - this timeout being
- // dependant on the pre-range vcsel period."
-
-
- if (type == VcselPeriodPreRange)
- {
- // "Set phase check limits"
- switch (period_pclks)
- {
- case 12:
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
- break;
-
- case 14:
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
- break;
-
- case 16:
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
- break;
-
- case 18:
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
- break;
-
- default:
- // invalid period
- return false;
- }
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
-
- // apply new VCSEL period
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
-
- // update timeouts
-
- // set_sequence_step_timeout() begin
- // (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE)
-
- uint16_t new_pre_range_timeout_mclks =
- vl53l0xTimeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks);
-
- uint16_t new_pre_range_timeout_encoded = vl53l0xEncodeTimeout(new_pre_range_timeout_mclks);
- vl53l0xWriteReg16Bit(dev, VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, new_pre_range_timeout_encoded);
-
- // set_sequence_step_timeout() end
-
- // set_sequence_step_timeout() begin
- // (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)
-
- uint16_t new_msrc_timeout_mclks =
- vl53l0xTimeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP,
- (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1));
-
- // set_sequence_step_timeout() end
- }
- else if (type == VcselPeriodFinalRange)
- {
- switch (period_pclks)
- {
- case 8:
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x30);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- break;
-
- case 10:
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- break;
-
- case 12:
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- break;
-
- case 14:
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- break;
-
- default:
- // invalid period
- return false;
- }
-
- // apply new VCSEL period
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
-
- // update timeouts
-
- // set_sequence_step_timeout() begin
- // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
-
- // "For the final range timeout, the pre-range timeout
- // must be added. To do this both final and pre-range
- // timeouts must be expressed in macro periods MClks
- // because they have different vcsel periods."
-
- uint16_t new_final_range_timeout_mclks =
- vl53l0xTimeoutMicrosecondsToMclks(timeouts.final_range_us, period_pclks);
-
- if (enables.pre_range)
- {
- new_final_range_timeout_mclks += timeouts.pre_range_mclks;
- }
-
- uint16_t new_final_range_timeout_encoded = vl53l0xEncodeTimeout(new_final_range_timeout_mclks);
- vl53l0xWriteReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, new_final_range_timeout_encoded);
-
- // set_sequence_step_timeout end
- }
- else
- {
- // invalid type
- return false;
- }
-
- // "Finally, the timing budget must be re-applied"
-
- vl53l0xSetMeasurementTimingBudget(dev, dev->measurement_timing_budget_us);
-
- // "Perform the phase calibration. This is needed after changing on vcsel period."
- // VL53L0X_perform_phase_calibration() begin
-
- uint8_t sequence_config = 0;
- i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x02);
+ //COMMENTED FIRMWARE
+ // uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks);
+
+ // SequenceStepEnables enables;
+ // SequenceStepTimeouts timeouts;
+
+ // vl53l0xGetSequenceStepEnables(dev, &enables);
+ // vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
+
+ // // "Apply specific settings for the requested clock period"
+ // // "Re-calculate and apply timeouts, in macro periods"
+
+ // // "When the VCSEL period for the pre or final range is changed,
+ // // the corresponding timeout must be read from the device using
+ // // the current VCSEL period, then the new VCSEL period can be
+ // // applied. The timeout then must be written back to the device
+ // // using the new VCSEL period.
+ // //
+ // // For the MSRC timeout, the same applies - this timeout being
+ // // dependant on the pre-range vcsel period."
+
+
+ // if (type == VcselPeriodPreRange)
+ // {
+ // // "Set phase check limits"
+ // switch (period_pclks)
+ // {
+ // case 12:
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
+ // break;
+
+ // case 14:
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
+ // break;
+
+ // case 16:
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
+ // break;
+
+ // case 18:
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
+ // break;
+
+ // default:
+ // // invalid period
+ // return false;
+ // }
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+
+ // // apply new VCSEL period
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
+
+ // // update timeouts
+
+ // // set_sequence_step_timeout() begin
+ // // (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE)
+
+ // uint16_t new_pre_range_timeout_mclks =
+ // vl53l0xTimeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks);
+
+ // uint16_t new_pre_range_timeout_encoded = vl53l0xEncodeTimeout(new_pre_range_timeout_mclks);
+ // vl53l0xWriteReg16Bit(dev, VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, new_pre_range_timeout_encoded);
+
+ // // set_sequence_step_timeout() end
+
+ // // set_sequence_step_timeout() begin
+ // // (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)
+
+ // uint16_t new_msrc_timeout_mclks =
+ // vl53l0xTimeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP,
+ // (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1));
+
+ // // set_sequence_step_timeout() end
+ // }
+ // else if (type == VcselPeriodFinalRange)
+ // {
+ // switch (period_pclks)
+ // {
+ // case 8:
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x30);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // break;
+
+ // case 10:
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // break;
+
+ // case 12:
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // break;
+
+ // case 14:
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // break;
+
+ // default:
+ // // invalid period
+ // return false;
+ // }
+
+ // // apply new VCSEL period
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
+
+ // // update timeouts
+
+ // // set_sequence_step_timeout() begin
+ // // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
+
+ // // "For the final range timeout, the pre-range timeout
+ // // must be added. To do this both final and pre-range
+ // // timeouts must be expressed in macro periods MClks
+ // // because they have different vcsel periods."
+
+ // uint16_t new_final_range_timeout_mclks =
+ // vl53l0xTimeoutMicrosecondsToMclks(timeouts.final_range_us, period_pclks);
+
+ // if (enables.pre_range)
+ // {
+ // new_final_range_timeout_mclks += timeouts.pre_range_mclks;
+ // }
+
+ // uint16_t new_final_range_timeout_encoded = vl53l0xEncodeTimeout(new_final_range_timeout_mclks);
+ // vl53l0xWriteReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, new_final_range_timeout_encoded);
+
+ // // set_sequence_step_timeout end
+ // }
+ // else
+ // {
+ // // invalid type
+ // return false;
+ // }
+
+ // // "Finally, the timing budget must be re-applied"
+
+ // vl53l0xSetMeasurementTimingBudget(dev, dev->measurement_timing_budget_us);
+
+ // // "Perform the phase calibration. This is needed after changing on vcsel period."
+ // // VL53L0X_perform_phase_calibration() begin
+
+ // uint8_t sequence_config = 0;
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x02);
bool ret = vl53l0xPerformSingleRefCalibration(dev, 0x0);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, sequence_config);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, sequence_config);
- // VL53L0X_perform_phase_calibration() end
+ // // VL53L0X_perform_phase_calibration() end
return ret;
}
@@ -760,19 +768,21 @@ bool vl53l0xSetVcselPulsePeriod(VL53L0xDev* dev, vcselPeriodType type, uint8_t p
// based on VL53L0X_get_vcsel_pulse_period()
uint8_t vl53l0xGetVcselPulsePeriod(VL53L0xDev* dev, vcselPeriodType type)
{
- if (type == VcselPeriodPreRange)
- {
- uint8_t temp = 0;
- i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD, &temp);
- return decodeVcselPeriod(temp);
- }
- else if (type == VcselPeriodFinalRange)
- {
- uint8_t temp = 0;
- i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD, &temp);
- return decodeVcselPeriod(temp);
- }
- else { return 255; }
+ //COMMENTED FIRMWARE
+ // if (type == VcselPeriodPreRange)
+ // {
+ // uint8_t temp = 0;
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD, &temp);
+ // return decodeVcselPeriod(temp);
+ // }
+ // else if (type == VcselPeriodFinalRange)
+ // {
+ // uint8_t temp = 0;
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD, &temp);
+ // return decodeVcselPeriod(temp);
+ // }
+ // else { return 255; }
+ return 255;
}
// Start continuous ranging measurements. If period_ms (optional) is 0 or not
@@ -783,51 +793,53 @@ uint8_t vl53l0xGetVcselPulsePeriod(VL53L0xDev* dev, vcselPeriodType type)
// based on VL53L0X_StartMeasurement()
void vl53l0xStartContinuous(VL53L0xDev* dev, uint32_t period_ms)
{
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, dev->stop_variable);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
-
- if (period_ms != 0)
- {
- // continuous timed mode
-
- // VL53L0X_SetInterMeasurementPeriodMilliSeconds() begin
-
- uint16_t osc_calibrate_val = vl53l0xReadReg16Bit(dev, VL53L0X_RA_OSC_CALIBRATE_VAL);
-
- if (osc_calibrate_val != 0)
- {
- period_ms *= osc_calibrate_val;
- }
-
- vl53l0xWriteReg32Bit(dev, VL53L0X_RA_SYSTEM_INTERMEASUREMENT_PERIOD, period_ms);
-
- // VL53L0X_SetInterMeasurementPeriodMilliSeconds() end
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x04); // VL53L0X_REG_SYSRANGE_MODE_TIMED
- }
- else
- {
- // continuous back-to-back mode
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
- }
+ //COMMENTED FIRMWARE
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, dev->stop_variable);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
+
+ // if (period_ms != 0)
+ // {
+ // // continuous timed mode
+
+ // // VL53L0X_SetInterMeasurementPeriodMilliSeconds() begin
+
+ // uint16_t osc_calibrate_val = vl53l0xReadReg16Bit(dev, VL53L0X_RA_OSC_CALIBRATE_VAL);
+
+ // if (osc_calibrate_val != 0)
+ // {
+ // period_ms *= osc_calibrate_val;
+ // }
+
+ // vl53l0xWriteReg32Bit(dev, VL53L0X_RA_SYSTEM_INTERMEASUREMENT_PERIOD, period_ms);
+
+ // // VL53L0X_SetInterMeasurementPeriodMilliSeconds() end
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x04); // VL53L0X_REG_SYSRANGE_MODE_TIMED
+ // }
+ // else
+ // {
+ // // continuous back-to-back mode
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
+ // }
}
// Stop continuous measurements
// based on VL53L0X_StopMeasurement()
void vl53l0xStopContinuous(VL53L0xDev* dev)
{
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ //COMMENTED FIRMWARE
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
}
// Returns a range reading in millimeters when continuous mode is active
@@ -835,28 +847,30 @@ void vl53l0xStopContinuous(VL53L0xDev* dev)
// single-shot range measurement)
uint16_t vl53l0xReadRangeContinuousMillimeters(VL53L0xDev* dev)
{
- startTimeout();
- uint8_t val = 0;
- while ((val & 0x07) == 0)
- {
- i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &val);
- if ((val & 0x07) == 0)
- {
- // Relaxation delay when polling interrupt
- vTaskDelay(M2T(1));
- }
- if (checkTimeoutExpired())
- {
- dev->did_timeout = true;
- return 65535;
- }
- }
+ //COMMENTED FIRMWARE
+ // startTimeout();
+ // uint8_t val = 0;
+ // while ((val & 0x07) == 0)
+ // {
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &val);
+ // if ((val & 0x07) == 0)
+ // {
+ // // Relaxation delay when polling interrupt
+ // vTaskDelay(M2T(1));
+ // }
+ // if (checkTimeoutExpired())
+ // {
+ // dev->did_timeout = true;
+ // return 65535;
+ // }
+ // }
// assumptions: Linearity Corrective Gain is 1000 (default);
// fractional ranging is not enabled
uint16_t range = vl53l0xReadReg16Bit(dev, VL53L0X_RA_RESULT_RANGE_STATUS + 10);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
+ //COMMENTED FIRMWARE
+ //i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
return range;
}
@@ -866,28 +880,29 @@ uint16_t vl53l0xReadRangeContinuousMillimeters(VL53L0xDev* dev)
// based on VL53L0X_PerformSingleRangingMeasurement()
uint16_t vl53l0xReadRangeSingleMillimeters(VL53L0xDev* dev)
{
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, dev->stop_variable);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
-
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01);
-
- // "Wait until start bit has been cleared"
- startTimeout();
- uint8_t val = 0x01;
- while (val & 0x01)
- {
- i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, &val);
- if (checkTimeoutExpired())
- {
- dev->did_timeout = true;
- return 65535;
- }
- }
+ //COMMENTED FIRMWARE
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, dev->stop_variable);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
+
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01);
+
+ // // "Wait until start bit has been cleared"
+ // startTimeout();
+ // uint8_t val = 0x01;
+ // while (val & 0x01)
+ // {
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, &val);
+ // if (checkTimeoutExpired())
+ // {
+ // dev->did_timeout = true;
+ // return 65535;
+ // }
+ // }
return vl53l0xReadRangeContinuousMillimeters(dev);
}
@@ -897,42 +912,43 @@ uint16_t vl53l0xReadRangeSingleMillimeters(VL53L0xDev* dev)
// but only gets reference SPAD count and type
bool vl53l0xGetSpadInfo(VL53L0xDev* dev, uint8_t * count, bool * type_is_aperture)
{
- uint8_t tmp;
+ //COMMENTED FIRMWARE
+ // uint8_t tmp;
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x06);
- i2cdevWriteBit(dev->I2Cx, dev->devAddr, 0x83, 2, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x07);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x81, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x06);
+ // i2cdevWriteBit(dev->I2Cx, dev->devAddr, 0x83, 2, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x07);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x81, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x94, 0x6b);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x83, 0x00);
- startTimeout();
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x94, 0x6b);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x83, 0x00);
+ // startTimeout();
- uint8_t val = 0x00;
- while (val == 0x00) {
- i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x83, &val);
- if (checkTimeoutExpired()) { return false; }
- };
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x83, 0x01);
- i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x92, &tmp);
+ // uint8_t val = 0x00;
+ // while (val == 0x00) {
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x83, &val);
+ // if (checkTimeoutExpired()) { return false; }
+ // };
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x83, 0x01);
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x92, &tmp);
- *count = tmp & 0x7f;
- *type_is_aperture = (tmp >> 7) & 0x01;
+ // *count = tmp & 0x7f;
+ // *type_is_aperture = (tmp >> 7) & 0x01;
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x81, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x06);
- i2cdevWriteBit(dev->I2Cx, dev->devAddr, 0x83, 2, 0);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x81, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x06);
+ // i2cdevWriteBit(dev->I2Cx, dev->devAddr, 0x83, 2, 0);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
return true;
}
@@ -941,14 +957,15 @@ bool vl53l0xGetSpadInfo(VL53L0xDev* dev, uint8_t * count, bool * type_is_apertur
// based on VL53L0X_GetSequenceStepEnables()
void vl53l0xGetSequenceStepEnables(VL53L0xDev* dev, SequenceStepEnables * enables)
{
- uint8_t sequence_config = 0;
- i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
-
- enables->tcc = (sequence_config >> 4) & 0x1;
- enables->dss = (sequence_config >> 3) & 0x1;
- enables->msrc = (sequence_config >> 2) & 0x1;
- enables->pre_range = (sequence_config >> 6) & 0x1;
- enables->final_range = (sequence_config >> 7) & 0x1;
+ //COMMENTED FIRMWARE
+ // uint8_t sequence_config = 0;
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
+
+ // enables->tcc = (sequence_config >> 4) & 0x1;
+ // enables->dss = (sequence_config >> 3) & 0x1;
+ // enables->msrc = (sequence_config >> 2) & 0x1;
+ // enables->pre_range = (sequence_config >> 6) & 0x1;
+ // enables->final_range = (sequence_config >> 7) & 0x1;
}
// Get sequence step timeouts
@@ -957,34 +974,35 @@ void vl53l0xGetSequenceStepEnables(VL53L0xDev* dev, SequenceStepEnables * enable
// intermediate values
void vl53l0xGetSequenceStepTimeouts(VL53L0xDev* dev, SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts)
{
- timeouts->pre_range_vcsel_period_pclks = vl53l0xGetVcselPulsePeriod(dev, VcselPeriodPreRange);
-
- uint8_t temp = 0;
- i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP, &temp);
- timeouts->msrc_dss_tcc_mclks = temp + 1;
- timeouts->msrc_dss_tcc_us =
- vl53l0xTimeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks,
- timeouts->pre_range_vcsel_period_pclks);
-
- uint16_t pre_range_encoded = vl53l0xReadReg16Bit(dev, VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
- timeouts->pre_range_mclks = vl53l0xDecodeTimeout(pre_range_encoded);
- timeouts->pre_range_us =
- vl53l0xTimeoutMclksToMicroseconds(timeouts->pre_range_mclks,
- timeouts->pre_range_vcsel_period_pclks);
-
- timeouts->final_range_vcsel_period_pclks = vl53l0xGetVcselPulsePeriod(dev, VcselPeriodFinalRange);
-
- uint16_t final_range_encoded = vl53l0xReadReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI);
- timeouts->final_range_mclks = vl53l0xDecodeTimeout(final_range_encoded);
-
- if (enables->pre_range)
- {
- timeouts->final_range_mclks -= timeouts->pre_range_mclks;
- }
-
- timeouts->final_range_us =
- vl53l0xTimeoutMclksToMicroseconds(timeouts->final_range_mclks,
- timeouts->final_range_vcsel_period_pclks);
+ //COMMENTED FIRMWARE
+ // timeouts->pre_range_vcsel_period_pclks = vl53l0xGetVcselPulsePeriod(dev, VcselPeriodPreRange);
+
+ // uint8_t temp = 0;
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP, &temp);
+ // timeouts->msrc_dss_tcc_mclks = temp + 1;
+ // timeouts->msrc_dss_tcc_us =
+ // vl53l0xTimeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks,
+ // timeouts->pre_range_vcsel_period_pclks);
+
+ // uint16_t pre_range_encoded = vl53l0xReadReg16Bit(dev, VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
+ // timeouts->pre_range_mclks = vl53l0xDecodeTimeout(pre_range_encoded);
+ // timeouts->pre_range_us =
+ // vl53l0xTimeoutMclksToMicroseconds(timeouts->pre_range_mclks,
+ // timeouts->pre_range_vcsel_period_pclks);
+
+ // timeouts->final_range_vcsel_period_pclks = vl53l0xGetVcselPulsePeriod(dev, VcselPeriodFinalRange);
+
+ // uint16_t final_range_encoded = vl53l0xReadReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI);
+ // timeouts->final_range_mclks = vl53l0xDecodeTimeout(final_range_encoded);
+
+ // if (enables->pre_range)
+ // {
+ // timeouts->final_range_mclks -= timeouts->pre_range_mclks;
+ // }
+
+ // timeouts->final_range_us =
+ // vl53l0xTimeoutMclksToMicroseconds(timeouts->final_range_mclks,
+ // timeouts->final_range_vcsel_period_pclks);
}
// Decode sequence step timeout in MCLKs from register value
@@ -1045,19 +1063,20 @@ uint32_t vl53l0xTimeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t v
// based on VL53L0X_perform_single_ref_calibration()
bool vl53l0xPerformSingleRefCalibration(VL53L0xDev* dev, uint8_t vhv_init_byte)
{
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
+ //COMMENTED FIRMWARE
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
- startTimeout();
- uint8_t temp = 0x00;
- while ((temp & 0x07) == 0)
- {
- i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &temp);
- if (checkTimeoutExpired()) { return false; }
- }
+ // startTimeout();
+ // uint8_t temp = 0x00;
+ // while ((temp & 0x07) == 0)
+ // {
+ // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &temp);
+ // if (checkTimeoutExpired()) { return false; }
+ // }
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
- i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x00);
+ // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x00);
return true;
}
@@ -1065,7 +1084,8 @@ bool vl53l0xPerformSingleRefCalibration(VL53L0xDev* dev, uint8_t vhv_init_byte)
uint16_t vl53l0xReadReg16Bit(VL53L0xDev* dev, uint8_t reg)
{
uint8_t buffer[2] = {};
- i2cdevReadReg8(dev->I2Cx, dev->devAddr, reg, 2, (uint8_t *)&buffer);
+ //COMMENTED FIRMWARE
+ // i2cdevReadReg8(dev->I2Cx, dev->devAddr, reg, 2, (uint8_t *)&buffer);
return ((uint16_t)(buffer[0]) << 8) | buffer[1];
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l1x.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l1x.c
index ace2d8a2b..e8b39e0f1 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l1x.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l1x.c
@@ -55,25 +55,25 @@ static int nextI2CAddress = VL53L1X_DEFAULT_ADDRESS+8;
bool vl53l1xInit(VL53L1_Dev_t *pdev, I2C_Dev *I2Cx)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
+//COMMENTED FIRMWARE
+// pdev->I2Cx = I2Cx;
+// pdev->devAddr = VL53L1X_DEFAULT_ADDRESS;
- pdev->I2Cx = I2Cx;
- pdev->devAddr = VL53L1X_DEFAULT_ADDRESS;
+// /* Move initialized sensor to a new I2C address */
+// int newAddress;
- /* Move initialized sensor to a new I2C address */
- int newAddress;
+// taskENTER_CRITICAL();
+// newAddress = nextI2CAddress++;
+// taskEXIT_CRITICAL();
- taskENTER_CRITICAL();
- newAddress = nextI2CAddress++;
- taskEXIT_CRITICAL();
+// vl53l1xSetI2CAddress(pdev, newAddress);
- vl53l1xSetI2CAddress(pdev, newAddress);
+// status = VL53L1_DataInit(pdev);
- status = VL53L1_DataInit(pdev);
-
- if (status == VL53L1_ERROR_NONE)
- {
- status = VL53L1_StaticInit(pdev);
- }
+// if (status == VL53L1_ERROR_NONE)
+// {
+// status = VL53L1_StaticInit(pdev);
+// }
return status == VL53L1_ERROR_NONE;
}
@@ -82,8 +82,8 @@ bool vl53l1xTestConnection(VL53L1_Dev_t* pdev)
{
VL53L1_DeviceInfo_t info;
VL53L1_Error status = VL53L1_ERROR_NONE;
-
- status = VL53L1_GetDeviceInfo(pdev, &info);
+ //COMMENTED FIRMWARE
+// status = VL53L1_GetDeviceInfo(pdev, &info);
return status == VL53L1_ERROR_NONE;
}
@@ -96,9 +96,9 @@ bool vl53l1xTestConnection(VL53L1_Dev_t* pdev)
VL53L1_Error vl53l1xSetI2CAddress(VL53L1_Dev_t* pdev, uint8_t address)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
-
- status = VL53L1_SetDeviceAddress(pdev, address);
- pdev->devAddr = address;
+//COMMENTED FIRMWARE
+// status = VL53L1_SetDeviceAddress(pdev, address);
+// pdev->devAddr = address;
return status;
}
@@ -114,11 +114,11 @@ VL53L1_Error VL53L1_WriteMulti(
uint32_t count)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
-
- if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, count, pdata))
- {
- status = VL53L1_ERROR_CONTROL_INTERFACE;
- }
+//COMMENTED FIRMWARE
+// if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, count, pdata))
+// {
+// status = VL53L1_ERROR_CONTROL_INTERFACE;
+// }
return status;
}
@@ -130,11 +130,11 @@ VL53L1_Error VL53L1_ReadMulti(
uint32_t count)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
-
- if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, count, pdata))
- {
- status = VL53L1_ERROR_CONTROL_INTERFACE;
- }
+ //COMMENTED FIRMWARE
+// if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, count, pdata))
+// {
+// status = VL53L1_ERROR_CONTROL_INTERFACE;
+// }
return status;
}
@@ -146,11 +146,11 @@ VL53L1_Error VL53L1_WrByte(
uint8_t data)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
-
- if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 1, &data))
- {
- status = VL53L1_ERROR_CONTROL_INTERFACE;
- }
+//COMMENTED FIRMWARE
+ // if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 1, &data))
+ // {
+ // status = VL53L1_ERROR_CONTROL_INTERFACE;
+ // }
return status;
}
@@ -162,11 +162,11 @@ VL53L1_Error VL53L1_WrWord(
uint16_t data)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
-
- if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 2, (uint8_t *)&data))
- {
- status = VL53L1_ERROR_CONTROL_INTERFACE;
- }
+ //COMMENTED FIRMWARE
+// if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 2, (uint8_t *)&data))
+// {
+// status = VL53L1_ERROR_CONTROL_INTERFACE;
+// }
return status;
}
@@ -178,11 +178,11 @@ VL53L1_Error VL53L1_WrDWord(
uint32_t data)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
-
- if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 4, (uint8_t *)&data))
- {
- status = VL53L1_ERROR_CONTROL_INTERFACE;
- }
+ //COMMENTED FIRMWARE
+// if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 4, (uint8_t *)&data))
+// {
+// status = VL53L1_ERROR_CONTROL_INTERFACE;
+// }
return status;
}
@@ -194,13 +194,14 @@ VL53L1_Error VL53L1_RdByte(
uint8_t *pdata)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- static uint8_t r8data;
+ //COMMENTED FIRMWARE
+// static uint8_t r8data;
- if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 1, &r8data))
- {
- status = VL53L1_ERROR_CONTROL_INTERFACE;
- }
- *pdata = r8data;
+// if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 1, &r8data))
+// {
+// status = VL53L1_ERROR_CONTROL_INTERFACE;
+// }
+// *pdata = r8data;
return status;
}
@@ -212,13 +213,14 @@ VL53L1_Error VL53L1_RdWord(
uint16_t *pdata)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- static uint16_t r16data;
-
- if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 2, (uint8_t *)&r16data))
- {
- status = VL53L1_ERROR_CONTROL_INTERFACE;
- }
- *pdata = r16data;
+ //COMMENTED FIRMWARE
+// static uint16_t r16data;
+
+// if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 2, (uint8_t *)&r16data))
+// {
+// status = VL53L1_ERROR_CONTROL_INTERFACE;
+// }
+// *pdata = r16data;
return status;
}
@@ -230,13 +232,14 @@ VL53L1_Error VL53L1_RdDWord(
uint32_t *pdata)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- static uint32_t r32data;
+ //COMMENTED FIRMWARE
+// static uint32_t r32data;
- if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 4, (uint8_t *)&r32data))
- {
- status = VL53L1_ERROR_CONTROL_INTERFACE;
- }
- *pdata = r32data;
+// if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 4, (uint8_t *)&r32data))
+// {
+// status = VL53L1_ERROR_CONTROL_INTERFACE;
+// }
+// *pdata = r32data;
return status;
}
@@ -250,14 +253,15 @@ VL53L1_Error VL53L1_WaitUs(
int32_t wait_us)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- uint32_t delay_ms = (wait_us + 900) / 1000;
+ //COMMENTED FIRMWARE
+ // uint32_t delay_ms = (wait_us + 900) / 1000;
- if(delay_ms == 0)
- {
- delay_ms = 1;
- }
+ // if(delay_ms == 0)
+ // {
+ // delay_ms = 1;
+ // }
- vTaskDelay(M2T(delay_ms));
+ // vTaskDelay(M2T(delay_ms));
return status;
}
@@ -267,7 +271,8 @@ VL53L1_Error VL53L1_WaitMs(
VL53L1_Dev_t *pdev,
int32_t wait_ms)
{
- vTaskDelay(M2T(wait_ms));
+ //COMMENTED FIRMWARE
+ //vTaskDelay(M2T(wait_ms));
return VL53L1_ERROR_NONE;
}
@@ -280,7 +285,8 @@ VL53L1_Error VL53L1_GetTickCount(
uint32_t *ptick_count_ms)
{
/* Returns current tick count in [ms] */
- *ptick_count_ms = xTaskGetTickCount();
+ //COMMENTED FIRMWARE
+ //*ptick_count_ms = xTaskGetTickCount();
return VL53L1_ERROR_NONE;
}
@@ -305,72 +311,73 @@ VL53L1_Error VL53L1_WaitValueMaskEx(
* poll_delay_ms);
*/
- VL53L1_Error status = VL53L1_ERROR_NONE;
- uint32_t start_time_ms = 0;
- uint32_t current_time_ms = 0;
- uint8_t byte_value = 0;
- uint8_t found = 0;
-#ifdef VL53L1_LOG_ENABLE
- uint32_t trace_functions = 0;
-#endif
-
- SUPPRESS_UNUSED_WARNING(poll_delay_ms);
-
-#ifdef VL53L1_LOG_ENABLE
- /* look up register name */
- VL53L1_get_register_name(
- index,
- register_name);
-
- /* Output to I2C logger for FMT/DFT */
- trace_i2c("WaitValueMaskEx(%5d, %s, 0x%02X, 0x%02X, %5d);\n",
- timeout_ms, register_name, value, mask, poll_delay_ms);
-#endif // VL53L1_LOG_ENABLE
-
- /* calculate time limit in absolute time */
-
- VL53L1_GetTickCount(&start_time_ms);
- pdev->new_data_ready_poll_duration_ms = 0;
-
- /* remember current trace functions and temporarily disable
- * function logging
- */
-
-#ifdef VL53L1_LOG_ENABLE
- trace_functions = _LOG_GET_TRACE_FUNCTIONS();
-#endif
-
- /* wait until value is found, timeout reached on error occurred */
-
- while ((status == VL53L1_ERROR_NONE) &&
- (pdev->new_data_ready_poll_duration_ms < timeout_ms) &&
- (found == 0))
- {
- status = VL53L1_RdByte(
- pdev,
- index,
- &byte_value);
-
- if ((byte_value & mask) == value)
- {
- found = 1;
- }
-
- if (status == VL53L1_ERROR_NONE &&
- found == 0 &&
- poll_delay_ms > 0)
- status = VL53L1_WaitMs(
- pdev,
- poll_delay_ms);
-
- /* Update polling time (Compare difference rather than absolute to
- negate 32bit wrap around issue) */
- VL53L1_GetTickCount(¤t_time_ms);
- pdev->new_data_ready_poll_duration_ms = current_time_ms - start_time_ms;
- }
-
- if (found == 0 && status == VL53L1_ERROR_NONE)
- status = VL53L1_ERROR_TIME_OUT;
+ VL53L1_Error status = VL53L1_ERROR_NONE;
+ //COMMENTED FIRMWARE
+// uint32_t start_time_ms = 0;
+// uint32_t current_time_ms = 0;
+// uint8_t byte_value = 0;
+// uint8_t found = 0;
+// #ifdef VL53L1_LOG_ENABLE
+// uint32_t trace_functions = 0;
+// #endif
+
+// SUPPRESS_UNUSED_WARNING(poll_delay_ms);
+
+// #ifdef VL53L1_LOG_ENABLE
+// /* look up register name */
+// VL53L1_get_register_name(
+// index,
+// register_name);
+
+// /* Output to I2C logger for FMT/DFT */
+// trace_i2c("WaitValueMaskEx(%5d, %s, 0x%02X, 0x%02X, %5d);\n",
+// timeout_ms, register_name, value, mask, poll_delay_ms);
+// #endif // VL53L1_LOG_ENABLE
+
+// /* calculate time limit in absolute time */
+
+// VL53L1_GetTickCount(&start_time_ms);
+// pdev->new_data_ready_poll_duration_ms = 0;
+
+// /* remember current trace functions and temporarily disable
+// * function logging
+// */
+
+// #ifdef VL53L1_LOG_ENABLE
+// trace_functions = _LOG_GET_TRACE_FUNCTIONS();
+// #endif
+
+// /* wait until value is found, timeout reached on error occurred */
+
+// while ((status == VL53L1_ERROR_NONE) &&
+// (pdev->new_data_ready_poll_duration_ms < timeout_ms) &&
+// (found == 0))
+// {
+// status = VL53L1_RdByte(
+// pdev,
+// index,
+// &byte_value);
+
+// if ((byte_value & mask) == value)
+// {
+// found = 1;
+// }
+
+// if (status == VL53L1_ERROR_NONE &&
+// found == 0 &&
+// poll_delay_ms > 0)
+// status = VL53L1_WaitMs(
+// pdev,
+// poll_delay_ms);
+
+// /* Update polling time (Compare difference rather than absolute to
+// negate 32bit wrap around issue) */
+// VL53L1_GetTickCount(¤t_time_ms);
+// pdev->new_data_ready_poll_duration_ms = current_time_ms - start_time_ms;
+// }
+
+// if (found == 0 && status == VL53L1_ERROR_NONE)
+// status = VL53L1_ERROR_TIME_OUT;
return status;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/watchdog.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/watchdog.c
index 2bb339487..46089f6fe 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/watchdog.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/watchdog.c
@@ -33,12 +33,12 @@
bool watchdogNormalStartTest(void)
{
bool wasNormalStart = true;
-
- if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST)) {
- RCC_ClearFlag();
- wasNormalStart = false;
- DEBUG_PRINT("The system resumed after watchdog timeout [WARNING]\n");
- }
+ //COMMENTED FIRMWARE
+ // if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST)) {
+ // RCC_ClearFlag();
+ // wasNormalStart = false;
+ // DEBUG_PRINT("The system resumed after watchdog timeout [WARNING]\n");
+ // }
return wasNormalStart;
}
@@ -46,31 +46,32 @@ bool watchdogNormalStartTest(void)
void watchdogInit(void)
{
- IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
+ //COMMENTED FIRMWARE
+ // IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
- // The watchdog uses the LSI oscillator for checking the timeout. The LSI
- // oscillator frequency is not very exact and the range is fairly large
- // and also differs between the Crazyflie 1.0 and 2.0:
- // MIN TYP MAX
- // Crazyflie 1.0 30 40 60 (in kHz)
- // Crazyflie 2.0 17 32 47 (in kHz)
- //
- IWDG_SetPrescaler(IWDG_Prescaler_32);
- // Divide the clock with 32 which gives
- // an interval of 17kHz/32 (CF2 min) to 60kHz/32 (CF1 max) =>
- // 1875 Hz to 531 Hz for the watchdog timer.
- //
- // The goal timeout is >100 ms, but it's acceptable
- // that the max timeout is a bit higher. Scaling the
- // reload counter for the fastest LSI then gives a
- // timeout of 100ms, which in turn gives a timeout
- // of 353ms for the slowest LSI. So the watchdog timeout
- // will be between 100ms and 353ms on all platforms.
- //
- // At prescaler 32 each bit is 1 ms this gives:
- // 1875 Hz * 0.1 s / 1 => 188
- IWDG_SetReload(188);
+ // // The watchdog uses the LSI oscillator for checking the timeout. The LSI
+ // // oscillator frequency is not very exact and the range is fairly large
+ // // and also differs between the Crazyflie 1.0 and 2.0:
+ // // MIN TYP MAX
+ // // Crazyflie 1.0 30 40 60 (in kHz)
+ // // Crazyflie 2.0 17 32 47 (in kHz)
+ // //
+ // IWDG_SetPrescaler(IWDG_Prescaler_32);
+ // // Divide the clock with 32 which gives
+ // // an interval of 17kHz/32 (CF2 min) to 60kHz/32 (CF1 max) =>
+ // // 1875 Hz to 531 Hz for the watchdog timer.
+ // //
+ // // The goal timeout is >100 ms, but it's acceptable
+ // // that the max timeout is a bit higher. Scaling the
+ // // reload counter for the fastest LSI then gives a
+ // // timeout of 100ms, which in turn gives a timeout
+ // // of 353ms for the slowest LSI. So the watchdog timeout
+ // // will be between 100ms and 353ms on all platforms.
+ // //
+ // // At prescaler 32 each bit is 1 ms this gives:
+ // // 1875 Hz * 0.1 s / 1 => 188
+ // IWDG_SetReload(188);
- watchdogReset();
- IWDG_Enable();
+ // watchdogReset();
+ // IWDG_Enable();
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ws2812_cf2.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ws2812_cf2.c
index 34e5bd7b9..c3f217631 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ws2812_cf2.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ws2812_cf2.c
@@ -64,107 +64,109 @@ static union {
void ws2812Init(void)
{
- uint16_t PrescalerValue;
-
- RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
- /* GPIOB Configuration: TIM3 Channel 1 as alternate function push-pull */
- // Configure the GPIO PB4 for the timer output
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- //Map timer to alternate functions
- GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3);
-
- /* Compute the prescaler value */
- PrescalerValue = 0;
- /* Time base configuration */
- TIM_TimeBaseStructure.TIM_Period = (105 - 1); // 800kHz
- TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
- TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
- TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
-
- /* PWM1 Mode configuration: Channel1 */
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
- TIM_OCInitStructure.TIM_Pulse = 0;
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
- TIM_OC2Init(TIM3, &TIM_OCInitStructure);
-
-// TIM_Cmd(TIM3, ENABLE); // Go!!!
- TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
- TIM_CtrlPWMOutputs(TIM3, ENABLE); // enable Timer 3
-
- /* configure DMA */
- /* DMA clock enable */
- RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
-
- /* DMA1 Channel5 Config TM */
- DMA_DeInit(DMA1_Stream5);
-
- ASSERT_DMA_SAFE(led_dma.buffer);
- // USART TX DMA Channel Config
- DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR2;
- DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)led_dma.buffer; // this is the buffer memory
- DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
- DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
- DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
- DMA_InitStructure.DMA_BufferSize = 0;
- DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
- DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
- DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
- DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
- DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
- DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
- DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
- DMA_InitStructure.DMA_Channel = DMA_Channel_5;
- DMA_Init(DMA1_Stream5, &DMA_InitStructure);
-
- NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream5_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_LOW_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- vSemaphoreCreateBinary(allLedDone);
-
- DMA_ITConfig(DMA1_Stream5, DMA_IT_TC, ENABLE);
- DMA_ITConfig(DMA1_Stream5, DMA_IT_HT, ENABLE);
-
- /* TIM3 CC2 DMA Request enable */
- TIM_DMACmd(TIM3, TIM_DMA_CC2, ENABLE);
+ //COMMENTED FIRMWARE
+// uint16_t PrescalerValue;
+
+// RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
+// RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+// /* GPIOB Configuration: TIM3 Channel 1 as alternate function push-pull */
+// // Configure the GPIO PB4 for the timer output
+// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+// GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
+// GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
+// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
+// GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+// //Map timer to alternate functions
+// GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3);
+
+// /* Compute the prescaler value */
+// PrescalerValue = 0;
+// /* Time base configuration */
+// TIM_TimeBaseStructure.TIM_Period = (105 - 1); // 800kHz
+// TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
+// TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+// TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+// TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+// TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
+
+// /* PWM1 Mode configuration: Channel1 */
+// TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+// TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+// TIM_OCInitStructure.TIM_Pulse = 0;
+// TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+// TIM_OC2Init(TIM3, &TIM_OCInitStructure);
+
+// // TIM_Cmd(TIM3, ENABLE); // Go!!!
+// TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
+// TIM_CtrlPWMOutputs(TIM3, ENABLE); // enable Timer 3
+
+// /* configure DMA */
+// /* DMA clock enable */
+// RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
+
+// /* DMA1 Channel5 Config TM */
+// DMA_DeInit(DMA1_Stream5);
+
+// ASSERT_DMA_SAFE(led_dma.buffer);
+// // USART TX DMA Channel Config
+// DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR2;
+// DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)led_dma.buffer; // this is the buffer memory
+// DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+// DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+// DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
+// DMA_InitStructure.DMA_BufferSize = 0;
+// DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+// DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
+// DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+// DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+// DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
+// DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
+// DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
+// DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
+// DMA_InitStructure.DMA_Channel = DMA_Channel_5;
+// DMA_Init(DMA1_Stream5, &DMA_InitStructure);
+
+// NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream5_IRQn;
+// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_LOW_PRI;
+// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+// NVIC_Init(&NVIC_InitStructure);
+
+// vSemaphoreCreateBinary(allLedDone);
+
+// DMA_ITConfig(DMA1_Stream5, DMA_IT_TC, ENABLE);
+// DMA_ITConfig(DMA1_Stream5, DMA_IT_HT, ENABLE);
+
+// /* TIM3 CC2 DMA Request enable */
+// TIM_DMACmd(TIM3, TIM_DMA_CC2, ENABLE);
}
static void fillLed(uint16_t *buffer, uint8_t *color)
{
- int i;
-
- for(i=0; i<8; i++) // GREEN data
- {
- buffer[i] = ((color[1]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
- }
- for(i=0; i<8; i++) // RED
- {
- buffer[8+i] = ((color[0]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
- }
- for(i=0; i<8; i++) // BLUE
- {
- buffer[16+i] = ((color[2]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
- }
+ //COMMENTED FIRMWARE
+ // int i;
+
+ // for(i=0; i<8; i++) // GREEN data
+ // {
+ // buffer[i] = ((color[1]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
+ // }
+ // for(i=0; i<8; i++) // RED
+ // {
+ // buffer[8+i] = ((color[0]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
+ // }
+ // for(i=0; i<8; i++) // BLUE
+ // {
+ // buffer[16+i] = ((color[2]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
+ // }
}
static int current_led = 0;
@@ -173,80 +175,83 @@ static uint8_t (*color_led)[3] = NULL;
void ws2812Send(uint8_t (*color)[3], uint16_t len)
{
- int i;
- if(len<1) return;
-
- //Wait for previous transfer to be finished
- xSemaphoreTake(allLedDone, portMAX_DELAY);
-
- // Set interrupt context ...
- current_led = 0;
- total_led = len;
- color_led = color;
-
- for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
- if (current_led<total_led)
- fillLed(led_dma.begin+(24*i), color_led[current_led]);
- else
- bzero(led_dma.begin+(24*i), sizeof(led_dma.begin));
- }
-
- for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
- if (current_led<total_led)
- fillLed(led_dma.end+(24*i), color_led[current_led]);
- else
- bzero(led_dma.end+(24*i), sizeof(led_dma.end));
- }
-
- DMA1_Stream5->NDTR = sizeof(led_dma.buffer) / sizeof(led_dma.buffer[0]); // load number of bytes to be transferred
- DMA_Cmd(DMA1_Stream5, ENABLE); // enable DMA channel 2
- TIM_Cmd(TIM3, ENABLE); // Go!!!
+ //COMMENTED FIRMWARE
+ // int i;
+ // if(len<1) return;
+
+ // //Wait for previous transfer to be finished
+ // xSemaphoreTake(allLedDone, portMAX_DELAY);
+
+ // // Set interrupt context ...
+ // current_led = 0;
+ // total_led = len;
+ // color_led = color;
+
+ // for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
+ // if (current_led<total_led)
+ // fillLed(led_dma.begin+(24*i), color_led[current_led]);
+ // else
+ // bzero(led_dma.begin+(24*i), sizeof(led_dma.begin));
+ // }
+
+ // for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
+ // if (current_led<total_led)
+ // fillLed(led_dma.end+(24*i), color_led[current_led]);
+ // else
+ // bzero(led_dma.end+(24*i), sizeof(led_dma.end));
+ // }
+
+ // DMA1_Stream5->NDTR = sizeof(led_dma.buffer) / sizeof(led_dma.buffer[0]); // load number of bytes to be transferred
+ // DMA_Cmd(DMA1_Stream5, ENABLE); // enable DMA channel 2
+ // TIM_Cmd(TIM3, ENABLE); // Go!!!
}
void ws2812DmaIsr(void)
{
- portBASE_TYPE xHigherPriorityTaskWoken;
- uint16_t * buffer;
- int i;
-
- if (total_led == 0)
- {
- TIM_Cmd(TIM3, DISABLE);
- DMA_Cmd(DMA1_Stream5, DISABLE);
- }
-
- if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_HTIF5))
- {
- DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_HTIF5);
- buffer = led_dma.begin;
- }
-
- if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_TCIF5))
- {
- DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_TCIF5);
- buffer = led_dma.end;
- }
-
- for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
- if (current_led<total_led)
- fillLed(buffer+(24*i), color_led[current_led]);
- else
- bzero(buffer+(24*i), sizeof(led_dma.end));
- }
-
- if (current_led >= total_led+2) {
- xSemaphoreGiveFromISR(allLedDone, &xHigherPriorityTaskWoken);
-
- TIM_Cmd(TIM3, DISABLE); // disable Timer 3
- DMA_Cmd(DMA1_Stream5, DISABLE); // disable DMA stream4
-
- total_led = 0;
- }
+ //COMMENTED FIRMWARE
+ // portBASE_TYPE xHigherPriorityTaskWoken;
+ // uint16_t * buffer;
+ // int i;
+
+ // if (total_led == 0)
+ // {
+ // TIM_Cmd(TIM3, DISABLE);
+ // DMA_Cmd(DMA1_Stream5, DISABLE);
+ // }
+
+ // if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_HTIF5))
+ // {
+ // DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_HTIF5);
+ // buffer = led_dma.begin;
+ // }
+
+ // if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_TCIF5))
+ // {
+ // DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_TCIF5);
+ // buffer = led_dma.end;
+ // }
+
+ // for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
+ // if (current_led<total_led)
+ // fillLed(buffer+(24*i), color_led[current_led]);
+ // else
+ // bzero(buffer+(24*i), sizeof(led_dma.end));
+ // }
+
+ // if (current_led >= total_led+2) {
+ // xSemaphoreGiveFromISR(allLedDone, &xHigherPriorityTaskWoken);
+
+ // TIM_Cmd(TIM3, DISABLE); // disable Timer 3
+ // DMA_Cmd(DMA1_Stream5, DISABLE); // disable DMA stream4
+
+ // total_led = 0;
+ // }
}
#ifndef USDDECK_USE_ALT_PINS_AND_SPI
void __attribute__((used)) DMA1_Stream5_IRQHandler(void)
{
- ws2812DmaIsr();
+ //COMMENTED FIRMWARE
+ //ws2812DmaIsr();
}
#endif
--
GitLab