diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_analog.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_analog.c
index 6296ff178a10349455c67c0b717ced046c285867..e0a6dabc598f415366943c03bf66f76b14c424ae 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_analog.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_analog.c
@@ -27,6 +27,8 @@
#include "deck.h"
#include "stm32fxxx.h"
+#include "stm32f4xx_conf.h"
+#include "stm32f4xx_adc.h"
static uint32_t stregResolution;
static uint32_t adcRange;
@@ -78,7 +80,7 @@ static uint16_t analogReadChannel(uint8_t channel)
// while(ADC_GetFlagStatus(ADC2, ADC_FLAG_EOC) == RESET);
/* Get the conversion value */
- return ADC_GetConversionValue(ADC2);
+ return 0;//ADC_GetConversionValue(ADC2);
}
uint16_t analogRead(const deckPin_t pin)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi3.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi3.c
index 4b16d397d300d50cfc1b94d619516cde295650c8..42e135816acdc12e14715d462b7be9ba2dd3be71 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi3.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/api/deck_spi3.c
@@ -189,10 +189,10 @@ static void spi3DMAInit()
// NVIC_InitStructure.NVIC_IRQChannel = SPI_RX_DMA_IRQ;
// NVIC_Init(&NVIC_InitStructure);
-// }
+}
-// static void spi3ConfigureWithSpeed(uint16_t baudRatePrescaler)
-// {
+static void spi3ConfigureWithSpeed(uint16_t baudRatePrescaler)
+{
// SPI_InitTypeDef SPI_InitStructure;
// SPI_I2S_DeInit(SPI);
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 77fd37a8caa6dd8a951c70c2ada39ba7339d3fa9..afc5ea72be13e1a8847228a5a46cddfa3cc245a5 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
@@ -49,24 +49,25 @@ static int driversLen;
// Init the toc access variables. Lazy initialisation: it is going to be done
// the first time any api function is called.
static void deckdriversInit() {
- static bool init = false;
- if (!init) {
- int i;
-
- drivers = &_deckDriver_start;
- driversLen = &_deckDriver_stop - &_deckDriver_start;
- init = true;
-
- DECK_DRV_DBG_PRINT("Found %d drivers\n", driversLen);
- for (i=0; i<driversLen; i++) {
- if (drivers[i]->name) {
- DECK_DRV_DBG_PRINT("VID:PID %02x:%02x (%s)\n", drivers[i]->vid, drivers[i]->pid, drivers[i]->name);
- } else {
- DECK_DRV_DBG_PRINT("VID:PID %02x:%02x\n", drivers[i]->vid, drivers[i]->pid);
- }
+ //COMMENTED FIRMWARE
+ // static bool init = false;
+ // if (!init) {
+ // int i;
+
+ // drivers = &_deckDriver_start;
+ // driversLen = &_deckDriver_stop - &_deckDriver_start;
+ // init = true;
+
+ // DECK_DRV_DBG_PRINT("Found %d drivers\n", driversLen);
+ // for (i=0; i<driversLen; i++) {
+ // if (drivers[i]->name) {
+ // DECK_DRV_DBG_PRINT("VID:PID %02x:%02x (%s)\n", drivers[i]->vid, drivers[i]->pid, drivers[i]->name);
+ // } else {
+ // DECK_DRV_DBG_PRINT("VID:PID %02x:%02x\n", drivers[i]->vid, drivers[i]->pid);
+ // }
- }
- }
+ // }
+ // }
}
int deckDriverCount() {
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 d5fec8bb79d6371648267e6e2d6d644db0a0f2dd..818c4532b12e70e9f0ea63828ec6ada3e2a2ea0b 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
@@ -111,7 +111,8 @@ static void populateDeckMemoryInfo(uint8_t buffer[], const uint8_t deckNr) {
memcpy(&buffer[OFFS_REQ_HASH], &deckMemDef->requiredHash, 4);
memcpy(&buffer[OFFS_REQ_LEN], &deckMemDef->requiredSize, 4);
memcpy(&buffer[OFFS_BASE_ADDR], &baseAddress, 4);
- strncpy((char*)&buffer[OFFS_NAME], info->driver->name, NAME_LEN_EX_ZERO_TREM);
+ //COMMENTED FIRMWARE
+ // strncpy((char*)&buffer[OFFS_NAME], info->driver->name, NAME_LEN_EX_ZERO_TREM);
}
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/locodeck.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/locodeck.h
index ad869a62a36bd8f38979b96aae3965c855e5e33f..9cc36b84eea4779a7a1bdacb3a7a7b3b0c55fcaa 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/locodeck.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/interface/locodeck.h
@@ -37,6 +37,7 @@
#include <stdint.h>
#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
//COMMENTED FIRMWARE
//#include "libdw1000.h"
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/activeMarkerDeck.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/activeMarkerDeck.c
index b5d1ecf58c6b2501285fb1caa1eacb1a7ce9ebcf..4eda70ff5af80d192d2d6697d800d44155a2f540 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/activeMarkerDeck.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/activeMarkerDeck.c
@@ -118,21 +118,22 @@ static bool activeMarkerDeckTest() {
return false;
}
-#ifndef ACTIVE_MARKER_DECK_TEST
- if (0 == strcmp("Qualisys0.A", versionString)) {
- deckFwVersion = version_0_A;
- } else if (0 == strcmp("Qualisys1.0", versionString)) {
- deckFwVersion = version_1_0;
- }
-
- isVerified = (versionUndefined != deckFwVersion);
- if (! isVerified) {
- DEBUG_PRINT("Incompatible deck FW\n");
- }
-#else
- isVerified = true;
- deckFwVersion = version_1_0;
-#endif
+//COMMENTED FIRMWARE
+// #ifndef ACTIVE_MARKER_DECK_TEST
+// if (0 == strcmp("Qualisys0.A", versionString)) {
+// deckFwVersion = version_0_A;
+// } else if (0 == strcmp("Qualisys1.0", versionString)) {
+// deckFwVersion = version_1_0;
+// }
+
+// isVerified = (versionUndefined != deckFwVersion);
+// if (! isVerified) {
+// DEBUG_PRINT("Incompatible deck FW\n");
+// }
+// #else
+// isVerified = true;
+// deckFwVersion = version_1_0;
+// #endif
return isVerified;
}
@@ -197,7 +198,9 @@ static void task(void *param) {
delay = POLL_UPDATE_PERIOD_MS;
}
- vTaskDelay(M2T(delay));
+ //COMMENTED FIRMWARE
+ //vTaskDelay(M2T(delay));
+ vTaskDelay(delay);
}
}
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 894f1d5c7edc32437a012ee12ca958d342ea9034..aaf59483271f17d37e7439c129c545b32857f3f0 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
@@ -37,6 +37,7 @@
#include "debug.h"
#include "deck.h"
#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
#include "task.h"
#include "queue.h"
//COMMENTED FIRMWARE
@@ -85,7 +86,9 @@ static void NinaTask(void *param)
static void Gap8Task(void *param)
{
systemWaitStart();
- vTaskDelay(M2T(1000));
+ //COMMENTED FIRMWARE
+ // vTaskDelay(M2T(1000));
+ vTaskDelay(1000);
// Pull the reset button to get a clean read out of the data
pinMode(DECK_GPIO_IO4, OUTPUT);
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/flowdeck_v1v2.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/flowdeck_v1v2.c
index b44fc7100b2612df70d844a338eb4405b817aef6..191593fa32f313640586242684bd893841611c2c 100755
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/flowdeck_v1v2.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/flowdeck_v1v2.c
@@ -83,82 +83,83 @@ static float flowStdFixed = 2.0f;
static void flowdeckTask(void *param)
{
- systemWaitStart();
-
- uint64_t lastTime = usecTimestamp();
- while(1) {
- vTaskDelay(10);
-
- pmw3901ReadMotion(NCS_PIN, ¤tMotion);
-
- // Flip motion information to comply with sensor mounting
- // (might need to be changed if mounted differently)
- int16_t accpx = -currentMotion.deltaY;
- int16_t accpy = -currentMotion.deltaX;
-
- // Outlier removal
- if (abs(accpx) < OULIER_LIMIT && abs(accpy) < OULIER_LIMIT) {
-
- if (useAdaptiveStd)
- {
- // The standard deviation is fitted by measurements flying over low and high texture
- // and looking at the shutter time
- float shutter_f = (float)currentMotion.shutter;
- stdFlow=0.0007984f *shutter_f + 0.4335f;
-
-
- // The formula with the amount of features instead
- /*float squal_f = (float)currentMotion.squal;
- stdFlow = -0.01257f * squal_f + 4.406f; */
- if (stdFlow < 0.1f) stdFlow=0.1f;
- } else {
- stdFlow = flowStdFixed;
- }
-
-
- // Form flow measurement struct and push into the EKF
- flowMeasurement_t flowData;
- flowData.stdDevX = stdFlow;
- flowData.stdDevY = stdFlow;
- flowData.dt = 0.01;
-
-#if defined(USE_MA_SMOOTHING)
- // Use MA Smoothing
- pixelAverages.averageX[pixelAverages.ptr] = (float32_t)accpx;
- pixelAverages.averageY[pixelAverages.ptr] = (float32_t)accpy;
-
- float32_t meanX;
- float32_t meanY;
-
- arm_mean_f32(pixelAverages.averageX, AVERAGE_HISTORY_LENGTH, &meanX);
- arm_mean_f32(pixelAverages.averageY, AVERAGE_HISTORY_LENGTH, &meanY);
-
- pixelAverages.ptr = (pixelAverages.ptr + 1) % AVERAGE_HISTORY_LENGTH;
-
- flowData.dpixelx = (float)meanX; // [pixels]
- flowData.dpixely = (float)meanY; // [pixels]
-#elif defined(USE_LP_FILTER)
- // Use LP filter measurements
- flowData.dpixelx = LP_CONSTANT * dpixelx_previous + (1.0f - LP_CONSTANT) * (float)accpx;
- flowData.dpixely = LP_CONSTANT * dpixely_previous + (1.0f - LP_CONSTANT) * (float)accpy;
- dpixelx_previous = flowData.dpixelx;
- dpixely_previous = flowData.dpixely;
-#else
- // Use raw measurements
- flowData.dpixelx = (float)accpx;
- flowData.dpixely = (float)accpy;
-#endif
- // Push measurements into the estimator if flow is not disabled
- // and the PMW flow sensor indicates motion detection
- if (!useFlowDisabled && currentMotion.motion == 0xB0) {
- flowData.dt = (float)(usecTimestamp()-lastTime)/1000000.0f;
- lastTime = usecTimestamp();
- estimatorEnqueueFlow(&flowData);
- }
- } else {
- outlierCount++;
- }
- }
+ //COMMENTED FIRMWARE
+// systemWaitStart();
+
+// uint64_t lastTime = usecTimestamp();
+// while(1) {
+// vTaskDelay(10);
+
+// pmw3901ReadMotion(NCS_PIN, ¤tMotion);
+
+// // Flip motion information to comply with sensor mounting
+// // (might need to be changed if mounted differently)
+// int16_t accpx = -currentMotion.deltaY;
+// int16_t accpy = -currentMotion.deltaX;
+
+// // Outlier removal
+// if (abs(accpx) < OULIER_LIMIT && abs(accpy) < OULIER_LIMIT) {
+
+// if (useAdaptiveStd)
+// {
+// // The standard deviation is fitted by measurements flying over low and high texture
+// // and looking at the shutter time
+// float shutter_f = (float)currentMotion.shutter;
+// stdFlow=0.0007984f *shutter_f + 0.4335f;
+
+
+// // The formula with the amount of features instead
+// /*float squal_f = (float)currentMotion.squal;
+// stdFlow = -0.01257f * squal_f + 4.406f; */
+// if (stdFlow < 0.1f) stdFlow=0.1f;
+// } else {
+// stdFlow = flowStdFixed;
+// }
+
+
+// // Form flow measurement struct and push into the EKF
+// flowMeasurement_t flowData;
+// flowData.stdDevX = stdFlow;
+// flowData.stdDevY = stdFlow;
+// flowData.dt = 0.01;
+
+// #if defined(USE_MA_SMOOTHING)
+// // Use MA Smoothing
+// pixelAverages.averageX[pixelAverages.ptr] = (float32_t)accpx;
+// pixelAverages.averageY[pixelAverages.ptr] = (float32_t)accpy;
+
+// float32_t meanX;
+// float32_t meanY;
+
+// arm_mean_f32(pixelAverages.averageX, AVERAGE_HISTORY_LENGTH, &meanX);
+// arm_mean_f32(pixelAverages.averageY, AVERAGE_HISTORY_LENGTH, &meanY);
+
+// pixelAverages.ptr = (pixelAverages.ptr + 1) % AVERAGE_HISTORY_LENGTH;
+
+// flowData.dpixelx = (float)meanX; // [pixels]
+// flowData.dpixely = (float)meanY; // [pixels]
+// #elif defined(USE_LP_FILTER)
+// // Use LP filter measurements
+// flowData.dpixelx = LP_CONSTANT * dpixelx_previous + (1.0f - LP_CONSTANT) * (float)accpx;
+// flowData.dpixely = LP_CONSTANT * dpixely_previous + (1.0f - LP_CONSTANT) * (float)accpy;
+// dpixelx_previous = flowData.dpixelx;
+// dpixely_previous = flowData.dpixely;
+// #else
+// // Use raw measurements
+// flowData.dpixelx = (float)accpx;
+// flowData.dpixely = (float)accpy;
+// #endif
+// // Push measurements into the estimator if flow is not disabled
+// // and the PMW flow sensor indicates motion detection
+// if (!useFlowDisabled && currentMotion.motion == 0xB0) {
+// flowData.dt = (float)(usecTimestamp()-lastTime)/1000000.0f;
+// lastTime = usecTimestamp();
+// estimatorEnqueueFlow(&flowData);
+// }
+// } else {
+// outlierCount++;
+// }
+// }
}
static void flowdeck1Init()
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 7bca41c2d1c716481e82b18f1143b1c6d9aaaf8d..505caf7688d24d73919658bf479b9f00f3fbb00d 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
@@ -121,14 +121,15 @@ static int32_t parse_coordinate(char ** sp) {
// a large number * 10 M
// 32 18.0489 N = 32 degrees + 18.0489 / 60 = 32.300815 N
- dm = strtol(*sp, &i, 10);
- degree = (dm / 100) * 10000000;
- minute = ((dm % 100) * 10000000) / 60;
- second = (strtol(i+1, &j, 10) * 1000) / 60;
- ret = degree + minute + second;
- skip_to_next(sp, ',');
- if (**sp == 'S' || **sp == 'W')
- ret *= -1;
+ //COMMENTED FIRMWARE
+ // dm = strtol(*sp, &i, 10);
+ // degree = (dm / 100) * 10000000;
+ // minute = ((dm % 100) * 10000000) / 60;
+ // second = (strtol(i+1, &j, 10) * 1000) / 60;
+ // ret = degree + minute + second;
+ // skip_to_next(sp, ',');
+ // if (**sp == 'S' || **sp == 'W')
+ // ret *= -1;
return ret;
}
@@ -140,12 +141,14 @@ static float parse_float(char * sp) {
char * i;
char * j;
- major = strtol(sp, &i, 10);
+ //COMMENTED FIRMWARE
+ //major = strtol(sp, &i, 10);
// Do decimals
- if (strncmp(i, ".", 1) == 0) {
- minor = strtol(i+1, &j, 10);
- deci_nbr = j - i - 1;
- }
+ //COMMENTED FIRMWARE
+ // if (strncmp(i, ".", 1) == 0) {
+ // minor = strtol(i+1, &j, 10);
+ // deci_nbr = j - i - 1;
+ // }
ret = (major * pow(10, deci_nbr) + minor) / pow(10, deci_nbr);
//printf("%i.%i == %f (%i) (%c)\n", major, minor, ret, deci_nbr, (int) *i);
return ret;
@@ -156,7 +159,8 @@ static void parse_next(char ** sp, FieldType t, void * value) {
//DEBUG_PRINT("[%s]\n", (*sp));
switch (t) {
case FIELD_INT:
- *((uint32_t*) value) = strtol(*sp, 0, 10);
+ //COMMENTED FIRMWARE
+ // *((uint32_t*) value) = strtol(*sp, 0, 10);
break;
case FIELD_FLOAT:
*((float*) value) = parse_float(*sp);
@@ -218,7 +222,8 @@ static bool verifyChecksum(const char * buff) {
while (buff[i] != '*' && i < MAX_LEN_SENTANCE-3) {
test_chksum ^= buff[i++];
}
- ref_chksum = strtol(&buff[i+1], 0, 16);
+ //COMMENTED FIRMWARE
+ // ref_chksum = strtol(&buff[i+1], 0, 16);
return (test_chksum == ref_chksum);
}
@@ -264,9 +269,10 @@ void gtgpsTask(void *param)
if (verifyChecksum(buff)) {
//DEBUG_PRINT("O");
for (j = 0; j < sizeof(parsers)/sizeof(parsers[0]); j++) {
- if (strncmp(parsers[j].token, buff, LEN_TOKEN) == 0) {
- parsers[j].parser(&buff[LEN_TOKEN]);
- }
+ //COMMENTED FIRMWARE
+ // if (strncmp(parsers[j].token, buff, LEN_TOKEN) == 0) {
+ // parsers[j].parser(&buff[LEN_TOKEN]);
+ // }
}
}
} else if (bi < MAX_LEN_SENTANCE) {
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 18a023097e515b44511217dd1e6bef9f6181c7df..23806a773e197a2cdce8e5f7f1700b68bf8e4143 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
@@ -465,29 +465,31 @@ static void gravityLightRender(uint8_t buffer[][3], float led_index, int intensi
static void gravityLight(uint8_t buffer[][3], bool reset)
{
- static int pitchid, rollid;
- static bool isInitialized = false;
+ //COMMENTED FIRMWARE
+ // static int pitchid, rollid;
+ // static bool isInitialized = false;
- if (!isInitialized) {
- pitchid = logGetVarId("stabilizer", "pitch");
- rollid = logGetVarId("stabilizer", "roll");
- isInitialized = true;
- }
+ // if (!isInitialized) {
+ // pitchid = logGetVarId("stabilizer", "pitch");
+ // rollid = logGetVarId("stabilizer", "roll");
+ // isInitialized = true;
+ // }
- float pitch = logGetFloat(pitchid); // -180 to 180
- float roll = logGetFloat(rollid); // -180 to 180
+ // float pitch = logGetFloat(pitchid); // -180 to 180
+ // float roll = logGetFloat(rollid); // -180 to 180
- float angle = gravityLightCalculateAngle(pitch, roll);
- float led_index = NBR_LEDS * angle / (2 * (float) M_PI);
- int intensity = LIMIT(sqrtf(pitch * pitch + roll * roll));
- gravityLightRender(buffer, led_index, intensity);
+ // float angle = gravityLightCalculateAngle(pitch, roll);
+ // float led_index = NBR_LEDS * angle / (2 * (float) M_PI);
+ // int intensity = LIMIT(sqrtf(pitch * pitch + roll * roll));
+ // gravityLightRender(buffer, led_index, intensity);
}
static float gravityLightCalculateAngle(float pitch, float roll) {
float angle = 0.0;
if (roll != 0) {
- angle = atanf(pitch / roll) + (float) M_PI_2;
+ //COMMENTED FIRMWARE
+ // angle = atanf(pitch / roll) + (float) M_PI_2;
if (roll < 0.0f) {
angle += (float) M_PI;
@@ -498,19 +500,20 @@ static float gravityLightCalculateAngle(float pitch, float roll) {
}
static void gravityLightRender(uint8_t buffer[][3], float led_index, int intensity) {
- float width = 5;
- float height = intensity;
-
- int i;
- for (i = 0; i < NBR_LEDS; i++) {
- float distance = fabsf(led_index - i);
- if (distance > NBR_LEDS / 2) {
- distance = NBR_LEDS - distance;
- }
-
- int col = height - distance * (height / (width / 2));
- SET_WHITE(buffer[i], LIMIT(col));
- }
+ //COMMENTED FIRMWARE
+ // float width = 5;
+ // float height = intensity;
+
+ // int i;
+ // for (i = 0; i < NBR_LEDS; i++) {
+ // float distance = fabsf(led_index - i);
+ // if (distance > NBR_LEDS / 2) {
+ // distance = NBR_LEDS - distance;
+ // }
+
+ // int col = height - distance * (height / (width / 2));
+ // SET_WHITE(buffer[i], LIMIT(col));
+ // }
}
@@ -990,23 +993,24 @@ static void checkLightSignalTrigger(void)
static void overrideWithLightSignal(uint8_t buffer[][3])
{
- uint8_t color;
- uint32_t diffMsec;
-
- if (lightSignal.active) {
- diffMsec = T2M(xTaskGetTickCount() - lightSignal.triggeredAt);
-
- if (diffMsec >= 1500) {
- lightSignal.active = 0;
- lightSignal.triggeredAt = 0;
- color = 0;
- } else {
- diffMsec = diffMsec % 300;
- color = (diffMsec <= 100) ? 255 : 0;
- }
-
- memset(buffer, color, NBR_LEDS * 3);
- }
+ //COMMENTED FIRMWARE
+ // uint8_t color;
+ // uint32_t diffMsec;
+
+ // if (lightSignal.active) {
+ // diffMsec = T2M(xTaskGetTickCount() - lightSignal.triggeredAt);
+
+ // if (diffMsec >= 1500) {
+ // lightSignal.active = 0;
+ // lightSignal.triggeredAt = 0;
+ // color = 0;
+ // } else {
+ // diffMsec = diffMsec % 300;
+ // color = (diffMsec <= 100) ? 255 : 0;
+ // }
+
+ // memset(buffer, color, NBR_LEDS * 3);
+ // }
}
/********** Ring init and switching **********/
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lhtesterdeck.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lhtesterdeck.c
index ece3f90d30cae62b36f63de7bbeae07cd8f069f0..26e06b7377d605bd08e6a28e57c4af15b8520bd0 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lhtesterdeck.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lhtesterdeck.c
@@ -81,7 +81,9 @@ static void timerFcn(xTimerHandle xTimer)
}
static void startSwTimer() {
- xTimerHandle timer = xTimerCreate("lhTesterTimer", M2T(1), pdTRUE, 0, timerFcn);
+ //COMMENTED FIRMWARE
+ //xTimerHandle timer = xTimerCreate("lhTesterTimer", M2T(1), pdTRUE, 0, timerFcn);
+ xTimerHandle timer = xTimerCreate("lhTesterTimer", 1, pdTRUE, 0, timerFcn);
xTimerStart(timer, 0);
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lighthouse.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lighthouse.c
index a15a4b4593b5383993f083e0df952da19f84c39d..00d2b1b839ea3602e860e7ab93122e8587dead8e 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lighthouse.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lighthouse.c
@@ -63,9 +63,10 @@ static void lighthouseInit(DeckInfo *info)
2*configMINIMAL_STACK_SIZE, NULL, LIGHTHOUSE_TASK_PRI, NULL);
xTimerHandle timer;
- timer = xTimerCreateStatic("ledTimer", M2T(FIFTH_SECOND), pdTRUE,
- NULL, ledTimerHandle, &timerBuffer);
- xTimerStart(timer, M2T(0));
+ //COMMENTED FIRMWARE
+ // timer = xTimerCreateStatic("ledTimer", M2T(FIFTH_SECOND), pdTRUE,
+ // NULL, ledTimerHandle, &timerBuffer);
+ // xTimerStart(timer, M2T(0));
isInit = true;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa2Tag.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa2Tag.c
index 8239867dd60b1930f5273ee8abc25ab09f3b205d..4a4d07074dec29f42e6415924aadfc863b7e1782 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa2Tag.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa2Tag.c
@@ -318,14 +318,15 @@ static bool isRangingOk()
}
static bool getAnchorPosition(const uint8_t anchorId, point_t* position) {
- tdoaAnchorContext_t anchorCtx;
- uint32_t now_ms = T2M(xTaskGetTickCount());
+ //COMMENTED FIRMWARE
+ // tdoaAnchorContext_t anchorCtx;
+ // uint32_t now_ms = T2M(xTaskGetTickCount());
- bool contextFound = tdoaStorageGetAnchorCtx(tdoaEngineState.anchorInfoArray, anchorId, now_ms, &anchorCtx);
- if (contextFound) {
- tdoaStorageGetAnchorPosition(&anchorCtx, position);
- return true;
- }
+ // bool contextFound = tdoaStorageGetAnchorCtx(tdoaEngineState.anchorInfoArray, anchorId, now_ms, &anchorCtx);
+ // if (contextFound) {
+ // tdoaStorageGetAnchorPosition(&anchorCtx, position);
+ // return true;
+ // }
return false;
}
@@ -335,7 +336,8 @@ static uint8_t getAnchorIdList(uint8_t unorderedAnchorList[], const int maxListS
}
static uint8_t getActiveAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
- uint32_t now_ms = T2M(xTaskGetTickCount());
+ //COMMENTED FIRMWARE
+ uint32_t now_ms = 0;//T2M(xTaskGetTickCount());
return tdoaStorageGetListOfActiveAnchorIds(tdoaEngineState.anchorInfoArray, unorderedAnchorList, maxListSize, now_ms);
}
@@ -352,14 +354,15 @@ static void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data, td
}
}
-// uwbAlgorithm_t uwbTdoa2TagAlgorithm = {
-// .init = Initialize,
-// .onEvent = onEvent,
-// .isRangingOk = isRangingOk,
-// .getAnchorPosition = getAnchorPosition,
-// .getAnchorIdList = getAnchorIdList,
-// .getActiveAnchorIdList = getActiveAnchorIdList,
-// };
+//COMMENTED FIRMWARE
+uwbAlgorithm_t uwbTdoa2TagAlgorithm = {
+ // .init = Initialize,
+ // .onEvent = onEvent,
+ // .isRangingOk = isRangingOk,
+ // .getAnchorPosition = getAnchorPosition,
+ // .getAnchorIdList = getAnchorIdList,
+ // .getActiveAnchorIdList = getActiveAnchorIdList,
+};
void lpsTdoa2TagSetOptions(lpsTdoa2AlgoOptions_t* newOptions) {
options = newOptions;
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa3Tag.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa3Tag.c
index 0b32ecbf7748b58b6806b465aed6c1fe46ac896d..ec0bf0731628882354bef6bfce82e8347092acec 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa3Tag.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTdoa3Tag.c
@@ -273,14 +273,15 @@ static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
}
static bool getAnchorPosition(const uint8_t anchorId, point_t* position) {
- tdoaAnchorContext_t anchorCtx;
- uint32_t now_ms = T2M(xTaskGetTickCount());
+ //COMMENTED FIRMWARE
+ // tdoaAnchorContext_t anchorCtx;
+ // uint32_t now_ms = T2M(xTaskGetTickCount());
- bool contextFound = tdoaStorageGetAnchorCtx(tdoaEngineState.anchorInfoArray, anchorId, now_ms, &anchorCtx);
- if (contextFound) {
- tdoaStorageGetAnchorPosition(&anchorCtx, position);
- return true;
- }
+ // bool contextFound = tdoaStorageGetAnchorCtx(tdoaEngineState.anchorInfoArray, anchorId, now_ms, &anchorCtx);
+ // if (contextFound) {
+ // tdoaStorageGetAnchorPosition(&anchorCtx, position);
+ // return true;
+ // }
return false;
}
@@ -290,7 +291,8 @@ static uint8_t getAnchorIdList(uint8_t unorderedAnchorList[], const int maxListS
}
static uint8_t getActiveAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
- uint32_t now_ms = T2M(xTaskGetTickCount());
+ //COMMENTED FIRMWARE
+ uint32_t now_ms = 0;//T2M(xTaskGetTickCount());
return tdoaStorageGetListOfActiveAnchorIds(tdoaEngineState.anchorInfoArray, unorderedAnchorList, maxListSize, now_ms);
}
@@ -316,11 +318,11 @@ static bool isRangingOk()
}
//COMMENTED FIRMWARE
-// uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
-// .init = Initialize,
-// .onEvent = onEvent,
-// .isRangingOk = isRangingOk,
-// .getAnchorPosition = getAnchorPosition,
-// .getAnchorIdList = getAnchorIdList,
-// .getActiveAnchorIdList = getActiveAnchorIdList,
-// };
+uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
+ // .init = Initialize,
+ // .onEvent = onEvent,
+ // .isRangingOk = isRangingOk,
+ // .getAnchorPosition = getAnchorPosition,
+ // .getAnchorIdList = getAnchorIdList,
+ // .getActiveAnchorIdList = getActiveAnchorIdList,
+};
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTwrTag.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTwrTag.c
index bed043dd815dc6b657632978fe2f06c96ab92b5d..c4371d6402caae9b9810fdadc98390a884de35e9 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTwrTag.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/lpsTwrTag.c
@@ -550,14 +550,14 @@ static uint8_t getActiveAnchorIdList(uint8_t unorderedAnchorList[], const int ma
}
//COMMENTED FIRMWARE
-// uwbAlgorithm_t uwbTwrTagAlgorithm = {
-// .init = twrTagInit,
-// .onEvent = twrTagOnEvent,
-// .isRangingOk = isRangingOk,
-// .getAnchorPosition = getAnchorPosition,
-// .getAnchorIdList = getAnchorIdList,
-// .getActiveAnchorIdList = getActiveAnchorIdList,
-// };
+uwbAlgorithm_t uwbTwrTagAlgorithm = {
+ // .init = twrTagInit,
+ // .onEvent = twrTagOnEvent,
+ // .isRangingOk = isRangingOk,
+ // .getAnchorPosition = getAnchorPosition,
+ // .getAnchorIdList = getAnchorIdList,
+ // .getActiveAnchorIdList = getActiveAnchorIdList,
+};
/**
* Log group for Two Way Ranging data
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/multiranger.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/multiranger.c
index cb4ef4fee95d66a329fc41d5968c1a5510757a00..7cdcf52dd48ba5e4f5d3778f794e6e666fb65ccc 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/multiranger.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/multiranger.c
@@ -66,7 +66,9 @@ static bool mrInitSensor(VL53L1_Dev_t *pdev, uint32_t pca95pin, char *name)
// Bring up VL53 by releasing XSHUT
pca95x4SetOutput(pca95pin);
// Let VL53 boot
- vTaskDelay(M2T(2));
+ //COMMENTED FIRMWARE
+ //vTaskDelay(M2T(2));
+ vTaskDelay(2);
// Init VL53
if (vl53l1xInit(pdev, I2C1_DEV))
{
@@ -92,7 +94,9 @@ static uint16_t mrGetMeasurementAndRestart(VL53L1_Dev_t *dev)
while (dataReady == 0)
{
status = VL53L1_GetMeasurementDataReady(dev, &dataReady);
- vTaskDelay(M2T(1));
+ //COMMENTED FIRMWARE
+ //vTaskDelay(M2T(1));
+ vTaskDelay(1);
}
status = VL53L1_GetRangingMeasurementData(dev, &rangingData);
@@ -128,7 +132,8 @@ static void mrTask(void *param)
while (1)
{
- vTaskDelayUntil(&lastWakeTime, M2T(100));
+ //COMMENTED FIRMWARE
+ //vTaskDelayUntil(&lastWakeTime, M2T(100));
rangeSet(rangeFront, mrGetMeasurementAndRestart(&devFront)/1000.0f);
rangeSet(rangeBack, mrGetMeasurementAndRestart(&devBack)/1000.0f);
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/oa.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/oa.c
index bf80e5b0409dc64ce3c0f9d3a7ef0c5851092843..30b1e3e601f373bdf556c4bd7ebe586f36e7c169 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/oa.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/oa.c
@@ -75,7 +75,8 @@ static void oaTask(void *param)
TickType_t lastWakeTime = xTaskGetTickCount();
while(1) {
- vTaskDelayUntil(&lastWakeTime, M2T(50));
+ //COMMENTED FIRMWARE
+ //vTaskDelayUntil(&lastWakeTime, M2T(50));
rangeFront = vl53l0xReadRangeContinuousMillimeters(&devFront);
rangeBack = vl53l0xReadRangeContinuousMillimeters(&devBack);
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 707555a7d012cfa4b417655674598706bf357a5c..1484ddc65a8e6d7c59a0757c01a869589d77b7e5 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
@@ -344,7 +344,9 @@ static void csLow(void)
static void delayMs(UINT ms)
{
- vTaskDelay(M2T(ms));
+ //COMMENTED FIRMWARE
+ //vTaskDelay(M2T(ms));
+ vTaskDelay(ms);
}
/* FatFS Disk Interface */
@@ -412,9 +414,10 @@ TCHAR* f_gets_without_comments (
if (c == '\n') {
break; /* Break on EOL */
}
- if (isspace((int)c)) {
- continue; /* Strip whitespace */
- }
+ //COMMENTED FIRWMARE
+ // if (isspace((int)c)) {
+ // continue; /* Strip whitespace */
+ // }
if (c == '#') {
isComment = true; /* keep reading until end of line */
}
@@ -435,24 +438,25 @@ static bool initSuccess = false;
static void usdInit(DeckInfo *info)
{
- if (!isInit) {
- memoryRegisterHandler(&memDef);
-
- logFileMutex = xSemaphoreCreateMutex();
- logBufferMutex = xSemaphoreCreateMutex();
- /* try to mount drives before creating the tasks */
- if (f_mount(&FatFs, "", 1) == FR_OK) {
- DEBUG_PRINT("mount SD-Card [OK].\n");
-
- /* create usd-log task */
- xTaskCreate(usdLogTask, USDLOG_TASK_NAME,
- USDLOG_TASK_STACKSIZE, NULL,
- USDLOG_TASK_PRI, NULL);
- } else {
- DEBUG_PRINT("mount SD-Card [FAIL].\n");
- }
- }
- isInit = true;
+ //COMMENTED FIRMWARE
+ // if (!isInit) {
+ // memoryRegisterHandler(&memDef);
+
+ // logFileMutex = xSemaphoreCreateMutex();
+ // logBufferMutex = xSemaphoreCreateMutex();
+ // /* try to mount drives before creating the tasks */
+ // if (f_mount(&FatFs, "", 1) == FR_OK) {
+ // DEBUG_PRINT("mount SD-Card [OK].\n");
+
+ // /* create usd-log task */
+ // xTaskCreate(usdLogTask, USDLOG_TASK_NAME,
+ // USDLOG_TASK_STACKSIZE, NULL,
+ // USDLOG_TASK_PRI, NULL);
+ // } else {
+ // DEBUG_PRINT("mount SD-Card [FAIL].\n");
+ // }
+ // }
+ // isInit = true;
}
static void usddeckWriteEventData(const usdLogEventConfig_t* cfg, const uint8_t* payload, uint8_t payloadSize)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/zranger.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/zranger.c
index 095e755d19458ba14b33f11b4792e4182e083e7d..13138475c60defeecd5172ba9426ae3aaf968936 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/zranger.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/deck/drivers/src/zranger.c
@@ -68,7 +68,8 @@ void zRangerInit(DeckInfo* info)
xTaskCreate(zRangerTask, ZRANGER_TASK_NAME, ZRANGER_TASK_STACKSIZE, NULL, ZRANGER_TASK_PRI, NULL);
// pre-compute constant in the measurement noise model for kalman
- expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA);
+ //COMMENTED FIRMWARE
+ //expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA);
isInit = true;
}
@@ -87,30 +88,32 @@ bool zRangerTest(void)
void zRangerTask(void* arg)
{
- systemWaitStart();
- TickType_t xLastWakeTime;
-
- vl53l0xSetVcselPulsePeriod(&dev, VcselPeriodPreRange, 18);
- vl53l0xSetVcselPulsePeriod(&dev, VcselPeriodFinalRange, 14);
- vl53l0xStartContinuous(&dev, 0);
-
- xLastWakeTime = xTaskGetTickCount();
-
- while (1) {
- vTaskDelayUntil(&xLastWakeTime, M2T(dev.measurement_timing_budget_ms));
-
- range_last = vl53l0xReadRangeContinuousMillimeters(&dev);
- rangeSet(rangeDown, range_last / 1000.0f);
-
- // check if range is feasible and push into the estimator
- // the sensor should not be able to measure >3 [m], and outliers typically
- // occur as >8 [m] measurements
- if (range_last < RANGE_OUTLIER_LIMIT) {
- float distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
- float stdDev = expStdA * (1.0f + expf( expCoeff * (distance - expPointA)));
- rangeEnqueueDownRangeInEstimator(distance, stdDev, xTaskGetTickCount());
- }
- }
+ //COMMENTED FIRMWARE
+ // systemWaitStart();
+ // TickType_t xLastWakeTime;
+
+ // vl53l0xSetVcselPulsePeriod(&dev, VcselPeriodPreRange, 18);
+ // vl53l0xSetVcselPulsePeriod(&dev, VcselPeriodFinalRange, 14);
+ // vl53l0xStartContinuous(&dev, 0);
+
+ // xLastWakeTime = xTaskGetTickCount();
+
+ // while (1) {
+ // //COMMENTED FIRMWARE
+ // //vTaskDelayUntil(&xLastWakeTime, M2T(dev.measurement_timing_budget_ms));
+
+ // range_last = vl53l0xReadRangeContinuousMillimeters(&dev);
+ // rangeSet(rangeDown, range_last / 1000.0f);
+
+ // // check if range is feasible and push into the estimator
+ // // the sensor should not be able to measure >3 [m], and outliers typically
+ // // occur as >8 [m] measurements
+ // if (range_last < RANGE_OUTLIER_LIMIT) {
+ // float distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
+ // float stdDev = expStdA * (1.0f + expf( expCoeff * (distance - expPointA)));
+ // rangeEnqueueDownRangeInEstimator(distance, stdDev, xTaskGetTickCount());
+ // }
+ // }
}
static const DeckDriver zranger_deck = {
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 d517511c0e167f3bc03a9deb537ae3163dac4ccd..014ba745bafd816540ca3149300ee28aad47ba5e 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
@@ -100,7 +100,8 @@ void zRanger2Init(DeckInfo* info)
xTaskCreate(zRanger2Task, ZRANGER2_TASK_NAME, ZRANGER2_TASK_STACKSIZE, NULL, ZRANGER2_TASK_PRI, NULL);
// pre-compute constant in the measurement noise model for kalman
- expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA);
+ //COMMENTED FIRMWARE
+ // expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA);
isInit = true;
}
@@ -115,34 +116,36 @@ bool zRanger2Test(void)
void zRanger2Task(void* arg)
{
- TickType_t lastWakeTime;
+ //COMMENTED FIRMWARE
+ // TickType_t lastWakeTime;
- systemWaitStart();
+ // systemWaitStart();
- // Restart sensor
- VL53L1_StopMeasurement(&dev);
- VL53L1_SetDistanceMode(&dev, VL53L1_DISTANCEMODE_MEDIUM);
- VL53L1_SetMeasurementTimingBudgetMicroSeconds(&dev, 25000);
+ // // Restart sensor
+ // VL53L1_StopMeasurement(&dev);
+ // VL53L1_SetDistanceMode(&dev, VL53L1_DISTANCEMODE_MEDIUM);
+ // VL53L1_SetMeasurementTimingBudgetMicroSeconds(&dev, 25000);
- VL53L1_StartMeasurement(&dev);
+ // VL53L1_StartMeasurement(&dev);
- lastWakeTime = xTaskGetTickCount();
+ // lastWakeTime = xTaskGetTickCount();
- while (1) {
- vTaskDelayUntil(&lastWakeTime, M2T(25));
+ // while (1) {
+ // //COMMENTED FIRMWARE
+ // //vTaskDelayUntil(&lastWakeTime, M2T(25));
- range_last = zRanger2GetMeasurementAndRestart(&dev);
- rangeSet(rangeDown, range_last / 1000.0f);
+ // range_last = zRanger2GetMeasurementAndRestart(&dev);
+ // rangeSet(rangeDown, range_last / 1000.0f);
- // check if range is feasible and push into the estimator
- // the sensor should not be able to measure >5 [m], and outliers typically
- // occur as >8 [m] measurements
- if (range_last < RANGE_OUTLIER_LIMIT) {
- float distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
- float stdDev = expStdA * (1.0f + expf( expCoeff * (distance - expPointA)));
- rangeEnqueueDownRangeInEstimator(distance, stdDev, xTaskGetTickCount());
- }
- }
+ // // check if range is feasible and push into the estimator
+ // // the sensor should not be able to measure >5 [m], and outliers typically
+ // // occur as >8 [m] measurements
+ // if (range_last < RANGE_OUTLIER_LIMIT) {
+ // float distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
+ // float stdDev = expStdA * (1.0f + expf( expCoeff * (distance - expPointA)));
+ // rangeEnqueueDownRangeInEstimator(distance, stdDev, xTaskGetTickCount());
+ // }
+ // }
}
static const DeckDriver zranger2_deck = {
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/exti.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/exti.c
index 79810be140d9ae8ac8c6b1847874f932ec5b870b..403ab928aede927cd6a63b1694936f18a8d0fe15 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/exti.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/exti.c
@@ -36,7 +36,8 @@ static bool isInit;
/* Interruption initialisation */
void extiInit()
{
- static NVIC_InitTypeDef NVIC_InitStructure;
+ //COMMENTED FIRMWARE
+ //static NVIC_InitTypeDef NVIC_InitStructure;
if (isInit)
return;
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/fatfs_sd.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/fatfs_sd.c
index 1fe8a06ee0c639fe729819c2acde94c289160b81..ae84f367740c7000bc860cc0c9f612acf5a69a7a 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/fatfs_sd.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/fatfs_sd.c
@@ -244,7 +244,7 @@ static BYTE sendCommand(sdSpiContext_t *context, BYTE cmd, DWORD arg) {
// Receive command resp
{
// BYTE maxTries = 10;
- // BYTE res;
+ BYTE res;
// do {
// res = context->xchgSpi(0xFF);
// } while ((res & 0x80) && --maxTries);
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/lps25h.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/lps25h.c
index 4cb77a181f16f5cd821f50129f30ae0d25d22407..83653ca5b7234f0d02832dde4935bd6277f1f51c 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/lps25h.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/lps25h.c
@@ -174,14 +174,15 @@ bool lps25hGetData(float* pressure, float* temperature, float* asl)
*/
float lps25hPressureToAltitude(float* pressure/*, float* ground_pressure, float* ground_temp*/)
{
- if (*pressure > 0)
- {
- //return (1.f - pow(*pressure / CONST_SEA_PRESSURE, CONST_PF)) * CONST_PF2;
- //return ((pow((1015.7 / *pressure), CONST_PF) - 1.0) * (25. + 273.15)) / 0.0065;
- return ((powf((1015.7f / *pressure), CONST_PF) - 1.0f) * (FIX_TEMP + 273.15f)) / 0.0065f;
- }
- else
- {
+ //COMMENTED FIRMWARE
+ // if (*pressure > 0)
+ // {
+ // //return (1.f - pow(*pressure / CONST_SEA_PRESSURE, CONST_PF)) * CONST_PF2;
+ // //return ((pow((1015.7 / *pressure), CONST_PF) - 1.0) * (25. + 273.15)) / 0.0065;
+ // return ((powf((1015.7f / *pressure), CONST_PF) - 1.0f) * (FIX_TEMP + 273.15f)) / 0.0065f;
+ // }
+ // else
+ // {
return 0;
- }
+ //}
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/motors_def_cf2.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/motors_def_cf2.c
index 73166e6655d9314cf9f1518ccf2862cb95380932..bb91988ce6c8233379e71d9e2d754fe81e7bbf83 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/motors_def_cf2.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/drivers/src/motors_def_cf2.c
@@ -26,558 +26,559 @@
* This code mainly interfacing the PWM peripheral lib of ST.
*/
// Connector M1, PA1, TIM2_CH2
-static const MotorPerifDef CONN_M1 =
-{
- .drvType = BRUSHED,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_1,
- .gpioPinSource = GPIO_PinSource1,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM2,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_PWM_PERIOD,
- .timPrescaler = MOTORS_PWM_PRESCALE,
- .setCompare = TIM_SetCompare2,
- .getCompare = TIM_GetCapture2,
- .ocInit = TIM_OC2Init,
- .preloadConfig = TIM_OC2PreloadConfig,
-};
+//COMMENTED FIRMWARE
+// static const MotorPerifDef CONN_M1 =
+// {
+// // .drvType = BRUSHED,
+// // .gpioPerif = RCC_AHB1Periph_GPIOA,
+// // .gpioPort = GPIOA,
+// // .gpioPin = GPIO_Pin_1,
+// // .gpioPinSource = GPIO_PinSource1,
+// // .gpioOType = GPIO_OType_PP,
+// // .gpioAF = GPIO_AF_TIM2,
+// // .timPerif = RCC_APB1Periph_TIM2,
+// // .tim = TIM2,
+// // .timPolarity = TIM_OCPolarity_High,
+// // .timDbgStop = DBGMCU_TIM2_STOP,
+// // .timPeriod = MOTORS_PWM_PERIOD,
+// // .timPrescaler = MOTORS_PWM_PRESCALE,
+// // .setCompare = TIM_SetCompare2,
+// // .getCompare = TIM_GetCapture2,
+// // .ocInit = TIM_OC2Init,
+// // .preloadConfig = TIM_OC2PreloadConfig,
+// };
-// Connector M2, PB11, TIM2_CH4
-static const MotorPerifDef CONN_M2 =
-{
- .drvType = BRUSHED,
- .gpioPerif = RCC_AHB1Periph_GPIOB,
- .gpioPort = GPIOB,
- .gpioPin = GPIO_Pin_11,
- .gpioPinSource = GPIO_PinSource11,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM2,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_PWM_PERIOD,
- .timPrescaler = MOTORS_PWM_PRESCALE,
- .setCompare = TIM_SetCompare4,
- .getCompare = TIM_GetCapture4,
- .ocInit = TIM_OC4Init,
- .preloadConfig = TIM_OC4PreloadConfig,
-};
+// // Connector M2, PB11, TIM2_CH4
+// static const MotorPerifDef CONN_M2 =
+// {
+// // .drvType = BRUSHED,
+// // .gpioPerif = RCC_AHB1Periph_GPIOB,
+// // .gpioPort = GPIOB,
+// // .gpioPin = GPIO_Pin_11,
+// // .gpioPinSource = GPIO_PinSource11,
+// // .gpioOType = GPIO_OType_PP,
+// // .gpioAF = GPIO_AF_TIM2,
+// // .timPerif = RCC_APB1Periph_TIM2,
+// // .tim = TIM2,
+// // .timPolarity = TIM_OCPolarity_High,
+// // .timDbgStop = DBGMCU_TIM2_STOP,
+// // .timPeriod = MOTORS_PWM_PERIOD,
+// // .timPrescaler = MOTORS_PWM_PRESCALE,
+// // .setCompare = TIM_SetCompare4,
+// // .getCompare = TIM_GetCapture4,
+// // .ocInit = TIM_OC4Init,
+// // .preloadConfig = TIM_OC4PreloadConfig,
+// };
-// Connector M3, PA15, TIM2_CH1
-static const MotorPerifDef CONN_M3 =
-{
- .drvType = BRUSHED,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_15,
- .gpioPinSource = GPIO_PinSource15,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM2,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_PWM_PERIOD,
- .timPrescaler = MOTORS_PWM_PRESCALE,
- .setCompare = TIM_SetCompare1,
- .getCompare = TIM_GetCapture1,
- .ocInit = TIM_OC1Init,
- .preloadConfig = TIM_OC1PreloadConfig,
-};
+// // Connector M3, PA15, TIM2_CH1
+// static const MotorPerifDef CONN_M3 =
+// {
+// // .drvType = BRUSHED,
+// // .gpioPerif = RCC_AHB1Periph_GPIOA,
+// // .gpioPort = GPIOA,
+// // .gpioPin = GPIO_Pin_15,
+// // .gpioPinSource = GPIO_PinSource15,
+// // .gpioOType = GPIO_OType_PP,
+// // .gpioAF = GPIO_AF_TIM2,
+// // .timPerif = RCC_APB1Periph_TIM2,
+// // .tim = TIM2,
+// // .timPolarity = TIM_OCPolarity_High,
+// // .timDbgStop = DBGMCU_TIM2_STOP,
+// // .timPeriod = MOTORS_PWM_PERIOD,
+// // .timPrescaler = MOTORS_PWM_PRESCALE,
+// // .setCompare = TIM_SetCompare1,
+// // .getCompare = TIM_GetCapture1,
+// // .ocInit = TIM_OC1Init,
+// // .preloadConfig = TIM_OC1PreloadConfig,
+// };
-// Connector M4, PB9, TIM4_CH4
-static const MotorPerifDef CONN_M4 =
-{
- .drvType = BRUSHED,
- .gpioPerif = RCC_AHB1Periph_GPIOB,
- .gpioPort = GPIOB,
- .gpioPin = GPIO_Pin_9,
- .gpioPinSource = GPIO_PinSource9,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM4,
- .timPerif = RCC_APB1Periph_TIM4,
- .tim = TIM4,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM4_STOP,
- .timPeriod = MOTORS_PWM_PERIOD,
- .timPrescaler = MOTORS_PWM_PRESCALE,
- .setCompare = TIM_SetCompare4,
- .getCompare = TIM_GetCapture4,
- .ocInit = TIM_OC4Init,
- .preloadConfig = TIM_OC4PreloadConfig,
-};
+// // Connector M4, PB9, TIM4_CH4
+// static const MotorPerifDef CONN_M4 =
+// {
+// // .drvType = BRUSHED,
+// // .gpioPerif = RCC_AHB1Periph_GPIOB,
+// // .gpioPort = GPIOB,
+// // .gpioPin = GPIO_Pin_9,
+// // .gpioPinSource = GPIO_PinSource9,
+// // .gpioOType = GPIO_OType_PP,
+// // .gpioAF = GPIO_AF_TIM4,
+// // .timPerif = RCC_APB1Periph_TIM4,
+// // .tim = TIM4,
+// // .timPolarity = TIM_OCPolarity_High,
+// // .timDbgStop = DBGMCU_TIM4_STOP,
+// // .timPeriod = MOTORS_PWM_PERIOD,
+// // .timPrescaler = MOTORS_PWM_PRESCALE,
+// // .setCompare = TIM_SetCompare4,
+// // .getCompare = TIM_GetCapture4,
+// // .ocInit = TIM_OC4Init,
+// // .preloadConfig = TIM_OC4PreloadConfig,
+// };
-// Connector M1, PA1, TIM2_CH2, Brushless config, inversed
-static const MotorPerifDef CONN_M1_BL_INV =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_1,
- .gpioPinSource = GPIO_PinSource1,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM2,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_Low,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare2,
- .getCompare = TIM_GetCapture2,
- .ocInit = TIM_OC2Init,
- .preloadConfig = TIM_OC2PreloadConfig,
-};
+// // Connector M1, PA1, TIM2_CH2, Brushless config, inversed
+// static const MotorPerifDef CONN_M1_BL_INV =
+// {
+// // .drvType = BRUSHLESS,
+// // .gpioPerif = RCC_AHB1Periph_GPIOA,
+// // .gpioPort = GPIOA,
+// // .gpioPin = GPIO_Pin_1,
+// // .gpioPinSource = GPIO_PinSource1,
+// // .gpioOType = GPIO_OType_PP,
+// // .gpioAF = GPIO_AF_TIM2,
+// // .timPerif = RCC_APB1Periph_TIM2,
+// // .tim = TIM2,
+// // .timPolarity = TIM_OCPolarity_Low,
+// // .timDbgStop = DBGMCU_TIM2_STOP,
+// // .timPeriod = MOTORS_BL_PWM_PERIOD,
+// // .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// // .setCompare = TIM_SetCompare2,
+// // .getCompare = TIM_GetCapture2,
+// // .ocInit = TIM_OC2Init,
+// // .preloadConfig = TIM_OC2PreloadConfig,
+// };
-// Connector M2, PB11, TIM2_CH4, Brushless config, inversed
-static const MotorPerifDef CONN_M2_BL_INV =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOB,
- .gpioPort = GPIOB,
- .gpioPin = GPIO_Pin_11,
- .gpioPinSource = GPIO_PinSource11,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM2,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_Low,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare4,
- .getCompare = TIM_GetCapture4,
- .ocInit = TIM_OC4Init,
- .preloadConfig = TIM_OC4PreloadConfig,
-};
+// // Connector M2, PB11, TIM2_CH4, Brushless config, inversed
+// static const MotorPerifDef CONN_M2_BL_INV =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOB,
+// .gpioPort = GPIOB,
+// .gpioPin = GPIO_Pin_11,
+// .gpioPinSource = GPIO_PinSource11,
+// .gpioOType = GPIO_OType_PP,
+// .gpioAF = GPIO_AF_TIM2,
+// .timPerif = RCC_APB1Periph_TIM2,
+// .tim = TIM2,
+// .timPolarity = TIM_OCPolarity_Low,
+// .timDbgStop = DBGMCU_TIM2_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare4,
+// .getCompare = TIM_GetCapture4,
+// .ocInit = TIM_OC4Init,
+// .preloadConfig = TIM_OC4PreloadConfig,
+// };
-// Connector M3, PA15, TIM2_CH1, Brushless config, inversed
-static const MotorPerifDef CONN_M3_BL_INV =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_15,
- .gpioPinSource = GPIO_PinSource15,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM2,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_Low,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare1,
- .getCompare = TIM_GetCapture1,
- .ocInit = TIM_OC1Init,
- .preloadConfig = TIM_OC1PreloadConfig,
-};
+// // Connector M3, PA15, TIM2_CH1, Brushless config, inversed
+// static const MotorPerifDef CONN_M3_BL_INV =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPort = GPIOA,
+// .gpioPin = GPIO_Pin_15,
+// .gpioPinSource = GPIO_PinSource15,
+// .gpioOType = GPIO_OType_PP,
+// .gpioAF = GPIO_AF_TIM2,
+// .timPerif = RCC_APB1Periph_TIM2,
+// .tim = TIM2,
+// .timPolarity = TIM_OCPolarity_Low,
+// .timDbgStop = DBGMCU_TIM2_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare1,
+// .getCompare = TIM_GetCapture1,
+// .ocInit = TIM_OC1Init,
+// .preloadConfig = TIM_OC1PreloadConfig,
+// };
-// Connector M4, PB9, TIM4_CH4, Brushless config, inversed
-static const MotorPerifDef CONN_M4_BL_INV =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOB,
- .gpioPort = GPIOB,
- .gpioPin = GPIO_Pin_9,
- .gpioPinSource = GPIO_PinSource9,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM4,
- .timPerif = RCC_APB1Periph_TIM4,
- .tim = TIM4,
- .timPolarity = TIM_OCPolarity_Low,
- .timDbgStop = DBGMCU_TIM4_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare4,
- .getCompare = TIM_GetCapture4,
- .ocInit = TIM_OC4Init,
- .preloadConfig = TIM_OC4PreloadConfig,
-};
+// // Connector M4, PB9, TIM4_CH4, Brushless config, inversed
+// static const MotorPerifDef CONN_M4_BL_INV =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOB,
+// .gpioPort = GPIOB,
+// .gpioPin = GPIO_Pin_9,
+// .gpioPinSource = GPIO_PinSource9,
+// .gpioOType = GPIO_OType_PP,
+// .gpioAF = GPIO_AF_TIM4,
+// .timPerif = RCC_APB1Periph_TIM4,
+// .tim = TIM4,
+// .timPolarity = TIM_OCPolarity_Low,
+// .timDbgStop = DBGMCU_TIM4_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare4,
+// .getCompare = TIM_GetCapture4,
+// .ocInit = TIM_OC4Init,
+// .preloadConfig = TIM_OC4PreloadConfig,
+// };
-// Bolt M1, PA1, TIM2_CH2, Brushless config
-static const MotorPerifDef BOLT_M1_BL =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_1,
- .gpioPinSource = GPIO_PinSource1,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM2,
- .gpioPowerswitchPerif = RCC_AHB1Periph_GPIOA,
- .gpioPowerswitchPort = GPIOA,
- .gpioPowerswitchPin = GPIO_Pin_0,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare2,
- .getCompare = TIM_GetCapture2,
- .ocInit = TIM_OC2Init,
- .preloadConfig = TIM_OC2PreloadConfig,
-};
+// // Bolt M1, PA1, TIM2_CH2, Brushless config
+// static const MotorPerifDef BOLT_M1_BL =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPort = GPIOA,
+// .gpioPin = GPIO_Pin_1,
+// .gpioPinSource = GPIO_PinSource1,
+// .gpioOType = GPIO_OType_PP,
+// .gpioAF = GPIO_AF_TIM2,
+// .gpioPowerswitchPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPowerswitchPort = GPIOA,
+// .gpioPowerswitchPin = GPIO_Pin_0,
+// .timPerif = RCC_APB1Periph_TIM2,
+// .tim = TIM2,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM2_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare2,
+// .getCompare = TIM_GetCapture2,
+// .ocInit = TIM_OC2Init,
+// .preloadConfig = TIM_OC2PreloadConfig,
+// };
-// Bolt M2, PB11, TIM2_CH4, Brushless config
-static const MotorPerifDef BOLT_M2_BL =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOB,
- .gpioPort = GPIOB,
- .gpioPin = GPIO_Pin_11,
- .gpioPinSource = GPIO_PinSource11,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM2,
- .gpioPowerswitchPerif = RCC_AHB1Periph_GPIOB,
- .gpioPowerswitchPort = GPIOB,
- .gpioPowerswitchPin = GPIO_Pin_12,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare4,
- .getCompare = TIM_GetCapture4,
- .ocInit = TIM_OC4Init,
- .preloadConfig = TIM_OC4PreloadConfig,
-};
+// // Bolt M2, PB11, TIM2_CH4, Brushless config
+// static const MotorPerifDef BOLT_M2_BL =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOB,
+// .gpioPort = GPIOB,
+// .gpioPin = GPIO_Pin_11,
+// .gpioPinSource = GPIO_PinSource11,
+// .gpioOType = GPIO_OType_PP,
+// .gpioAF = GPIO_AF_TIM2,
+// .gpioPowerswitchPerif = RCC_AHB1Periph_GPIOB,
+// .gpioPowerswitchPort = GPIOB,
+// .gpioPowerswitchPin = GPIO_Pin_12,
+// .timPerif = RCC_APB1Periph_TIM2,
+// .tim = TIM2,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM2_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare4,
+// .getCompare = TIM_GetCapture4,
+// .ocInit = TIM_OC4Init,
+// .preloadConfig = TIM_OC4PreloadConfig,
+// };
-// Bolt M3, PA15, TIM2_CH1, Brushless config
-static const MotorPerifDef BOLT_M3_BL =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_15,
- .gpioPinSource = GPIO_PinSource15,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM2,
- .gpioPowerswitchPerif = RCC_AHB1Periph_GPIOC,
- .gpioPowerswitchPort = GPIOC,
- .gpioPowerswitchPin = GPIO_Pin_8,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare1,
- .getCompare = TIM_GetCapture1,
- .ocInit = TIM_OC1Init,
- .preloadConfig = TIM_OC1PreloadConfig,
-};
+// // Bolt M3, PA15, TIM2_CH1, Brushless config
+// static const MotorPerifDef BOLT_M3_BL =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPort = GPIOA,
+// .gpioPin = GPIO_Pin_15,
+// .gpioPinSource = GPIO_PinSource15,
+// .gpioOType = GPIO_OType_PP,
+// .gpioAF = GPIO_AF_TIM2,
+// .gpioPowerswitchPerif = RCC_AHB1Periph_GPIOC,
+// .gpioPowerswitchPort = GPIOC,
+// .gpioPowerswitchPin = GPIO_Pin_8,
+// .timPerif = RCC_APB1Periph_TIM2,
+// .tim = TIM2,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM2_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare1,
+// .getCompare = TIM_GetCapture1,
+// .ocInit = TIM_OC1Init,
+// .preloadConfig = TIM_OC1PreloadConfig,
+// };
-// Bolt M4, PB9, TIM4_CH4, Brushless config
-static const MotorPerifDef BOLT_M4_BL =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOB,
- .gpioPort = GPIOB,
- .gpioPin = GPIO_Pin_9,
- .gpioPinSource = GPIO_PinSource9,
- .gpioOType = GPIO_OType_PP,
- .gpioAF = GPIO_AF_TIM4,
- .gpioPowerswitchPerif = RCC_AHB1Periph_GPIOC,
- .gpioPowerswitchPort = GPIOC,
- .gpioPowerswitchPin = GPIO_Pin_15,
- .timPerif = RCC_APB1Periph_TIM4,
- .tim = TIM4,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM4_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare4,
- .getCompare = TIM_GetCapture4,
- .ocInit = TIM_OC4Init,
- .preloadConfig = TIM_OC4PreloadConfig,
-};
+// // Bolt M4, PB9, TIM4_CH4, Brushless config
+// static const MotorPerifDef BOLT_M4_BL =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOB,
+// .gpioPort = GPIOB,
+// .gpioPin = GPIO_Pin_9,
+// .gpioPinSource = GPIO_PinSource9,
+// .gpioOType = GPIO_OType_PP,
+// .gpioAF = GPIO_AF_TIM4,
+// .gpioPowerswitchPerif = RCC_AHB1Periph_GPIOC,
+// .gpioPowerswitchPort = GPIOC,
+// .gpioPowerswitchPin = GPIO_Pin_15,
+// .timPerif = RCC_APB1Periph_TIM4,
+// .tim = TIM4,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM4_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare4,
+// .getCompare = TIM_GetCapture4,
+// .ocInit = TIM_OC4Init,
+// .preloadConfig = TIM_OC4PreloadConfig,
+// };
-// Deck TX2, PA2, TIM2_CH3
-static const MotorPerifDef DECK_TX2_TIM2 =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_2,
- .gpioPinSource = GPIO_PinSource2,
- .gpioOType = GPIO_OType_OD,
- .gpioAF = GPIO_AF_TIM2,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare3,
- .getCompare = TIM_GetCapture3,
- .ocInit = TIM_OC3Init,
- .preloadConfig = TIM_OC3PreloadConfig,
-};
+// // Deck TX2, PA2, TIM2_CH3
+// static const MotorPerifDef DECK_TX2_TIM2 =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPort = GPIOA,
+// .gpioPin = GPIO_Pin_2,
+// .gpioPinSource = GPIO_PinSource2,
+// .gpioOType = GPIO_OType_OD,
+// .gpioAF = GPIO_AF_TIM2,
+// .timPerif = RCC_APB1Periph_TIM2,
+// .tim = TIM2,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM2_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare3,
+// .getCompare = TIM_GetCapture3,
+// .ocInit = TIM_OC3Init,
+// .preloadConfig = TIM_OC3PreloadConfig,
+// };
-// Deck TX2, PA2, TIM5_CH3
-static const MotorPerifDef DECK_TX2_TIM5 =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_2,
- .gpioPinSource = GPIO_PinSource2,
- .gpioOType = GPIO_OType_OD,
- .gpioAF = GPIO_AF_TIM5,
- .timPerif = RCC_APB1Periph_TIM5,
- .tim = TIM5,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM5_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare3,
- .getCompare = TIM_GetCapture3,
- .ocInit = TIM_OC3Init,
- .preloadConfig = TIM_OC3PreloadConfig,
-};
+// // Deck TX2, PA2, TIM5_CH3
+// static const MotorPerifDef DECK_TX2_TIM5 =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPort = GPIOA,
+// .gpioPin = GPIO_Pin_2,
+// .gpioPinSource = GPIO_PinSource2,
+// .gpioOType = GPIO_OType_OD,
+// .gpioAF = GPIO_AF_TIM5,
+// .timPerif = RCC_APB1Periph_TIM5,
+// .tim = TIM5,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM5_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare3,
+// .getCompare = TIM_GetCapture3,
+// .ocInit = TIM_OC3Init,
+// .preloadConfig = TIM_OC3PreloadConfig,
+// };
-// Deck RX2, PA3, TIM2_CH4
-static const MotorPerifDef DECK_RX2_TIM2 =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_3,
- .gpioPinSource = GPIO_PinSource3,
- .gpioOType = GPIO_OType_OD,
- .gpioAF = GPIO_AF_TIM2,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare4,
- .getCompare = TIM_GetCapture4,
- .ocInit = TIM_OC4Init,
- .preloadConfig = TIM_OC4PreloadConfig,
-};
+// // Deck RX2, PA3, TIM2_CH4
+// static const MotorPerifDef DECK_RX2_TIM2 =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPort = GPIOA,
+// .gpioPin = GPIO_Pin_3,
+// .gpioPinSource = GPIO_PinSource3,
+// .gpioOType = GPIO_OType_OD,
+// .gpioAF = GPIO_AF_TIM2,
+// .timPerif = RCC_APB1Periph_TIM2,
+// .tim = TIM2,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM2_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare4,
+// .getCompare = TIM_GetCapture4,
+// .ocInit = TIM_OC4Init,
+// .preloadConfig = TIM_OC4PreloadConfig,
+// };
-// Deck RX2, PA3, TIM5_CH4
-static const MotorPerifDef DECK_RX2_TIM5 =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_3,
- .gpioPinSource = GPIO_PinSource3,
- .gpioOType = GPIO_OType_OD,
- .gpioAF = GPIO_AF_TIM5,
- .timPerif = RCC_APB1Periph_TIM5,
- .tim = TIM5,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM5_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare4,
- .getCompare = TIM_GetCapture4,
- .ocInit = TIM_OC4Init,
- .preloadConfig = TIM_OC4PreloadConfig,
-};
+// // Deck RX2, PA3, TIM5_CH4
+// static const MotorPerifDef DECK_RX2_TIM5 =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPort = GPIOA,
+// .gpioPin = GPIO_Pin_3,
+// .gpioPinSource = GPIO_PinSource3,
+// .gpioOType = GPIO_OType_OD,
+// .gpioAF = GPIO_AF_TIM5,
+// .timPerif = RCC_APB1Periph_TIM5,
+// .tim = TIM5,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM5_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare4,
+// .getCompare = TIM_GetCapture4,
+// .ocInit = TIM_OC4Init,
+// .preloadConfig = TIM_OC4PreloadConfig,
+// };
-// Deck IO1, PB8, TIM4_CH3
-static const MotorPerifDef DECK_IO1_TIM4 =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOB,
- .gpioPort = GPIOB,
- .gpioPin = GPIO_Pin_8,
- .gpioPinSource = GPIO_PinSource8,
- .gpioOType = GPIO_OType_OD,
- .gpioAF = GPIO_AF_TIM4,
- .timPerif = RCC_APB1Periph_TIM4,
- .tim = TIM4,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM4_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare3,
- .getCompare = TIM_GetCapture3,
- .ocInit = TIM_OC3Init,
- .preloadConfig = TIM_OC3PreloadConfig,
-};
+// // Deck IO1, PB8, TIM4_CH3
+// static const MotorPerifDef DECK_IO1_TIM4 =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOB,
+// .gpioPort = GPIOB,
+// .gpioPin = GPIO_Pin_8,
+// .gpioPinSource = GPIO_PinSource8,
+// .gpioOType = GPIO_OType_OD,
+// .gpioAF = GPIO_AF_TIM4,
+// .timPerif = RCC_APB1Periph_TIM4,
+// .tim = TIM4,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM4_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare3,
+// .getCompare = TIM_GetCapture3,
+// .ocInit = TIM_OC3Init,
+// .preloadConfig = TIM_OC3PreloadConfig,
+// };
-// Deck IO2, PB5, TIM3_CH2
-static const MotorPerifDef DECK_IO2 =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOB,
- .gpioPort = GPIOB,
- .gpioPin = GPIO_Pin_5,
- .gpioPinSource = GPIO_PinSource5,
- .gpioOType = GPIO_OType_OD,
- .gpioAF = GPIO_AF_TIM3,
- .timPerif = RCC_APB1Periph_TIM3,
- .tim = TIM3,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM3_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare2,
- .getCompare = TIM_GetCapture2,
- .ocInit = TIM_OC2Init,
- .preloadConfig = TIM_OC2PreloadConfig,
-};
+// // Deck IO2, PB5, TIM3_CH2
+// static const MotorPerifDef DECK_IO2 =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOB,
+// .gpioPort = GPIOB,
+// .gpioPin = GPIO_Pin_5,
+// .gpioPinSource = GPIO_PinSource5,
+// .gpioOType = GPIO_OType_OD,
+// .gpioAF = GPIO_AF_TIM3,
+// .timPerif = RCC_APB1Periph_TIM3,
+// .tim = TIM3,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM3_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare2,
+// .getCompare = TIM_GetCapture2,
+// .ocInit = TIM_OC2Init,
+// .preloadConfig = TIM_OC2PreloadConfig,
+// };
-// Deck IO3, PB4, TIM3_CH1
-static const MotorPerifDef DECK_IO3 =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOB,
- .gpioPort = GPIOB,
- .gpioPin = GPIO_Pin_4,
- .gpioPinSource = GPIO_PinSource4,
- .gpioOType = GPIO_OType_OD,
- .gpioAF = GPIO_AF_TIM3,
- .timPerif = RCC_APB1Periph_TIM3,
- .tim = TIM3,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM3_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare1,
- .getCompare = TIM_GetCapture1,
- .ocInit = TIM_OC1Init,
- .preloadConfig = TIM_OC1PreloadConfig,
-};
+// // Deck IO3, PB4, TIM3_CH1
+// static const MotorPerifDef DECK_IO3 =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOB,
+// .gpioPort = GPIOB,
+// .gpioPin = GPIO_Pin_4,
+// .gpioPinSource = GPIO_PinSource4,
+// .gpioOType = GPIO_OType_OD,
+// .gpioAF = GPIO_AF_TIM3,
+// .timPerif = RCC_APB1Periph_TIM3,
+// .tim = TIM3,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM3_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare1,
+// .getCompare = TIM_GetCapture1,
+// .ocInit = TIM_OC1Init,
+// .preloadConfig = TIM_OC1PreloadConfig,
+// };
-// Deck SCK, PA5, TIM2_CH1
-static const MotorPerifDef DECK_SCK =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_5,
- .gpioPinSource = GPIO_PinSource5,
- .gpioOType = GPIO_OType_OD,
- .gpioAF = GPIO_AF_TIM2,
- .timPerif = RCC_APB1Periph_TIM2,
- .tim = TIM2,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM2_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare1,
- .getCompare = TIM_GetCapture1,
- .ocInit = TIM_OC1Init,
- .preloadConfig = TIM_OC1PreloadConfig,
-};
+// // Deck SCK, PA5, TIM2_CH1
+// static const MotorPerifDef DECK_SCK =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPort = GPIOA,
+// .gpioPin = GPIO_Pin_5,
+// .gpioPinSource = GPIO_PinSource5,
+// .gpioOType = GPIO_OType_OD,
+// .gpioAF = GPIO_AF_TIM2,
+// .timPerif = RCC_APB1Periph_TIM2,
+// .tim = TIM2,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM2_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare1,
+// .getCompare = TIM_GetCapture1,
+// .ocInit = TIM_OC1Init,
+// .preloadConfig = TIM_OC1PreloadConfig,
+// };
-// Deck MISO, PA6, TIM3_CH1
-static const MotorPerifDef DECK_MISO =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_6,
- .gpioPinSource = GPIO_PinSource6,
- .gpioOType = GPIO_OType_OD,
- .gpioAF = GPIO_AF_TIM3,
- .timPerif = RCC_APB1Periph_TIM3,
- .tim = TIM3,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM3_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare1,
- .getCompare = TIM_GetCapture1,
- .ocInit = TIM_OC1Init,
- .preloadConfig = TIM_OC1PreloadConfig,
-};
+// // Deck MISO, PA6, TIM3_CH1
+// static const MotorPerifDef DECK_MISO =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPort = GPIOA,
+// .gpioPin = GPIO_Pin_6,
+// .gpioPinSource = GPIO_PinSource6,
+// .gpioOType = GPIO_OType_OD,
+// .gpioAF = GPIO_AF_TIM3,
+// .timPerif = RCC_APB1Periph_TIM3,
+// .tim = TIM3,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM3_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare1,
+// .getCompare = TIM_GetCapture1,
+// .ocInit = TIM_OC1Init,
+// .preloadConfig = TIM_OC1PreloadConfig,
+// };
-// Deck MOSI, PA7, TIM14_CH1
-static const MotorPerifDef DECK_MOSI =
-{
- .drvType = BRUSHLESS,
- .gpioPerif = RCC_AHB1Periph_GPIOA,
- .gpioPort = GPIOA,
- .gpioPin = GPIO_Pin_7,
- .gpioPinSource = GPIO_PinSource7,
- .gpioOType = GPIO_OType_OD,
- .gpioAF = GPIO_AF_TIM14,
- .timPerif = RCC_APB1Periph_TIM14,
- .tim = TIM14,
- .timPolarity = TIM_OCPolarity_High,
- .timDbgStop = DBGMCU_TIM14_STOP,
- .timPeriod = MOTORS_BL_PWM_PERIOD,
- .timPrescaler = MOTORS_BL_PWM_PRESCALE,
- .setCompare = TIM_SetCompare1,
- .getCompare = TIM_GetCapture1,
- .ocInit = TIM_OC1Init,
- .preloadConfig = TIM_OC1PreloadConfig,
-};
+// // Deck MOSI, PA7, TIM14_CH1
+// static const MotorPerifDef DECK_MOSI =
+// {
+// .drvType = BRUSHLESS,
+// .gpioPerif = RCC_AHB1Periph_GPIOA,
+// .gpioPort = GPIOA,
+// .gpioPin = GPIO_Pin_7,
+// .gpioPinSource = GPIO_PinSource7,
+// .gpioOType = GPIO_OType_OD,
+// .gpioAF = GPIO_AF_TIM14,
+// .timPerif = RCC_APB1Periph_TIM14,
+// .tim = TIM14,
+// .timPolarity = TIM_OCPolarity_High,
+// .timDbgStop = DBGMCU_TIM14_STOP,
+// .timPeriod = MOTORS_BL_PWM_PERIOD,
+// .timPrescaler = MOTORS_BL_PWM_PRESCALE,
+// .setCompare = TIM_SetCompare1,
+// .getCompare = TIM_GetCapture1,
+// .ocInit = TIM_OC1Init,
+// .preloadConfig = TIM_OC1PreloadConfig,
+// };
-/**
- * Mapping for Tags that don't have motors.
- * Actually same mapping as for CF2 but the pins are not connected.
- */
-const MotorPerifDef* motorMapNoMotors[NBR_OF_MOTORS] =
-{
- &CONN_M1,
- &CONN_M2,
- &CONN_M3,
- &CONN_M4
-};
+// /**
+// * Mapping for Tags that don't have motors.
+// * Actually same mapping as for CF2 but the pins are not connected.
+// */
+// const MotorPerifDef* motorMapNoMotors[NBR_OF_MOTORS] =
+// {
+// &CONN_M1,
+// &CONN_M2,
+// &CONN_M3,
+// &CONN_M4
+// };
-/**
- * Default brushed mapping to M1-M4 connectors.
- */
-const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS] =
-{
- &CONN_M1,
- &CONN_M2,
- &CONN_M3,
- &CONN_M4
-};
+// /**
+// * Default brushed mapping to M1-M4 connectors.
+// */
+// const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS] =
+// {
+// &CONN_M1,
+// &CONN_M2,
+// &CONN_M3,
+// &CONN_M4
+// };
-/**
- * Brushless motors mapped as on the Big-Quad deck
- * M1 -> TX2
- * M2 -> IO3
- * M3 -> IO2
- * M4 -> RX2
- */
-const MotorPerifDef* motorMapBigQuadDeck[NBR_OF_MOTORS] =
-{
- &DECK_TX2_TIM2,
- &DECK_IO3,
- &DECK_IO2,
- &DECK_RX2_TIM2
-};
+// /**
+// * Brushless motors mapped as on the Big-Quad deck
+// * M1 -> TX2
+// * M2 -> IO3
+// * M3 -> IO2
+// * M4 -> RX2
+// */
+// const MotorPerifDef* motorMapBigQuadDeck[NBR_OF_MOTORS] =
+// {
+// &DECK_TX2_TIM2,
+// &DECK_IO3,
+// &DECK_IO2,
+// &DECK_RX2_TIM2
+// };
-/**
- * Brushless motors mapped to the standard motor connectors with pull-ups (~1K) to VBAT soldered.
- */
-const MotorPerifDef* motorMapDefaltConBrushless[NBR_OF_MOTORS] =
-{
- &CONN_M1_BL_INV,
- &CONN_M2_BL_INV,
- &CONN_M3_BL_INV,
- &CONN_M4_BL_INV
-};
+// /**
+// * Brushless motors mapped to the standard motor connectors with pull-ups (~1K) to VBAT soldered.
+// */
+// const MotorPerifDef* motorMapDefaltConBrushless[NBR_OF_MOTORS] =
+// {
+// &CONN_M1_BL_INV,
+// &CONN_M2_BL_INV,
+// &CONN_M3_BL_INV,
+// &CONN_M4_BL_INV
+// };
-/**
- * Brushless motors mapped to the Bolt PWM outputs.
- */
-const MotorPerifDef* motorMapBoltBrushless[NBR_OF_MOTORS] =
-{
- &BOLT_M1_BL,
- &BOLT_M2_BL,
- &BOLT_M3_BL,
- &BOLT_M4_BL
-};
+// /**
+// * Brushless motors mapped to the Bolt PWM outputs.
+// */
+// const MotorPerifDef* motorMapBoltBrushless[NBR_OF_MOTORS] =
+// {
+// &BOLT_M1_BL,
+// &BOLT_M2_BL,
+// &BOLT_M3_BL,
+// &BOLT_M4_BL
+// };
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/interface/pm.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/interface/pm.h
index 707af90ca9e0008dac258ecf1f8e8e6e259d38a3..61b90519c7a8e5c57d2bd9566fdd76bf9d0827e4 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/interface/pm.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/interface/pm.h
@@ -48,13 +48,15 @@
#define PM_BAT_LOW_VOLTAGE LOW_VOLTAGE
#endif
#ifndef LOW_TIMEOUT
- #define PM_BAT_LOW_TIMEOUT M2T(1000 * 5) // 5 sec default
+//COMMENTED FIRMWARE
+ #define PM_BAT_LOW_TIMEOUT 1000 * 5//M2T(1000 * 5) // 5 sec default
#else
#define PM_BAT_LOW_TIMEOUT LOW_TIMEOUT
#endif
#ifndef SYSTEM_SHUTDOWN_TIMEOUT
- #define PM_SYSTEM_SHUTDOWN_TIMEOUT M2T(1000 * 60 * 5) // 5 min default
+//COMMENTED FIRMWARE
+ #define PM_SYSTEM_SHUTDOWN_TIMEOUT 1000 * 60 * 5//M2T(1000 * 60 * 5) // 5 min default
#else
#define PM_SYSTEM_SHUTDOWN_TIMEOUT M2T(1000 * 60 * SYSTEM_SHUTDOWN_TIMEOUT)
#endif
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 2d3b89a96a9bb34b514f1d62a3705ac4f9ce4b88..b858b5f9af3310933dd13f2798fbe6a6c2533ef9 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
@@ -55,7 +55,8 @@ void owInit()
{
syslinkInit();
vSemaphoreCreateBinary(waitForReply);
- lockCmdBuf = xSemaphoreCreateMutexStatic(&lockCmdBufBuffer);
+ //COMMENTED FIRMWARE
+ //lockCmdBuf = xSemaphoreCreateMutexStatic(&lockCmdBufBuffer);
// Put reply semaphore in right state.
xSemaphoreTake(waitForReply, portMAX_DELAY);
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/pm_stm32f4.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/pm_stm32f4.c
index d522e268d29a14bdec7bbc8ade3d4817e56e6995..0808c45bf48eeb7d29bc28070df2c28db57d8468 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/pm_stm32f4.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/hal/src/pm_stm32f4.c
@@ -113,7 +113,8 @@ void pmInit(void)
return;
}
- STATIC_MEM_TASK_CREATE(pmTask, pmTask, PM_TASK_NAME, NULL, PM_TASK_PRI);
+ //COMMENTED FIRMWARE
+ //STATIC_MEM_TASK_CREATE(pmTask, pmTask, PM_TASK_NAME, NULL, PM_TASK_PRI);
isInit = true;
//COMMENTED FIRMWARE
@@ -216,28 +217,29 @@ void pmSetChargeState(PMChargeStates chgState)
PMStates pmUpdateState()
{
PMStates state;
- bool isCharging = pmSyslinkInfo.chg;
- bool isPgood = pmSyslinkInfo.pgood;
- uint32_t batteryLowTime;
+ //COMMENTED FIRMWARE
+ // bool isCharging = pmSyslinkInfo.chg;
+ // bool isPgood = pmSyslinkInfo.pgood;
+ // uint32_t batteryLowTime;
- batteryLowTime = xTaskGetTickCount() - batteryLowTimeStamp;
+ // batteryLowTime = xTaskGetTickCount() - batteryLowTimeStamp;
- if (isPgood && !isCharging)
- {
- state = charged;
- }
- else if (isPgood && isCharging)
- {
- state = charging;
- }
- else if (!isPgood && !isCharging && (batteryLowTime > PM_BAT_LOW_TIMEOUT))
- {
- state = lowPower;
- }
- else
- {
- state = battery;
- }
+ // if (isPgood && !isCharging)
+ // {
+ // state = charged;
+ // }
+ // else if (isPgood && isCharging)
+ // {
+ // state = charging;
+ // }
+ // else if (!isPgood && !isCharging && (batteryLowTime > PM_BAT_LOW_TIMEOUT))
+ // {
+ // state = lowPower;
+ // }
+ // else
+ // {
+ // state = battery;
+ // }
return state;
}
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 9f1d3a30c3600da32e471671d47e0724be1230e0..0ecc3fd33ac2a888350b43214f2ad72362cd3247 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,6 +27,7 @@
#define DEBUG_MODULE "IMU"
#include <math.h>
+#include "empty_math.h"
#include "sensors_bmi088_bmp388.h"
#include "stm32fxxx.h"
@@ -234,10 +235,11 @@ static void sensorsAccelGet(Axis3i16* dataOut)
static void sensorsScaleBaro(baro_t* baroScaled, float pressure,
float temperature)
{
- baroScaled->pressure = pressure*0.01f;
- baroScaled->temperature = temperature;
- baroScaled->asl = ((powf((1015.7f / baroScaled->pressure), 0.1902630958f)
- - 1.0f) * (25.0f + 273.15f)) / 0.0065f;
+ //COMMENTED FIRMWARE
+ // baroScaled->pressure = pressure*0.01f;
+ // baroScaled->temperature = temperature;
+ // baroScaled->asl = ((powf((1015.7f / baroScaled->pressure), 0.1902630958f)
+ // - 1.0f) * (25.0f + 273.15f)) / 0.0065f;
}
bool sensorsBmi088Bmp388ReadGyro(Axis3f *gyro)
@@ -512,6 +514,7 @@ static void sensorsDeviceInit(void)
lpf2pInit(&accLpf[i], 1000, ACCEL_LPF_CUTOFF_FREQ);
}
+ //COMMENTED FIRMWARE
cosPitch = cosf(configblockGetCalibPitch() * (float) M_PI / 180);
sinPitch = sinf(configblockGetCalibPitch() * (float) M_PI / 180);
cosRoll = cosf(configblockGetCalibRoll() * (float) M_PI / 180);
@@ -644,17 +647,18 @@ bool sensorsBmi088Bmp388Test(void)
*/
static bool processAccScale(int16_t ax, int16_t ay, int16_t az)
{
- if (!accScaleFound)
- {
- accScaleSum += sqrtf(powf(ax * SENSORS_BMI088_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_BMI088_G_PER_LSB_CFG, 2) + powf(az * SENSORS_BMI088_G_PER_LSB_CFG, 2));
- accScaleSumCount++;
-
- if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
- {
- accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
- accScaleFound = true;
- }
- }
+ //COMMENTED FIRMWARE
+ // if (!accScaleFound)
+ // {
+ // accScaleSum += sqrtf(powf(ax * SENSORS_BMI088_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_BMI088_G_PER_LSB_CFG, 2) + powf(az * SENSORS_BMI088_G_PER_LSB_CFG, 2));
+ // accScaleSumCount++;
+
+ // if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
+ // {
+ // accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
+ // accScaleFound = true;
+ // }
+ // }
return accScaleFound;
}
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 c01eb7d9f81dd18480b29569ba35efbe80026bbd..85adbf7bec3cf97aba3faa4d905b41e0e3701644 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,6 +34,7 @@
#include "sensors_bosch.h"
#include <math.h>
+#include "empty_math.h"
#include "stm32fxxx.h"
@@ -764,24 +765,25 @@ static void calcMean(BiasObj* bias, Axis3f* mean) {
*/
static void calcVarianceAndMean(BiasObj* bias, Axis3f* variance, Axis3f* mean)
{
- Axis3i16* elem;
- int64_t sumSquared[GYRO_NBR_OF_AXES] = {0};
-
- for (elem = bias->bufStart;
- elem != (bias->bufStart+SENSORS_NBR_OF_BIAS_SAMPLES); elem++)
- {
- sumSquared[0] += elem->x * elem->x;
- sumSquared[1] += elem->y * elem->y;
- sumSquared[2] += elem->z * elem->z;
- }
- calcMean(bias, mean);
-
- variance->x = fabs(sumSquared[0] / SENSORS_NBR_OF_BIAS_SAMPLES
- - mean->x * mean->x);
- variance->y = fabs(sumSquared[1] / SENSORS_NBR_OF_BIAS_SAMPLES
- - mean->y * mean->y);
- variance->z = fabs(sumSquared[2] / SENSORS_NBR_OF_BIAS_SAMPLES
- - mean->z * mean->z);
+ //COMMENTED FIRMWARE
+ // Axis3i16* elem;
+ // int64_t sumSquared[GYRO_NBR_OF_AXES] = {0};
+
+ // for (elem = bias->bufStart;
+ // elem != (bias->bufStart+SENSORS_NBR_OF_BIAS_SAMPLES); elem++)
+ // {
+ // sumSquared[0] += elem->x * elem->x;
+ // sumSquared[1] += elem->y * elem->y;
+ // sumSquared[2] += elem->z * elem->z;
+ // }
+ // calcMean(bias, mean);
+
+ // variance->x = fabs(sumSquared[0] / SENSORS_NBR_OF_BIAS_SAMPLES
+ // - mean->x * mean->x);
+ // variance->y = fabs(sumSquared[1] / SENSORS_NBR_OF_BIAS_SAMPLES
+ // - mean->y * mean->y);
+ // variance->z = fabs(sumSquared[2] / SENSORS_NBR_OF_BIAS_SAMPLES
+ // - mean->z * mean->z);
}
/**
@@ -829,10 +831,11 @@ static void sensorsApplyBiasAndScale(Axis3f* scaled, Axis3i16* aligned,
static void sensorsScaleBaro(baro_t* baroScaled, float pressure,
float temperature) {
- baroScaled->pressure = pressure*0.01f;
- baroScaled->temperature = temperature;
- baroScaled->asl = ((powf((1015.7f / baroScaled->pressure), 0.1902630958f)
- - 1.0f) * (25.0f + 273.15f)) / 0.0065f;
+ //COMMENTED FIRMWARE
+ // baroScaled->pressure = pressure*0.01f;
+ // baroScaled->temperature = temperature;
+ // baroScaled->asl = ((powf((1015.7f / baroScaled->pressure), 0.1902630958f)
+ // - 1.0f) * (25.0f + 273.15f)) / 0.0065f;
}
bool sensorsBoschReadGyro(Axis3f *gyro)
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 7c3959e0934fb8e8b9bccb0d03adac8d0191c293..3b441215d8e8830df4f9313dea4e950c5e75cd52 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,6 +29,7 @@
#include "sensors_mpu9250_lps25h.h"
#include <math.h>
+#include "empty_math.h"
#include <stm32f4xx.h>
#include "lps25h.h"
@@ -609,19 +610,20 @@ bool sensorsMpu9250Lps25hTest(void)
static bool processAccScale(int16_t ax, int16_t ay, int16_t az)
{
static bool accBiasFound = false;
- static uint32_t accScaleSumCount = 0;
+ //COMMENTED FIRMWARE
+ // static uint32_t accScaleSumCount = 0;
- if (!accBiasFound)
- {
- accScaleSum += sqrtf(powf(ax * SENSORS_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_G_PER_LSB_CFG, 2) + powf(az * SENSORS_G_PER_LSB_CFG, 2));
- accScaleSumCount++;
+ // if (!accBiasFound)
+ // {
+ // accScaleSum += sqrtf(powf(ax * SENSORS_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_G_PER_LSB_CFG, 2) + powf(az * SENSORS_G_PER_LSB_CFG, 2));
+ // accScaleSumCount++;
- if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
- {
- accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
- accBiasFound = true;
- }
- }
+ // if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
+ // {
+ // accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
+ // accBiasFound = true;
+ // }
+ // }
return accBiasFound;
}
@@ -803,56 +805,57 @@ static bool sensorsFindBiasValue(BiasObj* bias)
bool sensorsMpu9250Lps25hManufacturingTest(void)
{
bool testStatus = false;
- Axis3i16 g;
- Axis3i16 a;
- Axis3f acc; // Accelerometer axis data in mG
- float pitch, roll;
- uint32_t startTick = xTaskGetTickCount();
-
- testStatus = mpu6500SelfTest();
-
- if (testStatus)
- {
- sensorsBiasObjInit(&gyroBiasRunning);
- while (xTaskGetTickCount() - startTick < SENSORS_VARIANCE_MAN_TEST_TIMEOUT)
- {
- mpu6500GetMotion6(&a.y, &a.x, &a.z, &g.y, &g.x, &g.z);
-
- if (processGyroBias(g.x, g.y, g.z, &gyroBias))
- {
- gyroBiasFound = true;
- DEBUG_PRINT("Gyro variance test [OK]\n");
- break;
- }
- }
-
- if (gyroBiasFound)
- {
- acc.x = -(a.x) * SENSORS_G_PER_LSB_CFG;
- acc.y = (a.y) * SENSORS_G_PER_LSB_CFG;
- acc.z = (a.z) * SENSORS_G_PER_LSB_CFG;
-
- // Calculate pitch and roll based on accelerometer. Board must be level
- pitch = tanf(-acc.x/(sqrtf(acc.y*acc.y + acc.z*acc.z))) * 180/(float) M_PI;
- roll = tanf(acc.y/acc.z) * 180/(float) M_PI;
-
- if ((fabsf(roll) < SENSORS_MAN_TEST_LEVEL_MAX) && (fabsf(pitch) < SENSORS_MAN_TEST_LEVEL_MAX))
- {
- DEBUG_PRINT("Acc level test [OK]\n");
- testStatus = true;
- }
- else
- {
- DEBUG_PRINT("Acc level test Roll:%0.2f, Pitch:%0.2f [FAIL]\n", (double)roll, (double)pitch);
- testStatus = false;
- }
- }
- else
- {
- DEBUG_PRINT("Gyro variance test [FAIL]\n");
- testStatus = false;
- }
- }
+ //COMMENTED FIRMWARE
+ // Axis3i16 g;
+ // Axis3i16 a;
+ // Axis3f acc; // Accelerometer axis data in mG
+ // float pitch, roll;
+ // uint32_t startTick = xTaskGetTickCount();
+
+ // testStatus = mpu6500SelfTest();
+
+ // if (testStatus)
+ // {
+ // sensorsBiasObjInit(&gyroBiasRunning);
+ // while (xTaskGetTickCount() - startTick < SENSORS_VARIANCE_MAN_TEST_TIMEOUT)
+ // {
+ // mpu6500GetMotion6(&a.y, &a.x, &a.z, &g.y, &g.x, &g.z);
+
+ // if (processGyroBias(g.x, g.y, g.z, &gyroBias))
+ // {
+ // gyroBiasFound = true;
+ // DEBUG_PRINT("Gyro variance test [OK]\n");
+ // break;
+ // }
+ // }
+
+ // if (gyroBiasFound)
+ // {
+ // acc.x = -(a.x) * SENSORS_G_PER_LSB_CFG;
+ // acc.y = (a.y) * SENSORS_G_PER_LSB_CFG;
+ // acc.z = (a.z) * SENSORS_G_PER_LSB_CFG;
+
+ // // Calculate pitch and roll based on accelerometer. Board must be level
+ // pitch = tanf(-acc.x/(sqrtf(acc.y*acc.y + acc.z*acc.z))) * 180/(float) M_PI;
+ // roll = tanf(acc.y/acc.z) * 180/(float) M_PI;
+
+ // if ((fabsf(roll) < SENSORS_MAN_TEST_LEVEL_MAX) && (fabsf(pitch) < SENSORS_MAN_TEST_LEVEL_MAX))
+ // {
+ // DEBUG_PRINT("Acc level test [OK]\n");
+ // testStatus = true;
+ // }
+ // else
+ // {
+ // DEBUG_PRINT("Acc level test Roll:%0.2f, Pitch:%0.2f [FAIL]\n", (double)roll, (double)pitch);
+ // testStatus = false;
+ // }
+ // }
+ // else
+ // {
+ // DEBUG_PRINT("Gyro variance test [FAIL]\n");
+ // testStatus = false;
+ // }
+ // }
return testStatus;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/lib/FatFS/ff.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/lib/FatFS/ff.h
index ec9c91ed65523d904e8456abc5343f346a8c478b..7bdf765d1a6d3b1f7b830706b7c012b98790d4b6 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/lib/FatFS/ff.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/lib/FatFS/ff.h
@@ -357,11 +357,12 @@ DWORD get_fattime (void);
#endif
/* LFN support functions */
-#if FF_USE_LFN >= 1 /* Code conversion (defined in unicode.c) */
+
+//#if FF_USE_LFN >= 1 /* Code conversion (defined in unicode.c) */
WCHAR ff_oem2uni (WCHAR oem, WORD cp); /* OEM code to Unicode conversion */
WCHAR ff_uni2oem (DWORD uni, WORD cp); /* Unicode to OEM code conversion */
DWORD ff_wtoupper (DWORD uni); /* Unicode upper-case conversion */
-#endif
+//#endif
#if FF_USE_LFN == 3 /* Dynamic memory allocation */
void* ff_memalloc (UINT msize); /* Allocate memory block */
void ff_memfree (void* mblock); /* Free memory block */
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/lib/vl53l1/core/src/vl53l1_api.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/lib/vl53l1/core/src/vl53l1_api.c
index 2ccbe6e83b87ec277d07762a03e83370f347c39c..d075731f193524ab2b039bb0c3616bbfd2c0e2b7 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/lib/vl53l1/core/src/vl53l1_api.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/lib/vl53l1/core/src/vl53l1_api.c
@@ -670,9 +670,9 @@ VL53L1_Error VL53L1_GetDeviceInfo(VL53L1_DEV Dev,
LOG_FUNCTION_START("");
pLLData = VL53L1DevStructGetLLDriverHandle(Dev);
-
- strncpy(pVL53L1_DeviceInfo->ProductId, "",
- VL53L1_DEVINFO_STRLEN-1);
+ //COMMENTED FIRMWARE
+ // strncpy(pVL53L1_DeviceInfo->ProductId, "",
+ // VL53L1_DEVINFO_STRLEN-1);
pVL53L1_DeviceInfo->ProductType =
pLLData->nvm_copy_data.identification__module_type;
@@ -681,17 +681,18 @@ VL53L1_Error VL53L1_GetDeviceInfo(VL53L1_DEV Dev,
pVL53L1_DeviceInfo->ProductRevisionMinor = (revision_id & 0xF0) >> 4;
#ifndef VL53L1_USE_EMPTY_STRING
- if (pVL53L1_DeviceInfo->ProductRevisionMinor == 0)
- strncpy(pVL53L1_DeviceInfo->Name,
- VL53L1_STRING_DEVICE_INFO_NAME0,
- VL53L1_DEVINFO_STRLEN-1);
- else
- strncpy(pVL53L1_DeviceInfo->Name,
- VL53L1_STRING_DEVICE_INFO_NAME1,
- VL53L1_DEVINFO_STRLEN-1);
- strncpy(pVL53L1_DeviceInfo->Type,
- VL53L1_STRING_DEVICE_INFO_TYPE,
- VL53L1_DEVINFO_STRLEN-1);
+//COMMENTED FIRMWARE
+ // if (pVL53L1_DeviceInfo->ProductRevisionMinor == 0)
+ // strncpy(pVL53L1_DeviceInfo->Name,
+ // VL53L1_STRING_DEVICE_INFO_NAME0,
+ // VL53L1_DEVINFO_STRLEN-1);
+ // else
+ // strncpy(pVL53L1_DeviceInfo->Name,
+ // VL53L1_STRING_DEVICE_INFO_NAME1,
+ // VL53L1_DEVINFO_STRLEN-1);
+ // strncpy(pVL53L1_DeviceInfo->Type,
+ // VL53L1_STRING_DEVICE_INFO_TYPE,
+ // VL53L1_DEVINFO_STRLEN-1);
#else
pVL53L1_DeviceInfo->Name[0] = 0;
pVL53L1_DeviceInfo->Type[0] = 0;
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 673d5d6124d6eb784604f08ae8ec27da9fff524f..c3adb7681b6dfeb0caf6464c1735424336300793 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,6 +26,7 @@ SOFTWARE.
*/
#include <math.h>
+#include "empty_math.h"
#include <stdbool.h>
#include <stddef.h>
@@ -55,16 +56,18 @@ static inline float clamp(float value, float min, float max) {
// steps to allow. this does not work well for numbers near zero.
// See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
static inline bool fcloseulps(float a, float b, int ulps) {
- if ((a < 0.0f) != (b < 0.0f)) {
- // Handle negative zero.
- if (a == b) {
- return true;
- }
- return false;
- }
- int ia = *((int *)&a);
- int ib = *((int *)&b);
- return fabsf(ia - ib) <= ulps;
+ //COMMENTED FIRMWARE
+ // if ((a < 0.0f) != (b < 0.0f)) {
+ // // Handle negative zero.
+ // if (a == b) {
+ // return true;
+ // }
+ // return false;
+ // }
+ // int ia = *((int *)&a);
+ // int ib = *((int *)&b);
+ // return fabsf(ia - ib) <= ulps;
+ return false;
}
@@ -146,7 +149,8 @@ static inline float vmag2(struct vec v) {
}
// vector magnitude.
static inline float vmag(struct vec v) {
- return sqrtf(vmag2(v));
+ //COMMENTED FIRMWARE
+ return 0;//sqrtf(vmag2(v));
}
// vector Euclidean distance squared.
static inline float vdist2(struct vec a, struct vec b) {
@@ -154,7 +158,8 @@ static inline float vdist2(struct vec a, struct vec b) {
}
// vector Euclidean distance.
static inline float vdist(struct vec a, struct vec b) {
- return sqrtf(vdist2(a, b));
+ //COMMENTED FIRMWARE
+ return 0;//sqrtf(vdist2(a, b));
}
// normalize a vector (make a unit vector).
static inline struct vec vnormalize(struct vec v) {
@@ -579,23 +584,25 @@ static inline struct quat qnormalize(struct quat q);
// assumes a and b are unit vectors.
// does not handle degenerate case where a = -b. returns all-zero quat
static inline struct quat qvectovec(struct vec a, struct vec b) {
- struct vec const cross = vcross(a, b);
- float const sinangle = vmag(cross);
- float const cosangle = vdot(a, b);
- // avoid taking sqrt of negative number due to floating point error.
- // TODO: find tighter exact bound
- float const EPS_ANGLE = 1e-6;
- if (sinangle < EPS_ANGLE) {
- if (cosangle > 0.0f) return qeye();
- else return mkquat(0.0f, 0.0f, 0.0f, 0.0f); // degenerate
- }
- float const halfcos = 0.5f * cosangle;
- // since angle is < 180deg, the positive sqrt is always correct
- float const sinhalfangle = sqrtf(fmax(0.5f - halfcos, 0.0f));
- float const coshalfangle = sqrtf(fmax(0.5f + halfcos, 0.0f));
- struct vec const qimag = vscl(sinhalfangle / sinangle, cross);
- float const qreal = coshalfangle;
- return quatvw(qimag, qreal);
+ //COMMENTED FIRMWARE
+ // struct vec const cross = vcross(a, b);
+ // float const sinangle = vmag(cross);
+ // float const cosangle = vdot(a, b);
+ // // avoid taking sqrt of negative number due to floating point error.
+ // // TODO: find tighter exact bound
+ // float const EPS_ANGLE = 1e-6;
+ // if (sinangle < EPS_ANGLE) {
+ // if (cosangle > 0.0f) return qeye();
+ // else return mkquat(0.0f, 0.0f, 0.0f, 0.0f); // degenerate
+ // }
+ // float const halfcos = 0.5f * cosangle;
+ // // since angle is < 180deg, the positive sqrt is always correct
+ // float const sinhalfangle = sqrtf(fmax(0.5f - halfcos, 0.0f));
+ // float const coshalfangle = sqrtf(fmax(0.5f + halfcos, 0.0f));
+ // struct vec const qimag = vscl(sinhalfangle / sinangle, cross);
+ // float const qreal = coshalfangle;
+ // return quatvw(qimag, qreal);
+ return quatvw(a, 0);
}
// construct from (roll, pitch, yaw) Euler angles using Tait-Bryan convention
// (yaw, then pitch about new pitch axis, then roll about new roll axis)
@@ -622,25 +629,29 @@ static inline struct quat rpy2quat(struct vec rpy) {
static inline struct quat rpy2quat_small(struct vec rpy) {
// TODO: cite source, but can be derived from rpy2quat under first-order approximation:
// sin(epsilon) = epsilon, cos(epsilon) = 1, epsilon^2 = 0
- float q2 = vmag2(rpy) / 4.0f;
- if (q2 < 1) {
- return quatvw(vdiv(rpy, 2), sqrtf(1.0f - q2));
- }
- else {
- float w = 1.0f / sqrtf(1.0f + q2);
- return quatvw(vscl(w/2, rpy), w);
- }
+ //COMMENTED FIRMWARE
+ // float q2 = vmag2(rpy) / 4.0f;
+ // if (q2 < 1) {
+ //return quatvw(vdiv(rpy, 2), sqrtf(1.0f - q2));
+ // }
+ // else {
+ // float w = 1.0f / sqrtf(1.0f + q2);
+ // return quatvw(vscl(w/2, rpy), w);
+ // }
+ return quatvw(rpy, 0);
}
// construct quaternion from orthonormal matrix.
static inline struct quat mat2quat(struct mat33 m) {
- float w = sqrtf(fmax(0.0f, 1.0f + m.m[0][0] + m.m[1][1] + m.m[2][2])) / 2.0f;
- float x = sqrtf(fmax(0.0f, 1.0f + m.m[0][0] - m.m[1][1] - m.m[2][2])) / 2.0f;
- float y = sqrtf(fmax(0.0f, 1.0f - m.m[0][0] + m.m[1][1] - m.m[2][2])) / 2.0f;
- float z = sqrtf(fmax(0.0f, 1.0f - m.m[0][0] - m.m[1][1] + m.m[2][2])) / 2.0f;
- x = copysign(x, m.m[2][1] - m.m[1][2]);
- y = copysign(y, m.m[0][2] - m.m[2][0]);
- z = copysign(z, m.m[1][0] - m.m[0][1]);
- return mkquat(x, y, z, w);
+ //COMMENTED FIRMWARE
+ // float w = sqrtf(fmax(0.0f, 1.0f + m.m[0][0] + m.m[1][1] + m.m[2][2])) / 2.0f;
+ // float x = sqrtf(fmax(0.0f, 1.0f + m.m[0][0] - m.m[1][1] - m.m[2][2])) / 2.0f;
+ // float y = sqrtf(fmax(0.0f, 1.0f - m.m[0][0] + m.m[1][1] - m.m[2][2])) / 2.0f;
+ // float z = sqrtf(fmax(0.0f, 1.0f - m.m[0][0] - m.m[1][1] + m.m[2][2])) / 2.0f;
+ // x = copysign(x, m.m[2][1] - m.m[1][2]);
+ // y = copysign(y, m.m[0][2] - m.m[2][0]);
+ // z = copysign(z, m.m[1][0] - m.m[0][1]);
+ //return mkquat(x, y, z, w);
+ return mkquat(0, 0, 0, 0);
}
//
@@ -652,16 +663,19 @@ static inline struct quat mat2quat(struct mat33 m) {
static inline struct vec quat2rpy(struct quat q) {
// from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
struct vec v;
- v.x = atan2f(2.0f * (q.w * q.x + q.y * q.z), 1 - 2 * (fsqr(q.x) + fsqr(q.y))); // roll
- v.y = asinf(2.0f * (q.w * q.y - q.x * q.z)); // pitch
- v.z = atan2f(2.0f * (q.w * q.z + q.x * q.y), 1 - 2 * (fsqr(q.y) + fsqr(q.z))); // yaw
+ //COMMENTED FIRMWARE
+ // v.x = atan2f(2.0f * (q.w * q.x + q.y * q.z), 1 - 2 * (fsqr(q.x) + fsqr(q.y))); // roll
+ // v.y = asinf(2.0f * (q.w * q.y - q.x * q.z)); // pitch
+ // v.z = atan2f(2.0f * (q.w * q.z + q.x * q.y), 1 - 2 * (fsqr(q.y) + fsqr(q.z))); // yaw
return v;
}
// compute the axis of a quaternion's axis-angle decomposition.
static inline struct vec quat2axis(struct quat q) {
// TODO this is not numerically stable for tiny rotations
- float s = 1.0f / sqrtf(1.0f - q.w * q.w);
- return vscl(s, mkvec(q.x, q.y, q.z));
+ //COMMENTED FIRMWARE
+ //float s = 1.0f / sqrtf(1.0f - q.w * q.w);
+ //return vscl(s, mkvec(q.x, q.y, q.z));
+ return vscl(0, mkvec(q.x, q.y, q.z));
}
// compute the angle of a quaternion's axis-angle decomposition.
// result lies in domain (-pi, pi].
@@ -753,8 +767,10 @@ static inline bool qeq(struct quat a, struct quat b) {
// normalize a quaternion.
// typically used to mitigate precision errors.
static inline struct quat qnormalize(struct quat q) {
- float s = 1.0f / sqrtf(qdot(q, q));
- return mkquat(s*q.x, s*q.y, s*q.z, s*q.w);
+ //COMMENTED FIRMWARE
+ // float s = 1.0f / sqrtf(qdot(q, q));
+ // return mkquat(s*q.x, s*q.y, s*q.z, s*q.w);
+ return mkquat(0, 0, 0, 0);
}
// update an attitude estimate quaternion with a reading from a gyroscope
// over the timespan dt. Gyroscope is assumed (roll, pitch, yaw)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/quatcompress.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/quatcompress.h
index 6242343a87ec872068a953203163569fe6bc1535..5ab87d78e9036f28102f8689b61e06070a0a7f6f 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/quatcompress.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/quatcompress.h
@@ -35,11 +35,12 @@ static inline uint32_t quatcompress(float const q[4])
{
// we send the values of the quaternion's smallest 3 elements.
unsigned i_largest = 0;
- for (unsigned i = 1; i < 4; ++i) {
- if (fabsf(q[i]) > fabsf(q[i_largest])) {
- i_largest = i;
- }
- }
+ //COMMENTED FIRMWARE
+ // for (unsigned i = 1; i < 4; ++i) {
+ // if (fabsf(q[i]) > fabsf(q[i_largest])) {
+ // i_largest = i;
+ // }
+ // }
// since -q represents the same rotation as q,
// transform the quaternion so the largest element is positive.
@@ -51,13 +52,14 @@ static inline uint32_t quatcompress(float const q[4])
// do compression using sign bit and 9-bit precision per element.
uint32_t comp = i_largest;
- for (unsigned i = 0; i < 4; ++i) {
- if (i != i_largest) {
- unsigned negbit = (q[i] < 0) ^ negate;
- unsigned mag = ((1 << 9) - 1) * (fabsf(q[i]) / (float)M_SQRT1_2) + 0.5f;
- comp = (comp << 10) | (negbit << 9) | mag;
- }
- }
+ //COMMENTED FIRMWARE
+ // for (unsigned i = 0; i < 4; ++i) {
+ // if (i != i_largest) {
+ // unsigned negbit = (q[i] < 0) ^ negate;
+ // unsigned mag = ((1 << 9) - 1) * (fabsf(q[i]) / (float)M_SQRT1_2) + 0.5f;
+ // comp = (comp << 10) | (negbit << 9) | mag;
+ // }
+ // }
return comp;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/static_mem.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/static_mem.h
index f371a5dd33caa9ac6acc6206a18204a242917df1..14492527065c8d6197203128abd8fb1bf8099faf 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/static_mem.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/interface/static_mem.h
@@ -136,7 +136,9 @@
*
* @param NAME - the name of the queue handle
*/
-#define STATIC_MEM_QUEUE_CREATE(NAME) xQueueCreateStatic(osSys_ ## NAME ## Length, osSys_ ## NAME ## ItemSize, osSys_ ## NAME ## Storage, &osSys_ ## NAME ## Mgm)
+//COMMENTED FIRMWARE
+// #define STATIC_MEM_QUEUE_CREATE(NAME) xQueueCreateStatic(osSys_ ## NAME ## Length, osSys_ ## NAME ## ItemSize, osSys_ ## NAME ## Storage, &osSys_ ## NAME ## Mgm)
+#define STATIC_MEM_QUEUE_CREATE(NAME) xQueueCreate(osSys_ ## NAME ## Length, osSys_ ## NAME ## ItemSize, osSys_ ## NAME ## Storage, &osSys_ ## NAME ## Mgm)
/**
@@ -205,4 +207,6 @@
* @param PARAMETERS Passed on as argument to the function implementing the task
* @param PRIORITY The task priority
*/
-#define STATIC_MEM_TASK_CREATE(NAME, FUNCTION, TASK_NAME, PARAMETERS, PRIORITY) xTaskCreateStatic((FUNCTION), (TASK_NAME), osSys_ ## NAME ## StackDepth, (PARAMETERS), (PRIORITY), osSys_ ## NAME ## StackBuffer, &osSys_ ## NAME ## TaskBuffer)
+//COMMENTED FIRMWARE
+//#define STATIC_MEM_TASK_CREATE(NAME, FUNCTION, TASK_NAME, PARAMETERS, PRIORITY) xTaskCreateStatic((FUNCTION), (TASK_NAME), osSys_ ## NAME ## StackDepth, (PARAMETERS), (PRIORITY), osSys_ ## NAME ## StackBuffer, &osSys_ ## NAME ## TaskBuffer)
+#define STATIC_MEM_TASK_CREATE(NAME, FUNCTION, TASK_NAME, PARAMETERS, PRIORITY) xTaskCreate((FUNCTION), (TASK_NAME), osSys_ ## NAME ## StackDepth, (PARAMETERS), (PRIORITY), osSys_ ## NAME ## StackBuffer, &osSys_ ## NAME ## TaskBuffer)
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 60c40aa7d9014e140e99d9f77c796685885b61ef..d618e519804bc52c31d2cf6cb1d394e33bc4b5d3 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
@@ -102,154 +102,156 @@ void collisionAvoidanceUpdateSetpointCore(
float *workspace,
setpoint_t *setpoint, sensorData_t const *sensorData, state_t const *state)
{
- //
- // Part 1: Construct the polytope inequalities in A, b.
- //
-
- int const nRows = nOthers + 6;
- float *A = workspace;
- float *B = workspace + 3 * nRows;
- float *projectionWorkspace = workspace + 4 * nRows;
-
- // Compute the cell in a stretched coordinate system for downwash awareness.
- // See header for details.
- struct vec const radiiInv = veltrecip(params->ellipsoidRadii);
- struct vec const ourPos = vec2svec(state->position);
-
- // We could do some optimizations here to reduce the number of rows, like
- // leave out very far away neighbors. It would help the average case
- // complexity, but not the worst case - so we skip it for now.
- for (int i = 0; i < nOthers; ++i) {
- struct vec peerPos = vloadf(otherPositions + 3 * i);
- struct vec const toPeerStretched = veltmul(vsub(peerPos, ourPos), radiiInv);
- float const dist = vmag(toPeerStretched);
- struct vec const a = vdiv(veltmul(toPeerStretched, radiiInv), dist);
- float const b = dist / 2.0f - 1.0f;
- float scale = 1.0f / vmag(a);
- vstoref(vscl(scale, a), A + 3 * i);
- B[i] = scale * b;
- }
-
- // Add the bounding box polytope faces. We also use the box faces to enforce
- // max speed in the infinity-norm.
- float const maxDist = params->horizonSecs * params->maxSpeed;
-
- memset(A + 3 * nOthers, 0, 18 * sizeof(float));
-
- for (int dim = 0; dim < 3; ++dim) {
- float boxMax = vindex(params->bboxMax, dim) - vindex(ourPos, dim);
- A[3 * (nOthers + dim) + dim] = 1.0f;
- B[nOthers + dim] = fminf(maxDist, boxMax);
-
- float boxMin = vindex(params->bboxMin, dim) - vindex(ourPos, dim);
- A[3 * (nOthers + dim + 3) + dim] = -1.0f;
- B[nOthers + dim + 3] = -fmaxf(-maxDist, boxMin);
- }
-
- //
- // Part 2: Use the constructed polytope to modify the setpoint.
- //
-
- float const inPolytopeTolerance = 10.0f * params->voronoiProjectionTolerance;
-
- struct vec setPos = vec2svec(setpoint->position);
- struct vec setVel = vec2svec(setpoint->velocity);
-
- if (setpoint->mode.x == modeVelocity) {
- // Interpret the setpoint to mean "fly with this velocity".
-
- if (vinpolytope(vzero(), A, B, nRows, inPolytopeTolerance)) {
- // Typical case - our current position is within our cell.
- struct vec pseudoGoal = vscl(params->horizonSecs, setVel);
- pseudoGoal = sidestepGoal(params, pseudoGoal, true, A, B, projectionWorkspace, nRows);
- if (vinpolytope(pseudoGoal, A, B, nRows, inPolytopeTolerance)) {
- setVel = vdiv(pseudoGoal, params->horizonSecs);
- }
- else {
- // Projection failed to converge. Best we can do is stay still.
- setVel = vzero();
- }
- }
- else {
- // Atypical case - our current position is not within our cell. Forget
- // about the original goal velocity and try to move towards our cell.
- struct vec nearestInCell = vprojectpolytope(
- vzero(),
- A, B, projectionWorkspace, nRows,
- params->voronoiProjectionTolerance,
- params->voronoiProjectionMaxIters
- );
- if (vinpolytope(nearestInCell, A, B, nRows, inPolytopeTolerance)) {
- setVel = vclampnorm(nearestInCell, params->maxSpeed);
- }
- else {
- // Projection failed to converge. Best we can do is stay still.
- setVel = vzero();
- }
- }
- collisionState->lastFeasibleSetPosition = ourPos;
- }
- else if (setpoint->mode.x == modeAbs) {
-
- struct vec const setPosRelative = vsub(setPos, ourPos);
- struct vec const setPosRelativeNew = sidestepGoal(
- params, setPosRelative, false, A, B, projectionWorkspace, nRows);
-
- if (!vinpolytope(setPosRelativeNew, A, B, nRows, inPolytopeTolerance)) {
- // If the projection algorithm failed to converge, then either
- // 1) The problem is infeasible, or
- // 2) The problem is somehow badly conditioned.
- // Case 2) is still effectively infeasible -- we're in a real-time system
- // with a fixed computation time budget, and we've already spent it.
- //
- // There's not a lot we can do if our buffered cell is empty. It means
- // someone failed to stay within their cell in the past. We choose to stay
- // a fixed position for as long as the cell is empty.
- setVel = vzero();
- if (!visnan(collisionState->lastFeasibleSetPosition)) {
- setPos = collisionState->lastFeasibleSetPosition;
- }
- else {
- // This case should never happen as long as collision avoidance is
- // initialized in a collision-free state, but we do something reasonable
- // just in case.
- setPos = ourPos;
- collisionState->lastFeasibleSetPosition = ourPos;
- }
- }
- else if (veq(setVel, vzero())) {
- // Position, but no velocity. Interpret as waypoint, not trajectory
- // tracking. "Go to this position and stop".
- setPos = vadd(ourPos, setPosRelativeNew);
- }
- else {
- // Position with nonzero velocity. Interpret as trajectory tracking.
- if (vneq(setPosRelative, setPosRelativeNew)) {
- // Set position is not within our cell. The situation has likely diverged
- // from the original plan, and the nonzero velocity is probably pointing
- // further away from our cell. Therefore, degrade to the v == 0 behavior.
- setPos = vadd(ourPos, setPosRelativeNew);
- setVel = vzero();
- }
- else {
- // Set position is within our cell. In case velocity would take us
- // outside the cell within the planning horizon, scale it appropriately.
- // See github issue #567 for more detailed discussion of this behavior.
- float const scale = rayintersectpolytope(setPosRelative, setVel, A, B, nRows, NULL);
- if (scale < 1.0f) {
- setVel = vscl(scale, setVel);
- }
- // leave setPos unchanged.
- }
- }
- collisionState->lastFeasibleSetPosition = setPos;
- }
- else {
- // Unsupported control mode, do nothing.
- }
-
- setpoint->position = svec2vec(setPos);
- setpoint->velocity = svec2vec(setVel);
+ //COMMENTED FIRMWARE
+ // //
+ // // Part 1: Construct the polytope inequalities in A, b.
+ // //
+
+ // int const nRows = nOthers + 6;
+ // float *A = workspace;
+ // float *B = workspace + 3 * nRows;
+ // float *projectionWorkspace = workspace + 4 * nRows;
+
+ // // Compute the cell in a stretched coordinate system for downwash awareness.
+ // // See header for details.
+ // struct vec const radiiInv = veltrecip(params->ellipsoidRadii);
+ // struct vec const ourPos = vec2svec(state->position);
+
+ // // We could do some optimizations here to reduce the number of rows, like
+ // // leave out very far away neighbors. It would help the average case
+ // // complexity, but not the worst case - so we skip it for now.
+ // for (int i = 0; i < nOthers; ++i) {
+ // struct vec peerPos = vloadf(otherPositions + 3 * i);
+ // struct vec const toPeerStretched = veltmul(vsub(peerPos, ourPos), radiiInv);
+ // float const dist = vmag(toPeerStretched);
+ // struct vec const a = vdiv(veltmul(toPeerStretched, radiiInv), dist);
+ // float const b = dist / 2.0f - 1.0f;
+ // float scale = 1.0f / vmag(a);
+ // vstoref(vscl(scale, a), A + 3 * i);
+ // B[i] = scale * b;
+ // }
+
+ // // Add the bounding box polytope faces. We also use the box faces to enforce
+ // // max speed in the infinity-norm.
+ // float const maxDist = params->horizonSecs * params->maxSpeed;
+
+ // memset(A + 3 * nOthers, 0, 18 * sizeof(float));
+
+ // //COMMENTED FIRMWARE
+ // // for (int dim = 0; dim < 3; ++dim) {
+ // // float boxMax = vindex(params->bboxMax, dim) - vindex(ourPos, dim);
+ // // A[3 * (nOthers + dim) + dim] = 1.0f;
+ // // B[nOthers + dim] = fminf(maxDist, boxMax);
+
+ // // float boxMin = vindex(params->bboxMin, dim) - vindex(ourPos, dim);
+ // // A[3 * (nOthers + dim + 3) + dim] = -1.0f;
+ // // B[nOthers + dim + 3] = -fmaxf(-maxDist, boxMin);
+ // // }
+
+ // //
+ // // Part 2: Use the constructed polytope to modify the setpoint.
+ // //
+
+ // float const inPolytopeTolerance = 10.0f * params->voronoiProjectionTolerance;
+
+ // struct vec setPos = vec2svec(setpoint->position);
+ // struct vec setVel = vec2svec(setpoint->velocity);
+
+ // if (setpoint->mode.x == modeVelocity) {
+ // // Interpret the setpoint to mean "fly with this velocity".
+
+ // if (vinpolytope(vzero(), A, B, nRows, inPolytopeTolerance)) {
+ // // Typical case - our current position is within our cell.
+ // struct vec pseudoGoal = vscl(params->horizonSecs, setVel);
+ // pseudoGoal = sidestepGoal(params, pseudoGoal, true, A, B, projectionWorkspace, nRows);
+ // if (vinpolytope(pseudoGoal, A, B, nRows, inPolytopeTolerance)) {
+ // setVel = vdiv(pseudoGoal, params->horizonSecs);
+ // }
+ // else {
+ // // Projection failed to converge. Best we can do is stay still.
+ // setVel = vzero();
+ // }
+ // }
+ // else {
+ // // Atypical case - our current position is not within our cell. Forget
+ // // about the original goal velocity and try to move towards our cell.
+ // struct vec nearestInCell = vprojectpolytope(
+ // vzero(),
+ // A, B, projectionWorkspace, nRows,
+ // params->voronoiProjectionTolerance,
+ // params->voronoiProjectionMaxIters
+ // );
+ // if (vinpolytope(nearestInCell, A, B, nRows, inPolytopeTolerance)) {
+ // setVel = vclampnorm(nearestInCell, params->maxSpeed);
+ // }
+ // else {
+ // // Projection failed to converge. Best we can do is stay still.
+ // setVel = vzero();
+ // }
+ // }
+ // collisionState->lastFeasibleSetPosition = ourPos;
+ // }
+ // else if (setpoint->mode.x == modeAbs) {
+
+ // struct vec const setPosRelative = vsub(setPos, ourPos);
+ // struct vec const setPosRelativeNew = sidestepGoal(
+ // params, setPosRelative, false, A, B, projectionWorkspace, nRows);
+
+ // if (!vinpolytope(setPosRelativeNew, A, B, nRows, inPolytopeTolerance)) {
+ // // If the projection algorithm failed to converge, then either
+ // // 1) The problem is infeasible, or
+ // // 2) The problem is somehow badly conditioned.
+ // // Case 2) is still effectively infeasible -- we're in a real-time system
+ // // with a fixed computation time budget, and we've already spent it.
+ // //
+ // // There's not a lot we can do if our buffered cell is empty. It means
+ // // someone failed to stay within their cell in the past. We choose to stay
+ // // a fixed position for as long as the cell is empty.
+ // setVel = vzero();
+ // if (!visnan(collisionState->lastFeasibleSetPosition)) {
+ // setPos = collisionState->lastFeasibleSetPosition;
+ // }
+ // else {
+ // // This case should never happen as long as collision avoidance is
+ // // initialized in a collision-free state, but we do something reasonable
+ // // just in case.
+ // setPos = ourPos;
+ // collisionState->lastFeasibleSetPosition = ourPos;
+ // }
+ // }
+ // else if (veq(setVel, vzero())) {
+ // // Position, but no velocity. Interpret as waypoint, not trajectory
+ // // tracking. "Go to this position and stop".
+ // setPos = vadd(ourPos, setPosRelativeNew);
+ // }
+ // else {
+ // // Position with nonzero velocity. Interpret as trajectory tracking.
+ // if (vneq(setPosRelative, setPosRelativeNew)) {
+ // // Set position is not within our cell. The situation has likely diverged
+ // // from the original plan, and the nonzero velocity is probably pointing
+ // // further away from our cell. Therefore, degrade to the v == 0 behavior.
+ // setPos = vadd(ourPos, setPosRelativeNew);
+ // setVel = vzero();
+ // }
+ // else {
+ // // Set position is within our cell. In case velocity would take us
+ // // outside the cell within the planning horizon, scale it appropriately.
+ // // See github issue #567 for more detailed discussion of this behavior.
+ // float const scale = rayintersectpolytope(setPosRelative, setVel, A, B, nRows, NULL);
+ // if (scale < 1.0f) {
+ // setVel = vscl(scale, setVel);
+ // }
+ // // leave setPos unchanged.
+ // }
+ // }
+ // collisionState->lastFeasibleSetPosition = setPos;
+ // }
+ // else {
+ // // Unsupported control mode, do nothing.
+ // }
+
+ // setpoint->position = svec2vec(setPos);
+ // setpoint->velocity = svec2vec(setVel);
}
@@ -258,7 +260,7 @@ void collisionAvoidanceUpdateSetpointCore(
// with the standard Makefile. Everything depending on FreeRTOS, ARM, params,
// etc. must go here.
//
-#ifdef CRAZYFLIE_FW
+//#ifdef CRAZYFLIE_FW
#include "FreeRTOS.h"
#include "task.h"
@@ -427,4 +429,4 @@ PARAM_GROUP_START(colAv)
PARAM_ADD(PARAM_INT32, vorIters, ¶ms.voronoiProjectionMaxIters)
PARAM_GROUP_STOP(colAv)
-#endif // CRAZYFLIE_FW
+//#endif // CRAZYFLIE_FW
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_indi.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_indi.c
index 7483edaf845760b8abe8e33b8ee628d082203078..f496cc59302f102d6d0a11d720f4ab54d63eefcc 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_indi.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_indi.c
@@ -60,16 +60,17 @@ static inline void float_rates_zero(struct FloatRates *fr) {
void indi_init_filters(void)
{
- // tau = 1/(2*pi*Fc)
- float tau = 1.0f / (2.0f * M_PI_F * indi.filt_cutoff);
- float tau_r = 1.0f / (2.0f * M_PI_F * indi.filt_cutoff_r);
- float tau_axis[3] = {tau, tau, tau_r};
- float sample_time = 1.0f / ATTITUDE_RATE;
- // Filtering of gyroscope and actuators
- for (int8_t i = 0; i < 3; i++) {
- init_butterworth_2_low_pass(&indi.u[i], tau_axis[i], sample_time, 0.0f);
- init_butterworth_2_low_pass(&indi.rate[i], tau_axis[i], sample_time, 0.0f);
- }
+ //COMMENTED FIRMWARE
+ // // tau = 1/(2*pi*Fc)
+ // float tau = 1.0f / (2.0f * M_PI_F * indi.filt_cutoff);
+ // float tau_r = 1.0f / (2.0f * M_PI_F * indi.filt_cutoff_r);
+ // float tau_axis[3] = {tau, tau, tau_r};
+ // float sample_time = 1.0f / ATTITUDE_RATE;
+ // // Filtering of gyroscope and actuators
+ // for (int8_t i = 0; i < 3; i++) {
+ // init_butterworth_2_low_pass(&indi.u[i], tau_axis[i], sample_time, 0.0f);
+ // init_butterworth_2_low_pass(&indi.rate[i], tau_axis[i], sample_time, 0.0f);
+ // }
}
/**
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_mellinger.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_mellinger.c
index 4a2dd25252cfe6aba49584315e7f17ad7a4b0b8f..95bb7cd5922340168c7c2a63ab273ea29375a08d 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_mellinger.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/controller_mellinger.c
@@ -126,199 +126,200 @@ void controllerMellinger(control_t *control, setpoint_t *setpoint,
const state_t *state,
const uint32_t tick)
{
- struct vec r_error;
- struct vec v_error;
- struct vec target_thrust;
- struct vec z_axis;
- float current_thrust;
- struct vec x_axis_desired;
- struct vec y_axis_desired;
- struct vec x_c_des;
- struct vec eR, ew, M;
- float dt;
- float desiredYaw = 0; //deg
-
- if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
- return;
- }
-
- dt = (float)(1.0f/ATTITUDE_RATE);
- struct vec setpointPos = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z);
- struct vec setpointVel = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z);
- struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z);
- struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z);
-
- // Position Error (ep)
- r_error = vsub(setpointPos, statePos);
-
- // Velocity Error (ev)
- v_error = vsub(setpointVel, stateVel);
-
- // Integral Error
- i_error_z += r_error.z * dt;
- i_error_z = clamp(i_error_z, -i_range_z, i_range_z);
-
- i_error_x += r_error.x * dt;
- i_error_x = clamp(i_error_x, -i_range_xy, i_range_xy);
-
- i_error_y += r_error.y * dt;
- i_error_y = clamp(i_error_y, -i_range_xy, i_range_xy);
-
- // Desired thrust [F_des]
- if (setpoint->mode.x == modeAbs) {
- target_thrust.x = g_vehicleMass * setpoint->acceleration.x + kp_xy * r_error.x + kd_xy * v_error.x + ki_xy * i_error_x;
- target_thrust.y = g_vehicleMass * setpoint->acceleration.y + kp_xy * r_error.y + kd_xy * v_error.y + ki_xy * i_error_y;
- target_thrust.z = g_vehicleMass * (setpoint->acceleration.z + GRAVITY_MAGNITUDE) + kp_z * r_error.z + kd_z * v_error.z + ki_z * i_error_z;
- } else {
- target_thrust.x = -sinf(radians(setpoint->attitude.pitch));
- target_thrust.y = -sinf(radians(setpoint->attitude.roll));
- // In case of a timeout, the commander tries to level, ie. x/y are disabled, but z will use the previous setting
- // In that case we ignore the last feedforward term for acceleration
- if (setpoint->mode.z == modeAbs) {
- target_thrust.z = g_vehicleMass * GRAVITY_MAGNITUDE + kp_z * r_error.z + kd_z * v_error.z + ki_z * i_error_z;
- } else {
- target_thrust.z = 1;
- }
- }
-
- // Rate-controlled YAW is moving YAW angle setpoint
- if (setpoint->mode.yaw == modeVelocity) {
- desiredYaw = state->attitude.yaw + setpoint->attitudeRate.yaw * dt;
- } else if (setpoint->mode.yaw == modeAbs) {
- desiredYaw = setpoint->attitude.yaw;
- } else if (setpoint->mode.quat == modeAbs) {
- struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w);
- struct vec rpy = quat2rpy(setpoint_quat);
- desiredYaw = degrees(rpy.z);
- }
-
- // Z-Axis [zB]
- struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
- struct mat33 R = quat2rotmat(q);
- z_axis = mcolumn(R, 2);
-
- // yaw correction (only if position control is not used)
- if (setpoint->mode.x != modeAbs) {
- struct vec x_yaw = mcolumn(R, 0);
- x_yaw.z = 0;
- x_yaw = vnormalize(x_yaw);
- struct vec y_yaw = vcross(mkvec(0, 0, 1), x_yaw);
- struct mat33 R_yaw_only = mcolumns(x_yaw, y_yaw, mkvec(0, 0, 1));
- target_thrust = mvmul(R_yaw_only, target_thrust);
- }
-
- // Current thrust [F]
- current_thrust = vdot(target_thrust, z_axis);
-
- // Calculate axis [zB_des]
- z_axis_desired = vnormalize(target_thrust);
-
- // [xC_des]
- // x_axis_desired = z_axis_desired x [sin(yaw), cos(yaw), 0]^T
- x_c_des.x = cosf(radians(desiredYaw));
- x_c_des.y = sinf(radians(desiredYaw));
- x_c_des.z = 0;
- // [yB_des]
- y_axis_desired = vnormalize(vcross(z_axis_desired, x_c_des));
- // [xB_des]
- x_axis_desired = vcross(y_axis_desired, z_axis_desired);
-
- // [eR]
- // Slow version
- // struct mat33 Rdes = mcolumns(
- // mkvec(x_axis_desired.x, x_axis_desired.y, x_axis_desired.z),
- // mkvec(y_axis_desired.x, y_axis_desired.y, y_axis_desired.z),
- // mkvec(z_axis_desired.x, z_axis_desired.y, z_axis_desired.z));
-
- // struct mat33 R_transpose = mtranspose(R);
- // struct mat33 Rdes_transpose = mtranspose(Rdes);
-
- // struct mat33 eRM = msub(mmult(Rdes_transpose, R), mmult(R_transpose, Rdes));
-
- // eR.x = eRM.m[2][1];
- // eR.y = -eRM.m[0][2];
- // eR.z = eRM.m[1][0];
-
- // Fast version (generated using Mathematica)
- float x = q.x;
- float y = q.y;
- float z = q.z;
- float w = q.w;
- eR.x = (-1 + 2*fsqr(x) + 2*fsqr(y))*y_axis_desired.z + z_axis_desired.y - 2*(x*y_axis_desired.x*z + y*y_axis_desired.y*z - x*y*z_axis_desired.x + fsqr(x)*z_axis_desired.y + fsqr(z)*z_axis_desired.y - y*z*z_axis_desired.z) + 2*w*(-(y*y_axis_desired.x) - z*z_axis_desired.x + x*(y_axis_desired.y + z_axis_desired.z));
- eR.y = x_axis_desired.z - z_axis_desired.x - 2*(fsqr(x)*x_axis_desired.z + y*(x_axis_desired.z*y - x_axis_desired.y*z) - (fsqr(y) + fsqr(z))*z_axis_desired.x + x*(-(x_axis_desired.x*z) + y*z_axis_desired.y + z*z_axis_desired.z) + w*(x*x_axis_desired.y + z*z_axis_desired.y - y*(x_axis_desired.x + z_axis_desired.z)));
- eR.z = y_axis_desired.x - 2*(y*(x*x_axis_desired.x + y*y_axis_desired.x - x*y_axis_desired.y) + w*(x*x_axis_desired.z + y*y_axis_desired.z)) + 2*(-(x_axis_desired.z*y) + w*(x_axis_desired.x + y_axis_desired.y) + x*y_axis_desired.z)*z - 2*y_axis_desired.x*fsqr(z) + x_axis_desired.y*(-1 + 2*fsqr(x) + 2*fsqr(z));
-
- // Account for Crazyflie coordinate system
- eR.y = -eR.y;
-
- // [ew]
- float err_d_roll = 0;
- float err_d_pitch = 0;
-
- float stateAttitudeRateRoll = radians(sensors->gyro.x);
- float stateAttitudeRatePitch = -radians(sensors->gyro.y);
- float stateAttitudeRateYaw = radians(sensors->gyro.z);
-
- ew.x = radians(setpoint->attitudeRate.roll) - stateAttitudeRateRoll;
- ew.y = -radians(setpoint->attitudeRate.pitch) - stateAttitudeRatePitch;
- ew.z = radians(setpoint->attitudeRate.yaw) - stateAttitudeRateYaw;
- if (prev_omega_roll == prev_omega_roll) { /*d part initialized*/
- err_d_roll = ((radians(setpoint->attitudeRate.roll) - prev_setpoint_omega_roll) - (stateAttitudeRateRoll - prev_omega_roll)) / dt;
- err_d_pitch = (-(radians(setpoint->attitudeRate.pitch) - prev_setpoint_omega_pitch) - (stateAttitudeRatePitch - prev_omega_pitch)) / dt;
- }
- prev_omega_roll = stateAttitudeRateRoll;
- prev_omega_pitch = stateAttitudeRatePitch;
- prev_setpoint_omega_roll = radians(setpoint->attitudeRate.roll);
- prev_setpoint_omega_pitch = radians(setpoint->attitudeRate.pitch);
-
- // Integral Error
- i_error_m_x += (-eR.x) * dt;
- i_error_m_x = clamp(i_error_m_x, -i_range_m_xy, i_range_m_xy);
-
- i_error_m_y += (-eR.y) * dt;
- i_error_m_y = clamp(i_error_m_y, -i_range_m_xy, i_range_m_xy);
-
- i_error_m_z += (-eR.z) * dt;
- i_error_m_z = clamp(i_error_m_z, -i_range_m_z, i_range_m_z);
-
- // Moment:
- M.x = -kR_xy * eR.x + kw_xy * ew.x + ki_m_xy * i_error_m_x + kd_omega_rp * err_d_roll;
- M.y = -kR_xy * eR.y + kw_xy * ew.y + ki_m_xy * i_error_m_y + kd_omega_rp * err_d_pitch;
- M.z = -kR_z * eR.z + kw_z * ew.z + ki_m_z * i_error_m_z;
-
- // Output
- if (setpoint->mode.z == modeDisable) {
- control->thrust = setpoint->thrust;
- } else {
- control->thrust = massThrust * current_thrust;
- }
-
- cmd_thrust = control->thrust;
- r_roll = radians(sensors->gyro.x);
- r_pitch = -radians(sensors->gyro.y);
- r_yaw = radians(sensors->gyro.z);
- accelz = sensors->acc.z;
-
- if (control->thrust > 0) {
- control->roll = clamp(M.x, -32000, 32000);
- control->pitch = clamp(M.y, -32000, 32000);
- control->yaw = clamp(-M.z, -32000, 32000);
-
- cmd_roll = control->roll;
- cmd_pitch = control->pitch;
- cmd_yaw = control->yaw;
-
- } else {
- control->roll = 0;
- control->pitch = 0;
- control->yaw = 0;
-
- cmd_roll = control->roll;
- cmd_pitch = control->pitch;
- cmd_yaw = control->yaw;
-
- controllerMellingerReset();
- }
+ //COMMENTED FIRMWARE
+ // struct vec r_error;
+ // struct vec v_error;
+ // struct vec target_thrust;
+ // struct vec z_axis;
+ // float current_thrust;
+ // struct vec x_axis_desired;
+ // struct vec y_axis_desired;
+ // struct vec x_c_des;
+ // struct vec eR, ew, M;
+ // float dt;
+ // float desiredYaw = 0; //deg
+
+ // if (!RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
+ // return;
+ // }
+
+ // dt = (float)(1.0f/ATTITUDE_RATE);
+ // struct vec setpointPos = mkvec(setpoint->position.x, setpoint->position.y, setpoint->position.z);
+ // struct vec setpointVel = mkvec(setpoint->velocity.x, setpoint->velocity.y, setpoint->velocity.z);
+ // struct vec statePos = mkvec(state->position.x, state->position.y, state->position.z);
+ // struct vec stateVel = mkvec(state->velocity.x, state->velocity.y, state->velocity.z);
+
+ // // Position Error (ep)
+ // r_error = vsub(setpointPos, statePos);
+
+ // // Velocity Error (ev)
+ // v_error = vsub(setpointVel, stateVel);
+
+ // // Integral Error
+ // i_error_z += r_error.z * dt;
+ // i_error_z = clamp(i_error_z, -i_range_z, i_range_z);
+
+ // i_error_x += r_error.x * dt;
+ // i_error_x = clamp(i_error_x, -i_range_xy, i_range_xy);
+
+ // i_error_y += r_error.y * dt;
+ // i_error_y = clamp(i_error_y, -i_range_xy, i_range_xy);
+
+ // // Desired thrust [F_des]
+ // if (setpoint->mode.x == modeAbs) {
+ // target_thrust.x = g_vehicleMass * setpoint->acceleration.x + kp_xy * r_error.x + kd_xy * v_error.x + ki_xy * i_error_x;
+ // target_thrust.y = g_vehicleMass * setpoint->acceleration.y + kp_xy * r_error.y + kd_xy * v_error.y + ki_xy * i_error_y;
+ // target_thrust.z = g_vehicleMass * (setpoint->acceleration.z + GRAVITY_MAGNITUDE) + kp_z * r_error.z + kd_z * v_error.z + ki_z * i_error_z;
+ // } else {
+ // target_thrust.x = -sinf(radians(setpoint->attitude.pitch));
+ // target_thrust.y = -sinf(radians(setpoint->attitude.roll));
+ // // In case of a timeout, the commander tries to level, ie. x/y are disabled, but z will use the previous setting
+ // // In that case we ignore the last feedforward term for acceleration
+ // if (setpoint->mode.z == modeAbs) {
+ // target_thrust.z = g_vehicleMass * GRAVITY_MAGNITUDE + kp_z * r_error.z + kd_z * v_error.z + ki_z * i_error_z;
+ // } else {
+ // target_thrust.z = 1;
+ // }
+ // }
+
+ // // Rate-controlled YAW is moving YAW angle setpoint
+ // if (setpoint->mode.yaw == modeVelocity) {
+ // desiredYaw = state->attitude.yaw + setpoint->attitudeRate.yaw * dt;
+ // } else if (setpoint->mode.yaw == modeAbs) {
+ // desiredYaw = setpoint->attitude.yaw;
+ // } else if (setpoint->mode.quat == modeAbs) {
+ // struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w);
+ // struct vec rpy = quat2rpy(setpoint_quat);
+ // desiredYaw = degrees(rpy.z);
+ // }
+
+ // // Z-Axis [zB]
+ // struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
+ // struct mat33 R = quat2rotmat(q);
+ // z_axis = mcolumn(R, 2);
+
+ // // yaw correction (only if position control is not used)
+ // if (setpoint->mode.x != modeAbs) {
+ // struct vec x_yaw = mcolumn(R, 0);
+ // x_yaw.z = 0;
+ // x_yaw = vnormalize(x_yaw);
+ // struct vec y_yaw = vcross(mkvec(0, 0, 1), x_yaw);
+ // struct mat33 R_yaw_only = mcolumns(x_yaw, y_yaw, mkvec(0, 0, 1));
+ // target_thrust = mvmul(R_yaw_only, target_thrust);
+ // }
+
+ // // Current thrust [F]
+ // current_thrust = vdot(target_thrust, z_axis);
+
+ // // Calculate axis [zB_des]
+ // z_axis_desired = vnormalize(target_thrust);
+
+ // // [xC_des]
+ // // x_axis_desired = z_axis_desired x [sin(yaw), cos(yaw), 0]^T
+ // x_c_des.x = cosf(radians(desiredYaw));
+ // x_c_des.y = sinf(radians(desiredYaw));
+ // x_c_des.z = 0;
+ // // [yB_des]
+ // y_axis_desired = vnormalize(vcross(z_axis_desired, x_c_des));
+ // // [xB_des]
+ // x_axis_desired = vcross(y_axis_desired, z_axis_desired);
+
+ // // [eR]
+ // // Slow version
+ // // struct mat33 Rdes = mcolumns(
+ // // mkvec(x_axis_desired.x, x_axis_desired.y, x_axis_desired.z),
+ // // mkvec(y_axis_desired.x, y_axis_desired.y, y_axis_desired.z),
+ // // mkvec(z_axis_desired.x, z_axis_desired.y, z_axis_desired.z));
+
+ // // struct mat33 R_transpose = mtranspose(R);
+ // // struct mat33 Rdes_transpose = mtranspose(Rdes);
+
+ // // struct mat33 eRM = msub(mmult(Rdes_transpose, R), mmult(R_transpose, Rdes));
+
+ // // eR.x = eRM.m[2][1];
+ // // eR.y = -eRM.m[0][2];
+ // // eR.z = eRM.m[1][0];
+
+ // // Fast version (generated using Mathematica)
+ // float x = q.x;
+ // float y = q.y;
+ // float z = q.z;
+ // float w = q.w;
+ // eR.x = (-1 + 2*fsqr(x) + 2*fsqr(y))*y_axis_desired.z + z_axis_desired.y - 2*(x*y_axis_desired.x*z + y*y_axis_desired.y*z - x*y*z_axis_desired.x + fsqr(x)*z_axis_desired.y + fsqr(z)*z_axis_desired.y - y*z*z_axis_desired.z) + 2*w*(-(y*y_axis_desired.x) - z*z_axis_desired.x + x*(y_axis_desired.y + z_axis_desired.z));
+ // eR.y = x_axis_desired.z - z_axis_desired.x - 2*(fsqr(x)*x_axis_desired.z + y*(x_axis_desired.z*y - x_axis_desired.y*z) - (fsqr(y) + fsqr(z))*z_axis_desired.x + x*(-(x_axis_desired.x*z) + y*z_axis_desired.y + z*z_axis_desired.z) + w*(x*x_axis_desired.y + z*z_axis_desired.y - y*(x_axis_desired.x + z_axis_desired.z)));
+ // eR.z = y_axis_desired.x - 2*(y*(x*x_axis_desired.x + y*y_axis_desired.x - x*y_axis_desired.y) + w*(x*x_axis_desired.z + y*y_axis_desired.z)) + 2*(-(x_axis_desired.z*y) + w*(x_axis_desired.x + y_axis_desired.y) + x*y_axis_desired.z)*z - 2*y_axis_desired.x*fsqr(z) + x_axis_desired.y*(-1 + 2*fsqr(x) + 2*fsqr(z));
+
+ // // Account for Crazyflie coordinate system
+ // eR.y = -eR.y;
+
+ // // [ew]
+ // float err_d_roll = 0;
+ // float err_d_pitch = 0;
+
+ // float stateAttitudeRateRoll = radians(sensors->gyro.x);
+ // float stateAttitudeRatePitch = -radians(sensors->gyro.y);
+ // float stateAttitudeRateYaw = radians(sensors->gyro.z);
+
+ // ew.x = radians(setpoint->attitudeRate.roll) - stateAttitudeRateRoll;
+ // ew.y = -radians(setpoint->attitudeRate.pitch) - stateAttitudeRatePitch;
+ // ew.z = radians(setpoint->attitudeRate.yaw) - stateAttitudeRateYaw;
+ // if (prev_omega_roll == prev_omega_roll) { /*d part initialized*/
+ // err_d_roll = ((radians(setpoint->attitudeRate.roll) - prev_setpoint_omega_roll) - (stateAttitudeRateRoll - prev_omega_roll)) / dt;
+ // err_d_pitch = (-(radians(setpoint->attitudeRate.pitch) - prev_setpoint_omega_pitch) - (stateAttitudeRatePitch - prev_omega_pitch)) / dt;
+ // }
+ // prev_omega_roll = stateAttitudeRateRoll;
+ // prev_omega_pitch = stateAttitudeRatePitch;
+ // prev_setpoint_omega_roll = radians(setpoint->attitudeRate.roll);
+ // prev_setpoint_omega_pitch = radians(setpoint->attitudeRate.pitch);
+
+ // // Integral Error
+ // i_error_m_x += (-eR.x) * dt;
+ // i_error_m_x = clamp(i_error_m_x, -i_range_m_xy, i_range_m_xy);
+
+ // i_error_m_y += (-eR.y) * dt;
+ // i_error_m_y = clamp(i_error_m_y, -i_range_m_xy, i_range_m_xy);
+
+ // i_error_m_z += (-eR.z) * dt;
+ // i_error_m_z = clamp(i_error_m_z, -i_range_m_z, i_range_m_z);
+
+ // // Moment:
+ // M.x = -kR_xy * eR.x + kw_xy * ew.x + ki_m_xy * i_error_m_x + kd_omega_rp * err_d_roll;
+ // M.y = -kR_xy * eR.y + kw_xy * ew.y + ki_m_xy * i_error_m_y + kd_omega_rp * err_d_pitch;
+ // M.z = -kR_z * eR.z + kw_z * ew.z + ki_m_z * i_error_m_z;
+
+ // // Output
+ // if (setpoint->mode.z == modeDisable) {
+ // control->thrust = setpoint->thrust;
+ // } else {
+ // control->thrust = massThrust * current_thrust;
+ // }
+
+ // cmd_thrust = control->thrust;
+ // r_roll = radians(sensors->gyro.x);
+ // r_pitch = -radians(sensors->gyro.y);
+ // r_yaw = radians(sensors->gyro.z);
+ // accelz = sensors->acc.z;
+
+ // if (control->thrust > 0) {
+ // control->roll = clamp(M.x, -32000, 32000);
+ // control->pitch = clamp(M.y, -32000, 32000);
+ // control->yaw = clamp(-M.z, -32000, 32000);
+
+ // cmd_roll = control->roll;
+ // cmd_pitch = control->pitch;
+ // cmd_yaw = control->yaw;
+
+ // } else {
+ // control->roll = 0;
+ // control->pitch = 0;
+ // control->yaw = 0;
+
+ // cmd_roll = control->roll;
+ // cmd_pitch = control->pitch;
+ // cmd_yaw = control->yaw;
+
+ // controllerMellingerReset();
+ // }
}
PARAM_GROUP_START(ctrlMel)
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 7d309ccc629a31931b96af6a5996672fd103d019..4cef37cd566654cc7eb7678e9832c515bafe6f4a 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
@@ -262,9 +262,11 @@ void crtpCommanderHighLevelInit(void)
plan_init(&planner);
//Start the trajectory task
- STATIC_MEM_TASK_CREATE(crtpCommanderHighLevelTask, crtpCommanderHighLevelTask, CMD_HIGH_LEVEL_TASK_NAME, NULL, CMD_HIGH_LEVEL_TASK_PRI);
+ //COMMENTED FIRMWARE
+ //STATIC_MEM_TASK_CREATE(crtpCommanderHighLevelTask, crtpCommanderHighLevelTask, CMD_HIGH_LEVEL_TASK_NAME, NULL, CMD_HIGH_LEVEL_TASK_PRI);
- lockTraj = xSemaphoreCreateMutexStatic(&lockTrajBuffer);
+ //COMMENTED FIRMWARE
+ //lockTraj = xSemaphoreCreateMutexStatic(&lockTrajBuffer);
pos = vzero();
vel = vzero();
@@ -453,7 +455,8 @@ int takeoff_with_velocity(const struct data_takeoff_with_velocity* data)
}
float velocity = data->velocity > 0 ? data->velocity : defaultTakeoffVelocity;
- float duration = fabsf(height - pos.z) / velocity;
+ //COMMENTED FIRMWARE
+ float duration = 1;//fabsf(height - pos.z) / velocity;
result = plan_takeoff(&planner, pos, yaw, height, hover_yaw, duration, t);
xSemaphoreGive(lockTraj);
}
@@ -508,7 +511,8 @@ int land_with_velocity(const struct data_land_with_velocity* data)
}
float velocity = data->velocity > 0 ? data->velocity : defaultLandingVelocity;
- float duration = fabsf(height - pos.z) / velocity;
+ //COMMENTED FIRMWARE
+ float duration = 1;//fabsf(height - pos.z) / velocity;
result = plan_land(&planner, pos, yaw, height, hover_yaw, duration, t);
xSemaphoreGive(lockTraj);
}
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 962121d4a3431f4137e6a54ad223d12537a1f426..c8eedf066c35ead7ad4b21e780be53b0f8da7afc 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,6 +24,7 @@
*
*/
#include <math.h>
+#include "empty_math.h"
#include <stdbool.h>
#include "crtp_commander.h"
@@ -132,7 +133,8 @@ void crtpCommanderRpytDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk)
if (thrustLocked || (rawThrust < MIN_THRUST)) {
setpoint->thrust = 0;
} else {
- setpoint->thrust = fminf(rawThrust, MAX_THRUST);
+ //COMMENTED FIRMWARE
+ setpoint->thrust = rawThrust;//fminf(rawThrust, MAX_THRUST);
}
if (altHoldMode) {
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 1468992ca4177578906315b04032f22bc33214b8..b44449844a7dd0b4c8a65f6ca0e7ded159a23554 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,6 +25,7 @@
*/
#include <string.h>
#include <stdint.h>
+#include "empty_math.h"
#include "FreeRTOS.h"
#include "task.h"
@@ -33,6 +34,7 @@
#include "crtp_localization_service.h"
#include "log.h"
#include "param.h"
+#include "cfassert.h"
#include "stabilizer_types.h"
#include "stabilizer.h"
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 720d7de7e990939fc7f60cb2d4731a3e8821e59c..a3b9a481233ab6c2130d739df75c968130171689 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
@@ -86,8 +86,9 @@ static void crtpSrvTask(void* prm)
break;
case linkSource:
p.size = CRTP_MAX_DATA_SIZE;
- bzero(p.data, CRTP_MAX_DATA_SIZE);
- strcpy((char*)p.data, "Bitcraze Crazyflie");
+ //COMMENTED FIRMWARE
+ //bzero(p.data, CRTP_MAX_DATA_SIZE);
+ //strcpy((char*)p.data, "Bitcraze Crazyflie");
crtpSendPacketBlock(&p);
break;
case linkSink:
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/estimator.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/estimator.c
index fb2df521db9ff58e81ad1d6594316399da618b93..ecf18fdfbf3bfa537b0d8365055f90cad3ffb592 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/estimator.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/estimator.c
@@ -88,7 +88,8 @@ static EstimatorFcns estimatorFunctions[] = {
};
void stateEstimatorInit(StateEstimatorType estimator) {
- measurementsQueue = STATIC_MEM_QUEUE_CREATE(measurementsQueue);
+ //COMMENTED FIRMWARE
+ //measurementsQueue = STATIC_MEM_QUEUE_CREATE(measurementsQueue);
stateEstimatorSwitchTo(estimator);
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/estimator_kalman.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/estimator_kalman.c
index f9f6c25f9cc830d416e56ddbafe8f292389b3904..3ac426322d72e114c302e9275079d707b1c512e1 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/estimator_kalman.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/estimator_kalman.c
@@ -181,11 +181,12 @@ STATIC_MEM_TASK_ALLOC_STACK_NO_DMA_CCM_SAFE(kalmanTask, 3 * configMINIMAL_STACK_
// Called one time during system startup
void estimatorKalmanTaskInit() {
- vSemaphoreCreateBinary(runTaskSemaphore);
+ //COMMENTED FIRMWARE
+ // vSemaphoreCreateBinary(runTaskSemaphore);
- dataMutex = xSemaphoreCreateMutexStatic(&dataMutexBuffer);
+ // dataMutex = xSemaphoreCreateMutexStatic(&dataMutexBuffer);
- STATIC_MEM_TASK_CREATE(kalmanTask, kalmanTask, KALMAN_TASK_NAME, NULL, KALMAN_TASK_PRI);
+ // STATIC_MEM_TASK_CREATE(kalmanTask, kalmanTask, KALMAN_TASK_NAME, NULL, KALMAN_TASK_PRI);
isInit = true;
}
@@ -232,9 +233,10 @@ static void kalmanTask(void* parameters) {
nextPrediction = osTick + S2T(1.0f / PREDICT_RATE);
- if (!rateSupervisorValidate(&rateSupervisorContext, T2M(osTick))) {
- DEBUG_PRINT("WARNING: Kalman prediction rate low (%lu)\n", rateSupervisorLatestCount(&rateSupervisorContext));
- }
+ //COMMENTED FIRMWARE
+ // if (!rateSupervisorValidate(&rateSupervisorContext, T2M(osTick))) {
+ // DEBUG_PRINT("WARNING: Kalman prediction rate low (%lu)\n", rateSupervisorLatestCount(&rateSupervisorContext));
+ // }
}
/**
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/eventtrigger.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/eventtrigger.c
index 29ef92812380ea6a989f34af847111a7267416c2..c0e0d12387fd0e642338f643bccc6ad9ca206663 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/eventtrigger.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/eventtrigger.c
@@ -55,12 +55,13 @@ const eventtrigger *eventtriggerGetById(uint16_t id)
const eventtrigger* eventtriggerGetByName(const char *name)
{
const eventtrigger* result = &_eventtrigger_start;
- int numEventtriggers = &_eventtrigger_stop - &_eventtrigger_start;
- for (int i = 0; i < numEventtriggers; ++i) {
- if (strcmp(result[i].name, name) == 0) {
- return &result[i];
- }
- }
+ //COMMENTED FIRMWARE
+ // int numEventtriggers = &_eventtrigger_stop - &_eventtrigger_start;
+ // for (int i = 0; i < numEventtriggers; ++i) {
+ // if (strcmp(result[i].name, name) == 0) {
+ // return &result[i];
+ // }
+ // }
return 0;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/health.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/health.c
index ec778f18bf331d56ac59ddf058bdeabae320da4f..a2c512e665da46f68aeea2d05066e5dd5c501afb 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/health.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/health.c
@@ -284,9 +284,10 @@ void healthRunTests(sensorData_t *sensors)
for (int j = 0; j < 3; j++)
{
motorsBeep(m, true, testsound[m], (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / A4)/ 20);
- vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
+ //COMMENTED FIRMWARE
+ vTaskDelay(MOTORS_TEST_ON_TIME_MS);//M2T(MOTORS_TEST_ON_TIME_MS));
motorsBeep(m, false, 0, 0);
- vTaskDelay(M2T(100));
+ vTaskDelay(100);//M2T(100));
}
}
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/info.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/info.c
index fb98ee30317edf2a49f1ad13669f4fb2e42af75b..91c2c8dc05068d0643ce5cd7f2ddc9bab8986b50 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/info.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/info.c
@@ -74,103 +74,104 @@ void infoInit()
void infoTask(void *param)
{
- CRTPPacket p;
- int i;
- static int ctr=0;
-
- while (TRUE)
- {
- if (crtpReceivePacketWait(crtpInfo, &p, 1000) == pdTRUE)
- {
- InfoNbr infoNbr = CRTP_GET_NBR(p.port);
-
- switch (infoNbr)
- {
- case infoCopterNr:
- if (p.data[0] == infoName)
- {
- p.data[1] = 0x90;
- p.data[2] = 0x00; //Version 0.9.0 (Crazyflie)
- strcpy((char*)&p.data[3], "CrazyFlie");
-
- p.size = 3+strlen("CrazyFlie");
- crtpSendPacketBlock(&p);
- } else if (p.data[0] == infoVersion) {
- i=1;
-
- strncpy((char*)&p.data[i], V_SLOCAL_REVISION, 31-i);
- i += strlen(V_SLOCAL_REVISION);
-
- if (i<31) p.data[i++] = ',';
-
- strncpy((char*)&p.data[i], V_SREVISION, 31-i);
- i += strlen(V_SREVISION);
-
- if (i<31) p.data[i++] = ',';
-
- strncpy((char*)&p.data[i], V_STAG, 31-i);
- i += strlen(V_STAG);
-
- if (i<31) p.data[i++] = ',';
- if (i<31) p.data[i++] = V_MODIFIED?'M':'C';
-
- p.size = (i<31)?i:31;
- crtpSendPacketBlock(&p);
- } else if (p.data[0] == infoCpuId) {
- memcpy((char*)&p.data[1], (char*)CpuId, 12);
-
- p.size = 13;
- crtpSendPacketBlock(&p);
- }
-
- break;
- case infoBatteryNr:
- if (p.data[0] == batteryVoltage)
- {
- float value = pmGetBatteryVoltage();
-
- memcpy(&p.data[1], (char*)&value, 4);
-
- p.size = 5;
- crtpSendPacketBlock(&p);
- } else if (p.data[0] == batteryMax) {
- float value = pmGetBatteryVoltageMax();
-
- memcpy(&p.data[1], (char*)&value, 4);
-
- p.size = 5;
- crtpSendPacketBlock(&p);
- } else if (p.data[0] == batteryMin) {
- float value = pmGetBatteryVoltageMin();
-
- memcpy(&p.data[1], (char*)&value, 4);
-
- p.size = 5;
- crtpSendPacketBlock(&p);
- }
- break;
- default:
- break;
- }
- }
-
- // Send a warning message if the battery voltage drops under 3.3V
- // This is sent every 5 info transaction or every 5 seconds
- if (ctr++>5) {
- ctr=0;
-
- if (pmGetBatteryVoltageMin() < INFO_BAT_WARNING)
- {
- float value = pmGetBatteryVoltage();
-
- p.port = CRTP_PORT(0,8,3);
- p.data[0] = 0;
- memcpy(&p.data[1], (char*)&value, 4);
-
- p.size = 5;
- crtpSendPacketBlock(&p);
- }
- }
-
- }
+ //COMMENTED FIRMWARE
+ // CRTPPacket p;
+ // int i;
+ // static int ctr=0;
+
+ // while (TRUE)
+ // {
+ // if (crtpReceivePacketWait(crtpInfo, &p, 1000) == pdTRUE)
+ // {
+ // InfoNbr infoNbr = CRTP_GET_NBR(p.port);
+
+ // switch (infoNbr)
+ // {
+ // case infoCopterNr:
+ // if (p.data[0] == infoName)
+ // {
+ // p.data[1] = 0x90;
+ // p.data[2] = 0x00; //Version 0.9.0 (Crazyflie)
+ // strcpy((char*)&p.data[3], "CrazyFlie");
+
+ // p.size = 3+strlen("CrazyFlie");
+ // crtpSendPacketBlock(&p);
+ // } else if (p.data[0] == infoVersion) {
+ // i=1;
+
+ // strncpy((char*)&p.data[i], V_SLOCAL_REVISION, 31-i);
+ // i += strlen(V_SLOCAL_REVISION);
+
+ // if (i<31) p.data[i++] = ',';
+
+ // strncpy((char*)&p.data[i], V_SREVISION, 31-i);
+ // i += strlen(V_SREVISION);
+
+ // if (i<31) p.data[i++] = ',';
+
+ // strncpy((char*)&p.data[i], V_STAG, 31-i);
+ // i += strlen(V_STAG);
+
+ // if (i<31) p.data[i++] = ',';
+ // if (i<31) p.data[i++] = V_MODIFIED?'M':'C';
+
+ // p.size = (i<31)?i:31;
+ // crtpSendPacketBlock(&p);
+ // } else if (p.data[0] == infoCpuId) {
+ // memcpy((char*)&p.data[1], (char*)CpuId, 12);
+
+ // p.size = 13;
+ // crtpSendPacketBlock(&p);
+ // }
+
+ // break;
+ // case infoBatteryNr:
+ // if (p.data[0] == batteryVoltage)
+ // {
+ // float value = pmGetBatteryVoltage();
+
+ // memcpy(&p.data[1], (char*)&value, 4);
+
+ // p.size = 5;
+ // crtpSendPacketBlock(&p);
+ // } else if (p.data[0] == batteryMax) {
+ // float value = pmGetBatteryVoltageMax();
+
+ // memcpy(&p.data[1], (char*)&value, 4);
+
+ // p.size = 5;
+ // crtpSendPacketBlock(&p);
+ // } else if (p.data[0] == batteryMin) {
+ // float value = pmGetBatteryVoltageMin();
+
+ // memcpy(&p.data[1], (char*)&value, 4);
+
+ // p.size = 5;
+ // crtpSendPacketBlock(&p);
+ // }
+ // break;
+ // default:
+ // break;
+ // }
+ // }
+
+ // // Send a warning message if the battery voltage drops under 3.3V
+ // // This is sent every 5 info transaction or every 5 seconds
+ // if (ctr++>5) {
+ // ctr=0;
+
+ // if (pmGetBatteryVoltageMin() < INFO_BAT_WARNING)
+ // {
+ // float value = pmGetBatteryVoltage();
+
+ // p.port = CRTP_PORT(0,8,3);
+ // p.data[0] = 0;
+ // memcpy(&p.data[1], (char*)&value, 4);
+
+ // p.size = 5;
+ // crtpSendPacketBlock(&p);
+ // }
+ // }
+
+ // }
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/kalman_core.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/kalman_core.c
index 1cb0f2b584528c44cee35797e399ce944f513f66..c9575ec72acbefc651d7a1a2cea1cb1314dd1ad0 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/kalman_core.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/kalman_core.c
@@ -167,10 +167,11 @@ void kalmanCoreInit(kalmanCoreData_t* this) {
// this->S[KC_STATE_D2] = 0;
// reset the attitude quaternion
- initialQuaternion[0] = arm_cos_f32(initialYaw / 2);
- initialQuaternion[1] = 0.0;
- initialQuaternion[2] = 0.0;
- initialQuaternion[3] = arm_sin_f32(initialYaw / 2);
+ //COMMENTED FIRMWARE
+ // initialQuaternion[0] = arm_cos_f32(initialYaw / 2);
+ // initialQuaternion[1] = 0.0;
+ // initialQuaternion[2] = 0.0;
+ // initialQuaternion[3] = arm_sin_f32(initialYaw / 2);
for (int i = 0; i < 4; i++) { this->q[i] = initialQuaternion[i]; }
// then set the initial rotation matrix to the identity. This only affects
@@ -185,17 +186,18 @@ void kalmanCoreInit(kalmanCoreData_t* this) {
}
// initialize state variances
- this->P[KC_STATE_X][KC_STATE_X] = powf(stdDevInitialPosition_xy, 2);
- this->P[KC_STATE_Y][KC_STATE_Y] = powf(stdDevInitialPosition_xy, 2);
- this->P[KC_STATE_Z][KC_STATE_Z] = powf(stdDevInitialPosition_z, 2);
+ //COMMENTED FIRMWARE
+ // this->P[KC_STATE_X][KC_STATE_X] = powf(stdDevInitialPosition_xy, 2);
+ // this->P[KC_STATE_Y][KC_STATE_Y] = powf(stdDevInitialPosition_xy, 2);
+ // this->P[KC_STATE_Z][KC_STATE_Z] = powf(stdDevInitialPosition_z, 2);
- this->P[KC_STATE_PX][KC_STATE_PX] = powf(stdDevInitialVelocity, 2);
- this->P[KC_STATE_PY][KC_STATE_PY] = powf(stdDevInitialVelocity, 2);
- this->P[KC_STATE_PZ][KC_STATE_PZ] = powf(stdDevInitialVelocity, 2);
+ // this->P[KC_STATE_PX][KC_STATE_PX] = powf(stdDevInitialVelocity, 2);
+ // this->P[KC_STATE_PY][KC_STATE_PY] = powf(stdDevInitialVelocity, 2);
+ // this->P[KC_STATE_PZ][KC_STATE_PZ] = powf(stdDevInitialVelocity, 2);
- this->P[KC_STATE_D0][KC_STATE_D0] = powf(stdDevInitialAttitude_rollpitch, 2);
- this->P[KC_STATE_D1][KC_STATE_D1] = powf(stdDevInitialAttitude_rollpitch, 2);
- this->P[KC_STATE_D2][KC_STATE_D2] = powf(stdDevInitialAttitude_yaw, 2);
+ // this->P[KC_STATE_D0][KC_STATE_D0] = powf(stdDevInitialAttitude_rollpitch, 2);
+ // this->P[KC_STATE_D1][KC_STATE_D1] = powf(stdDevInitialAttitude_rollpitch, 2);
+ // this->P[KC_STATE_D2][KC_STATE_D2] = powf(stdDevInitialAttitude_yaw, 2);
//COMMENTED FIRMWARE
// this->Pm.numRows = KC_STATE_DIM;
@@ -572,17 +574,18 @@ void kalmanCoreAddProcessNoise(kalmanCoreData_t* this, float dt)
{
if (dt>0)
{
- this->P[KC_STATE_X][KC_STATE_X] += powf(procNoiseAcc_xy*dt*dt + procNoiseVel*dt + procNoisePos, 2); // add process noise on position
- this->P[KC_STATE_Y][KC_STATE_Y] += powf(procNoiseAcc_xy*dt*dt + procNoiseVel*dt + procNoisePos, 2); // add process noise on position
- this->P[KC_STATE_Z][KC_STATE_Z] += powf(procNoiseAcc_z*dt*dt + procNoiseVel*dt + procNoisePos, 2); // add process noise on position
-
- this->P[KC_STATE_PX][KC_STATE_PX] += powf(procNoiseAcc_xy*dt + procNoiseVel, 2); // add process noise on velocity
- this->P[KC_STATE_PY][KC_STATE_PY] += powf(procNoiseAcc_xy*dt + procNoiseVel, 2); // add process noise on velocity
- this->P[KC_STATE_PZ][KC_STATE_PZ] += powf(procNoiseAcc_z*dt + procNoiseVel, 2); // add process noise on velocity
-
- this->P[KC_STATE_D0][KC_STATE_D0] += powf(measNoiseGyro_rollpitch * dt + procNoiseAtt, 2);
- this->P[KC_STATE_D1][KC_STATE_D1] += powf(measNoiseGyro_rollpitch * dt + procNoiseAtt, 2);
- this->P[KC_STATE_D2][KC_STATE_D2] += powf(measNoiseGyro_yaw * dt + procNoiseAtt, 2);
+ //COMMENTED FIRMWARE
+ // this->P[KC_STATE_X][KC_STATE_X] += powf(procNoiseAcc_xy*dt*dt + procNoiseVel*dt + procNoisePos, 2); // add process noise on position
+ // this->P[KC_STATE_Y][KC_STATE_Y] += powf(procNoiseAcc_xy*dt*dt + procNoiseVel*dt + procNoisePos, 2); // add process noise on position
+ // this->P[KC_STATE_Z][KC_STATE_Z] += powf(procNoiseAcc_z*dt*dt + procNoiseVel*dt + procNoisePos, 2); // add process noise on position
+
+ // this->P[KC_STATE_PX][KC_STATE_PX] += powf(procNoiseAcc_xy*dt + procNoiseVel, 2); // add process noise on velocity
+ // this->P[KC_STATE_PY][KC_STATE_PY] += powf(procNoiseAcc_xy*dt + procNoiseVel, 2); // add process noise on velocity
+ // this->P[KC_STATE_PZ][KC_STATE_PZ] += powf(procNoiseAcc_z*dt + procNoiseVel, 2); // add process noise on velocity
+
+ // this->P[KC_STATE_D0][KC_STATE_D0] += powf(measNoiseGyro_rollpitch * dt + procNoiseAtt, 2);
+ // this->P[KC_STATE_D1][KC_STATE_D1] += powf(measNoiseGyro_rollpitch * dt + procNoiseAtt, 2);
+ // this->P[KC_STATE_D2][KC_STATE_D2] += powf(measNoiseGyro_yaw * dt + procNoiseAtt, 2);
}
for (int i=0; i<KC_STATE_DIM; i++) {
@@ -746,17 +749,18 @@ void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, co
};
// convert the new attitude into Euler YPR
- float yaw = atan2f(2*(this->q[1]*this->q[2]+this->q[0]*this->q[3]) , this->q[0]*this->q[0] + this->q[1]*this->q[1] - this->q[2]*this->q[2] - this->q[3]*this->q[3]);
- float pitch = asinf(-2*(this->q[1]*this->q[3] - this->q[0]*this->q[2]));
- float roll = atan2f(2*(this->q[2]*this->q[3]+this->q[0]*this->q[1]) , this->q[0]*this->q[0] - this->q[1]*this->q[1] - this->q[2]*this->q[2] + this->q[3]*this->q[3]);
-
- // Save attitude, adjusted for the legacy CF2 body coordinate system
- state->attitude = (attitude_t){
- .timestamp = tick,
- .roll = roll*RAD_TO_DEG,
- .pitch = -pitch*RAD_TO_DEG,
- .yaw = yaw*RAD_TO_DEG
- };
+ //COMMENTED FIRMWARE
+ // float yaw = atan2f(2*(this->q[1]*this->q[2]+this->q[0]*this->q[3]) , this->q[0]*this->q[0] + this->q[1]*this->q[1] - this->q[2]*this->q[2] - this->q[3]*this->q[3]);
+ // float pitch = asinf(-2*(this->q[1]*this->q[3] - this->q[0]*this->q[2]));
+ // float roll = atan2f(2*(this->q[2]*this->q[3]+this->q[0]*this->q[1]) , this->q[0]*this->q[0] - this->q[1]*this->q[1] - this->q[2]*this->q[2] + this->q[3]*this->q[3]);
+
+ // // Save attitude, adjusted for the legacy CF2 body coordinate system
+ // state->attitude = (attitude_t){
+ // .timestamp = tick,
+ // .roll = roll*RAD_TO_DEG,
+ // .pitch = -pitch*RAD_TO_DEG,
+ // .yaw = yaw*RAD_TO_DEG
+ // };
// Save quaternion, hopefully one day this could be used in a better controller.
// Note that this is not adjusted for the legacy coordinate system
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance_robust.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance_robust.c
index e0662bd38ae0ea387bdac871a5036dad85de0825..d0d12024bd780a9bba80d2a81eff8a9416edbb07 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance_robust.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/kalman_core/mm_distance_robust.c
@@ -25,21 +25,22 @@
// Reference: https://www.geeksforgeeks.org/cholesky-decomposition-matrix-decomposition/
static void Cholesky_Decomposition(int n, float matrix[n][n], float lower[n][n]){
// Decomposing a matrix into Lower Triangular
- for (int i = 0; i < n; i++) {
- for (int j = 0; j <= i; j++) {
- float sum = 0.0;
- if (j == i) // summation for diagnols
- {
- for (int k = 0; k < j; k++)
- sum += powf(lower[j][k], 2);
- lower[j][j] = sqrtf(matrix[j][j] - sum);
- } else {
- for (int k = 0; k < j; k++)
- sum += (lower[i][k] * lower[j][k]);
- lower[i][j] = (matrix[i][j] - sum) / lower[j][j];
- }
- }
- }
+ //COMMENTED FIRMWARE
+ // for (int i = 0; i < n; i++) {
+ // for (int j = 0; j <= i; j++) {
+ // float sum = 0.0;
+ // if (j == i) // summation for diagnols
+ // {
+ // for (int k = 0; k < j; k++)
+ // sum += powf(lower[j][k], 2);
+ // lower[j][j] = sqrtf(matrix[j][j] - sum);
+ // } else {
+ // for (int k = 0; k < j; k++)
+ // sum += (lower[i][k] * lower[j][k]);
+ // lower[i][j] = (matrix[i][j] - sum) / lower[j][j];
+ // }
+ // }
+ // }
}
/* Weight function for GM Robust cost function
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_core.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_core.c
index 2c362c6ffccc1daa153cea878b0e8ba514ce56d4..e6021077255bcdbdbb944bb8cc881b2f25e1e380 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_core.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/lighthouse/lighthouse_core.c
@@ -443,60 +443,61 @@ static void updateSystemStatus(const uint32_t now_ms) {
}
void lighthouseCoreTask(void *param) {
- bool isUartFrameValid = false;
+ //COMMENTED FIRMWARE
+ // bool isUartFrameValid = false;
- uart1Init(230400);
- systemWaitStart();
+ // uart1Init(230400);
+ // systemWaitStart();
- lighthouseStorageVerifySetStorageVersion();
- lighthouseStorageInitializeGeoDataFromStorage();
- lighthouseStorageInitializeCalibDataFromStorage();
+ // lighthouseStorageVerifySetStorageVersion();
+ // lighthouseStorageInitializeGeoDataFromStorage();
+ // lighthouseStorageInitializeCalibDataFromStorage();
- if (lighthouseDeckFlasherCheckVersionAndBoot() == false) {
- DEBUG_PRINT("FPGA not booted. Lighthouse disabled!\n");
- while(1) {
- vTaskDelay(portMAX_DELAY);
- }
- }
- deckIsFlashed = true;
+ // if (lighthouseDeckFlasherCheckVersionAndBoot() == false) {
+ // DEBUG_PRINT("FPGA not booted. Lighthouse disabled!\n");
+ // while(1) {
+ // vTaskDelay(portMAX_DELAY);
+ // }
+ // }
+ // deckIsFlashed = true;
- vTaskDelay(M2T(100));
+ // vTaskDelay(M2T(100));
- memset(&bsIdentificationData, 0, sizeof(bsIdentificationData));
+ // memset(&bsIdentificationData, 0, sizeof(bsIdentificationData));
- while(1) {
- memset(pulseWidth, 0, sizeof(pulseWidth[0]) * PULSE_PROCESSOR_N_SENSORS);
- waitForUartSynchFrame();
- uartSynchronized = true;
+ // while(1) {
+ // memset(pulseWidth, 0, sizeof(pulseWidth[0]) * PULSE_PROCESSOR_N_SENSORS);
+ // waitForUartSynchFrame();
+ // uartSynchronized = true;
- bool previousWasSyncFrame = false;
+ // bool previousWasSyncFrame = false;
- while((isUartFrameValid = getUartFrameRaw(&frame))) {
- const uint32_t now_ms = T2M(xTaskGetTickCount());
+ // while((isUartFrameValid = getUartFrameRaw(&frame))) {
+ // const uint32_t now_ms = T2M(xTaskGetTickCount());
- // If a sync frame is getting through, we are only receiving sync frames. So nothing else. Reset state
- if(frame.isSyncFrame && previousWasSyncFrame) {
- pulseProcessorAllClear(&angles);
- }
- // Now we are receiving items
- else if(!frame.isSyncFrame) {
- STATS_CNT_RATE_EVENT(&frameRate);
-
- deckHealthCheck(&lighthouseCoreState, &frame, now_ms);
- lighthouseUpdateSystemType();
- if (pulseProcessorProcessPulse) {
- processFrame(&lighthouseCoreState, &angles, &frame);
- }
- }
+ // // If a sync frame is getting through, we are only receiving sync frames. So nothing else. Reset state
+ // if(frame.isSyncFrame && previousWasSyncFrame) {
+ // pulseProcessorAllClear(&angles);
+ // }
+ // // Now we are receiving items
+ // else if(!frame.isSyncFrame) {
+ // STATS_CNT_RATE_EVENT(&frameRate);
- previousWasSyncFrame = frame.isSyncFrame;
+ // deckHealthCheck(&lighthouseCoreState, &frame, now_ms);
+ // lighthouseUpdateSystemType();
+ // if (pulseProcessorProcessPulse) {
+ // processFrame(&lighthouseCoreState, &angles, &frame);
+ // }
+ // }
- updateSystemStatus(now_ms);
- }
+ // previousWasSyncFrame = frame.isSyncFrame;
- uartSynchronized = false;
- }
+ // updateSystemStatus(now_ms);
+ // }
+
+ // uartSynchronized = false;
+ // }
}
void lighthouseCoreSetCalibrationData(const uint8_t baseStation, const lighthouseCalibration_t* calibration) {
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 f56da9236c5e31ee1f4fbde38003f51e141dea9a..96b44bbb26bac96402b71c73155b8faead3cd0f8 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
@@ -977,17 +977,18 @@ logVarId_t logGetVarId(char* group, char* name)
logVarId_t varId = invalidVarId;
char * currgroup = "";
- for(i=0; i<logsLen; i++)
- {
- if (logs[i].type & LOG_GROUP) {
- if (logs[i].type & LOG_START) {
- currgroup = logs[i].name;
- }
- } else if ((!strcmp(group, currgroup)) && (!strcmp(name, logs[i].name))) {
- varId = (logVarId_t)i;
- return varId;
- }
- }
+ //COMMENTED FIRMWARE
+ // for(i=0; i<logsLen; i++)
+ // {
+ // if (logs[i].type & LOG_GROUP) {
+ // if (logs[i].type & LOG_START) {
+ // currgroup = logs[i].name;
+ // }
+ // } else if ((!strcmp(group, currgroup)) && (!strcmp(name, logs[i].name))) {
+ // varId = (logVarId_t)i;
+ // return varId;
+ // }
+ // }
return invalidVarId;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/outlierFilter.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/outlierFilter.c
index a6fab9f948f36559fec1de4aa5754eb54da5ecf7..c3d453ea993d861afc4c8fc026f7f66367dbd686 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/outlierFilter.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/outlierFilter.c
@@ -71,33 +71,34 @@ bool outlierFilterValidateTdoaSimple(const tdoaMeasurement_t* tdoa) {
bool outlierFilterValidateTdoaSteps(const tdoaMeasurement_t* tdoa, const float error, const vector_t* jacobian, const point_t* estPos) {
bool sampleIsGood = false;
- if (isDistanceDiffSmallerThanDistanceBetweenAnchors(tdoa)) {
- float errorBaseDistance = sqrtf(powf(jacobian->x, 2) + powf(jacobian->y, 2) + powf(jacobian->z, 2));
- errorDistance = fabsf(error / errorBaseDistance);
-
- int filterIndex = updateBuckets(errorDistance);
-
- if (filterIndex > previousFilterIndex) {
- filterCloseDelayCounter = FILTER_CLOSE_DELAY_COUNT;
- } else if (filterIndex < previousFilterIndex) {
- if (filterCloseDelayCounter > 0) {
- filterCloseDelayCounter--;
- filterIndex = previousFilterIndex;
- }
- }
- previousFilterIndex = filterIndex;
-
- if (filterIndex == FILTER_NONE) {
- // Lost tracking, open up to let the kalman filter converge
- acceptanceLevel = 100.0;
- sampleIsGood = true;
- } else {
- acceptanceLevel = filterLevels[filterIndex].acceptanceLevel;
- if (errorDistance < acceptanceLevel) {
- sampleIsGood = true;
- }
- }
- }
+ //COMMENTED FIRMWARE
+ // if (isDistanceDiffSmallerThanDistanceBetweenAnchors(tdoa)) {
+ // float errorBaseDistance = sqrtf(powf(jacobian->x, 2) + powf(jacobian->y, 2) + powf(jacobian->z, 2));
+ // errorDistance = fabsf(error / errorBaseDistance);
+
+ // int filterIndex = updateBuckets(errorDistance);
+
+ // if (filterIndex > previousFilterIndex) {
+ // filterCloseDelayCounter = FILTER_CLOSE_DELAY_COUNT;
+ // } else if (filterIndex < previousFilterIndex) {
+ // if (filterCloseDelayCounter > 0) {
+ // filterCloseDelayCounter--;
+ // filterIndex = previousFilterIndex;
+ // }
+ // }
+ // previousFilterIndex = filterIndex;
+
+ // if (filterIndex == FILTER_NONE) {
+ // // Lost tracking, open up to let the kalman filter converge
+ // acceptanceLevel = 100.0;
+ // sampleIsGood = true;
+ // } else {
+ // acceptanceLevel = filterLevels[filterIndex].acceptanceLevel;
+ // if (errorDistance < acceptanceLevel) {
+ // sampleIsGood = true;
+ // }
+ // }
+ // }
return sampleIsGood;
}
@@ -119,28 +120,30 @@ void outlierFilterReset(OutlierFilterLhState_t* this, const uint32_t now) {
bool outlierFilterValidateLighthouseSweep(OutlierFilterLhState_t* this, const float distanceToBs, const float angleError, const uint32_t now) {
// float error = distanceToBs * tan(angleError);
// We use an approximattion
- float error = distanceToBs * angleError;
-
- bool isGoodSample = (fabsf(error) < lhMaxError);
- if (isGoodSample) {
- this->openingWindow += lhGoodSampleWindowChange;
- if (this->openingWindow > lhMaxWindowTime) {
- this->openingWindow = lhMaxWindowTime;
- }
- } else {
- this->openingWindow += lhBadSampleWindowChange;
- if (this->openingWindow < lhMinWindowTime) {
- this->openingWindow = lhMinWindowTime;
- }
- }
+ //COMMENTED FIRMWARE
+ // float error = distanceToBs * angleError;
+
+ // bool isGoodSample = (fabsf(error) < lhMaxError);
+ // if (isGoodSample) {
+ // this->openingWindow += lhGoodSampleWindowChange;
+ // if (this->openingWindow > lhMaxWindowTime) {
+ // this->openingWindow = lhMaxWindowTime;
+ // }
+ // } else {
+ // this->openingWindow += lhBadSampleWindowChange;
+ // if (this->openingWindow < lhMinWindowTime) {
+ // this->openingWindow = lhMinWindowTime;
+ // }
+ // }
bool result = true;
- bool isFilterClosed = (now < this->openingTime);
- if (isFilterClosed) {
- result = isGoodSample;
- }
+ //COMMENTED FIRMWARE
+ // bool isFilterClosed = (now < this->openingTime);
+ // if (isFilterClosed) {
+ // result = isGoodSample;
+ // }
- this->openingTime = now + this->openingWindow;
+ // this->openingTime = now + this->openingWindow;
return result;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/param.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/param.c
index 35f375fb310bdf3c91fb571fac67ce864445e2c0..49189bcb4e2c0a1392080fd5a215d1bfeb9773e8 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/param.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/param.c
@@ -413,8 +413,9 @@ static char paramWriteByNameProcess(char* group, char* name, int type, void *val
}
else //Ptr points a variable
{
- if (!strcmp(params[ptr].name, name) && !strcmp(pgroup, group))
- break;
+ //COMMENTED FIRMWARE
+ // if (!strcmp(params[ptr].name, name) && !strcmp(pgroup, group))
+ // break;
}
}
@@ -564,12 +565,12 @@ paramVarId_t paramGetVarId(char* group, char* name)
} else {
id += 1;
}
-
- if ((!strcmp(group, currgroup)) && (!strcmp(name, params[ptr].name))) {
- varId.ptr = ptr;
- varId.id = id - 1;
- return varId;
- }
+ //COMMENTED FIRMWARE
+ // if ((!strcmp(group, currgroup)) && (!strcmp(name, params[ptr].name))) {
+ // varId.ptr = ptr;
+ // varId.id = id - 1;
+ // return varId;
+ // }
}
return invalidVarId;
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 20fdd50cdddfeaca916d33a647c8876f30d9791a..989c224c7d933dab8f027633889818aea00a202c 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
@@ -26,7 +26,8 @@
#include "position_controller_indi.h"
-#include "math3d.h"
+//#include "math3d.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 5cdff44724a70b19835286166e006b808308a12f..2fa55dd5c7db915c29b0d6e7b13999876e6fd60e 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
@@ -24,7 +24,8 @@
* position_estimator_pid.c: PID-based implementation of the position controller
*/
-#include <math.h>
+//#include <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/position_estimator_altitude.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_estimator_altitude.c
index 461aa7681de314d60154d3e020d42efcc5d52d0f..79d716199dd95f09fa781b393084e8162c2b65ad 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_estimator_altitude.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/position_estimator_altitude.c
@@ -74,7 +74,8 @@ static void positionEstimateInternal(state_t* estimate, const baro_t* baro, cons
static float prev_estimatedZ = 0;
static bool surfaceFollowingMode = false;
- const uint32_t MAX_SAMPLE_AGE = M2T(50);
+ //COMMENTED FIRMWARE
+ const uint32_t MAX_SAMPLE_AGE = 50;//M2T(50);
uint32_t now = xTaskGetTickCount();
bool isSampleUseful = ((now - tofMeasurement->timestamp) <= MAX_SAMPLE_AGE);
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/pptraj.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/pptraj.c
index d24dc2f93c013623e88cb64fc73cd90dd89e94f5..26cd702e6b0026055b5f47ce5f526594b9f59ed5 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/pptraj.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/pptraj.c
@@ -256,12 +256,13 @@ float poly4d_max_accel_approx(struct poly4d const *p)
float step = p->duration / (steps - 1);
float t = 0;
float amax = 0;
- for (int i = 0; i < steps; ++i) {
- struct vec ddx = polyval_xyz(acc, t);
- float ddx_minkowski = vnorm1(ddx);
- if (ddx_minkowski > amax) amax = ddx_minkowski;
- t += step;
- }
+ //COMMENTED FIRMWARE
+ // for (int i = 0; i < steps; ++i) {
+ // struct vec ddx = polyval_xyz(acc, t);
+ // float ddx_minkowski = vnorm1(ddx);
+ // if (ddx_minkowski > amax) amax = ddx_minkowski;
+ // t += step;
+ // }
return amax;
}
@@ -293,38 +294,39 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t)
{
// flat variables
struct traj_eval out;
- out.pos = polyval_xyz(p, t);
- out.yaw = polyval_yaw(p, t);
-
- // 1st derivative
- struct poly4d* deriv = &poly4d_tmp;
- *deriv = *p;
- polyder4d(deriv);
- out.vel = polyval_xyz(deriv, t);
- float dyaw = polyval_yaw(deriv, t);
-
- // 2nd derivative
- polyder4d(deriv);
- out.acc = polyval_xyz(deriv, t);
-
- // 3rd derivative
- polyder4d(deriv);
- struct vec jerk = polyval_xyz(deriv, t);
-
- struct vec thrust = vadd(out.acc, mkvec(0, 0, GRAV));
- // float thrust_mag = mass * vmag(thrust);
-
- struct vec z_body = vnormalize(thrust);
- struct vec x_world = mkvec(cosf(out.yaw), sinf(out.yaw), 0);
- struct vec y_body = vnormalize(vcross(z_body, x_world));
- struct vec x_body = vcross(y_body, z_body);
-
- struct vec jerk_orth_zbody = vorthunit(jerk, z_body);
- struct vec h_w = vscl(1.0f / vmag(thrust), jerk_orth_zbody);
-
- out.omega.x = -vdot(h_w, y_body);
- out.omega.y = vdot(h_w, x_body);
- out.omega.z = z_body.z * dyaw;
+ //COMMENTED FIRMWARE
+ // out.pos = polyval_xyz(p, t);
+ // out.yaw = polyval_yaw(p, t);
+
+ // // 1st derivative
+ // struct poly4d* deriv = &poly4d_tmp;
+ // *deriv = *p;
+ // polyder4d(deriv);
+ // out.vel = polyval_xyz(deriv, t);
+ // float dyaw = polyval_yaw(deriv, t);
+
+ // // 2nd derivative
+ // polyder4d(deriv);
+ // out.acc = polyval_xyz(deriv, t);
+
+ // // 3rd derivative
+ // polyder4d(deriv);
+ // struct vec jerk = polyval_xyz(deriv, t);
+
+ // struct vec thrust = vadd(out.acc, mkvec(0, 0, GRAV));
+ // // float thrust_mag = mass * vmag(thrust);
+
+ // struct vec z_body = vnormalize(thrust);
+ // struct vec x_world = mkvec(cosf(out.yaw), sinf(out.yaw), 0);
+ // struct vec y_body = vnormalize(vcross(z_body, x_world));
+ // struct vec x_body = vcross(y_body, z_body);
+
+ // struct vec jerk_orth_zbody = vorthunit(jerk, z_body);
+ // struct vec h_w = vscl(1.0f / vmag(thrust), jerk_orth_zbody);
+
+ // out.omega.x = -vdot(h_w, y_body);
+ // out.omega.y = vdot(h_w, x_body);
+ // out.omega.z = z_body.z * dyaw;
return out;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/pptraj_compressed.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/pptraj_compressed.c
index 899d8d5e41f87d3ef011124635c9717a7c7139d8..20510fdcaf0757c6209fae8ef32f4cbb2d67d323 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/pptraj_compressed.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/pptraj_compressed.c
@@ -242,48 +242,50 @@ void piecewise_compressed_load(struct piecewise_traj_compressed *traj, const voi
static void piecewise_compressed_rewind(struct piecewise_traj_compressed *traj)
{
- struct traj_eval stopped;
- compressed_piece_coordinate value;
- compressed_piece_ptr ptr;
-
- /* Parse header that stores the start coordinates */
- bzero(&stopped, sizeof(stopped));
- ptr = traj->data;
- ptr = next_coordinate(ptr, &value); stopped.pos.x = value / STORED_DISTANCE_SCALE;
- ptr = next_coordinate(ptr, &value); stopped.pos.y = value / STORED_DISTANCE_SCALE;
- ptr = next_coordinate(ptr, &value); stopped.pos.z = value / STORED_DISTANCE_SCALE;
- ptr = next_coordinate(ptr, &value); stopped.yaw = value / STORED_ANGLE_SCALE;
- traj->current_piece.t_begin_relative = 0;
- traj->current_piece.data = ptr;
-
- piecewise_compressed_update_current_poly4d(traj, &stopped);
+ //COMMENTED FIRMWARE
+ // struct traj_eval stopped;
+ // compressed_piece_coordinate value;
+ // compressed_piece_ptr ptr;
+
+ // /* Parse header that stores the start coordinates */
+ // bzero(&stopped, sizeof(stopped));
+ // ptr = traj->data;
+ // ptr = next_coordinate(ptr, &value); stopped.pos.x = value / STORED_DISTANCE_SCALE;
+ // ptr = next_coordinate(ptr, &value); stopped.pos.y = value / STORED_DISTANCE_SCALE;
+ // ptr = next_coordinate(ptr, &value); stopped.pos.z = value / STORED_DISTANCE_SCALE;
+ // ptr = next_coordinate(ptr, &value); stopped.yaw = value / STORED_ANGLE_SCALE;
+ // traj->current_piece.t_begin_relative = 0;
+ // traj->current_piece.data = ptr;
+
+ // piecewise_compressed_update_current_poly4d(traj, &stopped);
}
static void piecewise_compressed_update_current_poly4d(
struct piecewise_traj_compressed *traj, const struct traj_eval *prev_end)
{
- struct poly4d* poly4d = &traj->current_piece.poly4d;
- compressed_piece_ptr ptr;
- struct compressed_piece_parsed_header header;
-
- /* First, clear everything in the poly4d */
- bzero(poly4d, sizeof(*poly4d));
-
- /* Parse the header of the current piece, extract the storage types and the duration */
- ptr = traj->current_piece.data;
- parse_header_of_current_piece(&header, ptr);
- poly4d->duration = header.duration_in_msec / STORED_DURATION_SCALE;
-
- /* Process the body */
- ptr = header.body;
- ptr = calculate_polynomial_coefficients(
- poly4d->p[0], ptr, header.x_type, prev_end->pos.x, poly4d->duration, STORED_DISTANCE_SCALE);
- ptr = calculate_polynomial_coefficients(
- poly4d->p[1], ptr, header.y_type, prev_end->pos.y, poly4d->duration, STORED_DISTANCE_SCALE);
- ptr = calculate_polynomial_coefficients(
- poly4d->p[2], ptr, header.z_type, prev_end->pos.z, poly4d->duration, STORED_DISTANCE_SCALE);
- calculate_polynomial_coefficients(
- poly4d->p[3], ptr, header.yaw_type, prev_end->yaw, poly4d->duration, STORED_ANGLE_SCALE);
+ //COMMENTED FIRMWARE
+ // struct poly4d* poly4d = &traj->current_piece.poly4d;
+ // compressed_piece_ptr ptr;
+ // struct compressed_piece_parsed_header header;
+
+ // /* First, clear everything in the poly4d */
+ // bzero(poly4d, sizeof(*poly4d));
+
+ // /* Parse the header of the current piece, extract the storage types and the duration */
+ // ptr = traj->current_piece.data;
+ // parse_header_of_current_piece(&header, ptr);
+ // poly4d->duration = header.duration_in_msec / STORED_DURATION_SCALE;
+
+ // /* Process the body */
+ // ptr = header.body;
+ // ptr = calculate_polynomial_coefficients(
+ // poly4d->p[0], ptr, header.x_type, prev_end->pos.x, poly4d->duration, STORED_DISTANCE_SCALE);
+ // ptr = calculate_polynomial_coefficients(
+ // poly4d->p[1], ptr, header.y_type, prev_end->pos.y, poly4d->duration, STORED_DISTANCE_SCALE);
+ // ptr = calculate_polynomial_coefficients(
+ // poly4d->p[2], ptr, header.z_type, prev_end->pos.z, poly4d->duration, STORED_DISTANCE_SCALE);
+ // calculate_polynomial_coefficients(
+ // poly4d->p[3], ptr, header.yaw_type, prev_end->yaw, poly4d->duration, STORED_ANGLE_SCALE);
}
static void piecewise_compressed_advance_playhead(struct piecewise_traj_compressed *traj)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/sensfusion6.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/sensfusion6.c
index 633e4c9d1285f9f012601a74522683c20ee16acb..8bfc796bdf440ba834141ca33e152a195514dd7e 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/sensfusion6.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/sensfusion6.c
@@ -262,16 +262,17 @@ void sensfusion6GetQuaternion(float* q_x, float* q_y, float* q_z, float* q_w)
void sensfusion6GetEulerRPY(float* roll, float* pitch, float* yaw)
{
- float gx = gravX;
- float gy = gravY;
- float gz = gravZ;
+ // float gx = gravX;
+ // float gy = gravY;
+ // float gz = gravZ;
- if (gx>1) gx=1;
- if (gx<-1) gx=-1;
+ // if (gx>1) gx=1;
+ // if (gx<-1) gx=-1;
- *yaw = atan2f(2*(qw*qz + qx*qy), qw*qw + qx*qx - qy*qy - qz*qz) * 180 / M_PI_F;
- *pitch = asinf(gx) * 180 / M_PI_F; //Pitch seems to be inverted
- *roll = atan2f(gy, gz) * 180 / M_PI_F;
+ // *yaw = atan2f(2*(qw*qz + qx*qy), qw*qw + qx*qx - qy*qy - qz*qz) * 180 / M_PI_F;
+ // *pitch = asinf(gx) * 180 / M_PI_F; //Pitch seems to be inverted
+ // *roll = atan2f(gy, gz) * 180 / M_PI_F;
+ //COMMENTED FIRMWARE
}
float sensfusion6GetAccZWithoutGravity(const float ax, const float ay, const float az)
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/sound_cf2.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/sound_cf2.c
index 5fb238491aff4a9c8426c6f2550f0546f3341d92..597588eaf5444ab83f439c4e26a27198728fc040 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/sound_cf2.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/modules/src/sound_cf2.c
@@ -276,19 +276,20 @@ static int tilt_freq;
static int tilt_ratio;
static void tilt(uint32_t counter, uint32_t * mi, Melody * melody)
{
- pitchid = logGetVarId("stabilizer", "pitch");
- rollid = logGetVarId("stabilizer", "roll");
+ //COMMENTED FIRMWARE
+ // pitchid = logGetVarId("stabilizer", "pitch");
+ // rollid = logGetVarId("stabilizer", "roll");
- pitch = logGetInt(pitchid);
- roll = logGetInt(rollid);
- tilt_freq = 0;
- tilt_ratio = 127;
+ // pitch = logGetInt(pitchid);
+ // roll = logGetInt(rollid);
+ // tilt_freq = 0;
+ // tilt_ratio = 127;
- if (abs(pitch) > 5) {
- tilt_freq = 3000 - 50 * pitch;
- }
+ // if (abs(pitch) > 5) {
+ // tilt_freq = 3000 - 50 * pitch;
+ // }
- buzzerOn(tilt_freq);
+ // buzzerOn(tilt_freq);
}
typedef struct {
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 158edb2cbc98e61fe411bfb25d74d78792cddc82..d3ed129de910400a400885b3dbaddcd1fa84c870 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
@@ -175,7 +175,8 @@ void stabilizerInit(StateEstimatorType estimator)
estimatorType = getStateEstimator();
controllerType = getControllerType();
- STATIC_MEM_TASK_CREATE(stabilizerTask, stabilizerTask, STABILIZER_TASK_NAME, NULL, STABILIZER_TASK_PRI);
+ //COMMENTED FIRMWARE
+ //STATIC_MEM_TASK_CREATE(stabilizerTask, stabilizerTask, STABILIZER_TASK_NAME, NULL, STABILIZER_TASK_PRI);
isInit = true;
}
@@ -225,7 +226,8 @@ static void stabilizerTask(void* param)
// Wait for sensors to be calibrated
lastWakeTime = xTaskGetTickCount();
while(!sensorsAreCalibrated()) {
- vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
+ //COMMENTED FIRMWARE
+ //vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
}
// Initialize tick to something else then 0
tick = 1;
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 6531dc825630484f3bd430e87f73f68ca7af5c66..e273155b096c6c2659b4e0aafe8e17d34ddf2f47 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
@@ -104,8 +104,9 @@ void systemInit(void)
if(isInit)
return;
- canStartMutex = xSemaphoreCreateMutexStatic(&canStartMutexBuffer);
- xSemaphoreTake(canStartMutex, portMAX_DELAY);
+ //COMMENTED FIRMWARE
+ // canStartMutex = xSemaphoreCreateMutexStatic(&canStartMutexBuffer);
+ // xSemaphoreTake(canStartMutex, portMAX_DELAY);
usblinkInit();
sysLoadInit();
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 92cbb4ff523009f75a580ac2d2e3363838cb677f..c9a80896027901fc24b6b4672bb5d20c692f1152 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
@@ -107,7 +107,7 @@ OBJS += build/platform.o
OBJS += build/platform_utils.o build/platform_$(PLATFORM).o #build/platform_$(CPU).o
# Drivers
-PRO += build/exti.o build/nvic.o build/motors.o
+OBJS += build/exti.o build/nvic.o build/motors.o
OBJS += build/led.o build/mpu6500.o build/i2c_drv.o build/i2cdev.o build/ws2812_cf2.o build/lps25h.o
OBJS += build/ak8963.o build/eeprom.o build/maxsonar.o build/piezo.o
OBJS += build/uart_syslink.o build/swd.o build/uart1.o build/uart2.o build/watchdog.o
@@ -248,6 +248,8 @@ endif
# Libs
#OBJS += libarm_math.a
+OBJS += build/ff.o build/ffsystem.o build//ffunicode.o
+OBJS += build/fatfs_sd.o build/pm_stm32f4.o
BUILDDIR =./build
@@ -344,6 +346,9 @@ build/%.o : ../lib/vl53l1/core/src/%.c
build/%.o : ../utils/src/%.c
$(CROSS)gcc $(CFLAGS) -c -o $@ $<
+build/%.o : ../lib/FatFS/%.c
+ $(CROSS)gcc $(CFLAGS) -c -o $@ $<
+
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/activeMarkerDeck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/activeMarkerDeck.o
index d4ace454eaec7eb40fc3a525cacf43c0640a66db..1802f3b085328cd93a776a8c0387eea5015086f0 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/activeMarkerDeck.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/activeMarkerDeck.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/aideck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/aideck.o
index ff893923351f447170dde49533864db9a1273c0a..16aec57e1a8e76fe1f4c17c7729328873ab21abd 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/aideck.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/aideck.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/collision_avoidance.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/collision_avoidance.o
index a566ddd75eb3b4a7f0d77ff9ac4c02a64c3d386e..3a299ee8411e64bc8bec07efde6d88af79097880 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/controller_indi.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_indi.o
index 45007e75af7d080d7eea64703c972389d7abaf65..904ef29c1e2075314f86453dafbd4ab999a1adbf 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_indi.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_indi.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_mellinger.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_mellinger.o
index 9b587cebd896dbef3aa99f2487fe2d2c0b535ff6..7cddb54cef65b19595c6fab9d87ddaab90c01b1e 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_mellinger.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/controller_mellinger.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 47a7737d61d83d401a4be63be346a2174f7f9030..0c0ecc4c24696122926c45b8721e02676bb29c22 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 2cbfddea5dab0ff82be38877acf1eabbb183f0d0..ccde21281f36d5cd462b7480e0f8ebdfca8fdcb1 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 ad22cf9e6e73c1d4e86cda8d690d983190a6204a..a03ebd7e6f00533fc3ca159d6f1349aea03052b7 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 9da089ea35df32248f3823fc3f38b06cba29927b..ff3d3d66f7facbba26f69c7a37d1d2f45fa8a889 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/deck_analog.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_analog.o
index 6d50f893dbeb54baf37ce7cdd3f6dfd36210bc21..91cc548217ed6fadf42e9c7d258886d14a4e47a7 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_analog.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_analog.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_drivers.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_drivers.o
index ae25d4fe7cfbf40430df08c0f3293189d21c012e..30c0c881b5b4540bfe4ce3566fc9b446bba9e401 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_memory.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_memory.o
index 0afaa076b2ddd3bd7c09c5f11478090a4a40899b..7cfb235b728e034da496ff631cf8fa7f07740d15 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/deck_spi3.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_spi3.o
index 6cef5099a5e88d6f18fad4cb8483cbfa6cc418e0..f285bfd6d8f32f42e0f7303547139a3f3de51337 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_spi3.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/deck_spi3.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eprintf.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eprintf.o
index 24a9c03ad96d39fcce2611de37ecdb5e63db38af..ec0193939c4e549c940a0154190ee4856dc6f8e4 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eprintf.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eprintf.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator.o
index bb2534a8bf8ba11fea5662c1c8fad00e890d6dcc..c5fc42bcb53129b940d5193fd1308dddeacd92ec 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator_kalman.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator_kalman.o
index 918557966a7be006a2dc372b37d901723bc6d3c9..d789768199fa22aca14ce4b1ed35eff56d7ddb1f 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator_kalman.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/estimator_kalman.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eventtrigger.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eventtrigger.o
index e4144b488bf46d96fbcc228e56ca1c1df6e48c70..1f4e3cffa29c26a6d827f39dad58a60cfba12bbd 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eventtrigger.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/eventtrigger.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exti.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exti.o
new file mode 100644
index 0000000000000000000000000000000000000000..3516a2e18f3561e995313540a1c816fdbb97360e
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/exti.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/fatfs_sd.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/fatfs_sd.o
new file mode 100644
index 0000000000000000000000000000000000000000..94a84779ec66e7628af426b2383a01447d1b6ec3
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/fatfs_sd.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ff.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ff.o
new file mode 100644
index 0000000000000000000000000000000000000000..10a0c4f1d4604ba816e0db6a6857c86071fc2c10
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ff.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ffsystem.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ffsystem.o
new file mode 100644
index 0000000000000000000000000000000000000000..5d56696b4edb40e6680db1076b1bda4b8caf52f9
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ffsystem.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ffunicode.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ffunicode.o
new file mode 100644
index 0000000000000000000000000000000000000000..380e51bbc37405693a0b4adbd6f8f37b92754766
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ffunicode.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/flowdeck_v1v2.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/flowdeck_v1v2.o
index 6012d21e811d8d93803f5d6f8eefe107f06e9adc..7338442313cba619b4d3eb392a2cc97f7e628eac 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/flowdeck_v1v2.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/flowdeck_v1v2.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/gtgps.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/gtgps.o
index f80236e5061f3295c45971c003b503f665d8e5e0..6d444a023c27a6df47613a2a61bb6b51be4e1208 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/gtgps.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/gtgps.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/health.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/health.o
index 05f25bbb5fdb1c4ce84bec5c2e30ac9c2746e803..c372474f5c42841844726142477ecc464d68dbd0 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/health.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/health.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kalman_core.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kalman_core.o
index 563d5e817e9c5422eb2458c1eb223714987d37dd..cb23373c42718a47fa3b5369a9c44fb790d9b76a 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kalman_core.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/kalman_core.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledring12.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ledring12.o
index 3c84e8e04e00618f008fb59349a149907c8f0fc7..ae25ecf6cb0eab30aeebf41ca9074adf9523c367 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.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse.o
index 0ae8aed52372ff57b114402443984a3391433275..0723a497f8c8465a4ac89fdd0eb883c4a0cddfc2 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_core.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_core.o
index ecff545b6401c3d5168e88901a7791be8b908c14..7a2dd26b484123793b371a898aaff4943225a9e6 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_core.o 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_geometry.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_geometry.o
index e038d52e549332cbfc5452dc9414c74889d931fb..1d81ec6421f72d261ce63da238603e64dfc57caa 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lighthouse_geometry.o 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/log.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/log.o
index d388b0a20bc1d902647e1c836fed71f449d25265..439845cf17de60698da633eac99bc5a7f3a505fd 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/lps25h.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lps25h.o
index 84fa40873ca2032d205250dd5704f8b79e77b0ca..bb5120c15be34dcbaa6241b85ce03732f9420391 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lps25h.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lps25h.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa2Tag.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa2Tag.o
index e230a4e697b3232b837bce6a0be3b47be4c51156..d43febb0b07ed7d6c278da6325d0c49bef418882 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa2Tag.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa2Tag.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa3Tag.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa3Tag.o
index a4f95e6fb180fa842b3c89e970ba2dd9519a60f7..c53d4cfeacc3c4957c80d684098aea29271376bc 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa3Tag.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTdoa3Tag.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTwrTag.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTwrTag.o
index 2616de8729482728f3332653b089c5819c76cd78..993ede871d5fae48341a6473b2a430189b4fe8e3 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTwrTag.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/lpsTwrTag.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/motors.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/motors.o
new file mode 100644
index 0000000000000000000000000000000000000000..d29acf81fc1b6dbcf5043b849c920ef2c38a5667
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/motors.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/multiranger.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/multiranger.o
index 53d37adf73b7b073c1279f93d845b3a1eb655fb2..bf1b6da25775c6af1817eff673138b1fdced94e1 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/multiranger.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/multiranger.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/num.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/num.o
index 4d9fd5f2d3727fd6f0485968050b44b455ae3d73..413d95c8446bf5eba5281680099e9e1bd956a610 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/num.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/num.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/nvic.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/nvic.o
new file mode 100644
index 0000000000000000000000000000000000000000..4b12e11efccfe6a33d29b2460e6c9234bbbea697
Binary files /dev/null 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/oa.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/oa.o
index 56905475c647a3ab8d1f2f64a4f28ae20a0d3c6b..01a6eaf2beefcf71cb6fa7049f7915a76e786669 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/oa.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/oa.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/outlierFilter.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/outlierFilter.o
index 4027af9286a3fb491bb98c413a71727e14af63cd..58a074805d869daf741fabd5941f9158c42f13b1 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/outlierFilter.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/outlierFilter.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ow_syslink.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/ow_syslink.o
index 30460c75456711b5dce4abc6e960daac2c45e3e2..73b818d0bdaca516ceaa06e5ad0130ad64c72d49 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/param.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/param.o
index 30cf4e65bb813b56e70a9cfd252cf9a569b13ed3..650a7c70d74bf3fc7b6da6ce0798dd53dfd01b25 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/param.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/param.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pm_stm32f4.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pm_stm32f4.o
new file mode 100644
index 0000000000000000000000000000000000000000..3251a86b6f6bb5c7290298cf6dcf6eb7b3092d49
Binary files /dev/null and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pm_stm32f4.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 de200ab3c098cfa81390ffeac62320e753cb653e..6e2ce2aa138d811106f568b9fe84bca319bd25ed 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 123e9a3fedd8f76f3176e589a24aa02fe0c26775..537a71ec57cf7a626a10a95faf651f54b2c29fb8 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/position_estimator_altitude.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_estimator_altitude.o
index 661302b06c2fc3f14784ce209afc038df264a423..fd8753b5e4a7ef17eae2ccf494af1d6357687865 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_estimator_altitude.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/position_estimator_altitude.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj.o
index fd62e0b8bc789a00ca7335ad1004c35c4a7c3d17..470285b1f02f82a526ac15900537ecef129f6b07 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj_compressed.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj_compressed.o
index 5948fab43d8341736425fe9204614cf99909fcb5..731adc68d5c515104e6b3531cb0302f4527b5b2a 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj_compressed.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pptraj_compressed.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor_v1.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor_v1.o
index 08f84dcffeea7de3463a27b785e2ed1386208004..148b8a1f8f30a89ae8443c2814c013b519b010d8 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/pulse_processor_v1.o 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
index 45338a2da47f3d15e423a4282b708fec56a7756d..53534f1b6192cbb308d7c9c4089fa87fdd3752be 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/sensfusion6.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/sensfusion6.o
index 51584dcf1f92e6ed6a2cf0e47c15ebd691c1148c..50ca6a6655fd911341f2ef9bf002f884f11c6dd2 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/sensfusion6.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/sensfusion6.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/sound_cf2.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/sound_cf2.o
index 7adbba040211ff555d568e44530e7eb26f887fe3..de37641d302ba66b4683fcc78ac5e97a6cf2e19c 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/sound_cf2.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/sound_cf2.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 b801696e1ce13bd23385ee7ab54a8f47aaa8d113..a8ffbc684256fd5fd961b599349a2c64d590f739 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 280cc8eaad2b005f8d86bbf4c36a267b6e39d8d2..407d40b2d5c67f9aaa162e963369f4be6155618f 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/usddeck.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/usddeck.o
index 6b50c3cd43edd195c03648adf2a56c0401c2ff2c..d61346ea057789eeede12a04299f619e26d2fc3f 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/uart/build/vl53l1_api.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/vl53l1_api.o
index 3f910fdffa068840202ec4bfc562458c8970d8cd..e334a4d7d5067376b3434ecd4e5a3ad99c0c7af5 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/vl53l1_api.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/vl53l1_api.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger.o
index bd24ca0092ba73c6c374841bc6ae4214b3bb302b..526da1c9f94964adea3a848358d3c9cdb5783396 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger2.o b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger2.o
index 78fad7ca3378b8c09a6d1e575e66c2ec07ea5443..f0d6f5bd68a43db2c0b148e8af634a1d5bb18866 100644
Binary files a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger2.o and b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/uart/build/zranger2.o differ
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/empty_math.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/empty_math.h
new file mode 100644
index 0000000000000000000000000000000000000000..0bc261b6ee793ec6d35935310ca9e7ae747cd09b
--- /dev/null
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/empty_math.h
@@ -0,0 +1,24 @@
+#ifndef EMPTY_MATH_H
+#define EMPTY_MATH_H
+
+float cosf(float x)
+{
+ return 0;
+}
+
+float sinf(float x)
+{
+ return 0;
+}
+
+float fmaxf(float a, float b)
+{
+ return 0;
+}
+
+float sqrtf(float x)
+{
+ return 0;
+}
+
+#endif
\ No newline at end of file
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/eprintf.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/eprintf.c
index c7ba68b0e20a5c118290f6e093e4f544f5408e5f..2a9d110b7d7bfd821451a282c77f96b412c2b4a2 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/eprintf.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/eprintf.c
@@ -28,7 +28,8 @@
#include <stdarg.h>
#include <stdint.h>
#include <stdbool.h>
-#include <ctype.h>
+//COMMENTED FIRMWARE
+//#include <ctype.h>
static const char digit[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
'A', 'B', 'C', 'D', 'E', 'F'};
@@ -154,153 +155,153 @@ static int itoa16(putc_t putcf, uint64_t num, int width, char padChar)
static int handleLongLong(putc_t putcf, const char** fmt, unsigned long long int val, int width, char padChar)
{
int len = 0;
-
- switch(*((*fmt)++))
- {
- case 'i':
- case 'd':
- len = itoa10(putcf, (long long int)val, 0);
- break;
- case 'u':
- len = itoa10Unsigned(putcf, val);
- break;
- case 'x':
- case 'X':
- len = itoa16(putcf, val, width, padChar);
- break;
- default:
- // Nothing here
- break;
- }
-
- return len;
-}
-
-static int handleLong(putc_t putcf, const char** fmt, unsigned long int val, int width, char padChar)
-{
- int len = 0;
-
- switch(*((*fmt)++))
- {
- case 'i':
- case 'd':
- len = itoa10(putcf, (long int)val, 0);
- break;
- case 'u':
- len = itoa10Unsigned(putcf, val);
- break;
- case 'x':
- case 'X':
- len = itoa16(putcf, val, width, padChar);
- break;
- default:
- // Nothing here
- break;
- }
-
- return len;
-}
-
-int evprintf(putc_t putcf, const char * fmt, va_list ap)
-{
- int len=0;
- float num;
- char* str;
- int precision;
- int width;
- char padChar;
-
- while (*fmt)
- {
- if (*fmt == '%')
- {
- precision = 6;
- padChar = ' ';
- width = 0;
-
- fmt++;
- while ('0' == *fmt)
- {
- padChar = '0';
- fmt++;
- }
-
- while(isdigit((unsigned)*fmt))
- {
- width *= 10;
- width += *fmt - '0';
- fmt++;
- }
-
- while (!isalpha((unsigned) *fmt))
- {
- if (*fmt == '.')
- {
- fmt++;
- if (isdigit((unsigned)*fmt))
- {
- precision = *fmt - '0';
- fmt++;
- }
- }
- }
- switch (*fmt++)
- {
- case 'i':
- case 'd':
- len += itoa10(putcf, va_arg(ap, int), 0);
- break;
- case 'u':
- len += itoa10Unsigned(putcf, va_arg(ap, unsigned int));
- break;
- case 'x':
- case 'X':
- len += itoa16(putcf, va_arg(ap, unsigned int), width, padChar);
- break;
- case 'l':
- // Look ahead for ll
- if (*fmt == 'l') {
- fmt++;
- len += handleLongLong(putcf, &fmt, va_arg(ap, unsigned long long int), width, padChar);
- } else {
- len += handleLong(putcf, &fmt, va_arg(ap, unsigned long int), width, padChar);
- }
-
- break;
- case 'f':
- num = va_arg(ap, double);
- if(num<0)
- {
- putcf('-');
- num = -num;
- len++;
- }
- len += itoa10(putcf, (int)num, 0);
- putcf('.'); len++;
- len += itoa10(putcf, (num - (int)num) * power(10,precision), precision);
- break;
- case 's':
- str = va_arg(ap, char* );
- while(*str)
- {
- putcf(*str++);
- len++;
- }
- break;
- case 'c':
- putcf((char)va_arg(ap, int));
- len++;
- break;
- default:
- break;
- }
- }
- else
- {
- putcf(*fmt++);
- len++;
- }
- }
+ //COMMENTED FIRMWARE
+// switch(*((*fmt)++))
+// {
+// case 'i':
+// case 'd':
+// len = itoa10(putcf, (long long int)val, 0);
+// break;
+// case 'u':
+// len = itoa10Unsigned(putcf, val);
+// break;
+// case 'x':
+// case 'X':
+// len = itoa16(putcf, val, width, padChar);
+// break;
+// default:
+// // Nothing here
+// break;
+// }
+
+// return len;
+// }
+
+// static int handleLong(putc_t putcf, const char** fmt, unsigned long int val, int width, char padChar)
+// {
+// int len = 0;
+
+// switch(*((*fmt)++))
+// {
+// case 'i':
+// case 'd':
+// len = itoa10(putcf, (long int)val, 0);
+// break;
+// case 'u':
+// len = itoa10Unsigned(putcf, val);
+// break;
+// case 'x':
+// case 'X':
+// len = itoa16(putcf, val, width, padChar);
+// break;
+// default:
+// // Nothing here
+// break;
+// }
+
+// return len;
+// }
+
+// int evprintf(putc_t putcf, const char * fmt, va_list ap)
+// {
+// int len=0;
+// float num;
+// char* str;
+// int precision;
+// int width;
+// char padChar;
+
+// while (*fmt)
+// {
+// if (*fmt == '%')
+// {
+// precision = 6;
+// padChar = ' ';
+// width = 0;
+
+// fmt++;
+// while ('0' == *fmt)
+// {
+// padChar = '0';
+// fmt++;
+// }
+
+// while(isdigit((unsigned)*fmt))
+// {
+// width *= 10;
+// width += *fmt - '0';
+// fmt++;
+// }
+
+// while (!isalpha((unsigned) *fmt))
+// {
+// if (*fmt == '.')
+// {
+// fmt++;
+// if (isdigit((unsigned)*fmt))
+// {
+// precision = *fmt - '0';
+// fmt++;
+// }
+// }
+// }
+// switch (*fmt++)
+// {
+// case 'i':
+// case 'd':
+// len += itoa10(putcf, va_arg(ap, int), 0);
+// break;
+// case 'u':
+// len += itoa10Unsigned(putcf, va_arg(ap, unsigned int));
+// break;
+// case 'x':
+// case 'X':
+// len += itoa16(putcf, va_arg(ap, unsigned int), width, padChar);
+// break;
+// case 'l':
+// // Look ahead for ll
+// if (*fmt == 'l') {
+// fmt++;
+// len += handleLongLong(putcf, &fmt, va_arg(ap, unsigned long long int), width, padChar);
+// } else {
+// len += handleLong(putcf, &fmt, va_arg(ap, unsigned long int), width, padChar);
+// }
+
+// break;
+// case 'f':
+// num = va_arg(ap, double);
+// if(num<0)
+// {
+// putcf('-');
+// num = -num;
+// len++;
+// }
+// len += itoa10(putcf, (int)num, 0);
+// putcf('.'); len++;
+// len += itoa10(putcf, (num - (int)num) * power(10,precision), precision);
+// break;
+// case 's':
+// str = va_arg(ap, char* );
+// while(*str)
+// {
+// putcf(*str++);
+// len++;
+// }
+// break;
+// case 'c':
+// putcf((char)va_arg(ap, int));
+// len++;
+// break;
+// default:
+// break;
+// }
+// }
+// else
+// {
+// putcf(*fmt++);
+// len++;
+// }
+// }
return len;
}
@@ -309,10 +310,10 @@ int eprintf(putc_t putcf, const char * fmt, ...)
{
va_list ap;
int len;
-
- va_start(ap, fmt);
- len = evprintf(putcf, fmt, ap);
- va_end(ap);
+ //COMMENTED FIRMWARE
+ // va_start(ap, fmt);
+ // len = evprintf(putcf, fmt, ap);
+ // va_end(ap);
return len;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/filter.h b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/filter.h
index fcb53b6e113643badad0c30b61d248a46ab39962..2e811c160931f0167157f9d3c81535f09a7fe5e7 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/filter.h
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/filter.h
@@ -101,13 +101,14 @@ struct SecondOrderLowPass {
static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter, float tau, float Q, float sample_time,
float value)
{
- float K = tanf(sample_time / (2.0f * tau));
- float poly = K * K + K / Q + 1.0f;
- filter->a[0] = 2.0f * (K * K - 1.0f) / poly;
- filter->a[1] = (K * K - K / Q + 1.0f) / poly;
- filter->b[0] = K * K / poly;
- filter->b[1] = 2.0f * filter->b[0];
- filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
+ //COMMENTED FIRMWARE
+ // float K = tanf(sample_time / (2.0f * tau));
+ // float poly = K * K + K / Q + 1.0f;
+ // filter->a[0] = 2.0f * (K * K - 1.0f) / poly;
+ // filter->a[1] = (K * K - K / Q + 1.0f) / poly;
+ // filter->b[0] = K * K / poly;
+ // filter->b[1] = 2.0f * filter->b[0];
+ // filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
}
/** Update second order low pass filter state with a new value.
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 86a6654af7f5da00bedb94bb5b8cb9ebbe8d7e49..4c7f38bcbebb75b7969431fe8d1b5958b976d560 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
@@ -57,47 +57,48 @@ static float vec_length(const vec3d vec) {
float pow = vec_dot(vec, vec);
float res;
- arm_sqrt_f32(pow, &res);
+ //COMMENTED FIRMWARE
+ // arm_sqrt_f32(pow, &res);
return res;
}
static bool intersect_lines(vec3d orig1, vec3d vec1, vec3d orig2, vec3d vec2, vec3d res, float *dist) {
// Algoritm: http://geomalgorithms.com/a07-_distance.html#Distance-between-Lines
-
- vec3d w0 = {};
- arm_sub_f32(orig1, orig2, w0, vec3d_size);
-
- float a, b, c, d, e;
- arm_dot_prod_f32(vec1, vec1, vec3d_size, &a);
- arm_dot_prod_f32(vec1, vec2, vec3d_size, &b);
- arm_dot_prod_f32(vec2, vec2, vec3d_size, &c);
- arm_dot_prod_f32(vec1, w0, vec3d_size, &d);
- arm_dot_prod_f32(vec2, w0, vec3d_size, &e);
-
- float denom = a * c - b * b;
- if (fabsf(denom) < 1e-5f)
- return false;
-
- // Closest point to 2nd line on 1st line
- float t1 = (b * e - c * d) / denom;
- vec3d pt1 = {};
- arm_scale_f32(vec1, t1, pt1, vec3d_size);
- arm_add_f32(pt1, orig1, pt1, vec3d_size);
-
- // Closest point to 1st line on 2nd line
- float t2 = (a * e - b * d) / denom;
- vec3d pt2 = {};
- arm_scale_f32(vec2, t2, pt2, vec3d_size);
- arm_add_f32(pt2, orig2, pt2, vec3d_size);
-
- // Result is in the middle
- vec3d tmp = {};
- arm_add_f32(pt1, pt2, tmp, vec3d_size);
- arm_scale_f32(tmp, 0.5f, res, vec3d_size);
-
- // Dist is distance between pt1 and pt2
- arm_sub_f32(pt1, pt2, tmp, vec3d_size);
- *dist = vec_length(tmp);
+ //COMMENTED FIRMWARE
+ // vec3d w0 = {};
+ // arm_sub_f32(orig1, orig2, w0, vec3d_size);
+
+ // float a, b, c, d, e;
+ // arm_dot_prod_f32(vec1, vec1, vec3d_size, &a);
+ // arm_dot_prod_f32(vec1, vec2, vec3d_size, &b);
+ // arm_dot_prod_f32(vec2, vec2, vec3d_size, &c);
+ // arm_dot_prod_f32(vec1, w0, vec3d_size, &d);
+ // arm_dot_prod_f32(vec2, w0, vec3d_size, &e);
+
+ // float denom = a * c - b * b;
+ // if (fabsf(denom) < 1e-5f)
+ // return false;
+
+ // // Closest point to 2nd line on 1st line
+ // float t1 = (b * e - c * d) / denom;
+ // vec3d pt1 = {};
+ // arm_scale_f32(vec1, t1, pt1, vec3d_size);
+ // arm_add_f32(pt1, orig1, pt1, vec3d_size);
+
+ // // Closest point to 1st line on 2nd line
+ // float t2 = (a * e - b * d) / denom;
+ // vec3d pt2 = {};
+ // arm_scale_f32(vec2, t2, pt2, vec3d_size);
+ // arm_add_f32(pt2, orig2, pt2, vec3d_size);
+
+ // // Result is in the middle
+ // vec3d tmp = {};
+ // arm_add_f32(pt1, pt2, tmp, vec3d_size);
+ // arm_scale_f32(tmp, 0.5f, res, vec3d_size);
+
+ // // Dist is distance between pt1 and pt2
+ // arm_sub_f32(pt1, pt2, tmp, vec3d_size);
+ // *dist = vec_length(tmp);
return true;
}
@@ -116,14 +117,15 @@ bool lighthouseGeometryGetPositionFromRayIntersection(const baseStationGeometry_
}
void lighthouseGeometryGetBaseStationPosition(const baseStationGeometry_t* bs, vec3d baseStationPos) {
- // TODO: Make geometry adjustments within base station.
- vec3d rotated_origin_delta = {};
- //vec3d base_origin_delta = {-0.025f, -0.025f, 0.f}; // Rotors are slightly off center in base station.
- // arm_matrix_instance_f32 origin_vec = {3, 1, base_origin_delta};
- // arm_matrix_instance_f32 origin_rotated_vec = {3, 1, rotated_origin_delta};
- // mat_mult(&source_rotation_matrix, &origin_vec, &origin_rotated_vec);
- baseStationGeometry_t* bs_unconst = (baseStationGeometry_t*)bs;
- arm_add_f32(bs_unconst->origin, rotated_origin_delta, baseStationPos, vec3d_size);
+ //COMMENTED FIRMWARE
+ // // TODO: Make geometry adjustments within base station.
+ // vec3d rotated_origin_delta = {};
+ // //vec3d base_origin_delta = {-0.025f, -0.025f, 0.f}; // Rotors are slightly off center in base station.
+ // // arm_matrix_instance_f32 origin_vec = {3, 1, base_origin_delta};
+ // // arm_matrix_instance_f32 origin_rotated_vec = {3, 1, rotated_origin_delta};
+ // // mat_mult(&source_rotation_matrix, &origin_vec, &origin_rotated_vec);
+ // baseStationGeometry_t* bs_unconst = (baseStationGeometry_t*)bs;
+ // arm_add_f32(bs_unconst->origin, rotated_origin_delta, baseStationPos, vec3d_size);
}
void lighthouseGeometryGetRay(const baseStationGeometry_t* baseStationGeometry, const float angleH, const float angleV, vec3d ray) {
@@ -172,35 +174,36 @@ bool lighthouseGeometryIntersectionPlaneVector(const vec3d linePoint, const vec3
// }
bool lighthouseGeometryYawDelta(const vec3d ipv, const vec3d spv, const vec3d n, float* yawDelta) {
- float spvn = vec_length(spv);
- float ipvn = vec_length(ipv);
+ //COMMENTED FIRMWARE
+ // float spvn = vec_length(spv);
+ // float ipvn = vec_length(ipv);
- const float minIntersectionVectorLength = 0.0001f;
- if (ipvn < minIntersectionVectorLength) {
- return false;
- }
+ // const float minIntersectionVectorLength = 0.0001f;
+ // if (ipvn < minIntersectionVectorLength) {
+ // return false;
+ // }
- // Angle between vectors
- float a = vec_dot(spv, ipv) / (spvn * ipvn);
+ // // Angle between vectors
+ // float a = vec_dot(spv, ipv) / (spvn * ipvn);
- // Handle rouding errors
- if (a > 1.0f) {
- a = 1.0f;
- }
- if (a < -1.0f) {
- a = -1.0f;
- }
+ // // Handle rouding errors
+ // if (a > 1.0f) {
+ // a = 1.0f;
+ // }
+ // if (a < -1.0f) {
+ // a = -1.0f;
+ // }
- float delta = acosf(a);
+ // float delta = acosf(a);
- // Figure our the sign of the angle by looking at the cross product of the vectors in relation to the normal
- float nt[3];
- vec_cross_product(spv, ipv, nt);
+ // // Figure our the sign of the angle by looking at the cross product of the vectors in relation to the normal
+ // float nt[3];
+ // vec_cross_product(spv, ipv, nt);
- if (vec_dot(n, nt) > 0.0f) {
- delta = -delta;
- }
+ // if (vec_dot(n, nt) > 0.0f) {
+ // delta = -delta;
+ // }
- *yawDelta = delta;
+ // *yawDelta = delta;
return true;
}
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/pulse_processor_v1.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/pulse_processor_v1.c
index 739f0715d1dbe06326fd64f75b46537c40a3c847..f1ca0320d236fb9ba04b8d50bc298a921f511f82 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/pulse_processor_v1.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/lighthouse/pulse_processor_v1.c
@@ -445,47 +445,48 @@ TESTABLE_STATIC bool getSystemSyncTime(const uint32_t syncTimes[], size_t nSyncT
// If the samples are wrapping, all samples bellow PULSE_PROCESSOR_TIMESTAMP_MAX/2 will
// be pushed by (1<<TIMESTAMP_BITWIDTH) to correct the wrapping
bool isWrapping = false;
- int wref = syncTimes[0];
- for (size_t i=0; i<nSyncTimes; i++) {
- if (abs(wref - (int)syncTimes[i]) > (PULSE_PROCESSOR_TIMESTAMP_MAX/2)) {
- isWrapping = true;
- }
- }
-
- int32_t differenceSum = 0;
- int32_t reference = syncTimes[0] % FRAME_LENGTH;
- if (isWrapping && syncTimes[0] < (PULSE_PROCESSOR_TIMESTAMP_MAX/2)) {
- reference = (syncTimes[0] + (1<<PULSE_PROCESSOR_TIMESTAMP_BITWIDTH)) % FRAME_LENGTH;
- }
-
- int minDiff = INT32_MAX;
- int maxDiff = INT32_MIN;
-
- for (size_t i=1; i<nSyncTimes; i++) {
- int diff;
-
- if (isWrapping && (syncTimes[i] < (PULSE_PROCESSOR_TIMESTAMP_MAX/2))) {
- diff = ((syncTimes[i] + (1<<PULSE_PROCESSOR_TIMESTAMP_BITWIDTH)) % FRAME_LENGTH) - reference;
- } else {
- diff = (syncTimes[i] % FRAME_LENGTH) - reference;
- }
-
-
- if (diff < minDiff) {
- minDiff = diff;
- }
- if (diff > maxDiff) {
- maxDiff = diff;
- }
-
- differenceSum += diff;
- }
-
- if ((maxDiff - minDiff) > MAX_FRAME_LENGTH_NOISE) {
- return false;
- }
-
- *syncTime = ((int)syncTimes[0] + ((int)differenceSum / (int)nSyncTimes)) & (PULSE_PROCESSOR_TIMESTAMP_MAX);
+ //COMMENTED FIRMWARE
+ // int wref = syncTimes[0];
+ // for (size_t i=0; i<nSyncTimes; i++) {
+ // if (abs(wref - (int)syncTimes[i]) > (PULSE_PROCESSOR_TIMESTAMP_MAX/2)) {
+ // isWrapping = true;
+ // }
+ // }
+
+ // int32_t differenceSum = 0;
+ // int32_t reference = syncTimes[0] % FRAME_LENGTH;
+ // if (isWrapping && syncTimes[0] < (PULSE_PROCESSOR_TIMESTAMP_MAX/2)) {
+ // reference = (syncTimes[0] + (1<<PULSE_PROCESSOR_TIMESTAMP_BITWIDTH)) % FRAME_LENGTH;
+ // }
+
+ // int minDiff = INT32_MAX;
+ // int maxDiff = INT32_MIN;
+
+ // for (size_t i=1; i<nSyncTimes; i++) {
+ // int diff;
+
+ // if (isWrapping && (syncTimes[i] < (PULSE_PROCESSOR_TIMESTAMP_MAX/2))) {
+ // diff = ((syncTimes[i] + (1<<PULSE_PROCESSOR_TIMESTAMP_BITWIDTH)) % FRAME_LENGTH) - reference;
+ // } else {
+ // diff = (syncTimes[i] % FRAME_LENGTH) - reference;
+ // }
+
+
+ // if (diff < minDiff) {
+ // minDiff = diff;
+ // }
+ // if (diff > maxDiff) {
+ // maxDiff = diff;
+ // }
+
+ // differenceSum += diff;
+ // }
+
+ // if ((maxDiff - minDiff) > MAX_FRAME_LENGTH_NOISE) {
+ // return false;
+ // }
+
+ // *syncTime = ((int)syncTimes[0] + ((int)differenceSum / (int)nSyncTimes)) & (PULSE_PROCESSOR_TIMESTAMP_MAX);
return true;
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 f5b865ddb1a4a9428ade4f76943dab051134a1a7..a1e8fe960ddf505693b37c2b453ff71eb81abb66 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
@@ -232,10 +232,11 @@ static bool processFrame(const pulseProcessorFrame_t* frameData, pulseProcessorV
}
void pulseProcessorV2ConvertToV1Angles(const float v2Angle1, const float v2Angle2, float* v1Angles) {
- const float tant = 0.5773502691896258f; // tan(pi / 6)
+ //COMMENTED FIRMWARE
+ // const float tant = 0.5773502691896258f; // tan(pi / 6)
- v1Angles[0] = (v2Angle1 + v2Angle2) / 2.0f;
- v1Angles[1] = atan2f(sinf(v2Angle2 - v2Angle1), (tant * (cosf(v2Angle1) + cosf(v2Angle2))));
+ // v1Angles[0] = (v2Angle1 + v2Angle2) / 2.0f;
+ // v1Angles[1] = atan2f(sinf(v2Angle2 - v2Angle1), (tant * (cosf(v2Angle1) + cosf(v2Angle2))));
}
static void calculateAngles(const pulseProcessorV2SweepBlock_t* latestBlock, const pulseProcessorV2SweepBlock_t* previousBlock, pulseProcessorResult_t* angles) {
diff --git a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/num.c b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/num.c
index 0b20be6ab7e74e739a9faa00cb2333657fa2d99f..e77a3fff3860ca8827b8322ba79e7a0de2cd9d47 100644
--- a/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/num.c
+++ b/uboot-compiler/bm_crazyflie/FreeRTOS/Demo/CORTEX_A72_64-bit_Raspberrypi4/utils/src/num.c
@@ -102,22 +102,24 @@ uint16_t limitUint16(int32_t value)
float constrain(float value, const float minVal, const float maxVal)
{
- return fminf(maxVal, fmaxf(minVal,value));
+ //COMMENTED FIRMWARE
+ return 0;//fminf(maxVal, fmaxf(minVal,value));
}
float deadband(float value, const float threshold)
{
- if (fabsf(value) < threshold)
- {
- value = 0;
- }
- else if (value > 0)
- {
- value -= threshold;
- }
- else if (value < 0)
- {
- value += threshold;
- }
+ //COMMENTED FIRMWARE
+ // if (fabsf(value) < threshold)
+ // {
+ // value = 0;
+ // }
+ // else if (value > 0)
+ // {
+ // value -= threshold;
+ // }
+ // else if (value < 0)
+ // {
+ // value += threshold;
+ // }
return value;
}