diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_analog.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_analog.c
index 27fed248975ae21d496062d26fda249f800d5ee3..6296ff178a10349455c67c0b717ced046c285867 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_analog.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_analog.c
@@ -68,13 +68,14 @@ void adcInit(void)
static uint16_t analogReadChannel(uint8_t channel)
{
/* According to datasheet, minimum sampling time for 12-bit conversion is 15 cycles. */
- ADC_RegularChannelConfig(ADC2, channel, 1, ADC_SampleTime_15Cycles);
+ //COMMENTED FIRMWARE
+ // ADC_RegularChannelConfig(ADC2, channel, 1, ADC_SampleTime_15Cycles);
- /* Start the conversion */
- ADC_SoftwareStartConv(ADC2);
+ // /* Start the conversion */
+ // ADC_SoftwareStartConv(ADC2);
- /* Wait until conversion completion */
- while(ADC_GetFlagStatus(ADC2, ADC_FLAG_EOC) == RESET);
+ // /* Wait until conversion completion */
+ // while(ADC_GetFlagStatus(ADC2, ADC_FLAG_EOC) == RESET);
/* Get the conversion value */
return ADC_GetConversionValue(ADC2);
@@ -82,25 +83,26 @@ static uint16_t analogReadChannel(uint8_t channel)
uint16_t analogRead(const deckPin_t pin)
{
- assert_param(deckGPIOMapping[pin.id].adcCh > -1);
+ //COMMENTED FIRMWARE
+ // assert_param(deckGPIOMapping[pin.id].adcCh > -1);
- /* Now set the GPIO pin to analog mode. */
+ // /* Now set the GPIO pin to analog mode. */
- /* Enable clock for the peripheral of the pin.*/
- RCC_AHB1PeriphClockCmd(deckGPIOMapping[pin.id].periph, ENABLE);
+ // /* Enable clock for the peripheral of the pin.*/
+ // RCC_AHB1PeriphClockCmd(deckGPIOMapping[pin.id].periph, ENABLE);
- /* Populate structure with RESET values. */
- GPIO_InitTypeDef GPIO_InitStructure;
- GPIO_StructInit(&GPIO_InitStructure);
+ // /* Populate structure with RESET values. */
+ // GPIO_InitTypeDef GPIO_InitStructure;
+ // GPIO_StructInit(&GPIO_InitStructure);
- /* Initialise the GPIO pin to analog mode. */
- GPIO_InitStructure.GPIO_Pin = deckGPIOMapping[pin.id].pin;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ // /* Initialise the GPIO pin to analog mode. */
+ // GPIO_InitStructure.GPIO_Pin = deckGPIOMapping[pin.id].pin;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
/* TODO: Any settling time before we can do ADC after init on the GPIO pin? */
- GPIO_Init(deckGPIOMapping[pin.id].port, &GPIO_InitStructure);
+ // GPIO_Init(deckGPIOMapping[pin.id].port, &GPIO_InitStructure);
/* Read the appropriate ADC channel. */
return analogReadChannel((uint8_t)deckGPIOMapping[pin.id].adcCh);
@@ -117,29 +119,30 @@ void analogReference(uint8_t type)
void analogReadResolution(uint8_t bits)
{
- ADC_InitTypeDef ADC_InitStructure;
-
- assert_param((bits >= 6) && (bits <= 12));
-
- adcRange = 1 << bits;
- switch (bits)
- {
- case 12: stregResolution = ADC_Resolution_12b; break;
- case 10: stregResolution = ADC_Resolution_10b; break;
- case 8: stregResolution = ADC_Resolution_8b; break;
- case 6: stregResolution = ADC_Resolution_6b; break;
- default: stregResolution = ADC_Resolution_12b; break;
- }
-
- /* Init ADC2 witch new resolution */
- ADC_InitStructure.ADC_Resolution = stregResolution;
- ADC_InitStructure.ADC_ScanConvMode = DISABLE;
- ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
- ADC_InitStructure.ADC_ExternalTrigConvEdge = 0;
- ADC_InitStructure.ADC_ExternalTrigConv = 0;
- ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
- ADC_InitStructure.ADC_NbrOfConversion = 1;
- ADC_Init(ADC2, &ADC_InitStructure);
+ //COMMENTED FIRMWARE
+ // ADC_InitTypeDef ADC_InitStructure;
+
+ // assert_param((bits >= 6) && (bits <= 12));
+
+ // adcRange = 1 << bits;
+ // switch (bits)
+ // {
+ // case 12: stregResolution = ADC_Resolution_12b; break;
+ // case 10: stregResolution = ADC_Resolution_10b; break;
+ // case 8: stregResolution = ADC_Resolution_8b; break;
+ // case 6: stregResolution = ADC_Resolution_6b; break;
+ // default: stregResolution = ADC_Resolution_12b; break;
+ // }
+
+ // /* Init ADC2 witch new resolution */
+ // ADC_InitStructure.ADC_Resolution = stregResolution;
+ // ADC_InitStructure.ADC_ScanConvMode = DISABLE;
+ // ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
+ // ADC_InitStructure.ADC_ExternalTrigConvEdge = 0;
+ // ADC_InitStructure.ADC_ExternalTrigConv = 0;
+ // ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
+ // ADC_InitStructure.ADC_NbrOfConversion = 1;
+ // ADC_Init(ADC2, &ADC_InitStructure);
}
float analogReadVoltage(const deckPin_t pin)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_constants.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_constants.c
index b7e31e8d816b24c8d34ac388112e1020fdb12c88..b49d8d7fd7e203f0eb19fa9ac2c1d54386186e1e 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_constants.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_constants.c
@@ -28,19 +28,20 @@
/* Mapping between Deck Pin number, real GPIO and ADC channel */
deckGPIOMapping_t deckGPIOMapping[13] = {
- {.periph= RCC_AHB1Periph_GPIOC, .port= GPIOC, .pin=GPIO_Pin_11, .adcCh=-1}, /* RX1 */
- {.periph= RCC_AHB1Periph_GPIOC, .port= GPIOC, .pin=GPIO_Pin_10, .adcCh=-1}, /* TX1 */
- {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_7, .adcCh=-1}, /* SDA */
- {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_6, .adcCh=-1}, /* SCL */
- {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_8, .adcCh=-1}, /* IO1 */
- {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_5, .adcCh=-1}, /* IO2 */
- {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_4, .adcCh=-1}, /* IO3 */
- {.periph= RCC_AHB1Periph_GPIOC, .port= GPIOC, .pin=GPIO_Pin_12, .adcCh=-1}, /* IO4 */
- {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_2, .adcCh=ADC_Channel_2}, /* TX2 */
- {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_3, .adcCh=ADC_Channel_3}, /* RX2 */
- {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_5, .adcCh=ADC_Channel_5}, /* SCK */
- {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_6, .adcCh=ADC_Channel_6}, /* MISO */
- {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_7, .adcCh=ADC_Channel_7}, /* MOSI */
+ //COMMENTED FIRMWARE
+ // {.periph= RCC_AHB1Periph_GPIOC, .port= GPIOC, .pin=GPIO_Pin_11, .adcCh=-1}, /* RX1 */
+ // {.periph= RCC_AHB1Periph_GPIOC, .port= GPIOC, .pin=GPIO_Pin_10, .adcCh=-1}, /* TX1 */
+ // {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_7, .adcCh=-1}, /* SDA */
+ // {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_6, .adcCh=-1}, /* SCL */
+ // {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_8, .adcCh=-1}, /* IO1 */
+ // {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_5, .adcCh=-1}, /* IO2 */
+ // {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_4, .adcCh=-1}, /* IO3 */
+ // {.periph= RCC_AHB1Periph_GPIOC, .port= GPIOC, .pin=GPIO_Pin_12, .adcCh=-1}, /* IO4 */
+ // {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_2, .adcCh=ADC_Channel_2}, /* TX2 */
+ // {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_3, .adcCh=ADC_Channel_3}, /* RX2 */
+ // {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_5, .adcCh=ADC_Channel_5}, /* SCK */
+ // {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_6, .adcCh=ADC_Channel_6}, /* MISO */
+ // {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_7, .adcCh=ADC_Channel_7}, /* MOSI */
};
// Pin definitions
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_digital.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_digital.c
index b8ae72c85a4db20faffd2a039868c1a9adb7128d..d0944da421cc2a1bb45d42bc323b6d271a994c07 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_digital.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_digital.c
@@ -30,31 +30,35 @@
void pinMode(const deckPin_t pin, const uint32_t mode)
{
- RCC_AHB1PeriphClockCmd(deckGPIOMapping[pin.id].periph, ENABLE);
+ //COMMENTED FIRMWARE
+ // RCC_AHB1PeriphClockCmd(deckGPIOMapping[pin.id].periph, ENABLE);
- GPIO_InitTypeDef GPIO_InitStructure = {0};
+ // GPIO_InitTypeDef GPIO_InitStructure = {0};
- GPIO_InitStructure.GPIO_Pin = deckGPIOMapping[pin.id].pin;
- GPIO_InitStructure.GPIO_Mode = (mode == OUTPUT) ? GPIO_Mode_OUT:GPIO_Mode_IN;
- if (mode == OUTPUT) GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- if (mode == INPUT_PULLUP) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
- if (mode == INPUT_PULLDOWN) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
- GPIO_Init(deckGPIOMapping[pin.id].port, &GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Pin = deckGPIOMapping[pin.id].pin;
+ // GPIO_InitStructure.GPIO_Mode = (mode == OUTPUT) ? GPIO_Mode_OUT:GPIO_Mode_IN;
+ // if (mode == OUTPUT) GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // if (mode == INPUT_PULLUP) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+ // if (mode == INPUT_PULLDOWN) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+ // GPIO_Init(deckGPIOMapping[pin.id].port, &GPIO_InitStructure);
}
void digitalWrite(const deckPin_t pin, const uint32_t val)
{
- BitAction action = Bit_RESET;
- if (val) {
- action = Bit_SET;
- }
+ //COMMENTED FIRMWARE
+ // BitAction action = Bit_RESET;
+ // if (val) {
+ // action = Bit_SET;
+ // }
- GPIO_WriteBit(deckGPIOMapping[pin.id].port, deckGPIOMapping[pin.id].pin, action);
+ // GPIO_WriteBit(deckGPIOMapping[pin.id].port, deckGPIOMapping[pin.id].pin, action);
}
int digitalRead(const deckPin_t pin)
{
- int val = GPIO_ReadInputDataBit(deckGPIOMapping[pin.id].port, deckGPIOMapping[pin.id].pin);
- return (val==Bit_SET)?HIGH:LOW;
+ //COMMENTED FIRMWARE
+ // int val = GPIO_ReadInputDataBit(deckGPIOMapping[pin.id].port, deckGPIOMapping[pin.id].pin);
+ // return (val==Bit_SET)?HIGH:LOW;
+ return 0;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi.c
index df9cd9ce6a1dbd864cccaf704fcf77c74c6440f3..6aff3645df38cc9c5b49759a0170daec70681182 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi.c
@@ -93,127 +93,130 @@ static void spiConfigureWithSpeed(uint16_t baudRatePrescaler);
void spiBegin(void)
{
- GPIO_InitTypeDef GPIO_InitStructure;
+ //COMMENTED FIRMWARE
+// GPIO_InitTypeDef GPIO_InitStructure;
- // binary semaphores created using xSemaphoreCreateBinary() are created in a state
- // such that the the semaphore must first be 'given' before it can be 'taken'
- txComplete = xSemaphoreCreateBinary();
- rxComplete = xSemaphoreCreateBinary();
- spiMutex = xSemaphoreCreateMutex();
+// // binary semaphores created using xSemaphoreCreateBinary() are created in a state
+// // such that the the semaphore must first be 'given' before it can be 'taken'
+// txComplete = xSemaphoreCreateBinary();
+// rxComplete = xSemaphoreCreateBinary();
+// spiMutex = xSemaphoreCreateMutex();
- /*!< Enable the SPI clock */
- SPI_CLK_INIT(SPI_CLK, ENABLE);
+// /*!< Enable the SPI clock */
+// SPI_CLK_INIT(SPI_CLK, ENABLE);
- /*!< Enable GPIO clocks */
- RCC_AHB1PeriphClockCmd(SPI_SCK_GPIO_CLK | SPI_MISO_GPIO_CLK |
- SPI_MOSI_GPIO_CLK, ENABLE);
+// /*!< Enable GPIO clocks */
+// RCC_AHB1PeriphClockCmd(SPI_SCK_GPIO_CLK | SPI_MISO_GPIO_CLK |
+// SPI_MOSI_GPIO_CLK, ENABLE);
- /*!< Enable DMA Clocks */
- SPI_DMA_CLK_INIT(SPI_DMA_CLK, ENABLE);
+// /*!< Enable DMA Clocks */
+// SPI_DMA_CLK_INIT(SPI_DMA_CLK, ENABLE);
- /*!< SPI pins configuration *************************************************/
+// /*!< SPI pins configuration *************************************************/
- /*!< Connect SPI pins to AF5 */
- GPIO_PinAFConfig(SPI_SCK_GPIO_PORT, SPI_SCK_SOURCE, SPI_SCK_AF);
- GPIO_PinAFConfig(SPI_MISO_GPIO_PORT, SPI_MISO_SOURCE, SPI_MISO_AF);
- GPIO_PinAFConfig(SPI_MOSI_GPIO_PORT, SPI_MOSI_SOURCE, SPI_MOSI_AF);
+// /*!< Connect SPI pins to AF5 */
+// GPIO_PinAFConfig(SPI_SCK_GPIO_PORT, SPI_SCK_SOURCE, SPI_SCK_AF);
+// GPIO_PinAFConfig(SPI_MISO_GPIO_PORT, SPI_MISO_SOURCE, SPI_MISO_AF);
+// GPIO_PinAFConfig(SPI_MOSI_GPIO_PORT, SPI_MOSI_SOURCE, SPI_MOSI_AF);
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-#ifdef DECK_SPI_MODE3
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
-#else
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
-#endif
+// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+// GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+// #ifdef DECK_SPI_MODE3
+// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+// #else
+// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
+// #endif
- /*!< SPI SCK pin configuration */
- GPIO_InitStructure.GPIO_Pin = SPI_SCK_PIN;
- GPIO_Init(SPI_SCK_GPIO_PORT, &GPIO_InitStructure);
+// /*!< SPI SCK pin configuration */
+// GPIO_InitStructure.GPIO_Pin = SPI_SCK_PIN;
+// GPIO_Init(SPI_SCK_GPIO_PORT, &GPIO_InitStructure);
- /*!< SPI MOSI pin configuration */
- GPIO_InitStructure.GPIO_Pin = SPI_MOSI_PIN;
- GPIO_Init(SPI_MOSI_GPIO_PORT, &GPIO_InitStructure);
+// /*!< SPI MOSI pin configuration */
+// GPIO_InitStructure.GPIO_Pin = SPI_MOSI_PIN;
+// GPIO_Init(SPI_MOSI_GPIO_PORT, &GPIO_InitStructure);
- /*!< SPI MISO pin configuration */
- GPIO_InitStructure.GPIO_Pin = SPI_MISO_PIN;
- GPIO_Init(SPI_MISO_GPIO_PORT, &GPIO_InitStructure);
+// /*!< SPI MISO pin configuration */
+// GPIO_InitStructure.GPIO_Pin = SPI_MISO_PIN;
+// GPIO_Init(SPI_MISO_GPIO_PORT, &GPIO_InitStructure);
- /*!< SPI DMA Initialization */
- spiDMAInit();
+// /*!< SPI DMA Initialization */
+// spiDMAInit();
- /*!< SPI configuration */
- spiConfigureWithSpeed(SPI_BAUDRATE_2MHZ);
+// /*!< SPI configuration */
+// spiConfigureWithSpeed(SPI_BAUDRATE_2MHZ);
- isInit = true;
+// isInit = true;
}
static void spiDMAInit()
{
- DMA_InitTypeDef DMA_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
-
- /* Configure DMA Initialization Structure */
- DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable ;
- DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
- DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single ;
- DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
- DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
- DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
- DMA_InitStructure.DMA_PeripheralBaseAddr =(uint32_t) (&(SPI->DR)) ;
- DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
- DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
- DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- DMA_InitStructure.DMA_Priority = DMA_Priority_High;
- DMA_InitStructure.DMA_BufferSize = 0; // set later
- DMA_InitStructure.DMA_Memory0BaseAddr = 0; // set later
-
- // Configure TX DMA
- DMA_InitStructure.DMA_Channel = SPI_TX_DMA_CHANNEL;
- DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
- DMA_Cmd(SPI_TX_DMA_STREAM,DISABLE);
- DMA_Init(SPI_TX_DMA_STREAM, &DMA_InitStructure);
-
- // Configure RX DMA
- DMA_InitStructure.DMA_Channel = SPI_RX_DMA_CHANNEL;
- DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
- DMA_Cmd(SPI_RX_DMA_STREAM,DISABLE);
- DMA_Init(SPI_RX_DMA_STREAM, &DMA_InitStructure);
-
- // Configure interrupts
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-
- NVIC_InitStructure.NVIC_IRQChannel = SPI_TX_DMA_IRQ;
- NVIC_Init(&NVIC_InitStructure);
-
- NVIC_InitStructure.NVIC_IRQChannel = SPI_RX_DMA_IRQ;
- NVIC_Init(&NVIC_InitStructure);
+ //COMMENTED FIRMWARE
+ // DMA_InitTypeDef DMA_InitStructure;
+ // NVIC_InitTypeDef NVIC_InitStructure;
+
+ // /* Configure DMA Initialization Structure */
+ // DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable ;
+ // DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
+ // DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single ;
+ // DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ // DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ // DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
+ // DMA_InitStructure.DMA_PeripheralBaseAddr =(uint32_t) (&(SPI->DR)) ;
+ // DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+ // DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+ // DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ // DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+ // DMA_InitStructure.DMA_BufferSize = 0; // set later
+ // DMA_InitStructure.DMA_Memory0BaseAddr = 0; // set later
+
+ // // Configure TX DMA
+ // DMA_InitStructure.DMA_Channel = SPI_TX_DMA_CHANNEL;
+ // DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+ // DMA_Cmd(SPI_TX_DMA_STREAM,DISABLE);
+ // DMA_Init(SPI_TX_DMA_STREAM, &DMA_InitStructure);
+
+ // // Configure RX DMA
+ // DMA_InitStructure.DMA_Channel = SPI_RX_DMA_CHANNEL;
+ // DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
+ // DMA_Cmd(SPI_RX_DMA_STREAM,DISABLE);
+ // DMA_Init(SPI_RX_DMA_STREAM, &DMA_InitStructure);
+
+ // // Configure interrupts
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+
+ // NVIC_InitStructure.NVIC_IRQChannel = SPI_TX_DMA_IRQ;
+ // NVIC_Init(&NVIC_InitStructure);
+
+ // NVIC_InitStructure.NVIC_IRQChannel = SPI_RX_DMA_IRQ;
+ // NVIC_Init(&NVIC_InitStructure);
}
static void spiConfigureWithSpeed(uint16_t baudRatePrescaler)
{
- SPI_InitTypeDef SPI_InitStructure;
-
- SPI_I2S_DeInit(SPI);
-
- SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
- SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
- SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
-#ifdef DECK_SPI_MODE3
- SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
- SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
-#else
- SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
- SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
-#endif
- SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
- SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
- SPI_InitStructure.SPI_CRCPolynomial = 0; // Not used
-
- SPI_InitStructure.SPI_BaudRatePrescaler = baudRatePrescaler;
- SPI_Init(SPI, &SPI_InitStructure);
+ //COMMENTED FIRMWARE
+// SPI_InitTypeDef SPI_InitStructure;
+
+// SPI_I2S_DeInit(SPI);
+
+// SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
+// SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
+// SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
+// #ifdef DECK_SPI_MODE3
+// SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
+// SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
+// #else
+// SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
+// SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
+// #endif
+// SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
+// SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
+// SPI_InitStructure.SPI_CRCPolynomial = 0; // Not used
+
+// SPI_InitStructure.SPI_BaudRatePrescaler = baudRatePrescaler;
+// SPI_Init(SPI, &SPI_InitStructure);
}
bool spiTest(void)
@@ -223,42 +226,44 @@ bool spiTest(void)
bool spiExchange(size_t length, const uint8_t * data_tx, uint8_t * data_rx)
{
- ASSERT_DMA_SAFE(data_tx);
- ASSERT_DMA_SAFE(data_rx);
+ //COMMENTED FIRMWARE
+ // ASSERT_DMA_SAFE(data_tx);
+ // ASSERT_DMA_SAFE(data_rx);
- // DMA already configured, just need to set memory addresses
- SPI_TX_DMA_STREAM->M0AR = (uint32_t)data_tx;
- SPI_TX_DMA_STREAM->NDTR = length;
+ // // DMA already configured, just need to set memory addresses
+ // SPI_TX_DMA_STREAM->M0AR = (uint32_t)data_tx;
+ // SPI_TX_DMA_STREAM->NDTR = length;
- SPI_RX_DMA_STREAM->M0AR = (uint32_t)data_rx;
- SPI_RX_DMA_STREAM->NDTR = length;
+ // SPI_RX_DMA_STREAM->M0AR = (uint32_t)data_rx;
+ // SPI_RX_DMA_STREAM->NDTR = length;
- // Enable SPI DMA Interrupts
- DMA_ITConfig(SPI_TX_DMA_STREAM, DMA_IT_TC, ENABLE);
- DMA_ITConfig(SPI_RX_DMA_STREAM, DMA_IT_TC, ENABLE);
+ // // Enable SPI DMA Interrupts
+ // DMA_ITConfig(SPI_TX_DMA_STREAM, DMA_IT_TC, ENABLE);
+ // DMA_ITConfig(SPI_RX_DMA_STREAM, DMA_IT_TC, ENABLE);
- // Clear DMA Flags
- DMA_ClearFlag(SPI_TX_DMA_STREAM, DMA_FLAG_FEIF5|DMA_FLAG_DMEIF5|DMA_FLAG_TEIF5|DMA_FLAG_HTIF5|DMA_FLAG_TCIF5);
- DMA_ClearFlag(SPI_RX_DMA_STREAM, DMA_FLAG_FEIF0|DMA_FLAG_DMEIF0|DMA_FLAG_TEIF0|DMA_FLAG_HTIF0|DMA_FLAG_TCIF0);
+ // // Clear DMA Flags
+ // DMA_ClearFlag(SPI_TX_DMA_STREAM, DMA_FLAG_FEIF5|DMA_FLAG_DMEIF5|DMA_FLAG_TEIF5|DMA_FLAG_HTIF5|DMA_FLAG_TCIF5);
+ // DMA_ClearFlag(SPI_RX_DMA_STREAM, DMA_FLAG_FEIF0|DMA_FLAG_DMEIF0|DMA_FLAG_TEIF0|DMA_FLAG_HTIF0|DMA_FLAG_TCIF0);
- // Enable DMA Streams
- DMA_Cmd(SPI_TX_DMA_STREAM,ENABLE);
- DMA_Cmd(SPI_RX_DMA_STREAM,ENABLE);
+ // // Enable DMA Streams
+ // DMA_Cmd(SPI_TX_DMA_STREAM,ENABLE);
+ // DMA_Cmd(SPI_RX_DMA_STREAM,ENABLE);
- // Enable SPI DMA requests
- SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Tx, ENABLE);
- SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Rx, ENABLE);
+ // // Enable SPI DMA requests
+ // SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Tx, ENABLE);
+ // SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Rx, ENABLE);
- // Enable peripheral
- SPI_Cmd(SPI, ENABLE);
+ // // Enable peripheral
+ // SPI_Cmd(SPI, ENABLE);
- // Wait for completion
- bool result = (xSemaphoreTake(txComplete, portMAX_DELAY) == pdTRUE)
- && (xSemaphoreTake(rxComplete, portMAX_DELAY) == pdTRUE);
+ // // Wait for completion
+ // bool result = (xSemaphoreTake(txComplete, portMAX_DELAY) == pdTRUE)
+ // && (xSemaphoreTake(rxComplete, portMAX_DELAY) == pdTRUE);
- // Disable peripheral
- SPI_Cmd(SPI, DISABLE);
- return result;
+ // // Disable peripheral
+ // SPI_Cmd(SPI, DISABLE);
+ //return result;
+ return false;
}
void spiBeginTransaction(uint16_t baudRatePrescaler)
@@ -274,52 +279,54 @@ void spiEndTransaction()
void __attribute__((used)) SPI_TX_DMA_IRQHandler(void)
{
- portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+ //COMMENTED FIRMWARE
+ // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
- // Stop and cleanup DMA stream
- DMA_ITConfig(SPI_TX_DMA_STREAM, DMA_IT_TC, DISABLE);
- DMA_ClearITPendingBit(SPI_TX_DMA_STREAM, SPI_TX_DMA_FLAG_TCIF);
+ // // Stop and cleanup DMA stream
+ // DMA_ITConfig(SPI_TX_DMA_STREAM, DMA_IT_TC, DISABLE);
+ // DMA_ClearITPendingBit(SPI_TX_DMA_STREAM, SPI_TX_DMA_FLAG_TCIF);
- // Clear stream flags
- DMA_ClearFlag(SPI_TX_DMA_STREAM,SPI_TX_DMA_FLAG_TCIF);
+ // // Clear stream flags
+ // DMA_ClearFlag(SPI_TX_DMA_STREAM,SPI_TX_DMA_FLAG_TCIF);
- // Disable SPI DMA requests
- SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Tx, DISABLE);
+ // // Disable SPI DMA requests
+ // SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Tx, DISABLE);
- // Disable streams
- DMA_Cmd(SPI_TX_DMA_STREAM,DISABLE);
+ // // Disable streams
+ // DMA_Cmd(SPI_TX_DMA_STREAM,DISABLE);
- // Give the semaphore, allowing the SPI transaction to complete
- xSemaphoreGiveFromISR(txComplete, &xHigherPriorityTaskWoken);
+ // // Give the semaphore, allowing the SPI transaction to complete
+ // xSemaphoreGiveFromISR(txComplete, &xHigherPriorityTaskWoken);
- if (xHigherPriorityTaskWoken)
- {
- portYIELD();
- }
+ // if (xHigherPriorityTaskWoken)
+ // {
+ // portYIELD();
+ // }
}
void __attribute__((used)) SPI_RX_DMA_IRQHandler(void)
{
- portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+ //COMMENTED FIRMWARE
+ // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
- // Stop and cleanup DMA stream
- DMA_ITConfig(SPI_RX_DMA_STREAM, DMA_IT_TC, DISABLE);
- DMA_ClearITPendingBit(SPI_RX_DMA_STREAM, SPI_RX_DMA_FLAG_TCIF);
+ // // Stop and cleanup DMA stream
+ // DMA_ITConfig(SPI_RX_DMA_STREAM, DMA_IT_TC, DISABLE);
+ // DMA_ClearITPendingBit(SPI_RX_DMA_STREAM, SPI_RX_DMA_FLAG_TCIF);
- // Clear stream flags
- DMA_ClearFlag(SPI_RX_DMA_STREAM,SPI_RX_DMA_FLAG_TCIF);
+ // // Clear stream flags
+ // DMA_ClearFlag(SPI_RX_DMA_STREAM,SPI_RX_DMA_FLAG_TCIF);
- // Disable SPI DMA requests
- SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Rx, DISABLE);
+ // // Disable SPI DMA requests
+ // SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Rx, DISABLE);
- // Disable streams
- DMA_Cmd(SPI_RX_DMA_STREAM,DISABLE);
+ // // Disable streams
+ // DMA_Cmd(SPI_RX_DMA_STREAM,DISABLE);
- // Give the semaphore, allowing the SPI transaction to complete
- xSemaphoreGiveFromISR(rxComplete, &xHigherPriorityTaskWoken);
+ // // Give the semaphore, allowing the SPI transaction to complete
+ // xSemaphoreGiveFromISR(rxComplete, &xHigherPriorityTaskWoken);
- if (xHigherPriorityTaskWoken)
- {
- portYIELD();
- }
+ // if (xHigherPriorityTaskWoken)
+ // {
+ // portYIELD();
+ // }
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi3.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi3.c
index 75bb87026e76c931785604346793c0319ddd2318..4b16d397d300d50cfc1b94d619516cde295650c8 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi3.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi3.c
@@ -94,118 +94,120 @@ static void spi3ConfigureWithSpeed(uint16_t baudRatePrescaler);
void spi3Begin(void)
{
- GPIO_InitTypeDef GPIO_InitStructure;
+ //COMMENTED FIRMWARE
+ // GPIO_InitTypeDef GPIO_InitStructure;
- // binary semaphores created using xSemaphoreCreateBinary() are created in a state
- // such that the the semaphore must first be 'given' before it can be 'taken'
- txComplete = xSemaphoreCreateBinary();
- rxComplete = xSemaphoreCreateBinary();
- spiMutex = xSemaphoreCreateMutex();
+ // // binary semaphores created using xSemaphoreCreateBinary() are created in a state
+ // // such that the the semaphore must first be 'given' before it can be 'taken'
+ // txComplete = xSemaphoreCreateBinary();
+ // rxComplete = xSemaphoreCreateBinary();
+ // spiMutex = xSemaphoreCreateMutex();
- /*!< Enable the SPI clock */
- SPI_CLK_INIT(SPI_CLK, ENABLE);
+ // /*!< Enable the SPI clock */
+ // SPI_CLK_INIT(SPI_CLK, ENABLE);
- /*!< Enable GPIO clocks */
- RCC_AHB1PeriphClockCmd(SPI_SCK_GPIO_CLK | SPI_MISO_GPIO_CLK |
- SPI_MOSI_GPIO_CLK, ENABLE);
+ // /*!< Enable GPIO clocks */
+ // RCC_AHB1PeriphClockCmd(SPI_SCK_GPIO_CLK | SPI_MISO_GPIO_CLK |
+ // SPI_MOSI_GPIO_CLK, ENABLE);
- /*!< Enable DMA Clocks */
- SPI_DMA_CLK_INIT(SPI_DMA_CLK, ENABLE);
+ // /*!< Enable DMA Clocks */
+ // SPI_DMA_CLK_INIT(SPI_DMA_CLK, ENABLE);
- /*!< SPI pins configuration *************************************************/
+ // /*!< SPI pins configuration *************************************************/
- /*!< Connect SPI pins to AF5 */
- GPIO_PinAFConfig(SPI_SCK_GPIO_PORT, SPI_SCK_SOURCE, SPI_SCK_AF);
- GPIO_PinAFConfig(SPI_MISO_GPIO_PORT, SPI_MISO_SOURCE, SPI_MISO_AF);
- GPIO_PinAFConfig(SPI_MOSI_GPIO_PORT, SPI_MOSI_SOURCE, SPI_MOSI_AF);
+ // /*!< Connect SPI pins to AF5 */
+ // GPIO_PinAFConfig(SPI_SCK_GPIO_PORT, SPI_SCK_SOURCE, SPI_SCK_AF);
+ // GPIO_PinAFConfig(SPI_MISO_GPIO_PORT, SPI_MISO_SOURCE, SPI_MISO_AF);
+ // GPIO_PinAFConfig(SPI_MOSI_GPIO_PORT, SPI_MOSI_SOURCE, SPI_MOSI_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;
- /*!< SPI SCK pin configuration */
- GPIO_InitStructure.GPIO_Pin = SPI_SCK_PIN;
- GPIO_Init(SPI_SCK_GPIO_PORT, &GPIO_InitStructure);
+ // /*!< SPI SCK pin configuration */
+ // GPIO_InitStructure.GPIO_Pin = SPI_SCK_PIN;
+ // GPIO_Init(SPI_SCK_GPIO_PORT, &GPIO_InitStructure);
- /*!< SPI MOSI pin configuration */
- GPIO_InitStructure.GPIO_Pin = SPI_MOSI_PIN;
- GPIO_Init(SPI_MOSI_GPIO_PORT, &GPIO_InitStructure);
+ // /*!< SPI MOSI pin configuration */
+ // GPIO_InitStructure.GPIO_Pin = SPI_MOSI_PIN;
+ // GPIO_Init(SPI_MOSI_GPIO_PORT, &GPIO_InitStructure);
- /*!< SPI MISO pin configuration */
- GPIO_InitStructure.GPIO_Pin = SPI_MISO_PIN;
- GPIO_Init(SPI_MISO_GPIO_PORT, &GPIO_InitStructure);
+ // /*!< SPI MISO pin configuration */
+ // GPIO_InitStructure.GPIO_Pin = SPI_MISO_PIN;
+ // GPIO_Init(SPI_MISO_GPIO_PORT, &GPIO_InitStructure);
- /*!< SPI DMA Initialization */
- spi3DMAInit();
+ // /*!< SPI DMA Initialization */
+ // spi3DMAInit();
- /*!< SPI configuration */
- spi3ConfigureWithSpeed(SPI_BAUDRATE_2MHZ);
+ // /*!< SPI configuration */
+ // spi3ConfigureWithSpeed(SPI_BAUDRATE_2MHZ);
- isInit = true;
+ // isInit = true;
}
static void spi3DMAInit()
{
- DMA_InitTypeDef DMA_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
-
- /* Configure DMA Initialization Structure */
- DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable ;
- DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
- DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single ;
- DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
- DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
- DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
- DMA_InitStructure.DMA_PeripheralBaseAddr =(uint32_t) (&(SPI->DR)) ;
- DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
- DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
- DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- DMA_InitStructure.DMA_Priority = DMA_Priority_High;
- DMA_InitStructure.DMA_BufferSize = 0; // set later
- DMA_InitStructure.DMA_Memory0BaseAddr = 0; // set later
-
- // Configure TX DMA
- DMA_InitStructure.DMA_Channel = SPI_TX_DMA_CHANNEL;
- DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
- DMA_Cmd(SPI_TX_DMA_STREAM,DISABLE);
- DMA_Init(SPI_TX_DMA_STREAM, &DMA_InitStructure);
-
- // Configure RX DMA
- DMA_InitStructure.DMA_Channel = SPI_RX_DMA_CHANNEL;
- DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
- DMA_Cmd(SPI_RX_DMA_STREAM,DISABLE);
- DMA_Init(SPI_RX_DMA_STREAM, &DMA_InitStructure);
-
- // Configure interrupts
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-
- NVIC_InitStructure.NVIC_IRQChannel = SPI_TX_DMA_IRQ;
- NVIC_Init(&NVIC_InitStructure);
-
- NVIC_InitStructure.NVIC_IRQChannel = SPI_RX_DMA_IRQ;
- NVIC_Init(&NVIC_InitStructure);
-}
-
-static void spi3ConfigureWithSpeed(uint16_t baudRatePrescaler)
-{
- SPI_InitTypeDef SPI_InitStructure;
-
- SPI_I2S_DeInit(SPI);
-
- SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
- SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
- SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
- SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
- SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
- SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
- SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
- SPI_InitStructure.SPI_CRCPolynomial = 0; // Not used
-
- SPI_InitStructure.SPI_BaudRatePrescaler = baudRatePrescaler;
- SPI_Init(SPI, &SPI_InitStructure);
+ //COMMENTED FIRMWARE
+// DMA_InitTypeDef DMA_InitStructure;
+// NVIC_InitTypeDef NVIC_InitStructure;
+
+// /* Configure DMA Initialization Structure */
+// DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable ;
+// DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
+// DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single ;
+// DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+// DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+// DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
+// DMA_InitStructure.DMA_PeripheralBaseAddr =(uint32_t) (&(SPI->DR)) ;
+// DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+// DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+// DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+// DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+// DMA_InitStructure.DMA_BufferSize = 0; // set later
+// DMA_InitStructure.DMA_Memory0BaseAddr = 0; // set later
+
+// // Configure TX DMA
+// DMA_InitStructure.DMA_Channel = SPI_TX_DMA_CHANNEL;
+// DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+// DMA_Cmd(SPI_TX_DMA_STREAM,DISABLE);
+// DMA_Init(SPI_TX_DMA_STREAM, &DMA_InitStructure);
+
+// // Configure RX DMA
+// DMA_InitStructure.DMA_Channel = SPI_RX_DMA_CHANNEL;
+// DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
+// DMA_Cmd(SPI_RX_DMA_STREAM,DISABLE);
+// DMA_Init(SPI_RX_DMA_STREAM, &DMA_InitStructure);
+
+// // Configure interrupts
+// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
+// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+
+// NVIC_InitStructure.NVIC_IRQChannel = SPI_TX_DMA_IRQ;
+// NVIC_Init(&NVIC_InitStructure);
+
+// NVIC_InitStructure.NVIC_IRQChannel = SPI_RX_DMA_IRQ;
+// NVIC_Init(&NVIC_InitStructure);
+// }
+
+// static void spi3ConfigureWithSpeed(uint16_t baudRatePrescaler)
+// {
+// SPI_InitTypeDef SPI_InitStructure;
+
+// SPI_I2S_DeInit(SPI);
+
+// SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
+// SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
+// SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
+// SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
+// SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
+// SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
+// SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
+// SPI_InitStructure.SPI_CRCPolynomial = 0; // Not used
+
+// SPI_InitStructure.SPI_BaudRatePrescaler = baudRatePrescaler;
+// SPI_Init(SPI, &SPI_InitStructure);
}
bool spi3Test(void)
@@ -215,42 +217,44 @@ bool spi3Test(void)
bool spi3Exchange(size_t length, const uint8_t * data_tx, uint8_t * data_rx)
{
- ASSERT_DMA_SAFE(data_tx);
- ASSERT_DMA_SAFE(data_rx);
+ //COMMENTED FIRMWARE
+ // ASSERT_DMA_SAFE(data_tx);
+ // ASSERT_DMA_SAFE(data_rx);
- // DMA already configured, just need to set memory addresses
- SPI_TX_DMA_STREAM->M0AR = (uint32_t)data_tx;
- SPI_TX_DMA_STREAM->NDTR = length;
+ // // DMA already configured, just need to set memory addresses
+ // SPI_TX_DMA_STREAM->M0AR = (uint32_t)data_tx;
+ // SPI_TX_DMA_STREAM->NDTR = length;
- SPI_RX_DMA_STREAM->M0AR = (uint32_t)data_rx;
- SPI_RX_DMA_STREAM->NDTR = length;
+ // SPI_RX_DMA_STREAM->M0AR = (uint32_t)data_rx;
+ // SPI_RX_DMA_STREAM->NDTR = length;
- // Enable SPI DMA Interrupts
- DMA_ITConfig(SPI_TX_DMA_STREAM, DMA_IT_TC, ENABLE);
- DMA_ITConfig(SPI_RX_DMA_STREAM, DMA_IT_TC, ENABLE);
+ // // Enable SPI DMA Interrupts
+ // DMA_ITConfig(SPI_TX_DMA_STREAM, DMA_IT_TC, ENABLE);
+ // DMA_ITConfig(SPI_RX_DMA_STREAM, DMA_IT_TC, ENABLE);
- // Clear DMA Flags
- DMA_ClearFlag(SPI_TX_DMA_STREAM, DMA_FLAG_FEIF7|DMA_FLAG_DMEIF7|DMA_FLAG_TEIF7|DMA_FLAG_HTIF7|DMA_FLAG_TCIF7);
- DMA_ClearFlag(SPI_RX_DMA_STREAM, DMA_FLAG_FEIF0|DMA_FLAG_DMEIF0|DMA_FLAG_TEIF0|DMA_FLAG_HTIF0|DMA_FLAG_TCIF0);
+ // // Clear DMA Flags
+ // DMA_ClearFlag(SPI_TX_DMA_STREAM, DMA_FLAG_FEIF7|DMA_FLAG_DMEIF7|DMA_FLAG_TEIF7|DMA_FLAG_HTIF7|DMA_FLAG_TCIF7);
+ // DMA_ClearFlag(SPI_RX_DMA_STREAM, DMA_FLAG_FEIF0|DMA_FLAG_DMEIF0|DMA_FLAG_TEIF0|DMA_FLAG_HTIF0|DMA_FLAG_TCIF0);
- // Enable DMA Streams
- DMA_Cmd(SPI_TX_DMA_STREAM,ENABLE);
- DMA_Cmd(SPI_RX_DMA_STREAM,ENABLE);
+ // // Enable DMA Streams
+ // DMA_Cmd(SPI_TX_DMA_STREAM,ENABLE);
+ // DMA_Cmd(SPI_RX_DMA_STREAM,ENABLE);
- // Enable SPI DMA requests
- SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Tx, ENABLE);
- SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Rx, ENABLE);
+ // // Enable SPI DMA requests
+ // SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Tx, ENABLE);
+ // SPI_I2S_DMACmd(SPI, SPI_I2S_DMAReq_Rx, ENABLE);
- // Enable peripheral
- SPI_Cmd(SPI, ENABLE);
+ // // Enable peripheral
+ // SPI_Cmd(SPI, ENABLE);
- // Wait for completion
- bool result = (xSemaphoreTake(txComplete, portMAX_DELAY) == pdTRUE)
- && (xSemaphoreTake(rxComplete, portMAX_DELAY) == pdTRUE);
+ // // Wait for completion
+ // bool result = (xSemaphoreTake(txComplete, portMAX_DELAY) == pdTRUE)
+ // && (xSemaphoreTake(rxComplete, portMAX_DELAY) == pdTRUE);
- // Disable peripheral
- SPI_Cmd(SPI, DISABLE);
- return result;
+ // // Disable peripheral
+ // SPI_Cmd(SPI, DISABLE);
+ // return result;
+ return false;
}
void spi3BeginTransaction(uint16_t baudRatePrescaler)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/lpsTdoa2Tag.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/lpsTdoa2Tag.h
index c0f76596e8802dc44a60f4013ecf5ee7cbc45c7a..258ed19b6a352243aa3ec6e7d51ef486cb7b833e 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/lpsTdoa2Tag.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/lpsTdoa2Tag.h
@@ -2,7 +2,8 @@
#define __LPS_TDOA2_TAG_H__
#include "locodeck.h"
-#include "libdw1000.h"
+//COMMENTED FIRMWARE
+//#include "libdw1000.h"
#include "mac.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/lpsTwrTag.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/lpsTwrTag.h
index f3768191421c3aab2b9baf4866a824c6ea06947e..a8ade79850072f6bc09fb6d6e5057e458e6e2f8b 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/lpsTwrTag.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/lpsTwrTag.h
@@ -2,7 +2,8 @@
#define __LPS_TWR_TAG_H__
#include "locodeck.h"
-#include "libdw1000.h"
+//COMMENTED FIRMWARE
+//#include "libdw1000.h"
#include "mac.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/activeMarkerDeck.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/activeMarkerDeck.c
index dfa07af32fc1088673b1a5217bb81c1dcca9ce62..b5d1ecf58c6b2501285fb1caa1eacb1a7ce9ebcf 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/activeMarkerDeck.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/activeMarkerDeck.c
@@ -70,7 +70,8 @@ static uint8_t requestedDeckMode = MODE_QUALISYS;
static uint8_t doPollDeckButtonSensor = 0;
static uint8_t deckButtonSensorValue = 0;
static uint32_t nextPollTime = 0;
-static const uint32_t pollIntervall = M2T(100);
+//COMMENTED FIRMWARE
+//static const uint32_t pollIntervall = M2T(100);
static bool i2cOk = false;
// defines eventTrigger_activeMarkerModeChanged
@@ -161,13 +162,14 @@ static void handleModeUpdate() {
}
static void handleButtonSensorRead() {
- if (doPollDeckButtonSensor) {
- uint32_t now = xTaskGetTickCount();
- if (now > nextPollTime) {
- i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, MEM_ADR_BUTTON_SENSOR, 1, &deckButtonSensorValue);
- nextPollTime = now + pollIntervall;
- }
- }
+ //COMMENTED FIRMWARE
+ // if (doPollDeckButtonSensor) {
+ // uint32_t now = xTaskGetTickCount();
+ // if (now > nextPollTime) {
+ // i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, MEM_ADR_BUTTON_SENSOR, 1, &deckButtonSensorValue);
+ // nextPollTime = now + pollIntervall;
+ // }
+ // }
}
static void task(void *param) {
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/ledring12.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/ledring12.c
index 4a86f542dd40451955a7e33ac4ec74ebe45048fe..8386b88a763c317f170f28ecad1ce85d1e1ce6f7 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/ledring12.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/ledring12.c
@@ -564,10 +564,11 @@ static void brightnessEffect(uint8_t buffer[][3], bool reset)
static void setHeadlightsOn(bool on)
{
- if (on)
- GPIO_SetBits(GPIOB, GPIO_Pin_4);
- else
- GPIO_ResetBits(GPIOB, GPIO_Pin_4);
+ //COMMENTED FIRMWARE
+ // if (on)
+ // GPIO_SetBits(GPIOB, GPIO_Pin_4);
+ // else
+ // GPIO_ResetBits(GPIOB, GPIO_Pin_4);
}
@@ -1045,30 +1046,31 @@ static void ledring12Timer(xTimerHandle timer)
static void ledring12Init(DeckInfo *info)
{
- if (isInit) {
- return;
- }
+ //COMMENTED FIRMWARE
+ // if (isInit) {
+ // return;
+ // }
- GPIO_InitTypeDef GPIO_InitStructure;
+ // GPIO_InitTypeDef GPIO_InitStructure;
- ws2812Init();
+ // ws2812Init();
- neffect = sizeof(effectsFct)/sizeof(effectsFct[0])-1;
+ // neffect = sizeof(effectsFct)/sizeof(effectsFct[0])-1;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ // GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
+ // GPIO_Init(GPIOB, &GPIO_InitStructure);
- memoryRegisterHandler(&ledringmemDef);
- memoryRegisterHandler(&timingmemDef);
+ // memoryRegisterHandler(&ledringmemDef);
+ // memoryRegisterHandler(&timingmemDef);
- isInit = true;
+ // isInit = true;
- timer = xTimerCreate( "ringTimer", M2T(50),
- pdTRUE, NULL, ledring12Timer );
- xTimerStart(timer, 100);
+ // timer = xTimerCreate( "ringTimer", M2T(50),
+ // pdTRUE, NULL, ledring12Timer );
+ // xTimerStart(timer, 100);
}
static bool handleLedringmemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer) {
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/locodeck.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/locodeck.c
index 21a788f23383054919a568e7240f7f92c1ad0532..d063387556f52585ac038ec89da3b605532fb2ca 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/locodeck.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/locodeck.c
@@ -127,8 +127,10 @@ static uwbAlgorithm_t *algorithm = &uwbTwrTagAlgorithm;
static bool isInit = false;
static TaskHandle_t uwbTaskHandle = 0;
static SemaphoreHandle_t algoSemaphore;
-static dwDevice_t dwm_device;
-static dwDevice_t *dwm = &dwm_device;
+
+//COMMENTED FIRMWARE
+// static dwDevice_t dwm_device;
+// static dwDevice_t *dwm = &dwm_device;
static QueueHandle_t lppShortQueue;
@@ -161,19 +163,20 @@ static const MemoryHandlerDef_t memDef = {
};
static void buildAnchorMemList(const uint32_t memAddr, const uint8_t readLen, uint8_t* dest, const uint32_t pageBase_address, const uint8_t anchorCount, const uint8_t unsortedAnchorList[]);
-static void txCallback(dwDevice_t *dev)
-{
- timeout = algorithm->onEvent(dev, eventPacketSent);
-}
+//COMMENTED FIRMWARE
+// static void txCallback(dwDevice_t *dev)
+// {
+// timeout = algorithm->onEvent(dev, eventPacketSent);
+// }
-static void rxCallback(dwDevice_t *dev)
-{
- timeout = algorithm->onEvent(dev, eventPacketReceived);
-}
+// static void rxCallback(dwDevice_t *dev)
+// {
+// timeout = algorithm->onEvent(dev, eventPacketReceived);
+// }
-static void rxTimeoutCallback(dwDevice_t * dev) {
- timeout = algorithm->onEvent(dev, eventReceiveTimeout);
-}
+// static void rxTimeoutCallback(dwDevice_t * dev) {
+// timeout = algorithm->onEvent(dev, eventReceiveTimeout);
+// }
static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* dest) {
bool result = false;
@@ -275,16 +278,16 @@ uint8_t locoDeckGetActiveAnchorIdList(uint8_t unorderedAnchorList[], const int m
static bool switchToMode(const lpsMode_t newMode) {
bool result = false;
+ //COMMENTED FIRMWARE
+ // if (lpsMode_auto != newMode && newMode <= LPS_NUMBER_OF_ALGORITHMS) {
+ // algoOptions.currentRangingMode = newMode;
+ // algorithm = algorithmsList[algoOptions.currentRangingMode].algorithm;
- if (lpsMode_auto != newMode && newMode <= LPS_NUMBER_OF_ALGORITHMS) {
- algoOptions.currentRangingMode = newMode;
- algorithm = algorithmsList[algoOptions.currentRangingMode].algorithm;
+ // algorithm->init(dwm);
+ // timeout = algorithm->onEvent(dwm, eventTimeout);
- algorithm->init(dwm);
- timeout = algorithm->onEvent(dwm, eventTimeout);
-
- result = true;
- }
+ // result = true;
+ // }
return result;
}
@@ -345,29 +348,30 @@ static void handleModeSwitch() {
}
static void uwbTask(void* parameters) {
- lppShortQueue = xQueueCreate(10, sizeof(lpsLppShortPacket_t));
-
- algoOptions.currentRangingMode = lpsMode_auto;
-
- systemWaitStart();
-
- while(1) {
- xSemaphoreTake(algoSemaphore, portMAX_DELAY);
- handleModeSwitch();
- xSemaphoreGive(algoSemaphore);
-
- if (ulTaskNotifyTake(pdTRUE, timeout / portTICK_PERIOD_MS) > 0) {
- do{
- xSemaphoreTake(algoSemaphore, portMAX_DELAY);
- dwHandleInterrupt(dwm);
- xSemaphoreGive(algoSemaphore);
- } while(digitalRead(GPIO_PIN_IRQ) != 0);
- } else {
- xSemaphoreTake(algoSemaphore, portMAX_DELAY);
- timeout = algorithm->onEvent(dwm, eventTimeout);
- xSemaphoreGive(algoSemaphore);
- }
- }
+ //COMMENTED FIRMWARE
+ // lppShortQueue = xQueueCreate(10, sizeof(lpsLppShortPacket_t));
+
+ // algoOptions.currentRangingMode = lpsMode_auto;
+
+ // systemWaitStart();
+
+ // while(1) {
+ // xSemaphoreTake(algoSemaphore, portMAX_DELAY);
+ // handleModeSwitch();
+ // xSemaphoreGive(algoSemaphore);
+
+ // if (ulTaskNotifyTake(pdTRUE, timeout / portTICK_PERIOD_MS) > 0) {
+ // do{
+ // xSemaphoreTake(algoSemaphore, portMAX_DELAY);
+ // dwHandleInterrupt(dwm);
+ // xSemaphoreGive(algoSemaphore);
+ // } while(digitalRead(GPIO_PIN_IRQ) != 0);
+ // } else {
+ // xSemaphoreTake(algoSemaphore, portMAX_DELAY);
+ // timeout = algorithm->onEvent(dwm, eventTimeout);
+ // xSemaphoreGive(algoSemaphore);
+ // }
+ // }
}
static lpsLppShortPacket_t lppShortPacket;
@@ -397,152 +401,159 @@ static uint8_t spiRxBuffer[196];
static uint16_t spiSpeed = SPI_BAUDRATE_2MHZ;
/************ Low level ops for libdw **********/
-static void spiWrite(dwDevice_t* dev, const void *header, size_t headerLength,
- const void* data, size_t dataLength)
-{
- spiBeginTransaction(spiSpeed);
- digitalWrite(CS_PIN, LOW);
- memcpy(spiTxBuffer, header, headerLength);
- memcpy(spiTxBuffer+headerLength, data, dataLength);
- spiExchange(headerLength+dataLength, spiTxBuffer, spiRxBuffer);
- digitalWrite(CS_PIN, HIGH);
- spiEndTransaction();
- STATS_CNT_RATE_EVENT(&spiWriteCount);
-}
+//COMMENTED FIRMWARE
+// static void spiWrite(dwDevice_t* dev, const void *header, size_t headerLength,
+// const void* data, size_t dataLength)
+// {
+// spiBeginTransaction(spiSpeed);
+// digitalWrite(CS_PIN, LOW);
+// memcpy(spiTxBuffer, header, headerLength);
+// memcpy(spiTxBuffer+headerLength, data, dataLength);
+// spiExchange(headerLength+dataLength, spiTxBuffer, spiRxBuffer);
+// digitalWrite(CS_PIN, HIGH);
+// spiEndTransaction();
+// STATS_CNT_RATE_EVENT(&spiWriteCount);
+// }
+
+//COMMENTED FIRMWARE
+// static void spiRead(dwDevice_t* dev, const void *header, size_t headerLength,
+// void* data, size_t dataLength)
+// {
+// spiBeginTransaction(spiSpeed);
+// digitalWrite(CS_PIN, LOW);
+// memcpy(spiTxBuffer, header, headerLength);
+// memset(spiTxBuffer+headerLength, 0, dataLength);
+// spiExchange(headerLength+dataLength, spiTxBuffer, spiRxBuffer);
+// memcpy(data, spiRxBuffer+headerLength, dataLength);
+// digitalWrite(CS_PIN, HIGH);
+// spiEndTransaction();
+// STATS_CNT_RATE_EVENT(&spiReadCount);
+// }
+
+// #if LOCODECK_USE_ALT_PINS
+// void __attribute__((used)) EXTI5_Callback(void)
+// #else
+// void __attribute__((used)) EXTI11_Callback(void)
+// #endif
+// {
+// portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+
+// // Unlock interrupt handling task
+// vTaskNotifyGiveFromISR(uwbTaskHandle, &xHigherPriorityTaskWoken);
+
+// if(xHigherPriorityTaskWoken) {
+// portYIELD();
+// }
+// }
+
+//COMMENTED FIRMWARE
+// static void spiSetSpeed(dwDevice_t* dev, dwSpiSpeed_t speed)
+// {
+// if (speed == dwSpiSpeedLow)
+// {
+// spiSpeed = SPI_BAUDRATE_2MHZ;
+// }
+// else if (speed == dwSpiSpeedHigh)
+// {
+// spiSpeed = SPI_BAUDRATE_21MHZ;
+// }
+// }
+
+
+//COMMENTED FIRMWARE
+// static void delayms(dwDevice_t* dev, unsigned int delay)
+// {
+// vTaskDelay(M2T(delay));
+// }
+
+//COMMENTED FIRMWARE
+// static dwOps_t dwOps = {
+// .spiRead = spiRead,
+// .spiWrite = spiWrite,
+// .spiSetSpeed = spiSetSpeed,
+// .delayms = delayms,
+// };
-static void spiRead(dwDevice_t* dev, const void *header, size_t headerLength,
- void* data, size_t dataLength)
-{
- spiBeginTransaction(spiSpeed);
- digitalWrite(CS_PIN, LOW);
- memcpy(spiTxBuffer, header, headerLength);
- memset(spiTxBuffer+headerLength, 0, dataLength);
- spiExchange(headerLength+dataLength, spiTxBuffer, spiRxBuffer);
- memcpy(data, spiRxBuffer+headerLength, dataLength);
- digitalWrite(CS_PIN, HIGH);
- spiEndTransaction();
- STATS_CNT_RATE_EVENT(&spiReadCount);
-}
+/*********** Deck driver initialization ***************/
-#if LOCODECK_USE_ALT_PINS
- void __attribute__((used)) EXTI5_Callback(void)
-#else
- void __attribute__((used)) EXTI11_Callback(void)
-#endif
- {
- portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+static void dwm1000Init(DeckInfo *info)
+{
+ //COMMENTED FIRMWARE
+ // EXTI_InitTypeDef EXTI_InitStructure;
- // Unlock interrupt handling task
- vTaskNotifyGiveFromISR(uwbTaskHandle, &xHigherPriorityTaskWoken);
+ // spiBegin();
- if(xHigherPriorityTaskWoken) {
- portYIELD();
- }
- }
+ // // Set up interrupt
+ // SYSCFG_EXTILineConfig(EXTI_PortSource, EXTI_PinSource);
-static void spiSetSpeed(dwDevice_t* dev, dwSpiSpeed_t speed)
-{
- if (speed == dwSpiSpeedLow)
- {
- spiSpeed = SPI_BAUDRATE_2MHZ;
- }
- else if (speed == dwSpiSpeedHigh)
- {
- spiSpeed = SPI_BAUDRATE_21MHZ;
- }
-}
+ // EXTI_InitStructure.EXTI_Line = EXTI_LineN;
+ // EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+ // EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
+ // EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+ // EXTI_Init(&EXTI_InitStructure);
-static void delayms(dwDevice_t* dev, unsigned int delay)
-{
- vTaskDelay(M2T(delay));
-}
+ // // Init pins
+ // pinMode(CS_PIN, OUTPUT);
+ // pinMode(GPIO_PIN_RESET, OUTPUT);
+ // pinMode(GPIO_PIN_IRQ, INPUT);
-static dwOps_t dwOps = {
- .spiRead = spiRead,
- .spiWrite = spiWrite,
- .spiSetSpeed = spiSetSpeed,
- .delayms = delayms,
-};
+ // // Reset the DW1000 chip
+ // digitalWrite(GPIO_PIN_RESET, 0);
+ // vTaskDelay(M2T(10));
+ // digitalWrite(GPIO_PIN_RESET, 1);
+ // vTaskDelay(M2T(10));
-/*********** Deck driver initialization ***************/
+ // // Initialize the driver
+ // dwInit(dwm, &dwOps); // Init libdw
-static void dwm1000Init(DeckInfo *info)
-{
- EXTI_InitTypeDef EXTI_InitStructure;
-
- spiBegin();
-
- // Set up interrupt
- SYSCFG_EXTILineConfig(EXTI_PortSource, EXTI_PinSource);
-
- EXTI_InitStructure.EXTI_Line = EXTI_LineN;
- EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
- EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
- EXTI_InitStructure.EXTI_LineCmd = ENABLE;
- EXTI_Init(&EXTI_InitStructure);
-
- // Init pins
- pinMode(CS_PIN, OUTPUT);
- pinMode(GPIO_PIN_RESET, OUTPUT);
- pinMode(GPIO_PIN_IRQ, INPUT);
-
- // Reset the DW1000 chip
- digitalWrite(GPIO_PIN_RESET, 0);
- vTaskDelay(M2T(10));
- digitalWrite(GPIO_PIN_RESET, 1);
- vTaskDelay(M2T(10));
-
- // Initialize the driver
- dwInit(dwm, &dwOps); // Init libdw
-
- int result = dwConfigure(dwm);
- if (result != 0) {
- isInit = false;
- DEBUG_PRINT("Failed to configure DW1000!\r\n");
- return;
- }
+ // int result = dwConfigure(dwm);
+ // if (result != 0) {
+ // isInit = false;
+ // DEBUG_PRINT("Failed to configure DW1000!\r\n");
+ // return;
+ // }
- dwEnableAllLeds(dwm);
+ // dwEnableAllLeds(dwm);
- dwTime_t delay = {.full = 0};
- dwSetAntenaDelay(dwm, delay);
+ // dwTime_t delay = {.full = 0};
+ // dwSetAntenaDelay(dwm, delay);
- dwAttachSentHandler(dwm, txCallback);
- dwAttachReceivedHandler(dwm, rxCallback);
- dwAttachReceiveTimeoutHandler(dwm, rxTimeoutCallback);
+ // dwAttachSentHandler(dwm, txCallback);
+ // dwAttachReceivedHandler(dwm, rxCallback);
+ // dwAttachReceiveTimeoutHandler(dwm, rxTimeoutCallback);
- dwNewConfiguration(dwm);
- dwSetDefaults(dwm);
+ // dwNewConfiguration(dwm);
+ // dwSetDefaults(dwm);
- #ifdef LPS_LONGER_RANGE
- dwEnableMode(dwm, MODE_SHORTDATA_MID_ACCURACY);
- #else
- dwEnableMode(dwm, MODE_SHORTDATA_FAST_ACCURACY);
- #endif
+ // #ifdef LPS_LONGER_RANGE
+ // dwEnableMode(dwm, MODE_SHORTDATA_MID_ACCURACY);
+ // #else
+ // dwEnableMode(dwm, MODE_SHORTDATA_FAST_ACCURACY);
+ // #endif
- dwSetChannel(dwm, CHANNEL_2);
- dwSetPreambleCode(dwm, PREAMBLE_CODE_64MHZ_9);
+ // dwSetChannel(dwm, CHANNEL_2);
+ // dwSetPreambleCode(dwm, PREAMBLE_CODE_64MHZ_9);
- #ifdef LPS_FULL_TX_POWER
- dwUseSmartPower(dwm, false);
- dwSetTxPower(dwm, 0x1F1F1F1Ful);
- #else
- dwUseSmartPower(dwm, true);
- #endif
+ // #ifdef LPS_FULL_TX_POWER
+ // dwUseSmartPower(dwm, false);
+ // dwSetTxPower(dwm, 0x1F1F1F1Ful);
+ // #else
+ // dwUseSmartPower(dwm, true);
+ // #endif
- dwSetReceiveWaitTimeout(dwm, DEFAULT_RX_TIMEOUT);
+ // dwSetReceiveWaitTimeout(dwm, DEFAULT_RX_TIMEOUT);
- dwCommitConfiguration(dwm);
+ // dwCommitConfiguration(dwm);
- memoryRegisterHandler(&memDef);
+ // memoryRegisterHandler(&memDef);
- algoSemaphore= xSemaphoreCreateMutex();
+ // algoSemaphore= xSemaphoreCreateMutex();
- xTaskCreate(uwbTask, LPS_DECK_TASK_NAME, 3 * configMINIMAL_STACK_SIZE, NULL,
- LPS_DECK_TASK_PRI, &uwbTaskHandle);
+ // xTaskCreate(uwbTask, LPS_DECK_TASK_NAME, 3 * configMINIMAL_STACK_SIZE, NULL,
+ // LPS_DECK_TASK_PRI, &uwbTaskHandle);
- isInit = true;
+ // isInit = true;
}
uint16_t locoDeckGetRangingState() {
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa2Tag.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa2Tag.c
index a49993f78990665b3072bbe7e018c703a5899284..8239867dd60b1930f5273ee8abc25ab09f3b205d 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa2Tag.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa2Tag.c
@@ -151,127 +151,128 @@ static void handleLppPacket(const int dataLength, const packet_t* rxPacket, tdoa
}
// Send an LPP packet, the radio will automatically go back in RX mode
-static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
-{
- static packet_t txPacket;
- dwIdle(dev);
-
- MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
-
- txPacket.payload[LPS_TDOA2_TYPE_INDEX] = LPP_HEADER_SHORT_PACKET;
- memcpy(&txPacket.payload[LPS_TDOA2_SEND_LPP_PAYLOAD_INDEX], packet->data, packet->length);
-
- txPacket.pan = 0xbccf;
- txPacket.sourceAddress = 0xbccf000000000000 | 0xff;
- txPacket.destAddress = options->anchorAddress[packet->dest];
-
- dwNewTransmit(dev);
- dwSetDefaults(dev);
- dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
-
- dwWaitForResponse(dev, true);
- dwStartTransmit(dev);
-}
-
-static bool rxcallback(dwDevice_t *dev) {
- tdoaStats_t* stats = &tdoaEngineState.stats;
- STATS_CNT_RATE_EVENT(&stats->packetsReceived);
-
- int dataLength = dwGetDataLength(dev);
- packet_t rxPacket;
-
- dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
- const rangePacket2_t* packet = (rangePacket2_t*)rxPacket.payload;
-
- bool lppSent = false;
- if (packet->type == PACKET_TYPE_TDOA2) {
- const uint8_t anchor = rxPacket.sourceAddress & 0xff;
-
- // Check if we need to send the current LPP packet
- if (lppPacketToSend && lppPacket.dest == anchor) {
- sendLppShort(dev, &lppPacket);
- lppSent = true;
- }
-
- dwTime_t arrival = {.full = 0};
- dwGetReceiveTimestamp(dev, &arrival);
-
- if (anchor < LOCODECK_NR_OF_TDOA2_ANCHORS) {
- uint32_t now_ms = T2M(xTaskGetTickCount());
-
- const int64_t rxAn_by_T_in_cl_T = arrival.full;
- const int64_t txAn_in_cl_An = packet->timestamps[anchor];
- const uint8_t seqNr = packet->sequenceNrs[anchor] & 0x7f;
-
- tdoaAnchorContext_t anchorCtx;
- tdoaEngineGetAnchorCtxForPacketProcessing(&tdoaEngineState, anchor, now_ms, &anchorCtx);
- updateRemoteData(&anchorCtx, packet);
- tdoaEngineProcessPacket(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
- tdoaStorageSetRxTxData(&anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
-
- logClockCorrection[anchor] = tdoaStorageGetClockCorrection(&anchorCtx);
-
- previousAnchor = anchor;
-
- handleLppPacket(dataLength, &rxPacket, &anchorCtx);
-
- rangingOk = true;
- }
- }
-
- return lppSent;
-}
-
-static void setRadioInReceiveMode(dwDevice_t *dev) {
- dwNewReceive(dev);
- dwSetDefaults(dev);
- dwStartReceive(dev);
-}
-
-static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
- switch(event) {
- case eventPacketReceived:
- if (rxcallback(dev)) {
- lppPacketToSend = false;
- } else {
- setRadioInReceiveMode(dev);
-
- // Discard lpp packet if we cannot send it for too long
- if (++lppPacketSendTryCounter >= TDOA2_LPP_PACKET_SEND_TIMEOUT) {
- lppPacketToSend = false;
- }
- }
-
- if (!lppPacketToSend) {
- // Get next lpp packet
- lppPacketToSend = lpsGetLppShort(&lppPacket);
- lppPacketSendTryCounter = 0;
- }
- break;
- case eventTimeout:
- setRadioInReceiveMode(dev);
- break;
- case eventReceiveTimeout:
- setRadioInReceiveMode(dev);
- break;
- case eventPacketSent:
- // Service packet sent, the radio is back to receive automatically
- break;
- default:
- ASSERT_FAILED();
- }
-
- uint32_t now = xTaskGetTickCount();
- uint16_t rangingState = 0;
- for (int anchor = 0; anchor < LOCODECK_NR_OF_TDOA2_ANCHORS; anchor++) {
- if (now < history[anchor].anchorStatusTimeout) {
- rangingState |= (1 << anchor);
- }
- }
- locoDeckSetRangingState(rangingState);
-
- return MAX_TIMEOUT;
-}
+//COMMENTED FIRMWARE
+// static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
+// {
+// static packet_t txPacket;
+// dwIdle(dev);
+
+// MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
+
+// txPacket.payload[LPS_TDOA2_TYPE_INDEX] = LPP_HEADER_SHORT_PACKET;
+// memcpy(&txPacket.payload[LPS_TDOA2_SEND_LPP_PAYLOAD_INDEX], packet->data, packet->length);
+
+// txPacket.pan = 0xbccf;
+// txPacket.sourceAddress = 0xbccf000000000000 | 0xff;
+// txPacket.destAddress = options->anchorAddress[packet->dest];
+
+// dwNewTransmit(dev);
+// dwSetDefaults(dev);
+// dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
+
+// dwWaitForResponse(dev, true);
+// dwStartTransmit(dev);
+// }
+
+// static bool rxcallback(dwDevice_t *dev) {
+// tdoaStats_t* stats = &tdoaEngineState.stats;
+// STATS_CNT_RATE_EVENT(&stats->packetsReceived);
+
+// int dataLength = dwGetDataLength(dev);
+// packet_t rxPacket;
+
+// dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
+// const rangePacket2_t* packet = (rangePacket2_t*)rxPacket.payload;
+
+// bool lppSent = false;
+// if (packet->type == PACKET_TYPE_TDOA2) {
+// const uint8_t anchor = rxPacket.sourceAddress & 0xff;
+
+// // Check if we need to send the current LPP packet
+// if (lppPacketToSend && lppPacket.dest == anchor) {
+// sendLppShort(dev, &lppPacket);
+// lppSent = true;
+// }
+
+// dwTime_t arrival = {.full = 0};
+// dwGetReceiveTimestamp(dev, &arrival);
+
+// if (anchor < LOCODECK_NR_OF_TDOA2_ANCHORS) {
+// uint32_t now_ms = T2M(xTaskGetTickCount());
+
+// const int64_t rxAn_by_T_in_cl_T = arrival.full;
+// const int64_t txAn_in_cl_An = packet->timestamps[anchor];
+// const uint8_t seqNr = packet->sequenceNrs[anchor] & 0x7f;
+
+// tdoaAnchorContext_t anchorCtx;
+// tdoaEngineGetAnchorCtxForPacketProcessing(&tdoaEngineState, anchor, now_ms, &anchorCtx);
+// updateRemoteData(&anchorCtx, packet);
+// tdoaEngineProcessPacket(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
+// tdoaStorageSetRxTxData(&anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
+
+// logClockCorrection[anchor] = tdoaStorageGetClockCorrection(&anchorCtx);
+
+// previousAnchor = anchor;
+
+// handleLppPacket(dataLength, &rxPacket, &anchorCtx);
+
+// rangingOk = true;
+// }
+// }
+
+// return lppSent;
+// }
+
+// static void setRadioInReceiveMode(dwDevice_t *dev) {
+// dwNewReceive(dev);
+// dwSetDefaults(dev);
+// dwStartReceive(dev);
+// }
+
+// static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
+// switch(event) {
+// case eventPacketReceived:
+// if (rxcallback(dev)) {
+// lppPacketToSend = false;
+// } else {
+// setRadioInReceiveMode(dev);
+
+// // Discard lpp packet if we cannot send it for too long
+// if (++lppPacketSendTryCounter >= TDOA2_LPP_PACKET_SEND_TIMEOUT) {
+// lppPacketToSend = false;
+// }
+// }
+
+// if (!lppPacketToSend) {
+// // Get next lpp packet
+// lppPacketToSend = lpsGetLppShort(&lppPacket);
+// lppPacketSendTryCounter = 0;
+// }
+// break;
+// case eventTimeout:
+// setRadioInReceiveMode(dev);
+// break;
+// case eventReceiveTimeout:
+// setRadioInReceiveMode(dev);
+// break;
+// case eventPacketSent:
+// // Service packet sent, the radio is back to receive automatically
+// break;
+// default:
+// ASSERT_FAILED();
+// }
+
+// uint32_t now = xTaskGetTickCount();
+// uint16_t rangingState = 0;
+// for (int anchor = 0; anchor < LOCODECK_NR_OF_TDOA2_ANCHORS; anchor++) {
+// if (now < history[anchor].anchorStatusTimeout) {
+// rangingState |= (1 << anchor);
+// }
+// }
+// locoDeckSetRangingState(rangingState);
+
+// return MAX_TIMEOUT;
+// }
static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
@@ -294,22 +295,22 @@ static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
}
}
+//COMMENTED FIRMWARE
+// static void Initialize(dwDevice_t *dev) {
+// uint32_t now_ms = T2M(xTaskGetTickCount());
+// tdoaEngineInit(&tdoaEngineState, now_ms, sendTdoaToEstimatorCallback, LOCODECK_TS_FREQ, TdoaEngineMatchingAlgorithmYoungest);
-static void Initialize(dwDevice_t *dev) {
- uint32_t now_ms = T2M(xTaskGetTickCount());
- tdoaEngineInit(&tdoaEngineState, now_ms, sendTdoaToEstimatorCallback, LOCODECK_TS_FREQ, TdoaEngineMatchingAlgorithmYoungest);
+// previousAnchor = 0;
- previousAnchor = 0;
+// lppPacketToSend = false;
- lppPacketToSend = false;
+// locoDeckSetRangingState(0);
+// dwSetReceiveWaitTimeout(dev, TDOA2_RECEIVE_TIMEOUT);
- locoDeckSetRangingState(0);
- dwSetReceiveWaitTimeout(dev, TDOA2_RECEIVE_TIMEOUT);
+// dwCommitConfiguration(dev);
- dwCommitConfiguration(dev);
-
- rangingOk = false;
-}
+// rangingOk = false;
+// }
static bool isRangingOk()
{
@@ -351,14 +352,14 @@ static void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data, td
}
}
-uwbAlgorithm_t uwbTdoa2TagAlgorithm = {
- .init = Initialize,
- .onEvent = onEvent,
- .isRangingOk = isRangingOk,
- .getAnchorPosition = getAnchorPosition,
- .getAnchorIdList = getAnchorIdList,
- .getActiveAnchorIdList = getActiveAnchorIdList,
-};
+// uwbAlgorithm_t uwbTdoa2TagAlgorithm = {
+// .init = Initialize,
+// .onEvent = onEvent,
+// .isRangingOk = isRangingOk,
+// .getAnchorPosition = getAnchorPosition,
+// .getAnchorIdList = getAnchorIdList,
+// .getActiveAnchorIdList = getActiveAnchorIdList,
+// };
void lpsTdoa2TagSetOptions(lpsTdoa2AlgoOptions_t* newOptions) {
options = newOptions;
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa3Tag.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa3Tag.c
index 51b0a1038e0d169ff011f35cd955de313017294f..0b32ecbf7748b58b6806b465aed6c1fe46ac896d 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa3Tag.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa3Tag.c
@@ -52,7 +52,7 @@ The implementation must handle
#include "tdoaStats.h"
#include "estimator.h"
-#include "libdw1000.h"
+//#include "libdw1000.h"
#include "mac.h"
#define DEBUG_MODULE "TDOA3"
@@ -162,100 +162,101 @@ static void handleLppPacket(const int dataLength, int rangePacketLength, const p
}
}
-static void rxcallback(dwDevice_t *dev) {
- tdoaStats_t* stats = &tdoaEngineState.stats;
- STATS_CNT_RATE_EVENT(&stats->packetsReceived);
-
- int dataLength = dwGetDataLength(dev);
- packet_t rxPacket;
-
- dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
- const uint8_t anchorId = rxPacket.sourceAddress & 0xff;
-
- dwTime_t arrival = {.full = 0};
- dwGetReceiveTimestamp(dev, &arrival);
- const int64_t rxAn_by_T_in_cl_T = arrival.full;
-
- const rangePacket3_t* packet = (rangePacket3_t*)rxPacket.payload;
- if (packet->header.type == PACKET_TYPE_TDOA3) {
- const int64_t txAn_in_cl_An = packet->header.txTimeStamp;;
- const uint8_t seqNr = packet->header.seq & 0x7f;;
-
- tdoaAnchorContext_t anchorCtx;
- uint32_t now_ms = T2M(xTaskGetTickCount());
-
- tdoaEngineGetAnchorCtxForPacketProcessing(&tdoaEngineState, anchorId, now_ms, &anchorCtx);
- int rangeDataLength = updateRemoteData(&anchorCtx, packet);
- tdoaEngineProcessPacket(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
- tdoaStorageSetRxTxData(&anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
- handleLppPacket(dataLength, rangeDataLength, &rxPacket, &anchorCtx);
-
- rangingOk = true;
- }
-}
-
-static void setRadioInReceiveMode(dwDevice_t *dev) {
- dwNewReceive(dev);
- dwSetDefaults(dev);
- dwStartReceive(dev);
-}
-
-static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
-{
- static packet_t txPacket;
- dwIdle(dev);
-
- MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
-
- txPacket.payload[LPS_TDOA3_TYPE] = LPP_HEADER_SHORT_PACKET;
- memcpy(&txPacket.payload[LPS_TDOA3_SEND_LPP_PAYLOAD], packet->data, packet->length);
-
- txPacket.pan = 0xbccf;
- txPacket.sourceAddress = 0xbccf000000000000 | 0xff;
- txPacket.destAddress = 0xbccf000000000000 | packet->dest;
-
- dwNewTransmit(dev);
- dwSetDefaults(dev);
- dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
-
- dwStartTransmit(dev);
-}
-
-static bool sendLpp(dwDevice_t *dev) {
- bool lppPacketToSend = lpsGetLppShort(&lppPacket);
- if (lppPacketToSend) {
- sendLppShort(dev, &lppPacket);
- return true;
- }
-
- return false;
-}
-
-static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
- switch(event) {
- case eventPacketReceived:
- rxcallback(dev);
- break;
- case eventTimeout:
- break;
- case eventReceiveTimeout:
- break;
- case eventPacketSent:
- // Service packet sent, the radio is back to receive automatically
- break;
- default:
- ASSERT_FAILED();
- }
-
- if(!sendLpp(dev)) {
- setRadioInReceiveMode(dev);
- }
-
- uint32_t now_ms = T2M(xTaskGetTickCount());
- tdoaStatsUpdate(&tdoaEngineState.stats, now_ms);
-
- return MAX_TIMEOUT;
-}
+//COMMENTED FIRMWARE
+// static void rxcallback(dwDevice_t *dev) {
+// tdoaStats_t* stats = &tdoaEngineState.stats;
+// STATS_CNT_RATE_EVENT(&stats->packetsReceived);
+
+// int dataLength = dwGetDataLength(dev);
+// packet_t rxPacket;
+
+// dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
+// const uint8_t anchorId = rxPacket.sourceAddress & 0xff;
+
+// dwTime_t arrival = {.full = 0};
+// dwGetReceiveTimestamp(dev, &arrival);
+// const int64_t rxAn_by_T_in_cl_T = arrival.full;
+
+// const rangePacket3_t* packet = (rangePacket3_t*)rxPacket.payload;
+// if (packet->header.type == PACKET_TYPE_TDOA3) {
+// const int64_t txAn_in_cl_An = packet->header.txTimeStamp;;
+// const uint8_t seqNr = packet->header.seq & 0x7f;;
+
+// tdoaAnchorContext_t anchorCtx;
+// uint32_t now_ms = T2M(xTaskGetTickCount());
+
+// tdoaEngineGetAnchorCtxForPacketProcessing(&tdoaEngineState, anchorId, now_ms, &anchorCtx);
+// int rangeDataLength = updateRemoteData(&anchorCtx, packet);
+// tdoaEngineProcessPacket(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
+// tdoaStorageSetRxTxData(&anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
+// handleLppPacket(dataLength, rangeDataLength, &rxPacket, &anchorCtx);
+
+// rangingOk = true;
+// }
+// }
+
+// static void setRadioInReceiveMode(dwDevice_t *dev) {
+// dwNewReceive(dev);
+// dwSetDefaults(dev);
+// dwStartReceive(dev);
+// }
+
+// static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
+// {
+// static packet_t txPacket;
+// dwIdle(dev);
+
+// MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
+
+// txPacket.payload[LPS_TDOA3_TYPE] = LPP_HEADER_SHORT_PACKET;
+// memcpy(&txPacket.payload[LPS_TDOA3_SEND_LPP_PAYLOAD], packet->data, packet->length);
+
+// txPacket.pan = 0xbccf;
+// txPacket.sourceAddress = 0xbccf000000000000 | 0xff;
+// txPacket.destAddress = 0xbccf000000000000 | packet->dest;
+
+// dwNewTransmit(dev);
+// dwSetDefaults(dev);
+// dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
+
+// dwStartTransmit(dev);
+// }
+
+// static bool sendLpp(dwDevice_t *dev) {
+// bool lppPacketToSend = lpsGetLppShort(&lppPacket);
+// if (lppPacketToSend) {
+// sendLppShort(dev, &lppPacket);
+// return true;
+// }
+
+// return false;
+// }
+
+// static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
+// switch(event) {
+// case eventPacketReceived:
+// rxcallback(dev);
+// break;
+// case eventTimeout:
+// break;
+// case eventReceiveTimeout:
+// break;
+// case eventPacketSent:
+// // Service packet sent, the radio is back to receive automatically
+// break;
+// default:
+// ASSERT_FAILED();
+// }
+
+// if(!sendLpp(dev)) {
+// setRadioInReceiveMode(dev);
+// }
+
+// uint32_t now_ms = T2M(xTaskGetTickCount());
+// tdoaStatsUpdate(&tdoaEngineState.stats, now_ms);
+
+// return MAX_TIMEOUT;
+// }
static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
estimatorEnqueueTDOA(tdoaMeasurement);
@@ -293,31 +294,33 @@ static uint8_t getActiveAnchorIdList(uint8_t unorderedAnchorList[], const int ma
return tdoaStorageGetListOfActiveAnchorIds(tdoaEngineState.anchorInfoArray, unorderedAnchorList, maxListSize, now_ms);
}
-static void Initialize(dwDevice_t *dev) {
- uint32_t now_ms = T2M(xTaskGetTickCount());
- tdoaEngineInit(&tdoaEngineState, now_ms, sendTdoaToEstimatorCallback, LOCODECK_TS_FREQ, TdoaEngineMatchingAlgorithmRandom);
+//COMMENTED FIRMWARE
+// static void Initialize(dwDevice_t *dev) {
+// uint32_t now_ms = T2M(xTaskGetTickCount());
+// tdoaEngineInit(&tdoaEngineState, now_ms, sendTdoaToEstimatorCallback, LOCODECK_TS_FREQ, TdoaEngineMatchingAlgorithmRandom);
- #ifdef LPS_2D_POSITION_HEIGHT
- DEBUG_PRINT("2D positioning enabled at %f m height\n", LPS_2D_POSITION_HEIGHT);
- #endif
+// #ifdef LPS_2D_POSITION_HEIGHT
+// DEBUG_PRINT("2D positioning enabled at %f m height\n", LPS_2D_POSITION_HEIGHT);
+// #endif
- dwSetReceiveWaitTimeout(dev, TDOA3_RECEIVE_TIMEOUT);
+// dwSetReceiveWaitTimeout(dev, TDOA3_RECEIVE_TIMEOUT);
- dwCommitConfiguration(dev);
+// dwCommitConfiguration(dev);
- rangingOk = false;
-}
+// rangingOk = false;
+// }
static bool isRangingOk()
{
return rangingOk;
}
-uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
- .init = Initialize,
- .onEvent = onEvent,
- .isRangingOk = isRangingOk,
- .getAnchorPosition = getAnchorPosition,
- .getAnchorIdList = getAnchorIdList,
- .getActiveAnchorIdList = getActiveAnchorIdList,
-};
+//COMMENTED FIRMWARE
+// uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
+// .init = Initialize,
+// .onEvent = onEvent,
+// .isRangingOk = isRangingOk,
+// .getAnchorPosition = getAnchorPosition,
+// .getAnchorIdList = getAnchorIdList,
+// .getActiveAnchorIdList = getActiveAnchorIdList,
+// };
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTwrTag.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTwrTag.c
index d99344dcc23ede3b3458a2d3e4ffdb281f8ae286..bed043dd815dc6b657632978fe2f06c96ab92b5d 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTwrTag.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTwrTag.c
@@ -99,10 +99,12 @@ static lpsTwrAlgoOptions_t* options = &defaultOptions;
// Outlier rejection
#define RANGING_HISTORY_LENGTH 32
#define OUTLIER_TH 4
-NO_DMA_CCM_SAFE_ZERO_INIT static struct {
- float32_t history[RANGING_HISTORY_LENGTH];
- size_t ptr;
-} rangingStats[LOCODECK_NR_OF_TWR_ANCHORS];
+
+//COMMENTED FIRMWARE
+// NO_DMA_CCM_SAFE_ZERO_INIT static struct {
+// float32_t history[RANGING_HISTORY_LENGTH];
+// size_t ptr;
+// } rangingStats[LOCODECK_NR_OF_TWR_ANCHORS];
// Rangin statistics
static uint8_t rangingPerSec[LOCODECK_NR_OF_TWR_ANCHORS];
@@ -112,12 +114,13 @@ static uint8_t succededRanging[LOCODECK_NR_OF_TWR_ANCHORS];
static uint8_t failedRanging[LOCODECK_NR_OF_TWR_ANCHORS];
// Timestamps for ranging
-static dwTime_t poll_tx;
-static dwTime_t poll_rx;
-static dwTime_t answer_tx;
-static dwTime_t answer_rx;
-static dwTime_t final_tx;
-static dwTime_t final_rx;
+//COMMENTED FIRMWARE
+// static dwTime_t poll_tx;
+// static dwTime_t poll_rx;
+// static dwTime_t answer_tx;
+// static dwTime_t answer_rx;
+// static dwTime_t final_tx;
+// static dwTime_t final_rx;
static packet_t txPacket;
static volatile uint8_t curr_seq = 0;
@@ -130,309 +133,313 @@ static lpsLppShortPacket_t lppShortPacket;
// TDMA handling
static bool tdmaSynchronized;
-static dwTime_t frameStart;
+//COMMENTED FIRMWARE
+// static dwTime_t frameStart;
static bool rangingOk;
static void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data);
-static void txcallback(dwDevice_t *dev)
-{
- dwTime_t departure;
- dwGetTransmitTimestamp(dev, &departure);
- departure.full += (options->antennaDelay / 2);
-
- switch (txPacket.payload[0]) {
- case LPS_TWR_POLL:
- poll_tx = departure;
- break;
- case LPS_TWR_FINAL:
- final_tx = departure;
- break;
- }
-}
-
-
-static uint32_t rxcallback(dwDevice_t *dev) {
- dwTime_t arival = { .full=0 };
- int dataLength = dwGetDataLength(dev);
-
- if (dataLength == 0) return 0;
-
- packet_t rxPacket;
- memset(&rxPacket, 0, MAC802154_HEADER_LENGTH);
-
- dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
-
- if (rxPacket.destAddress != options->tagAddress) {
- dwNewReceive(dev);
- dwSetDefaults(dev);
- dwStartReceive(dev);
- return MAX_TIMEOUT;
- }
-
- txPacket.destAddress = rxPacket.sourceAddress;
- txPacket.sourceAddress = rxPacket.destAddress;
-
- switch(rxPacket.payload[LPS_TWR_TYPE]) {
- // Tag received messages
- case LPS_TWR_ANSWER:
- if (rxPacket.payload[LPS_TWR_SEQ] != curr_seq) {
- return 0;
- }
-
- if (dataLength - MAC802154_HEADER_LENGTH > 3) {
- if (rxPacket.payload[LPS_TWR_LPP_HEADER] == LPP_HEADER_SHORT_PACKET) {
- int srcId = -1;
-
- for (int i=0; i<LOCODECK_NR_OF_TWR_ANCHORS; i++) {
- if (rxPacket.sourceAddress == options->anchorAddress[i]) {
- srcId = i;
- break;
- }
- }
-
- if (srcId >= 0) {
- lpsHandleLppShortPacket(srcId, &rxPacket.payload[LPS_TWR_LPP_TYPE]);
- }
- }
- }
-
- txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_FINAL;
- txPacket.payload[LPS_TWR_SEQ] = rxPacket.payload[LPS_TWR_SEQ];
-
- dwGetReceiveTimestamp(dev, &arival);
- arival.full -= (options->antennaDelay / 2);
- answer_rx = arival;
-
- dwNewTransmit(dev);
- dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);
-
- dwWaitForResponse(dev, true);
- dwStartTransmit(dev);
-
- break;
- case LPS_TWR_REPORT:
- {
- lpsTwrTagReportPayload_t *report = (lpsTwrTagReportPayload_t *)(rxPacket.payload+2);
- double tround1, treply1, treply2, tround2, tprop_ctn, tprop;
-
- if (rxPacket.payload[LPS_TWR_SEQ] != curr_seq) {
- return 0;
- }
-
- memcpy(&poll_rx, &report->pollRx, 5);
- memcpy(&answer_tx, &report->answerTx, 5);
- memcpy(&final_rx, &report->finalRx, 5);
-
- tround1 = answer_rx.low32 - poll_tx.low32;
- treply1 = answer_tx.low32 - poll_rx.low32;
- tround2 = final_rx.low32 - answer_tx.low32;
- treply2 = final_tx.low32 - answer_rx.low32;
-
- tprop_ctn = ((tround1*tround2) - (treply1*treply2)) / (tround1 + tround2 + treply1 + treply2);
-
- tprop = tprop_ctn / LOCODECK_TS_FREQ;
- state.distance[current_anchor] = SPEED_OF_LIGHT * tprop;
- state.pressures[current_anchor] = report->asl;
-
- // Outliers rejection
- rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
- float32_t mean;
- float32_t stddev;
-
- arm_std_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &stddev);
- arm_mean_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &mean);
- float32_t diff = fabsf(mean - state.distance[current_anchor]);
-
- rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = state.distance[current_anchor];
-
- rangingOk = true;
-
- if ((options->combinedAnchorPositionOk || options->anchorPosition[current_anchor].timestamp) &&
- (diff < (OUTLIER_TH*stddev))) {
- distanceMeasurement_t dist;
- dist.distance = state.distance[current_anchor];
- dist.x = options->anchorPosition[current_anchor].x;
- dist.y = options->anchorPosition[current_anchor].y;
- dist.z = options->anchorPosition[current_anchor].z;
- dist.anchorId = current_anchor;
- dist.stdDev = 0.25;
- estimatorEnqueueDistance(&dist);
- }
-
- if (options->useTdma && current_anchor == 0) {
- // Final packet is sent by us and received by the anchor
- // We use it as synchonisation time for TDMA
- dwTime_t offset = { .full =final_tx.full - final_rx.full };
- frameStart.full = TDMA_LAST_FRAME(final_rx.full) + offset.full;
- tdmaSynchronized = true;
- }
-
- ranging_complete = true;
-
- return 0;
- break;
- }
- }
- return MAX_TIMEOUT;
-}
-
+//COMMENTED FIRMWARE
+// static void txcallback(dwDevice_t *dev)
+// {
+// dwTime_t departure;
+// dwGetTransmitTimestamp(dev, &departure);
+// departure.full += (options->antennaDelay / 2);
+
+// switch (txPacket.payload[0]) {
+// case LPS_TWR_POLL:
+// poll_tx = departure;
+// break;
+// case LPS_TWR_FINAL:
+// final_tx = departure;
+// break;
+// }
+// }
+
+//COMMENTED FIRMWARE
+// static uint32_t rxcallback(dwDevice_t *dev) {
+// dwTime_t arival = { .full=0 };
+// int dataLength = dwGetDataLength(dev);
+
+// if (dataLength == 0) return 0;
+
+// packet_t rxPacket;
+// memset(&rxPacket, 0, MAC802154_HEADER_LENGTH);
+
+// dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
+
+// if (rxPacket.destAddress != options->tagAddress) {
+// dwNewReceive(dev);
+// dwSetDefaults(dev);
+// dwStartReceive(dev);
+// return MAX_TIMEOUT;
+// }
+
+// txPacket.destAddress = rxPacket.sourceAddress;
+// txPacket.sourceAddress = rxPacket.destAddress;
+
+// switch(rxPacket.payload[LPS_TWR_TYPE]) {
+// // Tag received messages
+// case LPS_TWR_ANSWER:
+// if (rxPacket.payload[LPS_TWR_SEQ] != curr_seq) {
+// return 0;
+// }
+
+// if (dataLength - MAC802154_HEADER_LENGTH > 3) {
+// if (rxPacket.payload[LPS_TWR_LPP_HEADER] == LPP_HEADER_SHORT_PACKET) {
+// int srcId = -1;
+
+// for (int i=0; i<LOCODECK_NR_OF_TWR_ANCHORS; i++) {
+// if (rxPacket.sourceAddress == options->anchorAddress[i]) {
+// srcId = i;
+// break;
+// }
+// }
+
+// if (srcId >= 0) {
+// lpsHandleLppShortPacket(srcId, &rxPacket.payload[LPS_TWR_LPP_TYPE]);
+// }
+// }
+// }
+
+// txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_FINAL;
+// txPacket.payload[LPS_TWR_SEQ] = rxPacket.payload[LPS_TWR_SEQ];
+
+// dwGetReceiveTimestamp(dev, &arival);
+// arival.full -= (options->antennaDelay / 2);
+// answer_rx = arival;
+
+// dwNewTransmit(dev);
+// dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);
+
+// dwWaitForResponse(dev, true);
+// dwStartTransmit(dev);
+
+// break;
+// case LPS_TWR_REPORT:
+// {
+// lpsTwrTagReportPayload_t *report = (lpsTwrTagReportPayload_t *)(rxPacket.payload+2);
+// double tround1, treply1, treply2, tround2, tprop_ctn, tprop;
+
+// if (rxPacket.payload[LPS_TWR_SEQ] != curr_seq) {
+// return 0;
+// }
+
+// memcpy(&poll_rx, &report->pollRx, 5);
+// memcpy(&answer_tx, &report->answerTx, 5);
+// memcpy(&final_rx, &report->finalRx, 5);
+
+// tround1 = answer_rx.low32 - poll_tx.low32;
+// treply1 = answer_tx.low32 - poll_rx.low32;
+// tround2 = final_rx.low32 - answer_tx.low32;
+// treply2 = final_tx.low32 - answer_rx.low32;
+
+// tprop_ctn = ((tround1*tround2) - (treply1*treply2)) / (tround1 + tround2 + treply1 + treply2);
+
+// tprop = tprop_ctn / LOCODECK_TS_FREQ;
+// state.distance[current_anchor] = SPEED_OF_LIGHT * tprop;
+// state.pressures[current_anchor] = report->asl;
+
+// // Outliers rejection
+// rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
+// float32_t mean;
+// float32_t stddev;
+
+// arm_std_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &stddev);
+// arm_mean_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &mean);
+// float32_t diff = fabsf(mean - state.distance[current_anchor]);
+
+// rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = state.distance[current_anchor];
+
+// rangingOk = true;
+
+// if ((options->combinedAnchorPositionOk || options->anchorPosition[current_anchor].timestamp) &&
+// (diff < (OUTLIER_TH*stddev))) {
+// distanceMeasurement_t dist;
+// dist.distance = state.distance[current_anchor];
+// dist.x = options->anchorPosition[current_anchor].x;
+// dist.y = options->anchorPosition[current_anchor].y;
+// dist.z = options->anchorPosition[current_anchor].z;
+// dist.anchorId = current_anchor;
+// dist.stdDev = 0.25;
+// estimatorEnqueueDistance(&dist);
+// }
+
+// if (options->useTdma && current_anchor == 0) {
+// // Final packet is sent by us and received by the anchor
+// // We use it as synchonisation time for TDMA
+// dwTime_t offset = { .full =final_tx.full - final_rx.full };
+// frameStart.full = TDMA_LAST_FRAME(final_rx.full) + offset.full;
+// tdmaSynchronized = true;
+// }
+
+// ranging_complete = true;
+
+// return 0;
+// break;
+// }
+// }
+// return MAX_TIMEOUT;
+// }
+
+//COMMENTED FIRMWARE
/* Adjust time for schedule transfer by DW1000 radio. Set 9 LSB to 0 */
-static uint32_t adjustTxRxTime(dwTime_t *time)
-{
- uint32_t added = (1<<9) - (time->low32 & ((1<<9)-1));
- time->low32 = (time->low32 & ~((1<<9)-1)) + (1<<9);
- return added;
-}
-
-/* Calculate the transmit time for a given timeslot in the current frame */
-static dwTime_t transmitTimeForSlot(int slot)
-{
- dwTime_t transmitTime = { .full = 0 };
- // Calculate start of the slot
- transmitTime.full = frameStart.full + slot*TDMA_SLOT_LEN;
-
- // DW1000 can only schedule time with 9 LSB at 0, adjust for it
- adjustTxRxTime(&transmitTime);
- return transmitTime;
-}
-
-static void initiateRanging(dwDevice_t *dev)
-{
- if (!options->useTdma || tdmaSynchronized) {
- if (options->useTdma) {
- // go to next TDMA frame
- frameStart.full += TDMA_FRAME_LEN;
- }
-
- current_anchor ++;
- if (current_anchor >= LOCODECK_NR_OF_TWR_ANCHORS) {
- current_anchor = 0;
- }
- } else {
- current_anchor = 0;
- }
-
- dwIdle(dev);
-
- txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_POLL;
- txPacket.payload[LPS_TWR_SEQ] = ++curr_seq;
-
- txPacket.sourceAddress = options->tagAddress;
- txPacket.destAddress = options->anchorAddress[current_anchor];
-
- dwNewTransmit(dev);
- dwSetDefaults(dev);
- dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);
-
- if (options->useTdma && tdmaSynchronized) {
- dwTime_t txTime = transmitTimeForSlot(options->tdmaSlot);
- dwSetTxRxTime(dev, txTime);
- }
-
- dwWaitForResponse(dev, true);
- dwStartTransmit(dev);
-}
-
-static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
-{
- dwIdle(dev);
-
- txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_LPP_SHORT;
- memcpy(&txPacket.payload[LPS_TWR_SEND_LPP_PAYLOAD], packet->data, packet->length);
-
- txPacket.sourceAddress = options->tagAddress;
- txPacket.destAddress = options->anchorAddress[packet->dest];
-
- dwNewTransmit(dev);
- dwSetDefaults(dev);
- dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
-
- dwWaitForResponse(dev, false);
- dwStartTransmit(dev);
-}
-
-static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
-{
- static uint32_t statisticStartTick = 0;
-
- if (statisticStartTick == 0) {
- statisticStartTick = xTaskGetTickCount();
- }
-
- switch(event) {
- case eventPacketReceived:
- return rxcallback(dev);
- break;
- case eventPacketSent:
- txcallback(dev);
-
- if (lpp_transaction) {
- return 0;
- }
- return MAX_TIMEOUT;
- break;
- case eventTimeout: // Comes back to timeout after each ranging attempt
- {
- uint16_t rangingState = locoDeckGetRangingState();
- if (!ranging_complete && !lpp_transaction) {
- rangingState &= ~(1<<current_anchor);
- if (state.failedRanging[current_anchor] < options->rangingFailedThreshold) {
- state.failedRanging[current_anchor] ++;
- rangingState |= (1<<current_anchor);
- }
-
- locSrvSendRangeFloat(current_anchor, NAN);
- failedRanging[current_anchor]++;
- } else {
- rangingState |= (1<<current_anchor);
- state.failedRanging[current_anchor] = 0;
-
- locSrvSendRangeFloat(current_anchor, state.distance[current_anchor]);
- succededRanging[current_anchor]++;
- }
- locoDeckSetRangingState(rangingState);
- }
-
- // Handle ranging statistic
- if (xTaskGetTickCount() > (statisticStartTick+1000)) {
- statisticStartTick = xTaskGetTickCount();
-
- for (int i=0; i<LOCODECK_NR_OF_TWR_ANCHORS; i++) {
- rangingPerSec[i] = failedRanging[i] + succededRanging[i];
- if (rangingPerSec[i] > 0) {
- rangingSuccessRate[i] = 100.0f*(float)succededRanging[i] / (float)rangingPerSec[i];
- } else {
- rangingSuccessRate[i] = 0.0f;
- }
-
- failedRanging[i] = 0;
- succededRanging[i] = 0;
- }
- }
-
-
- if (lpsGetLppShort(&lppShortPacket)) {
- lpp_transaction = true;
- sendLppShort(dev, &lppShortPacket);
- } else {
- lpp_transaction = false;
- ranging_complete = false;
- initiateRanging(dev);
- }
- return MAX_TIMEOUT;
- break;
- case eventReceiveTimeout:
- case eventReceiveFailed:
- return 0;
- break;
- default:
- configASSERT(false);
- }
-
- return MAX_TIMEOUT;
-}
+// static uint32_t adjustTxRxTime(dwTime_t *time)
+// {
+// uint32_t added = (1<<9) - (time->low32 & ((1<<9)-1));
+// time->low32 = (time->low32 & ~((1<<9)-1)) + (1<<9);
+// return added;
+// }
+
+// /* Calculate the transmit time for a given timeslot in the current frame */
+// static dwTime_t transmitTimeForSlot(int slot)
+// {
+// dwTime_t transmitTime = { .full = 0 };
+// // Calculate start of the slot
+// transmitTime.full = frameStart.full + slot*TDMA_SLOT_LEN;
+
+// // DW1000 can only schedule time with 9 LSB at 0, adjust for it
+// adjustTxRxTime(&transmitTime);
+// return transmitTime;
+// }
+
+//COMMENTED FIRMWARE
+// static void initiateRanging(dwDevice_t *dev)
+// {
+// if (!options->useTdma || tdmaSynchronized) {
+// if (options->useTdma) {
+// // go to next TDMA frame
+// frameStart.full += TDMA_FRAME_LEN;
+// }
+
+// current_anchor ++;
+// if (current_anchor >= LOCODECK_NR_OF_TWR_ANCHORS) {
+// current_anchor = 0;
+// }
+// } else {
+// current_anchor = 0;
+// }
+
+// dwIdle(dev);
+
+// txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_POLL;
+// txPacket.payload[LPS_TWR_SEQ] = ++curr_seq;
+
+// txPacket.sourceAddress = options->tagAddress;
+// txPacket.destAddress = options->anchorAddress[current_anchor];
+
+// dwNewTransmit(dev);
+// dwSetDefaults(dev);
+// dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);
+
+// if (options->useTdma && tdmaSynchronized) {
+// dwTime_t txTime = transmitTimeForSlot(options->tdmaSlot);
+// dwSetTxRxTime(dev, txTime);
+// }
+
+// dwWaitForResponse(dev, true);
+// dwStartTransmit(dev);
+// }
+
+// static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
+// {
+// dwIdle(dev);
+
+// txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_LPP_SHORT;
+// memcpy(&txPacket.payload[LPS_TWR_SEND_LPP_PAYLOAD], packet->data, packet->length);
+
+// txPacket.sourceAddress = options->tagAddress;
+// txPacket.destAddress = options->anchorAddress[packet->dest];
+
+// dwNewTransmit(dev);
+// dwSetDefaults(dev);
+// dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
+
+// dwWaitForResponse(dev, false);
+// dwStartTransmit(dev);
+// }
+
+// static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
+// {
+// static uint32_t statisticStartTick = 0;
+
+// if (statisticStartTick == 0) {
+// statisticStartTick = xTaskGetTickCount();
+// }
+
+// switch(event) {
+// case eventPacketReceived:
+// return rxcallback(dev);
+// break;
+// case eventPacketSent:
+// txcallback(dev);
+
+// if (lpp_transaction) {
+// return 0;
+// }
+// return MAX_TIMEOUT;
+// break;
+// case eventTimeout: // Comes back to timeout after each ranging attempt
+// {
+// uint16_t rangingState = locoDeckGetRangingState();
+// if (!ranging_complete && !lpp_transaction) {
+// rangingState &= ~(1<<current_anchor);
+// if (state.failedRanging[current_anchor] < options->rangingFailedThreshold) {
+// state.failedRanging[current_anchor] ++;
+// rangingState |= (1<<current_anchor);
+// }
+
+// locSrvSendRangeFloat(current_anchor, NAN);
+// failedRanging[current_anchor]++;
+// } else {
+// rangingState |= (1<<current_anchor);
+// state.failedRanging[current_anchor] = 0;
+
+// locSrvSendRangeFloat(current_anchor, state.distance[current_anchor]);
+// succededRanging[current_anchor]++;
+// }
+// locoDeckSetRangingState(rangingState);
+// }
+
+// // Handle ranging statistic
+// if (xTaskGetTickCount() > (statisticStartTick+1000)) {
+// statisticStartTick = xTaskGetTickCount();
+
+// for (int i=0; i<LOCODECK_NR_OF_TWR_ANCHORS; i++) {
+// rangingPerSec[i] = failedRanging[i] + succededRanging[i];
+// if (rangingPerSec[i] > 0) {
+// rangingSuccessRate[i] = 100.0f*(float)succededRanging[i] / (float)rangingPerSec[i];
+// } else {
+// rangingSuccessRate[i] = 0.0f;
+// }
+
+// failedRanging[i] = 0;
+// succededRanging[i] = 0;
+// }
+// }
+
+
+// if (lpsGetLppShort(&lppShortPacket)) {
+// lpp_transaction = true;
+// sendLppShort(dev, &lppShortPacket);
+// } else {
+// lpp_transaction = false;
+// ranging_complete = false;
+// initiateRanging(dev);
+// }
+// return MAX_TIMEOUT;
+// break;
+// case eventReceiveTimeout:
+// case eventReceiveFailed:
+// return 0;
+// break;
+// default:
+// configASSERT(false);
+// }
+
+// return MAX_TIMEOUT;
+// }
// Loco Posisioning Protocol (LPP) handling
static void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data)
@@ -463,41 +470,41 @@ static void updateTagTdmaSlot(lpsTwrAlgoOptions_t * options)
options->tagAddress += options->tdmaSlot;
}
+//COMMENTED FIRMWARE
+// static void twrTagInit(dwDevice_t *dev)
+// {
+// updateTagTdmaSlot(options);
-static void twrTagInit(dwDevice_t *dev)
-{
- updateTagTdmaSlot(options);
-
- // Initialize the packet in the TX buffer
- memset(&txPacket, 0, sizeof(txPacket));
- MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
- txPacket.pan = 0xbccf;
+// // Initialize the packet in the TX buffer
+// memset(&txPacket, 0, sizeof(txPacket));
+// MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
+// txPacket.pan = 0xbccf;
- memset(&poll_tx, 0, sizeof(poll_tx));
- memset(&poll_rx, 0, sizeof(poll_rx));
- memset(&answer_tx, 0, sizeof(answer_tx));
- memset(&answer_rx, 0, sizeof(answer_rx));
- memset(&final_tx, 0, sizeof(final_tx));
- memset(&final_rx, 0, sizeof(final_rx));
+// memset(&poll_tx, 0, sizeof(poll_tx));
+// memset(&poll_rx, 0, sizeof(poll_rx));
+// memset(&answer_tx, 0, sizeof(answer_tx));
+// memset(&answer_rx, 0, sizeof(answer_rx));
+// memset(&final_tx, 0, sizeof(final_tx));
+// memset(&final_rx, 0, sizeof(final_rx));
- curr_seq = 0;
- current_anchor = 0;
+// curr_seq = 0;
+// current_anchor = 0;
- locoDeckSetRangingState(0);
- ranging_complete = false;
+// locoDeckSetRangingState(0);
+// ranging_complete = false;
- tdmaSynchronized = false;
+// tdmaSynchronized = false;
- memset(state.distance, 0, sizeof(state.distance));
- memset(state.pressures, 0, sizeof(state.pressures));
- memset(state.failedRanging, 0, sizeof(state.failedRanging));
+// memset(state.distance, 0, sizeof(state.distance));
+// memset(state.pressures, 0, sizeof(state.pressures));
+// memset(state.failedRanging, 0, sizeof(state.failedRanging));
- dwSetReceiveWaitTimeout(dev, TWR_RECEIVE_TIMEOUT);
+// dwSetReceiveWaitTimeout(dev, TWR_RECEIVE_TIMEOUT);
- dwCommitConfiguration(dev);
+// dwCommitConfiguration(dev);
- rangingOk = false;
-}
+// rangingOk = false;
+// }
static bool isRangingOk()
{
@@ -542,14 +549,15 @@ static uint8_t getActiveAnchorIdList(uint8_t unorderedAnchorList[], const int ma
return count;
}
-uwbAlgorithm_t uwbTwrTagAlgorithm = {
- .init = twrTagInit,
- .onEvent = twrTagOnEvent,
- .isRangingOk = isRangingOk,
- .getAnchorPosition = getAnchorPosition,
- .getAnchorIdList = getAnchorIdList,
- .getActiveAnchorIdList = getActiveAnchorIdList,
-};
+//COMMENTED FIRMWARE
+// uwbAlgorithm_t uwbTwrTagAlgorithm = {
+// .init = twrTagInit,
+// .onEvent = twrTagOnEvent,
+// .isRangingOk = isRangingOk,
+// .getAnchorPosition = getAnchorPosition,
+// .getAnchorIdList = getAnchorIdList,
+// .getActiveAnchorIdList = getActiveAnchorIdList,
+// };
/**
* Log group for Two Way Ranging data
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptest.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptest.c
index 52a3c7d0164db1d542d241845788582eaeb3240a..f29fa51c4c42f2a78835155cdefc7f7f154386fb 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptest.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptest.c
@@ -81,21 +81,23 @@ typedef struct _etGpio
static EtGpio etGpioIn[ET_NBR_PINS] =
{
- {ET_GPIO_PORT_TX1, ET_GPIO_PIN_TX1, "TX1"},
- {ET_GPIO_PORT_RX1, ET_GPIO_PIN_RX1, "RX1"},
- {ET_GPIO_PORT_TX2, ET_GPIO_PIN_TX2, "TX2"},
- {ET_GPIO_PORT_RX2, ET_GPIO_PIN_RX2, "RX2"},
- {ET_GPIO_PORT_SCK, ET_GPIO_PIN_SCK, "SCK"},
- {ET_GPIO_PORT_MOSI, ET_GPIO_PIN_MOSI, "MOSI"},
- {ET_GPIO_PORT_MISO, ET_GPIO_PIN_MISO, "MISO"},
- {ET_GPIO_PORT_IO1, ET_GPIO_PIN_IO1, "IO1"},
- {ET_GPIO_PORT_IO2, ET_GPIO_PIN_IO2, "IO2"},
- {ET_GPIO_PORT_IO3, ET_GPIO_PIN_IO3, "IO3"},
- {ET_GPIO_PORT_IO4, ET_GPIO_PIN_IO4, "IO4"}
+ //COMMENTED FIRMWARE
+ // {ET_GPIO_PORT_TX1, ET_GPIO_PIN_TX1, "TX1"},
+ // {ET_GPIO_PORT_RX1, ET_GPIO_PIN_RX1, "RX1"},
+ // {ET_GPIO_PORT_TX2, ET_GPIO_PIN_TX2, "TX2"},
+ // {ET_GPIO_PORT_RX2, ET_GPIO_PIN_RX2, "RX2"},
+ // {ET_GPIO_PORT_SCK, ET_GPIO_PIN_SCK, "SCK"},
+ // {ET_GPIO_PORT_MOSI, ET_GPIO_PIN_MOSI, "MOSI"},
+ // {ET_GPIO_PORT_MISO, ET_GPIO_PIN_MISO, "MISO"},
+ // {ET_GPIO_PORT_IO1, ET_GPIO_PIN_IO1, "IO1"},
+ // {ET_GPIO_PORT_IO2, ET_GPIO_PIN_IO2, "IO2"},
+ // {ET_GPIO_PORT_IO3, ET_GPIO_PIN_IO3, "IO3"},
+ // {ET_GPIO_PORT_IO4, ET_GPIO_PIN_IO4, "IO4"}
};
-static EtGpio etGpioSDA = {ET_GPIO_PORT_SDA, ET_GPIO_PIN_SDA, "SDA"};
-static EtGpio etGpioSCL = {ET_GPIO_PORT_SCL, ET_GPIO_PIN_SCL, "SCL"};
+//COMMENTED FIRMWARE
+// static EtGpio etGpioSDA = {ET_GPIO_PORT_SDA, ET_GPIO_PIN_SDA, "SDA"};
+// static EtGpio etGpioSCL = {ET_GPIO_PORT_SCL, ET_GPIO_PIN_SCL, "SCL"};
static bool isInit;
@@ -107,76 +109,77 @@ static bool exptestRun(void)
int i;
volatile int delay;
bool status = true;
- GPIO_InitTypeDef GPIO_InitStructure;
- GpioRegBuf gpioSaved;
-
- isInit = true;
-
- status &= sensorsManufacturingTest();
-
-
- // Enable GPIOs
- RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);
-
- decktestSaveGPIOStatesABC(&gpioSaved);
-
- for (i = 0; i < ET_NBR_PINS; i++)
- {
- //Initialize the pins as inputs
- GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
- }
-
- for (i = 0; i < ET_NBR_PINS && status; i++)
- {
- // Configure pin as output to poke others
- GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
-
- // Test high
- GPIO_SetBits(etGpioIn[i].port, etGpioIn[i].pin);
- for (delay = 0; delay < 1000; delay++);
- if (!exptestTestAllPins(1))
- {
- status = false;
- }
-
- // Test low
- GPIO_ResetBits(etGpioIn[i].port, etGpioIn[i].pin);
- for (delay = 0; delay < 1000; delay++);
- if (!exptestTestAllPins(0))
- {
- status = false;
- }
-
- // Restore
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
- GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
- }
-
- decktestRestoreGPIOStatesABC(&gpioSaved);
-
- if (status)
- {
- // Configure SDA & SCL to turn on OK leds
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_InitStructure.GPIO_Pin = etGpioSDA.pin;
- GPIO_Init(etGpioSDA.port, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = etGpioSCL.pin;
- GPIO_Init(etGpioSCL.port, &GPIO_InitStructure);
- // Turn on OK LEDs.
- GPIO_ResetBits(etGpioSDA.port, etGpioSDA.pin);
- GPIO_ResetBits(etGpioSCL.port, etGpioSCL.pin);
- }
+ //COMMENTED FIRMWARE
+ // GPIO_InitTypeDef GPIO_InitStructure;
+ // GpioRegBuf gpioSaved;
+
+ // isInit = true;
+
+ // status &= sensorsManufacturingTest();
+
+
+ // // Enable GPIOs
+ // RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);
+
+ // decktestSaveGPIOStatesABC(&gpioSaved);
+
+ // for (i = 0; i < ET_NBR_PINS; i++)
+ // {
+ // //Initialize the pins as inputs
+ // GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+ // }
+
+ // for (i = 0; i < ET_NBR_PINS && status; i++)
+ // {
+ // // Configure pin as output to poke others
+ // GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+
+ // // Test high
+ // GPIO_SetBits(etGpioIn[i].port, etGpioIn[i].pin);
+ // for (delay = 0; delay < 1000; delay++);
+ // if (!exptestTestAllPins(1))
+ // {
+ // status = false;
+ // }
+
+ // // Test low
+ // GPIO_ResetBits(etGpioIn[i].port, etGpioIn[i].pin);
+ // for (delay = 0; delay < 1000; delay++);
+ // if (!exptestTestAllPins(0))
+ // {
+ // status = false;
+ // }
+
+ // // Restore
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ // GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+ // }
+
+ // decktestRestoreGPIOStatesABC(&gpioSaved);
+
+ // if (status)
+ // {
+ // // Configure SDA & SCL to turn on OK leds
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_InitStructure.GPIO_Pin = etGpioSDA.pin;
+ // GPIO_Init(etGpioSDA.port, &GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Pin = etGpioSCL.pin;
+ // GPIO_Init(etGpioSCL.port, &GPIO_InitStructure);
+ // // Turn on OK LEDs.
+ // GPIO_ResetBits(etGpioSDA.port, etGpioSDA.pin);
+ // GPIO_ResetBits(etGpioSCL.port, etGpioSCL.pin);
+ // }
return status;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptestBolt.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptestBolt.c
index 9f5d24c27398165870f3a20c89a71df43cd688cc..b1a26cbd839ee64162a7590ff28647185e4fa164 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptestBolt.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptestBolt.c
@@ -56,7 +56,8 @@
#define ET_GPIO_PIN_MISO GPIO_Pin_7
#define ET_GPIO_PORT_SDA GPIOB
-#define ET_GPIO_PIN_SDA GPIO_Pin_7
+//COMMENTED FIRMWARE
+// #define ET_GPIO_PIN_SDA GPIO_Pin_7
#define ET_GPIO_PORT_SCL GPIOB
#define ET_GPIO_PIN_SCL GPIO_Pin_6
@@ -100,33 +101,36 @@ typedef struct _etGpio
static EtGpio etGpioIn[ET_NBR_PINS] =
{
- {ET_GPIO_PORT_TX1, ET_GPIO_PIN_TX1, "TX1"},
- {ET_GPIO_PORT_RX1, ET_GPIO_PIN_RX1, "RX1"},
- {ET_GPIO_PORT_TX2, ET_GPIO_PIN_TX2, "TX2"},
- {ET_GPIO_PORT_RX2, ET_GPIO_PIN_RX2, "RX2"},
- {ET_GPIO_PORT_SCK, ET_GPIO_PIN_SCK, "SCK"},
- {ET_GPIO_PORT_MOSI, ET_GPIO_PIN_MOSI, "MOSI"},
- {ET_GPIO_PORT_MISO, ET_GPIO_PIN_MISO, "MISO"},
- {ET_GPIO_PORT_IO1, ET_GPIO_PIN_IO1, "IO1"},
- {ET_GPIO_PORT_IO2, ET_GPIO_PIN_IO2, "IO2"},
- {ET_GPIO_PORT_IO3, ET_GPIO_PIN_IO3, "IO3"},
- {ET_GPIO_PORT_IO4, ET_GPIO_PIN_IO4, "IO4"}
+ //COMMENTED FIRMWARE
+ // {ET_GPIO_PORT_TX1, ET_GPIO_PIN_TX1, "TX1"},
+ // {ET_GPIO_PORT_RX1, ET_GPIO_PIN_RX1, "RX1"},
+ // {ET_GPIO_PORT_TX2, ET_GPIO_PIN_TX2, "TX2"},
+ // {ET_GPIO_PORT_RX2, ET_GPIO_PIN_RX2, "RX2"},
+ // {ET_GPIO_PORT_SCK, ET_GPIO_PIN_SCK, "SCK"},
+ // {ET_GPIO_PORT_MOSI, ET_GPIO_PIN_MOSI, "MOSI"},
+ // {ET_GPIO_PORT_MISO, ET_GPIO_PIN_MISO, "MISO"},
+ // {ET_GPIO_PORT_IO1, ET_GPIO_PIN_IO1, "IO1"},
+ // {ET_GPIO_PORT_IO2, ET_GPIO_PIN_IO2, "IO2"},
+ // {ET_GPIO_PORT_IO3, ET_GPIO_PIN_IO3, "IO3"},
+ // {ET_GPIO_PORT_IO4, ET_GPIO_PIN_IO4, "IO4"}
};
static EtGpio etMotorGpio[ET_NBR_MOTOR_PINS] =
{
- {ET_GPIO_PORT_M1, ET_GPIO_PIN_M1, "M1"},
- {ET_GPIO_PORT_M2, ET_GPIO_PIN_M2, "M2"},
- {ET_GPIO_PORT_M3, ET_GPIO_PIN_M3, "M3"},
- {ET_GPIO_PORT_M4, ET_GPIO_PIN_M4, "M4"},
- {ET_GPIO_PORT_M1_OR, ET_GPIO_PIN_M1_OR, "M1_OR"},
- {ET_GPIO_PORT_M2_OR, ET_GPIO_PIN_M2_OR, "M2_OR"},
- {ET_GPIO_PORT_M3_OR, ET_GPIO_PIN_M3_OR, "M3_OR"},
- {ET_GPIO_PORT_M4_OR, ET_GPIO_PIN_M4_OR, "M4_OR"}
+ //COMMENTED FIRMWARE
+ // {ET_GPIO_PORT_M1, ET_GPIO_PIN_M1, "M1"},
+ // {ET_GPIO_PORT_M2, ET_GPIO_PIN_M2, "M2"},
+ // {ET_GPIO_PORT_M3, ET_GPIO_PIN_M3, "M3"},
+ // {ET_GPIO_PORT_M4, ET_GPIO_PIN_M4, "M4"},
+ // {ET_GPIO_PORT_M1_OR, ET_GPIO_PIN_M1_OR, "M1_OR"},
+ // {ET_GPIO_PORT_M2_OR, ET_GPIO_PIN_M2_OR, "M2_OR"},
+ // {ET_GPIO_PORT_M3_OR, ET_GPIO_PIN_M3_OR, "M3_OR"},
+ // {ET_GPIO_PORT_M4_OR, ET_GPIO_PIN_M4_OR, "M4_OR"}
};
-static EtGpio etGpioSDA = {ET_GPIO_PORT_SDA, ET_GPIO_PIN_SDA, "SDA"};
-static EtGpio etGpioSCL = {ET_GPIO_PORT_SCL, ET_GPIO_PIN_SCL, "SCL"};
+//COMMENTED FIRMWARE
+// static EtGpio etGpioSDA = {ET_GPIO_PORT_SDA, ET_GPIO_PIN_SDA, "SDA"};
+// static EtGpio etGpioSCL = {ET_GPIO_PORT_SCL, ET_GPIO_PIN_SCL, "SCL"};
static bool isInit;
@@ -135,121 +139,122 @@ static bool exptestTestPin(EtGpio *etPin, bool test);
static bool exptestRun(void)
{
+ //COMMENTED FIRMWARE
int i;
volatile int delay;
bool status = true;
- GPIO_InitTypeDef GPIO_InitStructure;
- GpioRegBuf gpioSaved;
-
- isInit = true;
-
- status &= sensorsManufacturingTest();
-
-
- // Enable GPIOs
- RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);
-
- decktestSaveGPIOStatesABC(&gpioSaved);
-
- for (i = 0; i < ET_NBR_PINS; i++)
- {
- //Initialize the pins as inputs
- GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
- }
-
- for (i = 0; i < ET_NBR_PINS && status; i++)
- {
- // Configure pin as output to poke others
- GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
-
- // Test high
- GPIO_SetBits(etGpioIn[i].port, etGpioIn[i].pin);
- for (delay = 0; delay < 1000; delay++);
- if (!exptestTestAllPins(1))
- {
- status = false;
- }
-
- // Test low
- GPIO_ResetBits(etGpioIn[i].port, etGpioIn[i].pin);
- for (delay = 0; delay < 1000; delay++);
- if (!exptestTestAllPins(0))
- {
- status = false;
- }
-
- // Restore
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
- GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
- }
-
- // Do Bolt specific tests. Test motor signals
- // Initialize the Motor signal pins as inputs
- for (i = 0; i < ET_NBR_MOTOR_PINS; i++)
- {
- //Initialize the pins as inputs
- GPIO_InitStructure.GPIO_Pin = etMotorGpio[i].pin;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_Init(etMotorGpio[i].port, &GPIO_InitStructure);
- }
-
- for (delay = 0; delay < 10000; delay++);
-
- for (i = 0; i < ET_NBR_SIG_PINS && status; i++)
- {
- if (!exptestTestPin(&etMotorGpio[i], 1))
- {
- status = false;
- }
- }
-
- for (i = ET_NBR_SIG_PINS; i < ET_NBR_MOTOR_PINS && status; i++)
- {
- // Initialize the mosfet pins as outputs.
- GPIO_InitStructure.GPIO_Pin = etMotorGpio[i].pin;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_Init(etMotorGpio[i].port, &GPIO_InitStructure);
- // Set high to enable mosfet to pull low
- GPIO_SetBits(etMotorGpio[i].port, etMotorGpio[i].pin);
-
- for (delay = 0; delay < 10000; delay++);
- if (!exptestTestPin(&etMotorGpio[i-ET_NBR_SIG_PINS], 0))
- {
- status = false;
- }
- }
-
- //decktestRestoreGPIOStatesABC(&gpioSaved);
-
- if (status)
- {
- // Configure SDA & SCL to turn on OK leds
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_InitStructure.GPIO_Pin = etGpioSDA.pin;
- GPIO_Init(etGpioSDA.port, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = etGpioSCL.pin;
- GPIO_Init(etGpioSCL.port, &GPIO_InitStructure);
- // Turn on OK LEDs.
- GPIO_ResetBits(etGpioSDA.port, etGpioSDA.pin);
- GPIO_ResetBits(etGpioSCL.port, etGpioSCL.pin);
- }
+ // GPIO_InitTypeDef GPIO_InitStructure;
+ // GpioRegBuf gpioSaved;
+
+ // isInit = true;
+
+ // status &= sensorsManufacturingTest();
+
+
+ // // Enable GPIOs
+ // RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);
+
+ // decktestSaveGPIOStatesABC(&gpioSaved);
+
+ // for (i = 0; i < ET_NBR_PINS; i++)
+ // {
+ // //Initialize the pins as inputs
+ // GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+ // }
+
+ // for (i = 0; i < ET_NBR_PINS && status; i++)
+ // {
+ // // Configure pin as output to poke others
+ // GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+
+ // // Test high
+ // GPIO_SetBits(etGpioIn[i].port, etGpioIn[i].pin);
+ // for (delay = 0; delay < 1000; delay++);
+ // if (!exptestTestAllPins(1))
+ // {
+ // status = false;
+ // }
+
+ // // Test low
+ // GPIO_ResetBits(etGpioIn[i].port, etGpioIn[i].pin);
+ // for (delay = 0; delay < 1000; delay++);
+ // if (!exptestTestAllPins(0))
+ // {
+ // status = false;
+ // }
+
+ // // Restore
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ // GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+ // }
+
+ // // Do Bolt specific tests. Test motor signals
+ // // Initialize the Motor signal pins as inputs
+ // for (i = 0; i < ET_NBR_MOTOR_PINS; i++)
+ // {
+ // //Initialize the pins as inputs
+ // GPIO_InitStructure.GPIO_Pin = etMotorGpio[i].pin;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_Init(etMotorGpio[i].port, &GPIO_InitStructure);
+ // }
+
+ // for (delay = 0; delay < 10000; delay++);
+
+ // for (i = 0; i < ET_NBR_SIG_PINS && status; i++)
+ // {
+ // if (!exptestTestPin(&etMotorGpio[i], 1))
+ // {
+ // status = false;
+ // }
+ // }
+
+ // for (i = ET_NBR_SIG_PINS; i < ET_NBR_MOTOR_PINS && status; i++)
+ // {
+ // // Initialize the mosfet pins as outputs.
+ // GPIO_InitStructure.GPIO_Pin = etMotorGpio[i].pin;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_Init(etMotorGpio[i].port, &GPIO_InitStructure);
+ // // Set high to enable mosfet to pull low
+ // GPIO_SetBits(etMotorGpio[i].port, etMotorGpio[i].pin);
+
+ // for (delay = 0; delay < 10000; delay++);
+ // if (!exptestTestPin(&etMotorGpio[i-ET_NBR_SIG_PINS], 0))
+ // {
+ // status = false;
+ // }
+ // }
+
+ // //decktestRestoreGPIOStatesABC(&gpioSaved);
+
+ // if (status)
+ // {
+ // // Configure SDA & SCL to turn on OK leds
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_InitStructure.GPIO_Pin = etGpioSDA.pin;
+ // GPIO_Init(etGpioSDA.port, &GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Pin = etGpioSCL.pin;
+ // GPIO_Init(etGpioSCL.port, &GPIO_InitStructure);
+ // // Turn on OK LEDs.
+ // GPIO_ResetBits(etGpioSDA.port, etGpioSDA.pin);
+ // GPIO_ResetBits(etGpioSCL.port, etGpioSCL.pin);
+ // }
return status;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptestRR.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptestRR.c
index 1714733d7ef21ca45462eeb7164c05d0ae4bdca2..5a3022cce0ef8cca0cc1a80deae4d360d9f5002b 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptestRR.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/exptestRR.c
@@ -67,15 +67,17 @@ typedef struct _etGpio
static EtGpio etRRGpioIn[ET_NBR_PINS] =
{
- {ET_GPIO_PORT_TX2, ET_GPIO_PIN_TX2, "TX2"},
- {ET_GPIO_PORT_RX2, ET_GPIO_PIN_RX2, "RX2"},
- {ET_GPIO_PORT_IO2, ET_GPIO_PIN_IO2, "IO2"},
- {ET_GPIO_PORT_IO3, ET_GPIO_PIN_IO3, "IO3"},
- {ET_GPIO_PORT_IO4, ET_GPIO_PIN_IO4, "IO4"}
+ //COMMENTED FIRMWARE
+ // {ET_GPIO_PORT_TX2, ET_GPIO_PIN_TX2, "TX2"},
+ // {ET_GPIO_PORT_RX2, ET_GPIO_PIN_RX2, "RX2"},
+ // {ET_GPIO_PORT_IO2, ET_GPIO_PIN_IO2, "IO2"},
+ // {ET_GPIO_PORT_IO3, ET_GPIO_PIN_IO3, "IO3"},
+ // {ET_GPIO_PORT_IO4, ET_GPIO_PIN_IO4, "IO4"}
};
-static EtGpio etRRGpioSDA = {ET_GPIO_PORT_SDA, ET_GPIO_PIN_SDA, "SDA"};
-static EtGpio etRRGpioSCL = {ET_GPIO_PORT_SCL, ET_GPIO_PIN_SCL, "SCL"};
+//COMMENTED FIRMWARE
+// static EtGpio etRRGpioSDA = {ET_GPIO_PORT_SDA, ET_GPIO_PIN_SDA, "SDA"};
+// static EtGpio etRRGpioSCL = {ET_GPIO_PORT_SCL, ET_GPIO_PIN_SCL, "SCL"};
static bool isInit;
@@ -84,79 +86,80 @@ static bool exptestRRTestPin(EtGpio *etPin, bool test);
static bool exptestRRRun(void)
{
+ //COMMENTED FIRMWARE
int i;
volatile int delay;
bool status = true;
- GPIO_InitTypeDef GPIO_InitStructure;
- GpioRegBuf gpioSaved;
-
- isInit = true;
-
- status &= sensorsManufacturingTest();
-
-
- // Enable GPIOs
- RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);
-
- decktestSaveGPIOStatesABC(&gpioSaved);
-
- for (i = 0; i < ET_NBR_PINS; i++)
- {
- //Initialize the pins as inputs
- GPIO_InitStructure.GPIO_Pin = etRRGpioIn[i].pin;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_Init(etRRGpioIn[i].port, &GPIO_InitStructure);
- }
-
- for (i = 0; i < ET_NBR_PINS && status; i++)
- {
- // Configure pin as output to poke others
- GPIO_InitStructure.GPIO_Pin = etRRGpioIn[i].pin;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_Init(etRRGpioIn[i].port, &GPIO_InitStructure);
-
- // Test high
- GPIO_SetBits(etRRGpioIn[i].port, etRRGpioIn[i].pin);
- for (delay = 0; delay < 1000; delay++);
- if (!exptestRRTestAllPins(1))
- {
- status = false;
- }
-
- // Test low
- GPIO_ResetBits(etRRGpioIn[i].port, etRRGpioIn[i].pin);
- for (delay = 0; delay < 1000; delay++);
- if (!exptestRRTestAllPins(0))
- {
- status = false;
- }
-
- // Restore
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
- GPIO_Init(etRRGpioIn[i].port, &GPIO_InitStructure);
- }
-
- decktestRestoreGPIOStatesABC(&gpioSaved);
-
- if (status)
- {
- // Configure SDA & SCL to turn on OK leds
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
- GPIO_InitStructure.GPIO_Pin = etRRGpioSDA.pin;
- GPIO_Init(etRRGpioSDA.port, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = etRRGpioSCL.pin;
- GPIO_Init(etRRGpioSCL.port, &GPIO_InitStructure);
- // Turn on OK LEDs.
- GPIO_ResetBits(etRRGpioSDA.port, etRRGpioSDA.pin);
- GPIO_ResetBits(etRRGpioSCL.port, etRRGpioSCL.pin);
- }
+ // GPIO_InitTypeDef GPIO_InitStructure;
+ // GpioRegBuf gpioSaved;
+
+ // isInit = true;
+
+ // status &= sensorsManufacturingTest();
+
+
+ // // Enable GPIOs
+ // RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);
+
+ // decktestSaveGPIOStatesABC(&gpioSaved);
+
+ // for (i = 0; i < ET_NBR_PINS; i++)
+ // {
+ // //Initialize the pins as inputs
+ // GPIO_InitStructure.GPIO_Pin = etRRGpioIn[i].pin;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_Init(etRRGpioIn[i].port, &GPIO_InitStructure);
+ // }
+
+ // for (i = 0; i < ET_NBR_PINS && status; i++)
+ // {
+ // // Configure pin as output to poke others
+ // GPIO_InitStructure.GPIO_Pin = etRRGpioIn[i].pin;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_Init(etRRGpioIn[i].port, &GPIO_InitStructure);
+
+ // // Test high
+ // GPIO_SetBits(etRRGpioIn[i].port, etRRGpioIn[i].pin);
+ // for (delay = 0; delay < 1000; delay++);
+ // if (!exptestRRTestAllPins(1))
+ // {
+ // status = false;
+ // }
+
+ // // Test low
+ // GPIO_ResetBits(etRRGpioIn[i].port, etRRGpioIn[i].pin);
+ // for (delay = 0; delay < 1000; delay++);
+ // if (!exptestRRTestAllPins(0))
+ // {
+ // status = false;
+ // }
+
+ // // Restore
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ // GPIO_Init(etRRGpioIn[i].port, &GPIO_InitStructure);
+ // }
+
+ // decktestRestoreGPIOStatesABC(&gpioSaved);
+
+ // if (status)
+ // {
+ // // Configure SDA & SCL to turn on OK leds
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+ // GPIO_InitStructure.GPIO_Pin = etRRGpioSDA.pin;
+ // GPIO_Init(etRRGpioSDA.port, &GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Pin = etRRGpioSCL.pin;
+ // GPIO_Init(etRRGpioSCL.port, &GPIO_InitStructure);
+ // // Turn on OK LEDs.
+ // GPIO_ResetBits(etRRGpioSDA.port, etRRGpioSDA.pin);
+ // GPIO_ResetBits(etRRGpioSCL.port, etRRGpioSCL.pin);
+ // }
return status;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/interface/deck_spi.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/interface/deck_spi.h
index c68b7677c0433d4c9efeefbabd4cfefb4ac78c3d..61f3449ae403770d2690a36773221056dbae20f9 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/interface/deck_spi.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/interface/deck_spi.h
@@ -30,6 +30,8 @@
#include <stdbool.h>
#include <string.h>
+#include "stm32f4xx_spi.h"
+
// Based on 84MHz peripheral clock
#define SPI_BAUDRATE_21MHZ SPI_BaudRatePrescaler_4 // 21MHz
#define SPI_BAUDRATE_12MHZ SPI_BaudRatePrescaler_8 // 11.5MHz
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/kalman_core/kalman_core.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/kalman_core/kalman_core.h
index 3eb84cfc1a66d5b6715e8974918acb36bb66668a..6ffba8945e50b6e33171d167b708789745cee772 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/kalman_core/kalman_core.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/kalman_core/kalman_core.h
@@ -87,7 +87,8 @@ typedef struct {
// The covariance matrix
__attribute__((aligned(4))) float P[KC_STATE_DIM][KC_STATE_DIM];
- arm_matrix_instance_f32 Pm;
+ //COMMENTED FIRMWARE
+ //arm_matrix_instance_f32 Pm;
// Indicates that the internal state is corrupt and should be reset
bool resetEstimation;
@@ -120,6 +121,7 @@ void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, co
void kalmanCoreDecoupleXY(kalmanCoreData_t* this);
-void kalmanCoreScalarUpdate(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, float error, float stdMeasNoise);
+//COMMENTED FIRMWARE
+// void kalmanCoreScalarUpdate(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, float error, float stdMeasNoise);
-void kalmanCoreUpdateWithPKE(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, arm_matrix_instance_f32 *Km, arm_matrix_instance_f32 *P_w_m, float error);
\ No newline at end of file
+// void kalmanCoreUpdateWithPKE(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, arm_matrix_instance_f32 *Km, arm_matrix_instance_f32 *P_w_m, float error);
\ No newline at end of file
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_commander_high_level.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_commander_high_level.c
index 973e5e454fcee9e8da5bddc9297bb2a875131b22..7d309ccc629a31931b96af6a5996672fd103d019 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_commander_high_level.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_commander_high_level.c
@@ -55,6 +55,7 @@ such as: take-off, landing, polynomial trajectories.
#include "param.h"
#include "static_mem.h"
#include "mem.h"
+#include "config.h"
// Local types
enum TrajectoryLocation_e {
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/kalman_core.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/kalman_core.c
index 5e92ba761ea887801d41a9d9963d3e1a23bcc4d5..1cb0f2b584528c44cee35797e399ce944f513f66 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/kalman_core.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/kalman_core.c
@@ -197,370 +197,375 @@ void kalmanCoreInit(kalmanCoreData_t* this) {
this->P[KC_STATE_D1][KC_STATE_D1] = powf(stdDevInitialAttitude_rollpitch, 2);
this->P[KC_STATE_D2][KC_STATE_D2] = powf(stdDevInitialAttitude_yaw, 2);
- this->Pm.numRows = KC_STATE_DIM;
- this->Pm.numCols = KC_STATE_DIM;
- this->Pm.pData = (float*)this->P;
+ //COMMENTED FIRMWARE
+ // this->Pm.numRows = KC_STATE_DIM;
+ // this->Pm.numCols = KC_STATE_DIM;
+ // this->Pm.pData = (float*)this->P;
this->baroReferenceHeight = 0.0;
}
-void kalmanCoreScalarUpdate(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, float error, float stdMeasNoise)
-{
- // The Kalman gain as a column vector
- NO_DMA_CCM_SAFE_ZERO_INIT static float K[KC_STATE_DIM];
- static arm_matrix_instance_f32 Km = {KC_STATE_DIM, 1, (float *)K};
-
- // Temporary matrices for the covariance updates
- NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((aligned(4))) static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
- static arm_matrix_instance_f32 tmpNN1m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN1d};
-
- NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((aligned(4))) static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
- static arm_matrix_instance_f32 tmpNN2m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN2d};
-
- NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((aligned(4))) static float tmpNN3d[KC_STATE_DIM * KC_STATE_DIM];
- static arm_matrix_instance_f32 tmpNN3m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN3d};
-
- NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((aligned(4))) static float HTd[KC_STATE_DIM * 1];
- static arm_matrix_instance_f32 HTm = {KC_STATE_DIM, 1, HTd};
-
- NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((aligned(4))) static float PHTd[KC_STATE_DIM * 1];
- static arm_matrix_instance_f32 PHTm = {KC_STATE_DIM, 1, PHTd};
-
- ASSERT(Hm->numRows == 1);
- ASSERT(Hm->numCols == KC_STATE_DIM);
-
- // ====== INNOVATION COVARIANCE ======
-
- mat_trans(Hm, &HTm);
- mat_mult(&this->Pm, &HTm, &PHTm); // PH'
- float R = stdMeasNoise*stdMeasNoise;
- float HPHR = R; // HPH' + R
- for (int i=0; i<KC_STATE_DIM; i++) { // Add the element of HPH' to the above
- HPHR += Hm->pData[i]*PHTd[i]; // this obviously only works if the update is scalar (as in this function)
- }
- ASSERT(!isnan(HPHR));
-
- // ====== MEASUREMENT UPDATE ======
- // Calculate the Kalman gain and perform the state update
- for (int i=0; i<KC_STATE_DIM; i++) {
- K[i] = PHTd[i]/HPHR; // kalman gain = (PH' (HPH' + R )^-1)
- this->S[i] = this->S[i] + K[i] * error; // state update
- }
- assertStateNotNaN(this);
-
- // ====== COVARIANCE UPDATE ======
- mat_mult(&Km, Hm, &tmpNN1m); // KH
- for (int i=0; i<KC_STATE_DIM; i++) { tmpNN1d[KC_STATE_DIM*i+i] -= 1; } // KH - I
- mat_trans(&tmpNN1m, &tmpNN2m); // (KH - I)'
- mat_mult(&tmpNN1m, &this->Pm, &tmpNN3m); // (KH - I)*P
- mat_mult(&tmpNN3m, &tmpNN2m, &this->Pm); // (KH - I)*P*(KH - I)'
- assertStateNotNaN(this);
- // add the measurement variance and ensure boundedness and symmetry
- // TODO: Why would it hit these bounds? Needs to be investigated.
- for (int i=0; i<KC_STATE_DIM; i++) {
- for (int j=i; j<KC_STATE_DIM; j++) {
- float v = K[i] * R * K[j];
- float p = 0.5f*this->P[i][j] + 0.5f*this->P[j][i] + v; // add measurement noise
- if (isnan(p) || p > MAX_COVARIANCE) {
- this->P[i][j] = this->P[j][i] = MAX_COVARIANCE;
- } else if ( i==j && p < MIN_COVARIANCE ) {
- this->P[i][j] = this->P[j][i] = MIN_COVARIANCE;
- } else {
- this->P[i][j] = this->P[j][i] = p;
- }
- }
- }
-
- assertStateNotNaN(this);
-}
-
-void kalmanCoreUpdateWithPKE(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, arm_matrix_instance_f32 *Km, arm_matrix_instance_f32 *P_w_m, float error)
-{
- // kalman filter update with weighted covariance matrix P_w_m, kalman gain Km, and innovation error
- // Temporary matrices for the covariance updates
- static float tmpNN1d[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 tmpNN1m = {KC_STATE_DIM, KC_STATE_DIM, (float *)tmpNN1d};
- for (int i=0; i<KC_STATE_DIM; i++){
- this->S[i] = this->S[i] + Km->pData[i] * error;
- }
- // ====== COVARIANCE UPDATE ====== //
- mat_mult(Km, Hm, &tmpNN1m); // KH, the Kalman Gain and H are the updated Kalman Gain and H
- mat_scale(&tmpNN1m, -1.0f, &tmpNN1m); // I-KH
- for (int i=0; i<KC_STATE_DIM; i++) { tmpNN1d[i][i] = 1.0f + tmpNN1d[i][i]; }
- float Ppo[KC_STATE_DIM][KC_STATE_DIM]={0};
- arm_matrix_instance_f32 Ppom = {KC_STATE_DIM, KC_STATE_DIM, (float *)Ppo};
- mat_mult(&tmpNN1m, P_w_m, &Ppom); // Pm = (I-KH)*P_w_m
- matrixcopy(KC_STATE_DIM, KC_STATE_DIM, this->P, Ppo);
-
- assertStateNotNaN(this);
-
- for (int i=0; i<KC_STATE_DIM; i++) {
- for (int j=i; j<KC_STATE_DIM; j++) {
- float p = 0.5f*this->P[i][j] + 0.5f*this->P[j][i];
- if (isnan(p) || p > MAX_COVARIANCE) {
- this->P[i][j] = this->P[j][i] = MAX_COVARIANCE;
- } else if ( i==j && p < MIN_COVARIANCE ) {
- this->P[i][j] = this->P[j][i] = MIN_COVARIANCE;
- } else {
- this->P[i][j] = this->P[j][i] = p;
- }
- }
- }
- assertStateNotNaN(this);
-
-}
-
-
-void kalmanCoreUpdateWithBaro(kalmanCoreData_t* this, float baroAsl, bool quadIsFlying)
-{
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
-
- h[KC_STATE_Z] = 1;
-
- if (!quadIsFlying || this->baroReferenceHeight < 1) {
- //TODO: maybe we could track the zero height as a state. Would be especially useful if UWB anchors had barometers.
- this->baroReferenceHeight = baroAsl;
- }
-
- float meas = (baroAsl - this->baroReferenceHeight);
- kalmanCoreScalarUpdate(this, &H, meas - this->S[KC_STATE_Z], measNoiseBaro);
-}
-
-void kalmanCorePredict(kalmanCoreData_t* this, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying)
-{
- /* Here we discretize (euler forward) and linearise the quadrocopter dynamics in order
- * to push the covariance forward.
- *
- * QUADROCOPTER DYNAMICS (see paper):
- *
- * \dot{x} = R(I + [[d]])p
- * \dot{p} = f/m * e3 - [[\omega]]p - g(I - [[d]])R^-1 e3 //drag negligible
- * \dot{d} = \omega
- *
- * where [[.]] is the cross-product matrix of .
- * \omega are the gyro measurements
- * e3 is the column vector [0 0 1]'
- * I is the identity
- * R is the current attitude as a rotation matrix
- * f/m is the mass-normalized motor force (acceleration in the body's z direction)
- * g is gravity
- * x, p, d are the quad's states
- * note that d (attitude error) is zero at the beginning of each iteration,
- * since error information is incorporated into R after each Kalman update.
- */
-
- // The linearized update matrix
- NO_DMA_CCM_SAFE_ZERO_INIT static float A[KC_STATE_DIM][KC_STATE_DIM];
- static __attribute__((aligned(4))) arm_matrix_instance_f32 Am = { KC_STATE_DIM, KC_STATE_DIM, (float *)A}; // linearized dynamics for covariance update;
-
- // Temporary matrices for the covariance updates
- NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
- static __attribute__((aligned(4))) arm_matrix_instance_f32 tmpNN1m = { KC_STATE_DIM, KC_STATE_DIM, tmpNN1d};
-
- NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
- static __attribute__((aligned(4))) arm_matrix_instance_f32 tmpNN2m = { KC_STATE_DIM, KC_STATE_DIM, tmpNN2d};
-
- float dt2 = dt*dt;
-
- // ====== DYNAMICS LINEARIZATION ======
- // Initialize as the identity
- A[KC_STATE_X][KC_STATE_X] = 1;
- A[KC_STATE_Y][KC_STATE_Y] = 1;
- A[KC_STATE_Z][KC_STATE_Z] = 1;
-
- A[KC_STATE_PX][KC_STATE_PX] = 1;
- A[KC_STATE_PY][KC_STATE_PY] = 1;
- A[KC_STATE_PZ][KC_STATE_PZ] = 1;
-
- A[KC_STATE_D0][KC_STATE_D0] = 1;
- A[KC_STATE_D1][KC_STATE_D1] = 1;
- A[KC_STATE_D2][KC_STATE_D2] = 1;
-
- // position from body-frame velocity
- A[KC_STATE_X][KC_STATE_PX] = this->R[0][0]*dt;
- A[KC_STATE_Y][KC_STATE_PX] = this->R[1][0]*dt;
- A[KC_STATE_Z][KC_STATE_PX] = this->R[2][0]*dt;
-
- A[KC_STATE_X][KC_STATE_PY] = this->R[0][1]*dt;
- A[KC_STATE_Y][KC_STATE_PY] = this->R[1][1]*dt;
- A[KC_STATE_Z][KC_STATE_PY] = this->R[2][1]*dt;
-
- A[KC_STATE_X][KC_STATE_PZ] = this->R[0][2]*dt;
- A[KC_STATE_Y][KC_STATE_PZ] = this->R[1][2]*dt;
- A[KC_STATE_Z][KC_STATE_PZ] = this->R[2][2]*dt;
-
- // position from attitude error
- A[KC_STATE_X][KC_STATE_D0] = (this->S[KC_STATE_PY]*this->R[0][2] - this->S[KC_STATE_PZ]*this->R[0][1])*dt;
- A[KC_STATE_Y][KC_STATE_D0] = (this->S[KC_STATE_PY]*this->R[1][2] - this->S[KC_STATE_PZ]*this->R[1][1])*dt;
- A[KC_STATE_Z][KC_STATE_D0] = (this->S[KC_STATE_PY]*this->R[2][2] - this->S[KC_STATE_PZ]*this->R[2][1])*dt;
-
- A[KC_STATE_X][KC_STATE_D1] = (- this->S[KC_STATE_PX]*this->R[0][2] + this->S[KC_STATE_PZ]*this->R[0][0])*dt;
- A[KC_STATE_Y][KC_STATE_D1] = (- this->S[KC_STATE_PX]*this->R[1][2] + this->S[KC_STATE_PZ]*this->R[1][0])*dt;
- A[KC_STATE_Z][KC_STATE_D1] = (- this->S[KC_STATE_PX]*this->R[2][2] + this->S[KC_STATE_PZ]*this->R[2][0])*dt;
-
- A[KC_STATE_X][KC_STATE_D2] = (this->S[KC_STATE_PX]*this->R[0][1] - this->S[KC_STATE_PY]*this->R[0][0])*dt;
- A[KC_STATE_Y][KC_STATE_D2] = (this->S[KC_STATE_PX]*this->R[1][1] - this->S[KC_STATE_PY]*this->R[1][0])*dt;
- A[KC_STATE_Z][KC_STATE_D2] = (this->S[KC_STATE_PX]*this->R[2][1] - this->S[KC_STATE_PY]*this->R[2][0])*dt;
-
- // body-frame velocity from body-frame velocity
- A[KC_STATE_PX][KC_STATE_PX] = 1; //drag negligible
- A[KC_STATE_PY][KC_STATE_PX] =-gyro->z*dt;
- A[KC_STATE_PZ][KC_STATE_PX] = gyro->y*dt;
-
- A[KC_STATE_PX][KC_STATE_PY] = gyro->z*dt;
- A[KC_STATE_PY][KC_STATE_PY] = 1; //drag negligible
- A[KC_STATE_PZ][KC_STATE_PY] =-gyro->x*dt;
-
- A[KC_STATE_PX][KC_STATE_PZ] =-gyro->y*dt;
- A[KC_STATE_PY][KC_STATE_PZ] = gyro->x*dt;
- A[KC_STATE_PZ][KC_STATE_PZ] = 1; //drag negligible
-
- // body-frame velocity from attitude error
- A[KC_STATE_PX][KC_STATE_D0] = 0;
- A[KC_STATE_PY][KC_STATE_D0] = -GRAVITY_MAGNITUDE*this->R[2][2]*dt;
- A[KC_STATE_PZ][KC_STATE_D0] = GRAVITY_MAGNITUDE*this->R[2][1]*dt;
-
- A[KC_STATE_PX][KC_STATE_D1] = GRAVITY_MAGNITUDE*this->R[2][2]*dt;
- A[KC_STATE_PY][KC_STATE_D1] = 0;
- A[KC_STATE_PZ][KC_STATE_D1] = -GRAVITY_MAGNITUDE*this->R[2][0]*dt;
-
- A[KC_STATE_PX][KC_STATE_D2] = -GRAVITY_MAGNITUDE*this->R[2][1]*dt;
- A[KC_STATE_PY][KC_STATE_D2] = GRAVITY_MAGNITUDE*this->R[2][0]*dt;
- A[KC_STATE_PZ][KC_STATE_D2] = 0;
-
- // attitude error from attitude error
- /**
- * At first glance, it may not be clear where the next values come from, since they do not appear directly in the
- * dynamics. In this prediction step, we skip the step of first updating attitude-error, and then incorporating the
- * new error into the current attitude (which requires a rotation of the attitude-error covariance). Instead, we
- * directly update the body attitude, however still need to rotate the covariance, which is what you see below.
- *
- * This comes from a second order approximation to:
- * Sigma_post = exps(-d) Sigma_pre exps(-d)'
- * ~ (I + [[-d]] + [[-d]]^2 / 2) Sigma_pre (I + [[-d]] + [[-d]]^2 / 2)'
- * where d is the attitude error expressed as Rodriges parameters, ie. d0 = 1/2*gyro.x*dt under the assumption that
- * d = [0,0,0] at the beginning of each prediction step and that gyro.x is constant over the sampling period
- *
- * As derived in "Covariance Correction Step for Kalman Filtering with an Attitude"
- * http://arc.aiaa.org/doi/abs/10.2514/1.G000848
- */
- float d0 = gyro->x*dt/2;
- float d1 = gyro->y*dt/2;
- float d2 = gyro->z*dt/2;
-
- A[KC_STATE_D0][KC_STATE_D0] = 1 - d1*d1/2 - d2*d2/2;
- A[KC_STATE_D0][KC_STATE_D1] = d2 + d0*d1/2;
- A[KC_STATE_D0][KC_STATE_D2] = -d1 + d0*d2/2;
-
- A[KC_STATE_D1][KC_STATE_D0] = -d2 + d0*d1/2;
- A[KC_STATE_D1][KC_STATE_D1] = 1 - d0*d0/2 - d2*d2/2;
- A[KC_STATE_D1][KC_STATE_D2] = d0 + d1*d2/2;
-
- A[KC_STATE_D2][KC_STATE_D0] = d1 + d0*d2/2;
- A[KC_STATE_D2][KC_STATE_D1] = -d0 + d1*d2/2;
- A[KC_STATE_D2][KC_STATE_D2] = 1 - d0*d0/2 - d1*d1/2;
-
-
- // ====== COVARIANCE UPDATE ======
- mat_mult(&Am, &this->Pm, &tmpNN1m); // A P
- mat_trans(&Am, &tmpNN2m); // A'
- mat_mult(&tmpNN1m, &tmpNN2m, &this->Pm); // A P A'
- // Process noise is added after the return from the prediction step
-
- // ====== PREDICTION STEP ======
- // The prediction depends on whether we're on the ground, or in flight.
- // When flying, the accelerometer directly measures thrust (hence is useless to estimate body angle while flying)
-
- float dx, dy, dz;
- float tmpSPX, tmpSPY, tmpSPZ;
- float zacc;
-
- if (quadIsFlying) // only acceleration in z direction
- {
- // Use accelerometer and not commanded thrust, as this has proper physical units
- zacc = acc->z;
-
- // position updates in the body frame (will be rotated to inertial frame)
- dx = this->S[KC_STATE_PX] * dt;
- dy = this->S[KC_STATE_PY] * dt;
- dz = this->S[KC_STATE_PZ] * dt + zacc * dt2 / 2.0f; // thrust can only be produced in the body's Z direction
-
- // position update
- this->S[KC_STATE_X] += this->R[0][0] * dx + this->R[0][1] * dy + this->R[0][2] * dz;
- this->S[KC_STATE_Y] += this->R[1][0] * dx + this->R[1][1] * dy + this->R[1][2] * dz;
- this->S[KC_STATE_Z] += this->R[2][0] * dx + this->R[2][1] * dy + this->R[2][2] * dz - GRAVITY_MAGNITUDE * dt2 / 2.0f;
-
- // keep previous time step's state for the update
- tmpSPX = this->S[KC_STATE_PX];
- tmpSPY = this->S[KC_STATE_PY];
- tmpSPZ = this->S[KC_STATE_PZ];
-
- // body-velocity update: accelerometers - gyros cross velocity - gravity in body frame
- this->S[KC_STATE_PX] += dt * (gyro->z * tmpSPY - gyro->y * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][0]);
- this->S[KC_STATE_PY] += dt * (-gyro->z * tmpSPX + gyro->x * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][1]);
- this->S[KC_STATE_PZ] += dt * (zacc + gyro->y * tmpSPX - gyro->x * tmpSPY - GRAVITY_MAGNITUDE * this->R[2][2]);
- }
- else // Acceleration can be in any direction, as measured by the accelerometer. This occurs, eg. in freefall or while being carried.
- {
- // position updates in the body frame (will be rotated to inertial frame)
- dx = this->S[KC_STATE_PX] * dt + acc->x * dt2 / 2.0f;
- dy = this->S[KC_STATE_PY] * dt + acc->y * dt2 / 2.0f;
- dz = this->S[KC_STATE_PZ] * dt + acc->z * dt2 / 2.0f; // thrust can only be produced in the body's Z direction
-
- // position update
- this->S[KC_STATE_X] += this->R[0][0] * dx + this->R[0][1] * dy + this->R[0][2] * dz;
- this->S[KC_STATE_Y] += this->R[1][0] * dx + this->R[1][1] * dy + this->R[1][2] * dz;
- this->S[KC_STATE_Z] += this->R[2][0] * dx + this->R[2][1] * dy + this->R[2][2] * dz - GRAVITY_MAGNITUDE * dt2 / 2.0f;
-
- // keep previous time step's state for the update
- tmpSPX = this->S[KC_STATE_PX];
- tmpSPY = this->S[KC_STATE_PY];
- tmpSPZ = this->S[KC_STATE_PZ];
-
- // body-velocity update: accelerometers - gyros cross velocity - gravity in body frame
- this->S[KC_STATE_PX] += dt * (acc->x + gyro->z * tmpSPY - gyro->y * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][0]);
- this->S[KC_STATE_PY] += dt * (acc->y - gyro->z * tmpSPX + gyro->x * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][1]);
- this->S[KC_STATE_PZ] += dt * (acc->z + gyro->y * tmpSPX - gyro->x * tmpSPY - GRAVITY_MAGNITUDE * this->R[2][2]);
- }
-
- // attitude update (rotate by gyroscope), we do this in quaternions
- // this is the gyroscope angular velocity integrated over the sample period
- float dtwx = dt*gyro->x;
- float dtwy = dt*gyro->y;
- float dtwz = dt*gyro->z;
-
- // compute the quaternion values in [w,x,y,z] order
- float angle = arm_sqrt(dtwx*dtwx + dtwy*dtwy + dtwz*dtwz);
- float ca = arm_cos_f32(angle/2.0f);
- float sa = arm_sin_f32(angle/2.0f);
- float dq[4] = {ca , sa*dtwx/angle , sa*dtwy/angle , sa*dtwz/angle};
-
- float tmpq0;
- float tmpq1;
- float tmpq2;
- float tmpq3;
-
- // rotate the quad's attitude by the delta quaternion vector computed above
- tmpq0 = dq[0]*this->q[0] - dq[1]*this->q[1] - dq[2]*this->q[2] - dq[3]*this->q[3];
- tmpq1 = dq[1]*this->q[0] + dq[0]*this->q[1] + dq[3]*this->q[2] - dq[2]*this->q[3];
- tmpq2 = dq[2]*this->q[0] - dq[3]*this->q[1] + dq[0]*this->q[2] + dq[1]*this->q[3];
- tmpq3 = dq[3]*this->q[0] + dq[2]*this->q[1] - dq[1]*this->q[2] + dq[0]*this->q[3];
-
- if (! quadIsFlying) {
- float keep = 1.0f - ROLLPITCH_ZERO_REVERSION;
-
- tmpq0 = keep * tmpq0 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[0];
- tmpq1 = keep * tmpq1 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[1];
- tmpq2 = keep * tmpq2 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[2];
- tmpq3 = keep * tmpq3 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[3];
- }
-
- // normalize and store the result
- float norm = arm_sqrt(tmpq0*tmpq0 + tmpq1*tmpq1 + tmpq2*tmpq2 + tmpq3*tmpq3);
- this->q[0] = tmpq0/norm; this->q[1] = tmpq1/norm; this->q[2] = tmpq2/norm; this->q[3] = tmpq3/norm;
- assertStateNotNaN(this);
-}
+//COMMENTED FIRMWARE
+// void kalmanCoreScalarUpdate(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, float error, float stdMeasNoise)
+// {
+// // The Kalman gain as a column vector
+// NO_DMA_CCM_SAFE_ZERO_INIT static float K[KC_STATE_DIM];
+// static arm_matrix_instance_f32 Km = {KC_STATE_DIM, 1, (float *)K};
+
+// // Temporary matrices for the covariance updates
+// NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((aligned(4))) static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
+// static arm_matrix_instance_f32 tmpNN1m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN1d};
+
+// NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((aligned(4))) static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
+// static arm_matrix_instance_f32 tmpNN2m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN2d};
+
+// NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((aligned(4))) static float tmpNN3d[KC_STATE_DIM * KC_STATE_DIM];
+// static arm_matrix_instance_f32 tmpNN3m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN3d};
+
+// NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((aligned(4))) static float HTd[KC_STATE_DIM * 1];
+// static arm_matrix_instance_f32 HTm = {KC_STATE_DIM, 1, HTd};
+
+// NO_DMA_CCM_SAFE_ZERO_INIT __attribute__((aligned(4))) static float PHTd[KC_STATE_DIM * 1];
+// static arm_matrix_instance_f32 PHTm = {KC_STATE_DIM, 1, PHTd};
+
+// ASSERT(Hm->numRows == 1);
+// ASSERT(Hm->numCols == KC_STATE_DIM);
+
+// // ====== INNOVATION COVARIANCE ======
+
+// mat_trans(Hm, &HTm);
+// mat_mult(&this->Pm, &HTm, &PHTm); // PH'
+// float R = stdMeasNoise*stdMeasNoise;
+// float HPHR = R; // HPH' + R
+// for (int i=0; i<KC_STATE_DIM; i++) { // Add the element of HPH' to the above
+// HPHR += Hm->pData[i]*PHTd[i]; // this obviously only works if the update is scalar (as in this function)
+// }
+// ASSERT(!isnan(HPHR));
+
+// // ====== MEASUREMENT UPDATE ======
+// // Calculate the Kalman gain and perform the state update
+// for (int i=0; i<KC_STATE_DIM; i++) {
+// K[i] = PHTd[i]/HPHR; // kalman gain = (PH' (HPH' + R )^-1)
+// this->S[i] = this->S[i] + K[i] * error; // state update
+// }
+// assertStateNotNaN(this);
+
+// // ====== COVARIANCE UPDATE ======
+// mat_mult(&Km, Hm, &tmpNN1m); // KH
+// for (int i=0; i<KC_STATE_DIM; i++) { tmpNN1d[KC_STATE_DIM*i+i] -= 1; } // KH - I
+// mat_trans(&tmpNN1m, &tmpNN2m); // (KH - I)'
+// mat_mult(&tmpNN1m, &this->Pm, &tmpNN3m); // (KH - I)*P
+// mat_mult(&tmpNN3m, &tmpNN2m, &this->Pm); // (KH - I)*P*(KH - I)'
+// assertStateNotNaN(this);
+// // add the measurement variance and ensure boundedness and symmetry
+// // TODO: Why would it hit these bounds? Needs to be investigated.
+// for (int i=0; i<KC_STATE_DIM; i++) {
+// for (int j=i; j<KC_STATE_DIM; j++) {
+// float v = K[i] * R * K[j];
+// float p = 0.5f*this->P[i][j] + 0.5f*this->P[j][i] + v; // add measurement noise
+// if (isnan(p) || p > MAX_COVARIANCE) {
+// this->P[i][j] = this->P[j][i] = MAX_COVARIANCE;
+// } else if ( i==j && p < MIN_COVARIANCE ) {
+// this->P[i][j] = this->P[j][i] = MIN_COVARIANCE;
+// } else {
+// this->P[i][j] = this->P[j][i] = p;
+// }
+// }
+// }
+
+// assertStateNotNaN(this);
+// }
+
+//COMMENTED FIRMWARE
+// void kalmanCoreUpdateWithPKE(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, arm_matrix_instance_f32 *Km, arm_matrix_instance_f32 *P_w_m, float error)
+// {
+// // kalman filter update with weighted covariance matrix P_w_m, kalman gain Km, and innovation error
+// // Temporary matrices for the covariance updates
+// static float tmpNN1d[KC_STATE_DIM][KC_STATE_DIM];
+// static arm_matrix_instance_f32 tmpNN1m = {KC_STATE_DIM, KC_STATE_DIM, (float *)tmpNN1d};
+// for (int i=0; i<KC_STATE_DIM; i++){
+// this->S[i] = this->S[i] + Km->pData[i] * error;
+// }
+// // ====== COVARIANCE UPDATE ====== //
+// mat_mult(Km, Hm, &tmpNN1m); // KH, the Kalman Gain and H are the updated Kalman Gain and H
+// mat_scale(&tmpNN1m, -1.0f, &tmpNN1m); // I-KH
+// for (int i=0; i<KC_STATE_DIM; i++) { tmpNN1d[i][i] = 1.0f + tmpNN1d[i][i]; }
+// float Ppo[KC_STATE_DIM][KC_STATE_DIM]={0};
+// arm_matrix_instance_f32 Ppom = {KC_STATE_DIM, KC_STATE_DIM, (float *)Ppo};
+// mat_mult(&tmpNN1m, P_w_m, &Ppom); // Pm = (I-KH)*P_w_m
+// matrixcopy(KC_STATE_DIM, KC_STATE_DIM, this->P, Ppo);
+
+// assertStateNotNaN(this);
+
+// for (int i=0; i<KC_STATE_DIM; i++) {
+// for (int j=i; j<KC_STATE_DIM; j++) {
+// float p = 0.5f*this->P[i][j] + 0.5f*this->P[j][i];
+// if (isnan(p) || p > MAX_COVARIANCE) {
+// this->P[i][j] = this->P[j][i] = MAX_COVARIANCE;
+// } else if ( i==j && p < MIN_COVARIANCE ) {
+// this->P[i][j] = this->P[j][i] = MIN_COVARIANCE;
+// } else {
+// this->P[i][j] = this->P[j][i] = p;
+// }
+// }
+// }
+// assertStateNotNaN(this);
+
+// }
+
+//COMMENTED FIRMWARE
+// void kalmanCoreUpdateWithBaro(kalmanCoreData_t* this, float baroAsl, bool quadIsFlying)
+// {
+// float h[KC_STATE_DIM] = {0};
+// arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+
+// h[KC_STATE_Z] = 1;
+
+// if (!quadIsFlying || this->baroReferenceHeight < 1) {
+// //TODO: maybe we could track the zero height as a state. Would be especially useful if UWB anchors had barometers.
+// this->baroReferenceHeight = baroAsl;
+// }
+
+// float meas = (baroAsl - this->baroReferenceHeight);
+// kalmanCoreScalarUpdate(this, &H, meas - this->S[KC_STATE_Z], measNoiseBaro);
+// }
+
+//COMMENTED FIRMWARE
+// void kalmanCorePredict(kalmanCoreData_t* this, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying)
+// {
+// /* Here we discretize (euler forward) and linearise the quadrocopter dynamics in order
+// * to push the covariance forward.
+// *
+// * QUADROCOPTER DYNAMICS (see paper):
+// *
+// * \dot{x} = R(I + [[d]])p
+// * \dot{p} = f/m * e3 - [[\omega]]p - g(I - [[d]])R^-1 e3 //drag negligible
+// * \dot{d} = \omega
+// *
+// * where [[.]] is the cross-product matrix of .
+// * \omega are the gyro measurements
+// * e3 is the column vector [0 0 1]'
+// * I is the identity
+// * R is the current attitude as a rotation matrix
+// * f/m is the mass-normalized motor force (acceleration in the body's z direction)
+// * g is gravity
+// * x, p, d are the quad's states
+// * note that d (attitude error) is zero at the beginning of each iteration,
+// * since error information is incorporated into R after each Kalman update.
+// */
+
+// // The linearized update matrix
+// //COMMENTED FIRMWARE
+// NO_DMA_CCM_SAFE_ZERO_INIT static float A[KC_STATE_DIM][KC_STATE_DIM];
+// static __attribute__((aligned(4))) arm_matrix_instance_f32 Am = { KC_STATE_DIM, KC_STATE_DIM, (float *)A}; // linearized dynamics for covariance update;
+
+// // Temporary matrices for the covariance updates
+// NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
+// static __attribute__((aligned(4))) arm_matrix_instance_f32 tmpNN1m = { KC_STATE_DIM, KC_STATE_DIM, tmpNN1d};
+
+// NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
+// static __attribute__((aligned(4))) arm_matrix_instance_f32 tmpNN2m = { KC_STATE_DIM, KC_STATE_DIM, tmpNN2d};
+
+// float dt2 = dt*dt;
+
+// // ====== DYNAMICS LINEARIZATION ======
+// // Initialize as the identity
+// A[KC_STATE_X][KC_STATE_X] = 1;
+// A[KC_STATE_Y][KC_STATE_Y] = 1;
+// A[KC_STATE_Z][KC_STATE_Z] = 1;
+
+// A[KC_STATE_PX][KC_STATE_PX] = 1;
+// A[KC_STATE_PY][KC_STATE_PY] = 1;
+// A[KC_STATE_PZ][KC_STATE_PZ] = 1;
+
+// A[KC_STATE_D0][KC_STATE_D0] = 1;
+// A[KC_STATE_D1][KC_STATE_D1] = 1;
+// A[KC_STATE_D2][KC_STATE_D2] = 1;
+
+// // position from body-frame velocity
+// A[KC_STATE_X][KC_STATE_PX] = this->R[0][0]*dt;
+// A[KC_STATE_Y][KC_STATE_PX] = this->R[1][0]*dt;
+// A[KC_STATE_Z][KC_STATE_PX] = this->R[2][0]*dt;
+
+// A[KC_STATE_X][KC_STATE_PY] = this->R[0][1]*dt;
+// A[KC_STATE_Y][KC_STATE_PY] = this->R[1][1]*dt;
+// A[KC_STATE_Z][KC_STATE_PY] = this->R[2][1]*dt;
+
+// A[KC_STATE_X][KC_STATE_PZ] = this->R[0][2]*dt;
+// A[KC_STATE_Y][KC_STATE_PZ] = this->R[1][2]*dt;
+// A[KC_STATE_Z][KC_STATE_PZ] = this->R[2][2]*dt;
+
+// // position from attitude error
+// A[KC_STATE_X][KC_STATE_D0] = (this->S[KC_STATE_PY]*this->R[0][2] - this->S[KC_STATE_PZ]*this->R[0][1])*dt;
+// A[KC_STATE_Y][KC_STATE_D0] = (this->S[KC_STATE_PY]*this->R[1][2] - this->S[KC_STATE_PZ]*this->R[1][1])*dt;
+// A[KC_STATE_Z][KC_STATE_D0] = (this->S[KC_STATE_PY]*this->R[2][2] - this->S[KC_STATE_PZ]*this->R[2][1])*dt;
+
+// A[KC_STATE_X][KC_STATE_D1] = (- this->S[KC_STATE_PX]*this->R[0][2] + this->S[KC_STATE_PZ]*this->R[0][0])*dt;
+// A[KC_STATE_Y][KC_STATE_D1] = (- this->S[KC_STATE_PX]*this->R[1][2] + this->S[KC_STATE_PZ]*this->R[1][0])*dt;
+// A[KC_STATE_Z][KC_STATE_D1] = (- this->S[KC_STATE_PX]*this->R[2][2] + this->S[KC_STATE_PZ]*this->R[2][0])*dt;
+
+// A[KC_STATE_X][KC_STATE_D2] = (this->S[KC_STATE_PX]*this->R[0][1] - this->S[KC_STATE_PY]*this->R[0][0])*dt;
+// A[KC_STATE_Y][KC_STATE_D2] = (this->S[KC_STATE_PX]*this->R[1][1] - this->S[KC_STATE_PY]*this->R[1][0])*dt;
+// A[KC_STATE_Z][KC_STATE_D2] = (this->S[KC_STATE_PX]*this->R[2][1] - this->S[KC_STATE_PY]*this->R[2][0])*dt;
+
+// // body-frame velocity from body-frame velocity
+// A[KC_STATE_PX][KC_STATE_PX] = 1; //drag negligible
+// A[KC_STATE_PY][KC_STATE_PX] =-gyro->z*dt;
+// A[KC_STATE_PZ][KC_STATE_PX] = gyro->y*dt;
+
+// A[KC_STATE_PX][KC_STATE_PY] = gyro->z*dt;
+// A[KC_STATE_PY][KC_STATE_PY] = 1; //drag negligible
+// A[KC_STATE_PZ][KC_STATE_PY] =-gyro->x*dt;
+
+// A[KC_STATE_PX][KC_STATE_PZ] =-gyro->y*dt;
+// A[KC_STATE_PY][KC_STATE_PZ] = gyro->x*dt;
+// A[KC_STATE_PZ][KC_STATE_PZ] = 1; //drag negligible
+
+// // body-frame velocity from attitude error
+// A[KC_STATE_PX][KC_STATE_D0] = 0;
+// A[KC_STATE_PY][KC_STATE_D0] = -GRAVITY_MAGNITUDE*this->R[2][2]*dt;
+// A[KC_STATE_PZ][KC_STATE_D0] = GRAVITY_MAGNITUDE*this->R[2][1]*dt;
+
+// A[KC_STATE_PX][KC_STATE_D1] = GRAVITY_MAGNITUDE*this->R[2][2]*dt;
+// A[KC_STATE_PY][KC_STATE_D1] = 0;
+// A[KC_STATE_PZ][KC_STATE_D1] = -GRAVITY_MAGNITUDE*this->R[2][0]*dt;
+
+// A[KC_STATE_PX][KC_STATE_D2] = -GRAVITY_MAGNITUDE*this->R[2][1]*dt;
+// A[KC_STATE_PY][KC_STATE_D2] = GRAVITY_MAGNITUDE*this->R[2][0]*dt;
+// A[KC_STATE_PZ][KC_STATE_D2] = 0;
+
+// // attitude error from attitude error
+// /**
+// * At first glance, it may not be clear where the next values come from, since they do not appear directly in the
+// * dynamics. In this prediction step, we skip the step of first updating attitude-error, and then incorporating the
+// * new error into the current attitude (which requires a rotation of the attitude-error covariance). Instead, we
+// * directly update the body attitude, however still need to rotate the covariance, which is what you see below.
+// *
+// * This comes from a second order approximation to:
+// * Sigma_post = exps(-d) Sigma_pre exps(-d)'
+// * ~ (I + [[-d]] + [[-d]]^2 / 2) Sigma_pre (I + [[-d]] + [[-d]]^2 / 2)'
+// * where d is the attitude error expressed as Rodriges parameters, ie. d0 = 1/2*gyro.x*dt under the assumption that
+// * d = [0,0,0] at the beginning of each prediction step and that gyro.x is constant over the sampling period
+// *
+// * As derived in "Covariance Correction Step for Kalman Filtering with an Attitude"
+// * http://arc.aiaa.org/doi/abs/10.2514/1.G000848
+// */
+// float d0 = gyro->x*dt/2;
+// float d1 = gyro->y*dt/2;
+// float d2 = gyro->z*dt/2;
+
+// A[KC_STATE_D0][KC_STATE_D0] = 1 - d1*d1/2 - d2*d2/2;
+// A[KC_STATE_D0][KC_STATE_D1] = d2 + d0*d1/2;
+// A[KC_STATE_D0][KC_STATE_D2] = -d1 + d0*d2/2;
+
+// A[KC_STATE_D1][KC_STATE_D0] = -d2 + d0*d1/2;
+// A[KC_STATE_D1][KC_STATE_D1] = 1 - d0*d0/2 - d2*d2/2;
+// A[KC_STATE_D1][KC_STATE_D2] = d0 + d1*d2/2;
+
+// A[KC_STATE_D2][KC_STATE_D0] = d1 + d0*d2/2;
+// A[KC_STATE_D2][KC_STATE_D1] = -d0 + d1*d2/2;
+// A[KC_STATE_D2][KC_STATE_D2] = 1 - d0*d0/2 - d1*d1/2;
+
+
+// // ====== COVARIANCE UPDATE ======
+// mat_mult(&Am, &this->Pm, &tmpNN1m); // A P
+// mat_trans(&Am, &tmpNN2m); // A'
+// mat_mult(&tmpNN1m, &tmpNN2m, &this->Pm); // A P A'
+// // Process noise is added after the return from the prediction step
+
+// // ====== PREDICTION STEP ======
+// // The prediction depends on whether we're on the ground, or in flight.
+// // When flying, the accelerometer directly measures thrust (hence is useless to estimate body angle while flying)
+
+// float dx, dy, dz;
+// float tmpSPX, tmpSPY, tmpSPZ;
+// float zacc;
+
+// if (quadIsFlying) // only acceleration in z direction
+// {
+// // Use accelerometer and not commanded thrust, as this has proper physical units
+// zacc = acc->z;
+
+// // position updates in the body frame (will be rotated to inertial frame)
+// dx = this->S[KC_STATE_PX] * dt;
+// dy = this->S[KC_STATE_PY] * dt;
+// dz = this->S[KC_STATE_PZ] * dt + zacc * dt2 / 2.0f; // thrust can only be produced in the body's Z direction
+
+// // position update
+// this->S[KC_STATE_X] += this->R[0][0] * dx + this->R[0][1] * dy + this->R[0][2] * dz;
+// this->S[KC_STATE_Y] += this->R[1][0] * dx + this->R[1][1] * dy + this->R[1][2] * dz;
+// this->S[KC_STATE_Z] += this->R[2][0] * dx + this->R[2][1] * dy + this->R[2][2] * dz - GRAVITY_MAGNITUDE * dt2 / 2.0f;
+
+// // keep previous time step's state for the update
+// tmpSPX = this->S[KC_STATE_PX];
+// tmpSPY = this->S[KC_STATE_PY];
+// tmpSPZ = this->S[KC_STATE_PZ];
+
+// // body-velocity update: accelerometers - gyros cross velocity - gravity in body frame
+// this->S[KC_STATE_PX] += dt * (gyro->z * tmpSPY - gyro->y * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][0]);
+// this->S[KC_STATE_PY] += dt * (-gyro->z * tmpSPX + gyro->x * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][1]);
+// this->S[KC_STATE_PZ] += dt * (zacc + gyro->y * tmpSPX - gyro->x * tmpSPY - GRAVITY_MAGNITUDE * this->R[2][2]);
+// }
+// else // Acceleration can be in any direction, as measured by the accelerometer. This occurs, eg. in freefall or while being carried.
+// {
+// // position updates in the body frame (will be rotated to inertial frame)
+// dx = this->S[KC_STATE_PX] * dt + acc->x * dt2 / 2.0f;
+// dy = this->S[KC_STATE_PY] * dt + acc->y * dt2 / 2.0f;
+// dz = this->S[KC_STATE_PZ] * dt + acc->z * dt2 / 2.0f; // thrust can only be produced in the body's Z direction
+
+// // position update
+// this->S[KC_STATE_X] += this->R[0][0] * dx + this->R[0][1] * dy + this->R[0][2] * dz;
+// this->S[KC_STATE_Y] += this->R[1][0] * dx + this->R[1][1] * dy + this->R[1][2] * dz;
+// this->S[KC_STATE_Z] += this->R[2][0] * dx + this->R[2][1] * dy + this->R[2][2] * dz - GRAVITY_MAGNITUDE * dt2 / 2.0f;
+
+// // keep previous time step's state for the update
+// tmpSPX = this->S[KC_STATE_PX];
+// tmpSPY = this->S[KC_STATE_PY];
+// tmpSPZ = this->S[KC_STATE_PZ];
+
+// // body-velocity update: accelerometers - gyros cross velocity - gravity in body frame
+// this->S[KC_STATE_PX] += dt * (acc->x + gyro->z * tmpSPY - gyro->y * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][0]);
+// this->S[KC_STATE_PY] += dt * (acc->y - gyro->z * tmpSPX + gyro->x * tmpSPZ - GRAVITY_MAGNITUDE * this->R[2][1]);
+// this->S[KC_STATE_PZ] += dt * (acc->z + gyro->y * tmpSPX - gyro->x * tmpSPY - GRAVITY_MAGNITUDE * this->R[2][2]);
+// }
+
+// // attitude update (rotate by gyroscope), we do this in quaternions
+// // this is the gyroscope angular velocity integrated over the sample period
+// float dtwx = dt*gyro->x;
+// float dtwy = dt*gyro->y;
+// float dtwz = dt*gyro->z;
+
+// // compute the quaternion values in [w,x,y,z] order
+// float angle = arm_sqrt(dtwx*dtwx + dtwy*dtwy + dtwz*dtwz);
+// float ca = arm_cos_f32(angle/2.0f);
+// float sa = arm_sin_f32(angle/2.0f);
+// float dq[4] = {ca , sa*dtwx/angle , sa*dtwy/angle , sa*dtwz/angle};
+
+// float tmpq0;
+// float tmpq1;
+// float tmpq2;
+// float tmpq3;
+
+// // rotate the quad's attitude by the delta quaternion vector computed above
+// tmpq0 = dq[0]*this->q[0] - dq[1]*this->q[1] - dq[2]*this->q[2] - dq[3]*this->q[3];
+// tmpq1 = dq[1]*this->q[0] + dq[0]*this->q[1] + dq[3]*this->q[2] - dq[2]*this->q[3];
+// tmpq2 = dq[2]*this->q[0] - dq[3]*this->q[1] + dq[0]*this->q[2] + dq[1]*this->q[3];
+// tmpq3 = dq[3]*this->q[0] + dq[2]*this->q[1] - dq[1]*this->q[2] + dq[0]*this->q[3];
+
+// if (! quadIsFlying) {
+// float keep = 1.0f - ROLLPITCH_ZERO_REVERSION;
+
+// tmpq0 = keep * tmpq0 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[0];
+// tmpq1 = keep * tmpq1 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[1];
+// tmpq2 = keep * tmpq2 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[2];
+// tmpq3 = keep * tmpq3 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[3];
+// }
+
+// // normalize and store the result
+// float norm = arm_sqrt(tmpq0*tmpq0 + tmpq1*tmpq1 + tmpq2*tmpq2 + tmpq3*tmpq3);
+// this->q[0] = tmpq0/norm; this->q[1] = tmpq1/norm; this->q[2] = tmpq2/norm; this->q[3] = tmpq3/norm;
+// assertStateNotNaN(this);
+// }
void kalmanCoreAddProcessNoise(kalmanCoreData_t* this, float dt)
@@ -597,120 +602,120 @@ void kalmanCoreAddProcessNoise(kalmanCoreData_t* this, float dt)
}
-
-void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick)
-{
- // Matrix to rotate the attitude covariances once updated
- NO_DMA_CCM_SAFE_ZERO_INIT static float A[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 Am = {KC_STATE_DIM, KC_STATE_DIM, (float *)A};
-
- // Temporary matrices for the covariance updates
- NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
- static arm_matrix_instance_f32 tmpNN1m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN1d};
-
- NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
- static arm_matrix_instance_f32 tmpNN2m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN2d};
-
- // Incorporate the attitude error (Kalman filter state) with the attitude
- float v0 = this->S[KC_STATE_D0];
- float v1 = this->S[KC_STATE_D1];
- float v2 = this->S[KC_STATE_D2];
-
- // Move attitude error into attitude if any of the angle errors are large enough
- if ((fabsf(v0) > 0.1e-3f || fabsf(v1) > 0.1e-3f || fabsf(v2) > 0.1e-3f) && (fabsf(v0) < 10 && fabsf(v1) < 10 && fabsf(v2) < 10))
- {
- float angle = arm_sqrt(v0*v0 + v1*v1 + v2*v2);
- float ca = arm_cos_f32(angle / 2.0f);
- float sa = arm_sin_f32(angle / 2.0f);
- float dq[4] = {ca, sa * v0 / angle, sa * v1 / angle, sa * v2 / angle};
-
- // rotate the quad's attitude by the delta quaternion vector computed above
- float tmpq0 = dq[0] * this->q[0] - dq[1] * this->q[1] - dq[2] * this->q[2] - dq[3] * this->q[3];
- float tmpq1 = dq[1] * this->q[0] + dq[0] * this->q[1] + dq[3] * this->q[2] - dq[2] * this->q[3];
- float tmpq2 = dq[2] * this->q[0] - dq[3] * this->q[1] + dq[0] * this->q[2] + dq[1] * this->q[3];
- float tmpq3 = dq[3] * this->q[0] + dq[2] * this->q[1] - dq[1] * this->q[2] + dq[0] * this->q[3];
-
- // normalize and store the result
- float norm = arm_sqrt(tmpq0 * tmpq0 + tmpq1 * tmpq1 + tmpq2 * tmpq2 + tmpq3 * tmpq3);
- this->q[0] = tmpq0 / norm;
- this->q[1] = tmpq1 / norm;
- this->q[2] = tmpq2 / norm;
- this->q[3] = tmpq3 / norm;
-
- /** Rotate the covariance, since we've rotated the body
- *
- * This comes from a second order approximation to:
- * Sigma_post = exps(-d) Sigma_pre exps(-d)'
- * ~ (I + [[-d]] + [[-d]]^2 / 2) Sigma_pre (I + [[-d]] + [[-d]]^2 / 2)'
- * where d is the attitude error expressed as Rodriges parameters, ie. d = tan(|v|/2)*v/|v|
- *
- * As derived in "Covariance Correction Step for Kalman Filtering with an Attitude"
- * http://arc.aiaa.org/doi/abs/10.2514/1.G000848
- */
-
- float d0 = v0/2; // the attitude error vector (v0,v1,v2) is small,
- float d1 = v1/2; // so we use a first order approximation to d0 = tan(|v0|/2)*v0/|v0|
- float d2 = v2/2;
-
- A[KC_STATE_X][KC_STATE_X] = 1;
- A[KC_STATE_Y][KC_STATE_Y] = 1;
- A[KC_STATE_Z][KC_STATE_Z] = 1;
-
- A[KC_STATE_PX][KC_STATE_PX] = 1;
- A[KC_STATE_PY][KC_STATE_PY] = 1;
- A[KC_STATE_PZ][KC_STATE_PZ] = 1;
-
- A[KC_STATE_D0][KC_STATE_D0] = 1 - d1*d1/2 - d2*d2/2;
- A[KC_STATE_D0][KC_STATE_D1] = d2 + d0*d1/2;
- A[KC_STATE_D0][KC_STATE_D2] = -d1 + d0*d2/2;
-
- A[KC_STATE_D1][KC_STATE_D0] = -d2 + d0*d1/2;
- A[KC_STATE_D1][KC_STATE_D1] = 1 - d0*d0/2 - d2*d2/2;
- A[KC_STATE_D1][KC_STATE_D2] = d0 + d1*d2/2;
-
- A[KC_STATE_D2][KC_STATE_D0] = d1 + d0*d2/2;
- A[KC_STATE_D2][KC_STATE_D1] = -d0 + d1*d2/2;
- A[KC_STATE_D2][KC_STATE_D2] = 1 - d0*d0/2 - d1*d1/2;
-
- mat_trans(&Am, &tmpNN1m); // A'
- mat_mult(&Am, &this->Pm, &tmpNN2m); // AP
- mat_mult(&tmpNN2m, &tmpNN1m, &this->Pm); //APA'
- }
-
- // convert the new attitude to a rotation matrix, such that we can rotate body-frame velocity and acc
- this->R[0][0] = this->q[0] * this->q[0] + this->q[1] * this->q[1] - this->q[2] * this->q[2] - this->q[3] * this->q[3];
- this->R[0][1] = 2 * this->q[1] * this->q[2] - 2 * this->q[0] * this->q[3];
- this->R[0][2] = 2 * this->q[1] * this->q[3] + 2 * this->q[0] * this->q[2];
-
- this->R[1][0] = 2 * this->q[1] * this->q[2] + 2 * this->q[0] * this->q[3];
- this->R[1][1] = this->q[0] * this->q[0] - this->q[1] * this->q[1] + this->q[2] * this->q[2] - this->q[3] * this->q[3];
- this->R[1][2] = 2 * this->q[2] * this->q[3] - 2 * this->q[0] * this->q[1];
-
- this->R[2][0] = 2 * this->q[1] * this->q[3] - 2 * this->q[0] * this->q[2];
- this->R[2][1] = 2 * this->q[2] * this->q[3] + 2 * this->q[0] * this->q[1];
- this->R[2][2] = this->q[0] * this->q[0] - this->q[1] * this->q[1] - this->q[2] * this->q[2] + this->q[3] * this->q[3];
-
- // reset the attitude error
- this->S[KC_STATE_D0] = 0;
- this->S[KC_STATE_D1] = 0;
- this->S[KC_STATE_D2] = 0;
-
- // enforce symmetry of the covariance matrix, and ensure the values stay bounded
- for (int i=0; i<KC_STATE_DIM; i++) {
- for (int j=i; j<KC_STATE_DIM; j++) {
- float p = 0.5f*this->P[i][j] + 0.5f*this->P[j][i];
- if (isnan(p) || p > MAX_COVARIANCE) {
- this->P[i][j] = this->P[j][i] = MAX_COVARIANCE;
- } else if ( i==j && p < MIN_COVARIANCE ) {
- this->P[i][j] = this->P[j][i] = MIN_COVARIANCE;
- } else {
- this->P[i][j] = this->P[j][i] = p;
- }
- }
- }
-
- assertStateNotNaN(this);
-}
+//COMMENTED FIRMWARE
+// void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick)
+// {
+// // Matrix to rotate the attitude covariances once updated
+// NO_DMA_CCM_SAFE_ZERO_INIT static float A[KC_STATE_DIM][KC_STATE_DIM];
+// static arm_matrix_instance_f32 Am = {KC_STATE_DIM, KC_STATE_DIM, (float *)A};
+
+// // Temporary matrices for the covariance updates
+// NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
+// static arm_matrix_instance_f32 tmpNN1m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN1d};
+
+// NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
+// static arm_matrix_instance_f32 tmpNN2m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN2d};
+
+// // Incorporate the attitude error (Kalman filter state) with the attitude
+// float v0 = this->S[KC_STATE_D0];
+// float v1 = this->S[KC_STATE_D1];
+// float v2 = this->S[KC_STATE_D2];
+
+// // Move attitude error into attitude if any of the angle errors are large enough
+// if ((fabsf(v0) > 0.1e-3f || fabsf(v1) > 0.1e-3f || fabsf(v2) > 0.1e-3f) && (fabsf(v0) < 10 && fabsf(v1) < 10 && fabsf(v2) < 10))
+// {
+// float angle = arm_sqrt(v0*v0 + v1*v1 + v2*v2);
+// float ca = arm_cos_f32(angle / 2.0f);
+// float sa = arm_sin_f32(angle / 2.0f);
+// float dq[4] = {ca, sa * v0 / angle, sa * v1 / angle, sa * v2 / angle};
+
+// // rotate the quad's attitude by the delta quaternion vector computed above
+// float tmpq0 = dq[0] * this->q[0] - dq[1] * this->q[1] - dq[2] * this->q[2] - dq[3] * this->q[3];
+// float tmpq1 = dq[1] * this->q[0] + dq[0] * this->q[1] + dq[3] * this->q[2] - dq[2] * this->q[3];
+// float tmpq2 = dq[2] * this->q[0] - dq[3] * this->q[1] + dq[0] * this->q[2] + dq[1] * this->q[3];
+// float tmpq3 = dq[3] * this->q[0] + dq[2] * this->q[1] - dq[1] * this->q[2] + dq[0] * this->q[3];
+
+// // normalize and store the result
+// float norm = arm_sqrt(tmpq0 * tmpq0 + tmpq1 * tmpq1 + tmpq2 * tmpq2 + tmpq3 * tmpq3);
+// this->q[0] = tmpq0 / norm;
+// this->q[1] = tmpq1 / norm;
+// this->q[2] = tmpq2 / norm;
+// this->q[3] = tmpq3 / norm;
+
+// /** Rotate the covariance, since we've rotated the body
+// *
+// * This comes from a second order approximation to:
+// * Sigma_post = exps(-d) Sigma_pre exps(-d)'
+// * ~ (I + [[-d]] + [[-d]]^2 / 2) Sigma_pre (I + [[-d]] + [[-d]]^2 / 2)'
+// * where d is the attitude error expressed as Rodriges parameters, ie. d = tan(|v|/2)*v/|v|
+// *
+// * As derived in "Covariance Correction Step for Kalman Filtering with an Attitude"
+// * http://arc.aiaa.org/doi/abs/10.2514/1.G000848
+// */
+
+// float d0 = v0/2; // the attitude error vector (v0,v1,v2) is small,
+// float d1 = v1/2; // so we use a first order approximation to d0 = tan(|v0|/2)*v0/|v0|
+// float d2 = v2/2;
+
+// A[KC_STATE_X][KC_STATE_X] = 1;
+// A[KC_STATE_Y][KC_STATE_Y] = 1;
+// A[KC_STATE_Z][KC_STATE_Z] = 1;
+
+// A[KC_STATE_PX][KC_STATE_PX] = 1;
+// A[KC_STATE_PY][KC_STATE_PY] = 1;
+// A[KC_STATE_PZ][KC_STATE_PZ] = 1;
+
+// A[KC_STATE_D0][KC_STATE_D0] = 1 - d1*d1/2 - d2*d2/2;
+// A[KC_STATE_D0][KC_STATE_D1] = d2 + d0*d1/2;
+// A[KC_STATE_D0][KC_STATE_D2] = -d1 + d0*d2/2;
+
+// A[KC_STATE_D1][KC_STATE_D0] = -d2 + d0*d1/2;
+// A[KC_STATE_D1][KC_STATE_D1] = 1 - d0*d0/2 - d2*d2/2;
+// A[KC_STATE_D1][KC_STATE_D2] = d0 + d1*d2/2;
+
+// A[KC_STATE_D2][KC_STATE_D0] = d1 + d0*d2/2;
+// A[KC_STATE_D2][KC_STATE_D1] = -d0 + d1*d2/2;
+// A[KC_STATE_D2][KC_STATE_D2] = 1 - d0*d0/2 - d1*d1/2;
+
+// mat_trans(&Am, &tmpNN1m); // A'
+// mat_mult(&Am, &this->Pm, &tmpNN2m); // AP
+// mat_mult(&tmpNN2m, &tmpNN1m, &this->Pm); //APA'
+// }
+
+// // convert the new attitude to a rotation matrix, such that we can rotate body-frame velocity and acc
+// this->R[0][0] = this->q[0] * this->q[0] + this->q[1] * this->q[1] - this->q[2] * this->q[2] - this->q[3] * this->q[3];
+// this->R[0][1] = 2 * this->q[1] * this->q[2] - 2 * this->q[0] * this->q[3];
+// this->R[0][2] = 2 * this->q[1] * this->q[3] + 2 * this->q[0] * this->q[2];
+
+// this->R[1][0] = 2 * this->q[1] * this->q[2] + 2 * this->q[0] * this->q[3];
+// this->R[1][1] = this->q[0] * this->q[0] - this->q[1] * this->q[1] + this->q[2] * this->q[2] - this->q[3] * this->q[3];
+// this->R[1][2] = 2 * this->q[2] * this->q[3] - 2 * this->q[0] * this->q[1];
+
+// this->R[2][0] = 2 * this->q[1] * this->q[3] - 2 * this->q[0] * this->q[2];
+// this->R[2][1] = 2 * this->q[2] * this->q[3] + 2 * this->q[0] * this->q[1];
+// this->R[2][2] = this->q[0] * this->q[0] - this->q[1] * this->q[1] - this->q[2] * this->q[2] + this->q[3] * this->q[3];
+
+// // reset the attitude error
+// this->S[KC_STATE_D0] = 0;
+// this->S[KC_STATE_D1] = 0;
+// this->S[KC_STATE_D2] = 0;
+
+// // enforce symmetry of the covariance matrix, and ensure the values stay bounded
+// for (int i=0; i<KC_STATE_DIM; i++) {
+// for (int j=i; j<KC_STATE_DIM; j++) {
+// float p = 0.5f*this->P[i][j] + 0.5f*this->P[j][i];
+// if (isnan(p) || p > MAX_COVARIANCE) {
+// this->P[i][j] = this->P[j][i] = MAX_COVARIANCE;
+// } else if ( i==j && p < MIN_COVARIANCE ) {
+// this->P[i][j] = this->P[j][i] = MIN_COVARIANCE;
+// } else {
+// this->P[i][j] = this->P[j][i] = p;
+// }
+// }
+// }
+
+// assertStateNotNaN(this);
+// }
void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, const Axis3f *acc, uint32_t tick)
{
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_absolute_height.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_absolute_height.c
index 8d41ba9eb8d4db43e41e48e4bf92d04cd985a3ef..fa009ef40b4e2cb95fd9f418a01cccc8165df3fc 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_absolute_height.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_absolute_height.c
@@ -27,8 +27,9 @@
// Measurement model where the measurement is the absolute height
void kalmanCoreUpdateWithAbsoluteHeight(kalmanCoreData_t* this, heightMeasurement_t* height) {
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
- h[KC_STATE_Z] = 1;
- kalmanCoreScalarUpdate(this, &H, height->height - this->S[KC_STATE_Z], height->stdDev);
+ //COMMENTED FIRMWARE
+ // float h[KC_STATE_DIM] = {0};
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+ // h[KC_STATE_Z] = 1;
+ // kalmanCoreScalarUpdate(this, &H, height->height - this->S[KC_STATE_Z], height->stdDev);
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance.c
index 68fd7a6d510cadbf1387cebf944c6a0239555d3c..2c930696627817190bea9449ff7daec2a68cff9c 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance.c
@@ -28,27 +28,28 @@
// Measurement model where the measurement is the distance to a known point in space
void kalmanCoreUpdateWithDistance(kalmanCoreData_t* this, distanceMeasurement_t* d) {
// a measurement of distance to point (x, y, z)
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+ //COMMENTED FIRMWARE
+ // float h[KC_STATE_DIM] = {0};
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
- float dx = this->S[KC_STATE_X] - d->x;
- float dy = this->S[KC_STATE_Y] - d->y;
- float dz = this->S[KC_STATE_Z] - d->z;
+ // float dx = this->S[KC_STATE_X] - d->x;
+ // float dy = this->S[KC_STATE_Y] - d->y;
+ // float dz = this->S[KC_STATE_Z] - d->z;
- float measuredDistance = d->distance;
+ // float measuredDistance = d->distance;
- float predictedDistance = arm_sqrt(powf(dx, 2) + powf(dy, 2) + powf(dz, 2));
- if (predictedDistance != 0.0f) {
- // The measurement is: z = sqrt(dx^2 + dy^2 + dz^2). The derivative dz/dX gives h.
- h[KC_STATE_X] = dx/predictedDistance;
- h[KC_STATE_Y] = dy/predictedDistance;
- h[KC_STATE_Z] = dz/predictedDistance;
- } else {
- // Avoid divide by zero
- h[KC_STATE_X] = 1.0f;
- h[KC_STATE_Y] = 0.0f;
- h[KC_STATE_Z] = 0.0f;
- }
+ // float predictedDistance = arm_sqrt(powf(dx, 2) + powf(dy, 2) + powf(dz, 2));
+ // if (predictedDistance != 0.0f) {
+ // // The measurement is: z = sqrt(dx^2 + dy^2 + dz^2). The derivative dz/dX gives h.
+ // h[KC_STATE_X] = dx/predictedDistance;
+ // h[KC_STATE_Y] = dy/predictedDistance;
+ // h[KC_STATE_Z] = dz/predictedDistance;
+ // } else {
+ // // Avoid divide by zero
+ // h[KC_STATE_X] = 1.0f;
+ // h[KC_STATE_Y] = 0.0f;
+ // h[KC_STATE_Z] = 0.0f;
+ // }
- kalmanCoreScalarUpdate(this, &H, measuredDistance-predictedDistance, d->stdDev);
+ // kalmanCoreScalarUpdate(this, &H, measuredDistance-predictedDistance, d->stdDev);
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance_robust.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance_robust.c
index 9b12da3dffd54435200ee4b1bc693091067ffb43..e0662bd38ae0ea387bdac871a5036dad85de0825 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance_robust.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance_robust.c
@@ -64,170 +64,171 @@ static void GM_state(float e, float * GM_e){
// robsut update function
void kalmanCoreRobustUpdateWithDistance(kalmanCoreData_t* this, distanceMeasurement_t *d)
{
- float dx = this->S[KC_STATE_X] - d->x;
- float dy = this->S[KC_STATE_Y] - d->y;
- float dz = this->S[KC_STATE_Z] - d->z;
- float measuredDistance = d->distance;
-
- float predictedDistance = arm_sqrt(powf(dx, 2) + powf(dy, 2) + powf(dz, 2));
- // innovation term based on x_check
- float error_check = measuredDistance - predictedDistance; // innovation term based on prior state
- // ---------------------- matrix defination ----------------------------- //
- static float P_chol[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 Pc_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)P_chol};
- static float Pc_tran[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 Pc_tran_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_tran};
-
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
- // The Kalman gain as a column vector
- static float Kw[KC_STATE_DIM];
- static arm_matrix_instance_f32 Kwm = {KC_STATE_DIM, 1, (float *)Kw};
-
- static float e_x[KC_STATE_DIM];
- static arm_matrix_instance_f32 e_x_m = {KC_STATE_DIM, 1, e_x};
-
- static float Pc_inv[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 Pc_inv_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_inv};
-
- // rescale matrix
- static float wx_inv[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 wx_invm = {KC_STATE_DIM, KC_STATE_DIM, (float *)wx_inv};
- // tmp matrix for P_chol inverse
- static float tmp1[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 tmp1m = {KC_STATE_DIM, KC_STATE_DIM, (float *)tmp1};
-
- static float Pc_w_inv[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 Pc_w_invm = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_w_inv};
-
- static float P_w[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 P_w_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)P_w};
-
- static float HTd[KC_STATE_DIM];
- static arm_matrix_instance_f32 HTm = {KC_STATE_DIM, 1, HTd};
-
- static float PHTd[KC_STATE_DIM];
- static arm_matrix_instance_f32 PHTm = {KC_STATE_DIM, 1, PHTd};
- // ------------------- Initialization -----------------------//
- // x prior (error state), set to be zeros. Not used for error state Kalman filter. Provide here for completeness
- // float xpr[STATE_DIM] = {0.0};
-
- // x_err comes from the KF update is the state of error state Kalman filter, set to be zero initially
- static float x_err[KC_STATE_DIM] = {0.0};
- static arm_matrix_instance_f32 x_errm = {KC_STATE_DIM, 1, x_err};
- static float X_state[KC_STATE_DIM] = {0.0};
- float P_iter[KC_STATE_DIM][KC_STATE_DIM];
- matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter,this->P); // init P_iter as P_prior
-
- float R_iter = d->stdDev * d->stdDev; // measurement covariance
- vectorcopy(KC_STATE_DIM, X_state, this->S); // copy Xpr to X_State and then update in each iterations
-
- // ---------------------- Start iteration ----------------------- //
- for (int iter = 0; iter < MAX_ITER; iter++){
- // cholesky decomposition for the prior covariance matrix
- Cholesky_Decomposition(KC_STATE_DIM, P_iter, P_chol); // P_chol is a lower triangular matrix
- mat_trans(&Pc_m, &Pc_tran_m);
-
- // decomposition for measurement covariance (scalar case)
- float R_chol = sqrtf(R_iter);
- // construct H matrix
- // X_state updates in each iteration
- float x_iter = X_state[KC_STATE_X], y_iter = X_state[KC_STATE_Y], z_iter = X_state[KC_STATE_Z];
- dx = x_iter - d->x; dy = y_iter - d->y; dz = z_iter - d->z;
-
- float predicted_iter = arm_sqrt(powf(dx, 2) + powf(dy, 2) + powf(dz, 2));
- // innovation term based on x_check
- float error_iter = measuredDistance - predicted_iter;
-
- float e_y = error_iter;
-
- if (predicted_iter != 0.0f) {
- // The measurement is: z = sqrt(dx^2 + dy^2 + dz^2). The derivative dz/dX gives h.
- h[KC_STATE_X] = dx/predicted_iter;
- h[KC_STATE_Y] = dy/predicted_iter;
- h[KC_STATE_Z] = dz/predicted_iter;
-
- } else {
- // Avoid divide by zero
- h[KC_STATE_X] = 1.0f;
- h[KC_STATE_Y] = 0.0f;
- h[KC_STATE_Z] = 0.0f;
- }
- // check the measurement noise
- if (fabsf(R_chol - 0.0f) < 0.0001f){
- e_y = error_iter / 0.0001f;
- }
- else{
- e_y = error_iter / R_chol;
- }
- // Make sure P_chol, lower trangular matrix, is numerically stable
- for (int col=0; col<KC_STATE_DIM; col++) {
- for (int row=col; row<KC_STATE_DIM; row++) {
- if (isnan(P_chol[row][col]) || P_chol[row][col] > UPPER_BOUND) {
- P_chol[row][col] = UPPER_BOUND;
- } else if(row!=col && P_chol[row][col] < LOWER_BOUND){
- P_chol[row][col] = LOWER_BOUND;
- } else if(row==col && P_chol[row][col]<0.0f){
- P_chol[row][col] = 0.0f;
- }
- }
- }
- // Matrix inversion is numerically sensitive.
- // Add small values on the diagonal of P_chol to avoid numerical problems.
- float dummy_value = 1e-9f;
- for (int k=0; k<KC_STATE_DIM; k++){
- P_chol[k][k] = P_chol[k][k] + dummy_value;
- }
- // keep P_chol
- matrixcopy(KC_STATE_DIM, KC_STATE_DIM, tmp1, P_chol);
- mat_inv(&tmp1m, &Pc_inv_m); // Pc_inv_m = inv(Pc_m) = inv(P_chol)
- mat_mult(&Pc_inv_m, &x_errm, &e_x_m); // e_x_m = Pc_inv_m.dot(x_errm)
-
- // compute w_x, w_y --> weighting matrix
- // Since w_x is diagnal matrix, directly compute the inverse
- for (int state_k = 0; state_k < KC_STATE_DIM; state_k++){
- GM_state(e_x[state_k], &wx_inv[state_k][state_k]);
- wx_inv[state_k][state_k] = (float)1.0 / wx_inv[state_k][state_k];
- }
-
- // rescale covariance matrix P
- mat_mult(&Pc_m, &wx_invm, &Pc_w_invm); // Pc_w_invm = P_chol.dot(linalg.inv(w_x))
- mat_mult(&Pc_w_invm, &Pc_tran_m, &P_w_m); // P_w_m = Pc_w_invm.dot(Pc_tran_m) = P_chol.dot(linalg.inv(w_x)).dot(P_chol.T)
-
- // rescale R matrix
- float w_y=0.0; float R_w = 0.0f;
- GM_UWB(e_y, &w_y); // compute the weighted measurement error: w_y
- if (fabsf(w_y - 0.0f) < 0.0001f){
- R_w = (R_chol * R_chol) / 0.0001f;
- }
- else{
- R_w = (R_chol * R_chol) / w_y;
- }
- // ====== INNOVATION COVARIANCE ====== //
-
- mat_trans(&H, &HTm);
- mat_mult(&P_w_m, &HTm, &PHTm); // PHTm = P_w.dot(H.T). The P is the updated P_w
-
- float HPHR = R_w; // HPH' + R. The R is the updated R_w
- for (int i=0; i<KC_STATE_DIM; i++) { // Add the element of HPH' to the above
- HPHR += h[i]*PHTd[i]; // this only works if the update is scalar (as in this function)
- }
- // ====== MEASUREMENT UPDATE ======
- // Calculate the Kalman gain and perform the state update
- for (int i=0; i<KC_STATE_DIM; i++) {
- Kw[i] = PHTd[i]/HPHR; // rescaled kalman gain = (PH' (HPH' + R )^-1) with the updated P_w and R_w
- //[Note]: The error_check here is the innovation term based on x_check, which doesn't change during iterations.
- x_err[i] = Kw[i] * error_check; // error state for next iteration
- X_state[i] = this->S[i] + x_err[i]; // convert to nominal state
- }
- // update P_iter matrix and R matrix for next iteration
- matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter, P_w);
- R_iter = R_w;
- }
-
-
- // After n iterations, we obtain the rescaled (1) P = P_iter, (2) R = R_iter, (3) Kw.
- // Call the kalman update function with weighted P, weighted K, h, and error_check
- kalmanCoreUpdateWithPKE(this, &H, &Kwm, &P_w_m, error_check);
+ //COMMENTED FIRMWARE
+ // float dx = this->S[KC_STATE_X] - d->x;
+ // float dy = this->S[KC_STATE_Y] - d->y;
+ // float dz = this->S[KC_STATE_Z] - d->z;
+ // float measuredDistance = d->distance;
+
+ // float predictedDistance = arm_sqrt(powf(dx, 2) + powf(dy, 2) + powf(dz, 2));
+ // // innovation term based on x_check
+ // float error_check = measuredDistance - predictedDistance; // innovation term based on prior state
+ // // ---------------------- matrix defination ----------------------------- //
+ // static float P_chol[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 Pc_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)P_chol};
+ // static float Pc_tran[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 Pc_tran_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_tran};
+
+ // float h[KC_STATE_DIM] = {0};
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+ // // The Kalman gain as a column vector
+ // static float Kw[KC_STATE_DIM];
+ // static arm_matrix_instance_f32 Kwm = {KC_STATE_DIM, 1, (float *)Kw};
+
+ // static float e_x[KC_STATE_DIM];
+ // static arm_matrix_instance_f32 e_x_m = {KC_STATE_DIM, 1, e_x};
+
+ // static float Pc_inv[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 Pc_inv_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_inv};
+
+ // // rescale matrix
+ // static float wx_inv[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 wx_invm = {KC_STATE_DIM, KC_STATE_DIM, (float *)wx_inv};
+ // // tmp matrix for P_chol inverse
+ // static float tmp1[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 tmp1m = {KC_STATE_DIM, KC_STATE_DIM, (float *)tmp1};
+
+ // static float Pc_w_inv[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 Pc_w_invm = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_w_inv};
+
+ // static float P_w[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 P_w_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)P_w};
+
+ // static float HTd[KC_STATE_DIM];
+ // static arm_matrix_instance_f32 HTm = {KC_STATE_DIM, 1, HTd};
+
+ // static float PHTd[KC_STATE_DIM];
+ // static arm_matrix_instance_f32 PHTm = {KC_STATE_DIM, 1, PHTd};
+ // // ------------------- Initialization -----------------------//
+ // // x prior (error state), set to be zeros. Not used for error state Kalman filter. Provide here for completeness
+ // // float xpr[STATE_DIM] = {0.0};
+
+ // // x_err comes from the KF update is the state of error state Kalman filter, set to be zero initially
+ // static float x_err[KC_STATE_DIM] = {0.0};
+ // static arm_matrix_instance_f32 x_errm = {KC_STATE_DIM, 1, x_err};
+ // static float X_state[KC_STATE_DIM] = {0.0};
+ // float P_iter[KC_STATE_DIM][KC_STATE_DIM];
+ // matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter,this->P); // init P_iter as P_prior
+
+ // float R_iter = d->stdDev * d->stdDev; // measurement covariance
+ // vectorcopy(KC_STATE_DIM, X_state, this->S); // copy Xpr to X_State and then update in each iterations
+
+ // // ---------------------- Start iteration ----------------------- //
+ // for (int iter = 0; iter < MAX_ITER; iter++){
+ // // cholesky decomposition for the prior covariance matrix
+ // Cholesky_Decomposition(KC_STATE_DIM, P_iter, P_chol); // P_chol is a lower triangular matrix
+ // mat_trans(&Pc_m, &Pc_tran_m);
+
+ // // decomposition for measurement covariance (scalar case)
+ // float R_chol = sqrtf(R_iter);
+ // // construct H matrix
+ // // X_state updates in each iteration
+ // float x_iter = X_state[KC_STATE_X], y_iter = X_state[KC_STATE_Y], z_iter = X_state[KC_STATE_Z];
+ // dx = x_iter - d->x; dy = y_iter - d->y; dz = z_iter - d->z;
+
+ // float predicted_iter = arm_sqrt(powf(dx, 2) + powf(dy, 2) + powf(dz, 2));
+ // // innovation term based on x_check
+ // float error_iter = measuredDistance - predicted_iter;
+
+ // float e_y = error_iter;
+
+ // if (predicted_iter != 0.0f) {
+ // // The measurement is: z = sqrt(dx^2 + dy^2 + dz^2). The derivative dz/dX gives h.
+ // h[KC_STATE_X] = dx/predicted_iter;
+ // h[KC_STATE_Y] = dy/predicted_iter;
+ // h[KC_STATE_Z] = dz/predicted_iter;
+
+ // } else {
+ // // Avoid divide by zero
+ // h[KC_STATE_X] = 1.0f;
+ // h[KC_STATE_Y] = 0.0f;
+ // h[KC_STATE_Z] = 0.0f;
+ // }
+ // // check the measurement noise
+ // if (fabsf(R_chol - 0.0f) < 0.0001f){
+ // e_y = error_iter / 0.0001f;
+ // }
+ // else{
+ // e_y = error_iter / R_chol;
+ // }
+ // // Make sure P_chol, lower trangular matrix, is numerically stable
+ // for (int col=0; col<KC_STATE_DIM; col++) {
+ // for (int row=col; row<KC_STATE_DIM; row++) {
+ // if (isnan(P_chol[row][col]) || P_chol[row][col] > UPPER_BOUND) {
+ // P_chol[row][col] = UPPER_BOUND;
+ // } else if(row!=col && P_chol[row][col] < LOWER_BOUND){
+ // P_chol[row][col] = LOWER_BOUND;
+ // } else if(row==col && P_chol[row][col]<0.0f){
+ // P_chol[row][col] = 0.0f;
+ // }
+ // }
+ // }
+ // // Matrix inversion is numerically sensitive.
+ // // Add small values on the diagonal of P_chol to avoid numerical problems.
+ // float dummy_value = 1e-9f;
+ // for (int k=0; k<KC_STATE_DIM; k++){
+ // P_chol[k][k] = P_chol[k][k] + dummy_value;
+ // }
+ // // keep P_chol
+ // matrixcopy(KC_STATE_DIM, KC_STATE_DIM, tmp1, P_chol);
+ // mat_inv(&tmp1m, &Pc_inv_m); // Pc_inv_m = inv(Pc_m) = inv(P_chol)
+ // mat_mult(&Pc_inv_m, &x_errm, &e_x_m); // e_x_m = Pc_inv_m.dot(x_errm)
+
+ // // compute w_x, w_y --> weighting matrix
+ // // Since w_x is diagnal matrix, directly compute the inverse
+ // for (int state_k = 0; state_k < KC_STATE_DIM; state_k++){
+ // GM_state(e_x[state_k], &wx_inv[state_k][state_k]);
+ // wx_inv[state_k][state_k] = (float)1.0 / wx_inv[state_k][state_k];
+ // }
+
+ // // rescale covariance matrix P
+ // mat_mult(&Pc_m, &wx_invm, &Pc_w_invm); // Pc_w_invm = P_chol.dot(linalg.inv(w_x))
+ // mat_mult(&Pc_w_invm, &Pc_tran_m, &P_w_m); // P_w_m = Pc_w_invm.dot(Pc_tran_m) = P_chol.dot(linalg.inv(w_x)).dot(P_chol.T)
+
+ // // rescale R matrix
+ // float w_y=0.0; float R_w = 0.0f;
+ // GM_UWB(e_y, &w_y); // compute the weighted measurement error: w_y
+ // if (fabsf(w_y - 0.0f) < 0.0001f){
+ // R_w = (R_chol * R_chol) / 0.0001f;
+ // }
+ // else{
+ // R_w = (R_chol * R_chol) / w_y;
+ // }
+ // // ====== INNOVATION COVARIANCE ====== //
+
+ // mat_trans(&H, &HTm);
+ // mat_mult(&P_w_m, &HTm, &PHTm); // PHTm = P_w.dot(H.T). The P is the updated P_w
+
+ // float HPHR = R_w; // HPH' + R. The R is the updated R_w
+ // for (int i=0; i<KC_STATE_DIM; i++) { // Add the element of HPH' to the above
+ // HPHR += h[i]*PHTd[i]; // this only works if the update is scalar (as in this function)
+ // }
+ // // ====== MEASUREMENT UPDATE ======
+ // // Calculate the Kalman gain and perform the state update
+ // for (int i=0; i<KC_STATE_DIM; i++) {
+ // Kw[i] = PHTd[i]/HPHR; // rescaled kalman gain = (PH' (HPH' + R )^-1) with the updated P_w and R_w
+ // //[Note]: The error_check here is the innovation term based on x_check, which doesn't change during iterations.
+ // x_err[i] = Kw[i] * error_check; // error state for next iteration
+ // X_state[i] = this->S[i] + x_err[i]; // convert to nominal state
+ // }
+ // // update P_iter matrix and R matrix for next iteration
+ // matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter, P_w);
+ // R_iter = R_w;
+ // }
+
+
+ // // After n iterations, we obtain the rescaled (1) P = P_iter, (2) R = R_iter, (3) Kw.
+ // // Call the kalman update function with weighted P, weighted K, h, and error_check
+ // kalmanCoreUpdateWithPKE(this, &H, &Kwm, &P_w_m, error_check);
}
\ No newline at end of file
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_flow.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_flow.c
index fb692d212ff05bb03c681fb7bd5852247271f8f9..3f4aeb705b3dd988444be584ff66a4ed30e937bb 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_flow.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_flow.c
@@ -34,69 +34,70 @@ static float measuredNY;
void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *flow, const Axis3f *gyro)
{
- // Inclusion of flow measurements in the EKF done by two scalar updates
-
- // ~~~ Camera constants ~~~
- // The angle of aperture is guessed from the raw data register and thankfully look to be symmetric
- float Npix = 30.0; // [pixels] (same in x and y)
- //float thetapix = DEG_TO_RAD * 4.0f; // [rad] (same in x and y)
- float thetapix = DEG_TO_RAD * 4.2f;
- //~~~ Body rates ~~~
- // TODO check if this is feasible or if some filtering has to be done
- float omegax_b = gyro->x * DEG_TO_RAD;
- float omegay_b = gyro->y * DEG_TO_RAD;
-
- // ~~~ Moves the body velocity into the global coordinate system ~~~
- // [bar{x},bar{y},bar{z}]_G = R*[bar{x},bar{y},bar{z}]_B
- //
- // \dot{x}_G = (R^T*[dot{x}_B,dot{y}_B,dot{z}_B])\dot \hat{x}_G
- // \dot{x}_G = (R^T*[dot{x}_B,dot{y}_B,dot{z}_B])\dot \hat{x}_G
- //
- // where \hat{} denotes a basis vector, \dot{} denotes a derivative and
- // _G and _B refer to the global/body coordinate systems.
-
- // Modification 1
- //dx_g = R[0][0] * S[KC_STATE_PX] + R[0][1] * S[KC_STATE_PY] + R[0][2] * S[KC_STATE_PZ];
- //dy_g = R[1][0] * S[KC_STATE_PX] + R[1][1] * S[KC_STATE_PY] + R[1][2] * S[KC_STATE_PZ];
-
-
- float dx_g = this->S[KC_STATE_PX];
- float dy_g = this->S[KC_STATE_PY];
- float z_g = 0.0;
- // Saturate elevation in prediction and correction to avoid singularities
- if ( this->S[KC_STATE_Z] < 0.1f ) {
- z_g = 0.1;
- } else {
- z_g = this->S[KC_STATE_Z];
- }
-
- // ~~~ X velocity prediction and update ~~~
- // predics the number of accumulated pixels in the x-direction
- float omegaFactor = 1.25f;
- float hx[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 Hx = {1, KC_STATE_DIM, hx};
- predictedNX = (flow->dt * Npix / thetapix ) * ((dx_g * this->R[2][2] / z_g) - omegaFactor * omegay_b);
- measuredNX = flow->dpixelx;
-
- // derive measurement equation with respect to dx (and z?)
- hx[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dx_g) / (-z_g * z_g));
- hx[KC_STATE_PX] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g);
-
- //First update
- kalmanCoreScalarUpdate(this, &Hx, measuredNX-predictedNX, flow->stdDevX);
-
- // ~~~ Y velocity prediction and update ~~~
- float hy[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 Hy = {1, KC_STATE_DIM, hy};
- predictedNY = (flow->dt * Npix / thetapix ) * ((dy_g * this->R[2][2] / z_g) + omegaFactor * omegax_b);
- measuredNY = flow->dpixely;
-
- // derive measurement equation with respect to dy (and z?)
- hy[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dy_g) / (-z_g * z_g));
- hy[KC_STATE_PY] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g);
-
- // Second update
- kalmanCoreScalarUpdate(this, &Hy, measuredNY-predictedNY, flow->stdDevY);
+ //COMMENTED FIRMWARE
+// // Inclusion of flow measurements in the EKF done by two scalar updates
+
+// // ~~~ Camera constants ~~~
+// // The angle of aperture is guessed from the raw data register and thankfully look to be symmetric
+// float Npix = 30.0; // [pixels] (same in x and y)
+// //float thetapix = DEG_TO_RAD * 4.0f; // [rad] (same in x and y)
+// float thetapix = DEG_TO_RAD * 4.2f;
+// //~~~ Body rates ~~~
+// // TODO check if this is feasible or if some filtering has to be done
+// float omegax_b = gyro->x * DEG_TO_RAD;
+// float omegay_b = gyro->y * DEG_TO_RAD;
+
+// // ~~~ Moves the body velocity into the global coordinate system ~~~
+// // [bar{x},bar{y},bar{z}]_G = R*[bar{x},bar{y},bar{z}]_B
+// //
+// // \dot{x}_G = (R^T*[dot{x}_B,dot{y}_B,dot{z}_B])\dot \hat{x}_G
+// // \dot{x}_G = (R^T*[dot{x}_B,dot{y}_B,dot{z}_B])\dot \hat{x}_G
+// //
+// // where \hat{} denotes a basis vector, \dot{} denotes a derivative and
+// // _G and _B refer to the global/body coordinate systems.
+
+// // Modification 1
+// //dx_g = R[0][0] * S[KC_STATE_PX] + R[0][1] * S[KC_STATE_PY] + R[0][2] * S[KC_STATE_PZ];
+// //dy_g = R[1][0] * S[KC_STATE_PX] + R[1][1] * S[KC_STATE_PY] + R[1][2] * S[KC_STATE_PZ];
+
+
+// float dx_g = this->S[KC_STATE_PX];
+// float dy_g = this->S[KC_STATE_PY];
+// float z_g = 0.0;
+// // Saturate elevation in prediction and correction to avoid singularities
+// if ( this->S[KC_STATE_Z] < 0.1f ) {
+// z_g = 0.1;
+// } else {
+// z_g = this->S[KC_STATE_Z];
+// }
+
+// // ~~~ X velocity prediction and update ~~~
+// // predics the number of accumulated pixels in the x-direction
+// float omegaFactor = 1.25f;
+// float hx[KC_STATE_DIM] = {0};
+// arm_matrix_instance_f32 Hx = {1, KC_STATE_DIM, hx};
+// predictedNX = (flow->dt * Npix / thetapix ) * ((dx_g * this->R[2][2] / z_g) - omegaFactor * omegay_b);
+// measuredNX = flow->dpixelx;
+
+// // derive measurement equation with respect to dx (and z?)
+// hx[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dx_g) / (-z_g * z_g));
+// hx[KC_STATE_PX] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g);
+
+// //First update
+// kalmanCoreScalarUpdate(this, &Hx, measuredNX-predictedNX, flow->stdDevX);
+
+// // ~~~ Y velocity prediction and update ~~~
+// float hy[KC_STATE_DIM] = {0};
+// arm_matrix_instance_f32 Hy = {1, KC_STATE_DIM, hy};
+// predictedNY = (flow->dt * Npix / thetapix ) * ((dy_g * this->R[2][2] / z_g) + omegaFactor * omegax_b);
+// measuredNY = flow->dpixely;
+
+// // derive measurement equation with respect to dy (and z?)
+// hy[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dy_g) / (-z_g * z_g));
+// hy[KC_STATE_PY] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g);
+
+// // Second update
+// kalmanCoreScalarUpdate(this, &Hy, measuredNY-predictedNY, flow->stdDevY);
}
/**
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_pose.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_pose.c
index 4f1fc7fbb6702fda0bdf0971af5d5f21d5d0c67c..6b17370efca78ce63842bb1401804ef9963ec19f 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_pose.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_pose.c
@@ -30,33 +30,34 @@ void kalmanCoreUpdateWithPose(kalmanCoreData_t* this, poseMeasurement_t *pose)
{
// a direct measurement of states x, y, and z, and orientation
// do a scalar update for each state, since this should be faster than updating all together
- for (int i=0; i<3; i++) {
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
- h[KC_STATE_X+i] = 1;
- kalmanCoreScalarUpdate(this, &H, pose->pos[i] - this->S[KC_STATE_X+i], pose->stdDevPos);
- }
+ //COMMENTED FIRMWARE
+ // for (int i=0; i<3; i++) {
+ // float h[KC_STATE_DIM] = {0};
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+ // h[KC_STATE_X+i] = 1;
+ // kalmanCoreScalarUpdate(this, &H, pose->pos[i] - this->S[KC_STATE_X+i], pose->stdDevPos);
+ // }
- // compute orientation error
- struct quat const q_ekf = mkquat(this->q[1], this->q[2], this->q[3], this->q[0]);
- struct quat const q_measured = mkquat(pose->quat.x, pose->quat.y, pose->quat.z, pose->quat.w);
- struct quat const q_residual = qqmul(qinv(q_ekf), q_measured);
- // small angle approximation, see eq. 141 in http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf
- struct vec const err_quat = vscl(2.0f / q_residual.w, quatimagpart(q_residual));
+ // // compute orientation error
+ // struct quat const q_ekf = mkquat(this->q[1], this->q[2], this->q[3], this->q[0]);
+ // struct quat const q_measured = mkquat(pose->quat.x, pose->quat.y, pose->quat.z, pose->quat.w);
+ // struct quat const q_residual = qqmul(qinv(q_ekf), q_measured);
+ // // small angle approximation, see eq. 141 in http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf
+ // struct vec const err_quat = vscl(2.0f / q_residual.w, quatimagpart(q_residual));
- // do a scalar update for each state
- {
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
- h[KC_STATE_D0] = 1;
- kalmanCoreScalarUpdate(this, &H, err_quat.x, pose->stdDevQuat);
- h[KC_STATE_D0] = 0;
+ // // do a scalar update for each state
+ // {
+ // float h[KC_STATE_DIM] = {0};
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+ // h[KC_STATE_D0] = 1;
+ // kalmanCoreScalarUpdate(this, &H, err_quat.x, pose->stdDevQuat);
+ // h[KC_STATE_D0] = 0;
- h[KC_STATE_D1] = 1;
- kalmanCoreScalarUpdate(this, &H, err_quat.y, pose->stdDevQuat);
- h[KC_STATE_D1] = 0;
+ // h[KC_STATE_D1] = 1;
+ // kalmanCoreScalarUpdate(this, &H, err_quat.y, pose->stdDevQuat);
+ // h[KC_STATE_D1] = 0;
- h[KC_STATE_D2] = 1;
- kalmanCoreScalarUpdate(this, &H, err_quat.z, pose->stdDevQuat);
- }
+ // h[KC_STATE_D2] = 1;
+ // kalmanCoreScalarUpdate(this, &H, err_quat.z, pose->stdDevQuat);
+ // }
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_position.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_position.c
index cae521958ebb519f6b16f622874c5168ce44dca4..0d20ddb8c996ac100a18fc1c9aa5d44c069bddf4 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_position.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_position.c
@@ -29,10 +29,11 @@ void kalmanCoreUpdateWithPosition(kalmanCoreData_t* this, positionMeasurement_t
{
// a direct measurement of states x, y, and z
// do a scalar update for each state, since this should be faster than updating all together
- for (int i=0; i<3; i++) {
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
- h[KC_STATE_X+i] = 1;
- kalmanCoreScalarUpdate(this, &H, xyz->pos[i] - this->S[KC_STATE_X+i], xyz->stdDev);
- }
+ //COMMENTED FIRMWARE
+ // for (int i=0; i<3; i++) {
+ // float h[KC_STATE_DIM] = {0};
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+ // h[KC_STATE_X+i] = 1;
+ // kalmanCoreScalarUpdate(this, &H, xyz->pos[i] - this->S[KC_STATE_X+i], xyz->stdDev);
+ // }
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_sweep_angles.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_sweep_angles.c
index 17c0aae93439f88bfea809602588e97930532d50..e295eb46ece2e5646cae45f67d1be2c65c018e9b 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_sweep_angles.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_sweep_angles.c
@@ -28,67 +28,68 @@
void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *sweepInfo, const uint32_t tick, OutlierFilterLhState_t* sweepOutlierFilterState) {
- // Rotate the sensor position from CF reference frame to global reference frame,
- // using the CF roatation matrix
- vec3d s;
- arm_matrix_instance_f32 Rcf_ = {3, 3, (float32_t *)this->R};
- arm_matrix_instance_f32 scf_ = {3, 1, (float32_t *)*sweepInfo->sensorPos};
- arm_matrix_instance_f32 s_ = {3, 1, s};
- mat_mult(&Rcf_, &scf_, &s_);
+ //COMMENTED FIRMWARE
+ // // Rotate the sensor position from CF reference frame to global reference frame,
+ // // using the CF roatation matrix
+ // vec3d s;
+ // arm_matrix_instance_f32 Rcf_ = {3, 3, (float32_t *)this->R};
+ // arm_matrix_instance_f32 scf_ = {3, 1, (float32_t *)*sweepInfo->sensorPos};
+ // arm_matrix_instance_f32 s_ = {3, 1, s};
+ // mat_mult(&Rcf_, &scf_, &s_);
- // Get the current state values of the position of the crazyflie (global reference frame) and add the relative sensor pos
- vec3d pcf = {this->S[KC_STATE_X] + s[0], this->S[KC_STATE_Y] + s[1], this->S[KC_STATE_Z] + s[2]};
+ // // Get the current state values of the position of the crazyflie (global reference frame) and add the relative sensor pos
+ // vec3d pcf = {this->S[KC_STATE_X] + s[0], this->S[KC_STATE_Y] + s[1], this->S[KC_STATE_Z] + s[2]};
- // Calculate the difference between the rotor and the sensor on the CF (global reference frame)
- const vec3d* pr = sweepInfo->rotorPos;
- vec3d stmp = {pcf[0] - (*pr)[0], pcf[1] - (*pr)[1], pcf[2] - (*pr)[2]};
- arm_matrix_instance_f32 stmp_ = {3, 1, stmp};
+ // // Calculate the difference between the rotor and the sensor on the CF (global reference frame)
+ // const vec3d* pr = sweepInfo->rotorPos;
+ // vec3d stmp = {pcf[0] - (*pr)[0], pcf[1] - (*pr)[1], pcf[2] - (*pr)[2]};
+ // arm_matrix_instance_f32 stmp_ = {3, 1, stmp};
- // Rotate the difference in position to the rotor reference frame,
- // using the rotor inverse rotation matrix
- vec3d sr;
- arm_matrix_instance_f32 Rr_inv_ = {3, 3, (float32_t *)(*sweepInfo->rotorRotInv)};
- arm_matrix_instance_f32 sr_ = {3, 1, sr};
- mat_mult(&Rr_inv_, &stmp_, &sr_);
+ // // Rotate the difference in position to the rotor reference frame,
+ // // using the rotor inverse rotation matrix
+ // vec3d sr;
+ // arm_matrix_instance_f32 Rr_inv_ = {3, 3, (float32_t *)(*sweepInfo->rotorRotInv)};
+ // arm_matrix_instance_f32 sr_ = {3, 1, sr};
+ // mat_mult(&Rr_inv_, &stmp_, &sr_);
- // The following computations are in the rotor refernece frame
- const float x = sr[0];
- const float y = sr[1];
- const float z = sr[2];
- const float t = sweepInfo->t;
- const float tan_t = tanf(t);
+ // // The following computations are in the rotor refernece frame
+ // const float x = sr[0];
+ // const float y = sr[1];
+ // const float z = sr[2];
+ // const float t = sweepInfo->t;
+ // const float tan_t = tanf(t);
- const float r2 = x * x + y * y;
- const float r = arm_sqrt(r2);
+ // const float r2 = x * x + y * y;
+ // const float r = arm_sqrt(r2);
- const float predictedSweepAngle = sweepInfo->calibrationMeasurementModel(x, y, z, t, sweepInfo->calib);
- const float measuredSweepAngle = sweepInfo->measuredSweepAngle;
- const float error = measuredSweepAngle - predictedSweepAngle;
+ // const float predictedSweepAngle = sweepInfo->calibrationMeasurementModel(x, y, z, t, sweepInfo->calib);
+ // const float measuredSweepAngle = sweepInfo->measuredSweepAngle;
+ // const float error = measuredSweepAngle - predictedSweepAngle;
- if (outlierFilterValidateLighthouseSweep(sweepOutlierFilterState, r, error, tick)) {
- // Calculate H vector (in the rotor reference frame)
- const float z_tan_t = z * tan_t;
- const float qNum = r2 - z_tan_t * z_tan_t;
- // Avoid singularity
- if (qNum > 0.0001f) {
- const float q = tan_t / arm_sqrt(qNum);
- vec3d gr = {(-y - x * z * q) / r2, (x - y * z * q) / r2 , q};
+ // if (outlierFilterValidateLighthouseSweep(sweepOutlierFilterState, r, error, tick)) {
+ // // Calculate H vector (in the rotor reference frame)
+ // const float z_tan_t = z * tan_t;
+ // const float qNum = r2 - z_tan_t * z_tan_t;
+ // // Avoid singularity
+ // if (qNum > 0.0001f) {
+ // const float q = tan_t / arm_sqrt(qNum);
+ // vec3d gr = {(-y - x * z * q) / r2, (x - y * z * q) / r2 , q};
- // gr is in the rotor reference frame, rotate back to the global
- // reference frame using the rotor rotation matrix
- vec3d g;
- arm_matrix_instance_f32 gr_ = {3, 1, gr};
- arm_matrix_instance_f32 Rr_ = {3, 3, (float32_t *)(*sweepInfo->rotorRot)};
- arm_matrix_instance_f32 g_ = {3, 1, g};
- mat_mult(&Rr_, &gr_, &g_);
+ // // gr is in the rotor reference frame, rotate back to the global
+ // // reference frame using the rotor rotation matrix
+ // vec3d g;
+ // arm_matrix_instance_f32 gr_ = {3, 1, gr};
+ // arm_matrix_instance_f32 Rr_ = {3, 3, (float32_t *)(*sweepInfo->rotorRot)};
+ // arm_matrix_instance_f32 g_ = {3, 1, g};
+ // mat_mult(&Rr_, &gr_, &g_);
- float h[KC_STATE_DIM] = {0};
- h[KC_STATE_X] = g[0];
- h[KC_STATE_Y] = g[1];
- h[KC_STATE_Z] = g[2];
+ // float h[KC_STATE_DIM] = {0};
+ // h[KC_STATE_X] = g[0];
+ // h[KC_STATE_Y] = g[1];
+ // h[KC_STATE_Z] = g[2];
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
- kalmanCoreScalarUpdate(this, &H, error, sweepInfo->stdDev);
- }
- }
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+ // kalmanCoreScalarUpdate(this, &H, error, sweepInfo->stdDev);
+ // }
+ // }
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tdoa.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tdoa.c
index 761e0e4b01415fcd368ab1000394db945dba2a8e..46f7e17c5f5e2e3c3bdda77521a3acf45fd0e93d 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tdoa.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tdoa.c
@@ -32,63 +32,64 @@ TESTABLE_STATIC uint32_t tdoaCount = 0;
void kalmanCoreUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa)
{
- if (tdoaCount >= 100)
- {
- /**
- * Measurement equation:
- * dR = dT + d1 - d0
- */
-
- float measurement = tdoa->distanceDiff;
-
- // predict based on current state
- float x = this->S[KC_STATE_X];
- float y = this->S[KC_STATE_Y];
- float z = this->S[KC_STATE_Z];
-
- float x1 = tdoa->anchorPositions[1].x, y1 = tdoa->anchorPositions[1].y, z1 = tdoa->anchorPositions[1].z;
- float x0 = tdoa->anchorPositions[0].x, y0 = tdoa->anchorPositions[0].y, z0 = tdoa->anchorPositions[0].z;
-
- float dx1 = x - x1;
- float dy1 = y - y1;
- float dz1 = z - z1;
-
- float dy0 = y - y0;
- float dx0 = x - x0;
- float dz0 = z - z0;
-
- float d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
- float d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));
-
- float predicted = d1 - d0;
- float error = measurement - predicted;
-
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
-
- if ((d0 != 0.0f) && (d1 != 0.0f)) {
- h[KC_STATE_X] = (dx1 / d1 - dx0 / d0);
- h[KC_STATE_Y] = (dy1 / d1 - dy0 / d0);
- h[KC_STATE_Z] = (dz1 / d1 - dz0 / d0);
-
- vector_t jacobian = {
- .x = h[KC_STATE_X],
- .y = h[KC_STATE_Y],
- .z = h[KC_STATE_Z],
- };
-
- point_t estimatedPosition = {
- .x = this->S[KC_STATE_X],
- .y = this->S[KC_STATE_Y],
- .z = this->S[KC_STATE_Z],
- };
-
- bool sampleIsGood = outlierFilterValidateTdoaSteps(tdoa, error, &jacobian, &estimatedPosition);
- if (sampleIsGood) {
- kalmanCoreScalarUpdate(this, &H, error, tdoa->stdDev);
- }
- }
- }
-
- tdoaCount++;
+ //COMMENTED FIRMWARE
+ // if (tdoaCount >= 100)
+ // {
+ // /**
+ // * Measurement equation:
+ // * dR = dT + d1 - d0
+ // */
+
+ // float measurement = tdoa->distanceDiff;
+
+ // // predict based on current state
+ // float x = this->S[KC_STATE_X];
+ // float y = this->S[KC_STATE_Y];
+ // float z = this->S[KC_STATE_Z];
+
+ // float x1 = tdoa->anchorPositions[1].x, y1 = tdoa->anchorPositions[1].y, z1 = tdoa->anchorPositions[1].z;
+ // float x0 = tdoa->anchorPositions[0].x, y0 = tdoa->anchorPositions[0].y, z0 = tdoa->anchorPositions[0].z;
+
+ // float dx1 = x - x1;
+ // float dy1 = y - y1;
+ // float dz1 = z - z1;
+
+ // float dy0 = y - y0;
+ // float dx0 = x - x0;
+ // float dz0 = z - z0;
+
+ // float d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
+ // float d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));
+
+ // float predicted = d1 - d0;
+ // float error = measurement - predicted;
+
+ // float h[KC_STATE_DIM] = {0};
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+
+ // if ((d0 != 0.0f) && (d1 != 0.0f)) {
+ // h[KC_STATE_X] = (dx1 / d1 - dx0 / d0);
+ // h[KC_STATE_Y] = (dy1 / d1 - dy0 / d0);
+ // h[KC_STATE_Z] = (dz1 / d1 - dz0 / d0);
+
+ // vector_t jacobian = {
+ // .x = h[KC_STATE_X],
+ // .y = h[KC_STATE_Y],
+ // .z = h[KC_STATE_Z],
+ // };
+
+ // point_t estimatedPosition = {
+ // .x = this->S[KC_STATE_X],
+ // .y = this->S[KC_STATE_Y],
+ // .z = this->S[KC_STATE_Z],
+ // };
+
+ // bool sampleIsGood = outlierFilterValidateTdoaSteps(tdoa, error, &jacobian, &estimatedPosition);
+ // if (sampleIsGood) {
+ // kalmanCoreScalarUpdate(this, &H, error, tdoa->stdDev);
+ // }
+ // }
+ // }
+
+ // tdoaCount++;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tdoa_robust.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tdoa_robust.c
index bd6f15caf4aaaad9ea19a2a1b27c5680aebefa21..13d31dcdb3b1916bb5786fae0ed487b187300c9f 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tdoa_robust.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tdoa_robust.c
@@ -65,174 +65,175 @@ static void GM_state(float e, float * GM_e){
// robsut update function
void kalmanCoreRobustUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa)
{
- // Measurement equation:
- // d_ij = d_j - d_i
- float measurement = 0.0f;
- float x = this->S[KC_STATE_X];
- float y = this->S[KC_STATE_Y];
- float z = this->S[KC_STATE_Z];
-
- float x1 = tdoa->anchorPositions[1].x, y1 = tdoa->anchorPositions[1].y, z1 = tdoa->anchorPositions[1].z;
- float x0 = tdoa->anchorPositions[0].x, y0 = tdoa->anchorPositions[0].y, z0 = tdoa->anchorPositions[0].z;
-
- float dx1 = x - x1; float dy1 = y - y1; float dz1 = z - z1;
- float dx0 = x - x0; float dy0 = y - y0; float dz0 = z - z0;
-
- float d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
- float d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));
- // if measurements make sense
- if ((d0 != 0.0f) && (d1 != 0.0f)) {
- float predicted = d1 - d0;
- measurement = tdoa->distanceDiff;
+ //COMMENTED FIRMWARE
+ // // Measurement equation:
+ // // d_ij = d_j - d_i
+ // float measurement = 0.0f;
+ // float x = this->S[KC_STATE_X];
+ // float y = this->S[KC_STATE_Y];
+ // float z = this->S[KC_STATE_Z];
+
+ // float x1 = tdoa->anchorPositions[1].x, y1 = tdoa->anchorPositions[1].y, z1 = tdoa->anchorPositions[1].z;
+ // float x0 = tdoa->anchorPositions[0].x, y0 = tdoa->anchorPositions[0].y, z0 = tdoa->anchorPositions[0].z;
+
+ // float dx1 = x - x1; float dy1 = y - y1; float dz1 = z - z1;
+ // float dx0 = x - x0; float dy0 = y - y0; float dz0 = z - z0;
+
+ // float d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
+ // float d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));
+ // // if measurements make sense
+ // if ((d0 != 0.0f) && (d1 != 0.0f)) {
+ // float predicted = d1 - d0;
+ // measurement = tdoa->distanceDiff;
- // innovation term based on prior x
- float error_check = measurement - predicted; // innovation term based on prior state
- // ---------------------- matrix defination ----------------------------- //
- static float P_chol[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 Pc_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)P_chol};
- static float Pc_tran[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 Pc_tran_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_tran};
-
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
- // The Kalman gain as a column vector
- static float Kw[KC_STATE_DIM];
- static arm_matrix_instance_f32 Kwm = {KC_STATE_DIM, 1, (float *)Kw};
+ // // innovation term based on prior x
+ // float error_check = measurement - predicted; // innovation term based on prior state
+ // // ---------------------- matrix defination ----------------------------- //
+ // static float P_chol[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 Pc_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)P_chol};
+ // static float Pc_tran[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 Pc_tran_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_tran};
+
+ // float h[KC_STATE_DIM] = {0};
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+ // // The Kalman gain as a column vector
+ // static float Kw[KC_STATE_DIM];
+ // static arm_matrix_instance_f32 Kwm = {KC_STATE_DIM, 1, (float *)Kw};
- static float e_x[KC_STATE_DIM];
- static arm_matrix_instance_f32 e_x_m = {KC_STATE_DIM, 1, e_x};
+ // static float e_x[KC_STATE_DIM];
+ // static arm_matrix_instance_f32 e_x_m = {KC_STATE_DIM, 1, e_x};
- static float Pc_inv[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 Pc_inv_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_inv};
+ // static float Pc_inv[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 Pc_inv_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_inv};
- // rescale matrix
- static float wx_inv[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 wx_invm = {KC_STATE_DIM, KC_STATE_DIM, (float *)wx_inv};
- // tmp matrix for P_chol inverse
- static float tmp1[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 tmp1m = {KC_STATE_DIM, KC_STATE_DIM, (float *)tmp1};
-
- static float Pc_w_inv[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 Pc_w_invm = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_w_inv};
-
- static float P_w[KC_STATE_DIM][KC_STATE_DIM];
- static arm_matrix_instance_f32 P_w_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)P_w};
-
- static float HTd[KC_STATE_DIM];
- static arm_matrix_instance_f32 HTm = {KC_STATE_DIM, 1, HTd};
-
- static float PHTd[KC_STATE_DIM];
- static arm_matrix_instance_f32 PHTm = {KC_STATE_DIM, 1, PHTd};
- // ------------------- Initialization -----------------------//
- // x prior (error state), set to be zeros. Not used for error state Kalman filter. Provide here for completeness
- // float xpr[STATE_DIM] = {0.0};
-
- // x_err comes from the KF update is the state of error state Kalman filter, set to be zero initially
- static float x_err[KC_STATE_DIM] = {0.0};
- static arm_matrix_instance_f32 x_errm = {KC_STATE_DIM, 1, x_err};
- static float X_state[KC_STATE_DIM] = {0.0};
- float P_iter[KC_STATE_DIM][KC_STATE_DIM];
- matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter,this->P); // init P_iter as P_prior
+ // // rescale matrix
+ // static float wx_inv[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 wx_invm = {KC_STATE_DIM, KC_STATE_DIM, (float *)wx_inv};
+ // // tmp matrix for P_chol inverse
+ // static float tmp1[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 tmp1m = {KC_STATE_DIM, KC_STATE_DIM, (float *)tmp1};
+
+ // static float Pc_w_inv[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 Pc_w_invm = {KC_STATE_DIM, KC_STATE_DIM, (float *)Pc_w_inv};
+
+ // static float P_w[KC_STATE_DIM][KC_STATE_DIM];
+ // static arm_matrix_instance_f32 P_w_m = {KC_STATE_DIM, KC_STATE_DIM, (float *)P_w};
+
+ // static float HTd[KC_STATE_DIM];
+ // static arm_matrix_instance_f32 HTm = {KC_STATE_DIM, 1, HTd};
+
+ // static float PHTd[KC_STATE_DIM];
+ // static arm_matrix_instance_f32 PHTm = {KC_STATE_DIM, 1, PHTd};
+ // // ------------------- Initialization -----------------------//
+ // // x prior (error state), set to be zeros. Not used for error state Kalman filter. Provide here for completeness
+ // // float xpr[STATE_DIM] = {0.0};
+
+ // // x_err comes from the KF update is the state of error state Kalman filter, set to be zero initially
+ // static float x_err[KC_STATE_DIM] = {0.0};
+ // static arm_matrix_instance_f32 x_errm = {KC_STATE_DIM, 1, x_err};
+ // static float X_state[KC_STATE_DIM] = {0.0};
+ // float P_iter[KC_STATE_DIM][KC_STATE_DIM];
+ // matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter,this->P); // init P_iter as P_prior
- float R_iter = tdoa->stdDev * tdoa->stdDev; // measurement covariance
- vectorcopy(KC_STATE_DIM, X_state, this->S); // copy Xpr to X_State and then update in each iterations
-
- // ---------------------- Start iteration ----------------------- //
- for (int iter = 0; iter < MAX_ITER; iter++){
- // cholesky decomposition for the prior covariance matrix
- Cholesky_Decomposition(KC_STATE_DIM, P_iter, P_chol); // P_chol is a lower triangular matrix
- mat_trans(&Pc_m, &Pc_tran_m);
-
- // decomposition for measurement covariance (scalar case)
- float R_chol = sqrtf(R_iter);
- // construct H matrix
- // X_state updates in each iteration
- float x_iter = X_state[KC_STATE_X], y_iter = X_state[KC_STATE_Y], z_iter = X_state[KC_STATE_Z];
-
- dx1 = x_iter - x1; dy1 = y_iter - y1; dz1 = z_iter - z1;
- dx0 = x_iter - x0; dy0 = y_iter - y0; dz0 = z_iter - z0;
-
- d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
- d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));
+ // float R_iter = tdoa->stdDev * tdoa->stdDev; // measurement covariance
+ // vectorcopy(KC_STATE_DIM, X_state, this->S); // copy Xpr to X_State and then update in each iterations
+
+ // // ---------------------- Start iteration ----------------------- //
+ // for (int iter = 0; iter < MAX_ITER; iter++){
+ // // cholesky decomposition for the prior covariance matrix
+ // Cholesky_Decomposition(KC_STATE_DIM, P_iter, P_chol); // P_chol is a lower triangular matrix
+ // mat_trans(&Pc_m, &Pc_tran_m);
+
+ // // decomposition for measurement covariance (scalar case)
+ // float R_chol = sqrtf(R_iter);
+ // // construct H matrix
+ // // X_state updates in each iteration
+ // float x_iter = X_state[KC_STATE_X], y_iter = X_state[KC_STATE_Y], z_iter = X_state[KC_STATE_Z];
+
+ // dx1 = x_iter - x1; dy1 = y_iter - y1; dz1 = z_iter - z1;
+ // dx0 = x_iter - x0; dy0 = y_iter - y0; dz0 = z_iter - z0;
+
+ // d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
+ // d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));
- float predicted_iter = d1 - d0; // predicted measurements in each iteration based on X_state
- float error_iter = measurement - predicted_iter; // innovation term based on iterated X_state
- float e_y = error_iter;
- if ((d0 != 0.0f) && (d1 != 0.0f)){
- // measurement Jacobian changes in each iteration w.r.t linearization point [x_iter, y_iter, z_iter]
- h[KC_STATE_X] = (dx1 / d1 - dx0 / d0);
- h[KC_STATE_Y] = (dy1 / d1 - dy0 / d0);
- h[KC_STATE_Z] = (dz1 / d1 - dz0 / d0);
-
- if (fabsf(R_chol - 0.0f) < 0.0001f){
- e_y = error_iter / 0.0001f;
- }
- else{
- e_y = error_iter / R_chol;
- }
- // Make sure P_chol, lower trangular matrix, is numerically stable
- for (int col=0; col<KC_STATE_DIM; col++) {
- for (int row=col; row<KC_STATE_DIM; row++) {
- if (isnan(P_chol[row][col]) || P_chol[row][col] > UPPER_BOUND) {
- P_chol[row][col] = UPPER_BOUND;
- } else if(row!=col && P_chol[row][col] < LOWER_BOUND){
- P_chol[row][col] = LOWER_BOUND;
- } else if(row==col && P_chol[row][col]<0.0f){
- P_chol[row][col] = 0.0f;
- }
- }
- }
- // Matrix inversion is numerically sensitive.
- // Add small values on the diagonal of P_chol to avoid numerical problems.
- float dummy_value = 1e-9f;
- for (int k=0; k<KC_STATE_DIM; k++){
- P_chol[k][k] = P_chol[k][k] + dummy_value;
- }
- // keep P_chol
- matrixcopy(KC_STATE_DIM, KC_STATE_DIM, tmp1, P_chol);
- mat_inv(&tmp1m, &Pc_inv_m); // Pc_inv_m = inv(Pc_m) = inv(P_chol)
- mat_mult(&Pc_inv_m, &x_errm, &e_x_m); // e_x_m = Pc_inv_m.dot(x_errm)
- // compute w_x, w_y --> weighting matrix
- // Since w_x is diagnal matrix, compute the inverse directly
- for (int state_k = 0; state_k < KC_STATE_DIM; state_k++){
- GM_state(e_x[state_k], &wx_inv[state_k][state_k]);
- wx_inv[state_k][state_k] = (float)1.0 / wx_inv[state_k][state_k];
- }
- // rescale covariance matrix P
- mat_mult(&Pc_m, &wx_invm, &Pc_w_invm); // Pc_w_invm = P_chol.dot(linalg.inv(w_x))
- mat_mult(&Pc_w_invm, &Pc_tran_m, &P_w_m); // P_w_m = Pc_w_invm.dot(Pc_tran_m) = P_chol.dot(linalg.inv(w_x)).dot(P_chol.T)
- // rescale R matrix
- float w_y=0.0; float R_w = 0.0f;
- GM_UWB(e_y, &w_y); // compute the weighted measurement error: w_y
- if (fabsf(w_y - 0.0f) < 0.0001f){
- R_w = (R_chol * R_chol) / 0.0001f;
- }else{
- R_w = (R_chol * R_chol) / w_y;
- }
- // ====== INNOVATION COVARIANCE ====== //
- mat_trans(&H, &HTm);
- mat_mult(&P_w_m, &HTm, &PHTm); // PHTm = P_w.dot(H.T). The P is the updated P_w
-
- float HPHR = R_w; // HPH' + R. The R is the updated R_w
- for (int i=0; i<KC_STATE_DIM; i++) { // Add the element of HPH' to the above
- HPHR += h[i]*PHTd[i]; // this only works if the update is scalar (as in this function)
- }
- // ====== MEASUREMENT UPDATE ======
- // Calculate the Kalman gain and perform the state update
- for (int i=0; i<KC_STATE_DIM; i++) {
- Kw[i] = PHTd[i]/HPHR; // rescaled kalman gain = (PH' (HPH' + R )^-1) with the updated P_w and R_w
- //[Note]: The error_check here is the innovation term based on prior state, which doesn't change during iterations.
- x_err[i] = Kw[i] * error_check; // error state for next iteration
- X_state[i] = this->S[i] + x_err[i]; // convert to nominal state
- }
- // update P_iter matrix and R matrix for next iteration
- matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter, P_w);
- R_iter = R_w;
- }
- }
- // After n iterations, we obtain the rescaled (1) P = P_iter, (2) R = R_iter, (3) Kw.
- // Call the kalman update function with weighted P, weighted K, h, and error_check
- kalmanCoreUpdateWithPKE(this, &H, &Kwm, &P_w_m, error_check);
-
- }
+ // float predicted_iter = d1 - d0; // predicted measurements in each iteration based on X_state
+ // float error_iter = measurement - predicted_iter; // innovation term based on iterated X_state
+ // float e_y = error_iter;
+ // if ((d0 != 0.0f) && (d1 != 0.0f)){
+ // // measurement Jacobian changes in each iteration w.r.t linearization point [x_iter, y_iter, z_iter]
+ // h[KC_STATE_X] = (dx1 / d1 - dx0 / d0);
+ // h[KC_STATE_Y] = (dy1 / d1 - dy0 / d0);
+ // h[KC_STATE_Z] = (dz1 / d1 - dz0 / d0);
+
+ // if (fabsf(R_chol - 0.0f) < 0.0001f){
+ // e_y = error_iter / 0.0001f;
+ // }
+ // else{
+ // e_y = error_iter / R_chol;
+ // }
+ // // Make sure P_chol, lower trangular matrix, is numerically stable
+ // for (int col=0; col<KC_STATE_DIM; col++) {
+ // for (int row=col; row<KC_STATE_DIM; row++) {
+ // if (isnan(P_chol[row][col]) || P_chol[row][col] > UPPER_BOUND) {
+ // P_chol[row][col] = UPPER_BOUND;
+ // } else if(row!=col && P_chol[row][col] < LOWER_BOUND){
+ // P_chol[row][col] = LOWER_BOUND;
+ // } else if(row==col && P_chol[row][col]<0.0f){
+ // P_chol[row][col] = 0.0f;
+ // }
+ // }
+ // }
+ // // Matrix inversion is numerically sensitive.
+ // // Add small values on the diagonal of P_chol to avoid numerical problems.
+ // float dummy_value = 1e-9f;
+ // for (int k=0; k<KC_STATE_DIM; k++){
+ // P_chol[k][k] = P_chol[k][k] + dummy_value;
+ // }
+ // // keep P_chol
+ // matrixcopy(KC_STATE_DIM, KC_STATE_DIM, tmp1, P_chol);
+ // mat_inv(&tmp1m, &Pc_inv_m); // Pc_inv_m = inv(Pc_m) = inv(P_chol)
+ // mat_mult(&Pc_inv_m, &x_errm, &e_x_m); // e_x_m = Pc_inv_m.dot(x_errm)
+ // // compute w_x, w_y --> weighting matrix
+ // // Since w_x is diagnal matrix, compute the inverse directly
+ // for (int state_k = 0; state_k < KC_STATE_DIM; state_k++){
+ // GM_state(e_x[state_k], &wx_inv[state_k][state_k]);
+ // wx_inv[state_k][state_k] = (float)1.0 / wx_inv[state_k][state_k];
+ // }
+ // // rescale covariance matrix P
+ // mat_mult(&Pc_m, &wx_invm, &Pc_w_invm); // Pc_w_invm = P_chol.dot(linalg.inv(w_x))
+ // mat_mult(&Pc_w_invm, &Pc_tran_m, &P_w_m); // P_w_m = Pc_w_invm.dot(Pc_tran_m) = P_chol.dot(linalg.inv(w_x)).dot(P_chol.T)
+ // // rescale R matrix
+ // float w_y=0.0; float R_w = 0.0f;
+ // GM_UWB(e_y, &w_y); // compute the weighted measurement error: w_y
+ // if (fabsf(w_y - 0.0f) < 0.0001f){
+ // R_w = (R_chol * R_chol) / 0.0001f;
+ // }else{
+ // R_w = (R_chol * R_chol) / w_y;
+ // }
+ // // ====== INNOVATION COVARIANCE ====== //
+ // mat_trans(&H, &HTm);
+ // mat_mult(&P_w_m, &HTm, &PHTm); // PHTm = P_w.dot(H.T). The P is the updated P_w
+
+ // float HPHR = R_w; // HPH' + R. The R is the updated R_w
+ // for (int i=0; i<KC_STATE_DIM; i++) { // Add the element of HPH' to the above
+ // HPHR += h[i]*PHTd[i]; // this only works if the update is scalar (as in this function)
+ // }
+ // // ====== MEASUREMENT UPDATE ======
+ // // Calculate the Kalman gain and perform the state update
+ // for (int i=0; i<KC_STATE_DIM; i++) {
+ // Kw[i] = PHTd[i]/HPHR; // rescaled kalman gain = (PH' (HPH' + R )^-1) with the updated P_w and R_w
+ // //[Note]: The error_check here is the innovation term based on prior state, which doesn't change during iterations.
+ // x_err[i] = Kw[i] * error_check; // error state for next iteration
+ // X_state[i] = this->S[i] + x_err[i]; // convert to nominal state
+ // }
+ // // update P_iter matrix and R matrix for next iteration
+ // matrixcopy(KC_STATE_DIM, KC_STATE_DIM, P_iter, P_w);
+ // R_iter = R_w;
+ // }
+ // }
+ // // After n iterations, we obtain the rescaled (1) P = P_iter, (2) R = R_iter, (3) Kw.
+ // // Call the kalman update function with weighted P, weighted K, h, and error_check
+ // kalmanCoreUpdateWithPKE(this, &H, &Kwm, &P_w_m, error_check);
+
+ // }
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tof.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tof.c
index 9d4b49280a030a20b311fe00258114540b2abcb2..a8e96875e05c5ac111b3adcda0d7ea99e3a6f2f8 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tof.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_tof.c
@@ -27,27 +27,28 @@
void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof)
{
- // Updates the filter with a measured distance in the zb direction using the
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+ //COMMENTED FIRMWARE
+ // // Updates the filter with a measured distance in the zb direction using the
+ // float h[KC_STATE_DIM] = {0};
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
- // Only update the filter if the measurement is reliable (\hat{h} -> infty when R[2][2] -> 0)
- if (fabs(this->R[2][2]) > 0.1 && this->R[2][2] > 0){
- float angle = fabsf(acosf(this->R[2][2])) - DEG_TO_RAD * (15.0f / 2.0f);
- if (angle < 0.0f) {
- angle = 0.0f;
- }
- //float predictedDistance = S[KC_STATE_Z] / cosf(angle);
- float predictedDistance = this->S[KC_STATE_Z] / this->R[2][2];
- float measuredDistance = tof->distance; // [m]
+ // // Only update the filter if the measurement is reliable (\hat{h} -> infty when R[2][2] -> 0)
+ // if (fabs(this->R[2][2]) > 0.1 && this->R[2][2] > 0){
+ // float angle = fabsf(acosf(this->R[2][2])) - DEG_TO_RAD * (15.0f / 2.0f);
+ // if (angle < 0.0f) {
+ // angle = 0.0f;
+ // }
+ // //float predictedDistance = S[KC_STATE_Z] / cosf(angle);
+ // float predictedDistance = this->S[KC_STATE_Z] / this->R[2][2];
+ // float measuredDistance = tof->distance; // [m]
- //Measurement equation
- //
- // h = z/((R*z_b)\dot z_b) = z/cos(alpha)
- h[KC_STATE_Z] = 1 / this->R[2][2];
- //h[KC_STATE_Z] = 1 / cosf(angle);
+ // //Measurement equation
+ // //
+ // // h = z/((R*z_b)\dot z_b) = z/cos(alpha)
+ // h[KC_STATE_Z] = 1 / this->R[2][2];
+ // //h[KC_STATE_Z] = 1 / cosf(angle);
- // Scalar update
- kalmanCoreScalarUpdate(this, &H, measuredDistance-predictedDistance, tof->stdDev);
- }
+ // // Scalar update
+ // kalmanCoreScalarUpdate(this, &H, measuredDistance-predictedDistance, tof->stdDev);
+ // }
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_yaw_error.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_yaw_error.c
index d9f903805423e4a2d9a5e93d4446bd2e5abed2c5..e0b2564907c56c427bea1ab2f56b82105de2a878 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_yaw_error.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_yaw_error.c
@@ -27,9 +27,10 @@
void kalmanCoreUpdateWithYawError(kalmanCoreData_t *this, yawErrorMeasurement_t *error)
{
- float h[KC_STATE_DIM] = {0};
- arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+ //COMMENTED FIRMWARE
+ // float h[KC_STATE_DIM] = {0};
+ // arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
- h[KC_STATE_D2] = 1;
- kalmanCoreScalarUpdate(this, &H, this->S[KC_STATE_D2] - error->yawError, error->stdDev);
+ // h[KC_STATE_D2] = 1;
+ // kalmanCoreScalarUpdate(this, &H, this->S[KC_STATE_D2] - error->yawError, error->stdDev);
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/Makefile b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/Makefile
index d8930a3e03daf97157f17f8f933c5f02fcf5d566..b9898090f9d4c5ed65fb4f550e2a047140e9170d 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/Makefile
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/Makefile
@@ -32,6 +32,8 @@ CFLAGS = -mcpu=cortex-a72 \
-I$(INCLUDEPATH23) \
-I$(INCLUDEPATH24) \
-I$(INCLUDEPATH25) \
+ -I$(INCLUDEPATH26) \
+ -I$(INCLUDEPATH27) \
-D__LINUX__
BUILTIN_OPS = -fno-builtin-memset
ASMFLAGS = -mcpu=cortex-a72
@@ -61,6 +63,8 @@ INCLUDEPATH22 ?= ../platform
INCLUDEPATH23 ?= ../modules/interface/lighthouse
INCLUDEPATH24 ?= ../deck/drivers/interface
INCLUDEPATH25 ?= ../modules/interface/kalman_core
+INCLUDEPATH26 ?= ../lib/FatFS
+INCLUDEPATH27 ?= ../utils/src/tdoa
PLATFORM ?= cf2
@@ -148,7 +152,7 @@ OBJS += build/controller.o build/controller_pid.o build/controller_mellinger.o b
OBJS += build/student_attitude_controller.o build/student_pid.o
OBJS += build/controller_student.o
OBJS += build/power_distribution_stock.o
-OBJS += build/collision_avoidance.o #health.o
+OBJS += build/collision_avoidance.o build/health.o
# Kalman estimator
OBJS += build/estimator_kalman.o build/kalman_core.o build/kalman_supervisor.o
@@ -159,7 +163,7 @@ OBJS += build/mm_tdoa_robust.o build/mm_distance_robust.o
OBJS += build/crtp_commander_high_level.o build/planner.o build/pptraj.o build/pptraj_compressed.o
# Deck Core
-OBJS += build/deck.o deck_info.o build/deck_drivers.o build/deck_test.o build/deck_memory.o
+OBJS += build/deck.o build/deck_info.o build/deck_drivers.o build/deck_test.o build/deck_memory.o
# Deck API
OBJS += build/deck_constants.o
@@ -175,12 +179,12 @@ OBJS += build/buzzdeck.o
OBJS += build/gtgps.o
OBJS += build/cppmdeck.o
OBJS += build/usddeck.o
-OBJS += build/zranger.o zranger2.o
+OBJS += build/zranger.o build/zranger2.o
OBJS += build/locodeck.o
OBJS += build/clockCorrectionEngine.o
OBJS += build/lpsTwrTag.o
OBJS += build/lpsTdoa2Tag.o
-OBJS += build/lpsTdoa3Tag.o tdoaEngineInstance.o tdoaEngine.o tdoaStats.o tdoaStorage.o
+OBJS += build/lpsTdoa3Tag.o build/tdoaEngineInstance.o build/tdoaEngine.o build/tdoaStats.o build/tdoaStorage.o
OBJS += build/outlierFilter.o
OBJS += build/flowdeck_v1v2.o
OBJS += build/oa.o
@@ -192,7 +196,7 @@ OBJS += build/activeMarkerDeck.o
ifeq ($(UART2_LINK), 1)
CFLAGS += -DUART2_LINK_COMM
else
-OBJS += aideck.o
+OBJS += build/aideck.o
endif
ifeq ($(LPS_TDOA_ENABLE), 1)
@@ -283,9 +287,6 @@ build/%.o : ../musl_libc/%.c
build/%.o : ../../../Source/%.c
$(CROSS)gcc $(CFLAGS) -c -o $@ $<
-build/%.o : ../../../Source/portable/GCC/ARM_CA72_64_BIT/%.c
- $(CROSS)gcc $(CFLAGS) -c -o $@ $<
-
build/%.o : ../../../Source/portable/GCC/ARM_CA72_64_BIT/%.S
$(CROSS)as $(ASMFLAGS) -c -o $@ $<
@@ -340,6 +341,9 @@ build/%.o : ../utils/src/tdoa/%.c
build/%.o : ../lib/vl53l1/core/src/%.c
$(CROSS)gcc $(CFLAGS) -c -o $@ $<
+build/%.o : ../utils/src/%.c
+ $(CROSS)gcc $(CFLAGS) -c -o $@ $<
+
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/activeMarkerDeck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/activeMarkerDeck.o
new file mode 100644
index 0000000000000000000000000000000000000000..d4ace454eaec7eb40fc3a525cacf43c0640a66db
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/activeMarkerDeck.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/aideck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/aideck.o
new file mode 100644
index 0000000000000000000000000000000000000000..ff893923351f447170dde49533864db9a1273c0a
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/aideck.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/bigquad.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/bigquad.o
new file mode 100644
index 0000000000000000000000000000000000000000..8f67334bf3aa7fba349274d76188b3890045ab0f
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/bigquad.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/buzzdeck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/buzzdeck.o
new file mode 100644
index 0000000000000000000000000000000000000000..a8cf2a882eef3882df87e5f0fc2683bd2a7173b9
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/buzzdeck.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/cfassert.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/cfassert.o
new file mode 100644
index 0000000000000000000000000000000000000000..828bdec417db66f62ba1f0a589972e198f3db4bd
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/cfassert.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/clockCorrectionEngine.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/clockCorrectionEngine.o
new file mode 100644
index 0000000000000000000000000000000000000000..c4bd6c852f222eb26f950a9e2ed933ddaae6bd2c
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/clockCorrectionEngine.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/cppmdeck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/cppmdeck.o
new file mode 100644
index 0000000000000000000000000000000000000000..a9c70c39d24dc7ee8ffc2c23405723368d4f8d59
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/cppmdeck.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/cpuid.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/cpuid.o
new file mode 100644
index 0000000000000000000000000000000000000000..6190496b593e74752ca7b3f693a55eb4f9eeefda
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/cpuid.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crc32.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crc32.o
new file mode 100644
index 0000000000000000000000000000000000000000..e0a3b990b4d1624c183e510ab5f4d510fb28b7cf
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crc32.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_commander_high_level.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_commander_high_level.o
new file mode 100644
index 0000000000000000000000000000000000000000..47a7737d61d83d401a4be63be346a2174f7f9030
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_commander_high_level.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/debug.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/debug.o
new file mode 100644
index 0000000000000000000000000000000000000000..5dc45cbd159bcb86d0bd06f8aaf9d138948f8e89
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/debug.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck.o
new file mode 100644
index 0000000000000000000000000000000000000000..3a0f830b982e6c8a4ec5620b68126a4d2c61f378
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_analog.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_analog.o
new file mode 100644
index 0000000000000000000000000000000000000000..6d50f893dbeb54baf37ce7cdd3f6dfd36210bc21
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_analog.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_constants.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_constants.o
new file mode 100644
index 0000000000000000000000000000000000000000..55e9a6eae14ff4d89ae78df723128e28e3e42624
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_constants.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_digital.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_digital.o
new file mode 100644
index 0000000000000000000000000000000000000000..0289c8e751b973116c59fa74a3d24d1f74ed4753
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_digital.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_drivers.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_drivers.o
new file mode 100644
index 0000000000000000000000000000000000000000..aaca6502919044831ed2501784d6c3b26ea6a817
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_drivers.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_info.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_info.o
new file mode 100644
index 0000000000000000000000000000000000000000..c9b4a9d8c649ac60beedf229ddebdfc2f9fa9add
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_info.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_memory.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_memory.o
new file mode 100644
index 0000000000000000000000000000000000000000..9f6725e855fe47cca9e56c71696e73fa53d36ea9
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_memory.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_spi.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_spi.o
new file mode 100644
index 0000000000000000000000000000000000000000..41bebca0b65906e729862ef06329be914cc338ed
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_spi.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_spi3.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_spi3.o
new file mode 100644
index 0000000000000000000000000000000000000000..6cef5099a5e88d6f18fad4cb8483cbfa6cc418e0
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_spi3.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_test.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_test.o
new file mode 100644
index 0000000000000000000000000000000000000000..b7d44112cda4d1ebf7c4dff815caf24e2ed687c5
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_test.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eprintf.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eprintf.o
new file mode 100644
index 0000000000000000000000000000000000000000..24a9c03ad96d39fcce2611de37ecdb5e63db38af
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eprintf.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator_kalman.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator_kalman.o
new file mode 100644
index 0000000000000000000000000000000000000000..918557966a7be006a2dc372b37d901723bc6d3c9
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator_kalman.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exptest.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exptest.o
new file mode 100644
index 0000000000000000000000000000000000000000..93189f286f562a345223a6e124c1cfe81e2be95a
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exptest.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exptestBolt.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exptestBolt.o
new file mode 100644
index 0000000000000000000000000000000000000000..944a009b7cd0bee73e4da017389e4fac0c625cbb
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exptestBolt.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exptestRR.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exptestRR.o
new file mode 100644
index 0000000000000000000000000000000000000000..b02c228187f2ef3ef0be96992af168443b1eee34
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exptestRR.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/filter.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/filter.o
new file mode 100644
index 0000000000000000000000000000000000000000..9a29d97d2388558e9058e4456155b6ac8cad2c61
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/filter.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/flowdeck_v1v2.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/flowdeck_v1v2.o
new file mode 100644
index 0000000000000000000000000000000000000000..6012d21e811d8d93803f5d6f8eefe107f06e9adc
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/flowdeck_v1v2.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/gtgps.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/gtgps.o
new file mode 100644
index 0000000000000000000000000000000000000000..f80236e5061f3295c45971c003b503f665d8e5e0
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/gtgps.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/health.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/health.o
new file mode 100644
index 0000000000000000000000000000000000000000..05f25bbb5fdb1c4ce84bec5c2e30ac9c2746e803
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/health.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kalman_core.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kalman_core.o
new file mode 100644
index 0000000000000000000000000000000000000000..563d5e817e9c5422eb2458c1eb223714987d37dd
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kalman_core.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kalman_supervisor.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kalman_supervisor.o
new file mode 100644
index 0000000000000000000000000000000000000000..591bc7f5c64713d56229af81fe475d91738eb534
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kalman_supervisor.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledring12.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledring12.o
new file mode 100644
index 0000000000000000000000000000000000000000..df44183d277c7a5e5f60f822788c3dc5f473e0f9
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledring12.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse.o
new file mode 100644
index 0000000000000000000000000000000000000000..0ae8aed52372ff57b114402443984a3391433275
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/locodeck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/locodeck.o
new file mode 100644
index 0000000000000000000000000000000000000000..cf0e4bce1887145027ce18f0d83298384cd4a9f2
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/locodeck.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa2Tag.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa2Tag.o
new file mode 100644
index 0000000000000000000000000000000000000000..e230a4e697b3232b837bce6a0be3b47be4c51156
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa2Tag.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa3Tag.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa3Tag.o
new file mode 100644
index 0000000000000000000000000000000000000000..a4f95e6fb180fa842b3c89e970ba2dd9519a60f7
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa3Tag.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTwrTag.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTwrTag.o
new file mode 100644
index 0000000000000000000000000000000000000000..2616de8729482728f3332653b089c5819c76cd78
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTwrTag.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_absolute_height.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_absolute_height.o
new file mode 100644
index 0000000000000000000000000000000000000000..d7c99d4c7651551f9dbfa3091c6e6c50781cb298
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_absolute_height.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_distance.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_distance.o
new file mode 100644
index 0000000000000000000000000000000000000000..813c4c723b1d1b52f18644f8dc547aa65b9cff60
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_distance.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_distance_robust.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_distance_robust.o
new file mode 100644
index 0000000000000000000000000000000000000000..64218c8a8ed0be9de814fad1e64ae01f6b3e82e1
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_distance_robust.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_flow.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_flow.o
new file mode 100644
index 0000000000000000000000000000000000000000..da92486b8886c9a3eb522efc3c65f2724d979fcc
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_flow.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_pose.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_pose.o
new file mode 100644
index 0000000000000000000000000000000000000000..d2dbcc0b61384672c7b10fabf4f08a78502c0ee9
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_pose.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_position.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_position.o
new file mode 100644
index 0000000000000000000000000000000000000000..0e6d0b9a4da57201ac5963fe61d11adb1269b14e
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_position.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_sweep_angles.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_sweep_angles.o
new file mode 100644
index 0000000000000000000000000000000000000000..87fc486d1f9fb4981ad76e1ab4b0c0a8f64bb445
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_sweep_angles.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_tdoa.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_tdoa.o
new file mode 100644
index 0000000000000000000000000000000000000000..cb8ee1445917c7146ff29405dbc1607da748677e
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_tdoa.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_tdoa_robust.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_tdoa_robust.o
new file mode 100644
index 0000000000000000000000000000000000000000..cc49247361997ea1bdb972cd45665f88f645a7d2
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_tdoa_robust.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_tof.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_tof.o
new file mode 100644
index 0000000000000000000000000000000000000000..8830ddd0e87cbf77c12b8e9eb3b6bbb4920d9b75
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_tof.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_yaw_error.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_yaw_error.o
new file mode 100644
index 0000000000000000000000000000000000000000..863f4e1c34ad4590b77f4fe86f566cb0cfd4014f
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/mm_yaw_error.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/multiranger.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/multiranger.o
new file mode 100644
index 0000000000000000000000000000000000000000..53d37adf73b7b073c1279f93d845b3a1eb655fb2
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/multiranger.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/num.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/num.o
new file mode 100644
index 0000000000000000000000000000000000000000..4d9fd5f2d3727fd6f0485968050b44b455ae3d73
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/num.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/oa.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/oa.o
new file mode 100644
index 0000000000000000000000000000000000000000..56905475c647a3ab8d1f2f64a4f28ae20a0d3c6b
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/oa.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/outlierFilter.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/outlierFilter.o
new file mode 100644
index 0000000000000000000000000000000000000000..4027af9286a3fb491bb98c413a71727e14af63cd
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/outlierFilter.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/planner.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/planner.o
new file mode 100644
index 0000000000000000000000000000000000000000..c2bd6ddde79620add0e19bff4afd404bc8e3a2d0
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/planner.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj.o
new file mode 100644
index 0000000000000000000000000000000000000000..fd62e0b8bc789a00ca7335ad1004c35c4a7c3d17
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj_compressed.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj_compressed.o
new file mode 100644
index 0000000000000000000000000000000000000000..5948fab43d8341736425fe9204614cf99909fcb5
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj_compressed.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaEngine.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaEngine.o
new file mode 100644
index 0000000000000000000000000000000000000000..aa5fadb61d4970d9a2d82005128b31436be69926
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaEngine.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaEngineInstance.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaEngineInstance.o
new file mode 100644
index 0000000000000000000000000000000000000000..da59c662603facd30a8e1c0c42df30bed72eb4d2
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaEngineInstance.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaStats.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaStats.o
new file mode 100644
index 0000000000000000000000000000000000000000..56bcff0ed8529d7e84039d2dd68ab78db6e546bd
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaStats.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaStorage.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaStorage.o
new file mode 100644
index 0000000000000000000000000000000000000000..41a4be52ab50c9e87468ca9ce111a0dad3ecc8d3
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/tdoaStorage.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/uart.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/uart.o
index 2e96d794b41454348d431df4d5775bb1b4035d61..90412596972e866cbc49a2fa1290d91f25e100d2 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/uart.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/uart.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usddeck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usddeck.o
new file mode 100644
index 0000000000000000000000000000000000000000..d9b66723802e7198793a63fe5a6ee8f930ae9eeb
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usddeck.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger.o
new file mode 100644
index 0000000000000000000000000000000000000000..bd24ca0092ba73c6c374841bc6ae4214b3bb302b
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger2.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger2.o
new file mode 100644
index 0000000000000000000000000000000000000000..78fad7ca3378b8c09a6d1e575e66c2ec07ea5443
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger2.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/cf_math.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/cf_math.h
index 4a8aae466fca2196d8851f4f200220b9c23428c5..b99fe11bf297dddc41fc63922c22194d1c524750 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/cf_math.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/cf_math.h
@@ -36,10 +36,9 @@
#pragma GCC diagnostic pop
#include "cfassert.h"
-
-
-// #define DEG_TO_RAD (PI/180.0f)
-// #define RAD_TO_DEG (180.0f/PI)
+//COMMENTED FIRMWARE
+#define DEG_TO_RAD (3.14/180.0f)
+#define RAD_TO_DEG (180.0f/3.14)
#define MIN(a, b) ((b) < (a) ? (b) : (a))
#define MAX(a, b) ((b) > (a) ? (b) : (a))