diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/config/FreeRTOSConfig.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/config/FreeRTOSConfig.h
index ad9af7c291e94d0fdf26d8b20e4a939148a3f503..6432827ab965015ca062675984252a1d564a7ca2 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/config/FreeRTOSConfig.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/config/FreeRTOSConfig.h
@@ -39,7 +39,6 @@
by writing to Richard Barry, contact details for whom are available on the
FreeRTOS WEB site.
- 1 tab == 4 spaces!
http://www.FreeRTOS.org - Documentation, latest information, license and
contact details.
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/activeMarkerUartTest.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/activeMarkerUartTest.c
index 0f94ec36566f09a9eefb53f9f1616a0a273c7adb..0e4a90b2645675e1ee791a98abf4b8dcb9349a54 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/activeMarkerUartTest.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/activeMarkerUartTest.c
@@ -67,7 +67,7 @@ static void task(void *param)
}
}
- vTaskDelay(M2T(100));
+ //vTaskDelay(M2T(100));
}
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/aidecktest.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/aidecktest.c
index 0103bb869fc0727b2872bfa2cbcc8f468951e4c4..9de7b78c35becb964ef6f4978d662c09bd13eeb3 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/aidecktest.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/test/aidecktest.c
@@ -233,7 +233,7 @@ static bool aitdecktestTest()
pinMode(DECK_GPIO_IO4, INPUT_PULLUP);
// Wait for the NINA to start
- vTaskDelay(M2T(1000));
+ // vTaskDelay(M2T(1000));
// Empty the buffer from NINA
while (uart2GetDataWithDefaultTimeout(&byte) == true)
;
@@ -250,7 +250,7 @@ static bool aitdecktestTest()
testHasBeenTriggered = true;
}
- vTaskDelay(M2T(100));
+ // vTaskDelay(M2T(100));
}
// Send test for button high NINA
@@ -297,7 +297,7 @@ static bool aitdecktestTest()
if (testOnGAP8(GAP8_GPIO_COMMAND|GAP8_GPIO_MASK, GAP8_GPIO_MASK_EXPECTED) == true)
{
- vTaskDelay(M2T(100));
+ // vTaskDelay(M2T(100));
// Send test for GPIO mask to NINA
if (testOnNinaMask(NINA_GAP8_GPIO_COMMAND, &gpio_mask) == true)
{
@@ -317,7 +317,7 @@ static bool aitdecktestTest()
{
DEBUG_PRINT("Set GAP8 gpio mask [FAILED]\r\n");
}
- vTaskDelay(M2T(100));
+ // vTaskDelay(M2T(100));
uint8_t not_mask = (~GAP8_GPIO_MASK) & 0X3F;
@@ -325,7 +325,7 @@ static bool aitdecktestTest()
// Send test for ~ GPIO mask to GAP8
if (testOnGAP8(GAP8_GPIO_COMMAND | not_mask, GAP8_GPIO_MASK_EXPECTED) == true)
{
- vTaskDelay(M2T(100));
+ // vTaskDelay(M2T(100));
// Send test for ~ GPIO mask to NINA
if (testOnNinaMask(NINA_GAP8_GPIO_COMMAND, &gpio_mask) == true)
{
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/zranger2.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/zranger2.c
index 014ba745bafd816540ca3149300ee28aad47ba5e..71767e1003a55eb3960505ee5b23a5171998b40e 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/zranger2.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/zranger2.c
@@ -69,7 +69,7 @@ static uint16_t zRanger2GetMeasurementAndRestart(VL53L1_Dev_t *dev)
while (dataReady == 0)
{
status = VL53L1_GetMeasurementDataReady(dev, &dataReady);
- vTaskDelay(M2T(1));
+ // vTaskDelay(M2T(1));
}
status = VL53L1_GetRangingMeasurementData(dev, &rangingData);
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/bosch/src/bstdr_comm_support.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/bosch/src/bstdr_comm_support.c
index eb5519a7c2702b664da7eb48093fd55b71df6101..3b40d43939c1ac2ea0f49b875fbcce385b6a7210 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/bosch/src/bstdr_comm_support.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/bosch/src/bstdr_comm_support.c
@@ -158,6 +158,6 @@ bstdr_ret_t bstdr_burst_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *reg_dat
void bstdr_ms_delay(uint32_t period)
{
/**< Delay code comes */
- vTaskDelay(M2T(period)); // Delay a while to let the device stabilize
+ // vTaskDelay(M2T(period)); // Delay a while to let the device stabilize
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/eeprom.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/eeprom.c
index 422e688ebec307a25c966026c24eab032b6e22c8..c76177aa7ce7f66ea20bee35ead915f39175fe74 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/eeprom.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/eeprom.c
@@ -181,7 +181,7 @@ bool eepromWriteBuffer(const uint8_t* buffer, uint16_t writeAddr, uint16_t len)
if (status) {
break;
}
- vTaskDelay(M2T(6));
+ //vTaskDelay(M2T(6));
}
if (!status) {
return false;
@@ -196,7 +196,7 @@ bool eepromWriteBuffer(const uint8_t* buffer, uint16_t writeAddr, uint16_t len)
break;
}
- vTaskDelay(M2T(1));
+ // vTaskDelay(M2T(1));
}
if (!status) {
return false;
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/nvic.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/nvic.c
index 9cc83b178320a5b28572c069c48ee075cdb5fd37..3aca5d57074128ded53c8b9ec0a138d54cefa35f 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/nvic.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/nvic.c
@@ -47,7 +47,7 @@ extern void tickFreeRTOS(void);
void DONT_DISCARD SysTick_Handler(void)
{
- tickFreeRTOS();
+ // tickFreeRTOS();
}
#ifdef NVIC_NOT_USED_BY_FREERTOS
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/amg8833.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/amg8833.c
index 97f1f778e7ea75a618b698bdbee5f8073ce1baa3..ab7e4949c5b84f164e348ee0cb6eb9dd9f876475 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/amg8833.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/amg8833.c
@@ -51,7 +51,7 @@ bool begin(AMG8833_Dev_t *dev, I2C_Dev *I2Cx)
bool interrupts_set = disableInterrupt(dev);
//set to 10 FPS
bool fps_set = write8(dev, AMG88xx_FPSC, (AMG88xx_FPS_10 & 0x01));
- vTaskDelay(M2T(10));
+ //vTaskDelay(M2T(10));
return mode_selected && software_resetted &&
interrupts_set && fps_set;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/ledseq.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/ledseq.c
index ff5cd881ac71872fccefdfe17cf51f015b56b48b..b6a387056b99a563eec4c752da4d5db54a6591a4 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/ledseq.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/ledseq.c
@@ -356,7 +356,7 @@ static void runLedseq( xTimerHandle xTimer ) {
if (step->action == 0) {
break;
}
- xTimerChangePeriod(xTimer, M2T(step->action), 0);
+ // xTimerChangePeriod(xTimer, M2T(step->action), 0);
xTimerStart(xTimer, 0);
leave = true;
break;
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/ow_syslink.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/ow_syslink.c
index b858b5f9af3310933dd13f2798fbe6a6c2533ef9..a3224d02ef45f3c69625cdf6d31b3740be1e6745 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/ow_syslink.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/ow_syslink.c
@@ -154,20 +154,20 @@ static bool owSyslinkTransfer(uint8_t type, uint8_t length)
syslinkSendPacket(&slp);
// Wait for reply
- if (xSemaphoreTake(waitForReply, M2T(5000)) == pdTRUE)
- //if (xSemaphoreTake(waitForReply, portMAX_DELAY))
- {
- // We have now got a reply and *owCmd has been filled with data
- if (owDataIsValid)
- {
- owDataIsValid = false;
- return true;
- }
- }
- else
- {
- DEBUG_PRINT("Cmd 0x%X timeout.\n", slp.type);
- }
+ // if (xSemaphoreTake(waitForReply, M2T(5000)) == pdTRUE)
+ // //if (xSemaphoreTake(waitForReply, portMAX_DELAY))
+ // {
+ // // We have now got a reply and *owCmd has been filled with data
+ // if (owDataIsValid)
+ // {
+ // owDataIsValid = false;
+ // return true;
+ // }
+ // }
+ // else
+ // {
+ // DEBUG_PRINT("Cmd 0x%X timeout.\n", slp.type);
+ // }
return false;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/radiolink.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/radiolink.c
index 738901f330ca8babb6e4d8f913681f028f5bf1a9..b94ddc26fc988a35a92174f2d9e8188e5b7614da 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/radiolink.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/radiolink.c
@@ -71,7 +71,8 @@ static uint32_t lastPacketTick;
static volatile P2PCallback p2p_callback;
static bool radiolinkIsConnected(void) {
- return (xTaskGetTickCount() - lastPacketTick) < M2T(RADIO_ACTIVITY_TIMEOUT_MS);
+ //return (xTaskGetTickCount() - lastPacketTick) < M2T(RADIO_ACTIVITY_TIMEOUT_MS);
+ return true;
}
static struct crtpLinkOperations radiolinkOp =
@@ -197,10 +198,10 @@ void radiolinkSyslinkDispatch(SyslinkPacket *slp)
static int radiolinkReceiveCRTPPacket(CRTPPacket *p)
{
- if (xQueueReceive(crtpPacketDelivery, p, M2T(100)) == pdTRUE)
- {
- return 0;
- }
+ // if (xQueueReceive(crtpPacketDelivery, p, M2T(100)) == pdTRUE)
+ // {
+ // return 0;
+ // }
return -1;
}
@@ -220,10 +221,10 @@ static int radiolinkSendCRTPPacket(CRTPPacket *p)
slp.length = p->size + 1;
memcpy(slp.data, &p->header, p->size + 1);
- if (xQueueSend(txQueue, &slp, M2T(100)) == pdTRUE)
- {
- return true;
- }
+ // if (xQueueSend(txQueue, &slp, M2T(100)) == pdTRUE)
+ // {
+ // return true;
+ // }
return false;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_bmi088_bmp388.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_bmi088_bmp388.c
index 0ecc3fd33ac2a888350b43214f2ad72362cd3247..7cb36b755fa8cfc85a5e728ccf6c1ada9c53018f 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_bmi088_bmp388.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_bmi088_bmp388.c
@@ -27,7 +27,7 @@
#define DEBUG_MODULE "IMU"
#include <math.h>
-#include "empty_math.h"
+// #include "empty_math.h"
#include "sensors_bmi088_bmp388.h"
#include "stm32fxxx.h"
@@ -219,7 +219,7 @@ bstdr_ret_t bmi088_burst_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *reg_da
void bmi088_ms_delay(uint32_t period)
{
/**< Delay code comes */
- vTaskDelay(M2T(period)); // Delay a while to let the device stabilize
+ // vTaskDelay(M2T(period)); // Delay a while to let the device stabilize
}
static uint16_t sensorsGyroGet(Axis3i16* dataOut)
@@ -372,7 +372,7 @@ static void sensorsDeviceInit(void)
isBarometerPresent = false;
// Wait for sensors to startup
- vTaskDelay(M2T(SENSORS_STARTUP_TIME_MS));
+ // vTaskDelay(M2T(SENSORS_STARTUP_TIME_MS));
/* BMI088
* The bmi088Dev structure should have been filled in by the backend
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_bosch.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_bosch.c
index 85adbf7bec3cf97aba3faa4d905b41e0e3701644..2607081aed8ad5e1df06e13a7222e715ce128a28 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_bosch.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_bosch.c
@@ -34,7 +34,7 @@
#include "sensors_bosch.h"
#include <math.h>
-#include "empty_math.h"
+// #include "empty_math.h"
#include "stm32fxxx.h"
@@ -230,7 +230,7 @@ static void sensorsDeviceInit(void)
isBarometerPresent = false;
// Wait for sensors to startup
- vTaskDelay(M2T(SENSORS_STARTUP_TIME_MS));
+ // vTaskDelay(M2T(SENSORS_STARTUP_TIME_MS));
/* BMI160 */
// assign bus read function
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_mpu9250_lps25h.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_mpu9250_lps25h.c
index 3b441215d8e8830df4f9313dea4e950c5e75cd52..5fa7a0b6c029fb790851ab2c7edb63c8bb7271ef 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_mpu9250_lps25h.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/sensors_mpu9250_lps25h.c
@@ -29,7 +29,7 @@
#include "sensors_mpu9250_lps25h.h"
#include <math.h>
-#include "empty_math.h"
+// #include "empty_math.h"
#include <stm32f4xx.h>
#include "lps25h.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usb.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usb.c
index 1f300a9b313399342b02d2310aa664c997ae7fa6..6d649462bc409cff81dc5bd280def3afea68e9c9 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usb.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usb.c
@@ -160,7 +160,7 @@ static void resetUSB(void) {
;
}
- USB_OTG_FlushTxFifo(&USB_OTG_dev, IN_EP);
+ //USB_OTG_FlushTxFifo(&USB_OTG_dev, IN_EP);
rxStopped = true;
doingTransfer = false;
@@ -168,48 +168,48 @@ static void resetUSB(void) {
static uint8_t usbd_cf_Setup(void *pdev , USB_SETUP_REQ *req)
{
- command = req->wIndex;
- if (command == 0x01) {
- crtpSetLink(usblinkGetLink());
-
- if (rxStopped && !xQueueIsQueueFullFromISR(usbDataRx)) {
- DCD_EP_PrepareRx(&USB_OTG_dev,
- OUT_EP,
- (uint8_t*)(inPacket.data),
- USB_RX_TX_PACKET_SIZE);
- rxStopped = false;
- }
- } else if(command == 0x02){
- //restart system and transition to DFU bootloader mode
- //enter bootloader specific to STM32f4xx
- enter_bootloader(0, 0x00000000);
- } else {
- crtpSetLink(radiolinkGetLink());
- }
+ // command = req->wIndex;
+ // if (command == 0x01) {
+ // crtpSetLink(usblinkGetLink());
+
+ // if (rxStopped && !xQueueIsQueueFullFromISR(usbDataRx)) {
+ // DCD_EP_PrepareRx(&USB_OTG_dev,
+ // OUT_EP,
+ // (uint8_t*)(inPacket.data),
+ // USB_RX_TX_PACKET_SIZE);
+ // rxStopped = false;
+ // }
+ // } else if(command == 0x02){
+ // //restart system and transition to DFU bootloader mode
+ // //enter bootloader specific to STM32f4xx
+ // enter_bootloader(0, 0x00000000);
+ // } else {
+ // crtpSetLink(radiolinkGetLink());
+ // }
return USBD_OK;
}
static uint8_t usbd_cf_Init (void *pdev,
uint8_t cfgidx)
{
- /* Open EP IN */
- DCD_EP_Open(pdev,
- IN_EP,
- USB_RX_TX_PACKET_SIZE,
- USB_OTG_EP_BULK);
-
- /* Open EP OUT */
- DCD_EP_Open(pdev,
- OUT_EP,
- USB_RX_TX_PACKET_SIZE,
- USB_OTG_EP_BULK);
-
- /* Prepare Out endpoint to receive next packet */
- DCD_EP_PrepareRx(pdev,
- OUT_EP,
- (uint8_t*)(inPacket.data),
- USB_RX_TX_PACKET_SIZE);
- rxStopped = false;
+ // /* Open EP IN */
+ // DCD_EP_Open(pdev,
+ // IN_EP,
+ // USB_RX_TX_PACKET_SIZE,
+ // USB_OTG_EP_BULK);
+
+ // /* Open EP OUT */
+ // DCD_EP_Open(pdev,
+ // OUT_EP,
+ // USB_RX_TX_PACKET_SIZE,
+ // USB_OTG_EP_BULK);
+
+ // /* Prepare Out endpoint to receive next packet */
+ // DCD_EP_PrepareRx(pdev,
+ // OUT_EP,
+ // (uint8_t*)(inPacket.data),
+ // USB_RX_TX_PACKET_SIZE);
+ // rxStopped = false;
return USBD_OK;
}
@@ -224,11 +224,11 @@ static uint8_t usbd_cf_Init (void *pdev,
static uint8_t usbd_cf_DeInit (void *pdev,
uint8_t cfgidx)
{
- /* Open EP IN */
- DCD_EP_Close(pdev, IN_EP);
+ // /* Open EP IN */
+ // DCD_EP_Close(pdev, IN_EP);
- /* Open EP OUT */
- DCD_EP_Close(pdev, OUT_EP);
+ // /* Open EP OUT */
+ // DCD_EP_Close(pdev, OUT_EP);
return USBD_OK;
}
@@ -242,40 +242,40 @@ static uint8_t usbd_cf_DeInit (void *pdev,
*/
static uint8_t usbd_cf_DataIn (void *pdev, uint8_t epnum)
{
- portBASE_TYPE xTaskWokenByReceive = pdFALSE;
+ // portBASE_TYPE xTaskWokenByReceive = pdFALSE;
- doingTransfer = false;
+ // doingTransfer = false;
- if (xQueueReceiveFromISR(usbDataTx, &outPacket, &xTaskWokenByReceive) == pdTRUE)
- {
- doingTransfer = true;
- DCD_EP_Tx (pdev,
- IN_EP,
- (uint8_t*)outPacket.data,
- outPacket.size);
- }
+ // if (xQueueReceiveFromISR(usbDataTx, &outPacket, &xTaskWokenByReceive) == pdTRUE)
+ // {
+ // doingTransfer = true;
+ // DCD_EP_Tx (pdev,
+ // IN_EP,
+ // (uint8_t*)outPacket.data,
+ // outPacket.size);
+ // }
- portYIELD_FROM_ISR(xTaskWokenByReceive);
+ // portYIELD_FROM_ISR(xTaskWokenByReceive);
return USBD_OK;
}
static uint8_t usbd_cf_SOF (void *pdev)
{
- portBASE_TYPE xTaskWokenByReceive = pdFALSE;
+ // portBASE_TYPE xTaskWokenByReceive = pdFALSE;
- if (!doingTransfer) {
- if (xQueueReceiveFromISR(usbDataTx, &outPacket, &xTaskWokenByReceive) == pdTRUE)
- {
- doingTransfer = true;
- DCD_EP_Tx (pdev,
- IN_EP,
- (uint8_t*)outPacket.data,
- outPacket.size);
- }
- }
+ // if (!doingTransfer) {
+ // if (xQueueReceiveFromISR(usbDataTx, &outPacket, &xTaskWokenByReceive) == pdTRUE)
+ // {
+ // doingTransfer = true;
+ // DCD_EP_Tx (pdev,
+ // IN_EP,
+ // (uint8_t*)outPacket.data,
+ // outPacket.size);
+ // }
+ // }
- portYIELD_FROM_ISR(xTaskWokenByReceive);
+ // portYIELD_FROM_ISR(xTaskWokenByReceive);
return USBD_OK;
}
@@ -289,28 +289,28 @@ static uint8_t usbd_cf_SOF (void *pdev)
*/
static uint8_t usbd_cf_DataOut (void *pdev, uint8_t epnum)
{
- uint8_t result;
- portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-
- /* Get the received data buffer and update the counter */
- inPacket.size = ((USB_OTG_CORE_HANDLE*)pdev)->dev.out_ep[epnum].xfer_count;
-
- if (xQueueSendFromISR(usbDataRx, &inPacket, &xHigherPriorityTaskWoken) == pdTRUE) {
- result = USBD_OK;
- } else {
- result = USBD_BUSY;
- }
-
- if (!xQueueIsQueueFullFromISR(usbDataRx)) {
- /* Prepare Out endpoint to receive next packet */
- DCD_EP_PrepareRx(pdev,
- OUT_EP,
- (uint8_t*)(inPacket.data),
- USB_RX_TX_PACKET_SIZE);
- rxStopped = false;
- } else {
- rxStopped = true;
- }
+ uint8_t result = USBD_OK;
+ // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+
+ // /* Get the received data buffer and update the counter */
+ // inPacket.size = ((USB_OTG_CORE_HANDLE*)pdev)->dev.out_ep[epnum].xfer_count;
+
+ // if (xQueueSendFromISR(usbDataRx, &inPacket, &xHigherPriorityTaskWoken) == pdTRUE) {
+ // result = USBD_OK;
+ // } else {
+ // result = USBD_BUSY;
+ // }
+
+ // if (!xQueueIsQueueFullFromISR(usbDataRx)) {
+ // /* Prepare Out endpoint to receive next packet */
+ // DCD_EP_PrepareRx(pdev,
+ // OUT_EP,
+ // (uint8_t*)(inPacket.data),
+ // USB_RX_TX_PACKET_SIZE);
+ // rxStopped = false;
+ // } else {
+ // rxStopped = true;
+ // }
return result;
}
@@ -424,20 +424,20 @@ bool usbTest(void)
bool usbGetDataBlocking(USBPacket *in)
{
- while (xQueueReceive(usbDataRx, in, portMAX_DELAY) != pdTRUE)
- ; // Don't return until we get some data on the USB
-
- // Disabling USB interrupt to make sure we can check and re-enable the endpoint
- // if it is not currently accepting data (ie. can happen if the RX queue was full)
- NVIC_DisableIRQ(OTG_FS_IRQn);
- if (rxStopped) {
- DCD_EP_PrepareRx(&USB_OTG_dev,
- OUT_EP,
- (uint8_t*)(inPacket.data),
- USB_RX_TX_PACKET_SIZE);
- rxStopped = false;
- }
- NVIC_EnableIRQ(OTG_FS_IRQn);
+ // while (xQueueReceive(usbDataRx, in, portMAX_DELAY) != pdTRUE)
+ // ; // Don't return until we get some data on the USB
+
+ // // Disabling USB interrupt to make sure we can check and re-enable the endpoint
+ // // if it is not currently accepting data (ie. can happen if the RX queue was full)
+ // NVIC_DisableIRQ(OTG_FS_IRQn);
+ // if (rxStopped) {
+ // DCD_EP_PrepareRx(&USB_OTG_dev,
+ // OUT_EP,
+ // (uint8_t*)(inPacket.data),
+ // USB_RX_TX_PACKET_SIZE);
+ // rxStopped = false;
+ // }
+ // NVIC_EnableIRQ(OTG_FS_IRQn);
return true;
}
@@ -449,5 +449,6 @@ bool usbSendData(uint32_t size, uint8_t* data)
outStage.size = size;
memcpy(outStage.data, data, size);
// Dont' block when sending
- return (xQueueSend(usbDataTx, &outStage, M2T(100)) == pdTRUE);
+ //return (xQueueSend(usbDataTx, &outStage, M2T(100)) == pdTRUE);
+ return true;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usbd_desc.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usbd_desc.c
index 42e681c5d8e040cd32cf2868b70c036f4a3e9a30..66599f35f153c26f696cde6a3efffdc7862c8151 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usbd_desc.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usbd_desc.c
@@ -221,15 +221,16 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length)
{
- if(speed == 0)
- {
- USBD_GetString ((uint8_t *)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length);
- }
- else
- {
- USBD_GetString ((uint8_t *)USBD_PRODUCT_FS_STRING, USBD_StrDesc, length);
- }
- return USBD_StrDesc;
+ // if(speed == 0)
+ // {
+ // USBD_GetString ((uint8_t *)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length);
+ // }
+ // else
+ // {
+ // USBD_GetString ((uint8_t *)USBD_PRODUCT_FS_STRING, USBD_StrDesc, length);
+ // }
+ // return USBD_StrDesc;
+ return 0;
}
/**
@@ -241,8 +242,9 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length)
*/
uint8_t * USBD_USR_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length)
{
- USBD_GetString ((uint8_t *)USBD_MANUFACTURER_STRING, USBD_StrDesc, length);
- return USBD_StrDesc;
+ //USBD_GetString ((uint8_t *)USBD_MANUFACTURER_STRING, USBD_StrDesc, length);
+ //return USBD_StrDesc;
+ return 0;
}
static const char bin2hex[] = {'0', '1', '2', '3', '4', '5', '6', '7',
@@ -272,7 +274,8 @@ uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length)
// USBD_GetString ((uint8_t *)serial_nr, USBD_StrDesc, length);
- return USBD_StrDesc;
+ //return USBD_StrDesc;
+ return 0;
}
/**
@@ -284,15 +287,16 @@ uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length)
*/
uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length)
{
- if(speed == USB_OTG_SPEED_HIGH)
- {
- USBD_GetString ((uint8_t *)USBD_CONFIGURATION_HS_STRING, USBD_StrDesc, length);
- }
- else
- {
- USBD_GetString ((uint8_t *)USBD_CONFIGURATION_FS_STRING, USBD_StrDesc, length);
- }
- return USBD_StrDesc;
+ // if(speed == USB_OTG_SPEED_HIGH)
+ // {
+ // USBD_GetString ((uint8_t *)USBD_CONFIGURATION_HS_STRING, USBD_StrDesc, length);
+ // }
+ // else
+ // {
+ // USBD_GetString ((uint8_t *)USBD_CONFIGURATION_FS_STRING, USBD_StrDesc, length);
+ // }
+ // return USBD_StrDesc;
+ return 0;
}
@@ -305,15 +309,16 @@ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length)
*/
uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length)
{
- if(speed == 0)
- {
- USBD_GetString ((uint8_t *)USBD_INTERFACE_HS_STRING, USBD_StrDesc, length);
- }
- else
- {
- USBD_GetString ((uint8_t *)USBD_INTERFACE_FS_STRING, USBD_StrDesc, length);
- }
- return USBD_StrDesc;
+ // if(speed == 0)
+ // {
+ // USBD_GetString ((uint8_t *)USBD_INTERFACE_HS_STRING, USBD_StrDesc, length);
+ // }
+ // else
+ // {
+ // USBD_GetString ((uint8_t *)USBD_INTERFACE_FS_STRING, USBD_StrDesc, length);
+ // }
+ // return USBD_StrDesc;
+ return 0;
}
/**
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usblink.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usblink.c
index 225edc6ad14b0bd167b6814109abe01a96c9838f..fb1834867eb7d1a27ca2a9a00fce345f80edcd99 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usblink.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/usblink.c
@@ -84,11 +84,11 @@ static void usblinkTask(void *param)
static int usblinkReceivePacket(CRTPPacket *p)
{
- if (xQueueReceive(crtpPacketDelivery, p, M2T(100)) == pdTRUE)
- {
- ledseqRun(&seq_linkUp);
- return 0;
- }
+ // if (xQueueReceive(crtpPacketDelivery, p, M2T(100)) == pdTRUE)
+ // {
+ // ledseqRun(&seq_linkUp);
+ // return 0;
+ // }
return -1;
}
@@ -130,10 +130,10 @@ void usblinkInit()
// Initialize the USB peripheral
usbInit();
- crtpPacketDelivery = STATIC_MEM_QUEUE_CREATE(crtpPacketDelivery);
+ //crtpPacketDelivery = STATIC_MEM_QUEUE_CREATE(crtpPacketDelivery);
DEBUG_QUEUE_MONITOR_REGISTER(crtpPacketDelivery);
- STATIC_MEM_TASK_CREATE(usblinkTask, usblinkTask, USBLINK_TASK_NAME, NULL, USBLINK_TASK_PRI);
+ //STATIC_MEM_TASK_CREATE(usblinkTask, usblinkTask, USBLINK_TASK_NAME, NULL, USBLINK_TASK_PRI);
isInit = true;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/math3d.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/math3d.h
index c3adb7681b6dfeb0caf6464c1735424336300793..3962908df76de0b7398549c04aa66f0bf9c234ae 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/math3d.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/math3d.h
@@ -26,7 +26,7 @@ SOFTWARE.
*/
#include <math.h>
-#include "empty_math.h"
+// #include "empty_math.h"
#include <stdbool.h>
#include <stddef.h>
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/app_handler.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/app_handler.c
index 389402ecf584c0d21a8195cf8ef121b0c89f67df..aae3053eb1bfbc3efce6f62fc3fb76222d4a6166 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/app_handler.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/app_handler.c
@@ -33,7 +33,7 @@
#include "static_mem.h"
#include "app.h"
-#include "empty_math.h"
+// #include "empty_math.h"
#ifndef APP_STACKSIZE
#define APP_STACKSIZE 300
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/collision_avoidance.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/collision_avoidance.c
index d618e519804bc52c31d2cf6cb1d394e33bc4b5d3..391e9a184e44a5486f9540df6aa68cebc9e3317e 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/collision_avoidance.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/collision_avoidance.c
@@ -40,12 +40,12 @@
// math operations.
static struct vec vec2svec(struct vec3_s v)
{
- return mkvec(v.x, v.y, v.z);
+ // return mkvec(v.x, v.y, v.z);
}
static struct vec3_s svec2vec(struct vec v)
{
- struct vec3_s vv = { .x = v.x, .y = v.y, .z = v.z, };
- return vv;
+ // struct vec3_s vv = { .x = v.x, .y = v.y, .z = v.z, };
+ // return vv;
}
// Computes a new goal position inside our buffered Voronoi cell.
@@ -74,24 +74,24 @@ static struct vec sidestepGoal(
bool modifyIfInside,
float const A[], float const B[], float projectionWorkspace[], int nRows)
{
- float const rayScale = rayintersectpolytope(vzero(), goal, A, B, nRows, NULL);
- if (rayScale >= 1.0f && !modifyIfInside) {
- return goal;
- }
- float const distance = vmag(goal);
- float const distFromWall = rayScale * distance;
- if (distFromWall <= params->sidestepThreshold) {
- struct vec sidestepDir = vcross(goal, mkvec(0.0f, 0.0f, 1.0f));
- float const sidestepAmount = fsqr(1.0f - distFromWall / params->sidestepThreshold);
- goal = vadd(goal, vscl(sidestepAmount, sidestepDir));
- }
- // Otherwise no sidestep, but still project
- return vprojectpolytope(
- goal,
- A, B, projectionWorkspace, nRows,
- params->voronoiProjectionTolerance,
- params->voronoiProjectionMaxIters
- );
+ // float const rayScale = rayintersectpolytope(vzero(), goal, A, B, nRows, NULL);
+ // if (rayScale >= 1.0f && !modifyIfInside) {
+ // return goal;
+ // }
+ // float const distance = vmag(goal);
+ // float const distFromWall = rayScale * distance;
+ // if (distFromWall <= params->sidestepThreshold) {
+ // struct vec sidestepDir = vcross(goal, mkvec(0.0f, 0.0f, 1.0f));
+ // float const sidestepAmount = fsqr(1.0f - distFromWall / params->sidestepThreshold);
+ // goal = vadd(goal, vscl(sidestepAmount, sidestepDir));
+ // }
+ // // Otherwise no sidestep, but still project
+ // return vprojectpolytope(
+ // goal,
+ // A, B, projectionWorkspace, nRows,
+ // params->voronoiProjectionTolerance,
+ // params->voronoiProjectionMaxIters
+ // );
}
void collisionAvoidanceUpdateSetpointCore(
@@ -269,22 +269,22 @@ void collisionAvoidanceUpdateSetpointCore(
#include "log.h"
-static uint8_t collisionAvoidanceEnable = 0;
+// static uint8_t collisionAvoidanceEnable = 0;
static collision_avoidance_params_t params = {
- .ellipsoidRadii = { .x = 0.3, .y = 0.3, .z = 0.9 },
- .bboxMin = { .x = -FLT_MAX, .y = -FLT_MAX, .z = -FLT_MAX },
- .bboxMax = { .x = FLT_MAX, .y = FLT_MAX, .z = FLT_MAX },
- .horizonSecs = 1.0f,
- .maxSpeed = 0.5f, // Fairly conservative.
- .sidestepThreshold = 0.25f,
- .maxPeerLocAgeMillis = 5000, // Probably longer than desired in most applications.
- .voronoiProjectionTolerance = 1e-5,
- .voronoiProjectionMaxIters = 100,
+ // .ellipsoidRadii = { .x = 0.3, .y = 0.3, .z = 0.9 },
+ // .bboxMin = { .x = -FLT_MAX, .y = -FLT_MAX, .z = -FLT_MAX },
+ // .bboxMax = { .x = FLT_MAX, .y = FLT_MAX, .z = FLT_MAX },
+ // .horizonSecs = 1.0f,
+ // .maxSpeed = 0.5f, // Fairly conservative.
+ // .sidestepThreshold = 0.25f,
+ // .maxPeerLocAgeMillis = 5000, // Probably longer than desired in most applications.
+ // .voronoiProjectionTolerance = 1e-5,
+ // .voronoiProjectionMaxIters = 100,
};
static collision_avoidance_state_t collisionState = {
- .lastFeasibleSetPosition = { .x = NAN, .y = NAN, .z = NAN },
+ // .lastFeasibleSetPosition = { .x = NAN, .y = NAN, .z = NAN },
};
void collisionAvoidanceInit()
@@ -293,58 +293,58 @@ void collisionAvoidanceInit()
bool collisionAvoidanceTest()
{
- return true;
+ // return true;
}
// Each face of the Voronoi cell is defined by a linear inequality a^T x <= b.
// The algorithm for projecting a point into a convex polytope requires 3 more
// floats of working space per face. The six extra faces come from the overall
// flight area bounding box.
-#define MAX_CELL_ROWS (PEER_LOCALIZATION_MAX_NEIGHBORS + 6)
-static float workspace[7 * MAX_CELL_ROWS];
+// #define MAX_CELL_ROWS (PEER_LOCALIZATION_MAX_NEIGHBORS + 6)
+// static float workspace[7 * MAX_CELL_ROWS];
// Latency counter for logging.
-static uint32_t latency = 0;
+// static uint32_t latency = 0;
void collisionAvoidanceUpdateSetpoint(
setpoint_t *setpoint, sensorData_t const *sensorData, state_t const *state, uint32_t tick)
{
- if (!collisionAvoidanceEnable) {
- return;
- }
+ // if (!collisionAvoidanceEnable) {
+ // return;
+ // }
- TickType_t const time = xTaskGetTickCount();
- bool doAgeFilter = params.maxPeerLocAgeMillis >= 0;
+ // TickType_t const time = xTaskGetTickCount();
+ // bool doAgeFilter = params.maxPeerLocAgeMillis >= 0;
- // Counts the actual number of neighbors after we filter stale measurements.
- int nOthers = 0;
+ // // Counts the actual number of neighbors after we filter stale measurements.
+ // int nOthers = 0;
- for (int i = 0; i < PEER_LOCALIZATION_MAX_NEIGHBORS; ++i) {
+ // for (int i = 0; i < PEER_LOCALIZATION_MAX_NEIGHBORS; ++i) {
- peerLocalizationOtherPosition_t const *otherPos = peerLocalizationGetPositionByIdx(i);
+ // peerLocalizationOtherPosition_t const *otherPos = peerLocalizationGetPositionByIdx(i);
- if (otherPos == NULL || otherPos->id == 0) {
- continue;
- }
+ // if (otherPos == NULL || otherPos->id == 0) {
+ // continue;
+ // }
- if (doAgeFilter && (time - otherPos->pos.timestamp > params.maxPeerLocAgeMillis)) {
- continue;
- }
+ // if (doAgeFilter && (time - otherPos->pos.timestamp > params.maxPeerLocAgeMillis)) {
+ // continue;
+ // }
- workspace[3 * nOthers + 0] = otherPos->pos.x;
- workspace[3 * nOthers + 1] = otherPos->pos.y;
- workspace[3 * nOthers + 2] = otherPos->pos.z;
- ++nOthers;
- }
+ // workspace[3 * nOthers + 0] = otherPos->pos.x;
+ // workspace[3 * nOthers + 1] = otherPos->pos.y;
+ // workspace[3 * nOthers + 2] = otherPos->pos.z;
+ // ++nOthers;
+ // }
- collisionAvoidanceUpdateSetpointCore(¶ms, &collisionState, nOthers, workspace, workspace, setpoint, sensorData, state);
+ // collisionAvoidanceUpdateSetpointCore(¶ms, &collisionState, nOthers, workspace, workspace, setpoint, sensorData, state);
- latency = xTaskGetTickCount() - time;
+ // latency = xTaskGetTickCount() - time;
}
-LOG_GROUP_START(colAv)
- LOG_ADD(LOG_UINT32, latency, &latency)
-LOG_GROUP_STOP(colAv)
+// LOG_GROUP_START(colAv)
+// LOG_ADD(LOG_UINT32, latency, &latency)
+// LOG_GROUP_STOP(colAv)
/**
@@ -396,7 +396,7 @@ PARAM_GROUP_START(colAv)
*
* Used to enable or disable the collision avoidance module.
*/
- PARAM_ADD_CORE(PARAM_UINT8, enable, &collisionAvoidanceEnable)
+ //PARAM_ADD_CORE(PARAM_UINT8, enable, &collisionAvoidanceEnable)
/**
* @brief The x radius of the ellipsoid collision volume
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/commander.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/commander.c
index 49fb2dcb6cd7144e58dcee3bc433d5af5fab4b5e..4cd861d18075c2cde5ed7445f1a2051cd9a8d0bd 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/commander.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/commander.c
@@ -55,37 +55,41 @@ STATIC_MEM_QUEUE_ALLOC(priorityQueue, 1, sizeof(int));
/* Public functions */
void commanderInit(void)
{
- setpointQueue = STATIC_MEM_QUEUE_CREATE(setpointQueue);
- ASSERT(setpointQueue);
- xQueueSend(setpointQueue, &nullSetpoint, 0);
+ //COMMENTED FIRMWARE
+
+ // setpointQueue = STATIC_MEM_QUEUE_CREATE(setpointQueue);
+ // ASSERT(setpointQueue);
+ // xQueueSend(setpointQueue, &nullSetpoint, 0);
- priorityQueue = STATIC_MEM_QUEUE_CREATE(priorityQueue);
- ASSERT(priorityQueue);
- xQueueSend(priorityQueue, &priorityDisable, 0);
+ // priorityQueue = STATIC_MEM_QUEUE_CREATE(priorityQueue);
+ // ASSERT(priorityQueue);
+ // xQueueSend(priorityQueue, &priorityDisable, 0);
- crtpCommanderInit();
- crtpCommanderHighLevelInit();
- lastUpdate = xTaskGetTickCount();
+ // crtpCommanderInit();
+ // crtpCommanderHighLevelInit();
+ // lastUpdate = xTaskGetTickCount();
- isInit = true;
+ // isInit = true;
}
void commanderSetSetpoint(setpoint_t *setpoint, int priority)
{
- int currentPriority;
-
- const BaseType_t peekResult = xQueuePeek(priorityQueue, ¤tPriority, 0);
- ASSERT(peekResult == pdTRUE);
-
- if (priority >= currentPriority) {
- setpoint->timestamp = xTaskGetTickCount();
- // This is a potential race but without effect on functionality
- xQueueOverwrite(setpointQueue, setpoint);
- xQueueOverwrite(priorityQueue, &priority);
- // Send the high-level planner to idle so it will forget its current state
- // and start over if we switch from low-level to high-level in the future.
- crtpCommanderHighLevelStop();
- }
+ //COMMENTED FIRMWARE
+
+ // int currentPriority;
+
+ // const BaseType_t peekResult = xQueuePeek(priorityQueue, ¤tPriority, 0);
+ // ASSERT(peekResult == pdTRUE);
+
+ // if (priority >= currentPriority) {
+ // setpoint->timestamp = xTaskGetTickCount();
+ // // This is a potential race but without effect on functionality
+ // xQueueOverwrite(setpointQueue, setpoint);
+ // xQueueOverwrite(priorityQueue, &priority);
+ // // Send the high-level planner to idle so it will forget its current state
+ // // and start over if we switch from low-level to high-level in the future.
+ // crtpCommanderHighLevelStop();
+ // }
}
void commanderNotifySetpointsStop(int remainValidMillisecs)
@@ -96,46 +100,48 @@ void commanderNotifySetpointsStop(int remainValidMillisecs)
// COMMANDER_WDT_TIMEOUT_SHUTDOWN - M2T(remainValidMillisecs),
// currentTime
// );
- int timeSetback = MIN(
- COMMANDER_WDT_TIMEOUT_SHUTDOWN - remainValidMillisecs,
- currentTime
- );
- xQueuePeek(setpointQueue, &tempSetpoint, 0);
- tempSetpoint.timestamp = currentTime - timeSetback;
- xQueueOverwrite(setpointQueue, &tempSetpoint);
- crtpCommanderHighLevelTellState(&lastState);
+ // int timeSetback = MIN(
+ // COMMANDER_WDT_TIMEOUT_SHUTDOWN - remainValidMillisecs,
+ // currentTime
+ // );
+ // xQueuePeek(setpointQueue, &tempSetpoint, 0);
+ // tempSetpoint.timestamp = currentTime - timeSetback;
+ // xQueueOverwrite(setpointQueue, &tempSetpoint);
+ // crtpCommanderHighLevelTellState(&lastState);
}
void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state)
{
- xQueuePeek(setpointQueue, setpoint, 0);
- lastUpdate = setpoint->timestamp;
- uint32_t currentTime = xTaskGetTickCount();
+ //COMMENTED FIRMWARE
- if ((currentTime - setpoint->timestamp) > COMMANDER_WDT_TIMEOUT_SHUTDOWN) {
- if (enableHighLevel) {
- crtpCommanderHighLevelGetSetpoint(setpoint, state);
- }
- if (!enableHighLevel || crtpCommanderHighLevelIsStopped()) {
- memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint));
- }
- } else if ((currentTime - setpoint->timestamp) > COMMANDER_WDT_TIMEOUT_STABILIZE) {
- xQueueOverwrite(priorityQueue, &priorityDisable);
- // Leveling ...
- setpoint->mode.x = modeDisable;
- setpoint->mode.y = modeDisable;
- setpoint->mode.roll = modeAbs;
- setpoint->mode.pitch = modeAbs;
- setpoint->mode.yaw = modeVelocity;
- setpoint->attitude.roll = 0;
- setpoint->attitude.pitch = 0;
- setpoint->attitudeRate.yaw = 0;
+ // xQueuePeek(setpointQueue, setpoint, 0);
+ // lastUpdate = setpoint->timestamp;
+ // uint32_t currentTime = xTaskGetTickCount();
+
+ // if ((currentTime - setpoint->timestamp) > COMMANDER_WDT_TIMEOUT_SHUTDOWN) {
+ // if (enableHighLevel) {
+ // crtpCommanderHighLevelGetSetpoint(setpoint, state);
+ // }
+ // if (!enableHighLevel || crtpCommanderHighLevelIsStopped()) {
+ // memcpy(setpoint, &nullSetpoint, sizeof(nullSetpoint));
+ // }
+ // } else if ((currentTime - setpoint->timestamp) > COMMANDER_WDT_TIMEOUT_STABILIZE) {
+ // xQueueOverwrite(priorityQueue, &priorityDisable);
+ // // Leveling ...
+ // setpoint->mode.x = modeDisable;
+ // setpoint->mode.y = modeDisable;
+ // setpoint->mode.roll = modeAbs;
+ // setpoint->mode.pitch = modeAbs;
+ // setpoint->mode.yaw = modeVelocity;
+ // setpoint->attitude.roll = 0;
+ // setpoint->attitude.pitch = 0;
+ // setpoint->attitudeRate.yaw = 0;
// Keep Z as it is
- }
+ // }
// This copying is not strictly necessary because stabilizer.c already keeps
// a static state_t containing the most recent state estimate. However, it is
// not accessible by the public interface.
- lastState = *state;
+ // lastState = *state;
}
bool commanderTest(void)
@@ -145,15 +151,16 @@ bool commanderTest(void)
uint32_t commanderGetInactivityTime(void)
{
- return xTaskGetTickCount() - lastUpdate;
+ // return xTaskGetTickCount() - lastUpdate;
+ return 1;
}
int commanderGetActivePriority(void)
{
int priority;
- const BaseType_t peekResult = xQueuePeek(priorityQueue, &priority, 0);
- ASSERT(peekResult == pdTRUE);
+ // const BaseType_t peekResult = xQueuePeek(priorityQueue, &priority, 0);
+ // ASSERT(peekResult == pdTRUE);
return priority;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_pid.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_pid.c
index 5a28d464772a6431c624b5239b855c4ba5875c11..b3442097752a33edf861ba2762340ab33caf1142 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_pid.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_pid.c
@@ -9,7 +9,7 @@
#include "log.h"
#include "param.h"
-#include "math3d.h"
+// #include "math3d.h"
#define ATTITUDE_UPDATE_DT (float)(1.0f/ATTITUDE_RATE)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp.c
index 04ac7a3ef8ca4ce3f4e4698f9e4399ecc99e4ef8..090f7718da51a71c88000cd1eee070c6b6ea6002 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp.c
@@ -86,197 +86,202 @@ STATIC_MEM_TASK_ALLOC_STACK_NO_DMA_CCM_SAFE(crtpRxTask, CRTP_RX_TASK_STACKSIZE);
void crtpInit(void)
{
- if(isInit)
- return;
+ // if(isInit)
+ // return;
- txQueue = xQueueCreate(CRTP_TX_QUEUE_SIZE, sizeof(CRTPPacket));
- DEBUG_QUEUE_MONITOR_REGISTER(txQueue);
+ // txQueue = xQueueCreate(CRTP_TX_QUEUE_SIZE, sizeof(CRTPPacket));
+ // DEBUG_QUEUE_MONITOR_REGISTER(txQueue);
- STATIC_MEM_TASK_CREATE(crtpTxTask, crtpTxTask, CRTP_TX_TASK_NAME, NULL, CRTP_TX_TASK_PRI);
- STATIC_MEM_TASK_CREATE(crtpRxTask, crtpRxTask, CRTP_RX_TASK_NAME, NULL, CRTP_RX_TASK_PRI);
+ // STATIC_MEM_TASK_CREATE(crtpTxTask, crtpTxTask, CRTP_TX_TASK_NAME, NULL, CRTP_TX_TASK_PRI);
+ // STATIC_MEM_TASK_CREATE(crtpRxTask, crtpRxTask, CRTP_RX_TASK_NAME, NULL, CRTP_RX_TASK_PRI);
- isInit = true;
+ // isInit = true;
}
bool crtpTest(void)
{
- return isInit;
+ //return isInit;
+ return -1;
}
void crtpInitTaskQueue(CRTPPort portId)
{
- ASSERT(queues[portId] == NULL);
+ // ASSERT(queues[portId] == NULL);
- queues[portId] = xQueueCreate(CRTP_RX_QUEUE_SIZE, sizeof(CRTPPacket));
- DEBUG_QUEUE_MONITOR_REGISTER(queues[portId]);
+ // queues[portId] = xQueueCreate(CRTP_RX_QUEUE_SIZE, sizeof(CRTPPacket));
+ // DEBUG_QUEUE_MONITOR_REGISTER(queues[portId]);
}
int crtpReceivePacket(CRTPPort portId, CRTPPacket *p)
{
- ASSERT(queues[portId]);
- ASSERT(p);
+ // ASSERT(queues[portId]);
+ // ASSERT(p);
- return xQueueReceive(queues[portId], p, 0);
+ // return xQueueReceive(queues[portId], p, 0);
}
int crtpReceivePacketBlock(CRTPPort portId, CRTPPacket *p)
{
- ASSERT(queues[portId]);
- ASSERT(p);
+ // ASSERT(queues[portId]);
+ // ASSERT(p);
- return xQueueReceive(queues[portId], p, portMAX_DELAY);
+ // return xQueueReceive(queues[portId], p, portMAX_DELAY);
+ return -1;
}
int crtpReceivePacketWait(CRTPPort portId, CRTPPacket *p, int wait)
{
- ASSERT(queues[portId]);
- ASSERT(p);
+ // ASSERT(queues[portId]);
+ // ASSERT(p);
- return xQueueReceive(queues[portId], p, M2T(wait));
+ // //return xQueueReceive(queues[portId], p, M2T(wait));
+ return (wait);
}
int crtpGetFreeTxQueuePackets(void)
{
- return (CRTP_TX_QUEUE_SIZE - uxQueueMessagesWaiting(txQueue));
+ // return (CRTP_TX_QUEUE_SIZE - uxQueueMessagesWaiting(txQueue));
}
void crtpTxTask(void *param)
{
- CRTPPacket p;
-
- while (true)
- {
- if (link != &nopLink)
- {
- if (xQueueReceive(txQueue, &p, portMAX_DELAY) == pdTRUE)
- {
- // Keep testing, if the link changes to USB it will go though
- while (link->sendPacket(&p) == false)
- {
- // Relaxation time
- vTaskDelay(M2T(10));
- }
- stats.txCount++;
- updateStats();
- }
- }
- else
- {
- vTaskDelay(M2T(10));
- }
- }
+ // CRTPPacket p;
+
+ // while (true)
+ // {
+ // if (link != &nopLink)
+ // {
+ // if (xQueueReceive(txQueue, &p, portMAX_DELAY) == pdTRUE)
+ // {
+ // // Keep testing, if the link changes to USB it will go though
+ // while (link->sendPacket(&p) == false)
+ // {
+ // // Relaxation time
+ // // vTaskDelay(M2T(10));
+ // }
+ // stats.txCount++;
+ // updateStats();
+ // }
+ // }
+ // else
+ // {
+ // // vTaskDelay(M2T(10));
+ // }
+ // }
}
void crtpRxTask(void *param)
{
- CRTPPacket p;
-
- while (true)
- {
- if (link != &nopLink)
- {
- if (!link->receivePacket(&p))
- {
- if (queues[p.port])
- {
- // Block, since we should never drop a packet
- xQueueSend(queues[p.port], &p, portMAX_DELAY);
- }
-
- if (callbacks[p.port])
- {
- callbacks[p.port](&p);
- }
-
- stats.rxCount++;
- updateStats();
- }
- }
- else
- {
- vTaskDelay(M2T(10));
- }
- }
+ // CRTPPacket p;
+
+ // while (true)
+ // {
+ // if (link != &nopLink)
+ // {
+ // if (!link->receivePacket(&p))
+ // {
+ // if (queues[p.port])
+ // {
+ // // Block, since we should never drop a packet
+ // xQueueSend(queues[p.port], &p, portMAX_DELAY);
+ // }
+
+ // if (callbacks[p.port])
+ // {
+ // callbacks[p.port](&p);
+ // }
+
+ // stats.rxCount++;
+ // updateStats();
+ // }
+ // }
+ // else
+ // {
+ // // vTaskDelay(M2T(10));
+ // }
+ // }
}
void crtpRegisterPortCB(int port, CrtpCallback cb)
{
- if (port>CRTP_NBR_OF_PORTS)
- return;
+ // if (port>CRTP_NBR_OF_PORTS)
+ // return;
- callbacks[port] = cb;
+ // callbacks[port] = cb;
}
int crtpSendPacket(CRTPPacket *p)
{
- ASSERT(p);
- ASSERT(p->size <= CRTP_MAX_DATA_SIZE);
+ // ASSERT(p);
+ // ASSERT(p->size <= CRTP_MAX_DATA_SIZE);
- return xQueueSend(txQueue, p, 0);
+ // return xQueueSend(txQueue, p, 0);
+ return -1;
}
int crtpSendPacketBlock(CRTPPacket *p)
{
- ASSERT(p);
- ASSERT(p->size <= CRTP_MAX_DATA_SIZE);
+ // ASSERT(p);
+ // ASSERT(p->size <= CRTP_MAX_DATA_SIZE);
- return xQueueSend(txQueue, p, portMAX_DELAY);
+ // return xQueueSend(txQueue, p, portMAX_DELAY);
+ return -1;
}
int crtpReset(void)
{
- xQueueReset(txQueue);
- if (link->reset) {
- link->reset();
- }
+ // xQueueReset(txQueue);
+ // if (link->reset) {
+ // link->reset();
+ // }
return 0;
}
bool crtpIsConnected(void)
{
- if (link->isConnected)
- return link->isConnected();
+ // if (link->isConnected)
+ // return link->isConnected();
return true;
}
void crtpSetLink(struct crtpLinkOperations * lk)
{
- if(link)
- link->setEnable(false);
-
- if (lk)
- link = lk;
- else
- link = &nopLink;
-
- link->setEnable(true);
-}
-
-static int nopFunc(void)
-{
- return ENETDOWN;
-}
-
-static void clearStats()
-{
- stats.rxCount = 0;
- stats.txCount = 0;
-}
-
-static void updateStats()
-{
- uint32_t now = xTaskGetTickCount();
- if (now > stats.nextStatisticsTime) {
- float interval = now - stats.previousStatisticsTime;
- stats.rxRate = (uint16_t)(1000.0f * stats.rxCount / interval);
- stats.txRate = (uint16_t)(1000.0f * stats.txCount / interval);
-
- clearStats();
- stats.previousStatisticsTime = now;
- stats.nextStatisticsTime = now + STATS_INTERVAL;
- }
+// if(link)
+// link->setEnable(false);
+
+// if (lk)
+// link = lk;
+// else
+// link = &nopLink;
+
+// link->setEnable(true);
+// }
+
+// static int nopFunc(void)
+// {
+// return ENETDOWN;
+// }
+
+// static void clearStats()
+// {
+// stats.rxCount = 0;
+// stats.txCount = 0;
+// }
+
+// static void updateStats()
+// {
+// uint32_t now = xTaskGetTickCount();
+// if (now > stats.nextStatisticsTime) {
+// float interval = now - stats.previousStatisticsTime;
+// stats.rxRate = (uint16_t)(1000.0f * stats.rxCount / interval);
+// stats.txRate = (uint16_t)(1000.0f * stats.txCount / interval);
+
+// clearStats();
+// stats.previousStatisticsTime = now;
+// stats.nextStatisticsTime = now + STATS_INTERVAL;
+// }
}
LOG_GROUP_START(crtp)
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 273db2e7f10e028309a0cc72048b75da7849b24b..59d59f74983fdca87fc87179e783f737e86b63aa 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
@@ -118,7 +118,7 @@ static const MemoryHandlerDef_t memDef = {
.write = handleMemWrite,
};
-STATIC_MEM_TASK_ALLOC(crtpCommanderHighLevelTask, CMD_HIGH_LEVEL_TASK_STACKSIZE);
+//STATIC_MEM_TASK_ALLOC(crtpCommanderHighLevelTask, CMD_HIGH_LEVEL_TASK_STACKSIZE);
// CRTP Packet definitions
@@ -224,8 +224,8 @@ struct data_start_trajectory {
// starts executing a specified trajectory
struct data_define_trajectory {
- uint8_t trajectoryId;
- struct trajectoryDescription description;
+ uint8_t trajectoryId;
+ struct trajectoryDescription description;
} __attribute__((packed));
// Private functions
@@ -246,579 +246,579 @@ static int define_trajectory(const struct data_define_trajectory* data);
// Helper functions
static struct vec state2vec(struct vec3_s v)
{
- return mkvec(v.x, v.y, v.z);
+ //return mkvec(v.x, v.y, v.z);
}
bool isInGroup(uint8_t g) {
- return g == ALL_GROUPS || (g & group_mask) != 0;
+ //return g == ALL_GROUPS || (g & group_mask) != 0;
}
void crtpCommanderHighLevelInit(void)
{
- if (isInit) {
- return;
- }
+ // if (isInit) {
+ // return;
+ // }
- memoryRegisterHandler(&memDef);
- plan_init(&planner);
+ // memoryRegisterHandler(&memDef);
+ // plan_init(&planner);
- //Start the trajectory task
- //COMMENTED FIRMWARE
- //STATIC_MEM_TASK_CREATE(crtpCommanderHighLevelTask, crtpCommanderHighLevelTask, CMD_HIGH_LEVEL_TASK_NAME, NULL, CMD_HIGH_LEVEL_TASK_PRI);
+ // //Start the trajectory task
+ // //COMMENTED FIRMWARE
+ // //STATIC_MEM_TASK_CREATE(crtpCommanderHighLevelTask, crtpCommanderHighLevelTask, CMD_HIGH_LEVEL_TASK_NAME, NULL, CMD_HIGH_LEVEL_TASK_PRI);
- //COMMENTED FIRMWARE
- //lockTraj = xSemaphoreCreateMutexStatic(&lockTrajBuffer);
+ // //COMMENTED FIRMWARE
+ // //lockTraj = xSemaphoreCreateMutexStatic(&lockTrajBuffer);
- pos = vzero();
- vel = vzero();
- yaw = 0;
+ // pos = vzero();
+ // vel = vzero();
+ // yaw = 0;
- isInit = true;
+ // isInit = true;
}
bool crtpCommanderHighLevelIsStopped()
{
- return plan_is_stopped(&planner);
+ //return plan_is_stopped(&planner);
}
void crtpCommanderHighLevelTellState(const state_t *state)
{
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- pos = state2vec(state->position);
- vel = state2vec(state->velocity);
- yaw = radians(state->attitude.yaw);
- xSemaphoreGive(lockTraj);
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // pos = state2vec(state->position);
+ // vel = state2vec(state->velocity);
+ // yaw = radians(state->attitude.yaw);
+ // xSemaphoreGive(lockTraj);
}
void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state)
{
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- float t = usecTimestamp() / 1e6;
- struct traj_eval ev = plan_current_goal(&planner, t);
- if (!is_traj_eval_valid(&ev)) {
- // programming error
- plan_stop(&planner);
- }
- xSemaphoreGive(lockTraj);
-
- // if we are on the ground, update the last setpoint with the current state estimate
- if (plan_is_stopped(&planner)) {
- pos = state2vec(state->position);
- vel = state2vec(state->velocity);
- yaw = radians(state->attitude.yaw);
- }
-
- if (is_traj_eval_valid(&ev)) {
- setpoint->position.x = ev.pos.x;
- setpoint->position.y = ev.pos.y;
- setpoint->position.z = ev.pos.z;
- setpoint->velocity.x = ev.vel.x;
- setpoint->velocity.y = ev.vel.y;
- setpoint->velocity.z = ev.vel.z;
- setpoint->attitude.yaw = degrees(ev.yaw);
- setpoint->attitudeRate.roll = degrees(ev.omega.x);
- setpoint->attitudeRate.pitch = degrees(ev.omega.y);
- setpoint->attitudeRate.yaw = degrees(ev.omega.z);
- setpoint->mode.x = modeAbs;
- setpoint->mode.y = modeAbs;
- setpoint->mode.z = modeAbs;
- setpoint->mode.roll = modeDisable;
- setpoint->mode.pitch = modeDisable;
- setpoint->mode.yaw = modeAbs;
- setpoint->mode.quat = modeDisable;
- setpoint->acceleration.x = ev.acc.x;
- setpoint->acceleration.y = ev.acc.y;
- setpoint->acceleration.z = ev.acc.z;
-
- // store the last setpoint
- pos = ev.pos;
- vel = ev.vel;
- yaw = ev.yaw;
- }
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // float t = usecTimestamp() / 1e6;
+ // struct traj_eval ev = plan_current_goal(&planner, t);
+ // if (!is_traj_eval_valid(&ev)) {
+ // // programming error
+ // plan_stop(&planner);
+ // }
+ // xSemaphoreGive(lockTraj);
+
+ // // if we are on the ground, update the last setpoint with the current state estimate
+ // if (plan_is_stopped(&planner)) {
+ // pos = state2vec(state->position);
+ // vel = state2vec(state->velocity);
+ // yaw = radians(state->attitude.yaw);
+ // }
+
+ // if (is_traj_eval_valid(&ev)) {
+ // setpoint->position.x = ev.pos.x;
+ // setpoint->position.y = ev.pos.y;
+ // setpoint->position.z = ev.pos.z;
+ // setpoint->velocity.x = ev.vel.x;
+ // setpoint->velocity.y = ev.vel.y;
+ // setpoint->velocity.z = ev.vel.z;
+ // setpoint->attitude.yaw = degrees(ev.yaw);
+ // setpoint->attitudeRate.roll = degrees(ev.omega.x);
+ // setpoint->attitudeRate.pitch = degrees(ev.omega.y);
+ // setpoint->attitudeRate.yaw = degrees(ev.omega.z);
+ // setpoint->mode.x = modeAbs;
+ // setpoint->mode.y = modeAbs;
+ // setpoint->mode.z = modeAbs;
+ // setpoint->mode.roll = modeDisable;
+ // setpoint->mode.pitch = modeDisable;
+ // setpoint->mode.yaw = modeAbs;
+ // setpoint->mode.quat = modeDisable;
+ // setpoint->acceleration.x = ev.acc.x;
+ // setpoint->acceleration.y = ev.acc.y;
+ // setpoint->acceleration.z = ev.acc.z;
+
+ // // store the last setpoint
+ // pos = ev.pos;
+ // vel = ev.vel;
+ // yaw = ev.yaw;
+ // }
}
static int handleCommand(const enum TrajectoryCommand_e command, const uint8_t* data)
{
- int ret = 0;
-
- switch(command)
- {
- case COMMAND_SET_GROUP_MASK:
- ret = set_group_mask((const struct data_set_group_mask*)data);
- break;
- case COMMAND_TAKEOFF:
- ret = takeoff((const struct data_takeoff*)data);
- break;
- case COMMAND_LAND:
- ret = land((const struct data_land*)data);
- break;
- case COMMAND_TAKEOFF_2:
- ret = takeoff2((const struct data_takeoff_2*)data);
- break;
- case COMMAND_LAND_2:
- ret = land2((const struct data_land_2*)data);
- break;
- case COMMAND_TAKEOFF_WITH_VELOCITY:
- ret = takeoff_with_velocity((const struct data_takeoff_with_velocity*)data);
- break;
- case COMMAND_LAND_WITH_VELOCITY:
- ret = land_with_velocity((const struct data_land_with_velocity*)data);
- break;
- case COMMAND_STOP:
- ret = stop((const struct data_stop*)data);
- break;
- case COMMAND_GO_TO:
- ret = go_to((const struct data_go_to*)data);
- break;
- case COMMAND_START_TRAJECTORY:
- ret = start_trajectory((const struct data_start_trajectory*)data);
- break;
- case COMMAND_DEFINE_TRAJECTORY:
- ret = define_trajectory((const struct data_define_trajectory*)data);
- break;
- default:
- ret = ENOEXEC;
- break;
- }
-
- return ret;
+ // int ret = 0;
+
+ // switch(command)
+ // {
+ // case COMMAND_SET_GROUP_MASK:
+ // ret = set_group_mask((const struct data_set_group_mask*)data);
+ // break;
+ // case COMMAND_TAKEOFF:
+ // ret = takeoff((const struct data_takeoff*)data);
+ // break;
+ // case COMMAND_LAND:
+ // ret = land((const struct data_land*)data);
+ // break;
+ // case COMMAND_TAKEOFF_2:
+ // ret = takeoff2((const struct data_takeoff_2*)data);
+ // break;
+ // case COMMAND_LAND_2:
+ // ret = land2((const struct data_land_2*)data);
+ // break;
+ // case COMMAND_TAKEOFF_WITH_VELOCITY:
+ // ret = takeoff_with_velocity((const struct data_takeoff_with_velocity*)data);
+ // break;
+ // case COMMAND_LAND_WITH_VELOCITY:
+ // ret = land_with_velocity((const struct data_land_with_velocity*)data);
+ // break;
+ // case COMMAND_STOP:
+ // ret = stop((const struct data_stop*)data);
+ // break;
+ // case COMMAND_GO_TO:
+ // ret = go_to((const struct data_go_to*)data);
+ // break;
+ // case COMMAND_START_TRAJECTORY:
+ // ret = start_trajectory((const struct data_start_trajectory*)data);
+ // break;
+ // case COMMAND_DEFINE_TRAJECTORY:
+ // ret = define_trajectory((const struct data_define_trajectory*)data);
+ // break;
+ // default:
+ // ret = ENOEXEC;
+ // break;
+ // }
+
+ // return ret;
}
void crtpCommanderHighLevelTask(void * prm)
{
- CRTPPacket p;
- crtpInitTaskQueue(CRTP_PORT_SETPOINT_HL);
+ // CRTPPacket p;
+ // crtpInitTaskQueue(CRTP_PORT_SETPOINT_HL);
- while(1) {
- crtpReceivePacketBlock(CRTP_PORT_SETPOINT_HL, &p);
+ // while(1) {
+ // crtpReceivePacketBlock(CRTP_PORT_SETPOINT_HL, &p);
- int ret = handleCommand(p.data[0], &p.data[1]);
+ // int ret = handleCommand(p.data[0], &p.data[1]);
- //answer
- p.data[3] = ret;
- p.size = 4;
- crtpSendPacketBlock(&p);
- }
+ // //answer
+ // p.data[3] = ret;
+ // p.size = 4;
+ // crtpSendPacketBlock(&p);
+ // }
}
int set_group_mask(const struct data_set_group_mask* data)
{
- group_mask = data->groupMask;
+ // group_mask = data->groupMask;
- return 0;
+ // return 0;
}
int takeoff(const struct data_takeoff* data)
{
- int result = 0;
- if (isInGroup(data->groupMask)) {
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- float t = usecTimestamp() / 1e6;
- result = plan_takeoff(&planner, pos, yaw, data->height, 0.0f, data->duration, t);
- xSemaphoreGive(lockTraj);
- }
- return result;
+ // int result = 0;
+ // if (isInGroup(data->groupMask)) {
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // float t = usecTimestamp() / 1e6;
+ // result = plan_takeoff(&planner, pos, yaw, data->height, 0.0f, data->duration, t);
+ // xSemaphoreGive(lockTraj);
+ // }
+ // return result;
}
int takeoff2(const struct data_takeoff_2* data)
{
- int result = 0;
- if (isInGroup(data->groupMask)) {
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- float t = usecTimestamp() / 1e6;
+ // int result = 0;
+ // if (isInGroup(data->groupMask)) {
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // float t = usecTimestamp() / 1e6;
- float hover_yaw = data->yaw;
- if (data->useCurrentYaw) {
- hover_yaw = yaw;
- }
+ // float hover_yaw = data->yaw;
+ // if (data->useCurrentYaw) {
+ // hover_yaw = yaw;
+ // }
- result = plan_takeoff(&planner, pos, yaw, data->height, hover_yaw, data->duration, t);
- xSemaphoreGive(lockTraj);
- }
- return result;
+ // result = plan_takeoff(&planner, pos, yaw, data->height, hover_yaw, data->duration, t);
+ // xSemaphoreGive(lockTraj);
+ // }
+ // return result;
}
int takeoff_with_velocity(const struct data_takeoff_with_velocity* data)
{
- int result = 0;
- if (isInGroup(data->groupMask)) {
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- float t = usecTimestamp() / 1e6;
+ // int result = 0;
+ // if (isInGroup(data->groupMask)) {
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // float t = usecTimestamp() / 1e6;
- float hover_yaw = data->yaw;
- if (data->useCurrentYaw) {
- hover_yaw = yaw;
- }
+ // float hover_yaw = data->yaw;
+ // if (data->useCurrentYaw) {
+ // hover_yaw = yaw;
+ // }
- float height = data->height;
- if (data->heightIsRelative) {
- height += pos.z;
- }
+ // float height = data->height;
+ // if (data->heightIsRelative) {
+ // height += pos.z;
+ // }
- float velocity = data->velocity > 0 ? data->velocity : defaultTakeoffVelocity;
- //COMMENTED FIRMWARE
- float duration = 1;//fabsf(height - pos.z) / velocity;
- result = plan_takeoff(&planner, pos, yaw, height, hover_yaw, duration, t);
- xSemaphoreGive(lockTraj);
- }
- return result;
+ // float velocity = data->velocity > 0 ? data->velocity : defaultTakeoffVelocity;
+ // //COMMENTED FIRMWARE
+ // float duration = 1;//fabsf(height - pos.z) / velocity;
+ // result = plan_takeoff(&planner, pos, yaw, height, hover_yaw, duration, t);
+ // xSemaphoreGive(lockTraj);
+ // }
+ //return result;
}
int land(const struct data_land* data)
{
- int result = 0;
- if (isInGroup(data->groupMask)) {
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- float t = usecTimestamp() / 1e6;
- result = plan_land(&planner, pos, yaw, data->height, 0.0f, data->duration, t);
- xSemaphoreGive(lockTraj);
- }
- return result;
+ // int result = 0;
+ // if (isInGroup(data->groupMask)) {
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // float t = usecTimestamp() / 1e6;
+ // result = plan_land(&planner, pos, yaw, data->height, 0.0f, data->duration, t);
+ // xSemaphoreGive(lockTraj);
+ // }
+ // return result;
}
int land2(const struct data_land_2* data)
{
- int result = 0;
- if (isInGroup(data->groupMask)) {
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- float t = usecTimestamp() / 1e6;
+ // int result = 0;
+ // if (isInGroup(data->groupMask)) {
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // float t = usecTimestamp() / 1e6;
- float hover_yaw = data->yaw;
- if (data->useCurrentYaw) {
- hover_yaw = yaw;
- }
+ // float hover_yaw = data->yaw;
+ // if (data->useCurrentYaw) {
+ // hover_yaw = yaw;
+ // }
- result = plan_land(&planner, pos, yaw, data->height, hover_yaw, data->duration, t);
- xSemaphoreGive(lockTraj);
- }
- return result;
+ // result = plan_land(&planner, pos, yaw, data->height, hover_yaw, data->duration, t);
+ // xSemaphoreGive(lockTraj);
+ // }
+ // return result;
}
int land_with_velocity(const struct data_land_with_velocity* data)
{
- int result = 0;
- if (isInGroup(data->groupMask)) {
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- float t = usecTimestamp() / 1e6;
+ // int result = 0;
+ // if (isInGroup(data->groupMask)) {
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // float t = usecTimestamp() / 1e6;
- float hover_yaw = data->yaw;
- if (data->useCurrentYaw) {
- hover_yaw = yaw;
- }
+ // float hover_yaw = data->yaw;
+ // if (data->useCurrentYaw) {
+ // hover_yaw = yaw;
+ // }
- float height = data->height;
- if (data->heightIsRelative) {
- height = pos.z - height;
- }
+ // float height = data->height;
+ // if (data->heightIsRelative) {
+ // height = pos.z - height;
+ // }
- float velocity = data->velocity > 0 ? data->velocity : defaultLandingVelocity;
- //COMMENTED FIRMWARE
- float duration = 1;//fabsf(height - pos.z) / velocity;
- result = plan_land(&planner, pos, yaw, height, hover_yaw, duration, t);
- xSemaphoreGive(lockTraj);
- }
- return result;
+ // float velocity = data->velocity > 0 ? data->velocity : defaultLandingVelocity;
+ // //COMMENTED FIRMWARE
+ // float duration = 1;//fabsf(height - pos.z) / velocity;
+ // result = plan_land(&planner, pos, yaw, height, hover_yaw, duration, t);
+ // xSemaphoreGive(lockTraj);
+ // }
+ // return result;
}
int stop(const struct data_stop* data)
{
- int result = 0;
- if (isInGroup(data->groupMask)) {
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- plan_stop(&planner);
- xSemaphoreGive(lockTraj);
- }
- return result;
+ // int result = 0;
+ // if (isInGroup(data->groupMask)) {
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // plan_stop(&planner);
+ // xSemaphoreGive(lockTraj);
+ // }
+ // return result;
}
int go_to(const struct data_go_to* data)
{
- static struct traj_eval ev = {
- // pos, vel, yaw will be filled before using
- .acc = {0.0f, 0.0f, 0.0f},
- .omega = {0.0f, 0.0f, 0.0f},
- };
-
- int result = 0;
- if (isInGroup(data->groupMask)) {
- struct vec hover_pos = mkvec(data->x, data->y, data->z);
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- float t = usecTimestamp() / 1e6;
- if (plan_is_stopped(&planner)) {
- ev.pos = pos;
- ev.vel = vel;
- ev.yaw = yaw;
- result = plan_go_to_from(&planner, &ev, data->relative, hover_pos, data->yaw, data->duration, t);
- }
- else {
- result = plan_go_to(&planner, data->relative, hover_pos, data->yaw, data->duration, t);
- }
- xSemaphoreGive(lockTraj);
- }
- return result;
+ // static struct traj_eval ev = {
+ // // pos, vel, yaw will be filled before using
+ // .acc = {0.0f, 0.0f, 0.0f},
+ // .omega = {0.0f, 0.0f, 0.0f},
+ // };
+
+ // int result = 0;
+ // if (isInGroup(data->groupMask)) {
+ // struct vec hover_pos = mkvec(data->x, data->y, data->z);
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // float t = usecTimestamp() / 1e6;
+ // if (plan_is_stopped(&planner)) {
+ // ev.pos = pos;
+ // ev.vel = vel;
+ // ev.yaw = yaw;
+ // result = plan_go_to_from(&planner, &ev, data->relative, hover_pos, data->yaw, data->duration, t);
+ // }
+ // else {
+ // result = plan_go_to(&planner, data->relative, hover_pos, data->yaw, data->duration, t);
+ // }
+ // xSemaphoreGive(lockTraj);
+ // }
+ // return result;
}
int start_trajectory(const struct data_start_trajectory* data)
{
- int result = 0;
- if (isInGroup(data->groupMask)) {
- if (data->trajectoryId < NUM_TRAJECTORY_DEFINITIONS) {
- struct trajectoryDescription* trajDesc = &trajectory_descriptions[data->trajectoryId];
- if ( trajDesc->trajectoryLocation == TRAJECTORY_LOCATION_MEM
- && trajDesc->trajectoryType == CRTP_CHL_TRAJECTORY_TYPE_POLY4D) {
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- float t = usecTimestamp() / 1e6;
- trajectory.t_begin = t;
- trajectory.timescale = data->timescale;
- trajectory.n_pieces = trajDesc->trajectoryIdentifier.mem.n_pieces;
- trajectory.pieces = (struct poly4d*)&trajectories_memory[trajDesc->trajectoryIdentifier.mem.offset];
- if (data->relative) {
- trajectory.shift = vzero();
- struct traj_eval traj_init;
- if (data->reversed) {
- traj_init = piecewise_eval_reversed(&trajectory, trajectory.t_begin);
- }
- else {
- traj_init = piecewise_eval(&trajectory, trajectory.t_begin);
- }
- struct vec shift_pos = vsub(pos, traj_init.pos);
- trajectory.shift = shift_pos;
- } else {
- trajectory.shift = vzero();
- }
- result = plan_start_trajectory(&planner, &trajectory, data->reversed);
- xSemaphoreGive(lockTraj);
- } else if (trajDesc->trajectoryLocation == TRAJECTORY_LOCATION_MEM
- && trajDesc->trajectoryType == CRTP_CHL_TRAJECTORY_TYPE_POLY4D_COMPRESSED) {
-
- if (data->timescale != 1 || data->reversed) {
- result = ENOEXEC;
- } else {
- xSemaphoreTake(lockTraj, portMAX_DELAY);
- float t = usecTimestamp() / 1e6;
- piecewise_compressed_load(
- &compressed_trajectory,
- &trajectories_memory[trajDesc->trajectoryIdentifier.mem.offset]
- );
- compressed_trajectory.t_begin = t;
- if (data->relative) {
- struct traj_eval traj_init = piecewise_compressed_eval(
- &compressed_trajectory, compressed_trajectory.t_begin
- );
- struct vec shift_pos = vsub(pos, traj_init.pos);
- compressed_trajectory.shift = shift_pos;
- } else {
- compressed_trajectory.shift = vzero();
- }
- result = plan_start_compressed_trajectory(&planner, &compressed_trajectory);
- xSemaphoreGive(lockTraj);
- }
-
- }
- }
- }
- return result;
+ // int result = 0;
+ // if (isInGroup(data->groupMask)) {
+ // if (data->trajectoryId < NUM_TRAJECTORY_DEFINITIONS) {
+ // struct trajectoryDescription* trajDesc = &trajectory_descriptions[data->trajectoryId];
+ // if ( trajDesc->trajectoryLocation == TRAJECTORY_LOCATION_MEM
+ // && trajDesc->trajectoryType == CRTP_CHL_TRAJECTORY_TYPE_POLY4D) {
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // float t = usecTimestamp() / 1e6;
+ // trajectory.t_begin = t;
+ // trajectory.timescale = data->timescale;
+ // trajectory.n_pieces = trajDesc->trajectoryIdentifier.mem.n_pieces;
+ // trajectory.pieces = (struct poly4d*)&trajectories_memory[trajDesc->trajectoryIdentifier.mem.offset];
+ // if (data->relative) {
+ // trajectory.shift = vzero();
+ // struct traj_eval traj_init;
+ // if (data->reversed) {
+ // traj_init = piecewise_eval_reversed(&trajectory, trajectory.t_begin);
+ // }
+ // else {
+ // traj_init = piecewise_eval(&trajectory, trajectory.t_begin);
+ // }
+ // struct vec shift_pos = vsub(pos, traj_init.pos);
+ // trajectory.shift = shift_pos;
+ // } else {
+ // trajectory.shift = vzero();
+ // }
+ // result = plan_start_trajectory(&planner, &trajectory, data->reversed);
+ // xSemaphoreGive(lockTraj);
+ // } else if (trajDesc->trajectoryLocation == TRAJECTORY_LOCATION_MEM
+ // && trajDesc->trajectoryType == CRTP_CHL_TRAJECTORY_TYPE_POLY4D_COMPRESSED) {
+
+ // if (data->timescale != 1 || data->reversed) {
+ // result = ENOEXEC;
+ // } else {
+ // xSemaphoreTake(lockTraj, portMAX_DELAY);
+ // float t = usecTimestamp() / 1e6;
+ // piecewise_compressed_load(
+ // &compressed_trajectory,
+ // &trajectories_memory[trajDesc->trajectoryIdentifier.mem.offset]
+ // );
+ // compressed_trajectory.t_begin = t;
+ // if (data->relative) {
+ // struct traj_eval traj_init = piecewise_compressed_eval(
+ // &compressed_trajectory, compressed_trajectory.t_begin
+ // );
+ // struct vec shift_pos = vsub(pos, traj_init.pos);
+ // compressed_trajectory.shift = shift_pos;
+ // } else {
+ // compressed_trajectory.shift = vzero();
+ // }
+ // result = plan_start_compressed_trajectory(&planner, &compressed_trajectory);
+ // xSemaphoreGive(lockTraj);
+ // }
+
+ // }
+ // }
+ // }
+ // return result;
}
int define_trajectory(const struct data_define_trajectory* data)
{
- if (data->trajectoryId >= NUM_TRAJECTORY_DEFINITIONS) {
- return ENOEXEC;
- }
- trajectory_descriptions[data->trajectoryId] = data->description;
- return 0;
+ // if (data->trajectoryId >= NUM_TRAJECTORY_DEFINITIONS) {
+ // return ENOEXEC;
+ // }
+ // trajectory_descriptions[data->trajectoryId] = data->description;
+ // return 0;
}
static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer) {
- return crtpCommanderHighLevelReadTrajectory(memAddr, readLen, buffer);
+ // return crtpCommanderHighLevelReadTrajectory(memAddr, readLen, buffer);
}
static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer) {
- return crtpCommanderHighLevelWriteTrajectory(memAddr, writeLen, buffer);
+ // return crtpCommanderHighLevelWriteTrajectory(memAddr, writeLen, buffer);
}
uint8_t* initCrtpPacket(CRTPPacket* packet, const enum TrajectoryCommand_e command)
{
- packet->port = CRTP_PORT_SETPOINT_HL;
- packet->data[0] = command;
- return &packet->data[1];
+ // packet->port = CRTP_PORT_SETPOINT_HL;
+ // packet->data[0] = command;
+ // return &packet->data[1];
}
int crtpCommanderHighLevelTakeoff(const float absoluteHeight_m, const float duration_s)
{
- struct data_takeoff_2 data =
- {
- .height = absoluteHeight_m,
- .duration = duration_s,
- .useCurrentYaw = true,
- .groupMask = ALL_GROUPS,
- };
+ // struct data_takeoff_2 data =
+ // {
+ // .height = absoluteHeight_m,
+ // .duration = duration_s,
+ // .useCurrentYaw = true,
+ // .groupMask = ALL_GROUPS,
+ // };
- return handleCommand(COMMAND_TAKEOFF_2, (const uint8_t*)&data);
+ // return handleCommand(COMMAND_TAKEOFF_2, (const uint8_t*)&data);
}
int crtpCommanderHighLevelTakeoffYaw(const float absoluteHeight_m, const float duration_s, const float yaw)
{
- struct data_takeoff_2 data =
- {
- .height = absoluteHeight_m,
- .duration = duration_s,
- .useCurrentYaw = false,
- .yaw = yaw,
- .groupMask = ALL_GROUPS,
- };
+ // struct data_takeoff_2 data =
+ // {
+ // .height = absoluteHeight_m,
+ // .duration = duration_s,
+ // .useCurrentYaw = false,
+ // .yaw = yaw,
+ // .groupMask = ALL_GROUPS,
+ // };
- return handleCommand(COMMAND_TAKEOFF_2, (const uint8_t*)&data);
+ // return handleCommand(COMMAND_TAKEOFF_2, (const uint8_t*)&data);
}
int crtpCommanderHighLevelTakeoffWithVelocity(const float height_m, const float velocity_m_s, bool relative)
{
- struct data_takeoff_with_velocity data =
- {
- .height = height_m,
- .heightIsRelative = relative,
- .velocity = velocity_m_s,
- .useCurrentYaw = true,
- .groupMask = ALL_GROUPS,
- };
+ // struct data_takeoff_with_velocity data =
+ // {
+ // .height = height_m,
+ // .heightIsRelative = relative,
+ // .velocity = velocity_m_s,
+ // .useCurrentYaw = true,
+ // .groupMask = ALL_GROUPS,
+ // };
- return handleCommand(COMMAND_TAKEOFF_WITH_VELOCITY, (const uint8_t*)&data);
+ // return handleCommand(COMMAND_TAKEOFF_WITH_VELOCITY, (const uint8_t*)&data);
}
int crtpCommanderHighLevelLand(const float absoluteHeight_m, const float duration_s)
{
- struct data_land_2 data =
- {
- .height = absoluteHeight_m,
- .duration = duration_s,
- .useCurrentYaw = true,
- .groupMask = ALL_GROUPS,
- };
+ // struct data_land_2 data =
+ // {
+ // .height = absoluteHeight_m,
+ // .duration = duration_s,
+ // .useCurrentYaw = true,
+ // .groupMask = ALL_GROUPS,
+ // };
- return handleCommand(COMMAND_LAND_2, (const uint8_t*)&data);
+ // return handleCommand(COMMAND_LAND_2, (const uint8_t*)&data);
}
int crtpCommanderHighLevelLandWithVelocity(const float height_m, const float velocity_m_s, bool relative)
{
- struct data_land_with_velocity data =
- {
- .height = height_m,
- .heightIsRelative = relative,
- .velocity = velocity_m_s,
- .useCurrentYaw = true,
- .groupMask = ALL_GROUPS,
- };
+ // struct data_land_with_velocity data =
+ // {
+ // .height = height_m,
+ // .heightIsRelative = relative,
+ // .velocity = velocity_m_s,
+ // .useCurrentYaw = true,
+ // .groupMask = ALL_GROUPS,
+ // };
- return handleCommand(COMMAND_LAND_WITH_VELOCITY, (const uint8_t*)&data);
+ // return handleCommand(COMMAND_LAND_WITH_VELOCITY, (const uint8_t*)&data);
}
int crtpCommanderHighLevelLandYaw(const float absoluteHeight_m, const float duration_s, const float yaw)
{
- struct data_land_2 data =
- {
- .height = absoluteHeight_m,
- .duration = duration_s,
- .useCurrentYaw = false,
- .yaw = yaw,
- .groupMask = ALL_GROUPS,
- };
+ // struct data_land_2 data =
+ // {
+ // .height = absoluteHeight_m,
+ // .duration = duration_s,
+ // .useCurrentYaw = false,
+ // .yaw = yaw,
+ // .groupMask = ALL_GROUPS,
+ // };
- return handleCommand(COMMAND_LAND_2, (const uint8_t*)&data);
+ // return handleCommand(COMMAND_LAND_2, (const uint8_t*)&data);
}
int crtpCommanderHighLevelStop()
-{
- struct data_stop data =
- {
- .groupMask = ALL_GROUPS,
- };
+ {
+// struct data_stop data =
+// {
+// .groupMask = ALL_GROUPS,
+// };
- return handleCommand(COMMAND_STOP, (const uint8_t*)&data);
+// return handleCommand(COMMAND_STOP, (const uint8_t*)&data);
}
int crtpCommanderHighLevelGoTo(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative)
{
- struct data_go_to data =
- {
- .x = x,
- .y = y,
- .z = z,
- .yaw = yaw,
- .duration = duration_s,
- .relative = relative,
- .groupMask = ALL_GROUPS,
- };
+ // struct data_go_to data =
+ // {
+ // .x = x,
+ // .y = y,
+ // .z = z,
+ // .yaw = yaw,
+ // .duration = duration_s,
+ // .relative = relative,
+ // .groupMask = ALL_GROUPS,
+ // };
- return handleCommand(COMMAND_GO_TO, (const uint8_t*)&data);
+ // return handleCommand(COMMAND_GO_TO, (const uint8_t*)&data);
}
bool crtpCommanderHighLevelIsTrajectoryDefined(uint8_t trajectoryId)
{
- return (
- trajectoryId < NUM_TRAJECTORY_DEFINITIONS &&
- trajectory_descriptions[trajectoryId].trajectoryLocation != TRAJECTORY_LOCATION_INVALID
- );
+ // return (
+ // trajectoryId < NUM_TRAJECTORY_DEFINITIONS &&
+ // trajectory_descriptions[trajectoryId].trajectoryLocation != TRAJECTORY_LOCATION_INVALID
+ // );
}
int crtpCommanderHighLevelStartTrajectory(const uint8_t trajectoryId, const float timeScale, const bool relative, const bool reversed)
{
- struct data_start_trajectory data =
- {
- .trajectoryId = trajectoryId,
- .timescale = timeScale,
- .relative = relative,
- .reversed = reversed,
- .groupMask = ALL_GROUPS,
- };
+ // struct data_start_trajectory data =
+ // {
+ // .trajectoryId = trajectoryId,
+ // .timescale = timeScale,
+ // .relative = relative,
+ // .reversed = reversed,
+ // .groupMask = ALL_GROUPS,
+ // };
- return handleCommand(COMMAND_START_TRAJECTORY, (const uint8_t*)&data);
+ // return handleCommand(COMMAND_START_TRAJECTORY, (const uint8_t*)&data);
}
int crtpCommanderHighLevelDefineTrajectory(const uint8_t trajectoryId, const crtpCommanderTrajectoryType_t type, const uint32_t offset, const uint8_t nPieces)
{
- struct data_define_trajectory data =
- {
- .trajectoryId = trajectoryId,
- .description.trajectoryLocation = TRAJECTORY_LOCATION_MEM,
- .description.trajectoryType = type,
- .description.trajectoryIdentifier.mem.offset = offset,
- .description.trajectoryIdentifier.mem.n_pieces = nPieces,
- };
+ // struct data_define_trajectory data =
+ // {
+ // .trajectoryId = trajectoryId,
+ // .description.trajectoryLocation = TRAJECTORY_LOCATION_MEM,
+ // .description.trajectoryType = type,
+ // .description.trajectoryIdentifier.mem.offset = offset,
+ // .description.trajectoryIdentifier.mem.n_pieces = nPieces,
+ // };
- return handleCommand(COMMAND_DEFINE_TRAJECTORY, (const uint8_t*)&data);
+ // return handleCommand(COMMAND_DEFINE_TRAJECTORY, (const uint8_t*)&data);
}
uint32_t crtpCommanderHighLevelTrajectoryMemSize()
{
- return sizeof(trajectories_memory);
+ // return sizeof(trajectories_memory);
}
bool crtpCommanderHighLevelWriteTrajectory(const uint32_t offset, const uint32_t length, const uint8_t* data)
{
- bool result = false;
+ // bool result = false;
- if ((offset + length) <= sizeof(trajectories_memory)) {
- memcpy(&(trajectories_memory[offset]), data, length);
- result = true;
- }
+ // if ((offset + length) <= sizeof(trajectories_memory)) {
+ // memcpy(&(trajectories_memory[offset]), data, length);
+ // result = true;
+ // }
- return result;
+ // return result;
}
bool crtpCommanderHighLevelReadTrajectory(const uint32_t offset, const uint32_t length, uint8_t* destination)
{
- bool result = false;
+ // bool result = false;
- if (offset + length <= sizeof(trajectories_memory) && memcpy(destination, &(trajectories_memory[offset]), length)) {
- result = true;
- }
+ // if (offset + length <= sizeof(trajectories_memory) && memcpy(destination, &(trajectories_memory[offset]), length)) {
+ // result = true;
+ // }
- return result;
+ // return result;
}
bool crtpCommanderHighLevelIsTrajectoryFinished() {
- float t = usecTimestamp() / 1e6;
- return plan_is_finished(&planner, t);
+ // float t = usecTimestamp() / 1e6;
+ // return plan_is_finished(&planner, t);
}
/**
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_commander_rpyt.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_commander_rpyt.c
index 26c543f9c3d039e7fd87d7a2f5969e2af36c4bbd..7d879bbeef6bbb28fc6bf7a3493aad3b1bd382f8 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_commander_rpyt.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_commander_rpyt.c
@@ -24,7 +24,7 @@
*
*/
#include <math.h>
-#include "empty_math.h"
+// #include "empty_math.h"
#include <stdbool.h>
#include "crtp_commander.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_localization_service.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_localization_service.c
index b44449844a7dd0b4c8a65f6ca0e7ded159a23554..ddb697cfbc70d46ce1cf2241202d7a882d932830 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_localization_service.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtp_localization_service.c
@@ -25,7 +25,7 @@
*/
#include <string.h>
#include <stdint.h>
-#include "empty_math.h"
+// #include "empty_math.h"
#include "FreeRTOS.h"
#include "task.h"
@@ -127,108 +127,122 @@ static void extPositionPackedHandler(CRTPPacket* pk);
void locSrvInit()
{
- if (isInit) {
- return;
- }
+ //COMMENTED FIRMWARE
- uint64_t address = configblockGetRadioAddress();
- my_id = address & 0xFF;
+ // if (isInit) {
+ // return;
+ // }
- crtpRegisterPortCB(CRTP_PORT_LOCALIZATION, locSrvCrtpCB);
- isInit = true;
+ // uint64_t address = configblockGetRadioAddress();
+ // my_id = address & 0xFF;
+
+ // crtpRegisterPortCB(CRTP_PORT_LOCALIZATION, locSrvCrtpCB);
+ // isInit = true;
}
static void locSrvCrtpCB(CRTPPacket* pk)
{
- switch (pk->channel)
- {
- case EXT_POSITION:
- extPositionHandler(pk);
- break;
- case GENERIC_TYPE:
- genericLocHandle(pk);
- break;
- case EXT_POSITION_PACKED:
- extPositionPackedHandler(pk);
- break;
- default:
- break;
- }
+ //COMMENTED FIRMWARE
+
+ // switch (pk->channel)
+ // {
+ // case EXT_POSITION:
+ // extPositionHandler(pk);
+ // break;
+ // case GENERIC_TYPE:
+ // genericLocHandle(pk);
+ // break;
+ // case EXT_POSITION_PACKED:
+ // extPositionPackedHandler(pk);
+ // break;
+ // default:
+ // break;
+ // }
}
static void updateLogFromExtPos()
{
- ext_pose.x = ext_pos.x;
- ext_pose.y = ext_pos.y;
- ext_pose.z = ext_pos.z;
+ //COMMENTED FIRMWARE
+
+ // ext_pose.x = ext_pos.x;
+ // ext_pose.y = ext_pos.y;
+ // ext_pose.z = ext_pos.z;
}
static void extPositionHandler(CRTPPacket* pk) {
- const struct CrtpExtPosition* data = (const struct CrtpExtPosition*)pk->data;
+ //COMMENTED FIRMWARE
+
+ // const struct CrtpExtPosition* data = (const struct CrtpExtPosition*)pk->data;
- ext_pos.x = data->x;
- ext_pos.y = data->y;
- ext_pos.z = data->z;
- ext_pos.stdDev = extPosStdDev;
- ext_pos.source = MeasurementSourceLocationService;
- updateLogFromExtPos();
+ // ext_pos.x = data->x;
+ // ext_pos.y = data->y;
+ // ext_pos.z = data->z;
+ // ext_pos.stdDev = extPosStdDev;
+ // ext_pos.source = MeasurementSourceLocationService;
+ // updateLogFromExtPos();
- estimatorEnqueuePosition(&ext_pos);
- tickOfLastPacket = xTaskGetTickCount();
+ // estimatorEnqueuePosition(&ext_pos);
+ // tickOfLastPacket = xTaskGetTickCount();
}
static void extPoseHandler(const CRTPPacket* pk) {
- const struct CrtpExtPose* data = (const struct CrtpExtPose*)&pk->data[1];
-
- ext_pose.x = data->x;
- ext_pose.y = data->y;
- ext_pose.z = data->z;
- ext_pose.quat.x = data->qx;
- ext_pose.quat.y = data->qy;
- ext_pose.quat.z = data->qz;
- ext_pose.quat.w = data->qw;
- ext_pose.stdDevPos = extPosStdDev;
- ext_pose.stdDevQuat = extQuatStdDev;
-
- estimatorEnqueuePose(&ext_pose);
- tickOfLastPacket = xTaskGetTickCount();
+ //COMMENTED FIRMWARE
+
+ // const struct CrtpExtPose* data = (const struct CrtpExtPose*)&pk->data[1];
+
+ // ext_pose.x = data->x;
+ // ext_pose.y = data->y;
+ // ext_pose.z = data->z;
+ // ext_pose.quat.x = data->qx;
+ // ext_pose.quat.y = data->qy;
+ // ext_pose.quat.z = data->qz;
+ // ext_pose.quat.w = data->qw;
+ // ext_pose.stdDevPos = extPosStdDev;
+ // ext_pose.stdDevQuat = extQuatStdDev;
+
+ // estimatorEnqueuePose(&ext_pose);
+ // tickOfLastPacket = xTaskGetTickCount();
}
static void extPosePackedHandler(const CRTPPacket* pk) {
- uint8_t numItems = (pk->size - 1) / sizeof(extPosePackedItem);
- for (uint8_t i = 0; i < numItems; ++i) {
- const extPosePackedItem* item = (const extPosePackedItem*)&pk->data[1 + i * sizeof(extPosePackedItem)];
- if (item->id == my_id) {
- ext_pose.x = item->x / 1000.0f;
- ext_pose.y = item->y / 1000.0f;
- ext_pose.z = item->z / 1000.0f;
- quatdecompress(item->quat, (float *)&ext_pose.quat.q0);
- ext_pose.stdDevPos = extPosStdDev;
- ext_pose.stdDevQuat = extQuatStdDev;
- estimatorEnqueuePose(&ext_pose);
- tickOfLastPacket = xTaskGetTickCount();
- } else {
- ext_pos.x = item->x / 1000.0f;
- ext_pos.y = item->y / 1000.0f;
- ext_pos.z = item->z / 1000.0f;
- ext_pos.stdDev = extPosStdDev;
- peerLocalizationTellPosition(item->id, &ext_pos);
- }
- }
+ //COMMENTED FIRMWARE
+
+ // uint8_t numItems = (pk->size - 1) / sizeof(extPosePackedItem);
+ // for (uint8_t i = 0; i < numItems; ++i) {
+ // const extPosePackedItem* item = (const extPosePackedItem*)&pk->data[1 + i * sizeof(extPosePackedItem)];
+ // if (item->id == my_id) {
+ // ext_pose.x = item->x / 1000.0f;
+ // ext_pose.y = item->y / 1000.0f;
+ // ext_pose.z = item->z / 1000.0f;
+ // quatdecompress(item->quat, (float *)&ext_pose.quat.q0);
+ // ext_pose.stdDevPos = extPosStdDev;
+ // ext_pose.stdDevQuat = extQuatStdDev;
+ // estimatorEnqueuePose(&ext_pose);
+ // tickOfLastPacket = xTaskGetTickCount();
+ // } else {
+ // ext_pos.x = item->x / 1000.0f;
+ // ext_pos.y = item->y / 1000.0f;
+ // ext_pos.z = item->z / 1000.0f;
+ // ext_pos.stdDev = extPosStdDev;
+ // peerLocalizationTellPosition(item->id, &ext_pos);
+ // }
+ // }
}
static void lpsShortLppPacketHandler(CRTPPacket* pk) {
- if (pk->size >= 2) {
- bool success = lpsSendLppShort(pk->data[1], &pk->data[2], pk->size-2);
-
- pk->port = CRTP_PORT_LOCALIZATION;
- pk->channel = GENERIC_TYPE;
- pk->size = 3;
- pk->data[0] = LPS_SHORT_LPP_PACKET;
- pk->data[2] = success?1:0;
- // This is best effort, i.e. the blocking version is not needed
- crtpSendPacket(pk);
- }
+ //COMMENTED FIRMWARE
+
+ // if (pk->size >= 2) {
+ // bool success = lpsSendLppShort(pk->data[1], &pk->data[2], pk->size-2);
+
+ // pk->port = CRTP_PORT_LOCALIZATION;
+ // pk->channel = GENERIC_TYPE;
+ // pk->size = 3;
+ // pk->data[0] = LPS_SHORT_LPP_PACKET;
+ // pk->data[2] = success?1:0;
+ // // This is best effort, i.e. the blocking version is not needed
+ // crtpSendPacket(pk);
+ // }
}
typedef union {
@@ -242,138 +256,150 @@ typedef union {
} __attribute__((packed)) LhPersistArgs_t;
static void lhPersistDataWorker(void* arg) {
- LhPersistArgs_t* args = (LhPersistArgs_t*) &arg;
-
- bool result = true;
-
- for (int baseStation = 0; baseStation < PULSE_PROCESSOR_N_BASE_STATIONS; baseStation++) {
- uint16_t mask = 1 << baseStation;
- bool storeGeo = (args->geoDataBsField & mask) != 0;
- bool storeCalibration = (args->calibrationDataBsField & mask) != 0;
- if (! lighthouseStoragePersistData(baseStation, storeGeo, storeCalibration)) {
- result = false;
- break;
- }
- }
-
- CRTPPacket response = {
- .port = CRTP_PORT_LOCALIZATION,
- .channel = GENERIC_TYPE,
- .size = 2,
- .data = {LH_PERSIST_DATA, result}
- };
-
- crtpSendPacketBlock(&response);
+ //COMMENTED FIRMWARE
+
+ // LhPersistArgs_t* args = (LhPersistArgs_t*) &arg;
+
+ // bool result = true;
+
+ // for (int baseStation = 0; baseStation < PULSE_PROCESSOR_N_BASE_STATIONS; baseStation++) {
+ // uint16_t mask = 1 << baseStation;
+ // bool storeGeo = (args->geoDataBsField & mask) != 0;
+ // bool storeCalibration = (args->calibrationDataBsField & mask) != 0;
+ // if (! lighthouseStoragePersistData(baseStation, storeGeo, storeCalibration)) {
+ // result = false;
+ // break;
+ // }
+ // }
+
+ // CRTPPacket response = {
+ // .port = CRTP_PORT_LOCALIZATION,
+ // .channel = GENERIC_TYPE,
+ // .size = 2,
+ // .data = {LH_PERSIST_DATA, result}
+ // };
+
+ // crtpSendPacketBlock(&response);
}
static void lhPersistDataHandler(CRTPPacket* pk) {
- if (pk->size >= (1 + sizeof(LhPersistArgs_t))) {
- LhPersistArgs_t* args = (LhPersistArgs_t*) &pk->data[1];
- workerSchedule(lhPersistDataWorker, (void*)args->combinedField);
- }
+ //COMMENTED FIRMWARE
+
+ // if (pk->size >= (1 + sizeof(LhPersistArgs_t))) {
+ // LhPersistArgs_t* args = (LhPersistArgs_t*) &pk->data[1];
+ // workerSchedule(lhPersistDataWorker, (void*)args->combinedField);
+ // }
}
static void genericLocHandle(CRTPPacket* pk)
{
- const uint8_t type = pk->data[0];
- if (pk->size < 1) return;
-
- switch (type) {
- case LPS_SHORT_LPP_PACKET:
- lpsShortLppPacketHandler(pk);
- break;
- case EMERGENCY_STOP:
- stabilizerSetEmergencyStop();
- break;
- case EMERGENCY_STOP_WATCHDOG:
- stabilizerSetEmergencyStopTimeout(DEFAULT_EMERGENCY_STOP_TIMEOUT);
- break;
- case EXT_POSE:
- extPoseHandler(pk);
- break;
- case EXT_POSE_PACKED:
- extPosePackedHandler(pk);
- break;
- case LH_PERSIST_DATA:
- lhPersistDataHandler(pk);
- break;
- default:
- // Nothing here
- break;
- }
+ //COMMENTED FIRMWARE
+
+ // const uint8_t type = pk->data[0];
+ // if (pk->size < 1) return;
+
+ // switch (type) {
+ // case LPS_SHORT_LPP_PACKET:
+ // lpsShortLppPacketHandler(pk);
+ // break;
+ // case EMERGENCY_STOP:
+ // stabilizerSetEmergencyStop();
+ // break;
+ // case EMERGENCY_STOP_WATCHDOG:
+ // stabilizerSetEmergencyStopTimeout(DEFAULT_EMERGENCY_STOP_TIMEOUT);
+ // break;
+ // case EXT_POSE:
+ // extPoseHandler(pk);
+ // break;
+ // case EXT_POSE_PACKED:
+ // extPosePackedHandler(pk);
+ // break;
+ // case LH_PERSIST_DATA:
+ // lhPersistDataHandler(pk);
+ // break;
+ // default:
+ // // Nothing here
+ // break;
+ // }
}
static void extPositionPackedHandler(CRTPPacket* pk)
{
- uint8_t numItems = pk->size / sizeof(extPositionPackedItem);
- for (uint8_t i = 0; i < numItems; ++i) {
- const extPositionPackedItem* item = (const extPositionPackedItem*)&pk->data[i * sizeof(extPositionPackedItem)];
- ext_pos.x = item->x / 1000.0f;
- ext_pos.y = item->y / 1000.0f;
- ext_pos.z = item->z / 1000.0f;
- ext_pos.stdDev = extPosStdDev;
- ext_pos.source = MeasurementSourceLocationService;
- if (item->id == my_id) {
- updateLogFromExtPos();
- estimatorEnqueuePosition(&ext_pos);
- tickOfLastPacket = xTaskGetTickCount();
- }
- else {
- peerLocalizationTellPosition(item->id, &ext_pos);
- }
- }
+ //COMMENTED FIRMWARE
+
+ // uint8_t numItems = pk->size / sizeof(extPositionPackedItem);
+ // for (uint8_t i = 0; i < numItems; ++i) {
+ // const extPositionPackedItem* item = (const extPositionPackedItem*)&pk->data[i * sizeof(extPositionPackedItem)];
+ // ext_pos.x = item->x / 1000.0f;
+ // ext_pos.y = item->y / 1000.0f;
+ // ext_pos.z = item->z / 1000.0f;
+ // ext_pos.stdDev = extPosStdDev;
+ // ext_pos.source = MeasurementSourceLocationService;
+ // if (item->id == my_id) {
+ // updateLogFromExtPos();
+ // estimatorEnqueuePosition(&ext_pos);
+ // tickOfLastPacket = xTaskGetTickCount();
+ // }
+ // else {
+ // peerLocalizationTellPosition(item->id, &ext_pos);
+ // }
+ // }
}
void locSrvSendRangeFloat(uint8_t id, float range)
{
- rangePacket *rp = (rangePacket *)pkRange.data;
-
- ASSERT(rangeIndex <= NBR_OF_RANGES_IN_PACKET);
-
- if (enableRangeStreamFloat)
- {
- rp->ranges[rangeIndex].id = id;
- rp->ranges[rangeIndex].range = range;
- rangeIndex++;
-
- if (rangeIndex >= 5)
- {
- rp->type = RANGE_STREAM_FLOAT;
- pkRange.port = CRTP_PORT_LOCALIZATION;
- pkRange.channel = GENERIC_TYPE;
- pkRange.size = sizeof(rangePacket);
- // This is best effort, i.e. the blocking version is not needed
- crtpSendPacket(&pkRange);
- rangeIndex = 0;
- }
- }
+ //COMMENTED FIRMWARE
+
+ // rangePacket *rp = (rangePacket *)pkRange.data;
+
+ // ASSERT(rangeIndex <= NBR_OF_RANGES_IN_PACKET);
+
+ // if (enableRangeStreamFloat)
+ // {
+ // rp->ranges[rangeIndex].id = id;
+ // rp->ranges[rangeIndex].range = range;
+ // rangeIndex++;
+
+ // if (rangeIndex >= 5)
+ // {
+ // rp->type = RANGE_STREAM_FLOAT;
+ // pkRange.port = CRTP_PORT_LOCALIZATION;
+ // pkRange.channel = GENERIC_TYPE;
+ // pkRange.size = sizeof(rangePacket);
+ // // This is best effort, i.e. the blocking version is not needed
+ // crtpSendPacket(&pkRange);
+ // rangeIndex = 0;
+ // }
+ // }
}
void locSrvSendLighthouseAngle(int basestation, pulseProcessorResult_t* angles)
{
- anglePacket *ap = (anglePacket *)LhAngle.data;
-
- if (enableLighthouseAngleStream) {
- ap->basestation = basestation;
-
- for(uint8_t its = 0; its < NBR_OF_SWEEPS_IN_PACKET; its++) {
- float angle_first_sensor = angles->sensorMeasurementsLh1[0].baseStatonMeasurements[basestation].correctedAngles[its];
- ap->sweeps[its].sweep = angle_first_sensor;
-
- for(uint8_t itd = 0; itd < NBR_OF_SENSOR_DIFFS_IN_PACKET; itd++) {
- float angle_other_sensor = angles->sensorMeasurementsLh1[itd + 1].baseStatonMeasurements[basestation].correctedAngles[its];
- uint16_t angle_diff = single2half(angle_first_sensor - angle_other_sensor);
- ap->sweeps[its].angleDiffs[itd].angleDiff = angle_diff;
- }
- }
-
- ap->type = LH_ANGLE_STREAM;
- LhAngle.port = CRTP_PORT_LOCALIZATION;
- LhAngle.channel = GENERIC_TYPE;
- LhAngle.size = sizeof(anglePacket);
- // This is best effort, i.e. the blocking version is not needed
- crtpSendPacket(&LhAngle);
- }
+ //COMMENTED FIRMWARE
+
+ // anglePacket *ap = (anglePacket *)LhAngle.data;
+
+ // if (enableLighthouseAngleStream) {
+ // ap->basestation = basestation;
+
+ // for(uint8_t its = 0; its < NBR_OF_SWEEPS_IN_PACKET; its++) {
+ // float angle_first_sensor = angles->sensorMeasurementsLh1[0].baseStatonMeasurements[basestation].correctedAngles[its];
+ // ap->sweeps[its].sweep = angle_first_sensor;
+
+ // for(uint8_t itd = 0; itd < NBR_OF_SENSOR_DIFFS_IN_PACKET; itd++) {
+ // float angle_other_sensor = angles->sensorMeasurementsLh1[itd + 1].baseStatonMeasurements[basestation].correctedAngles[its];
+ // uint16_t angle_diff = single2half(angle_first_sensor - angle_other_sensor);
+ // ap->sweeps[its].angleDiffs[itd].angleDiff = angle_diff;
+ // }
+ // }
+
+ // ap->type = LH_ANGLE_STREAM;
+ // LhAngle.port = CRTP_PORT_LOCALIZATION;
+ // LhAngle.channel = GENERIC_TYPE;
+ // LhAngle.size = sizeof(anglePacket);
+ // // This is best effort, i.e. the blocking version is not needed
+ // crtpSendPacket(&LhAngle);
+ // }
}
// This logging group is deprecated
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtpservice.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtpservice.c
index a3b9a481233ab6c2130d739df75c968130171689..d42a2d839a22c5f1fa0d9cc3a0964fef4d7379e8 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtpservice.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/crtpservice.c
@@ -80,7 +80,7 @@ static void crtpSrvTask(void* prm)
{
case linkEcho:
if (echoDelay > 0) {
- vTaskDelay(M2T(echoDelay));
+ // vTaskDelay(M2T(echoDelay));
}
crtpSendPacketBlock(&p);
break;
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/log.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/log.c
index ee8535d068267e0def18fe10593e7acea304cf25..29786a998d8c882981ce191499448bdb6b1107e7 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/log.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/log.c
@@ -49,7 +49,7 @@
#include "cfassert.h"
#include "debug.h"
#include "static_mem.h"
-#include "empty_math.h"
+// #include "empty_math.h"
#if 0
#define LOG_DEBUG(fmt, ...) DEBUG_PRINT("D/log " fmt, ## __VA_ARGS__)
@@ -436,8 +436,8 @@ static int logCreateBlock(unsigned char id, struct ops_setting * settings, int l
return ENOMEM;
logBlocks[i].id = id;
- logBlocks[i].timer = xTimerCreateStatic("logTimer", M2T(1000), pdTRUE,
- &logBlocks[i], logBlockTimed, &logBlocks[i].timerBuffer);
+ // logBlocks[i].timer = xTimerCreateStatic("logTimer", M2T(1000), pdTRUE,
+ // &logBlocks[i], logBlockTimed, &logBlocks[i].timerBuffer);
logBlocks[i].ops = NULL;
if (logBlocks[i].timer == NULL)
@@ -465,8 +465,8 @@ static int logCreateBlockV2(unsigned char id, struct ops_setting_v2 * settings,
return ENOMEM;
logBlocks[i].id = id;
- logBlocks[i].timer = xTimerCreateStatic("logTimer", M2T(1000), pdTRUE,
- &logBlocks[i], logBlockTimed, &logBlocks[i].timerBuffer);
+ // logBlocks[i].timer = xTimerCreateStatic("logTimer", M2T(1000), pdTRUE,
+ // &logBlocks[i], logBlockTimed, &logBlocks[i].timerBuffer);
logBlocks[i].ops = NULL;
if (logBlocks[i].timer == NULL)
@@ -670,7 +670,7 @@ static int logStartBlock(int id, unsigned int period)
if (period>0)
{
- xTimerChangePeriod(logBlocks[i].timer, M2T(period), 100);
+ // xTimerChangePeriod(logBlocks[i].timer, M2T(period), 100);
xTimerStart(logBlocks[i].timer, 100);
} else {
// single-shoot run
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_controller_indi.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_controller_indi.c
index 989c224c7d933dab8f027633889818aea00a202c..a039c63fb6ea84be0b8182f3edb00b4025bb7bb2 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_controller_indi.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_controller_indi.c
@@ -27,7 +27,7 @@
#include "position_controller_indi.h"
//#include "math3d.h"
-#include "empty_math.h"
+// #include "empty_math.h"
// Position controller gains
float K_xi_x = 1.0f;
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_controller_pid.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_controller_pid.c
index 2fa55dd5c7db915c29b0d6e7b13999876e6fd60e..d36f0a2183e8e2c647b32f8d3594b7f1d8c35d5b 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_controller_pid.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_controller_pid.c
@@ -25,7 +25,7 @@
*/
//#include <math.h>
-#include "empty_math.h"
+// #include "empty_math.h"
#include "num.h"
#include "commander.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/stabilizer.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/stabilizer.c
index d3ed129de910400a400885b3dbaddcd1fa84c870..a9070418df892e7034cc78c7b504ec45feb93237 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/stabilizer.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/stabilizer.c
@@ -1,3 +1,7 @@
+ //COMMENTED FIRMWARE
+
+
+
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
@@ -116,93 +120,93 @@ static void stabilizerTask(void* param);
static void calcSensorToOutputLatency(const sensorData_t *sensorData)
{
- uint64_t outTimestamp = usecTimestamp();
- inToOutLatency = outTimestamp - sensorData->interruptTimestamp;
+ // uint64_t outTimestamp = usecTimestamp();
+ // inToOutLatency = outTimestamp - sensorData->interruptTimestamp;
}
static void compressState()
{
- stateCompressed.x = state.position.x * 1000.0f;
- stateCompressed.y = state.position.y * 1000.0f;
- stateCompressed.z = state.position.z * 1000.0f;
-
- stateCompressed.vx = state.velocity.x * 1000.0f;
- stateCompressed.vy = state.velocity.y * 1000.0f;
- stateCompressed.vz = state.velocity.z * 1000.0f;
-
- stateCompressed.ax = state.acc.x * 9.81f * 1000.0f;
- stateCompressed.ay = state.acc.y * 9.81f * 1000.0f;
- stateCompressed.az = (state.acc.z + 1) * 9.81f * 1000.0f;
-
- float const q[4] = {
- state.attitudeQuaternion.x,
- state.attitudeQuaternion.y,
- state.attitudeQuaternion.z,
- state.attitudeQuaternion.w};
- stateCompressed.quat = quatcompress(q);
-
- float const deg2millirad = ((float)M_PI * 1000.0f) / 180.0f;
- stateCompressed.rateRoll = sensorData.gyro.x * deg2millirad;
- stateCompressed.ratePitch = -sensorData.gyro.y * deg2millirad;
- stateCompressed.rateYaw = sensorData.gyro.z * deg2millirad;
+ // stateCompressed.x = state.position.x * 1000.0f;
+ // stateCompressed.y = state.position.y * 1000.0f;
+ // stateCompressed.z = state.position.z * 1000.0f;
+
+ // stateCompressed.vx = state.velocity.x * 1000.0f;
+ // stateCompressed.vy = state.velocity.y * 1000.0f;
+ // stateCompressed.vz = state.velocity.z * 1000.0f;
+
+ // stateCompressed.ax = state.acc.x * 9.81f * 1000.0f;
+ // stateCompressed.ay = state.acc.y * 9.81f * 1000.0f;
+ // stateCompressed.az = (state.acc.z + 1) * 9.81f * 1000.0f;
+
+ // float const q[4] = {
+ // state.attitudeQuaternion.x,
+ // state.attitudeQuaternion.y,
+ // state.attitudeQuaternion.z,
+ // state.attitudeQuaternion.w};
+ // stateCompressed.quat = quatcompress(q);
+
+ // float const deg2millirad = ((float)M_PI * 1000.0f) / 180.0f;
+ // stateCompressed.rateRoll = sensorData.gyro.x * deg2millirad;
+ // stateCompressed.ratePitch = -sensorData.gyro.y * deg2millirad;
+ // stateCompressed.rateYaw = sensorData.gyro.z * deg2millirad;
}
static void compressSetpoint()
{
- setpointCompressed.x = setpoint.position.x * 1000.0f;
- setpointCompressed.y = setpoint.position.y * 1000.0f;
- setpointCompressed.z = setpoint.position.z * 1000.0f;
+ // setpointCompressed.x = setpoint.position.x * 1000.0f;
+ // setpointCompressed.y = setpoint.position.y * 1000.0f;
+ // setpointCompressed.z = setpoint.position.z * 1000.0f;
- setpointCompressed.vx = setpoint.velocity.x * 1000.0f;
- setpointCompressed.vy = setpoint.velocity.y * 1000.0f;
- setpointCompressed.vz = setpoint.velocity.z * 1000.0f;
+ // setpointCompressed.vx = setpoint.velocity.x * 1000.0f;
+ // setpointCompressed.vy = setpoint.velocity.y * 1000.0f;
+ // setpointCompressed.vz = setpoint.velocity.z * 1000.0f;
- setpointCompressed.ax = setpoint.acceleration.x * 1000.0f;
- setpointCompressed.ay = setpoint.acceleration.y * 1000.0f;
- setpointCompressed.az = setpoint.acceleration.z * 1000.0f;
+ // setpointCompressed.ax = setpoint.acceleration.x * 1000.0f;
+ // setpointCompressed.ay = setpoint.acceleration.y * 1000.0f;
+ // setpointCompressed.az = setpoint.acceleration.z * 1000.0f;
}
void stabilizerInit(StateEstimatorType estimator)
{
- if(isInit)
- return;
+ // if(isInit)
+ // return;
- sensorsInit();
- stateEstimatorInit(estimator);
- controllerInit(ControllerTypeAny);
- powerDistributionInit();
- collisionAvoidanceInit();
- estimatorType = getStateEstimator();
- controllerType = getControllerType();
+ // sensorsInit();
+ // stateEstimatorInit(estimator);
+ // controllerInit(ControllerTypeAny);
+ // powerDistributionInit();
+ // collisionAvoidanceInit();
+ // estimatorType = getStateEstimator();
+ // controllerType = getControllerType();
//COMMENTED FIRMWARE
//STATIC_MEM_TASK_CREATE(stabilizerTask, stabilizerTask, STABILIZER_TASK_NAME, NULL, STABILIZER_TASK_PRI);
- isInit = true;
+ // isInit = true;
}
bool stabilizerTest(void)
{
- bool pass = true;
+ // bool pass = true;
- pass &= sensorsTest();
- pass &= stateEstimatorTest();
- pass &= controllerTest();
- pass &= powerDistributionTest();
- pass &= collisionAvoidanceTest();
+ // pass &= sensorsTest();
+ // pass &= stateEstimatorTest();
+ // pass &= controllerTest();
+ // pass &= powerDistributionTest();
+ // pass &= collisionAvoidanceTest();
- return pass;
+ // return pass;
}
static void checkEmergencyStopTimeout()
{
- if (emergencyStopTimeout >= 0) {
- emergencyStopTimeout -= 1;
+ // if (emergencyStopTimeout >= 0) {
+ // emergencyStopTimeout -= 1;
- if (emergencyStopTimeout == 0) {
- emergencyStop = true;
- }
- }
+ // if (emergencyStopTimeout == 0) {
+ // emergencyStop = true;
+ // }
+ // }
}
/* The stabilizer loop runs at 1kHz (stock) or 500Hz (kalman). It is the
@@ -212,109 +216,113 @@ static void checkEmergencyStopTimeout()
static void stabilizerTask(void* param)
{
- uint32_t tick;
- uint32_t lastWakeTime;
-
- //CHANGED FIRMWARE
- vTaskSetApplicationTaskTag(0, (void*)3);
-
- //Wait for the system to be fully started to start stabilization loop
- systemWaitStart();
-
- DEBUG_PRINT("Wait for sensor calibration...\n");
-
- // Wait for sensors to be calibrated
- lastWakeTime = xTaskGetTickCount();
- while(!sensorsAreCalibrated()) {
- //COMMENTED FIRMWARE
- //vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
- }
- // Initialize tick to something else then 0
- tick = 1;
-
- rateSupervisorInit(&rateSupervisorContext, xTaskGetTickCount(), M2T(1000), 997, 1003, 1);
-
- DEBUG_PRINT("Ready to fly.\n");
-
- while(1) {
- // The sensor should unlock at 1kHz
- sensorsWaitDataReady();
-
- // update sensorData struct (for logging variables)
- sensorsAcquire(&sensorData, tick);
-
- if (healthShallWeRunTest()) {
- healthRunTests(&sensorData);
- } else {
- // allow to update estimator dynamically
- if (getStateEstimator() != estimatorType) {
- stateEstimatorSwitchTo(estimatorType);
- estimatorType = getStateEstimator();
- }
- // allow to update controller dynamically
- if (getControllerType() != controllerType) {
- controllerInit(controllerType);
- controllerType = getControllerType();
- }
-
- stateEstimator(&state, tick);
- compressState();
-
- commanderGetSetpoint(&setpoint, &state);
- compressSetpoint();
-
- collisionAvoidanceUpdateSetpoint(&setpoint, &sensorData, &state, tick);
-
- controller(&control, &setpoint, &sensorData, &state, tick);
-
- checkEmergencyStopTimeout();
-
- //
- // The supervisor module keeps track of Crazyflie state such as if
- // we are ok to fly, or if the Crazyflie is in flight.
- //
- supervisorUpdate(&sensorData);
-
- if (emergencyStop || (systemIsArmed() == false)) {
- powerStop();
- } else {
- powerDistribution(&control);
- }
-
- // Log data to uSD card if configured
- if ( usddeckLoggingEnabled()
- && usddeckLoggingMode() == usddeckLoggingMode_SynchronousStabilizer
- && RATE_DO_EXECUTE(usddeckFrequency(), tick)) {
- usddeckTriggerLogging();
- }
- }
- calcSensorToOutputLatency(&sensorData);
- tick++;
- STATS_CNT_RATE_EVENT(&stabilizerRate);
-
- if (!rateSupervisorValidate(&rateSupervisorContext, xTaskGetTickCount())) {
- if (!rateWarningDisplayed) {
- DEBUG_PRINT("WARNING: stabilizer loop rate is off (%lu)\n", rateSupervisorLatestCount(&rateSupervisorContext));
- rateWarningDisplayed = true;
- }
- }
- }
+ // uint32_t tick;
+ // uint32_t lastWakeTime;
+
+ // //CHANGED FIRMWARE
+ // vTaskSetApplicationTaskTag(0, (void*)3);
+
+ // //Wait for the system to be fully started to start stabilization loop
+ // systemWaitStart();
+
+ // DEBUG_PRINT("Wait for sensor calibration...\n");
+
+ // // Wait for sensors to be calibrated
+ // lastWakeTime = xTaskGetTickCount();
+ // while(!sensorsAreCalibrated()) {
+ // //COMMENTED FIRMWARE
+ // //vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
+ // }
+ // // Initialize tick to something else then 0
+ // tick = 1;
+
+ // rateSupervisorInit(&rateSupervisorContext, xTaskGetTickCount(), M2T(1000), 997, 1003, 1);
+
+ // DEBUG_PRINT("Ready to fly.\n");
+
+ // while(1) {
+ // // The sensor should unlock at 1kHz
+ // sensorsWaitDataReady();
+
+ // // update sensorData struct (for logging variables)
+ // sensorsAcquire(&sensorData, tick);
+
+ // if (healthShallWeRunTest()) {
+ // healthRunTests(&sensorData);
+ // } else {
+ // // allow to update estimator dynamically
+ // if (getStateEstimator() != estimatorType) {
+ // stateEstimatorSwitchTo(estimatorType);
+ // estimatorType = getStateEstimator();
+ // }
+ // // allow to update controller dynamically
+ // if (getControllerType() != controllerType) {
+ // controllerInit(controllerType);
+ // controllerType = getControllerType();
+ // }
+
+ // stateEstimator(&state, tick);
+ // compressState();
+
+ // commanderGetSetpoint(&setpoint, &state);
+ // compressSetpoint();
+
+ // collisionAvoidanceUpdateSetpoint(&setpoint, &sensorData, &state, tick);
+
+ // controller(&control, &setpoint, &sensorData, &state, tick);
+
+ // checkEmergencyStopTimeout();
+
+ // //
+ // // The supervisor module keeps track of Crazyflie state such as if
+ // // we are ok to fly, or if the Crazyflie is in flight.
+ // //
+ // supervisorUpdate(&sensorData);
+
+ // if (emergencyStop || (systemIsArmed() == false)) {
+ // powerStop();
+ // } else {
+ // powerDistribution(&control);
+ // }
+
+ // // Log data to uSD card if configured
+ // if ( usddeckLoggingEnabled()
+ // && usddeckLoggingMode() == usddeckLoggingMode_SynchronousStabilizer
+ // && RATE_DO_EXECUTE(usddeckFrequency(), tick)) {
+ // usddeckTriggerLogging();
+ // }
+ // }
+ // calcSensorToOutputLatency(&sensorData);
+ // tick++;
+ // STATS_CNT_RATE_EVENT(&stabilizerRate);
+
+ // if (!rateSupervisorValidate(&rateSupervisorContext, xTaskGetTickCount())) {
+ // if (!rateWarningDisplayed) {
+ // DEBUG_PRINT("WARNING: stabilizer loop rate is off (%lu)\n", rateSupervisorLatestCount(&rateSupervisorContext));
+ // rateWarningDisplayed = true;
+ // }
+ // }
+ // }
}
+ //COMMENTED FIRMWARE
+
void stabilizerSetEmergencyStop()
{
- emergencyStop = true;
+ // emergencyStop = true;
}
void stabilizerResetEmergencyStop()
{
- emergencyStop = false;
+ // emergencyStop = false;
}
void stabilizerSetEmergencyStopTimeout(int timeout)
{
- emergencyStop = false;
- emergencyStopTimeout = timeout;
+ //COMMENTED FIRMWARE
+
+ // emergencyStop = false;
+ // emergencyStopTimeout = timeout;
}
/**
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/system.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/system.c
index e273155b096c6c2659b4e0aafe8e17d34ddf2f47..25ce54c7226df148edc11b61b04183300dc01709 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/system.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/system.c
@@ -332,25 +332,25 @@ bool systemIsArmed()
return armed || forceArm;
}
-void vApplicationIdleHook( void )
-{
- //COMMENTED FIRMWARE
- static uint32_t tickOfLatestWatchdogReset = 0;//M2T(0);
-
- portTickType tickCount = xTaskGetTickCount();
-
- if (tickCount - tickOfLatestWatchdogReset > M2T(WATCHDOG_RESET_PERIOD_MS))
- {
- tickOfLatestWatchdogReset = tickCount;
- watchdogReset();
- }
-
- // Enter sleep mode. Does not work when debugging chip with SWD.
- // Currently saves about 20mA STM32F405 current consumption (~30%).
-#ifndef DEBUG
- { __asm volatile ("wfi"); }
-#endif
-}
+//COMMENTED FIRMWARE
+// void vApplicationIdleHook( void )
+// {
+// static uint32_t tickOfLatestWatchdogReset = 0;//M2T(0);
+
+// portTickType tickCount = xTaskGetTickCount();
+
+// if (tickCount - tickOfLatestWatchdogReset > M2T(WATCHDOG_RESET_PERIOD_MS))
+// {
+// tickOfLatestWatchdogReset = tickCount;
+// watchdogReset();
+// }
+
+// // Enter sleep mode. Does not work when debugging chip with SWD.
+// // Currently saves about 20mA STM32F405 current consumption (~30%).
+// #ifndef DEBUG
+// { __asm volatile ("wfi"); }
+// #endif
+// }
/**
* This parameter group contain read-only parameters pertaining to the CPU
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/amg8833.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/amg8833.o
index 2376aa95779ed0de3a03b696450abe50f484d5e8..433d495f3ec82662fe19ee86b6b0fb6873e25153 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/amg8833.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/amg8833.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/app_handler.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/app_handler.o
index ab5084c1a5bf06a62af738d0aaf2625d60c89fd2..2e7f54357d3c24a1bd8ffc9b109bd9330d5faf8e 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/app_handler.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/app_handler.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/bstdr_comm_support.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/bstdr_comm_support.o
index 757410f56892a75874ed2054bcc7ce12f616848c..426e805a538f26fe60b71cc403bb508bed171ce6 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/bstdr_comm_support.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/bstdr_comm_support.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/collision_avoidance.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/collision_avoidance.o
index 3a299ee8411e64bc8bec07efde6d88af79097880..d1302efa29c024d9f58c8ba585c28c19ded92201 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/collision_avoidance.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/collision_avoidance.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/commander.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/commander.o
index e7f89ade1cd3e271ce9d84cbe7e5b842d33dcf58..4549b8b88f3b5f3d0f8a298b89fdf37e55797bab 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/commander.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/commander.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_pid.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_pid.o
index 56584439328e8718ca291368ab87f316790ed281..fb33c6964c74a59799aa902984711f373758917c 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_pid.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_pid.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp.o
index 6578f168aa5e570fa95f161cdf92806e3fbc829a..e61e42c62711980cdcd58d4acbf6dd946689902d 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp.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
index d7f6d5a7c7e32f40a1b7c1b54addb891bac0697a..833d1218d84e3c000670fee0f87d86b68e8fe4a0 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_commander_high_level.o 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/crtp_commander_rpyt.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_commander_rpyt.o
index 0f500c388a4ac10fecb59eb1ba890c0498d98b3d..072de0ab34d86f65dc9483029fffcd77d9863864 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_commander_rpyt.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_commander_rpyt.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_localization_service.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_localization_service.o
index a03ebd7e6f00533fc3ca159d6f1349aea03052b7..2293d9f82a6fc22b22aeda6377923fb040ce1b4f 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_localization_service.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtp_localization_service.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtpservice.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtpservice.o
index ff3d3d66f7facbba26f69c7a37d1d2f45fa8a889..929cf65b3c3f0eceecc5b3ca3f73e3b6ae87a317 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtpservice.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/crtpservice.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eeprom.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eeprom.o
index a98d05b327c5324d0b44f47b21155250994261e3..8f3604e85df424f2aee8f46e2a43fc89f4d7edbe 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eeprom.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eeprom.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledseq.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledseq.o
index 6c169f3880b341a454583ef91a649102b60010e9..9b6e7153d1ee3e7e753d6ade78dbeedbea81ab2a 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledseq.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledseq.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/log.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/log.o
index fca09cc6567395d6072e28cfdde18013ebf5317e..8cde0c14f9b44d975f7371c64edc50d9d29d4d89 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/log.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/log.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/nvic.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/nvic.o
index 4b12e11efccfe6a33d29b2460e6c9234bbbea697..c895b19d8ef17e9261c943c5e6f2246188d7670d 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/nvic.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/nvic.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ow_syslink.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ow_syslink.o
index 73b818d0bdaca516ceaa06e5ad0130ad64c72d49..b5b39e8654c62366d378de943e3977e8e28269a6 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ow_syslink.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ow_syslink.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_controller_indi.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_controller_indi.o
index 6e2ce2aa138d811106f568b9fe84bca319bd25ed..fc0b9d2d2d308b03de3d07509bc05d1f0afb5acd 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_controller_indi.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_controller_indi.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_controller_pid.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_controller_pid.o
index 537a71ec57cf7a626a10a95faf651f54b2c29fb8..123e9a3fedd8f76f3176e589a24aa02fe0c26775 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_controller_pid.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_controller_pid.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor_v2.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor_v2.o
index 56e35f2d43605b767069d408215d7646449c4c09..a69c683d31581c8a6d4035a69ff96ca85ba0f07b 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor_v2.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor_v2.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/radiolink.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/radiolink.o
index 55639c9769b21bb2384da3f0d8e0f8da02e1704b..4d46204d37108766e1fb7c5f133416dda8d59a66 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/radiolink.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/radiolink.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/stabilizer.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/stabilizer.o
index a8ffbc684256fd5fd961b599349a2c64d590f739..2699746cfba29afd2e4fabb1e45e154e9d5e1a8a 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/stabilizer.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/stabilizer.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/system.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/system.o
index 407d40b2d5c67f9aaa162e963369f4be6155618f..881efa1a2c52053b339a81a64ab48bbb538d4ac8 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/system.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/system.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usb.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usb.o
index d05a00e9083557dc7b84e11f29d1120064296693..a2e88598bf5fc7579314498bb6175e996fcf3268 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usb.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usb.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usbd_desc.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usbd_desc.o
index 301bba0cf6314fbddcadbdfccaff99df6f2b277c..b44bbbc8d8328cfd2e5118c29d27107399d6beec 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usbd_desc.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usbd_desc.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usblink.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usblink.o
index 353f6f791d849714c88d09dff8c7ea5a6a78362b..35532c2951ec759fea05da68906b2a3c2f5d65c7 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usblink.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usblink.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/pulse_processor_v2.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/pulse_processor_v2.c
index a1e8fe960ddf505693b37c2b453ff71eb81abb66..9db6d86e85afe05af8a6033a4a840d31e7f911c0 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/pulse_processor_v2.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/pulse_processor_v2.c
@@ -46,99 +46,99 @@ static const uint32_t NO_OFFSET = 0;
// The cycle times come from the base stations and are expressed in a 48 MHz clock, we use 24 MHz clock hence the "/ 2".
static const uint32_t CYCLE_PERIODS[V2_N_CHANNELS] = {
- 959000 / 2, 957000 / 2, 953000 / 2, 949000 / 2,
- 947000 / 2, 943000 / 2, 941000 / 2, 939000 / 2,
- 937000 / 2, 929000 / 2, 919000 / 2, 911000 / 2,
- 907000 / 2, 901000 / 2, 893000 / 2, 887000 / 2
+ // 959000 / 2, 957000 / 2, 953000 / 2, 949000 / 2,
+ // 947000 / 2, 943000 / 2, 941000 / 2, 939000 / 2,
+ // 937000 / 2, 929000 / 2, 919000 / 2, 911000 / 2,
+ // 907000 / 2, 901000 / 2, 893000 / 2, 887000 / 2
};
static inline uint32_t cyclePeriodToMicroseconds(uint32_t cyclePeriod) {
- return cyclePeriod / 24;
+ // return cyclePeriod / 24;
}
// Reset angles after fixed period to avoid using stale data
static void clearStaleAnglesAfterTimeout(pulseProcessorResult_t *angles) {
- uint64_t timestamp = usecTimestamp();
- for (int bs = 0; bs < PULSE_PROCESSOR_N_BASE_STATIONS; ++bs) {
- uint64_t elapsed_us = timestamp - angles->lastUsecTimestamp[bs];
- if (elapsed_us > cyclePeriodToMicroseconds(CYCLE_PERIODS[bs])) {
- pulseProcessorClear(angles, bs);
- }
- }
+ // uint64_t timestamp = usecTimestamp();
+ // for (int bs = 0; bs < PULSE_PROCESSOR_N_BASE_STATIONS; ++bs) {
+ // uint64_t elapsed_us = timestamp - angles->lastUsecTimestamp[bs];
+ // if (elapsed_us > cyclePeriodToMicroseconds(CYCLE_PERIODS[bs])) {
+ // pulseProcessorClear(angles, bs);
+ // }
+ // }
}
TESTABLE_STATIC bool processWorkspaceBlock(const pulseProcessorFrame_t slots[], pulseProcessorV2SweepBlock_t* block) {
- // Check we have data for all sensors
- uint8_t sensorMask = 0;
- for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) {
- const pulseProcessorFrame_t* frame = &slots[i];
- sensorMask |= (1 << frame->sensor);
- }
-
- if (sensorMask != 0xf) {
- // All sensors not present - discard
- return false;
- }
-
- // Channel - should all be the same or not set
- block->channel = NO_CHANNEL;
- for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) {
- const pulseProcessorFrame_t* frame = &slots[i];
-
- if (frame->channelFound) {
- if (block->channel == NO_CHANNEL) {
- block->channel = frame->channel;
- }
-
- if (block->channel != frame->channel) {
- // Multiple channels in the block - discard
- return false;
- }
- }
- }
-
- if (block->channel == NO_CHANNEL) {
- // Channel is missing - discard
- return false;
- }
-
- // Offset - should be offset on one and only one sensor
- int indexWithOffset = NO_SENSOR;
- for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) {
- const pulseProcessorFrame_t* frame = &slots[i];
-
- if (frame->offset != NO_OFFSET) {
- if (indexWithOffset == NO_SENSOR) {
- indexWithOffset = i;
- } else {
- // Duplicate offsets - discard
- return false;
- }
- }
- }
-
- if (indexWithOffset == NO_SENSOR) {
- // No offset found - discard
- return false;
- }
-
- // Calculate offsets for all sensors
- const pulseProcessorFrame_t* baseSensor = &slots[indexWithOffset];
- for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) {
- const pulseProcessorFrame_t* frame = &slots[i];
- uint8_t sensor = frame->sensor;
-
- if (i == indexWithOffset) {
- block->offset[sensor] = frame->offset;
- } else {
- uint32_t timestamp_delta = TS_DIFF(baseSensor->timestamp, frame->timestamp);
- block->offset[sensor] = TS_DIFF(baseSensor->offset, timestamp_delta);
- }
- }
-
- block->timestamp0 = TS_DIFF(baseSensor->timestamp, baseSensor->offset);
-
- return true;
+ // // Check we have data for all sensors
+ // uint8_t sensorMask = 0;
+ // for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) {
+ // const pulseProcessorFrame_t* frame = &slots[i];
+ // sensorMask |= (1 << frame->sensor);
+ // }
+
+ // if (sensorMask != 0xf) {
+ // // All sensors not present - discard
+ // return false;
+ // }
+
+ // // Channel - should all be the same or not set
+ // block->channel = NO_CHANNEL;
+ // for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) {
+ // const pulseProcessorFrame_t* frame = &slots[i];
+
+ // if (frame->channelFound) {
+ // if (block->channel == NO_CHANNEL) {
+ // block->channel = frame->channel;
+ // }
+
+ // if (block->channel != frame->channel) {
+ // // Multiple channels in the block - discard
+ // return false;
+ // }
+ // }
+ // }
+
+ // if (block->channel == NO_CHANNEL) {
+ // // Channel is missing - discard
+ // return false;
+ // }
+
+ // // Offset - should be offset on one and only one sensor
+ // int indexWithOffset = NO_SENSOR;
+ // for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) {
+ // const pulseProcessorFrame_t* frame = &slots[i];
+
+ // if (frame->offset != NO_OFFSET) {
+ // if (indexWithOffset == NO_SENSOR) {
+ // indexWithOffset = i;
+ // } else {
+ // // Duplicate offsets - discard
+ // return false;
+ // }
+ // }
+ // }
+
+ // if (indexWithOffset == NO_SENSOR) {
+ // // No offset found - discard
+ // return false;
+ // }
+
+ // // Calculate offsets for all sensors
+ // const pulseProcessorFrame_t* baseSensor = &slots[indexWithOffset];
+ // for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) {
+ // const pulseProcessorFrame_t* frame = &slots[i];
+ // uint8_t sensor = frame->sensor;
+
+ // if (i == indexWithOffset) {
+ // block->offset[sensor] = frame->offset;
+ // } else {
+ // uint32_t timestamp_delta = TS_DIFF(baseSensor->timestamp, frame->timestamp);
+ // block->offset[sensor] = TS_DIFF(baseSensor->offset, timestamp_delta);
+ // }
+ // }
+
+ // block->timestamp0 = TS_DIFF(baseSensor->timestamp, baseSensor->offset);
+
+ // return true;
}
/**
@@ -149,86 +149,86 @@ TESTABLE_STATIC bool processWorkspaceBlock(const pulseProcessorFrame_t slots[],
* @param pulseWorkspace
*/
TESTABLE_STATIC void augmentFramesInWorkspace(pulseProcessorV2PulseWorkspace_t* pulseWorkspace) {
- const int slotsUsed = pulseWorkspace->slotsUsed;
-
- for (int i = 0; i < slotsUsed - 1; i++) {
- pulseProcessorFrame_t* previousFrame = &pulseWorkspace->slots[i];
- const pulseProcessorFrame_t* frame = &pulseWorkspace->slots[i + 1];
- if (! previousFrame->channelFound) {
- if (frame->channelFound) {
- previousFrame->channel = frame->channel;
- previousFrame->channelFound = frame->channelFound;
- i++;
- }
- }
- }
+ // const int slotsUsed = pulseWorkspace->slotsUsed;
+
+ // for (int i = 0; i < slotsUsed - 1; i++) {
+ // pulseProcessorFrame_t* previousFrame = &pulseWorkspace->slots[i];
+ // const pulseProcessorFrame_t* frame = &pulseWorkspace->slots[i + 1];
+ // if (! previousFrame->channelFound) {
+ // if (frame->channelFound) {
+ // previousFrame->channel = frame->channel;
+ // previousFrame->channelFound = frame->channelFound;
+ // i++;
+ // }
+ // }
+ // }
}
static int processWorkspace(pulseProcessorV2PulseWorkspace_t* pulseWorkspace, pulseProcessorV2BlockWorkspace_t* blockWorkspace) {
- augmentFramesInWorkspace(pulseWorkspace);
-
- // Handle missing channels
- // Sometimes the FPGA failes to decode the bitstream for a sensor, but we
- // can assume the timestamp is correct anyway. To know which channel it
- // has, we add the limitation that we only accept blocks where all sensors are present.
- // Further more we only accept workspaces with full and consecutive blocks.
-
- const int slotsUsed = pulseWorkspace->slotsUsed;
- // We must have at least one block
- if (slotsUsed < PULSE_PROCESSOR_N_SENSORS) {
- return 0;
- }
-
- // The number of slots used must be a multiple of the block size
- if ((slotsUsed % PULSE_PROCESSOR_N_SENSORS) != 0) {
- return 0;
- }
-
- // Process one block at a time in the workspace
- int blocksInWorkspace = slotsUsed / PULSE_PROCESSOR_N_SENSORS;
- for (int blockIndex = 0; blockIndex < blocksInWorkspace; blockIndex++) {
- int blockBaseIndex = blockIndex * PULSE_PROCESSOR_N_SENSORS;
- if (! processWorkspaceBlock(&pulseWorkspace->slots[blockBaseIndex], &blockWorkspace->blocks[blockIndex])) {
- // If one block is bad, reject the full workspace
- return 0;
- }
- }
-
- return blocksInWorkspace;
+ // augmentFramesInWorkspace(pulseWorkspace);
+
+ // // Handle missing channels
+ // // Sometimes the FPGA failes to decode the bitstream for a sensor, but we
+ // // can assume the timestamp is correct anyway. To know which channel it
+ // // has, we add the limitation that we only accept blocks where all sensors are present.
+ // // Further more we only accept workspaces with full and consecutive blocks.
+
+ // const int slotsUsed = pulseWorkspace->slotsUsed;
+ // // We must have at least one block
+ // if (slotsUsed < PULSE_PROCESSOR_N_SENSORS) {
+ // return 0;
+ // }
+
+ // // The number of slots used must be a multiple of the block size
+ // if ((slotsUsed % PULSE_PROCESSOR_N_SENSORS) != 0) {
+ // return 0;
+ // }
+
+ // // Process one block at a time in the workspace
+ // int blocksInWorkspace = slotsUsed / PULSE_PROCESSOR_N_SENSORS;
+ // for (int blockIndex = 0; blockIndex < blocksInWorkspace; blockIndex++) {
+ // int blockBaseIndex = blockIndex * PULSE_PROCESSOR_N_SENSORS;
+ // if (! processWorkspaceBlock(&pulseWorkspace->slots[blockBaseIndex], &blockWorkspace->blocks[blockIndex])) {
+ // // If one block is bad, reject the full workspace
+ // return 0;
+ // }
+ // }
+
+ // return blocksInWorkspace;
}
TESTABLE_STATIC bool storePulse(const pulseProcessorFrame_t* frameData, pulseProcessorV2PulseWorkspace_t* pulseWorkspace) {
- bool result = false;
- if (pulseWorkspace->slotsUsed < PULSE_PROCESSOR_N_WORKSPACE) {
- memcpy(&pulseWorkspace->slots[pulseWorkspace->slotsUsed], frameData, sizeof(pulseProcessorFrame_t));
- pulseWorkspace->slotsUsed += 1;
- result = true;
- }
-
- return result;
+ // bool result = false;
+ // if (pulseWorkspace->slotsUsed < PULSE_PROCESSOR_N_WORKSPACE) {
+ // memcpy(&pulseWorkspace->slots[pulseWorkspace->slotsUsed], frameData, sizeof(pulseProcessorFrame_t));
+ // pulseWorkspace->slotsUsed += 1;
+ // result = true;
+ // }
+
+ // return result;
}
TESTABLE_STATIC void clearWorkspace(pulseProcessorV2PulseWorkspace_t* pulseWorkspace) {
- pulseWorkspace->slotsUsed = 0;
+ // pulseWorkspace->slotsUsed = 0;
}
static bool processFrame(const pulseProcessorFrame_t* frameData, pulseProcessorV2PulseWorkspace_t* pulseWorkspace, pulseProcessorV2BlockWorkspace_t* blockWorkspace) {
- int nrOfBlocks = 0;
+ // int nrOfBlocks = 0;
- // Sensor timestamps may arrive in the wrong order, we need an abs() when checking the diff
- const bool isFirstFrameInNewWorkspace = TS_ABS_DIFF_LARGER_THAN(frameData->timestamp, pulseWorkspace->latestTimestamp, MAX_TICKS_SENSOR_TO_SENSOR);
- if (isFirstFrameInNewWorkspace) {
- nrOfBlocks = processWorkspace(pulseWorkspace, blockWorkspace);
- clearWorkspace(pulseWorkspace);
- }
+ // // Sensor timestamps may arrive in the wrong order, we need an abs() when checking the diff
+ // const bool isFirstFrameInNewWorkspace = TS_ABS_DIFF_LARGER_THAN(frameData->timestamp, pulseWorkspace->latestTimestamp, MAX_TICKS_SENSOR_TO_SENSOR);
+ // if (isFirstFrameInNewWorkspace) {
+ // nrOfBlocks = processWorkspace(pulseWorkspace, blockWorkspace);
+ // clearWorkspace(pulseWorkspace);
+ // }
- pulseWorkspace->latestTimestamp = frameData->timestamp;
+ // pulseWorkspace->latestTimestamp = frameData->timestamp;
- if (! storePulse(frameData, pulseWorkspace)) {
- clearWorkspace(pulseWorkspace);
- }
+ // if (! storePulse(frameData, pulseWorkspace)) {
+ // clearWorkspace(pulseWorkspace);
+ // }
- return nrOfBlocks;
+ // return nrOfBlocks;
}
void pulseProcessorV2ConvertToV1Angles(const float v2Angle1, const float v2Angle2, float* v1Angles) {
@@ -240,87 +240,87 @@ void pulseProcessorV2ConvertToV1Angles(const float v2Angle1, const float v2Angle
}
static void calculateAngles(const pulseProcessorV2SweepBlock_t* latestBlock, const pulseProcessorV2SweepBlock_t* previousBlock, pulseProcessorResult_t* angles) {
- const uint8_t channel = latestBlock->channel;
-
- for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) {
- uint32_t firstOffset = previousBlock->offset[i];
- uint32_t secondOffset = latestBlock->offset[i];
- uint32_t period = CYCLE_PERIODS[channel];
-
- float firstBeam = (firstOffset * 2 * M_PI_F / period) - M_PI_F + M_PI_F / 3.0f;
- float secondBeam = (secondOffset * 2 * M_PI_F / period) - M_PI_F - M_PI_F / 3.0f;
-
- pulseProcessorBaseStationMeasuremnt_t* measurement = &angles->sensorMeasurementsLh2[i].baseStatonMeasurements[channel];
- measurement->angles[0] = firstBeam;
- measurement->angles[1] = secondBeam;
- measurement->validCount = 2;
- }
- angles->lastUsecTimestamp[channel] = usecTimestamp();
+ // const uint8_t channel = latestBlock->channel;
+
+ // for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) {
+ // uint32_t firstOffset = previousBlock->offset[i];
+ // uint32_t secondOffset = latestBlock->offset[i];
+ // uint32_t period = CYCLE_PERIODS[channel];
+
+ // float firstBeam = (firstOffset * 2 * M_PI_F / period) - M_PI_F + M_PI_F / 3.0f;
+ // float secondBeam = (secondOffset * 2 * M_PI_F / period) - M_PI_F - M_PI_F / 3.0f;
+
+ // pulseProcessorBaseStationMeasuremnt_t* measurement = &angles->sensorMeasurementsLh2[i].baseStatonMeasurements[channel];
+ // measurement->angles[0] = firstBeam;
+ // measurement->angles[1] = secondBeam;
+ // measurement->validCount = 2;
+ // }
+ // angles->lastUsecTimestamp[channel] = usecTimestamp();
}
TESTABLE_STATIC bool isBlockPairGood(const pulseProcessorV2SweepBlock_t* latest, const pulseProcessorV2SweepBlock_t* storage) {
- if (latest->channel != storage->channel) {
- return false;
- }
+ // if (latest->channel != storage->channel) {
+ // return false;
+ // }
- if (TS_ABS_DIFF_LARGER_THAN(latest->timestamp0, storage->timestamp0, MAX_TICKS_BETWEEN_SWEEP_STARTS_TWO_BLOCKS)) {
- return false;
- }
+ // if (TS_ABS_DIFF_LARGER_THAN(latest->timestamp0, storage->timestamp0, MAX_TICKS_BETWEEN_SWEEP_STARTS_TWO_BLOCKS)) {
+ // return false;
+ // }
- return true;
+ // return true;
}
TESTABLE_STATIC bool handleCalibrationData(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData) {
- bool isFullMessage = false;
+ // bool isFullMessage = false;
- if (frameData->channelFound && frameData->channel < PULSE_PROCESSOR_N_BASE_STATIONS) {
- const uint8_t channel = frameData->channel;
- if (frameData->offset != NO_OFFSET) {
- const uint32_t prevTimestamp0 = state->v2.ootxTimestamps[channel];
- const uint32_t timestamp0 = TS_DIFF(frameData->timestamp, frameData->offset);
+ // if (frameData->channelFound && frameData->channel < PULSE_PROCESSOR_N_BASE_STATIONS) {
+ // const uint8_t channel = frameData->channel;
+ // if (frameData->offset != NO_OFFSET) {
+ // const uint32_t prevTimestamp0 = state->v2.ootxTimestamps[channel];
+ // const uint32_t timestamp0 = TS_DIFF(frameData->timestamp, frameData->offset);
- if (TS_ABS_DIFF_LARGER_THAN(timestamp0, prevTimestamp0, MIN_TICKS_BETWEEN_SLOW_BITS)) {
- isFullMessage = ootxDecoderProcessBit(&state->ootxDecoder[channel], frameData->slowbit);
- }
+ // if (TS_ABS_DIFF_LARGER_THAN(timestamp0, prevTimestamp0, MIN_TICKS_BETWEEN_SLOW_BITS)) {
+ // isFullMessage = ootxDecoderProcessBit(&state->ootxDecoder[channel], frameData->slowbit);
+ // }
- state->v2.ootxTimestamps[channel] = timestamp0;
- }
- }
+ // state->v2.ootxTimestamps[channel] = timestamp0;
+ // }
+ // }
- return isFullMessage;
+ // return isFullMessage;
}
bool handleAngles(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis) {
- bool anglesMeasured = false;
-
- clearStaleAnglesAfterTimeout(angles);
-
- int nrOfBlocks = processFrame(frameData, &state->v2.pulseWorkspace, &state->v2.blockWorkspace);
- for (int i = 0; i < nrOfBlocks; i++) {
- const pulseProcessorV2SweepBlock_t* block = &state->v2.blockWorkspace.blocks[i];
- const uint8_t channel = block->channel;
- if (channel < PULSE_PROCESSOR_N_BASE_STATIONS) {
- pulseProcessorV2SweepBlock_t* previousBlock = &state->v2.blocks[channel];
- if (isBlockPairGood(block, previousBlock)) {
- calculateAngles(block, previousBlock, angles);
-
- *baseStation = channel;
- *axis = sweepIdSecond;
- angles->measurementType = lighthouseBsTypeV2;
-
- anglesMeasured = true;
- } else {
- memcpy(previousBlock, block, sizeof(pulseProcessorV2SweepBlock_t));
- }
- }
- }
-
- return anglesMeasured;
+ // bool anglesMeasured = false;
+
+ // clearStaleAnglesAfterTimeout(angles);
+
+ // int nrOfBlocks = processFrame(frameData, &state->v2.pulseWorkspace, &state->v2.blockWorkspace);
+ // for (int i = 0; i < nrOfBlocks; i++) {
+ // const pulseProcessorV2SweepBlock_t* block = &state->v2.blockWorkspace.blocks[i];
+ // const uint8_t channel = block->channel;
+ // if (channel < PULSE_PROCESSOR_N_BASE_STATIONS) {
+ // pulseProcessorV2SweepBlock_t* previousBlock = &state->v2.blocks[channel];
+ // if (isBlockPairGood(block, previousBlock)) {
+ // calculateAngles(block, previousBlock, angles);
+
+ // *baseStation = channel;
+ // *axis = sweepIdSecond;
+ // angles->measurementType = lighthouseBsTypeV2;
+
+ // anglesMeasured = true;
+ // } else {
+ // memcpy(previousBlock, block, sizeof(pulseProcessorV2SweepBlock_t));
+ // }
+ // }
+ // }
+
+ // return anglesMeasured;
}
bool pulseProcessorV2ProcessPulse(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis, bool* calibDataIsDecoded) {
- *calibDataIsDecoded = handleCalibrationData(state, frameData);
- return handleAngles(state, frameData, angles, baseStation, axis);
+ // *calibDataIsDecoded = handleCalibrationData(state, frameData);
+ // return handleAngles(state, frameData, angles, baseStation, axis);
}
uint8_t pulseProcessorV2AnglesQuality() {
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Source/include/queue.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Source/include/queue.h
index a0439c5204014a5e74e3e109e01a74bc7e13d7b0..08525a41318e2be9bee2bff2cd48e79a81a1c8a0 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Source/include/queue.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Source/include/queue.h
@@ -229,7 +229,7 @@ typedef struct QueueDefinition * QueueSetMemberHandle_t;
* \ingroup QueueManagement
*/
#if( configSUPPORT_STATIC_ALLOCATION == 1 )
- #define xQueueCreateStatic( uxQueueLength, uxItemSize, pucQueueStorage, pxQueueBuffer ) xQueueGenericCreateStatic( ( uxQueueLength ), ( uxItemSize ), ( pucQueueStorage ), ( pxQueueBuffer ), ( queueQUEUE_TYPE_BASE ) )
+ //#define xQueueCreateStatic( uxQueueLength, uxItemSize, pucQueueStorage, pxQueueBuffer ) xQueueGenericCreateStatic( ( uxQueueLength ), ( uxItemSize ), ( pucQueueStorage ), ( pxQueueBuffer ), ( queueQUEUE_TYPE_BASE ) )
#endif /* configSUPPORT_STATIC_ALLOCATION */
/**
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Source/timers.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Source/timers.c
index bfcc4eaffc69d6d3009d5eb9b23d09e2ce3282f5..4b963c8d2202b20f32b6463dcfb852bf5ba3b7f9 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Source/timers.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Source/timers.c
@@ -959,7 +959,7 @@ static void prvCheckForValidListAndQueue( void )
static StaticQueue_t xStaticTimerQueue; /*lint !e956 Ok to declare in this manner to prevent additional conditional compilation guards in other locations. */
static uint8_t ucStaticTimerQueueStorage[ ( size_t ) configTIMER_QUEUE_LENGTH * sizeof( DaemonTaskMessage_t ) ]; /*lint !e956 Ok to declare in this manner to prevent additional conditional compilation guards in other locations. */
- xTimerQueue = xQueueCreateStatic( ( UBaseType_t ) configTIMER_QUEUE_LENGTH, ( UBaseType_t ) sizeof( DaemonTaskMessage_t ), &( ucStaticTimerQueueStorage[ 0 ] ), &xStaticTimerQueue );
+ //xTimerQueue = xQueueCreateStatic( ( UBaseType_t ) configTIMER_QUEUE_LENGTH, ( UBaseType_t ) sizeof( DaemonTaskMessage_t ), &( ucStaticTimerQueueStorage[ 0 ] ), &xStaticTimerQueue );
}
#else
{