diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck.c
index 7201c105b98afd1457a459e8791ab55179ea7a30..70576aab99f010bdaf0f8237a9f24ed1ecdb4572 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck.c
@@ -26,7 +26,8 @@
#define DEBUG_MODULE "DECK_CORE"
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#include "deck.h"
#include "deck_memory.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_drivers.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_drivers.c
index 8cd762ee63e445561008c07af78a4e0bae7f1f06..77fd37a8caa6dd8a951c70c2ada39ba7339d3fa9 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_drivers.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_drivers.c
@@ -27,7 +27,8 @@
#define DEBUG_MODULE "DECK_DRIVERS"
#include <stdlib.h>
-#include <string.h>
+//COMMENTED FIRMWARE
+// #include <string.h>
#include "deck.h"
#include "debug.h"
@@ -97,14 +98,15 @@ const DeckDriver* deckFindDriverByVidPid(uint8_t vid, uint8_t pid) {
}
const DeckDriver* deckFindDriverByName(char* name) {
- int i;
+ //COMMENTED FIRMWARE
+ // int i;
- deckdriversInit();
+ // deckdriversInit();
- for (i=0; i<driversLen; i++) {
- if (!strcmp(name, drivers[i]->name)) {
- return drivers[i];
- }
- }
+ // for (i=0; i<driversLen; i++) {
+ // if (!strcmp(name, drivers[i]->name)) {
+ // return drivers[i];
+ // }
+ // }
return NULL;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_info.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_info.c
index 351b68f5b1215ef2032839fd96b672b54008d6b6..1f30e8c26d5070f2d06ca708087f3789ab50962e 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_info.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_info.c
@@ -26,7 +26,8 @@
#include <stdlib.h>
#include <stdbool.h>
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#define DEBUG_MODULE "DECK_INFO"
@@ -99,16 +100,17 @@ static const DeckDriver * findDriver(DeckInfo *deck)
char name[30];
const DeckDriver *driver = &dummyDriver;
- deckTlvGetString(&deck->tlv, DECK_INFO_NAME, name, 30);
+ //COMMENTED FIRMWARE
+ // deckTlvGetString(&deck->tlv, DECK_INFO_NAME, name, 30);
- if (deck->vid) {
- driver = deckFindDriverByVidPid(deck->vid, deck->pid);
- } else if (strlen(name)>0) {
- driver = deckFindDriverByName(name);
- }
+ // if (deck->vid) {
+ // driver = deckFindDriverByVidPid(deck->vid, deck->pid);
+ // } else if (strlen(name)>0) {
+ // driver = deckFindDriverByName(name);
+ // }
- if (driver == NULL)
- driver = &dummyDriver;
+ // if (driver == NULL)
+ // driver = &dummyDriver;
return driver;
}
@@ -176,87 +178,88 @@ static bool infoDecode(DeckInfo * info)
static void enumerateDecks(void)
{
- uint8_t nDecks = 0;
- bool noError = true;
-
- owInit();
-
- if (owScan(&nDecks))
- {
- DECK_INFO_DBG_PRINT("Found %d deck memor%s.\n", nDecks, nDecks>1?"ies":"y");
- } else {
- DEBUG_PRINT("Error scanning for deck memories, "
- "no deck drivers will be initialised\n");
- nDecks = 0;
- }
-
-#ifndef IGNORE_OW_DECKS
- for (int i = 0; i < nDecks; i++)
- {
- DECK_INFO_DBG_PRINT("Enumerating deck %i\n", i);
- if (owRead(i, 0, sizeof(deckInfos[0].raw), (uint8_t *)&deckInfos[i]))
- {
- if (infoDecode(&deckInfos[i]))
- {
- deckInfos[i].driver = findDriver(&deckInfos[i]);
- printDeckInfo(&deckInfos[i]);
- } else {
-#ifdef DEBUG
- DEBUG_PRINT("Deck %i has corrupt OW memory. "
- "Ignoring the deck in DEBUG mode.\n", i);
- deckInfos[i].driver = &dummyDriver;
-#else
- DEBUG_PRINT("Deck %i has corrupt OW memory. "
- "No driver will be initialized!\n", i);
- noError = false;
-#endif
- }
- }
- else
- {
- DEBUG_PRINT("Reading deck nr:%d [FAILED]. "
- "No driver will be initialized!\n", i);
- noError = false;
- }
- }
-#else
- DEBUG_PRINT("Ignoring all OW decks because of compile flag.\n");
- nDecks = 0;
-#endif
-
- // Add build-forced driver
- if (strlen(deck_force) > 0) {
- DEBUG_PRINT("DECK_FORCE=%s found\n", deck_force);
- //split deck_force into multiple, separated by colons, if available
- char delim[] = ":";
-
- char temp_deck_force[strlen(deck_force) + 1];
- strcpy(temp_deck_force, deck_force);
- char * token = strtok(temp_deck_force, delim);
-
- while (token) {
- deck_force = token;
-
- const DeckDriver *driver = deckFindDriverByName(deck_force);
- if (!driver) {
- DEBUG_PRINT("WARNING: compile-time forced driver %s not found\n", deck_force);
- } else if (driver->init || driver->test) {
- if (nDecks <= DECK_MAX_COUNT)
- {
- nDecks++;
- deckInfos[nDecks - 1].driver = driver;
- DEBUG_PRINT("compile-time forced driver %s added\n", deck_force);
- } else {
- DEBUG_PRINT("WARNING: No room for compile-time forced driver\n");
- }
- }
- token = strtok(NULL, delim);
- }
- }
-
- if (noError) {
- count = nDecks;
- }
+ //COMMENTED FIRMWARE
+// uint8_t nDecks = 0;
+// bool noError = true;
+
+// owInit();
+
+// if (owScan(&nDecks))
+// {
+// DECK_INFO_DBG_PRINT("Found %d deck memor%s.\n", nDecks, nDecks>1?"ies":"y");
+// } else {
+// DEBUG_PRINT("Error scanning for deck memories, "
+// "no deck drivers will be initialised\n");
+// nDecks = 0;
+// }
+
+// #ifndef IGNORE_OW_DECKS
+// for (int i = 0; i < nDecks; i++)
+// {
+// DECK_INFO_DBG_PRINT("Enumerating deck %i\n", i);
+// if (owRead(i, 0, sizeof(deckInfos[0].raw), (uint8_t *)&deckInfos[i]))
+// {
+// if (infoDecode(&deckInfos[i]))
+// {
+// deckInfos[i].driver = findDriver(&deckInfos[i]);
+// printDeckInfo(&deckInfos[i]);
+// } else {
+// #ifdef DEBUG
+// DEBUG_PRINT("Deck %i has corrupt OW memory. "
+// "Ignoring the deck in DEBUG mode.\n", i);
+// deckInfos[i].driver = &dummyDriver;
+// #else
+// DEBUG_PRINT("Deck %i has corrupt OW memory. "
+// "No driver will be initialized!\n", i);
+// noError = false;
+// #endif
+// }
+// }
+// else
+// {
+// DEBUG_PRINT("Reading deck nr:%d [FAILED]. "
+// "No driver will be initialized!\n", i);
+// noError = false;
+// }
+// }
+// #else
+// DEBUG_PRINT("Ignoring all OW decks because of compile flag.\n");
+// nDecks = 0;
+// #endif
+
+// // Add build-forced driver
+// if (strlen(deck_force) > 0) {
+// DEBUG_PRINT("DECK_FORCE=%s found\n", deck_force);
+// //split deck_force into multiple, separated by colons, if available
+// char delim[] = ":";
+
+// char temp_deck_force[strlen(deck_force) + 1];
+// strcpy(temp_deck_force, deck_force);
+// char * token = strtok(temp_deck_force, delim);
+
+// while (token) {
+// deck_force = token;
+
+// const DeckDriver *driver = deckFindDriverByName(deck_force);
+// if (!driver) {
+// DEBUG_PRINT("WARNING: compile-time forced driver %s not found\n", deck_force);
+// } else if (driver->init || driver->test) {
+// if (nDecks <= DECK_MAX_COUNT)
+// {
+// nDecks++;
+// deckInfos[nDecks - 1].driver = driver;
+// DEBUG_PRINT("compile-time forced driver %s added\n", deck_force);
+// } else {
+// DEBUG_PRINT("WARNING: No room for compile-time forced driver\n");
+// }
+// }
+// token = strtok(NULL, delim);
+// }
+// }
+
+// if (noError) {
+// count = nDecks;
+// }
return;
}
@@ -311,25 +314,27 @@ bool deckTlvHasElement(TlvArea *tlv, int type) {
}
int deckTlvGetString(TlvArea *tlv, int type, char *string, int length) {
- int pos = findType(tlv, type);
- int strlength = 0;
+ //COMMENTED FIRMWARE
+ // int pos = findType(tlv, type);
+ // int strlength = 0;
- if (pos >= 0) {
- strlength = tlv->data[pos+1];
+ // if (pos >= 0) {
+ // strlength = tlv->data[pos+1];
- if (strlength > (length-1)) {
- strlength = length-1;
- }
+ // if (strlength > (length-1)) {
+ // strlength = length-1;
+ // }
- memcpy(string, &tlv->data[pos+2], strlength);
- string[strlength] = '\0';
+ // memcpy(string, &tlv->data[pos+2], strlength);
+ // string[strlength] = '\0';
- return strlength;
- } else {
- string[0] = '\0';
+ // return strlength;
+ // } else {
+ // string[0] = '\0';
- return -1;
- }
+ // return -1;
+ // }
+ return 0;
}
char* deckTlvGetBuffer(TlvArea *tlv, int type, int *length) {
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_memory.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_memory.c
index c3b1c62db936c55709b38b55362133fdb0abb5d3..d5fec8bb79d6371648267e6e2d6d644db0a0f2dd 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_memory.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_memory.c
@@ -28,7 +28,8 @@
*
*/
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#include "deck_memory.h"
#include "deck_core.h"
#include "mem.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_test.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_test.c
index 98eeeaf640c0ab2e3661c75cc773eeb2ca027032..31608a04d73bef17bd570ecfc0bcbe236ba0cd58 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_test.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/core/deck_test.c
@@ -23,7 +23,8 @@
*
* deck_test.c - Test utility functions for testing decks.
*/
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#include "stm32fxxx.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/aideck.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/aideck.c
index 9cb3bd45c56b476d8ae04de781634d9c5eceeee0..894f1d5c7edc32437a012ee12ca958d342ea9034 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/aideck.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/aideck.c
@@ -26,7 +26,8 @@
#define DEBUG_MODULE "AIDECK"
#include <stdint.h>
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#include <stdio.h>
#include "stm32fxxx.h"
@@ -38,7 +39,8 @@
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#include <stdlib.h>
#include <math.h>
#include "log.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/bigquad.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/bigquad.c
index 08c95ae5298ce9e2cf735e0e33d8286d813ceb1a..ad6eed88cde2024af6a15c7dc38ed9acf11a9f74 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/bigquad.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/bigquad.c
@@ -26,7 +26,8 @@
#define DEBUG_MODULE "BIGQUAD"
#include <stdint.h>
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#include "stm32fxxx.h"
#include "config.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/cppmdeck.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/cppmdeck.c
index de4bd7e51f39d5d8dc78aeb2d0ccff8c65ebb438..adb140e1e29c9279ce57ce8e04cfd5e8c3e4e3f7 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/cppmdeck.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/cppmdeck.c
@@ -26,7 +26,8 @@
#define DEBUG_MODULE "CPPM"
#include <stdint.h>
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#include "stm32fxxx.h"
#include "config.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/gtgps.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/gtgps.c
index 3deb2d9e360a019c34e1a2e81f5146b9e0a09cca..7bca41c2d1c716481e82b18f1143b1c6d9aaaf8d 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/gtgps.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/gtgps.c
@@ -26,7 +26,8 @@
#define DEBUG_MODULE "GTGPS"
#include <stdint.h>
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#include <stdio.h>
#include "stm32fxxx.h"
@@ -38,7 +39,8 @@
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#include <stdlib.h>
#include <math.h>
#include "log.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/ledring12.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/ledring12.c
index 8386b88a763c317f170f28ecad1ce85d1e1ce6f7..18a023097e515b44511217dd1e6bef9f6181c7df 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/ledring12.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/ledring12.c
@@ -26,7 +26,8 @@
#include <stdint.h>
#include <math.h>
-#include <string.h>
+//COMMENTED FIRMWARE
+//#include <string.h>
#include "stm32fxxx.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/usddeck.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/usddeck.c
index fc3cd4e62ac44a9713edbcd629db5fca0d4b13ff..707555a7d012cfa4b417655674598706bf357a5c 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/usddeck.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/usddeck.c
@@ -523,188 +523,189 @@ static void usddeckEventtriggerCallback(const eventtrigger *event)
static void usdLogTask(void* prm)
{
- TickType_t lastWakeTime = xTaskGetTickCount();
-
- DEBUG_PRINT("wait for sensors\n");
-
- systemWaitStart();
- /* wait until sensor calibration is done
- * (memory of bias calculation buffer is free again) */
- while(!sensorsAreCalibrated()) {
- vTaskDelayUntil(&lastWakeTime, F2T(10));
- }
-
- // loop to break out in case of errors
- while (true) {
- /* open config file */
- // loop to break out in case of errors
- while (f_open(&logFile, "config.txt", FA_READ) == FR_OK) {
- /* try to read configuration */
- char readBuffer[32];
- char* endptr;
-
- // version
- TCHAR* line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
- if (!line) break;
- int version = strtol(line, &endptr, 10);
- if (version != 1) break;
- // buffer size
- line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
- if (!line) break;
- usdLogConfig.bufferSize = strtol(line, &endptr, 10);
- // file name
- line = f_gets_without_comments(usdLogConfig.filename, sizeof(usdLogConfig.filename), &logFile);
- if (!line) break;
-
- int l = strlen(usdLogConfig.filename);
- if (l > sizeof(usdLogConfig.filename) - 3) {
- l = sizeof(usdLogConfig.filename) - 3;
- }
- usdLogConfig.filename[l] = '0';
- usdLogConfig.filename[l+1] = '0';
- usdLogConfig.filename[l+2] = 0;
-
- // enable on startup
- line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
- if (!line) break;
- usdLogConfig.enableOnStartup = strtol(line, &endptr, 10);
-
- // loop over event triggers "on:<name>"
- usdLogConfig.numEventConfigs = 0;
- usdLogConfig.fixedFrequencyEventIdx = MAX_USD_LOG_EVENTS;
- usdLogConfig.frequency = 10; // use non-zero default value for task loop below
- usdLogEventConfig_t *cfg = &usdLogConfig.eventConfigs[0];
- const char* eventName = 0;
- line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
- while (line) {
- if (strncmp(line, "on:", 3) == 0) {
- // special mode for non-event-based logging
- if (strcmp(&line[3], FIXED_FREQUENCY_EVENT_NAME) == 0) {
- // frequency
- line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
- if (!line) break;
- usdLogConfig.frequency = strtol(line, &endptr, 10);
- // mode
- line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
- if (!line) break;
- usdLogConfig.mode = strtol(line, &endptr, 10);
- cfg->eventId = FIXED_FREQUENCY_EVENT_ID;
- eventName = FIXED_FREQUENCY_EVENT_NAME;
- usdLogConfig.fixedFrequencyEventIdx = usdLogConfig.numEventConfigs;
- } else {
- // handle event triggers
- const eventtrigger *et = eventtriggerGetByName(&line[3]);
- if (et) {
- cfg->eventId = eventtriggerGetId(et);
- eventName = et->name;
- } else {
- DEBUG_PRINT("Unknown event %s\n", &line[3]);
- line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
- continue;
- }
- }
-
- // Add log variables
- cfg->numVars = 0;
- cfg->numBytes = 0;
- while (true) {
- line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
- if (!line || strncmp(line, "on:", 3) == 0)
- break;
- char *group = line;
- char *name = 0;
- for (int i = 0; i < strlen(line); ++i) {
- if (line[i] == '.') {
- line[i] = 0;
- name = &line[i + 1];
- i = strlen(name);
- break;
- }
- }
- logVarId_t varid = logGetVarId(group, name);
- if (!logVarIdIsValid(varid)) {
- DEBUG_PRINT("Unknown log variable %s.%s\n", group, name);
- continue;
- }
- if (cfg->numVars < MAX_USD_LOG_VARIABLES_PER_EVENT) {
- cfg->varIds[cfg->numVars] = varid;
- ++cfg->numVars;
- cfg->numBytes += logVarSize(logGetType(varid));
- } else {
- DEBUG_PRINT("Skip log variable %s: %s.%s (out of storage)\n", eventName, group, name);
- continue;
- }
- }
- if (usdLogConfig.numEventConfigs < MAX_USD_LOG_EVENTS - 1) {
- ++usdLogConfig.numEventConfigs;
- cfg = &usdLogConfig.eventConfigs[usdLogConfig.numEventConfigs];
- } else {
- DEBUG_PRINT("Skip config after event %s (out of storage)\n", eventName);
- break;
- }
- } else {
- line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
- }
- }
- f_close(&logFile);
-
- eventtriggerRegisterCallback(eventtriggerHandler_USD, &usddeckEventtriggerCallback);
-
- DEBUG_PRINT("Config read [OK].\n");
- // DEBUG_PRINT("Frequency: %d Hz. Buffer size: %d\n",
- // usdLogConfig.frequency, usdLogConfig.bufferSize);
- // DEBUG_PRINT("enOnStartup: %d. mode: %d\n", usdLogConfig.enableOnStartup, usdLogConfig.mode);
- // DEBUG_PRINT("slots: %d, %d\n", usdLogConfig.numSlots, usdLogConfig.numBytes);
- initSuccess = true;
- break;
- }
-
- if (!initSuccess) {
- DEBUG_PRINT("Config read [FAIL].\n");
- break;
- }
-
- /* allocate memory for buffer */
- DEBUG_PRINT("malloc buffer %d bytes ", usdLogConfig.bufferSize);
- // vTaskDelay(10); // small delay to allow debug message to be send
- uint8_t* logBufferData = pvPortMalloc(usdLogConfig.bufferSize);
- if (logBufferData) {
- DEBUG_PRINT("[OK].\n");
- } else {
- DEBUG_PRINT("[FAIL].\n");
- break;
- }
- ringBuffer_init(&logBuffer, logBufferData, usdLogConfig.bufferSize);
-
- /* create queue to hand over pointer to usdLogData */
- // usdLogQueue = xQueueCreate(usdLogConfig.queueSize, sizeof(uint8_t*));
-
- xHandleWriteTask = 0;
- enableLogging = usdLogConfig.enableOnStartup; // enable logging if desired
-
- /* create usd-write task */
- xTaskCreate(usdWriteTask, USDWRITE_TASK_NAME,
- USDWRITE_TASK_STACKSIZE, 0,
- USDWRITE_TASK_PRI, &xHandleWriteTask);
-
- bool lastEnableLogging = enableLogging;
- while(1) {
- vTaskDelayUntil(&lastWakeTime, F2T(usdLogConfig.frequency));
-
- // if logging was just disabled, resume the writer task to give up mutex
- if (!enableLogging && lastEnableLogging != enableLogging) {
- vTaskResume(xHandleWriteTask);
- }
-
- if (enableLogging && usdLogConfig.mode == usddeckLoggingMode_Asynchronous) {
- usddeckTriggerLogging();
- }
- lastEnableLogging = enableLogging;
- }
- }
-
- /* something went wrong */
- vTaskDelete(NULL);
+ //COMMENTED FIRMWARE
+ // TickType_t lastWakeTime = xTaskGetTickCount();
+
+ // DEBUG_PRINT("wait for sensors\n");
+
+ // systemWaitStart();
+ // /* wait until sensor calibration is done
+ // * (memory of bias calculation buffer is free again) */
+ // while(!sensorsAreCalibrated()) {
+ // vTaskDelayUntil(&lastWakeTime, F2T(10));
+ // }
+
+ // // loop to break out in case of errors
+ // while (true) {
+ // /* open config file */
+ // // loop to break out in case of errors
+ // while (f_open(&logFile, "config.txt", FA_READ) == FR_OK) {
+ // /* try to read configuration */
+ // char readBuffer[32];
+ // char* endptr;
+
+ // // version
+ // TCHAR* line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+ // if (!line) break;
+ // int version = strtol(line, &endptr, 10);
+ // if (version != 1) break;
+ // // buffer size
+ // line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+ // if (!line) break;
+ // usdLogConfig.bufferSize = strtol(line, &endptr, 10);
+ // // file name
+ // line = f_gets_without_comments(usdLogConfig.filename, sizeof(usdLogConfig.filename), &logFile);
+ // if (!line) break;
+
+ // int l = strlen(usdLogConfig.filename);
+ // if (l > sizeof(usdLogConfig.filename) - 3) {
+ // l = sizeof(usdLogConfig.filename) - 3;
+ // }
+ // usdLogConfig.filename[l] = '0';
+ // usdLogConfig.filename[l+1] = '0';
+ // usdLogConfig.filename[l+2] = 0;
+
+ // // enable on startup
+ // line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+ // if (!line) break;
+ // usdLogConfig.enableOnStartup = strtol(line, &endptr, 10);
+
+ // // loop over event triggers "on:<name>"
+ // usdLogConfig.numEventConfigs = 0;
+ // usdLogConfig.fixedFrequencyEventIdx = MAX_USD_LOG_EVENTS;
+ // usdLogConfig.frequency = 10; // use non-zero default value for task loop below
+ // usdLogEventConfig_t *cfg = &usdLogConfig.eventConfigs[0];
+ // const char* eventName = 0;
+ // line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+ // while (line) {
+ // if (strncmp(line, "on:", 3) == 0) {
+ // // special mode for non-event-based logging
+ // if (strcmp(&line[3], FIXED_FREQUENCY_EVENT_NAME) == 0) {
+ // // frequency
+ // line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+ // if (!line) break;
+ // usdLogConfig.frequency = strtol(line, &endptr, 10);
+ // // mode
+ // line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+ // if (!line) break;
+ // usdLogConfig.mode = strtol(line, &endptr, 10);
+ // cfg->eventId = FIXED_FREQUENCY_EVENT_ID;
+ // eventName = FIXED_FREQUENCY_EVENT_NAME;
+ // usdLogConfig.fixedFrequencyEventIdx = usdLogConfig.numEventConfigs;
+ // } else {
+ // // handle event triggers
+ // const eventtrigger *et = eventtriggerGetByName(&line[3]);
+ // if (et) {
+ // cfg->eventId = eventtriggerGetId(et);
+ // eventName = et->name;
+ // } else {
+ // DEBUG_PRINT("Unknown event %s\n", &line[3]);
+ // line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+ // continue;
+ // }
+ // }
+
+ // // Add log variables
+ // cfg->numVars = 0;
+ // cfg->numBytes = 0;
+ // while (true) {
+ // line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+ // if (!line || strncmp(line, "on:", 3) == 0)
+ // break;
+ // char *group = line;
+ // char *name = 0;
+ // for (int i = 0; i < strlen(line); ++i) {
+ // if (line[i] == '.') {
+ // line[i] = 0;
+ // name = &line[i + 1];
+ // i = strlen(name);
+ // break;
+ // }
+ // }
+ // logVarId_t varid = logGetVarId(group, name);
+ // if (!logVarIdIsValid(varid)) {
+ // DEBUG_PRINT("Unknown log variable %s.%s\n", group, name);
+ // continue;
+ // }
+ // if (cfg->numVars < MAX_USD_LOG_VARIABLES_PER_EVENT) {
+ // cfg->varIds[cfg->numVars] = varid;
+ // ++cfg->numVars;
+ // cfg->numBytes += logVarSize(logGetType(varid));
+ // } else {
+ // DEBUG_PRINT("Skip log variable %s: %s.%s (out of storage)\n", eventName, group, name);
+ // continue;
+ // }
+ // }
+ // if (usdLogConfig.numEventConfigs < MAX_USD_LOG_EVENTS - 1) {
+ // ++usdLogConfig.numEventConfigs;
+ // cfg = &usdLogConfig.eventConfigs[usdLogConfig.numEventConfigs];
+ // } else {
+ // DEBUG_PRINT("Skip config after event %s (out of storage)\n", eventName);
+ // break;
+ // }
+ // } else {
+ // line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+ // }
+ // }
+ // f_close(&logFile);
+
+ // eventtriggerRegisterCallback(eventtriggerHandler_USD, &usddeckEventtriggerCallback);
+
+ // DEBUG_PRINT("Config read [OK].\n");
+ // // DEBUG_PRINT("Frequency: %d Hz. Buffer size: %d\n",
+ // // usdLogConfig.frequency, usdLogConfig.bufferSize);
+ // // DEBUG_PRINT("enOnStartup: %d. mode: %d\n", usdLogConfig.enableOnStartup, usdLogConfig.mode);
+ // // DEBUG_PRINT("slots: %d, %d\n", usdLogConfig.numSlots, usdLogConfig.numBytes);
+ // initSuccess = true;
+ // break;
+ // }
+
+ // if (!initSuccess) {
+ // DEBUG_PRINT("Config read [FAIL].\n");
+ // break;
+ // }
+
+ // /* allocate memory for buffer */
+ // DEBUG_PRINT("malloc buffer %d bytes ", usdLogConfig.bufferSize);
+ // // vTaskDelay(10); // small delay to allow debug message to be send
+ // uint8_t* logBufferData = pvPortMalloc(usdLogConfig.bufferSize);
+ // if (logBufferData) {
+ // DEBUG_PRINT("[OK].\n");
+ // } else {
+ // DEBUG_PRINT("[FAIL].\n");
+ // break;
+ // }
+ // ringBuffer_init(&logBuffer, logBufferData, usdLogConfig.bufferSize);
+
+ // /* create queue to hand over pointer to usdLogData */
+ // // usdLogQueue = xQueueCreate(usdLogConfig.queueSize, sizeof(uint8_t*));
+
+ // xHandleWriteTask = 0;
+ // enableLogging = usdLogConfig.enableOnStartup; // enable logging if desired
+
+ // /* create usd-write task */
+ // xTaskCreate(usdWriteTask, USDWRITE_TASK_NAME,
+ // USDWRITE_TASK_STACKSIZE, 0,
+ // USDWRITE_TASK_PRI, &xHandleWriteTask);
+
+ // bool lastEnableLogging = enableLogging;
+ // while(1) {
+ // vTaskDelayUntil(&lastWakeTime, F2T(usdLogConfig.frequency));
+
+ // // if logging was just disabled, resume the writer task to give up mutex
+ // if (!enableLogging && lastEnableLogging != enableLogging) {
+ // vTaskResume(xHandleWriteTask);
+ // }
+
+ // if (enableLogging && usdLogConfig.mode == usddeckLoggingMode_Asynchronous) {
+ // usddeckTriggerLogging();
+ // }
+ // lastEnableLogging = enableLogging;
+ // }
+ // }
+
+ // /* something went wrong */
+ // vTaskDelete(NULL);
}
bool usddeckLoggingEnabled(void)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_deck_flasher.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_deck_flasher.c
index 2cac9e757e11f13c72b1beb1e3240cb91e22c9b6..48b5ec717d8c20f2ff85747fd2a5020177932b4e 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_deck_flasher.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_deck_flasher.c
@@ -55,59 +55,62 @@ static bool inBootloaderMode = true;
static bool hasStarted = false;
bool lighthouseDeckFlasherCheckVersionAndBoot() {
- lhblInit();
-
- #ifdef LH_FLASH_BOOTLOADER
- // Flash deck bootloader using SPI (factory and recovery flashing)
- lhflashInit();
- lhflashFlashBootloader();
- #endif
-
- uint8_t bootloaderVersion = 0;
- if (lhblGetVersion(&bootloaderVersion) == false) {
- DEBUG_PRINT("Cannot communicate with lighthouse bootloader, aborting!\n");
- return false;
- }
- DEBUG_PRINT("Lighthouse bootloader version: %d\n", bootloaderVersion);
-
- // Wakeup mem
- lhblFlashWakeup();
- vTaskDelay(M2T(1));
-
- // Decoding bitstream version for console
- static char deckBitstream[READ_BUFFER_LENGTH];
- lhblFlashRead(LH_FW_ADDR, VERSION_STRING_MAX_LENGTH, (uint8_t*)deckBitstream);
- deckBitstream[VERSION_STRING_MAX_LENGTH] = 0;
- int deckVersion = strtol(&deckBitstream[2], NULL, 10);
-
- // Checking that the bitstream has the right checksum
- crc32Context_t crcContext;
- crc32ContextInit(&crcContext);
-
- for (int i=0; i<=LIGHTHOUSE_BITSTREAM_SIZE; i+=READ_BUFFER_LENGTH) {
- int length = ((i + READ_BUFFER_LENGTH) < LIGHTHOUSE_BITSTREAM_SIZE)?READ_BUFFER_LENGTH:LIGHTHOUSE_BITSTREAM_SIZE-i;
- lhblFlashRead(LH_FW_ADDR + i, length, (uint8_t*)deckBitstream);
- crc32Update(&crcContext, deckBitstream, length);
- }
-
- uint32_t crc = crc32Out(&crcContext);
- bool pass = crc == LIGHTHOUSE_BITSTREAM_CRC;
- DEBUG_PRINT("Bitstream CRC32: %x %s\n", (int)crc, pass?"[PASS]":"[FAIL]");
-
- // Launch LH deck FW
- if (pass) {
- DEBUG_PRINT("Firmware version %d verified, booting deck!\n", deckVersion);
- lhblBootToFW();
- inBootloaderMode = false;
- } else {
- DEBUG_PRINT("The deck bitstream does not match the required bitstream.\n");
- DEBUG_PRINT("We require lighthouse bitstream of size %d and CRC32 %x.\n", LIGHTHOUSE_BITSTREAM_SIZE, LIGHTHOUSE_BITSTREAM_CRC);
- DEBUG_PRINT("Leaving the deck in bootloader mode ...\n");
- }
-
- hasStarted = true;
-
- return pass;
+ // lhblInit();
+
+ // #ifdef LH_FLASH_BOOTLOADER
+ // // Flash deck bootloader using SPI (factory and recovery flashing)
+ // lhflashInit();
+ // lhflashFlashBootloader();
+ // #endif
+
+ // uint8_t bootloaderVersion = 0;
+ // if (lhblGetVersion(&bootloaderVersion) == false) {
+ // DEBUG_PRINT("Cannot communicate with lighthouse bootloader, aborting!\n");
+ // return false;
+ // }
+ // DEBUG_PRINT("Lighthouse bootloader version: %d\n", bootloaderVersion);
+
+ // // Wakeup mem
+ // lhblFlashWakeup();
+ // vTaskDelay(M2T(1));
+
+ // // Decoding bitstream version for console
+ // static char deckBitstream[READ_BUFFER_LENGTH];
+ // lhblFlashRead(LH_FW_ADDR, VERSION_STRING_MAX_LENGTH, (uint8_t*)deckBitstream);
+ // deckBitstream[VERSION_STRING_MAX_LENGTH] = 0;
+ // //COMMENTED FIRMWARE
+ // //int deckVersion = strtol(&deckBitstream[2], NULL, 10);
+
+ // // Checking that the bitstream has the right checksum
+ // crc32Context_t crcContext;
+ // crc32ContextInit(&crcContext);
+
+ // for (int i=0; i<=LIGHTHOUSE_BITSTREAM_SIZE; i+=READ_BUFFER_LENGTH) {
+ // int length = ((i + READ_BUFFER_LENGTH) < LIGHTHOUSE_BITSTREAM_SIZE)?READ_BUFFER_LENGTH:LIGHTHOUSE_BITSTREAM_SIZE-i;
+ // lhblFlashRead(LH_FW_ADDR + i, length, (uint8_t*)deckBitstream);
+ // crc32Update(&crcContext, deckBitstream, length);
+ // }
+
+ // uint32_t crc = crc32Out(&crcContext);
+ // bool pass = crc == LIGHTHOUSE_BITSTREAM_CRC;
+ // DEBUG_PRINT("Bitstream CRC32: %x %s\n", (int)crc, pass?"[PASS]":"[FAIL]");
+
+ // // Launch LH deck FW
+ // if (pass) {
+ // //COMMENTED FIRMWARE
+ // //DEBUG_PRINT("Firmware version %d verified, booting deck!\n", deckVersion);
+ // lhblBootToFW();
+ // inBootloaderMode = false;
+ // } else {
+ // DEBUG_PRINT("The deck bitstream does not match the required bitstream.\n");
+ // DEBUG_PRINT("We require lighthouse bitstream of size %d and CRC32 %x.\n", LIGHTHOUSE_BITSTREAM_SIZE, LIGHTHOUSE_BITSTREAM_CRC);
+ // DEBUG_PRINT("Leaving the deck in bootloader mode ...\n");
+ // }
+
+ // hasStarted = true;
+
+ // return pass;
+ return true;
}
bool lighthouseDeckFlasherRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_position_est.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_position_est.c
index 2fdf61641823cd9e0d80a3211c2841dfaf139e40..c2f2bf5e6fbf5190d7ef27a8a743869988e85406 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_position_est.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_position_est.c
@@ -182,23 +182,24 @@ void lighthousePositionSetGeometryData(const uint8_t baseStation, const baseStat
}
static void preProcessGeometryData(mat3d bsRot, mat3d bsRotInverted, mat3d lh1Rotor2Rot, mat3d lh1Rotor2RotInverted) {
- // For a rotation matrix inverse and transpose is equal. Use transpose instead
- arm_matrix_instance_f32 bsRot_ = {3, 3, (float32_t *)bsRot};
- arm_matrix_instance_f32 bsRotInverted_ = {3, 3, (float32_t *)bsRotInverted};
- mat_trans(&bsRot_, &bsRotInverted_);
-
- // In a LH1 system, the axis of rotation of the second rotor is perpendicular to the first rotor
- mat3d secondRotorInvertedR = {
- {1, 0, 0},
- {0, 0, -1},
- {0, 1, 0}
- };
- arm_matrix_instance_f32 secondRotorInvertedR_ = {3, 3, (float32_t *)secondRotorInvertedR};
- arm_matrix_instance_f32 lh1Rotor2Rot_ = {3, 3, (float32_t *)lh1Rotor2Rot};
- mat_mult(&bsRot_, &secondRotorInvertedR_, &lh1Rotor2Rot_);
-
- arm_matrix_instance_f32 lh1Rotor2RotInverted_ = {3, 3, (float32_t *)lh1Rotor2RotInverted};
- mat_trans(&lh1Rotor2Rot_, &lh1Rotor2RotInverted_);
+ //COMMENTED FIRMWARE
+ // // For a rotation matrix inverse and transpose is equal. Use transpose instead
+ // arm_matrix_instance_f32 bsRot_ = {3, 3, (float32_t *)bsRot};
+ // arm_matrix_instance_f32 bsRotInverted_ = {3, 3, (float32_t *)bsRotInverted};
+ // mat_trans(&bsRot_, &bsRotInverted_);
+
+ // // In a LH1 system, the axis of rotation of the second rotor is perpendicular to the first rotor
+ // mat3d secondRotorInvertedR = {
+ // {1, 0, 0},
+ // {0, 0, -1},
+ // {0, 1, 0}
+ // };
+ // arm_matrix_instance_f32 secondRotorInvertedR_ = {3, 3, (float32_t *)secondRotorInvertedR};
+ // arm_matrix_instance_f32 lh1Rotor2Rot_ = {3, 3, (float32_t *)lh1Rotor2Rot};
+ // mat_mult(&bsRot_, &secondRotorInvertedR_, &lh1Rotor2Rot_);
+
+ // arm_matrix_instance_f32 lh1Rotor2RotInverted_ = {3, 3, (float32_t *)lh1Rotor2RotInverted};
+ // mat_trans(&lh1Rotor2Rot_, &lh1Rotor2RotInverted_);
}
@@ -369,74 +370,76 @@ static void estimatePositionSweeps(const pulseProcessor_t* appState, pulseProces
}
}
-static bool estimateYawDeltaOneBaseStation(const int bs, const pulseProcessorResult_t* angles, const baseStationGeometry_t baseStationGeometries[], const float cfPos[3], const float n[3], const arm_matrix_instance_f32 *RR, float *yawDelta) {
- const baseStationGeometry_t* baseStationGeometry = &baseStationGeometries[bs];
-
- vec3d baseStationPos;
- lighthouseGeometryGetBaseStationPosition(baseStationGeometry, baseStationPos);
-
- vec3d rays[PULSE_PROCESSOR_N_SENSORS];
- for (int sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
- const pulseProcessorBaseStationMeasuremnt_t* bsMeasurement = &angles->sensorMeasurementsLh1[sensor].baseStatonMeasurements[bs];
- lighthouseGeometryGetRay(baseStationGeometry, bsMeasurement->correctedAngles[0], bsMeasurement->correctedAngles[1], rays[sensor]);
- }
-
- // Intersection points of rays and the deck
- vec3d intersectionPoints[PULSE_PROCESSOR_N_SENSORS];
- for (int sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
- bool exists = lighthouseGeometryIntersectionPlaneVector(baseStationPos, rays[sensor], cfPos, n, intersectionPoints[sensor]);
- if (! exists) {
- return false;
- }
- }
-
- // Calculate positions of sensors. Rotate relative postiions using the rotation matrix and add current position
- vec3d sensorPoints[PULSE_PROCESSOR_N_SENSORS];
- for (int sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
- lighthouseGeometryGetSensorPosition(cfPos, RR, sensorDeckPositions[sensor], sensorPoints[sensor]);
- }
-
- // Calculate diagonals (sensors 0 - 3 and 1 - 2) for intersection and sensor points
- vec3d ipv1 = {intersectionPoints[3][0] - intersectionPoints[0][0], intersectionPoints[3][1] - intersectionPoints[0][1], intersectionPoints[3][2] - intersectionPoints[0][2]};
- vec3d ipv2 = {intersectionPoints[2][0] - intersectionPoints[1][0], intersectionPoints[2][1] - intersectionPoints[1][1], intersectionPoints[2][2] - intersectionPoints[1][2]};
- vec3d spv1 = {sensorPoints[3][0] - sensorPoints[0][0], sensorPoints[3][1] - sensorPoints[0][1], sensorPoints[3][2] - sensorPoints[0][2]};
- vec3d spv2 = {sensorPoints[2][0] - sensorPoints[1][0], sensorPoints[2][1] - sensorPoints[1][1], sensorPoints[2][2] - sensorPoints[1][2]};
-
- // Calculate yaw delta for the two diagonals and average
- float yawDelta1, yawDelta2;
- if (lighthouseGeometryYawDelta(ipv1, spv1, n, &yawDelta1) && lighthouseGeometryYawDelta(ipv2, spv2, n, &yawDelta2)) {
- *yawDelta = (yawDelta1 + yawDelta2) / 2.0f;
- return true;
- } else {
- *yawDelta = 0.0f;
- return false;
- }
-}
+//COMMENTED FIRMWARE
+// static bool estimateYawDeltaOneBaseStation(const int bs, const pulseProcessorResult_t* angles, const baseStationGeometry_t baseStationGeometries[], const float cfPos[3], const float n[3], const arm_matrix_instance_f32 *RR, float *yawDelta) {
+// const baseStationGeometry_t* baseStationGeometry = &baseStationGeometries[bs];
+
+// vec3d baseStationPos;
+// lighthouseGeometryGetBaseStationPosition(baseStationGeometry, baseStationPos);
+
+// vec3d rays[PULSE_PROCESSOR_N_SENSORS];
+// for (int sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
+// const pulseProcessorBaseStationMeasuremnt_t* bsMeasurement = &angles->sensorMeasurementsLh1[sensor].baseStatonMeasurements[bs];
+// lighthouseGeometryGetRay(baseStationGeometry, bsMeasurement->correctedAngles[0], bsMeasurement->correctedAngles[1], rays[sensor]);
+// }
+
+// // Intersection points of rays and the deck
+// vec3d intersectionPoints[PULSE_PROCESSOR_N_SENSORS];
+// for (int sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
+// bool exists = lighthouseGeometryIntersectionPlaneVector(baseStationPos, rays[sensor], cfPos, n, intersectionPoints[sensor]);
+// if (! exists) {
+// return false;
+// }
+// }
+
+// // Calculate positions of sensors. Rotate relative postiions using the rotation matrix and add current position
+// vec3d sensorPoints[PULSE_PROCESSOR_N_SENSORS];
+// for (int sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
+// lighthouseGeometryGetSensorPosition(cfPos, RR, sensorDeckPositions[sensor], sensorPoints[sensor]);
+// }
+
+// // Calculate diagonals (sensors 0 - 3 and 1 - 2) for intersection and sensor points
+// vec3d ipv1 = {intersectionPoints[3][0] - intersectionPoints[0][0], intersectionPoints[3][1] - intersectionPoints[0][1], intersectionPoints[3][2] - intersectionPoints[0][2]};
+// vec3d ipv2 = {intersectionPoints[2][0] - intersectionPoints[1][0], intersectionPoints[2][1] - intersectionPoints[1][1], intersectionPoints[2][2] - intersectionPoints[1][2]};
+// vec3d spv1 = {sensorPoints[3][0] - sensorPoints[0][0], sensorPoints[3][1] - sensorPoints[0][1], sensorPoints[3][2] - sensorPoints[0][2]};
+// vec3d spv2 = {sensorPoints[2][0] - sensorPoints[1][0], sensorPoints[2][1] - sensorPoints[1][1], sensorPoints[2][2] - sensorPoints[1][2]};
+
+// // Calculate yaw delta for the two diagonals and average
+// float yawDelta1, yawDelta2;
+// if (lighthouseGeometryYawDelta(ipv1, spv1, n, &yawDelta1) && lighthouseGeometryYawDelta(ipv2, spv2, n, &yawDelta2)) {
+// *yawDelta = (yawDelta1 + yawDelta2) / 2.0f;
+// return true;
+// } else {
+// *yawDelta = 0.0f;
+// return false;
+// }
+// }
static void estimateYaw(const pulseProcessor_t *state, pulseProcessorResult_t* angles, int baseStation) {
// TODO Most of these calculations should be moved into the estimator instead. It is a
// bit dirty to get the state from the kalman filer here and calculate the yaw error outside
// the estimator, but it will do for now.
+ //COMMENTED FIRMWARE
// Get data from the current estimated state
- point_t cfPosP;
- estimatorKalmanGetEstimatedPos(&cfPosP);
- vec3d cfPos = {cfPosP.x, cfPosP.y, cfPosP.z};
-
- // Rotation matrix
- float R[3][3];
- estimatorKalmanGetEstimatedRot((float*)R);
- arm_matrix_instance_f32 RR = {3, 3, (float*)R};
-
- // Normal to the deck: (0, 0, 1), rotated using the rotation matrix
- const vec3d n = {R[0][2], R[1][2], R[2][2]};
-
- // Calculate yaw delta using only one base station for now
- float yawDelta;
- if (estimateYawDeltaOneBaseStation(baseStation, angles, state->bsGeometry, cfPos, n, &RR, &yawDelta)) {
- yawErrorMeasurement_t yawDeltaMeasurement = {.yawError = yawDelta, .stdDev = 0.01};
- estimatorEnqueueYawError(&yawDeltaMeasurement);
- }
+ // point_t cfPosP;
+ // estimatorKalmanGetEstimatedPos(&cfPosP);
+ // vec3d cfPos = {cfPosP.x, cfPosP.y, cfPosP.z};
+
+ // // Rotation matrix
+ // float R[3][3];
+ // estimatorKalmanGetEstimatedRot((float*)R);
+ // arm_matrix_instance_f32 RR = {3, 3, (float*)R};
+
+ // // Normal to the deck: (0, 0, 1), rotated using the rotation matrix
+ // const vec3d n = {R[0][2], R[1][2], R[2][2]};
+
+ // // Calculate yaw delta using only one base station for now
+ // float yawDelta;
+ // if (estimateYawDeltaOneBaseStation(baseStation, angles, state->bsGeometry, cfPos, n, &RR, &yawDelta)) {
+ // yawErrorMeasurement_t yawDeltaMeasurement = {.yawError = yawDelta, .stdDev = 0.01};
+ // estimatorEnqueueYawError(&yawDeltaMeasurement);
+ // }
}
void lighthousePositionEstimatePoseCrossingBeams(const pulseProcessor_t *state, pulseProcessorResult_t* angles, int baseStation) {
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_storage.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_storage.c
index 966cd5037750be1b95f2970945166d73325b3132..c33a849dc6102b6f0e2da1658be4b9bae1a7172b 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_storage.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_storage.c
@@ -51,13 +51,14 @@ static lighthouseCalibration_t calibBuffer;
static void generateStorageKey(char* buf, const char* base, const uint8_t baseStation) {
+ //COMMENTED FIRMWARE
// TOOD make an implementation that supports baseStations with 2 digits
- ASSERT(baseStation <= 9);
+ // ASSERT(baseStation <= 9);
- const int baseLen = strlen(base);
- memcpy(buf, base, baseLen);
- buf[baseLen] = '0' + baseStation;
- buf[baseLen + 1] = '\0';
+ // const int baseLen = strlen(base);
+ // memcpy(buf, base, baseLen);
+ // buf[baseLen] = '0' + baseStation;
+ // buf[baseLen + 1] = '\0';
}
bool lighthouseStoragePersistData(const uint8_t baseStation, const bool geoData, const bool calibData) {
@@ -99,19 +100,21 @@ void lighthouseStoragePersistSystemType(lighthouseBaseStationType_t type) {
}
void lighthouseStorageVerifySetStorageVersion() {
- const int bufLen = 5;
- char buffer[bufLen];
-
- const size_t fetched = storageFetch(STORAGE_VERSION_KEY, buffer, bufLen);
- if (fetched == 0) {
- storageStore(STORAGE_VERSION_KEY, CURRENT_STORAGE_VERSION, strlen(CURRENT_STORAGE_VERSION) + 1);
- } else {
- if (strcmp(buffer, CURRENT_STORAGE_VERSION) != 0) {
- // The storage format version is wrong! What to do?
- // No need to handle until we bump the storage version, assert for now.
- ASSERT_FAILED();
- }
- }
+ //COMMENTED FIRMWARE
+//#include <string.h>
+ // const int bufLen = 5;
+ // char buffer[bufLen];
+
+ // const size_t fetched = storageFetch(STORAGE_VERSION_KEY, buffer, bufLen);
+ // if (fetched == 0) {
+ // storageStore(STORAGE_VERSION_KEY, CURRENT_STORAGE_VERSION, strlen(CURRENT_STORAGE_VERSION) + 1);
+ // } else {
+ // if (strcmp(buffer, CURRENT_STORAGE_VERSION) != 0) {
+ // // The storage format version is wrong! What to do?
+ // // No need to handle until we bump the storage version, assert for now.
+ // ASSERT_FAILED();
+ // }
+ // }
}
void lighthouseStorageInitializeGeoDataFromStorage() {
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/Makefile b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/Makefile
index b9898090f9d4c5ed65fb4f550e2a047140e9170d..92cbb4ff523009f75a580ac2d2e3363838cb677f 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/Makefile
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/Makefile
@@ -233,7 +233,7 @@ OBJS += build/exptestBolt.o
# Utilities
OBJS += build/filter.o build/cpuid.o build/cfassert.o build/eprintf.o build/crc32.o build/num.o build/debug.o
-OBJS += build/version.o build/FreeRTOS-openocd.o
+OBJS += build/FreeRTOS-openocd.o #build/version.o
OBJS += build/configblockeeprom.o
OBJS += build/sleepus.o build/statsCnt.o build/rateSupervisor.o
OBJS += build/lighthouse_core.o build/pulse_processor.o build/pulse_processor_v1.o build/pulse_processor_v2.o build/lighthouse_geometry.o build/ootx_decoder.o build/lighthouse_calibration.o build/lighthouse_deck_flasher.o build/lighthouse_position_est.o build/lighthouse_storage.o
@@ -247,7 +247,7 @@ CFLAGS += -DDEBUG_PRINT_ON_SEGGER_RTT
endif
# Libs
-OBJS += libarm_math.a
+#OBJS += libarm_math.a
BUILDDIR =./build
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/FreeRTOS-openocd.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/FreeRTOS-openocd.o
new file mode 100644
index 0000000000000000000000000000000000000000..727de34522f8dcc8e1ff6b0144a2d2f25dfe4a07
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/FreeRTOS-openocd.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/configblockeeprom.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/configblockeeprom.o
new file mode 100644
index 0000000000000000000000000000000000000000..7cfd3dc6012c5420a242a5e6c1bbf76fde25ac86
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/configblockeeprom.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_drivers.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_drivers.o
index aaca6502919044831ed2501784d6c3b26ea6a817..ae25d4fe7cfbf40430df08c0f3293189d21c012e 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_drivers.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_drivers.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_info.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_info.o
index c9b4a9d8c649ac60beedf229ddebdfc2f9fa9add..7f4e12d421b03247c74a8f75b743f8474138c063 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_info.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_info.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_memory.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_memory.o
index 9f6725e855fe47cca9e56c71696e73fa53d36ea9..0afaa076b2ddd3bd7c09c5f11478090a4a40899b 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_memory.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_memory.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/filter.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/filter.o
index 9a29d97d2388558e9058e4456155b6ac8cad2c61..cb39e9e230a88d1ee42499ffa447346a0120d891 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/filter.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/filter.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kve.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kve.o
new file mode 100644
index 0000000000000000000000000000000000000000..3cc7cd5cac9f3672c08cf92918bc3dad3908610d
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kve.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kve_storage.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kve_storage.o
new file mode 100644
index 0000000000000000000000000000000000000000..bb589c50d254181289a09126a2c2c07c7e634dee
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kve_storage.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledring12.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledring12.o
index df44183d277c7a5e5f60f822788c3dc5f473e0f9..3c84e8e04e00618f008fb59349a149907c8f0fc7 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledring12.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledring12.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_calibration.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_calibration.o
new file mode 100644
index 0000000000000000000000000000000000000000..c730ca7d6df16d090c7e4d7d323d567086a81751
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_calibration.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_core.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_core.o
new file mode 100644
index 0000000000000000000000000000000000000000..ecff545b6401c3d5168e88901a7791be8b908c14
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_core.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_deck_flasher.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_deck_flasher.o
new file mode 100644
index 0000000000000000000000000000000000000000..097343a06a1efa1824f2bdfbe5a8d6c629f52baf
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_deck_flasher.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_geometry.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_geometry.o
new file mode 100644
index 0000000000000000000000000000000000000000..e038d52e549332cbfc5452dc9414c74889d931fb
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_geometry.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_position_est.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_position_est.o
new file mode 100644
index 0000000000000000000000000000000000000000..a4437f31cd39997f388c49640d529b98a0d8f53f
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_position_est.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_storage.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_storage.o
new file mode 100644
index 0000000000000000000000000000000000000000..3c935ec63656ec21468dfc1e7e57e10526b22025
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_storage.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ootx_decoder.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ootx_decoder.o
new file mode 100644
index 0000000000000000000000000000000000000000..294f839552f411f1d157cd2d8e51ebf6f61147ed
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ootx_decoder.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor.o
new file mode 100644
index 0000000000000000000000000000000000000000..03c7ea650e3d7dac2b3f93c0c1d8f45a608d9973
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor_v1.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor_v1.o
new file mode 100644
index 0000000000000000000000000000000000000000..08f84dcffeea7de3463a27b785e2ed1386208004
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor_v1.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
new file mode 100644
index 0000000000000000000000000000000000000000..45338a2da47f3d15e423a4282b708fec56a7756d
Binary files /dev/null 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/rateSupervisor.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/rateSupervisor.o
new file mode 100644
index 0000000000000000000000000000000000000000..bf5cd726265aafced0e36b2e798766cbed7e498c
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/rateSupervisor.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/sleepus.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/sleepus.o
new file mode 100644
index 0000000000000000000000000000000000000000..4847cf406090ba84e160ae0c788f270be742b591
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/sleepus.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/statsCnt.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/statsCnt.o
new file mode 100644
index 0000000000000000000000000000000000000000..5a859d921780b3bf7a477923cc4435d221e3f49b
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/statsCnt.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usddeck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usddeck.o
index d9b66723802e7198793a63fe5a6ee8f930ae9eeb..6b50c3cd43edd195c03648adf2a56c0401c2ff2c 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usddeck.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usddeck.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/filter.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/filter.c
index 60fc6500a84a2d944b00d2b366037cc0e6a099a0..b1ec64b3cc6dc7754ad3d5c5028d754eaae50522 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/filter.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/filter.c
@@ -73,16 +73,17 @@ void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq)
void lpf2pSetCutoffFreq(lpf2pData* lpfData, float sample_freq, float cutoff_freq)
{
- float fr = sample_freq/cutoff_freq;
- float ohm = tanf(M_PI_F/fr);
- float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm;
- lpfData->b0 = ohm*ohm/c;
- lpfData->b1 = 2.0f*lpfData->b0;
- lpfData->b2 = lpfData->b0;
- lpfData->a1 = 2.0f*(ohm*ohm-1.0f)/c;
- lpfData->a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
- lpfData->delay_element_1 = 0.0f;
- lpfData->delay_element_2 = 0.0f;
+ //COMMENTED FIRWARE
+ // float fr = sample_freq/cutoff_freq;
+ // float ohm = tanf(M_PI_F/fr);
+ // float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm;
+ // lpfData->b0 = ohm*ohm/c;
+ // lpfData->b1 = 2.0f*lpfData->b0;
+ // lpfData->b2 = lpfData->b0;
+ // lpfData->a1 = 2.0f*(ohm*ohm-1.0f)/c;
+ // lpfData->a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
+ // lpfData->delay_element_1 = 0.0f;
+ // lpfData->delay_element_2 = 0.0f;
}
float lpf2pApply(lpf2pData* lpfData, float sample)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/kve/kve.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/kve/kve.c
index b49dc2af4eeb4788dc99bf968b473e901e1c6345..7fc8e21c711be6d4189bb850df19443826cf3068 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/kve/kve.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/kve/kve.c
@@ -24,34 +24,35 @@ static size_t min(size_t a, size_t b)
// Utility function
static bool appendItemToEnd(kveMemory_t *kve, size_t address, const char* key, const void* buffer, size_t length) {
- size_t itemAddress = kveStorageFindEnd(kve, address);
+ //COMMENTED FIRMWARE
+ // size_t itemAddress = kveStorageFindEnd(kve, address);
- // If it is over the end of the memory, table corrupted
- // Do not write anything ...
- if (KVE_STORAGE_IS_VALID(itemAddress) == false) {
- DEBUG_PRINT("Error: table corrupted!\n");
- return false;
- }
-
- // Test that there is enough space to write the item
- if ((itemAddress + sizeof(kveItemHeader_t) + strlen(key) + length + END_TAG_LENDTH) < kve->memorySize) {
- itemAddress += kveStorageWriteItem(kve, itemAddress, key, buffer, length);
- kveStorageWriteEnd(kve, itemAddress);
- } else {
- // Otherwise, defrag and try to insert again!
- kveDefrag(kve);
-
- itemAddress = kveStorageFindEnd(kve, FIRST_ITEM_ADDRESS);
-
- if ((itemAddress + sizeof(kveItemHeader_t) + strlen(key) + length + END_TAG_LENDTH) < kve->memorySize) {
- itemAddress += kveStorageWriteItem(kve, itemAddress, key, buffer, length);
- kveStorageWriteEnd(kve, itemAddress);
- } else {
- // Memory full!
- DEBUG_PRINT("Error: memory full!");
- return false;
- }
- }
+ // // If it is over the end of the memory, table corrupted
+ // // Do not write anything ...
+ // if (KVE_STORAGE_IS_VALID(itemAddress) == false) {
+ // DEBUG_PRINT("Error: table corrupted!\n");
+ // return false;
+ // }
+
+ // // Test that there is enough space to write the item
+ // if ((itemAddress + sizeof(kveItemHeader_t) + strlen(key) + length + END_TAG_LENDTH) < kve->memorySize) {
+ // itemAddress += kveStorageWriteItem(kve, itemAddress, key, buffer, length);
+ // kveStorageWriteEnd(kve, itemAddress);
+ // } else {
+ // // Otherwise, defrag and try to insert again!
+ // kveDefrag(kve);
+
+ // itemAddress = kveStorageFindEnd(kve, FIRST_ITEM_ADDRESS);
+
+ // if ((itemAddress + sizeof(kveItemHeader_t) + strlen(key) + length + END_TAG_LENDTH) < kve->memorySize) {
+ // itemAddress += kveStorageWriteItem(kve, itemAddress, key, buffer, length);
+ // kveStorageWriteEnd(kve, itemAddress);
+ // } else {
+ // // Memory full!
+ // DEBUG_PRINT("Error: memory full!");
+ // return false;
+ // }
+ // }
return true;
}
@@ -90,25 +91,26 @@ void kveDefrag(kveMemory_t *kve) {
}
bool kveStore(kveMemory_t *kve, char* key, const void* buffer, size_t length) {
- size_t itemAddress;
-
- // Search if the key is already present in the table
- itemAddress = kveStorageFindItemByKey(kve, FIRST_ITEM_ADDRESS, key);
- if (KVE_STORAGE_IS_VALID(itemAddress) == false) {
- // Item does not exit, find the end of the table to insert it
- return appendItemToEnd(kve, FIRST_ITEM_ADDRESS, key, buffer, length);
- } else {
- // Item exist, verify that the data has the same size
- kveItemHeader_t currentItem = kveStorageGetItemInfo(kve, itemAddress);
- uint16_t newLength = length + 3 + strlen(key);
- if (currentItem.full_length != newLength) {
- // If not, delete the item and find the end of the table
- kveStorageWriteHole(kve, itemAddress, currentItem.full_length);
- return appendItemToEnd(kve, FIRST_ITEM_ADDRESS, key, buffer, length);
- } else {
- kveStorageWriteItem(kve, itemAddress, key, buffer, length);
- }
- }
+ //COMMENTED FIRMWARE
+ // size_t itemAddress;
+
+ // // Search if the key is already present in the table
+ // itemAddress = kveStorageFindItemByKey(kve, FIRST_ITEM_ADDRESS, key);
+ // if (KVE_STORAGE_IS_VALID(itemAddress) == false) {
+ // // Item does not exit, find the end of the table to insert it
+ // return appendItemToEnd(kve, FIRST_ITEM_ADDRESS, key, buffer, length);
+ // } else {
+ // // Item exist, verify that the data has the same size
+ // kveItemHeader_t currentItem = kveStorageGetItemInfo(kve, itemAddress);
+ // uint16_t newLength = length + 3 + strlen(key);
+ // if (currentItem.full_length != newLength) {
+ // // If not, delete the item and find the end of the table
+ // kveStorageWriteHole(kve, itemAddress, currentItem.full_length);
+ // return appendItemToEnd(kve, FIRST_ITEM_ADDRESS, key, buffer, length);
+ // } else {
+ // kveStorageWriteItem(kve, itemAddress, key, buffer, length);
+ // }
+ // }
return true;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/kve/kve_storage.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/kve/kve_storage.c
index 32f83aba2ce3c4a11a94588c7997aa72db43e4b3..1e5e779e2b192d93b3ea69447c2a7598be46cb7d 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/kve/kve_storage.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/kve/kve_storage.c
@@ -45,15 +45,16 @@ static size_t min(size_t a, size_t b)
int kveStorageWriteItem(kveMemory_t *kve, size_t address, const char* key, const void* buffer, size_t length)
{
kveItemHeader_t header;
- header.key_length = strlen(key);
- header.full_length = 2 + 1 + header.key_length + length;
+ //COMMENTED FIRMWARE
+// header.key_length = strlen(key);
+// header.full_length = 2 + 1 + header.key_length + length;
- // Write full_length, key_length, key and buffer
- kve->write(address, &header, sizeof(header));
- kve->write(address + sizeof(header), key, header.key_length);
- kve->write(address + sizeof(header) + header.key_length, buffer, length);
+// // Write full_length, key_length, key and buffer
+// kve->write(address, &header, sizeof(header));
+// kve->write(address + sizeof(header), key, header.key_length);
+// kve->write(address + sizeof(header) + header.key_length, buffer, length);
- kve->flush();
+// kve->flush();
return header.full_length;
}
@@ -98,31 +99,32 @@ void kveStorageMoveMemory(kveMemory_t *kve, size_t sourceAddress, size_t destina
}
size_t kveStorageFindItemByKey(kveMemory_t *kve, size_t address, const char * key) {
- char searchBuffer[255];
- size_t currentAddress = address;
- uint16_t length;
- uint8_t keyLength;
- uint8_t searchedKeyLength = strlen(key);
-
-
- while (currentAddress < (kve->memorySize - 3)) {
- kve->read(currentAddress, searchBuffer, 3);
- length = searchBuffer[0] + (searchBuffer[1]<<8);
- keyLength = searchBuffer[2];
-
- if (length == END_TAG) {
- return SIZE_MAX;
- }
-
- if (keyLength == searchedKeyLength) {
- kve->read(currentAddress + 3, &searchBuffer, searchedKeyLength);
- if (!memcmp(key, searchBuffer, keyLength)) {
- return currentAddress;
- }
- }
-
- currentAddress += length;
- }
+ //COMMENTED FIRMWARE
+ // char searchBuffer[255];
+ // size_t currentAddress = address;
+ // uint16_t length;
+ // uint8_t keyLength;
+ // uint8_t searchedKeyLength = strlen(key);
+
+
+ // while (currentAddress < (kve->memorySize - 3)) {
+ // kve->read(currentAddress, searchBuffer, 3);
+ // length = searchBuffer[0] + (searchBuffer[1]<<8);
+ // keyLength = searchBuffer[2];
+
+ // if (length == END_TAG) {
+ // return SIZE_MAX;
+ // }
+
+ // if (keyLength == searchedKeyLength) {
+ // kve->read(currentAddress + 3, &searchBuffer, searchedKeyLength);
+ // if (!memcmp(key, searchBuffer, keyLength)) {
+ // return currentAddress;
+ // }
+ // }
+
+ // currentAddress += length;
+ // }
return SIZE_MAX;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/lighthouse_calibration.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/lighthouse_calibration.c
index 316a4f0022eb3e945311afd71ba42f798d65d46e..0caf2ec049d64265c20b4f74d0f68c3024e35051 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/lighthouse_calibration.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/lighthouse_calibration.c
@@ -59,57 +59,60 @@ void lighthouseCalibrationInitFromFrame(lighthouseCalibration_t *calib, struct o
}
static void idealToDistortedV1(const lighthouseCalibration_t* calib, const float* ideal, float* distorted) {
- const float ax = ideal[0];
- const float ay = ideal[1];
+ //COMMENTED FIRWARE
+ // const float ax = ideal[0];
+ // const float ay = ideal[1];
- const float x = 1.0f;
- const float y = tanf(ax);
- const float z = tanf(ay);
- const float tIgnore = 0.0f;
+ // const float x = 1.0f;
+ // const float y = tanf(ax);
+ // const float z = tanf(ay);
+ // const float tIgnore = 0.0f;
- distorted[0] = lighthouseCalibrationMeasurementModelLh1(x, y, z, tIgnore, &calib->sweep[0]);
- distorted[1] = lighthouseCalibrationMeasurementModelLh1(x, z, -y, tIgnore, &calib->sweep[1]);
+ // distorted[0] = lighthouseCalibrationMeasurementModelLh1(x, y, z, tIgnore, &calib->sweep[0]);
+ // distorted[1] = lighthouseCalibrationMeasurementModelLh1(x, z, -y, tIgnore, &calib->sweep[1]);
}
static void idealToDistortedV2(const lighthouseCalibration_t* calib, const float* ideal, float* distorted) {
- const float t30 = M_PI_F / 6.0f;
- const float tan30 = 0.5773502691896258; // const float tan30 = tanf(t30);
+ //COMMENTED FIRMWARE
+ // const float t30 = M_PI_F / 6.0f;
+ // const float tan30 = 0.5773502691896258; // const float tan30 = tanf(t30);
- const float a1 = ideal[0];
- const float a2 = ideal[1];
+ // const float a1 = ideal[0];
+ // const float a2 = ideal[1];
- const float x = 1.0f;
- const float y = tanf((a2 + a1) / 2.0f);
- const float z = sinf(a2 - a1) / (tan30 * (cosf(a2) + cosf(a1)));
+ // const float x = 1.0f;
+ // const float y = tanf((a2 + a1) / 2.0f);
+ // const float z = sinf(a2 - a1) / (tan30 * (cosf(a2) + cosf(a1)));
- distorted[0] = lighthouseCalibrationMeasurementModelLh2(x, y, z, -t30, &calib->sweep[0]);
- distorted[1] = lighthouseCalibrationMeasurementModelLh2(x, y, z, t30, &calib->sweep[1]);
+ // distorted[0] = lighthouseCalibrationMeasurementModelLh2(x, y, z, -t30, &calib->sweep[0]);
+ // distorted[1] = lighthouseCalibrationMeasurementModelLh2(x, y, z, t30, &calib->sweep[1]);
}
typedef void (* idealToDistortedFcn_t)(const lighthouseCalibration_t* calib, const float* ideal, float* distorted);
static void lighthouseCalibrationApply(const lighthouseCalibration_t* calib, const float* rawAngles, float* correctedAngles, idealToDistortedFcn_t idealToDistorted) {
- const double max_delta = 0.0005;
+ //COMMENTED FIRWARE
+ // const double max_delta = 0.0005;
- // Use distorted angle as a starting point
- float* estmatedAngles = correctedAngles;
- estmatedAngles[0] = rawAngles[0];
- estmatedAngles[1] = rawAngles[1];
+ // // Use distorted angle as a starting point
+ // float* estmatedAngles = correctedAngles;
+ // estmatedAngles[0] = rawAngles[0];
+ // estmatedAngles[1] = rawAngles[1];
- for (int i = 0; i < 5; i++) {
- float currentDistortedAngles[2];
- idealToDistorted(calib, estmatedAngles, currentDistortedAngles);
+ // for (int i = 0; i < 5; i++) {
+ // float currentDistortedAngles[2];
+ // idealToDistorted(calib, estmatedAngles, currentDistortedAngles);
- const float delta0 = rawAngles[0] - currentDistortedAngles[0];
- const float delta1 = rawAngles[1] - currentDistortedAngles[1];
+ // const float delta0 = rawAngles[0] - currentDistortedAngles[0];
+ // const float delta1 = rawAngles[1] - currentDistortedAngles[1];
- estmatedAngles[0] = estmatedAngles[0] + delta0;
- estmatedAngles[1] = estmatedAngles[1] + delta1;
+ // estmatedAngles[0] = estmatedAngles[0] + delta0;
+ // estmatedAngles[1] = estmatedAngles[1] + delta1;
- if (fabs((double)delta0) < max_delta && fabs((double)delta1) < max_delta) {
- break;
- }
- }
+ // if (fabs((double)delta0) < max_delta && fabs((double)delta1) < max_delta) {
+ // break;
+ // }
+ // }
}
void lighthouseCalibrationApplyV1(const lighthouseCalibration_t* calib, const float* rawAngles, float* correctedAngles) {
@@ -126,25 +129,29 @@ void lighthouseCalibrationApplyNothing(const float rawAngles[2], float corrected
}
float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, const float t, const lighthouseCalibrationSweep_t* calib) {
- const float ax = atan2f(y, x);
- const float ay = atan2f(z, x);
- const float r = arm_sqrt(x * x + y * y);
+ //COMMENTED FIRWARE
+ // const float ax = atan2f(y, x);
+ // const float ay = atan2f(z, x);
+ // const float r = arm_sqrt(x * x + y * y);
- const float compTilt = asinf(clip1(z * tanf(calib->tilt) / r));
- const float compGib = -calib->gibmag * arm_sin_f32(ax + calib->gibphase);
- const float compCurve = calib->curve * ay * ay;
+ // const float compTilt = asinf(clip1(z * tanf(calib->tilt) / r));
+ // const float compGib = -calib->gibmag * arm_sin_f32(ax + calib->gibphase);
+ // const float compCurve = calib->curve * ay * ay;
- return ax - (compTilt + calib->phase + compGib + compCurve);
+ // return ax - (compTilt + calib->phase + compGib + compCurve);
+ return 0;
}
float lighthouseCalibrationMeasurementModelLh2(const float x, const float y, const float z, const float t, const lighthouseCalibrationSweep_t* calib) {
- const float ax = atan2f(y, x);
- // const float ay = atan2f(z, x);
- const float r = arm_sqrt(x * x + y * y);
+ //COMMENTED FIRWARE
+ // const float ax = atan2f(y, x);
+ // // const float ay = atan2f(z, x);
+ // const float r = arm_sqrt(x * x + y * y);
- const float base = ax + asinf(clip1(z * tanf(t - calib->tilt) / r));
- const float compGib = -calib->gibmag * arm_cos_f32(ax + calib->gibphase);
- // TODO krri Figure out how to use curve and ogee calibration parameters
+ // const float base = ax + asinf(clip1(z * tanf(t - calib->tilt) / r));
+ // const float compGib = -calib->gibmag * arm_cos_f32(ax + calib->gibphase);
+ // // TODO krri Figure out how to use curve and ogee calibration parameters
- return base - (calib->phase + compGib);
+ // return base - (calib->phase + compGib);
+ return 0;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/lighthouse_geometry.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/lighthouse_geometry.c
index b5a8601207dc476fc0f3abcd69d0b1c05d30de4f..86a6654af7f5da00bedb94bb5b8cb9ebbe8d7e49 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/lighthouse_geometry.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/lighthouse_geometry.c
@@ -127,18 +127,20 @@ void lighthouseGeometryGetBaseStationPosition(const baseStationGeometry_t* bs, v
}
void lighthouseGeometryGetRay(const baseStationGeometry_t* baseStationGeometry, const float angleH, const float angleV, vec3d ray) {
- vec3d a = {arm_sin_f32(angleH), -arm_cos_f32(angleH), 0}; // Normal vector to X plane
- vec3d b = {-arm_sin_f32(angleV), 0, arm_cos_f32(angleV)}; // Normal vector to Y plane
-
- vec3d raw_ray = {};
- vec_cross_product(b, a, raw_ray); // Intersection of two planes -> ray vector.
- float len = vec_length(raw_ray);
- arm_scale_f32(raw_ray, 1 / len, raw_ray, vec3d_size); // Normalize raw ray length.
-
- arm_matrix_instance_f32 source_rotation_matrix = {3, 3, (float32_t *)baseStationGeometry->mat};
- arm_matrix_instance_f32 ray_vec = {3, 1, raw_ray};
- arm_matrix_instance_f32 ray_rotated_vec = {3, 1, ray};
- mat_mult(&source_rotation_matrix, &ray_vec, &ray_rotated_vec);
+ //COMMENTED FIRWARE
+ // vec3d a = {arm_sin_f32(angleH), -arm_cos_f32(angleH), 0}; // Normal vector to X plane
+ // vec3d b = {-arm_sin_f32(angleV), 0, arm_cos_f32(angleV)}; // Normal vector to Y plane
+
+ // vec3d raw_ray = {};
+ // vec_cross_product(b, a, raw_ray); // Intersection of two planes -> ray vector.
+ // float len = vec_length(raw_ray);
+ // arm_scale_f32(raw_ray, 1 / len, raw_ray, vec3d_size); // Normalize raw ray length.
+
+
+ // arm_matrix_instance_f32 source_rotation_matrix = {3, 3, (float32_t *)baseStationGeometry->mat};
+ // arm_matrix_instance_f32 ray_vec = {3, 1, raw_ray};
+ // arm_matrix_instance_f32 ray_rotated_vec = {3, 1, ray};
+ // mat_mult(&source_rotation_matrix, &ray_vec, &ray_rotated_vec);
}
bool lighthouseGeometryIntersectionPlaneVector(const vec3d linePoint, const vec3d lineVec, const vec3d planePoint, const vec3d PlaneNormal, vec3d intersectionPoint) {