diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/deck/api/deck_analog.c b/crazyflie_software/crazyflie-firmware-2021.06/src/deck/api/deck_analog.c
index f1ae95f9b89fca7f055932763358730e76dbbe8a..27fed248975ae21d496062d26fda249f800d5ee3 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/deck/api/deck_analog.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/deck/api/deck_analog.c
@@ -36,33 +36,33 @@ void adcInit(void)
/*
* Note: This function initializes only ADC2, and only for single channel, single conversion mode. No DMA, no interrupts, no bells or whistles.
*/
-
+ //COMMENTED FIRMWARE
/* Note that this de-initializes registers for all ADCs (ADCx) */
- ADC_DeInit();
+ // ADC_DeInit();
- /* Define ADC init structures */
- ADC_InitTypeDef ADC_InitStructure;
- ADC_CommonInitTypeDef ADC_CommonInitStructure;
+ // /* Define ADC init structures */
+ // ADC_InitTypeDef ADC_InitStructure;
+ // ADC_CommonInitTypeDef ADC_CommonInitStructure;
- /* Populates structures with reset values */
- ADC_StructInit(&ADC_InitStructure);
- ADC_CommonStructInit(&ADC_CommonInitStructure);
+ // /* Populates structures with reset values */
+ // ADC_StructInit(&ADC_InitStructure);
+ // ADC_CommonStructInit(&ADC_CommonInitStructure);
- /* enable ADC clock */
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE);
+ // /* enable ADC clock */
+ // RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE);
- /* init ADCs in independent mode, div clock by two */
- ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent;
- ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2; /* HCLK = 168MHz, PCLK2 = 84MHz, ADCCLK = 42MHz (when using ADC_Prescaler_Div2) */
- ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled;
- ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
- ADC_CommonInit(&ADC_CommonInitStructure);
+ // /* init ADCs in independent mode, div clock by two */
+ // ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent;
+ // ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2; /* HCLK = 168MHz, PCLK2 = 84MHz, ADCCLK = 42MHz (when using ADC_Prescaler_Div2) */
+ // ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled;
+ // ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
+ // ADC_CommonInit(&ADC_CommonInitStructure);
- /* Init ADC2: 12bit, single-conversion. For Arduino compatibility set 10bit */
- analogReadResolution(12);
+ // /* Init ADC2: 12bit, single-conversion. For Arduino compatibility set 10bit */
+ // analogReadResolution(12);
- /* Enable ADC2 */
- ADC_Cmd(ADC2, ENABLE);
+ // /* Enable ADC2 */
+ // ADC_Cmd(ADC2, ENABLE);
}
static uint16_t analogReadChannel(uint8_t channel)
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ak8963.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ak8963.c
index d248f41f8c53fc9e50957fe877035586d9e16428..7aa6b20c939ed895be8736bab51d29974f3d46e5 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ak8963.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ak8963.c
@@ -66,54 +66,57 @@ void ak8963Init(I2C_Dev *i2cPort)
*/
bool ak8963TestConnection()
{
- if (i2cdevReadByte(I2Cx, devAddr, AK8963_RA_WIA, buffer) == 1)
- {
- return (buffer[0] == 0x48);
- }
- return false;
+ //COMMENTED FIRMWARE
+ // if (i2cdevReadByte(I2Cx, devAddr, AK8963_RA_WIA, buffer) == 1)
+ // {
+ // return (buffer[0] == 0x48);
+ // }
+ // return false;
+ return true;
}
bool ak8963SelfTest()
{
bool testStatus = true;
- int16_t mx, my, mz; // positive magnetometer measurements
- uint8_t confSave;
- uint8_t timeout = 20;
+ //COMMENTED FIRMWARE
+ // int16_t mx, my, mz; // positive magnetometer measurements
+ // uint8_t confSave;
+ // uint8_t timeout = 20;
- // Save register values
- if (i2cdevReadByte(I2Cx, devAddr, AK8963_RA_CNTL, &confSave) == false)
- {
- // TODO: error handling
- return false;
- }
+ // // Save register values
+ // if (i2cdevReadByte(I2Cx, devAddr, AK8963_RA_CNTL, &confSave) == false)
+ // {
+ // // TODO: error handling
+ // return false;
+ // }
- // Power down
- ak8963SetMode(AK8963_MODE_POWERDOWN);
- ak8963SetSelfTest(true);
- ak8963SetMode(AK8963_MODE_16BIT | AK8963_MODE_SELFTEST);
- // Clear ST1 by reading ST2
- ak8963GetOverflowStatus();
- while (!ak8963GetDataReady() && timeout--)
- {
- vTaskDelay(M2T(1));
- }
- ak8963GetHeading(&mx, &my, &mz);
- // Power down
- ak8963SetMode(AK8963_MODE_POWERDOWN);
+ // // Power down
+ // ak8963SetMode(AK8963_MODE_POWERDOWN);
+ // ak8963SetSelfTest(true);
+ // ak8963SetMode(AK8963_MODE_16BIT | AK8963_MODE_SELFTEST);
+ // // Clear ST1 by reading ST2
+ // ak8963GetOverflowStatus();
+ // while (!ak8963GetDataReady() && timeout--)
+ // {
+ // vTaskDelay(M2T(1));
+ // }
+ // ak8963GetHeading(&mx, &my, &mz);
+ // // Power down
+ // ak8963SetMode(AK8963_MODE_POWERDOWN);
- if (ak8963EvaluateSelfTest(AK8963_ST_X_MIN, AK8963_ST_X_MAX, mx, "X") &&
- ak8963EvaluateSelfTest(AK8963_ST_Y_MIN, AK8963_ST_Y_MAX, my, "Y") &&
- ak8963EvaluateSelfTest(AK8963_ST_Z_MIN, AK8963_ST_Z_MAX, mz, "Z"))
- {
- DEBUG_PRINT("Self test [OK].\n");
- }
- else
- {
- testStatus = false;
- }
+ // if (ak8963EvaluateSelfTest(AK8963_ST_X_MIN, AK8963_ST_X_MAX, mx, "X") &&
+ // ak8963EvaluateSelfTest(AK8963_ST_Y_MIN, AK8963_ST_Y_MAX, my, "Y") &&
+ // ak8963EvaluateSelfTest(AK8963_ST_Z_MIN, AK8963_ST_Z_MAX, mz, "Z"))
+ // {
+ // DEBUG_PRINT("Self test [OK].\n");
+ // }
+ // else
+ // {
+ // testStatus = false;
+ // }
- // Power up with saved config
- ak8963SetMode(confSave);
+ // // Power up with saved config
+ // ak8963SetMode(confSave);
return testStatus;
}
@@ -127,12 +130,13 @@ bool ak8963SelfTest()
*/
static bool ak8963EvaluateSelfTest(int16_t min, int16_t max, int16_t value, char* string)
{
- if (value < min || value > max)
- {
- DEBUG_PRINT("Self test %s [FAIL]. low: %d, high: %d, measured: %d\n",
- string, min, max, value);
- return false;
- }
+ //COMMENTED FIRMWARE
+ // if (value < min || value > max)
+ // {
+ // DEBUG_PRINT("Self test %s [FAIL]. low: %d, high: %d, measured: %d\n",
+ // string, min, max, value);
+ // return false;
+ // }
return true;
}
@@ -140,7 +144,8 @@ static bool ak8963EvaluateSelfTest(int16_t min, int16_t max, int16_t value, char
uint8_t ak8963GetDeviceID()
{
- i2cdevReadByte(I2Cx, devAddr, AK8963_RA_WIA, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_WIA, buffer);
return buffer[0];
}
@@ -148,7 +153,8 @@ uint8_t ak8963GetDeviceID()
uint8_t ak8963GetInfo()
{
- i2cdevReadByte(I2Cx, devAddr, AK8963_RA_INFO, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_INFO, buffer);
return buffer[0];
}
@@ -156,7 +162,8 @@ uint8_t ak8963GetInfo()
bool ak8963GetDataReady()
{
- i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST1, AK8963_ST1_DRDY_BIT, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST1, AK8963_ST1_DRDY_BIT, buffer);
return buffer[0];
}
@@ -165,112 +172,131 @@ void ak8963GetHeading(int16_t *x, int16_t *y, int16_t *z)
{
// i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_MODE_SINGLE);
// delay(10);
- i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_HXL, 6, buffer);
- *x = (((int16_t) buffer[1]) << 8) | buffer[0];
- *y = (((int16_t) buffer[3]) << 8) | buffer[2];
- *z = (((int16_t) buffer[5]) << 8) | buffer[4];
+//COMMENTED FIRMWARE
+ // i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_HXL, 6, buffer);
+ // *x = (((int16_t) buffer[1]) << 8) | buffer[0];
+ // *y = (((int16_t) buffer[3]) << 8) | buffer[2];
+ // *z = (((int16_t) buffer[5]) << 8) | buffer[4];
}
int16_t ak8963GetHeadingX()
{
- i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_MODE_SINGLE);
-// delay(10);
- i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_HXL, 2, buffer);
- return (((int16_t) buffer[1]) << 8) | buffer[0];
+ //COMMENTED FIRMWARE
+// i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_MODE_SINGLE);
+// // delay(10);
+// i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_HXL, 2, buffer);
+// return (((int16_t) buffer[1]) << 8) | buffer[0];
}
int16_t ak8963GetHeadingY()
{
- i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_MODE_SINGLE);
-// delay(10);
- i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_HYL, 2, buffer);
- return (((int16_t) buffer[1]) << 8) | buffer[0];
+ //COMMENTED FIRMWARE
+// i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_MODE_SINGLE);
+// // delay(10);
+// i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_HYL, 2, buffer);
+// return (((int16_t) buffer[1]) << 8) | buffer[0];
}
int16_t ak8963GetHeadingZ()
{
- i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_MODE_SINGLE);
-// delay(10);
- i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_HZL, 2, buffer);
- return (((int16_t) buffer[1]) << 8) | buffer[0];
+ //COMMENTED FIRMWARE
+// i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_MODE_SINGLE);
+// // delay(10);
+// i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_HZL, 2, buffer);
+// return (((int16_t) buffer[1]) << 8) | buffer[0];
}
// ST2 register
bool ak8963GetOverflowStatus()
{
- i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST2, AK8963_ST2_HOFL_BIT, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST2, AK8963_ST2_HOFL_BIT, buffer);
return buffer[0];
}
bool ak8963GetDataError()
{
- i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST2, AK8963_ST2_DERR_BIT, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST2, AK8963_ST2_DERR_BIT, buffer);
return buffer[0];
}
// CNTL register
uint8_t ak8963GetMode()
{
- i2cdevReadByte(I2Cx, devAddr, AK8963_RA_CNTL, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_CNTL, buffer);
return buffer[0];
}
void ak8963SetMode(uint8_t mode)
{
- i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, mode);
+ //COMMENTED FIRMWARE
+ //i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, mode);
}
void ak8963Reset()
{
- i2cdevWriteBits(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_CNTL_MODE_BIT,
+ //COMMENTED FIRMWARE
+ //i2cdevWriteBits(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_CNTL_MODE_BIT,
AK8963_CNTL_MODE_LENGTH, AK8963_MODE_POWERDOWN);
}
// ASTC register
void ak8963SetSelfTest(bool enabled)
{
- i2cdevWriteBit(I2Cx, devAddr, AK8963_RA_ASTC, AK8963_ASTC_SELF_BIT, enabled);
+ //COMMENTED FIRMWARE
+ //i2cdevWriteBit(I2Cx, devAddr, AK8963_RA_ASTC, AK8963_ASTC_SELF_BIT, enabled);
}
// I2CDIS
void ak8963DisableI2C()
{
- i2cdevWriteBit(I2Cx, devAddr, AK8963_RA_I2CDIS, AK8963_I2CDIS_BIT, true);
+ //COMMENTED FIRMWARE
+ //i2cdevWriteBit(I2Cx, devAddr, AK8963_RA_I2CDIS, AK8963_I2CDIS_BIT, true);
}
// ASA* registers
void ak8963GetAdjustment(int8_t *x, int8_t *y, int8_t *z)
{
- i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_ASAX, 3, buffer);
- *x = buffer[0];
- *y = buffer[1];
- *z = buffer[2];
+ //COMMENTED FIRMWARE
+ // i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_ASAX, 3, buffer);
+ // *x = buffer[0];
+ // *y = buffer[1];
+ // *z = buffer[2];
}
void ak8963SetAdjustment(int8_t x, int8_t y, int8_t z)
{
- buffer[0] = x;
- buffer[1] = y;
- buffer[2] = z;
- i2cdevWriteReg8(I2Cx, devAddr, AK8963_RA_ASAX, 3, buffer);
+ //COMMENTED FIRMWARE
+ // buffer[0] = x;
+ // buffer[1] = y;
+ // buffer[2] = z;
+ // i2cdevWriteReg8(I2Cx, devAddr, AK8963_RA_ASAX, 3, buffer);
}
uint8_t ak8963GetAdjustmentX()
{
- i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAX, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAX, buffer);
return buffer[0];
}
void ak8963SetAdjustmentX(uint8_t x)
{
- i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAX, x);
+ //COMMENTED FIRMWARE
+ //i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAX, x);
}
uint8_t ak8963GetAdjustmentY()
{
- i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAY, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAY, buffer);
return buffer[0];
}
void ak8963SetAdjustmentY(uint8_t y)
{
- i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAY, y);
+ //COMMENTED FIRMWARE
+ //i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAY, y);
}
uint8_t ak8963GetAdjustmentZ()
{
- i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAZ, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAZ, buffer);
return buffer[0];
}
void ak8963SetAdjustmentZ(uint8_t z)
{
- i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAZ, z);
+ //COMMENTED FIRMWARE
+ //i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAZ, z);
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/cppm.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/cppm.c
index e0ac6de67750905007a30ae00a811786ff76ecc3..a3cf95cee3c3af5153fc0be789121bee13c5c9ca 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/cppm.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/cppm.c
@@ -65,42 +65,43 @@ static bool isAvailible;
void cppmInit(void)
{
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_ICInitTypeDef TIM_ICInitStructure;
- GPIO_InitTypeDef GPIO_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
-
- RCC_AHB1PeriphClockCmd(CPPM_GPIO_RCC, ENABLE);
- RCC_APB1PeriphClockCmd(CPPM_TIMER_RCC, ENABLE);
-
- // Configure the GPIO for the timer input
- GPIO_StructInit(&GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_InitStructure.GPIO_Pin = CPPM_GPIO_PIN;
- GPIO_Init(CPPM_GPIO_PORT, &GPIO_InitStructure);
-
- GPIO_PinAFConfig(CPPM_GPIO_PORT, CPPM_GPIO_SOURCE, CPPM_GPIO_AF);
-
- // Time base configuration. 1us tick.
- TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
- TIM_TimeBaseStructure.TIM_Prescaler = CPPM_TIM_PRESCALER;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseInit(CPPM_TIMER, &TIM_TimeBaseStructure);
-
- // Setup input capture using default config.
- TIM_ICStructInit(&TIM_ICInitStructure);
- TIM_ICInit(CPPM_TIMER, &TIM_ICInitStructure);
-
- NVIC_InitStructure.NVIC_IRQChannel = TIM8_TRG_COM_TIM14_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_CPPM_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- captureQueue = STATIC_MEM_QUEUE_CREATE(captureQueue);
-
- TIM_ITConfig(CPPM_TIMER, TIM_IT_Update | TIM_IT_CC1, ENABLE);
- TIM_Cmd(CPPM_TIMER, ENABLE);
+ //COMMENTED FIRMWARE
+ // TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ // TIM_ICInitTypeDef TIM_ICInitStructure;
+ // GPIO_InitTypeDef GPIO_InitStructure;
+ // NVIC_InitTypeDef NVIC_InitStructure;
+
+ // RCC_AHB1PeriphClockCmd(CPPM_GPIO_RCC, ENABLE);
+ // RCC_APB1PeriphClockCmd(CPPM_TIMER_RCC, ENABLE);
+
+ // // Configure the GPIO for the timer input
+ // GPIO_StructInit(&GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ // GPIO_InitStructure.GPIO_Pin = CPPM_GPIO_PIN;
+ // GPIO_Init(CPPM_GPIO_PORT, &GPIO_InitStructure);
+
+ // GPIO_PinAFConfig(CPPM_GPIO_PORT, CPPM_GPIO_SOURCE, CPPM_GPIO_AF);
+
+ // // Time base configuration. 1us tick.
+ // TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
+ // TIM_TimeBaseStructure.TIM_Prescaler = CPPM_TIM_PRESCALER;
+ // TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ // TIM_TimeBaseInit(CPPM_TIMER, &TIM_TimeBaseStructure);
+
+ // // Setup input capture using default config.
+ // TIM_ICStructInit(&TIM_ICInitStructure);
+ // TIM_ICInit(CPPM_TIMER, &TIM_ICInitStructure);
+
+ // NVIC_InitStructure.NVIC_IRQChannel = TIM8_TRG_COM_TIM14_IRQn;
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_CPPM_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ // NVIC_Init(&NVIC_InitStructure);
+
+ // captureQueue = STATIC_MEM_QUEUE_CREATE(captureQueue);
+
+ // TIM_ITConfig(CPPM_TIMER, TIM_IT_Update | TIM_IT_CC1, ENABLE);
+ // TIM_Cmd(CPPM_TIMER, ENABLE);
}
bool cppmIsAvailible(void)
@@ -154,32 +155,33 @@ uint16_t cppmConvert2uint16(uint16_t timestamp)
void __attribute__((used)) TIM8_TRG_COM_TIM14_IRQHandler()
{
- uint16_t capureVal;
- uint16_t capureValDiff;
- portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-
- if (TIM_GetITStatus(CPPM_TIMER, TIM_IT_CC1) != RESET)
- {
- if (TIM_GetFlagStatus(CPPM_TIMER, TIM_FLAG_CC1OF) != RESET)
- {
- //TODO: Handle overflow error
- }
-
- capureVal = TIM_GetCapture1(CPPM_TIMER);
- capureValDiff = capureVal - prevCapureVal;
- prevCapureVal = capureVal;
-
- xQueueSendFromISR(captureQueue, &capureValDiff, &xHigherPriorityTaskWoken);
-
- captureFlag = true;
- TIM_ClearITPendingBit(CPPM_TIMER, TIM_IT_CC1);
- }
-
- if (TIM_GetITStatus(CPPM_TIMER, TIM_IT_Update) != RESET)
- {
- // Update input status
- isAvailible = (captureFlag == true);
- captureFlag = false;
- TIM_ClearITPendingBit(CPPM_TIMER, TIM_IT_Update);
- }
+ //COMMENTED FIRMWARE
+ // uint16_t capureVal;
+ // uint16_t capureValDiff;
+ // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+
+ // if (TIM_GetITStatus(CPPM_TIMER, TIM_IT_CC1) != RESET)
+ // {
+ // if (TIM_GetFlagStatus(CPPM_TIMER, TIM_FLAG_CC1OF) != RESET)
+ // {
+ // //TODO: Handle overflow error
+ // }
+
+ // capureVal = TIM_GetCapture1(CPPM_TIMER);
+ // capureValDiff = capureVal - prevCapureVal;
+ // prevCapureVal = capureVal;
+
+ // xQueueSendFromISR(captureQueue, &capureValDiff, &xHigherPriorityTaskWoken);
+
+ // captureFlag = true;
+ // TIM_ClearITPendingBit(CPPM_TIMER, TIM_IT_CC1);
+ // }
+
+ // if (TIM_GetITStatus(CPPM_TIMER, TIM_IT_Update) != RESET)
+ // {
+ // // Update input status
+ // isAvailible = (captureFlag == true);
+ // captureFlag = false;
+ // TIM_ClearITPendingBit(CPPM_TIMER, TIM_IT_Update);
+ // }
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/eeprom.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/eeprom.c
index 9aedf5cc5821da3f66813c31c8109bbf1df4e040..422e688ebec307a25c966026c24eab032b6e22c8 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/eeprom.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/eeprom.c
@@ -131,7 +131,8 @@ bool eepromTestConnection(void)
if (!isInit)
return false;
- status = i2cdevRead16(I2Cx, devAddr, 0, 1, &tmp);
+ //COMMENTED FIRMWARE
+ status = true;//i2cdevRead16(I2Cx, devAddr, 0, 1, &tmp);
return status;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/exti.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/exti.c
index 86675d7a08d704bb9184476641f44f7a087c0459..79810be140d9ae8ac8c6b1847874f932ec5b870b 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/exti.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/exti.c
@@ -40,46 +40,46 @@ void extiInit()
if (isInit)
return;
+ //COMMENTED FIRMWARE
+ // // This is required for the EXTI interrupt configuration since EXTI
+ // // lines are set via the SYSCFG peripheral; eg.
+ // // SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOC, EXTI_PinSource13);
+ // RCC_AHB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
- // This is required for the EXTI interrupt configuration since EXTI
- // lines are set via the SYSCFG peripheral; eg.
- // SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOC, EXTI_PinSource13);
- RCC_AHB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
+ // // Here we enable all EXTI interrupt handlers to save conflicting
+ // // reinitialization code for the 9_5 and 15_10 handlers. Note that
+ // // the individual EXTI interrupts still need to be enabled.
- // Here we enable all EXTI interrupt handlers to save conflicting
- // reinitialization code for the 9_5 and 15_10 handlers. Note that
- // the individual EXTI interrupts still need to be enabled.
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI0_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannel = EXTI0_IRQn;
+ // NVIC_Init(&NVIC_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI0_PRI;
- NVIC_InitStructure.NVIC_IRQChannel = EXTI0_IRQn;
- NVIC_Init(&NVIC_InitStructure);
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI1_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannel = EXTI1_IRQn;
+ // NVIC_Init(&NVIC_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI1_PRI;
- NVIC_InitStructure.NVIC_IRQChannel = EXTI1_IRQn;
- NVIC_Init(&NVIC_InitStructure);
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI2_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
+ // NVIC_Init(&NVIC_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI2_PRI;
- NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
- NVIC_Init(&NVIC_InitStructure);
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI3_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannel = EXTI3_IRQn;
+ // NVIC_Init(&NVIC_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI3_PRI;
- NVIC_InitStructure.NVIC_IRQChannel = EXTI3_IRQn;
- NVIC_Init(&NVIC_InitStructure);
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI4_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannel = EXTI4_IRQn;
+ // NVIC_Init(&NVIC_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI4_PRI;
- NVIC_InitStructure.NVIC_IRQChannel = EXTI4_IRQn;
- NVIC_Init(&NVIC_InitStructure);
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI9_5_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
+ // NVIC_Init(&NVIC_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI9_5_PRI;
- NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
- NVIC_Init(&NVIC_InitStructure);
-
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI15_10_PRI;
- NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
- NVIC_Init(&NVIC_InitStructure);
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI15_10_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
+ // NVIC_Init(&NVIC_InitStructure);
isInit = true;
}
@@ -91,100 +91,107 @@ bool extiTest(void)
void __attribute__((used)) EXTI0_IRQHandler(void)
{
- NVIC_ClearPendingIRQ(EXTI0_IRQn);
- EXTI_ClearITPendingBit(EXTI_Line0);
- EXTI0_Callback();
+ //COMMENTED FIRMWARE
+ // NVIC_ClearPendingIRQ(EXTI0_IRQn);
+ // EXTI_ClearITPendingBit(EXTI_Line0);
+ // EXTI0_Callback();
}
void __attribute__((used)) EXTI1_IRQHandler(void)
{
- NVIC_ClearPendingIRQ(EXTI1_IRQn);
- EXTI_ClearITPendingBit(EXTI_Line1);
- EXTI1_Callback();
+ //COMMENTED FIRMWARE
+ // NVIC_ClearPendingIRQ(EXTI1_IRQn);
+ // EXTI_ClearITPendingBit(EXTI_Line1);
+ // EXTI1_Callback();
}
void __attribute__((used)) EXTI2_IRQHandler(void)
{
- NVIC_ClearPendingIRQ(EXTI2_IRQn);
- EXTI_ClearITPendingBit(EXTI_Line2);
- EXTI2_Callback();
+ //COMMENTED FIRMWARE
+ // NVIC_ClearPendingIRQ(EXTI2_IRQn);
+ // EXTI_ClearITPendingBit(EXTI_Line2);
+ // EXTI2_Callback();
}
void __attribute__((used)) EXTI3_IRQHandler(void)
{
- NVIC_ClearPendingIRQ(EXTI3_IRQn);
- EXTI_ClearITPendingBit(EXTI_Line3);
- EXTI3_Callback();
+ //COMMENTED FIRMWARE
+ // NVIC_ClearPendingIRQ(EXTI3_IRQn);
+ // EXTI_ClearITPendingBit(EXTI_Line3);
+ // EXTI3_Callback();
}
void __attribute__((used)) EXTI4_IRQHandler(void)
{
- NVIC_ClearPendingIRQ(EXTI4_IRQn);
- EXTI_ClearITPendingBit(EXTI_Line4);
- EXTI4_Callback();
+ //COMMENTED FIRMWARE
+ // NVIC_ClearPendingIRQ(EXTI4_IRQn);
+ // EXTI_ClearITPendingBit(EXTI_Line4);
+ // EXTI4_Callback();
}
void __attribute__((used)) EXTI9_5_IRQHandler(void)
{
- NVIC_ClearPendingIRQ(EXTI9_5_IRQn);
- if (EXTI_GetITStatus(EXTI_Line5) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line5);
- EXTI5_Callback();
- }
-
- if (EXTI_GetITStatus(EXTI_Line6) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line6);
- EXTI6_Callback();
- }
-
- if (EXTI_GetITStatus(EXTI_Line7) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line7);
- EXTI7_Callback();
- }
-
- if (EXTI_GetITStatus(EXTI_Line8) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line8);
- EXTI8_Callback();
- }
-
- if (EXTI_GetITStatus(EXTI_Line9) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line9);
- EXTI9_Callback();
- }
+ //COMMENTED FIRMWARE
+ // NVIC_ClearPendingIRQ(EXTI9_5_IRQn);
+ // if (EXTI_GetITStatus(EXTI_Line5) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line5);
+ // EXTI5_Callback();
+ // }
+
+ // if (EXTI_GetITStatus(EXTI_Line6) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line6);
+ // EXTI6_Callback();
+ // }
+
+ // if (EXTI_GetITStatus(EXTI_Line7) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line7);
+ // EXTI7_Callback();
+ // }
+
+ // if (EXTI_GetITStatus(EXTI_Line8) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line8);
+ // EXTI8_Callback();
+ // }
+
+ // if (EXTI_GetITStatus(EXTI_Line9) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line9);
+ // EXTI9_Callback();
+ // }
}
void __attribute__((used)) EXTI15_10_IRQHandler(void)
{
- NVIC_ClearPendingIRQ(EXTI15_10_IRQn);
- if (EXTI_GetITStatus(EXTI_Line10) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line10);
- EXTI10_Callback();
- }
-
- if (EXTI_GetITStatus(EXTI_Line11) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line11);
- EXTI11_Callback();
- }
-
- if (EXTI_GetITStatus(EXTI_Line12) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line12);
- EXTI12_Callback();
- }
-
- if (EXTI_GetITStatus(EXTI_Line13) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line13);
- EXTI13_Callback();
- }
-
- if (EXTI_GetITStatus(EXTI_Line14) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line14);
- EXTI14_Callback();
- }
-
- if (EXTI_GetITStatus(EXTI_Line15) == SET) {
- EXTI_ClearITPendingBit(EXTI_Line15);
- EXTI15_Callback();
- }
+ //COMMENTED FIRMWARE
+ // NVIC_ClearPendingIRQ(EXTI15_10_IRQn);
+ // if (EXTI_GetITStatus(EXTI_Line10) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line10);
+ // EXTI10_Callback();
+ // }
+
+ // if (EXTI_GetITStatus(EXTI_Line11) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line11);
+ // EXTI11_Callback();
+ // }
+
+ // if (EXTI_GetITStatus(EXTI_Line12) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line12);
+ // EXTI12_Callback();
+ // }
+
+ // if (EXTI_GetITStatus(EXTI_Line13) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line13);
+ // EXTI13_Callback();
+ // }
+
+ // if (EXTI_GetITStatus(EXTI_Line14) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line14);
+ // EXTI14_Callback();
+ // }
+
+ // if (EXTI_GetITStatus(EXTI_Line15) == SET) {
+ // EXTI_ClearITPendingBit(EXTI_Line15);
+ // EXTI15_Callback();
+ // }
}
void __attribute__((weak)) EXTI0_Callback(void) { }
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/fatfs_sd.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/fatfs_sd.c
index d424cc8c90c1ce601553aee5415f0a3d48739d9a..1fe8a06ee0c639fe729819c2acde94c289160b81 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/fatfs_sd.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/fatfs_sd.c
@@ -54,14 +54,15 @@ static int waitForCardReady(sdSpiContext_t *context, UINT timeoutMs) {
BYTE d;
uint32_t timeout = timeoutMs;
- while ((d = context->xchgSpi(0xFF)) != 0xFF && timeout)
- {
- // Waiting can take a while so release the SPI bus in between
- context->csHigh(0);
- context->delayMs(1);
- context->csLow();
- timeout--;
- }
+ //COMMENTED FIRMWARE
+ // while ((d = context->xchgSpi(0xFF)) != 0xFF && timeout)
+ // {
+ // // Waiting can take a while so release the SPI bus in between
+ // context->csHigh(0);
+ // context->delayMs(1);
+ // context->csLow();
+ // timeout--;
+ // }
return (d == 0xFF) ? INT_READY : INT_TIMEOUT;
}
@@ -93,36 +94,37 @@ static int select(sdSpiContext_t *context) {
/* power on */
static void powerOn(sdSpiContext_t *context)
{
- uint8_t cmdbuff[6];
- uint32_t cnt = 0x1FFF;
-
- deselect(context);
- // Send 80 dummy clocks (10 * 8) to wake up
- for (BYTE n = 10; n; n--) {
- context->xchgSpi(0xFF);
- }
- /* slave select without wait*/
- context->csLow();
-
- /* make idle state */
- cmdbuff[0] = 0x40 | CMD0; /* CMD0:GO_IDLE_STATE */
- cmdbuff[1] = 0;
- cmdbuff[2] = 0;
- cmdbuff[3] = 0;
- cmdbuff[4] = 0;
- cmdbuff[5] = 0x95; /* CRC */
-
- context->xmitSpiMulti(cmdbuff, sizeof(cmdbuff));
-
- /* wait response */
- while ((context->xchgSpi(0xFF) != 0x01) && cnt)
- {
- cnt--;
- }
-
- deselect(context);
-
- powerFlag = 1;
+ //COMMENTED FIRMWARE
+ // uint8_t cmdbuff[6];
+ // uint32_t cnt = 0x1FFF;
+
+ // deselect(context);
+ // // Send 80 dummy clocks (10 * 8) to wake up
+ // for (BYTE n = 10; n; n--) {
+ // context->xchgSpi(0xFF);
+ // }
+ // /* slave select without wait*/
+ // context->csLow();
+
+ // /* make idle state */
+ // cmdbuff[0] = 0x40 | CMD0; /* CMD0:GO_IDLE_STATE */
+ // cmdbuff[1] = 0;
+ // cmdbuff[2] = 0;
+ // cmdbuff[3] = 0;
+ // cmdbuff[4] = 0;
+ // cmdbuff[5] = 0x95; /* CRC */
+
+ // context->xmitSpiMulti(cmdbuff, sizeof(cmdbuff));
+
+ // /* wait response */
+ // while ((context->xchgSpi(0xFF) != 0x01) && cnt)
+ // {
+ // cnt--;
+ // }
+
+ // deselect(context);
+
+ // powerFlag = 1;
}
/* power off */
@@ -139,50 +141,52 @@ static uint8_t checkPower(sdSpiContext_t *context)
static int receiveDataBlock(sdSpiContext_t *context, BYTE *data, UINT length) {
- BYTE token;
+ //COMMENTED FIRMWARE
+ // BYTE token;
- context->timer1 = 2; // In centi-seconds
- do {
- token = context->xchgSpi(0xFF);
- } while ((token == 0xFF) && context->timer1);
+ // context->timer1 = 2; // In centi-seconds
+ // do {
+ // token = context->xchgSpi(0xFF);
+ // } while ((token == 0xFF) && context->timer1);
- if(token != 0xFE){
- return INT_ERROR;
- }
+ // if(token != 0xFE){
+ // return INT_ERROR;
+ // }
- // Store trailing data to the buffer
- context->rcvrSpiMulti(data, length);
+ // // Store trailing data to the buffer
+ // context->rcvrSpiMulti(data, length);
- // Discard CRC
- context->xchgSpi(0xFF);
- context->xchgSpi(0xFF);
+ // // Discard CRC
+ // context->xchgSpi(0xFF);
+ // context->xchgSpi(0xFF);
return INT_READY;
}
static int transmitDataBlock(sdSpiContext_t *context, const BYTE *data, BYTE token) {
- if (!waitForCardReady(context, 500)) {
- return INT_TIMEOUT;
- }
+ //COMMENTED FIRMWARE
+ // if (!waitForCardReady(context, 500)) {
+ // return INT_TIMEOUT;
+ // }
- context->xchgSpi(token);
+ // context->xchgSpi(token);
- // Send data if token is other than StopTran
- if (token != 0xFD) {
- context->xmitSpiMulti(data, 512);
+ // // Send data if token is other than StopTran
+ // if (token != 0xFD) {
+ // context->xmitSpiMulti(data, 512);
- // Dummy CRC
- context->xchgSpi(0xFF);
- context->xchgSpi(0xFF);
+ // // Dummy CRC
+ // context->xchgSpi(0xFF);
+ // context->xchgSpi(0xFF);
- BYTE resp = context->xchgSpi(0xFF);
+ // BYTE resp = context->xchgSpi(0xFF);
- if ((resp & 0x1F) != 0x05) {
- // Data packet was not accepted
- return INT_ERROR;
- }
- }
+ // if ((resp & 0x1F) != 0x05) {
+ // // Data packet was not accepted
+ // return INT_ERROR;
+ // }
+ // }
return INT_READY;
}
@@ -190,60 +194,60 @@ static int transmitDataBlock(sdSpiContext_t *context, const BYTE *data, BYTE tok
static BYTE sendCommand(sdSpiContext_t *context, BYTE cmd, DWORD arg) {
BYTE cmdbuff[6];
+ //COMMENTED FIRMWARE
+ // // Send a CMD55 prior to ACMD<n>
+ // if (cmd & 0x80) {
+ // cmd &= 0x7F;
+ // BYTE res = sendCommand(context, CMD55, 0);
- // Send a CMD55 prior to ACMD<n>
- if (cmd & 0x80) {
- cmd &= 0x7F;
- BYTE res = sendCommand(context, CMD55, 0);
-
- if (res > 1) {
- return res;
- }
- }
+ // if (res > 1) {
+ // return res;
+ // }
+ // }
- if (waitForCardReady(context, 500) != INT_READY) {
- return INT_ERROR;
- }
+ // if (waitForCardReady(context, 500) != INT_READY) {
+ // return INT_ERROR;
+ // }
- // Start + command index
- cmdbuff[0] = (0x40 | cmd);
+ // // Start + command index
+ // cmdbuff[0] = (0x40 | cmd);
- // Argument[31..24]
- cmdbuff[1] = ((BYTE)(arg >> 24));
- // Argument[23..16]
- cmdbuff[2] = ((BYTE)(arg >> 16));
- // Argument[15..8]
- cmdbuff[3] = ((BYTE)(arg >> 8));
- // Argument[7..0]
- cmdbuff[4] = ((BYTE)arg);
+ // // Argument[31..24]
+ // cmdbuff[1] = ((BYTE)(arg >> 24));
+ // // Argument[23..16]
+ // cmdbuff[2] = ((BYTE)(arg >> 16));
+ // // Argument[15..8]
+ // cmdbuff[3] = ((BYTE)(arg >> 8));
+ // // Argument[7..0]
+ // cmdbuff[4] = ((BYTE)arg);
- // Dummy CRC + Stop
- BYTE crc = 0x01;
- if (cmd == CMD0) {
- crc = 0x95;
- }
- if (cmd == CMD8) {
- crc = 0x87;
- }
- cmdbuff[5] = (crc);
+ // // Dummy CRC + Stop
+ // BYTE crc = 0x01;
+ // if (cmd == CMD0) {
+ // crc = 0x95;
+ // }
+ // if (cmd == CMD8) {
+ // crc = 0x87;
+ // }
+ // cmdbuff[5] = (crc);
- context->xmitSpiMulti(cmdbuff, 6);
+ // context->xmitSpiMulti(cmdbuff, 6);
- if (cmd == CMD12) {
- // Discard one byte when CMD12
- context->xchgSpi(0xFF);
- }
+ // if (cmd == CMD12) {
+ // // Discard one byte when CMD12
+ // context->xchgSpi(0xFF);
+ // }
// Receive command resp
{
- BYTE maxTries = 10;
- BYTE res;
- do {
- res = context->xchgSpi(0xFF);
- } while ((res & 0x80) && --maxTries);
+ // BYTE maxTries = 10;
+ // BYTE res;
+ // do {
+ // res = context->xchgSpi(0xFF);
+ // } while ((res & 0x80) && --maxTries);
// Return value: R1 resp (bit7==1:Failed to send)
return res;
@@ -253,95 +257,96 @@ static BYTE sendCommand(sdSpiContext_t *context, BYTE cmd, DWORD arg) {
DSTATUS SD_disk_initialize(void* usrOps) {
sdSpiContext_t *context = (sdSpiContext_t *)usrOps;
- BYTE cardType = 0;
-
- if (!usrOps) {
- return RES_PARERR;
- }
-
- context->initSpi();
-
- // Check is card is inserted
- if (context->stat & STA_NODISK) {
- return context->stat;
- }
-
- if (checkPower(context))
- {
- powerOff(context);
- }
-
- context->setSlowSpiMode();
-
- powerOn(context);
-
- if (select(context) == INT_READY)
- {
- // Put the card SPI/Idle state
- if (sendCommand(context, CMD0, 0) == 1) {
- context->timer1 = 10;
-
- // SDv2?
- if (sendCommand(context, CMD8, 0x1AA) == 1) {
- BYTE ocr[4];
-
- // Get 32 bit return value of R7 resp
- for (BYTE n = 0; n < 4; n++) {
- ocr[n] = context->xchgSpi(0xFF);
- }
-
- // Does the card support vcc of 2.7-3.6V?
- if (ocr[2] == 0x01 && ocr[3] == 0xAA) {
- // Wait for end of initialization with ACMD41(HCS)
- while (context->timer1 && sendCommand(context, ACMD41, 1UL << 30)) { /* Do nothing */ }
-
- // Check CCS bit in the OCR
- if (context->timer1 && sendCommand(context, CMD58, 0) == 0) {
- for (BYTE n = 0; n < 4; n++) {
- ocr[n] = context->xchgSpi(0xFF);
- }
-
- // Card id SDv2
- cardType = (ocr[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2;
- }
- }
- } else {
- BYTE cmd;
-
- // SDv1 or MMC?
- if (sendCommand(context, ACMD41, 0) <= 1) {
- // SDv1
- cardType = CT_SD1;
- cmd = ACMD41;
- } else {
- // MMCv3
- cardType = CT_MMC;
- cmd = CMD1;
- }
-
- // Wait for end of initialization
- while (context->timer1 && sendCommand(context, cmd, 0)) { /* Do nothing */ }
-
- // Set block length to 512
- if (!context->timer1 || sendCommand(context, CMD16, 512) != 0) {
- cardType = 0;
- }
- }
- }
-
- context->cardType = cardType;
- deselect(context);
- }
-
- if (cardType) {
- // OK
- context->setFastSpiMode();
-
- // Clear STA_NOINIT flag
- context->stat &= ~STA_NOINIT;
- } else { /* Failed */
- context->stat = STA_NOINIT;
- }
+ //COMMENTED FIRMWARE
+ // BYTE cardType = 0;
+
+ // if (!usrOps) {
+ // return RES_PARERR;
+ // }
+
+ // context->initSpi();
+
+ // // Check is card is inserted
+ // if (context->stat & STA_NODISK) {
+ // return context->stat;
+ // }
+
+ // if (checkPower(context))
+ // {
+ // powerOff(context);
+ // }
+
+ // context->setSlowSpiMode();
+
+ // powerOn(context);
+
+ // if (select(context) == INT_READY)
+ // {
+ // // Put the card SPI/Idle state
+ // if (sendCommand(context, CMD0, 0) == 1) {
+ // context->timer1 = 10;
+
+ // // SDv2?
+ // if (sendCommand(context, CMD8, 0x1AA) == 1) {
+ // BYTE ocr[4];
+
+ // // Get 32 bit return value of R7 resp
+ // for (BYTE n = 0; n < 4; n++) {
+ // ocr[n] = context->xchgSpi(0xFF);
+ // }
+
+ // // Does the card support vcc of 2.7-3.6V?
+ // if (ocr[2] == 0x01 && ocr[3] == 0xAA) {
+ // // Wait for end of initialization with ACMD41(HCS)
+ // while (context->timer1 && sendCommand(context, ACMD41, 1UL << 30)) { /* Do nothing */ }
+
+ // // Check CCS bit in the OCR
+ // if (context->timer1 && sendCommand(context, CMD58, 0) == 0) {
+ // for (BYTE n = 0; n < 4; n++) {
+ // ocr[n] = context->xchgSpi(0xFF);
+ // }
+
+ // // Card id SDv2
+ // cardType = (ocr[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2;
+ // }
+ // }
+ // } else {
+ // BYTE cmd;
+
+ // // SDv1 or MMC?
+ // if (sendCommand(context, ACMD41, 0) <= 1) {
+ // // SDv1
+ // cardType = CT_SD1;
+ // cmd = ACMD41;
+ // } else {
+ // // MMCv3
+ // cardType = CT_MMC;
+ // cmd = CMD1;
+ // }
+
+ // // Wait for end of initialization
+ // while (context->timer1 && sendCommand(context, cmd, 0)) { /* Do nothing */ }
+
+ // // Set block length to 512
+ // if (!context->timer1 || sendCommand(context, CMD16, 512) != 0) {
+ // cardType = 0;
+ // }
+ // }
+ // }
+
+ // context->cardType = cardType;
+ // deselect(context);
+ // }
+
+ // if (cardType) {
+ // // OK
+ // context->setFastSpiMode();
+
+ // // Clear STA_NOINIT flag
+ // context->stat &= ~STA_NOINIT;
+ // } else { /* Failed */
+ // context->stat = STA_NOINIT;
+ // }
return context->stat;
}
@@ -355,243 +360,246 @@ DSTATUS SD_disk_status(void * usrOps) {
DRESULT SD_disk_read(BYTE *buff, DWORD sector, UINT count, void * usrOps) {
- sdSpiContext_t *context = (sdSpiContext_t *)usrOps;
-
- if (!usrOps || !count) {
- return RES_PARERR;
- }
-
- // Check if drive is ready
- if (context->stat & STA_NOINIT) {
- return RES_NOTRDY;
- }
-
- // LBA ot BA conversion (byte addressing cards)
- if (!(context->cardType & CT_BLOCK)) {
- sector *= 512;
- }
-
- if (select(context) == INT_READY)
- {
-
- if (count == 1) {
- // Single sector read
- // READ_SINGLE_BLOCK
- if ((sendCommand(context, CMD17, sector) == 0) && receiveDataBlock(context, buff, 512)) {
- count = 0;
- }
- } else {
- // Multiple sector read
- // READ_MULTIPLE_BLOCK
- if (sendCommand(context, CMD18, sector) == 0) {
- do {
- if (!receiveDataBlock(context, buff, 512)) {
- break;
- }
- buff += 512;
- } while (--count);
-
- // STOP_TRANSMISSION
- sendCommand(context, CMD12, 0);
- }
- }
-
- deselect(context);
- }
+ //COMMENTED FIRMWARE
+ // sdSpiContext_t *context = (sdSpiContext_t *)usrOps;
+
+ // if (!usrOps || !count) {
+ // return RES_PARERR;
+ // }
+
+ // // Check if drive is ready
+ // if (context->stat & STA_NOINIT) {
+ // return RES_NOTRDY;
+ // }
+
+ // // LBA ot BA conversion (byte addressing cards)
+ // if (!(context->cardType & CT_BLOCK)) {
+ // sector *= 512;
+ // }
+
+ // if (select(context) == INT_READY)
+ // {
+
+ // if (count == 1) {
+ // // Single sector read
+ // // READ_SINGLE_BLOCK
+ // if ((sendCommand(context, CMD17, sector) == 0) && receiveDataBlock(context, buff, 512)) {
+ // count = 0;
+ // }
+ // } else {
+ // // Multiple sector read
+ // // READ_MULTIPLE_BLOCK
+ // if (sendCommand(context, CMD18, sector) == 0) {
+ // do {
+ // if (!receiveDataBlock(context, buff, 512)) {
+ // break;
+ // }
+ // buff += 512;
+ // } while (--count);
+
+ // // STOP_TRANSMISSION
+ // sendCommand(context, CMD12, 0);
+ // }
+ // }
+
+ // deselect(context);
+ // }
return count ? RES_ERROR : RES_OK;
}
DRESULT SD_disk_write(const BYTE *buff, DWORD sector, UINT count, void * usrOps) {
- sdSpiContext_t *context = (sdSpiContext_t *)usrOps;
-
- if (!usrOps || !count) {
- return RES_PARERR;
- }
-
- // Check drive status
- if (context->stat & STA_NOINIT) {
- return RES_NOTRDY;
- }
-
- // Check write protect
- if (context->stat & STA_PROTECT) {
- return RES_WRPRT;
- }
-
- // LBA ==> BA conversion (byte addressing cards)
- if (!(context->cardType & CT_BLOCK)) {
- sector *= 512;
- }
-
- if (select(context) == INT_READY)
- {
- if (count == 1) {
- // Single sector write
- // WRITE_BLOCK
- if ((sendCommand(context, CMD24, sector) == 0) && transmitDataBlock(context, buff, 0xFE)) {
- count = 0;
- }
- } else {
- // Multiple sector write
- // Set number of sectors
- if (context->cardType & CT_SDC) sendCommand(context, ACMD23, count);
-
- // WRITE_MULTIPLE_BLOCK
- if (sendCommand(context, CMD25, sector) == 0) {
- do {
- if (!transmitDataBlock(context, buff, 0xFC)) {
- break;
- }
-
- buff += 512;
- } while (--count);
-
- // STOP_TRAN
- if (!transmitDataBlock(context, 0, 0xFD)) {
- count = 1;
- }
- }
- }
-
- deselect(context);
- }
+ //COMMENTED FIRMWARE
+ // sdSpiContext_t *context = (sdSpiContext_t *)usrOps;
+
+ // if (!usrOps || !count) {
+ // return RES_PARERR;
+ // }
+
+ // // Check drive status
+ // if (context->stat & STA_NOINIT) {
+ // return RES_NOTRDY;
+ // }
+
+ // // Check write protect
+ // if (context->stat & STA_PROTECT) {
+ // return RES_WRPRT;
+ // }
+
+ // // LBA ==> BA conversion (byte addressing cards)
+ // if (!(context->cardType & CT_BLOCK)) {
+ // sector *= 512;
+ // }
+
+ // if (select(context) == INT_READY)
+ // {
+ // if (count == 1) {
+ // // Single sector write
+ // // WRITE_BLOCK
+ // if ((sendCommand(context, CMD24, sector) == 0) && transmitDataBlock(context, buff, 0xFE)) {
+ // count = 0;
+ // }
+ // } else {
+ // // Multiple sector write
+ // // Set number of sectors
+ // if (context->cardType & CT_SDC) sendCommand(context, ACMD23, count);
+
+ // // WRITE_MULTIPLE_BLOCK
+ // if (sendCommand(context, CMD25, sector) == 0) {
+ // do {
+ // if (!transmitDataBlock(context, buff, 0xFC)) {
+ // break;
+ // }
+
+ // buff += 512;
+ // } while (--count);
+
+ // // STOP_TRAN
+ // if (!transmitDataBlock(context, 0, 0xFD)) {
+ // count = 1;
+ // }
+ // }
+ // }
+
+ // deselect(context);
+ // }
return count ? RES_ERROR : RES_OK;
}
DRESULT SD_disk_ioctl(BYTE cmd, void* buff, void* usrOps) {
- sdSpiContext_t *context = (sdSpiContext_t *)usrOps;
- BYTE csd[16];
+ //COMMENTED FIRMWARE
+ // sdSpiContext_t *context = (sdSpiContext_t *)usrOps;
+ // BYTE csd[16];
- if (!usrOps) {
- return RES_PARERR;
- }
+ // if (!usrOps) {
+ // return RES_PARERR;
+ // }
- // Check if drive is ready
- if (context->stat & STA_NOINIT) {
- return RES_NOTRDY;
- }
+ // // Check if drive is ready
+ // if (context->stat & STA_NOINIT) {
+ // return RES_NOTRDY;
+ // }
DRESULT res = RES_ERROR;
- if (select(context) == INT_READY)
- {
- switch (cmd) {
- case CTRL_SYNC :
- // Wait for end of internal write process of the drive
- if (waitForCardReady(context, 500)) {
- res = RES_OK;
- }
- break;
-
- case GET_SECTOR_COUNT :
- // Get drive capacity in unit of sector (DWORD)
- if ((sendCommand(context, CMD9, 0) == 0) &&
- receiveDataBlock(context, csd, 16))
- {
- if ((csd[0] >> 6) == 1)
- {
- // SDC ver 2.00
- DWORD csize = csd[9] + ((WORD)csd[8] << 8) + ((DWORD)(csd[7] & 63) << 16) + 1;
- *(DWORD*)buff = csize << 10;
- } else {
- // SDC ver 1.XX or MMC ver 3
- BYTE n = (csd[5] & 15) + ((csd[10] & 128) >> 7) + ((csd[9] & 3) << 1) + 2;
- DWORD csize = (csd[8] >> 6) + ((WORD)csd[7] << 2) + ((WORD)(csd[6] & 3) << 10) + 1;
- *(DWORD*)buff = csize << (n - 9);
- }
- res = RES_OK;
- }
- break;
-
- case GET_BLOCK_SIZE : /* Get erase block size in unit of sector (DWORD) */
- if (context->cardType & CT_SD2)
- {
- // SDC ver 2.00
- if (sendCommand(context, ACMD13, 0) == 0)
- {
- // Read SD status
- context->xchgSpi(0xFF);
- if (receiveDataBlock(context, csd, 16))
- {
- // Read partial block
- for (BYTE n = 64 - 16; n; n--)
- {
- // Purge trailing data
- context->xchgSpi(0xFF);
- }
-
- *(DWORD*)buff = 16UL << (csd[10] >> 4);
- res = RES_OK;
- }
- }
- } else {
- // SDC ver 1.XX or MMC
- if ((sendCommand(context, CMD9, 0) == 0) &&
- receiveDataBlock(context, csd, 16))
- {
- // Read CSD
- if (context->cardType & CT_SD1)
- {
- // SDC ver 1.XX
- *(DWORD*)buff = (((csd[10] & 63) << 1) + ((WORD)(csd[11] & 128) >> 7) + 1) << ((csd[13] >> 6) - 1);
- } else {
- // MMC
- *(DWORD*)buff = ((WORD)((csd[10] & 124) >> 2) + 1) * (((csd[11] & 3) << 3) + ((csd[11] & 224) >> 5) + 1);
- }
-
- res = RES_OK;
- }
- }
- break;
-
- case CTRL_TRIM :
- // Erase a block of sectors (used when _USE_ERASE == 1)
- // Check if the card is SDC
- if (!(context->cardType & CT_SDC)) {
- break;
- }
-
- // Get CSD
- if (SD_disk_ioctl(MMC_GET_CSD, csd, context)) {
- break;
- }
-
- // Check if sector erase can be applied to the card
- if (!(csd[0] >> 6) && !(csd[10] & 0x40)) {
- break;
- }
-
- // Load sector block
- {
- DWORD* dp = buff;
- DWORD st = dp[0];
- DWORD ed = dp[1];
- if (!(context->cardType & CT_BLOCK)) {
- st *= 512;
- ed *= 512;
- }
-
- /* Erase sector block */
- if (sendCommand(context, CMD32, st) == 0 &&
- sendCommand(context, CMD33, ed) == 0 &&
- sendCommand(context, CMD38, 0) == 0 &&
- waitForCardReady(context, 30000)) {
- // FatFs does not check result of this command
- res = RES_OK;
- }
- }
- break;
-
- default:
- res = RES_PARERR;
- }
-
- deselect(context);
- }
+ // if (select(context) == INT_READY)
+ // {
+ // switch (cmd) {
+ // case CTRL_SYNC :
+ // // Wait for end of internal write process of the drive
+ // if (waitForCardReady(context, 500)) {
+ // res = RES_OK;
+ // }
+ // break;
+
+ // case GET_SECTOR_COUNT :
+ // // Get drive capacity in unit of sector (DWORD)
+ // if ((sendCommand(context, CMD9, 0) == 0) &&
+ // receiveDataBlock(context, csd, 16))
+ // {
+ // if ((csd[0] >> 6) == 1)
+ // {
+ // // SDC ver 2.00
+ // DWORD csize = csd[9] + ((WORD)csd[8] << 8) + ((DWORD)(csd[7] & 63) << 16) + 1;
+ // *(DWORD*)buff = csize << 10;
+ // } else {
+ // // SDC ver 1.XX or MMC ver 3
+ // BYTE n = (csd[5] & 15) + ((csd[10] & 128) >> 7) + ((csd[9] & 3) << 1) + 2;
+ // DWORD csize = (csd[8] >> 6) + ((WORD)csd[7] << 2) + ((WORD)(csd[6] & 3) << 10) + 1;
+ // *(DWORD*)buff = csize << (n - 9);
+ // }
+ // res = RES_OK;
+ // }
+ // break;
+
+ // case GET_BLOCK_SIZE : /* Get erase block size in unit of sector (DWORD) */
+ // if (context->cardType & CT_SD2)
+ // {
+ // // SDC ver 2.00
+ // if (sendCommand(context, ACMD13, 0) == 0)
+ // {
+ // // Read SD status
+ // context->xchgSpi(0xFF);
+ // if (receiveDataBlock(context, csd, 16))
+ // {
+ // // Read partial block
+ // for (BYTE n = 64 - 16; n; n--)
+ // {
+ // // Purge trailing data
+ // context->xchgSpi(0xFF);
+ // }
+
+ // *(DWORD*)buff = 16UL << (csd[10] >> 4);
+ // res = RES_OK;
+ // }
+ // }
+ // } else {
+ // // SDC ver 1.XX or MMC
+ // if ((sendCommand(context, CMD9, 0) == 0) &&
+ // receiveDataBlock(context, csd, 16))
+ // {
+ // // Read CSD
+ // if (context->cardType & CT_SD1)
+ // {
+ // // SDC ver 1.XX
+ // *(DWORD*)buff = (((csd[10] & 63) << 1) + ((WORD)(csd[11] & 128) >> 7) + 1) << ((csd[13] >> 6) - 1);
+ // } else {
+ // // MMC
+ // *(DWORD*)buff = ((WORD)((csd[10] & 124) >> 2) + 1) * (((csd[11] & 3) << 3) + ((csd[11] & 224) >> 5) + 1);
+ // }
+
+ // res = RES_OK;
+ // }
+ // }
+ // break;
+
+ // case CTRL_TRIM :
+ // // Erase a block of sectors (used when _USE_ERASE == 1)
+ // // Check if the card is SDC
+ // if (!(context->cardType & CT_SDC)) {
+ // break;
+ // }
+
+ // // Get CSD
+ // if (SD_disk_ioctl(MMC_GET_CSD, csd, context)) {
+ // break;
+ // }
+
+ // // Check if sector erase can be applied to the card
+ // if (!(csd[0] >> 6) && !(csd[10] & 0x40)) {
+ // break;
+ // }
+
+ // // Load sector block
+ // {
+ // DWORD* dp = buff;
+ // DWORD st = dp[0];
+ // DWORD ed = dp[1];
+ // if (!(context->cardType & CT_BLOCK)) {
+ // st *= 512;
+ // ed *= 512;
+ // }
+
+ // /* Erase sector block */
+ // if (sendCommand(context, CMD32, st) == 0 &&
+ // sendCommand(context, CMD33, ed) == 0 &&
+ // sendCommand(context, CMD38, 0) == 0 &&
+ // waitForCardReady(context, 30000)) {
+ // // FatFs does not check result of this command
+ // res = RES_OK;
+ // }
+ // }
+ // break;
+
+ // default:
+ // res = RES_PARERR;
+ // }
+
+ // deselect(context);
+ // }
return res;
}
@@ -601,29 +609,29 @@ DRESULT SD_disk_ioctl(BYTE cmd, void* buff, void* usrOps) {
// Changed to 100ms to relax timers as all waits are 200ms or more --TA
void SD_disk_timerproc (void* usrOps) {
sdSpiContext_t *context = (sdSpiContext_t *)usrOps;
-
- {
- WORD n = context->timer1;
- if (n) {
- context->timer1 = --n;
- }
- }
-
- {
- WORD n = context->timer2;
- if (n) {
- context->timer2 = --n;
- }
- }
+ //COMMENTED FIRMWARE
+ // {
+ // WORD n = context->timer1;
+ // if (n) {
+ // context->timer1 = --n;
+ // }
+ // }
+
+ // {
+ // WORD n = context->timer2;
+ // if (n) {
+ // context->timer2 = --n;
+ // }
+ // }
BYTE s = context->stat;
- // Write enabled
- s &= ~STA_PROTECT;
+ // // Write enabled
+ // s &= ~STA_PROTECT;
- // Card is in socket
- s &= ~STA_NODISK;
+ // // Card is in socket
+ // s &= ~STA_NODISK;
context->stat = s;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/hmc5883l.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/hmc5883l.c
index a77e22720a740c4712cc6ca9dfecbca90dbf679d..da45f4a516aa91988cb5eb49f85859bf6da5d71d 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/hmc5883l.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/hmc5883l.c
@@ -57,18 +57,18 @@ void hmc5883lInit(I2C_Dev *i2cPort)
{
if (isInit)
return;
+ //COMMENTED FIRMWARE
+ // I2Cx = i2cPort;
+ // devAddr = HMC5883L_ADDRESS;
- I2Cx = i2cPort;
- devAddr = HMC5883L_ADDRESS;
+ // // write CONFIG_A register
+ // i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A,
+ // (HMC5883L_AVERAGING_8 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
+ // (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
+ // (HMC5883L_BIAS_NORMAL << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));
- // write CONFIG_A register
- i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A,
- (HMC5883L_AVERAGING_8 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
- (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
- (HMC5883L_BIAS_NORMAL << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));
-
- // write CONFIG_B register
- hmc5883lSetGain(HMC5883L_GAIN_660);
+ // // write CONFIG_B register
+ // hmc5883lSetGain(HMC5883L_GAIN_660);
isInit = true;
}
@@ -79,10 +79,11 @@ void hmc5883lInit(I2C_Dev *i2cPort)
*/
bool hmc5883lTestConnection()
{
- if (i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_ID_A, 3, buffer))
- {
- return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3');
- }
+ //COMMENTED FIRMWARE
+ // if (i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_ID_A, 3, buffer))
+ // {
+ // return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3');
+ // }
return false;
}
@@ -93,66 +94,67 @@ bool hmc5883lTestConnection()
bool hmc5883lSelfTest()
{
bool testStatus = true;
- int16_t mxp, myp, mzp; // positive magnetometer measurements
- int16_t mxn, myn, mzn; // negative magnetometer measurements
- struct
- {
- uint8_t configA;
- uint8_t configB;
- uint8_t mode;
- } regSave;
-
- // Save register values
- if (i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, sizeof(regSave), (uint8_t *)®Save) == false)
- {
- // TODO: error handling
- return false;
- }
- // Set gain (sensitivity)
- hmc5883lSetGain(HMC5883L_ST_GAIN);
-
- // Write CONFIG_A register and do positive test
- i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A,
- (HMC5883L_AVERAGING_1 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
- (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
- (HMC5883L_BIAS_POSITIVE << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));
-
- /* Perform test measurement & check results */
- hmc5883lSetMode(HMC5883L_MODE_SINGLE);
- vTaskDelay(M2T(HMC5883L_ST_DELAY_MS));
- hmc5883lGetHeading(&mxp, &myp, &mzp);
-
- // Write CONFIG_A register and do negative test
- i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A,
- (HMC5883L_AVERAGING_1 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
- (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
- (HMC5883L_BIAS_NEGATIVE << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));
-
- /* Perform test measurement & check results */
- hmc5883lSetMode(HMC5883L_MODE_SINGLE);
- vTaskDelay(M2T(HMC5883L_ST_DELAY_MS));
- hmc5883lGetHeading(&mxn, &myn, &mzn);
-
- if (hmc5883lEvaluateSelfTest(HMC5883L_ST_X_MIN, HMC5883L_ST_X_MAX, mxp, "pos X") &&
- hmc5883lEvaluateSelfTest(HMC5883L_ST_Y_MIN, HMC5883L_ST_Y_MAX, myp, "pos Y") &&
- hmc5883lEvaluateSelfTest(HMC5883L_ST_Z_MIN, HMC5883L_ST_Z_MAX, mzp, "pos Z") &&
- hmc5883lEvaluateSelfTest(-HMC5883L_ST_X_MAX, -HMC5883L_ST_X_MIN, mxn, "neg X") &&
- hmc5883lEvaluateSelfTest(-HMC5883L_ST_Y_MAX, -HMC5883L_ST_Y_MIN, myn, "neg Y") &&
- hmc5883lEvaluateSelfTest(-HMC5883L_ST_Z_MAX, -HMC5883L_ST_Z_MIN, mzn, "neg Z"))
- {
- DEBUG_PRINT("Self test [OK].\n");
- }
- else
- {
- testStatus = false;
- }
-
- // Restore registers
- if (i2cdevWriteReg8(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, sizeof(regSave), (uint8_t *)®Save) == false)
- {
- // TODO: error handling
- return false;
- }
+ //COMMENTED FIRMWARE
+ // int16_t mxp, myp, mzp; // positive magnetometer measurements
+ // int16_t mxn, myn, mzn; // negative magnetometer measurements
+ // struct
+ // {
+ // uint8_t configA;
+ // uint8_t configB;
+ // uint8_t mode;
+ // } regSave;
+
+ // // Save register values
+ // if (i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, sizeof(regSave), (uint8_t *)®Save) == false)
+ // {
+ // // TODO: error handling
+ // return false;
+ // }
+ // // Set gain (sensitivity)
+ // hmc5883lSetGain(HMC5883L_ST_GAIN);
+
+ // // Write CONFIG_A register and do positive test
+ // i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A,
+ // (HMC5883L_AVERAGING_1 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
+ // (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
+ // (HMC5883L_BIAS_POSITIVE << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));
+
+ // /* Perform test measurement & check results */
+ // hmc5883lSetMode(HMC5883L_MODE_SINGLE);
+ // vTaskDelay(M2T(HMC5883L_ST_DELAY_MS));
+ // hmc5883lGetHeading(&mxp, &myp, &mzp);
+
+ // // Write CONFIG_A register and do negative test
+ // i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A,
+ // (HMC5883L_AVERAGING_1 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
+ // (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
+ // (HMC5883L_BIAS_NEGATIVE << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));
+
+ // /* Perform test measurement & check results */
+ // hmc5883lSetMode(HMC5883L_MODE_SINGLE);
+ // vTaskDelay(M2T(HMC5883L_ST_DELAY_MS));
+ // hmc5883lGetHeading(&mxn, &myn, &mzn);
+
+ // if (hmc5883lEvaluateSelfTest(HMC5883L_ST_X_MIN, HMC5883L_ST_X_MAX, mxp, "pos X") &&
+ // hmc5883lEvaluateSelfTest(HMC5883L_ST_Y_MIN, HMC5883L_ST_Y_MAX, myp, "pos Y") &&
+ // hmc5883lEvaluateSelfTest(HMC5883L_ST_Z_MIN, HMC5883L_ST_Z_MAX, mzp, "pos Z") &&
+ // hmc5883lEvaluateSelfTest(-HMC5883L_ST_X_MAX, -HMC5883L_ST_X_MIN, mxn, "neg X") &&
+ // hmc5883lEvaluateSelfTest(-HMC5883L_ST_Y_MAX, -HMC5883L_ST_Y_MIN, myn, "neg Y") &&
+ // hmc5883lEvaluateSelfTest(-HMC5883L_ST_Z_MAX, -HMC5883L_ST_Z_MIN, mzn, "neg Z"))
+ // {
+ // DEBUG_PRINT("Self test [OK].\n");
+ // }
+ // else
+ // {
+ // testStatus = false;
+ // }
+
+ // // Restore registers
+ // if (i2cdevWriteReg8(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, sizeof(regSave), (uint8_t *)®Save) == false)
+ // {
+ // // TODO: error handling
+ // return false;
+ // }
return testStatus;
}
@@ -166,12 +168,13 @@ bool hmc5883lSelfTest()
*/
bool hmc5883lEvaluateSelfTest(int16_t min, int16_t max, int16_t value, char* string)
{
- if (value < min || value > max)
- {
- DEBUG_PRINT("Self test %s [FAIL]. low: %d, high: %d, measured: %d\n",
- string, min, max, value);
- return false;
- }
+ //COMMENTED FIRMWARE
+ // if (value < min || value > max)
+ // {
+ // DEBUG_PRINT("Self test %s [FAIL]. low: %d, high: %d, measured: %d\n",
+ // string, min, max, value);
+ // return false;
+ // }
return true;
}
@@ -186,7 +189,8 @@ bool hmc5883lEvaluateSelfTest(int16_t min, int16_t max, int16_t value, char* str
*/
uint8_t hmc5883lGetSampleAveraging()
{
- i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, buffer);
return buffer[0];
}
/** Set number of samples averaged per measurement.
@@ -197,7 +201,8 @@ uint8_t hmc5883lGetSampleAveraging()
*/
void hmc5883lSetSampleAveraging(uint8_t averaging)
{
- i2cdevWriteBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, averaging);
+ //COMMENTED FIRMWARE
+ //i2cdevWriteBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, averaging);
}
/** Get data output rate value.
* The Table below shows all selectable output rates in continuous measurement
@@ -224,7 +229,8 @@ void hmc5883lSetSampleAveraging(uint8_t averaging)
*/
uint8_t hmc5883lGetDataRate()
{
- i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, buffer);
return buffer[0];
}
/** Set data output rate value.
@@ -237,7 +243,8 @@ uint8_t hmc5883lGetDataRate()
*/
void hmc5883lSetDataRate(uint8_t rate)
{
- i2cdevWriteBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, rate);
+ //COMMENTED FIRMWARE
+ //i2cdevWriteBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, rate);
}
/** Get measurement bias value.
* @return Current bias value (0-2 for normal/positive/negative respectively)
@@ -248,7 +255,8 @@ void hmc5883lSetDataRate(uint8_t rate)
*/
uint8_t hmc5883lGetMeasurementBias()
{
- i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, buffer);
return buffer[0];
}
/** Set measurement bias value.
@@ -260,7 +268,8 @@ uint8_t hmc5883lGetMeasurementBias()
*/
void hmc5883lSetMeasurementBias(uint8_t bias)
{
- i2cdevWriteBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, bias);
+ //COMMENTED FIRMWARE
+ //i2cdevWriteBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, bias);
}
// CONFIG_B register
@@ -290,7 +299,8 @@ void hmc5883lSetMeasurementBias(uint8_t bias)
*/
uint8_t hmc5883lGetGain()
{
- i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_B, HMC5883L_CRB_GAIN_BIT, HMC5883L_CRB_GAIN_LENGTH, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_B, HMC5883L_CRB_GAIN_BIT, HMC5883L_CRB_GAIN_LENGTH, buffer);
return buffer[0];
}
/** Set magnetic field gain value.
@@ -305,7 +315,8 @@ void hmc5883lSetGain(uint8_t gain)
// use this method to guarantee that bits 4-0 are set to zero, which is a
// requirement specified in the datasheet; it's actually more efficient than
// using the I2Cdev.writeBits method
- i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1));
+ //COMMENTED FIRMWARE
+ //i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1));
}
// MODE register
@@ -334,7 +345,8 @@ void hmc5883lSetGain(uint8_t gain)
*/
uint8_t hmc5883lGetMode()
{
- i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODEREG_BIT, HMC5883L_MODEREG_LENGTH, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODEREG_BIT, HMC5883L_MODEREG_LENGTH, buffer);
return buffer[0];
}
/** Set measurement mode.
@@ -352,8 +364,9 @@ void hmc5883lSetMode(uint8_t newMode)
// use this method to guarantee that bits 7-2 are set to zero, which is a
// requirement specified in the datasheet; it's actually more efficient than
// using the I2Cdev.writeBits method
- i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, mode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
- mode = newMode;// track to tell if we have to clear bit 7 after a read
+ //COMMENTED FIRMWARE
+ // i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, mode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+ // mode = newMode;// track to tell if we have to clear bit 7 after a read
}
// DATA* registers
@@ -371,11 +384,12 @@ void hmc5883lSetMode(uint8_t newMode)
*/
void hmc5883lGetHeading(int16_t *x, int16_t *y, int16_t *z)
{
- i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
- if (mode == HMC5883L_MODE_SINGLE) i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
- *x = (((int16_t)buffer[0]) << 8) | buffer[1];
- *y = (((int16_t)buffer[4]) << 8) | buffer[5];
- *z = (((int16_t)buffer[2]) << 8) | buffer[3];
+ //COMMENTED FIRMWARE
+ // i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+ // if (mode == HMC5883L_MODE_SINGLE) i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+ // *x = (((int16_t)buffer[0]) << 8) | buffer[1];
+ // *y = (((int16_t)buffer[4]) << 8) | buffer[5];
+ // *z = (((int16_t)buffer[2]) << 8) | buffer[3];
}
/** Get X-axis heading measurement.
* @return 16-bit signed integer with X-axis heading
@@ -385,8 +399,9 @@ int16_t hmc5883lGetHeadingX()
{
// each axis read requires that ALL axis registers be read, even if only
// one is used; this was not done ineffiently in the code by accident
- i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
- if (mode == HMC5883L_MODE_SINGLE) i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+ //COMMENTED FIRMWARE
+ // i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+ // if (mode == HMC5883L_MODE_SINGLE) i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
/** Get Y-axis heading measurement.
@@ -397,8 +412,9 @@ int16_t hmc5883lGetHeadingY()
{
// each axis read requires that ALL axis registers be read, even if only
// one is used; this was not done ineffiently in the code by accident
- i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
- if (mode == HMC5883L_MODE_SINGLE) i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+ //COMMENTED FIRMWARE
+ // i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+ // if (mode == HMC5883L_MODE_SINGLE) i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
return (((int16_t)buffer[4]) << 8) | buffer[5];
}
/** Get Z-axis heading measurement.
@@ -409,8 +425,9 @@ int16_t hmc5883lGetHeadingZ()
{
// each axis read requires that ALL axis registers be read, even if only
// one is used; this was not done ineffiently in the code by accident
- i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
- if (mode == HMC5883L_MODE_SINGLE) i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+ //COMMENTED FIRMWARE
+ // i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+ // if (mode == HMC5883L_MODE_SINGLE) i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
return (((int16_t)buffer[2]) << 8) | buffer[3];
}
@@ -429,7 +446,8 @@ int16_t hmc5883lGetHeadingZ()
*/
bool hmc5883lGetLockStatus()
{
- i2cdevReadBit(I2Cx, devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_LOCK_BIT, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadBit(I2Cx, devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_LOCK_BIT, buffer);
return buffer[0];
}
/** Get data ready status.
@@ -444,7 +462,8 @@ bool hmc5883lGetLockStatus()
*/
bool hmc5883lGetReadyStatus()
{
- i2cdevReadBit(I2Cx, devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_READY_BIT, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadBit(I2Cx, devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_READY_BIT, buffer);
return buffer[0];
}
@@ -455,7 +474,8 @@ bool hmc5883lGetReadyStatus()
*/
uint8_t hmc5883lGetIDA()
{
- i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_A, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_A, buffer);
return buffer[0];
}
/** Get identification byte B
@@ -463,7 +483,8 @@ uint8_t hmc5883lGetIDA()
*/
uint8_t hmc5883lGetIDB()
{
- i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_B, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_B, buffer);
return buffer[0];
}
/** Get identification byte C
@@ -471,6 +492,7 @@ uint8_t hmc5883lGetIDB()
*/
uint8_t hmc5883lGetIDC()
{
- i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_C, buffer);
+ //COMMENTED FIRMWARE
+ //i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_C, buffer);
return buffer[0];
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2c_drv.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2c_drv.c
index 79bd7ed9ec1a89d04117d7c84e130397c21a38d4..869a29ba6596f4492d22487988a831942ff5ba80 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2c_drv.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2c_drv.c
@@ -221,61 +221,62 @@ static void i2cNotifyClient(I2cDrv* i2c)
static void i2cdrvTryToRestartBus(I2cDrv* i2c)
{
- I2C_InitTypeDef I2C_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
- GPIO_InitTypeDef GPIO_InitStructure;
-
- // Enable GPIOA clock
- RCC_AHB1PeriphClockCmd(i2c->def->gpioSDAPerif, ENABLE);
- RCC_AHB1PeriphClockCmd(i2c->def->gpioSCLPerif, ENABLE);
- // Enable I2C_SENSORS clock
- RCC_APB1PeriphClockCmd(i2c->def->i2cPerif, ENABLE);
-
- // Configure I2C_SENSORS pins to unlock bus.
- GPIO_StructInit(&GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
- GPIO_InitStructure.GPIO_Pin = i2c->def->gpioSCLPin; // SCL
- GPIO_Init(i2c->def->gpioSCLPort, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = i2c->def->gpioSDAPin; // SDA
- GPIO_Init(i2c->def->gpioSDAPort, &GPIO_InitStructure);
-
- i2cdrvdevUnlockBus(i2c->def->gpioSCLPort, i2c->def->gpioSDAPort, i2c->def->gpioSCLPin, i2c->def->gpioSDAPin);
-
- // Configure I2C_SENSORS pins for AF.
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- GPIO_InitStructure.GPIO_Pin = i2c->def->gpioSCLPin; // SCL
- GPIO_Init(i2c->def->gpioSCLPort, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = i2c->def->gpioSDAPin; // SDA
- GPIO_Init(i2c->def->gpioSDAPort, &GPIO_InitStructure);
-
- //Map gpios to alternate functions
- GPIO_PinAFConfig(i2c->def->gpioSCLPort, i2c->def->gpioSCLPinSource, i2c->def->gpioAF);
- GPIO_PinAFConfig(i2c->def->gpioSDAPort, i2c->def->gpioSDAPinSource, i2c->def->gpioAF);
-
- // I2C_SENSORS configuration
- I2C_DeInit(i2c->def->i2cPort);
- I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
- I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
- I2C_InitStructure.I2C_OwnAddress1 = I2C_SLAVE_ADDRESS7;
- I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
- I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
- I2C_InitStructure.I2C_ClockSpeed = i2c->def->i2cClockSpeed;
- I2C_Init(i2c->def->i2cPort, &I2C_InitStructure);
-
- // Enable I2C_SENSORS error interrupts
- I2C_ITConfig(i2c->def->i2cPort, I2C_IT_ERR, ENABLE);
-
- NVIC_InitStructure.NVIC_IRQChannel = i2c->def->i2cEVIRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannel = i2c->def->i2cERIRQn;
- NVIC_Init(&NVIC_InitStructure);
-
- i2cdrvDmaSetupBus(i2c);
+ //COMMENTED FIRMWARE
+ // I2C_InitTypeDef I2C_InitStructure;
+ // NVIC_InitTypeDef NVIC_InitStructure;
+ // GPIO_InitTypeDef GPIO_InitStructure;
+
+ // // Enable GPIOA clock
+ // RCC_AHB1PeriphClockCmd(i2c->def->gpioSDAPerif, ENABLE);
+ // RCC_AHB1PeriphClockCmd(i2c->def->gpioSCLPerif, ENABLE);
+ // // Enable I2C_SENSORS clock
+ // RCC_APB1PeriphClockCmd(i2c->def->i2cPerif, ENABLE);
+
+ // // Configure I2C_SENSORS pins to unlock bus.
+ // GPIO_StructInit(&GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
+ // GPIO_InitStructure.GPIO_Pin = i2c->def->gpioSCLPin; // SCL
+ // GPIO_Init(i2c->def->gpioSCLPort, &GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Pin = i2c->def->gpioSDAPin; // SDA
+ // GPIO_Init(i2c->def->gpioSDAPort, &GPIO_InitStructure);
+
+ // i2cdrvdevUnlockBus(i2c->def->gpioSCLPort, i2c->def->gpioSDAPort, i2c->def->gpioSCLPin, i2c->def->gpioSDAPin);
+
+ // // Configure I2C_SENSORS pins for AF.
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ // GPIO_InitStructure.GPIO_Pin = i2c->def->gpioSCLPin; // SCL
+ // GPIO_Init(i2c->def->gpioSCLPort, &GPIO_InitStructure);
+ // GPIO_InitStructure.GPIO_Pin = i2c->def->gpioSDAPin; // SDA
+ // GPIO_Init(i2c->def->gpioSDAPort, &GPIO_InitStructure);
+
+ // //Map gpios to alternate functions
+ // GPIO_PinAFConfig(i2c->def->gpioSCLPort, i2c->def->gpioSCLPinSource, i2c->def->gpioAF);
+ // GPIO_PinAFConfig(i2c->def->gpioSDAPort, i2c->def->gpioSDAPinSource, i2c->def->gpioAF);
+
+ // // I2C_SENSORS configuration
+ // I2C_DeInit(i2c->def->i2cPort);
+ // I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
+ // I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
+ // I2C_InitStructure.I2C_OwnAddress1 = I2C_SLAVE_ADDRESS7;
+ // I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
+ // I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
+ // I2C_InitStructure.I2C_ClockSpeed = i2c->def->i2cClockSpeed;
+ // I2C_Init(i2c->def->i2cPort, &I2C_InitStructure);
+
+ // // Enable I2C_SENSORS error interrupts
+ // I2C_ITConfig(i2c->def->i2cPort, I2C_IT_ERR, ENABLE);
+
+ // NVIC_InitStructure.NVIC_IRQChannel = i2c->def->i2cEVIRQn;
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ // NVIC_Init(&NVIC_InitStructure);
+ // NVIC_InitStructure.NVIC_IRQChannel = i2c->def->i2cERIRQn;
+ // NVIC_Init(&NVIC_InitStructure);
+
+ // i2cdrvDmaSetupBus(i2c);
}
static void i2cdrvDmaSetupBus(I2cDrv* i2c)
@@ -401,26 +402,27 @@ bool i2cdrvMessageTransfer(I2cDrv* i2c, I2cMessage* message)
{
bool status = false;
- xSemaphoreTake(i2c->isBusFreeMutex, portMAX_DELAY); // Protect message data
- // Copy message
- memcpy((char*)&i2c->txMessage, (char*)message, sizeof(I2cMessage));
- // We can now start the ISR sending this message.
- i2cdrvStartTransfer(i2c);
- // Wait for transaction to be done
- if (xSemaphoreTake(i2c->isBusFreeSemaphore, I2C_MESSAGE_TIMEOUT) == pdTRUE)
- {
- if (i2c->txMessage.status == i2cAck)
- {
- status = true;
- }
- }
- else
- {
- i2cdrvClearDMA(i2c);
- i2cdrvTryToRestartBus(i2c);
- //TODO: If bus is really hanged... fail safe
- }
- xSemaphoreGive(i2c->isBusFreeMutex);
+ //COMMENTED FIRMWARE
+ // xSemaphoreTake(i2c->isBusFreeMutex, portMAX_DELAY); // Protect message data
+ // // Copy message
+ // memcpy((char*)&i2c->txMessage, (char*)message, sizeof(I2cMessage));
+ // // We can now start the ISR sending this message.
+ // i2cdrvStartTransfer(i2c);
+ // // Wait for transaction to be done
+ // if (xSemaphoreTake(i2c->isBusFreeSemaphore, I2C_MESSAGE_TIMEOUT) == pdTRUE)
+ // {
+ // if (i2c->txMessage.status == i2cAck)
+ // {
+ // status = true;
+ // }
+ // }
+ // else
+ // {
+ // i2cdrvClearDMA(i2c);
+ // i2cdrvTryToRestartBus(i2c);
+ // //TODO: If bus is really hanged... fail safe
+ // }
+ // xSemaphoreGive(i2c->isBusFreeMutex);
return status;
}
@@ -428,210 +430,214 @@ bool i2cdrvMessageTransfer(I2cDrv* i2c, I2cMessage* message)
static void i2cdrvEventIsrHandler(I2cDrv* i2c)
{
- uint16_t SR1;
- uint16_t SR2;
-
- // read the status register first
- SR1 = i2c->def->i2cPort->SR1;
-
-#ifdef I2CDRV_DEBUG_LOG_EVENTS
- // Debug code
- eventDebug[eventPos][0] = usecTimestamp();
- eventDebug[eventPos][1] = SR1;
- if (++eventPos == 1024)
- {
- eventPos = 0;
- }
-#endif
-
- // Start bit event
- if (SR1 & I2C_SR1_SB)
- {
- i2c->messageIndex = 0;
-
- if(i2c->txMessage.direction == i2cWrite ||
- i2c->txMessage.internalAddress != I2C_NO_INTERNAL_ADDRESS)
- {
- I2C_Send7bitAddress(i2c->def->i2cPort, i2c->txMessage.slaveAddress << 1, I2C_Direction_Transmitter);
- }
- else
- {
- I2C_AcknowledgeConfig(i2c->def->i2cPort, ENABLE);
- I2C_Send7bitAddress(i2c->def->i2cPort, i2c->txMessage.slaveAddress << 1, I2C_Direction_Receiver);
- }
- }
- // Address event
- else if (SR1 & I2C_SR1_ADDR)
- {
- if(i2c->txMessage.direction == i2cWrite ||
- i2c->txMessage.internalAddress != I2C_NO_INTERNAL_ADDRESS)
- {
- SR2 = i2c->def->i2cPort->SR2; // clear ADDR
- // In write mode transmit is always empty so can send up to two bytes
- if (i2c->txMessage.internalAddress != I2C_NO_INTERNAL_ADDRESS)
- {
- if (i2c->txMessage.isInternal16bit)
- {
- I2C_SendData(i2c->def->i2cPort, (i2c->txMessage.internalAddress & 0xFF00) >> 8);
- I2C_SendData(i2c->def->i2cPort, (i2c->txMessage.internalAddress & 0x00FF));
- }
- else
- {
- I2C_SendData(i2c->def->i2cPort, (i2c->txMessage.internalAddress & 0x00FF));
- }
- i2c->txMessage.internalAddress = I2C_NO_INTERNAL_ADDRESS;
- }
- I2C_ITConfig(i2c->def->i2cPort, I2C_IT_BUF, ENABLE); // allow us to have an EV7
- }
- else // Reading, start DMA transfer
- {
- if(i2c->txMessage.messageLength == 1)
- {
- I2C_AcknowledgeConfig(i2c->def->i2cPort, DISABLE);
- }
- else
- {
- I2C_DMALastTransferCmd(i2c->def->i2cPort, ENABLE); // No repetitive start
- }
- // Disable buffer I2C interrupts
- I2C_ITConfig(i2c->def->i2cPort, I2C_IT_EVT | I2C_IT_BUF, DISABLE);
- // Enable the Transfer Complete interrupt
- DMA_ITConfig(i2c->def->dmaRxStream, DMA_IT_TC | DMA_IT_TE, ENABLE);
- I2C_DMACmd(i2c->def->i2cPort, ENABLE); // Enable before ADDR clear
-
- // Workaround to enable DMA for Renode simulation.
- // The I2C uses the DMA for reading, but the Renode implementation lacks some functionality
- // and for a message to be read the DMA needs to be enabled manually.
- // Without setting this bit the I2C reading fails.
- // With added functionality it should be possible to remove.
- DMA_Cmd(i2c->def->dmaRxStream, ENABLE); // Workaround
-
- __DMB(); // Make sure instructions (clear address) are in correct order
- SR2 = i2c->def->i2cPort->SR2; // clear ADDR
- }
- }
- // Byte transfer finished
- else if (SR1 & I2C_SR1_BTF)
- {
- SR2 = i2c->def->i2cPort->SR2;
- if (SR2 & I2C_SR2_TRA) // In write mode?
- {
- if (i2c->txMessage.direction == i2cRead) // internal address read
- {
- /* Internal address written, switch to read */
- i2c->def->i2cPort->CR1 = (I2C_CR1_START | I2C_CR1_PE); // Generate start
- }
- else
- {
- i2cNotifyClient(i2c);
- // Are there any other messages to transact? If so stop else repeated start.
- i2cTryNextMessage(i2c);
- }
- }
- else // Reading. Shouldn't happen since we use DMA for reading.
- {
- ASSERT(1);
- i2c->txMessage.buffer[i2c->messageIndex++] = I2C_ReceiveData(i2c->def->i2cPort);
- if(i2c->messageIndex == i2c->txMessage.messageLength)
- {
- i2cNotifyClient(i2c);
- // Are there any other messages to transact?
- i2cTryNextMessage(i2c);
- }
- }
- // A second BTF interrupt might occur if we don't wait for it to clear.
- // TODO Implement better method.
- while (i2c->def->i2cPort->CR1 & 0x0100) { ; }
- }
- // Byte received
- else if (SR1 & I2C_SR1_RXNE) // Should not happen when we use DMA for reception.
- {
- i2c->txMessage.buffer[i2c->messageIndex++] = I2C_ReceiveData(i2c->def->i2cPort);
- if(i2c->messageIndex == i2c->txMessage.messageLength)
- {
- I2C_ITConfig(i2c->def->i2cPort, I2C_IT_BUF, DISABLE); // disable RXE to get BTF
- }
- }
- // Byte ready to be transmitted
- else if (SR1 & I2C_SR1_TXE)
- {
- if (i2c->txMessage.direction == i2cRead)
- {
- // Disable TXE to flush and get BTF to switch to read.
- // Switch must be done in BTF or strange things happen.
- I2C_ITConfig(i2c->def->i2cPort, I2C_IT_BUF, DISABLE);
- }
- else
- {
- I2C_SendData(i2c->def->i2cPort, i2c->txMessage.buffer[i2c->messageIndex++]);
- if(i2c->messageIndex == i2c->txMessage.messageLength)
- {
- // Disable TXE to allow the buffer to flush and get BTF
- I2C_ITConfig(i2c->def->i2cPort, I2C_IT_BUF, DISABLE);
- }
- }
- }
+ //COMMENTED FIRMWARE
+// uint16_t SR1;
+// uint16_t SR2;
+
+// // read the status register first
+// SR1 = i2c->def->i2cPort->SR1;
+
+// #ifdef I2CDRV_DEBUG_LOG_EVENTS
+// // Debug code
+// eventDebug[eventPos][0] = usecTimestamp();
+// eventDebug[eventPos][1] = SR1;
+// if (++eventPos == 1024)
+// {
+// eventPos = 0;
+// }
+// #endif
+
+// // Start bit event
+// if (SR1 & I2C_SR1_SB)
+// {
+// i2c->messageIndex = 0;
+
+// if(i2c->txMessage.direction == i2cWrite ||
+// i2c->txMessage.internalAddress != I2C_NO_INTERNAL_ADDRESS)
+// {
+// I2C_Send7bitAddress(i2c->def->i2cPort, i2c->txMessage.slaveAddress << 1, I2C_Direction_Transmitter);
+// }
+// else
+// {
+// I2C_AcknowledgeConfig(i2c->def->i2cPort, ENABLE);
+// I2C_Send7bitAddress(i2c->def->i2cPort, i2c->txMessage.slaveAddress << 1, I2C_Direction_Receiver);
+// }
+// }
+// // Address event
+// else if (SR1 & I2C_SR1_ADDR)
+// {
+// if(i2c->txMessage.direction == i2cWrite ||
+// i2c->txMessage.internalAddress != I2C_NO_INTERNAL_ADDRESS)
+// {
+// SR2 = i2c->def->i2cPort->SR2; // clear ADDR
+// // In write mode transmit is always empty so can send up to two bytes
+// if (i2c->txMessage.internalAddress != I2C_NO_INTERNAL_ADDRESS)
+// {
+// if (i2c->txMessage.isInternal16bit)
+// {
+// I2C_SendData(i2c->def->i2cPort, (i2c->txMessage.internalAddress & 0xFF00) >> 8);
+// I2C_SendData(i2c->def->i2cPort, (i2c->txMessage.internalAddress & 0x00FF));
+// }
+// else
+// {
+// I2C_SendData(i2c->def->i2cPort, (i2c->txMessage.internalAddress & 0x00FF));
+// }
+// i2c->txMessage.internalAddress = I2C_NO_INTERNAL_ADDRESS;
+// }
+// I2C_ITConfig(i2c->def->i2cPort, I2C_IT_BUF, ENABLE); // allow us to have an EV7
+// }
+// else // Reading, start DMA transfer
+// {
+// if(i2c->txMessage.messageLength == 1)
+// {
+// I2C_AcknowledgeConfig(i2c->def->i2cPort, DISABLE);
+// }
+// else
+// {
+// I2C_DMALastTransferCmd(i2c->def->i2cPort, ENABLE); // No repetitive start
+// }
+// // Disable buffer I2C interrupts
+// I2C_ITConfig(i2c->def->i2cPort, I2C_IT_EVT | I2C_IT_BUF, DISABLE);
+// // Enable the Transfer Complete interrupt
+// DMA_ITConfig(i2c->def->dmaRxStream, DMA_IT_TC | DMA_IT_TE, ENABLE);
+// I2C_DMACmd(i2c->def->i2cPort, ENABLE); // Enable before ADDR clear
+
+// // Workaround to enable DMA for Renode simulation.
+// // The I2C uses the DMA for reading, but the Renode implementation lacks some functionality
+// // and for a message to be read the DMA needs to be enabled manually.
+// // Without setting this bit the I2C reading fails.
+// // With added functionality it should be possible to remove.
+// DMA_Cmd(i2c->def->dmaRxStream, ENABLE); // Workaround
+
+// __DMB(); // Make sure instructions (clear address) are in correct order
+// SR2 = i2c->def->i2cPort->SR2; // clear ADDR
+// }
+// }
+// // Byte transfer finished
+// else if (SR1 & I2C_SR1_BTF)
+// {
+// SR2 = i2c->def->i2cPort->SR2;
+// if (SR2 & I2C_SR2_TRA) // In write mode?
+// {
+// if (i2c->txMessage.direction == i2cRead) // internal address read
+// {
+// /* Internal address written, switch to read */
+// i2c->def->i2cPort->CR1 = (I2C_CR1_START | I2C_CR1_PE); // Generate start
+// }
+// else
+// {
+// i2cNotifyClient(i2c);
+// // Are there any other messages to transact? If so stop else repeated start.
+// i2cTryNextMessage(i2c);
+// }
+// }
+// else // Reading. Shouldn't happen since we use DMA for reading.
+// {
+// ASSERT(1);
+// i2c->txMessage.buffer[i2c->messageIndex++] = I2C_ReceiveData(i2c->def->i2cPort);
+// if(i2c->messageIndex == i2c->txMessage.messageLength)
+// {
+// i2cNotifyClient(i2c);
+// // Are there any other messages to transact?
+// i2cTryNextMessage(i2c);
+// }
+// }
+// // A second BTF interrupt might occur if we don't wait for it to clear.
+// // TODO Implement better method.
+// while (i2c->def->i2cPort->CR1 & 0x0100) { ; }
+// }
+// // Byte received
+// else if (SR1 & I2C_SR1_RXNE) // Should not happen when we use DMA for reception.
+// {
+// i2c->txMessage.buffer[i2c->messageIndex++] = I2C_ReceiveData(i2c->def->i2cPort);
+// if(i2c->messageIndex == i2c->txMessage.messageLength)
+// {
+// I2C_ITConfig(i2c->def->i2cPort, I2C_IT_BUF, DISABLE); // disable RXE to get BTF
+// }
+// }
+// // Byte ready to be transmitted
+// else if (SR1 & I2C_SR1_TXE)
+// {
+// if (i2c->txMessage.direction == i2cRead)
+// {
+// // Disable TXE to flush and get BTF to switch to read.
+// // Switch must be done in BTF or strange things happen.
+// I2C_ITConfig(i2c->def->i2cPort, I2C_IT_BUF, DISABLE);
+// }
+// else
+// {
+// I2C_SendData(i2c->def->i2cPort, i2c->txMessage.buffer[i2c->messageIndex++]);
+// if(i2c->messageIndex == i2c->txMessage.messageLength)
+// {
+// // Disable TXE to allow the buffer to flush and get BTF
+// I2C_ITConfig(i2c->def->i2cPort, I2C_IT_BUF, DISABLE);
+// }
+// }
+// }
}
static void i2cdrvErrorIsrHandler(I2cDrv* i2c)
{
- if (I2C_GetFlagStatus(i2c->def->i2cPort, I2C_FLAG_AF))
- {
- if(i2c->txMessage.nbrOfRetries-- > 0)
- {
- // Retry by generating start
- i2c->def->i2cPort->CR1 = (I2C_CR1_START | I2C_CR1_PE);
- }
- else
- {
- // Failed so notify client and try next message if any.
- i2c->txMessage.status = i2cNack;
- i2cNotifyClient(i2c);
- i2cTryNextMessage(i2c);
- }
- I2C_ClearFlag(i2c->def->i2cPort, I2C_FLAG_AF);
- }
- if (I2C_GetFlagStatus(i2c->def->i2cPort, I2C_FLAG_BERR))
- {
- I2C_ClearFlag(i2c->def->i2cPort, I2C_FLAG_BERR);
- }
- if (I2C_GetFlagStatus(i2c->def->i2cPort, I2C_FLAG_OVR))
- {
- I2C_ClearFlag(i2c->def->i2cPort, I2C_FLAG_OVR);
- }
- if (I2C_GetFlagStatus(i2c->def->i2cPort, I2C_FLAG_ARLO))
- {
- I2C_ClearFlag(i2c->def->i2cPort,I2C_FLAG_ARLO);
- }
+ //COMMENTED FIRMWARE
+ // if (I2C_GetFlagStatus(i2c->def->i2cPort, I2C_FLAG_AF))
+ // {
+ // if(i2c->txMessage.nbrOfRetries-- > 0)
+ // {
+ // // Retry by generating start
+ // i2c->def->i2cPort->CR1 = (I2C_CR1_START | I2C_CR1_PE);
+ // }
+ // else
+ // {
+ // // Failed so notify client and try next message if any.
+ // i2c->txMessage.status = i2cNack;
+ // i2cNotifyClient(i2c);
+ // i2cTryNextMessage(i2c);
+ // }
+ // I2C_ClearFlag(i2c->def->i2cPort, I2C_FLAG_AF);
+ // }
+ // if (I2C_GetFlagStatus(i2c->def->i2cPort, I2C_FLAG_BERR))
+ // {
+ // I2C_ClearFlag(i2c->def->i2cPort, I2C_FLAG_BERR);
+ // }
+ // if (I2C_GetFlagStatus(i2c->def->i2cPort, I2C_FLAG_OVR))
+ // {
+ // I2C_ClearFlag(i2c->def->i2cPort, I2C_FLAG_OVR);
+ // }
+ // if (I2C_GetFlagStatus(i2c->def->i2cPort, I2C_FLAG_ARLO))
+ // {
+ // I2C_ClearFlag(i2c->def->i2cPort,I2C_FLAG_ARLO);
+ // }
}
static void i2cdrvClearDMA(I2cDrv* i2c)
{
- DMA_Cmd(i2c->def->dmaRxStream, DISABLE);
- DMA_ClearITPendingBit(i2c->def->dmaRxStream, i2c->def->dmaRxTCFlag);
- I2C_DMACmd(i2c->def->i2cPort, DISABLE);
- I2C_DMALastTransferCmd(i2c->def->i2cPort, DISABLE);
- DMA_ITConfig(i2c->def->dmaRxStream, DMA_IT_TC | DMA_IT_TE, DISABLE);
+ //COMMENTED FIRMWARE
+ // DMA_Cmd(i2c->def->dmaRxStream, DISABLE);
+ // DMA_ClearITPendingBit(i2c->def->dmaRxStream, i2c->def->dmaRxTCFlag);
+ // I2C_DMACmd(i2c->def->i2cPort, DISABLE);
+ // I2C_DMALastTransferCmd(i2c->def->i2cPort, DISABLE);
+ // DMA_ITConfig(i2c->def->dmaRxStream, DMA_IT_TC | DMA_IT_TE, DISABLE);
}
static void i2cdrvDmaIsrHandler(I2cDrv* i2c)
{
- if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTCFlag)) // Tranasfer complete
- {
- i2cdrvClearDMA(i2c);
- i2cNotifyClient(i2c);
- // Are there any other messages to transact?
- i2cTryNextMessage(i2c);
- }
- if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTEFlag)) // Transfer error
- {
- DMA_ClearITPendingBit(i2c->def->dmaRxStream, i2c->def->dmaRxTEFlag);
- //TODO: Best thing we could do?
- i2c->txMessage.status = i2cNack;
- i2cNotifyClient(i2c);
- i2cTryNextMessage(i2c);
- }
+ //COMMENTED FIRMWARE
+ // if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTCFlag)) // Tranasfer complete
+ // {
+ // i2cdrvClearDMA(i2c);
+ // i2cNotifyClient(i2c);
+ // // Are there any other messages to transact?
+ // i2cTryNextMessage(i2c);
+ // }
+ // if (DMA_GetFlagStatus(i2c->def->dmaRxStream, i2c->def->dmaRxTEFlag)) // Transfer error
+ // {
+ // DMA_ClearITPendingBit(i2c->def->dmaRxStream, i2c->def->dmaRxTEFlag);
+ // //TODO: Best thing we could do?
+ // i2c->txMessage.status = i2cNack;
+ // i2cNotifyClient(i2c);
+ // i2cTryNextMessage(i2c);
+ // }
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2cdev.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2cdev.c
index 3fa753bf2b626f1ba4441ffb944e35c08028aef2..81dd6ab1af7965fe289c0677d191ccd29fc2fff4 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2cdev.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2cdev.c
@@ -41,7 +41,8 @@
int i2cdevInit(I2C_Dev *dev)
{
- i2cdrvInit(dev);
+ //COMMENTED FIRMWARE
+ //i2cdrvInit(dev);
return true;
}
@@ -57,9 +58,9 @@ bool i2cdevReadBit(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
{
uint8_t byte;
bool status;
-
- status = i2cdevReadReg8(dev, devAddress, memAddress, 1, &byte);
- *data = byte & (1 << bitNum);
+ //COMMENTED FIRMWARE
+ // status = i2cdevReadReg8(dev, devAddress, memAddress, 1, &byte);
+ // *data = byte & (1 << bitNum);
return status;
}
@@ -68,15 +69,16 @@ bool i2cdevReadBits(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
uint8_t bitStart, uint8_t length, uint8_t *data)
{
bool status;
- uint8_t byte;
-
- if ((status = i2cdevReadByte(dev, devAddress, memAddress, &byte)) == true)
- {
- uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
- byte &= mask;
- byte >>= (bitStart - length + 1);
- *data = byte;
- }
+ //COMMENTED FIRMWARE
+ // uint8_t byte;
+
+ // if ((status = i2cdevReadByte(dev, devAddress, memAddress, &byte)) == true)
+ // {
+ // uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+ // byte &= mask;
+ // byte >>= (bitStart - length + 1);
+ // *data = byte;
+ // }
return status;
}
@@ -84,7 +86,8 @@ bool i2cdevRead(I2C_Dev *dev, uint8_t devAddress, uint16_t len, uint8_t *data)
{
I2cMessage message;
- i2cdrvCreateMessage(&message, devAddress, i2cRead, len, data);
+ //COMMENTED FIRMWARE
+ //i2cdrvCreateMessage(&message, devAddress, i2cRead, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
@@ -93,9 +96,9 @@ bool i2cdevReadReg8(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
uint16_t len, uint8_t *data)
{
I2cMessage message;
-
- i2cdrvCreateMessageIntAddr(&message, devAddress, false, memAddress,
- i2cRead, len, data);
+ //COMMENTED FIRMWARE
+ // i2cdrvCreateMessageIntAddr(&message, devAddress, false, memAddress,
+ // i2cRead, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
@@ -103,10 +106,11 @@ bool i2cdevReadReg8(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
bool i2cdevReadReg16(I2C_Dev *dev, uint8_t devAddress, uint16_t memAddress,
uint16_t len, uint8_t *data)
{
- I2cMessage message;
+ //COMMENTED FIRMWARE
+ // I2cMessage message;
- i2cdrvCreateMessageIntAddr(&message, devAddress, true, memAddress,
- i2cRead, len, data);
+ // i2cdrvCreateMessageIntAddr(&message, devAddress, true, memAddress,
+ // i2cRead, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
@@ -121,8 +125,9 @@ bool i2cdevWriteBit(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
uint8_t bitNum, uint8_t data)
{
uint8_t byte;
- i2cdevReadByte(dev, devAddress, memAddress, &byte);
- byte = (data != 0) ? (byte | (1 << bitNum)) : (byte & ~(1 << bitNum));
+ //COMMENTED FIRMWARE
+ // i2cdevReadByte(dev, devAddress, memAddress, &byte);
+ // byte = (data != 0) ? (byte | (1 << bitNum)) : (byte & ~(1 << bitNum));
return i2cdevWriteByte(dev, devAddress, memAddress, byte);
}
@@ -130,17 +135,18 @@ bool i2cdevWriteBits(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
uint8_t bitStart, uint8_t length, uint8_t data)
{
bool status;
- uint8_t byte;
-
- if ((status = i2cdevReadByte(dev, devAddress, memAddress, &byte)) == true)
- {
- uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
- data <<= (bitStart - length + 1); // shift data into correct position
- data &= mask; // zero all non-important bits in data
- byte &= ~(mask); // zero all important bits in existing byte
- byte |= data; // combine data with existing byte
- status = i2cdevWriteByte(dev, devAddress, memAddress, byte);
- }
+ //COMMENTED FIRMWARE
+ // uint8_t byte;
+
+ // if ((status = i2cdevReadByte(dev, devAddress, memAddress, &byte)) == true)
+ // {
+ // uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+ // data <<= (bitStart - length + 1); // shift data into correct position
+ // data &= mask; // zero all non-important bits in data
+ // byte &= ~(mask); // zero all important bits in existing byte
+ // byte |= data; // combine data with existing byte
+ // status = i2cdevWriteByte(dev, devAddress, memAddress, byte);
+ // }
return status;
}
@@ -149,7 +155,8 @@ bool i2cdevWrite(I2C_Dev *dev, uint8_t devAddress, uint16_t len, const uint8_t *
{
I2cMessage message;
- i2cdrvCreateMessage(&message, devAddress, i2cWrite, len, data);
+ //COMMENTED FIRMWARE
+ //i2cdrvCreateMessage(&message, devAddress, i2cWrite, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
@@ -159,8 +166,9 @@ bool i2cdevWriteReg8(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
{
I2cMessage message;
- i2cdrvCreateMessageIntAddr(&message, devAddress, false, memAddress,
- i2cWrite, len, data);
+ //COMMENTED FIRMWARE
+ // i2cdrvCreateMessageIntAddr(&message, devAddress, false, memAddress,
+ // i2cWrite, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
@@ -170,8 +178,9 @@ bool i2cdevWriteReg16(I2C_Dev *dev, uint8_t devAddress, uint16_t memAddress,
{
I2cMessage message;
- i2cdrvCreateMessageIntAddr(&message, devAddress, true, memAddress,
- i2cWrite, len, data);
+ //COMMENTED FIRMWARE
+ // i2cdrvCreateMessageIntAddr(&message, devAddress, true, memAddress,
+ // i2cWrite, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2croutines.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2croutines.c
index b0f8a2f520e82f68c5e5f240b8edd6a04fa7274a..d0171b93a614dd3cba2988b04006f1110afca826 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2croutines.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/i2croutines.c
@@ -66,111 +66,112 @@ ErrorStatus I2C_Master_BufferRead(I2C_TypeDef* I2Cx, uint8_t* pBuffer,
uint32_t timeoutMs)
{
- __IO uint32_t temp = 0;
- __IO uint32_t Timeout = 0;
-
- /* Enable I2C errors interrupts (used in all modes: Polling, DMA and Interrupts */
- I2Cx->CR2 |= I2C_IT_ERR;
-
- if (Mode == DMA) /* I2Cx Master Reception using DMA */
- {
- /* Configure I2Cx DMA channel */
- I2C_DMAConfig(I2Cx, pBuffer, NumByteToRead, I2C_DIRECTION_RX);
- /* Set Last bit to have a NACK on the last received byte */
- I2Cx->CR2 |= CR2_LAST_Set;
- /* Enable I2C DMA requests */
- I2Cx->CR2 |= CR2_DMAEN_Set;
- Timeout = 0xFFFF;
- /* Send START condition */
- I2Cx->CR1 |= CR1_START_Set;
- /* Wait until SB flag is set: EV5 */
- while ((I2Cx->SR1 & 0x0001) != 0x0001)
- {
- if (Timeout-- == 0)
- return ERROR;
- }
- Timeout = 0xFFFF;
- /* Send slave address */
- /* Set the address bit0 for read */
- SlaveAddress |= OAR1_ADD0_Set;
- I2Cx->DR = SlaveAddress;
- /* Wait until ADDR is set: EV6 */
- while ((I2Cx->SR1 & 0x0002) != 0x0002)
- {
- if (Timeout-- == 0)
- return ERROR;
- }
- /* Clear ADDR flag by reading SR2 register */
- temp = I2Cx->SR2;
- if (I2Cx == I2C1)
- {
- /* Wait until DMA end of transfer */
- //while (!DMA_GetFlagStatus(DMA1_FLAG_TC7));
- xSemaphoreTake(i2cdevDmaEventI2c1, M2T(timeoutMs));
- /* Disable DMA Channel */
- DMA_Cmd(I2C1_DMA_CHANNEL_RX, DISABLE);
- /* Clear the DMA Transfer Complete flag */
- DMA_ClearFlag(DMA1_FLAG_TC7);
- }
- else /* I2Cx = I2C2*/
- {
- /* Wait until DMA end of transfer */
- //while (!DMA_GetFlagStatus(DMA1_FLAG_TC5))
- xSemaphoreTake(i2cdevDmaEventI2c2, M2T(timeoutMs));
- /* Disable DMA Channel */
- DMA_Cmd(I2C2_DMA_CHANNEL_RX, DISABLE);
- /* Clear the DMA Transfer Complete flag */
- DMA_ClearFlag(DMA1_FLAG_TC5);
- }
- /* Program the STOP */
- I2Cx->CR1 |= CR1_STOP_Set;
- /* Make sure that the STOP bit is cleared by Hardware before CR1 write access */
- while ((I2Cx->CR1 & 0x200) == 0x200)
- ;
- }
- /* I2Cx Master Reception using Interrupts with highest priority in an application */
- else
- {
- /* Enable EVT IT*/
- I2Cx->CR2 |= I2C_IT_EVT;
- /* Enable BUF IT */
- I2Cx->CR2 |= I2C_IT_BUF;
- SlaveAddress |= OAR1_ADD0_Set;
- if (I2Cx== I2C1)
- {
- /* Set the I2C direction to reception */
- I2CDirection1 = I2C_DIRECTION_RX;
- Buffer_Rx1 = pBuffer;
- Address1 = SlaveAddress;
- NumbOfBytes1 = NumByteToRead;
- }
- else
- {
- /* Set the I2C direction to reception */
- I2CDirection2 = I2C_DIRECTION_RX;
- Buffer_Rx2 = pBuffer;
- Address2 = SlaveAddress;
- NumbOfBytes2 = NumByteToRead;
- }
- /* Send START condition */
- I2Cx->CR1 |= CR1_START_Set;
- Timeout = timeoutMs * I2CDEV_LOOPS_PER_MS;
- /* Wait until the START condition is generated on the bus: START bit is cleared by hardware */
- while ((I2Cx->CR1 & 0x100) == 0x100 && Timeout)
- {
- Timeout--;
- }
- /* Wait until BUSY flag is reset (until a STOP is generated) */
- while ((I2Cx->SR2 & 0x0002) == 0x0002 && Timeout)
- {
- Timeout--;
- }
- /* Enable Acknowledgement to be ready for another reception */
- I2Cx->CR1 |= CR1_ACK_Set;
-
- if (Timeout == 0)
- return ERROR;
- }
+ //COMMENTED FIRMWARE
+ // __IO uint32_t temp = 0;
+ // __IO uint32_t Timeout = 0;
+
+ // /* Enable I2C errors interrupts (used in all modes: Polling, DMA and Interrupts */
+ // I2Cx->CR2 |= I2C_IT_ERR;
+
+ // if (Mode == DMA) /* I2Cx Master Reception using DMA */
+ // {
+ // /* Configure I2Cx DMA channel */
+ // I2C_DMAConfig(I2Cx, pBuffer, NumByteToRead, I2C_DIRECTION_RX);
+ // /* Set Last bit to have a NACK on the last received byte */
+ // I2Cx->CR2 |= CR2_LAST_Set;
+ // /* Enable I2C DMA requests */
+ // I2Cx->CR2 |= CR2_DMAEN_Set;
+ // Timeout = 0xFFFF;
+ // /* Send START condition */
+ // I2Cx->CR1 |= CR1_START_Set;
+ // /* Wait until SB flag is set: EV5 */
+ // while ((I2Cx->SR1 & 0x0001) != 0x0001)
+ // {
+ // if (Timeout-- == 0)
+ // return ERROR;
+ // }
+ // Timeout = 0xFFFF;
+ // /* Send slave address */
+ // /* Set the address bit0 for read */
+ // SlaveAddress |= OAR1_ADD0_Set;
+ // I2Cx->DR = SlaveAddress;
+ // /* Wait until ADDR is set: EV6 */
+ // while ((I2Cx->SR1 & 0x0002) != 0x0002)
+ // {
+ // if (Timeout-- == 0)
+ // return ERROR;
+ // }
+ // /* Clear ADDR flag by reading SR2 register */
+ // temp = I2Cx->SR2;
+ // if (I2Cx == I2C1)
+ // {
+ // /* Wait until DMA end of transfer */
+ // //while (!DMA_GetFlagStatus(DMA1_FLAG_TC7));
+ // xSemaphoreTake(i2cdevDmaEventI2c1, M2T(timeoutMs));
+ // /* Disable DMA Channel */
+ // DMA_Cmd(I2C1_DMA_CHANNEL_RX, DISABLE);
+ // /* Clear the DMA Transfer Complete flag */
+ // DMA_ClearFlag(DMA1_FLAG_TC7);
+ // }
+ // else /* I2Cx = I2C2*/
+ // {
+ // /* Wait until DMA end of transfer */
+ // //while (!DMA_GetFlagStatus(DMA1_FLAG_TC5))
+ // xSemaphoreTake(i2cdevDmaEventI2c2, M2T(timeoutMs));
+ // /* Disable DMA Channel */
+ // DMA_Cmd(I2C2_DMA_CHANNEL_RX, DISABLE);
+ // /* Clear the DMA Transfer Complete flag */
+ // DMA_ClearFlag(DMA1_FLAG_TC5);
+ // }
+ // /* Program the STOP */
+ // I2Cx->CR1 |= CR1_STOP_Set;
+ // /* Make sure that the STOP bit is cleared by Hardware before CR1 write access */
+ // while ((I2Cx->CR1 & 0x200) == 0x200)
+ // ;
+ // }
+ // /* I2Cx Master Reception using Interrupts with highest priority in an application */
+ // else
+ // {
+ // /* Enable EVT IT*/
+ // I2Cx->CR2 |= I2C_IT_EVT;
+ // /* Enable BUF IT */
+ // I2Cx->CR2 |= I2C_IT_BUF;
+ // SlaveAddress |= OAR1_ADD0_Set;
+ // if (I2Cx== I2C1)
+ // {
+ // /* Set the I2C direction to reception */
+ // I2CDirection1 = I2C_DIRECTION_RX;
+ // Buffer_Rx1 = pBuffer;
+ // Address1 = SlaveAddress;
+ // NumbOfBytes1 = NumByteToRead;
+ // }
+ // else
+ // {
+ // /* Set the I2C direction to reception */
+ // I2CDirection2 = I2C_DIRECTION_RX;
+ // Buffer_Rx2 = pBuffer;
+ // Address2 = SlaveAddress;
+ // NumbOfBytes2 = NumByteToRead;
+ // }
+ // /* Send START condition */
+ // I2Cx->CR1 |= CR1_START_Set;
+ // Timeout = timeoutMs * I2CDEV_LOOPS_PER_MS;
+ // /* Wait until the START condition is generated on the bus: START bit is cleared by hardware */
+ // while ((I2Cx->CR1 & 0x100) == 0x100 && Timeout)
+ // {
+ // Timeout--;
+ // }
+ // /* Wait until BUSY flag is reset (until a STOP is generated) */
+ // while ((I2Cx->SR2 & 0x0002) == 0x0002 && Timeout)
+ // {
+ // Timeout--;
+ // }
+ // /* Enable Acknowledgement to be ready for another reception */
+ // I2Cx->CR1 |= CR1_ACK_Set;
+
+ // if (Timeout == 0)
+ // return ERROR;
+ // }
return SUCCESS;
@@ -189,114 +190,114 @@ ErrorStatus I2C_Master_BufferWrite(I2C_TypeDef* I2Cx, uint8_t* pBuffer,
uint32_t NumByteToWrite, I2C_ProgrammingModel Mode, uint8_t SlaveAddress,
uint32_t timeoutMs)
{
-
- __IO uint32_t temp = 0;
- __IO uint32_t Timeout = 0;
-
- /* Enable Error IT (used in all modes: DMA, Polling and Interrupts */
- I2Cx->CR2 |= I2C_IT_ERR;
- if (Mode == DMA) /* I2Cx Master Transmission using DMA */
- {
- Timeout = 0xFFFF;
- /* Configure the DMA channel for I2Cx transmission */
- I2C_DMAConfig(I2Cx, pBuffer, NumByteToWrite, I2C_DIRECTION_TX);
- /* Enable the I2Cx DMA requests */
- I2Cx->CR2 |= CR2_DMAEN_Set;
- /* Send START condition */
- I2Cx->CR1 |= CR1_START_Set;
- /* Wait until SB flag is set: EV5 */
- while ((I2Cx->SR1 & 0x0001) != 0x0001)
- {
- if (Timeout-- == 0)
- return ERROR;
- }
- Timeout = 0xFFFF;
- /* Send slave address */
- /* Reset the address bit0 for write */
- SlaveAddress &= OAR1_ADD0_Reset;
- I2Cx->DR = SlaveAddress;
- /* Wait until ADDR is set: EV6 */
- while ((I2Cx->SR1 & 0x0002) != 0x0002)
- {
- if (Timeout-- == 0)
- return ERROR;
- }
-
- /* Clear ADDR flag by reading SR2 register */
- temp = I2Cx->SR2;
- if (I2Cx == I2C1)
- {
- /* Wait until DMA end of transfer */
-// while (!DMA_GetFlagStatus(DMA1_FLAG_TC6));
- xSemaphoreTake(i2cdevDmaEventI2c1, M2T(5));
- /* Disable the DMA1 Channel 6 */
- DMA_Cmd(I2C1_DMA_CHANNEL_TX, DISABLE);
- /* Clear the DMA Transfer complete flag */
- DMA_ClearFlag(DMA1_FLAG_TC6);
- }
- else /* I2Cx = I2C2 */
- {
- /* Wait until DMA end of transfer */
- //while (!DMA_GetFlagStatus(DMA1_FLAG_TC4))
- xSemaphoreTake(i2cdevDmaEventI2c2, M2T(5));
- /* Disable the DMA1 Channel 4 */
- DMA_Cmd(I2C2_DMA_CHANNEL_TX, DISABLE);
- /* Clear the DMA Transfer complete flag */
- DMA_ClearFlag(DMA1_FLAG_TC4);
- }
-
- /* EV8_2: Wait until BTF is set before programming the STOP */
- while ((I2Cx->SR1 & 0x00004) != 0x000004)
- ;
- /* Program the STOP */
- I2Cx->CR1 |= CR1_STOP_Set;
- /* Make sure that the STOP bit is cleared by Hardware */
- while ((I2Cx->CR1 & 0x200) == 0x200)
- ;
- }
- /* I2Cx Master Transmission using Interrupt with highest priority in the application */
- else
- {
- /* Enable EVT IT*/
- I2Cx->CR2 |= I2C_IT_EVT;
- /* Enable BUF IT */
- I2Cx->CR2 |= I2C_IT_BUF;
- /* Set the I2C direction to Transmission */
- SlaveAddress &= OAR1_ADD0_Reset;
- if (I2Cx== I2C1)
- {
- /* Set the I2C direction to reception */
- I2CDirection1 = I2C_DIRECTION_TX;
- Buffer_Tx1 = pBuffer;
- Address1 = SlaveAddress;
- NumbOfBytes1 = NumByteToWrite;
- }
- else
- {
- /* Set the I2C direction to reception */
- I2CDirection2 = I2C_DIRECTION_TX;
- Buffer_Tx2 = pBuffer;
- Address2 = SlaveAddress;
- NumbOfBytes2 = NumByteToWrite;
- }
- /* Send START condition */
- I2Cx->CR1 |= CR1_START_Set;
- Timeout = timeoutMs * I2CDEV_LOOPS_PER_MS;
- /* Wait until the START condition is generated on the bus: the START bit is cleared by hardware */
- while ((I2Cx->CR1 & 0x100) == 0x100 && Timeout)
- {
- Timeout--;
- }
- /* Wait until BUSY flag is reset: a STOP has been generated on the bus signaling the end
- of transmission */
- while ((I2Cx->SR2 & 0x0002) == 0x0002 && Timeout)
- {
- Timeout--;
- }
-
- if (Timeout == 0)
- return ERROR;
- }
+ //COMMENTED FIRMWARE
+// __IO uint32_t temp = 0;
+// __IO uint32_t Timeout = 0;
+
+// /* Enable Error IT (used in all modes: DMA, Polling and Interrupts */
+// I2Cx->CR2 |= I2C_IT_ERR;
+// if (Mode == DMA) /* I2Cx Master Transmission using DMA */
+// {
+// Timeout = 0xFFFF;
+// /* Configure the DMA channel for I2Cx transmission */
+// I2C_DMAConfig(I2Cx, pBuffer, NumByteToWrite, I2C_DIRECTION_TX);
+// /* Enable the I2Cx DMA requests */
+// I2Cx->CR2 |= CR2_DMAEN_Set;
+// /* Send START condition */
+// I2Cx->CR1 |= CR1_START_Set;
+// /* Wait until SB flag is set: EV5 */
+// while ((I2Cx->SR1 & 0x0001) != 0x0001)
+// {
+// if (Timeout-- == 0)
+// return ERROR;
+// }
+// Timeout = 0xFFFF;
+// /* Send slave address */
+// /* Reset the address bit0 for write */
+// SlaveAddress &= OAR1_ADD0_Reset;
+// I2Cx->DR = SlaveAddress;
+// /* Wait until ADDR is set: EV6 */
+// while ((I2Cx->SR1 & 0x0002) != 0x0002)
+// {
+// if (Timeout-- == 0)
+// return ERROR;
+// }
+
+// /* Clear ADDR flag by reading SR2 register */
+// temp = I2Cx->SR2;
+// if (I2Cx == I2C1)
+// {
+// /* Wait until DMA end of transfer */
+// // while (!DMA_GetFlagStatus(DMA1_FLAG_TC6));
+// xSemaphoreTake(i2cdevDmaEventI2c1, M2T(5));
+// /* Disable the DMA1 Channel 6 */
+// DMA_Cmd(I2C1_DMA_CHANNEL_TX, DISABLE);
+// /* Clear the DMA Transfer complete flag */
+// DMA_ClearFlag(DMA1_FLAG_TC6);
+// }
+// else /* I2Cx = I2C2 */
+// {
+// /* Wait until DMA end of transfer */
+// //while (!DMA_GetFlagStatus(DMA1_FLAG_TC4))
+// xSemaphoreTake(i2cdevDmaEventI2c2, M2T(5));
+// /* Disable the DMA1 Channel 4 */
+// DMA_Cmd(I2C2_DMA_CHANNEL_TX, DISABLE);
+// /* Clear the DMA Transfer complete flag */
+// DMA_ClearFlag(DMA1_FLAG_TC4);
+// }
+
+// /* EV8_2: Wait until BTF is set before programming the STOP */
+// while ((I2Cx->SR1 & 0x00004) != 0x000004)
+// ;
+// /* Program the STOP */
+// I2Cx->CR1 |= CR1_STOP_Set;
+// /* Make sure that the STOP bit is cleared by Hardware */
+// while ((I2Cx->CR1 & 0x200) == 0x200)
+// ;
+// }
+// /* I2Cx Master Transmission using Interrupt with highest priority in the application */
+// else
+// {
+// /* Enable EVT IT*/
+// I2Cx->CR2 |= I2C_IT_EVT;
+// /* Enable BUF IT */
+// I2Cx->CR2 |= I2C_IT_BUF;
+// /* Set the I2C direction to Transmission */
+// SlaveAddress &= OAR1_ADD0_Reset;
+// if (I2Cx== I2C1)
+// {
+// /* Set the I2C direction to reception */
+// I2CDirection1 = I2C_DIRECTION_TX;
+// Buffer_Tx1 = pBuffer;
+// Address1 = SlaveAddress;
+// NumbOfBytes1 = NumByteToWrite;
+// }
+// else
+// {
+// /* Set the I2C direction to reception */
+// I2CDirection2 = I2C_DIRECTION_TX;
+// Buffer_Tx2 = pBuffer;
+// Address2 = SlaveAddress;
+// NumbOfBytes2 = NumByteToWrite;
+// }
+// /* Send START condition */
+// I2Cx->CR1 |= CR1_START_Set;
+// Timeout = timeoutMs * I2CDEV_LOOPS_PER_MS;
+// /* Wait until the START condition is generated on the bus: the START bit is cleared by hardware */
+// while ((I2Cx->CR1 & 0x100) == 0x100 && Timeout)
+// {
+// Timeout--;
+// }
+// /* Wait until BUSY flag is reset: a STOP has been generated on the bus signaling the end
+// of transmission */
+// while ((I2Cx->SR2 & 0x0002) == 0x0002 && Timeout)
+// {
+// Timeout--;
+// }
+
+// if (Timeout == 0)
+// return ERROR;
+// }
return SUCCESS;
temp++; //To avoid GCC warning!
@@ -311,21 +312,22 @@ ErrorStatus I2C_Master_BufferWrite(I2C_TypeDef* I2Cx, uint8_t* pBuffer,
void I2C_Slave_BufferReadWrite(I2C_TypeDef* I2Cx, I2C_ProgrammingModel Mode)
{
- /* Enable Event IT needed for ADDR and STOPF events ITs */
- I2Cx->CR2 |= I2C_IT_EVT;
- /* Enable Error IT */
- I2Cx->CR2 |= I2C_IT_ERR;
-
- if (Mode == DMA) /* I2Cx Slave Transmission using DMA */
- {
- /* Enable I2Cx DMA requests */
- I2Cx->CR2 |= CR2_DMAEN_Set;
- }
- else /* I2Cx Slave Transmission using Interrupt with highest priority in the application */
- {
- /* Enable Buffer IT (TXE and RXNE ITs) */
- I2Cx->CR2 |= I2C_IT_BUF;
- }
+ //COMMENTED FIRMWARE
+ // /* Enable Event IT needed for ADDR and STOPF events ITs */
+ // I2Cx->CR2 |= I2C_IT_EVT;
+ // /* Enable Error IT */
+ // I2Cx->CR2 |= I2C_IT_ERR;
+
+ // if (Mode == DMA) /* I2Cx Slave Transmission using DMA */
+ // {
+ // /* Enable I2Cx DMA requests */
+ // I2Cx->CR2 |= CR2_DMAEN_Set;
+ // }
+ // else /* I2Cx Slave Transmission using Interrupt with highest priority in the application */
+ // {
+ // /* Enable Buffer IT (TXE and RXNE ITs) */
+ // I2Cx->CR2 |= I2C_IT_BUF;
+ // }
}
/**
@@ -335,126 +337,127 @@ void I2C_Slave_BufferReadWrite(I2C_TypeDef* I2Cx, I2C_ProgrammingModel Mode)
*/
void I2C_LowLevel_Init(I2C_TypeDef* I2Cx)
{
- GPIO_InitTypeDef GPIO_InitStructure;
- I2C_InitTypeDef I2C_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
-
- /* GPIOB clock enable */
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
- /* Enable the DMA1 clock */
- RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
- if (I2Cx == I2C1)
- {
- /* I2C1 clock enable */
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
- /* I2C1 SDA and SCL configuration */
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- /* Enable I2C1 reset state */
- RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C1, ENABLE);
- /* Release I2C1 from reset state */
- RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C1, DISABLE);
-
- NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_HIGH_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_LOW_PRI;
- NVIC_Init(&NVIC_InitStructure);
- }
- else /* I2Cx = I2C2 */
- {
-
- /* I2C2 clock enable */
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
- /* I2C1 SDA and SCL configuration */
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- /* Enable I2C2 reset state */
- RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C2, ENABLE);
- /* Release I2C2 from reset state */
- RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C2, DISABLE);
-
- NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_HIGH_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_LOW_PRI;
- NVIC_Init(&NVIC_InitStructure);
- }
-
- /* I2C1 and I2C2 configuration */
- I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
- I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
- I2C_InitStructure.I2C_OwnAddress1 = OwnAddress1;
- I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
- I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
- I2C_InitStructure.I2C_ClockSpeed = ClockSpeed;
- I2C_Init(I2C1, &I2C_InitStructure);
- I2C_InitStructure.I2C_OwnAddress1 = OwnAddress2;
- I2C_Init(I2C2, &I2C_InitStructure);
-
- if (I2Cx == I2C1)
-
- { /* I2C1 TX DMA Channel configuration */
- DMA_DeInit(I2C1_DMA_CHANNEL_TX);
- I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C1_DR_Address;
- I2CDMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t) 0; /* This parameter will be configured durig communication */
- I2CDMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; /* This parameter will be configured durig communication */
- I2CDMA_InitStructure.DMA_BufferSize = 0xFFFF; /* This parameter will be configured durig communication */
- I2CDMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- I2CDMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
- I2CDMA_InitStructure.DMA_PeripheralDataSize = DMA_MemoryDataSize_Byte;
- I2CDMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
- I2CDMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
- I2CDMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
- I2CDMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
- DMA_Init(I2C1_DMA_CHANNEL_TX, &I2CDMA_InitStructure);
-
- /* I2C1 RX DMA Channel configuration */
- DMA_DeInit(I2C1_DMA_CHANNEL_RX);
- DMA_Init(I2C1_DMA_CHANNEL_RX, &I2CDMA_InitStructure);
- }
-
- else /* I2Cx = I2C2 */
-
- {
- /* I2C2 TX DMA Channel configuration */
- DMA_DeInit(I2C2_DMA_CHANNEL_TX);
- I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C2_DR_Address;
- I2CDMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t) 0; /* This parameter will be configured durig communication */
- I2CDMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; /* This parameter will be configured durig communication */
- I2CDMA_InitStructure.DMA_BufferSize = 0xFFFF; /* This parameter will be configured durig communication */
- I2CDMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- I2CDMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
- I2CDMA_InitStructure.DMA_PeripheralDataSize = DMA_MemoryDataSize_Byte;
- I2CDMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
- I2CDMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
- I2CDMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
- I2CDMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
- DMA_Init(I2C2_DMA_CHANNEL_TX, &I2CDMA_InitStructure);
-
- /* I2C2 RX DMA Channel configuration */
- DMA_DeInit(I2C2_DMA_CHANNEL_RX);
- DMA_Init(I2C2_DMA_CHANNEL_RX, &I2CDMA_InitStructure);
-
- }
+ //COMMENTED FIRMWARE
+ // GPIO_InitTypeDef GPIO_InitStructure;
+ // I2C_InitTypeDef I2C_InitStructure;
+ // NVIC_InitTypeDef NVIC_InitStructure;
+
+ // /* GPIOB clock enable */
+ // RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+ // /* Enable the DMA1 clock */
+ // RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
+ // if (I2Cx == I2C1)
+ // {
+ // /* I2C1 clock enable */
+ // RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
+ // /* I2C1 SDA and SCL configuration */
+ // GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+ // GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ // GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
+ // GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ // /* Enable I2C1 reset state */
+ // RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C1, ENABLE);
+ // /* Release I2C1 from reset state */
+ // RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C1, DISABLE);
+
+ // NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn;
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_HIGH_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ // NVIC_Init(&NVIC_InitStructure);
+ // NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn;
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_LOW_PRI;
+ // NVIC_Init(&NVIC_InitStructure);
+ // }
+ // else /* I2Cx = I2C2 */
+ // {
+
+ // /* I2C2 clock enable */
+ // RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
+ // /* I2C1 SDA and SCL configuration */
+ // GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+ // GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ // GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
+ // GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ // /* Enable I2C2 reset state */
+ // RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C2, ENABLE);
+ // /* Release I2C2 from reset state */
+ // RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C2, DISABLE);
+
+ // NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_HIGH_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ // NVIC_Init(&NVIC_InitStructure);
+ // NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_I2C_LOW_PRI;
+ // NVIC_Init(&NVIC_InitStructure);
+ // }
+
+ // /* I2C1 and I2C2 configuration */
+ // I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
+ // I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
+ // I2C_InitStructure.I2C_OwnAddress1 = OwnAddress1;
+ // I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
+ // I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
+ // I2C_InitStructure.I2C_ClockSpeed = ClockSpeed;
+ // I2C_Init(I2C1, &I2C_InitStructure);
+ // I2C_InitStructure.I2C_OwnAddress1 = OwnAddress2;
+ // I2C_Init(I2C2, &I2C_InitStructure);
+
+ // if (I2Cx == I2C1)
+
+ // { /* I2C1 TX DMA Channel configuration */
+ // DMA_DeInit(I2C1_DMA_CHANNEL_TX);
+ // I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C1_DR_Address;
+ // I2CDMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t) 0; /* This parameter will be configured durig communication */
+ // I2CDMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; /* This parameter will be configured durig communication */
+ // I2CDMA_InitStructure.DMA_BufferSize = 0xFFFF; /* This parameter will be configured durig communication */
+ // I2CDMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ // I2CDMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ // I2CDMA_InitStructure.DMA_PeripheralDataSize = DMA_MemoryDataSize_Byte;
+ // I2CDMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ // I2CDMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
+ // I2CDMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
+ // I2CDMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
+ // DMA_Init(I2C1_DMA_CHANNEL_TX, &I2CDMA_InitStructure);
+
+ // /* I2C1 RX DMA Channel configuration */
+ // DMA_DeInit(I2C1_DMA_CHANNEL_RX);
+ // DMA_Init(I2C1_DMA_CHANNEL_RX, &I2CDMA_InitStructure);
+ // }
+
+ // else /* I2Cx = I2C2 */
+
+ // {
+ // /* I2C2 TX DMA Channel configuration */
+ // DMA_DeInit(I2C2_DMA_CHANNEL_TX);
+ // I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C2_DR_Address;
+ // I2CDMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t) 0; /* This parameter will be configured durig communication */
+ // I2CDMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; /* This parameter will be configured durig communication */
+ // I2CDMA_InitStructure.DMA_BufferSize = 0xFFFF; /* This parameter will be configured durig communication */
+ // I2CDMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ // I2CDMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ // I2CDMA_InitStructure.DMA_PeripheralDataSize = DMA_MemoryDataSize_Byte;
+ // I2CDMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ // I2CDMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
+ // I2CDMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
+ // I2CDMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
+ // DMA_Init(I2C2_DMA_CHANNEL_TX, &I2CDMA_InitStructure);
+
+ // /* I2C2 RX DMA Channel configuration */
+ // DMA_DeInit(I2C2_DMA_CHANNEL_RX);
+ // DMA_Init(I2C2_DMA_CHANNEL_RX, &I2CDMA_InitStructure);
+
+ // }
}
/**
@@ -465,54 +468,55 @@ void I2C_LowLevel_Init(I2C_TypeDef* I2Cx)
void I2C_DMAConfig(I2C_TypeDef* I2Cx, uint8_t* pBuffer, uint32_t BufferSize,
uint32_t Direction)
{
- /* Initialize the DMA with the new parameters */
- if (Direction == I2C_DIRECTION_TX)
- {
- /* Configure the DMA Tx Channel with the buffer address and the buffer size */
- I2CDMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t) pBuffer;
- I2CDMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
- I2CDMA_InitStructure.DMA_BufferSize = (uint32_t) BufferSize;
-
- if (I2Cx == I2C1)
- {
- I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C1_DR_Address;
- DMA_Cmd(I2C1_DMA_CHANNEL_TX, DISABLE);
- DMA_Init(I2C1_DMA_CHANNEL_TX, &I2CDMA_InitStructure);
- DMA_Cmd(I2C1_DMA_CHANNEL_TX, ENABLE);
- }
- else
- {
- I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C2_DR_Address;
- DMA_Cmd(I2C2_DMA_CHANNEL_TX, DISABLE);
- DMA_Init(I2C2_DMA_CHANNEL_TX, &I2CDMA_InitStructure);
- DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
- DMA_Cmd(I2C2_DMA_CHANNEL_TX, ENABLE);
- }
- }
- else /* Reception */
- {
- /* Configure the DMA Rx Channel with the buffer address and the buffer size */
- I2CDMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t) pBuffer;
- I2CDMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
- I2CDMA_InitStructure.DMA_BufferSize = (uint32_t) BufferSize;
- if (I2Cx == I2C1)
- {
-
- I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C1_DR_Address;
- DMA_Cmd(I2C1_DMA_CHANNEL_RX, DISABLE);
- DMA_Init(I2C1_DMA_CHANNEL_RX, &I2CDMA_InitStructure);
- DMA_Cmd(I2C1_DMA_CHANNEL_RX, ENABLE);
- }
-
- else
- {
- I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C2_DR_Address;
- DMA_Cmd(I2C2_DMA_CHANNEL_RX, DISABLE);
- DMA_Init(I2C2_DMA_CHANNEL_RX, &I2CDMA_InitStructure);
- DMA_Cmd(I2C2_DMA_CHANNEL_RX, ENABLE);
- }
-
- }
+ //COMMENTED FIRMWARE
+ // /* Initialize the DMA with the new parameters */
+ // if (Direction == I2C_DIRECTION_TX)
+ // {
+ // /* Configure the DMA Tx Channel with the buffer address and the buffer size */
+ // I2CDMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t) pBuffer;
+ // I2CDMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
+ // I2CDMA_InitStructure.DMA_BufferSize = (uint32_t) BufferSize;
+
+ // if (I2Cx == I2C1)
+ // {
+ // I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C1_DR_Address;
+ // DMA_Cmd(I2C1_DMA_CHANNEL_TX, DISABLE);
+ // DMA_Init(I2C1_DMA_CHANNEL_TX, &I2CDMA_InitStructure);
+ // DMA_Cmd(I2C1_DMA_CHANNEL_TX, ENABLE);
+ // }
+ // else
+ // {
+ // I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C2_DR_Address;
+ // DMA_Cmd(I2C2_DMA_CHANNEL_TX, DISABLE);
+ // DMA_Init(I2C2_DMA_CHANNEL_TX, &I2CDMA_InitStructure);
+ // DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
+ // DMA_Cmd(I2C2_DMA_CHANNEL_TX, ENABLE);
+ // }
+ // }
+ // else /* Reception */
+ // {
+ // /* Configure the DMA Rx Channel with the buffer address and the buffer size */
+ // I2CDMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t) pBuffer;
+ // I2CDMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
+ // I2CDMA_InitStructure.DMA_BufferSize = (uint32_t) BufferSize;
+ // if (I2Cx == I2C1)
+ // {
+
+ // I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C1_DR_Address;
+ // DMA_Cmd(I2C1_DMA_CHANNEL_RX, DISABLE);
+ // DMA_Init(I2C1_DMA_CHANNEL_RX, &I2CDMA_InitStructure);
+ // DMA_Cmd(I2C1_DMA_CHANNEL_RX, ENABLE);
+ // }
+
+ // else
+ // {
+ // I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) I2C2_DR_Address;
+ // DMA_Cmd(I2C2_DMA_CHANNEL_RX, DISABLE);
+ // DMA_Init(I2C2_DMA_CHANNEL_RX, &I2CDMA_InitStructure);
+ // DMA_Cmd(I2C2_DMA_CHANNEL_RX, ENABLE);
+ // }
+
+ // }
}
/**
@@ -522,116 +526,116 @@ void I2C_DMAConfig(I2C_TypeDef* I2Cx, uint8_t* pBuffer, uint32_t BufferSize,
*/
void __attribute__((used)) I2C1_EV_IRQHandler(void)
{
-
- __IO uint32_t SR1Register = 0;
- __IO uint32_t SR2Register = 0;
-
- /* Read the I2C SR1 and SR2 status registers */
- SR1Register = I2C1->SR1;
- SR2Register = I2C1->SR2;
-
- /* If SB = 1, I2C master sent a START on the bus: EV5) */
- if ((SR1Register & 0x0001) == 0x0001)
- {
- /* Send the slave address for transmssion or for reception (according to the configured value
- in the write master write routine */
- I2C1->DR = Address1;
- SR1Register = 0;
- SR2Register = 0;
- }
- /* If I2C is Master (MSL flag = 1) */
-
- if ((SR2Register & 0x0001) == 0x0001)
- {
- /* If ADDR = 1, EV6 */
- if ((SR1Register & 0x0002) == 0x0002)
- {
- /* Write the first data in case the Master is Transmitter */
- if (I2CDirection1 == I2C_DIRECTION_TX)
- {
- /* Initialize the Transmit counter */
- Tx_Idx1 = 0;
- /* Write the first data in the data register */
- I2C1->DR = Buffer_Tx1[Tx_Idx1++];
- /* Decrement the number of bytes to be written */
- NumbOfBytes1--;
- /* If no further data to be sent, disable the I2C BUF IT
- in order to not have a TxE interrupt */
- if (NumbOfBytes1 == 0)
- {
- I2C1->CR2 &= (uint16_t) ~I2C_IT_BUF;
- }
- }
- /* Master Receiver */
- else
- {
- /* Initialize Receive counter */
- Rx_Idx1 = 0;
- /* At this stage, ADDR is cleared because both SR1 and SR2 were read.*/
- /* EV6_1: used for single byte reception. The ACK disable and the STOP
- Programming should be done just after ADDR is cleared. */
- if (NumbOfBytes1 == 1)
- {
- /* Clear ACK */
- I2C1->CR1 &= CR1_ACK_Reset;
- /* Program the STOP */
- I2C1->CR1 |= CR1_STOP_Set;
- }
- }
- SR1Register = 0;
- SR2Register = 0;
- }
- /* Master transmits the remaing data: from data2 until the last one. */
- /* If TXE is set */
- if ((SR1Register & 0x0084) == 0x0080)
- {
- /* If there is still data to write */
- if (NumbOfBytes1 != 0)
- {
- /* Write the data in DR register */
- I2C1->DR = Buffer_Tx1[Tx_Idx1++];
- /* Decrment the number of data to be written */
- NumbOfBytes1--;
- /* If no data remains to write, disable the BUF IT in order
- to not have again a TxE interrupt. */
- if (NumbOfBytes1 == 0)
- {
- /* Disable the BUF IT */
- I2C1->CR2 &= (uint16_t) ~I2C_IT_BUF;
- }
- }
- SR1Register = 0;
- SR2Register = 0;
- }
- /* If BTF and TXE are set (EV8_2), program the STOP */
- if ((SR1Register & 0x0084) == 0x0084)
- {
- /* Program the STOP */
- I2C1->CR1 |= CR1_STOP_Set;
- /* Disable EVT IT In order to not have again a BTF IT */
- I2C1->CR2 &= (uint16_t) ~I2C_IT_EVT;
- SR1Register = 0;
- SR2Register = 0;
- }
- /* If RXNE is set */
- if ((SR1Register & 0x0040) == 0x0040)
- {
- /* Read the data register */
- Buffer_Rx1[Rx_Idx1++] = I2C1->DR;
- /* Decrement the number of bytes to be read */
- NumbOfBytes1--;
- /* If it remains only one byte to read, disable ACK and program the STOP (EV7_1) */
- if (NumbOfBytes1 == 1)
- {
- /* Clear ACK */
- I2C1->CR1 &= CR1_ACK_Reset;
- /* Program the STOP */
- I2C1->CR1 |= CR1_STOP_Set;
- }
- SR1Register = 0;
- SR2Register = 0;
- }
- }
+ //COMMENTED FIRMWARE
+ // __IO uint32_t SR1Register = 0;
+ // __IO uint32_t SR2Register = 0;
+
+ // /* Read the I2C SR1 and SR2 status registers */
+ // SR1Register = I2C1->SR1;
+ // SR2Register = I2C1->SR2;
+
+ // /* If SB = 1, I2C master sent a START on the bus: EV5) */
+ // if ((SR1Register & 0x0001) == 0x0001)
+ // {
+ // /* Send the slave address for transmssion or for reception (according to the configured value
+ // in the write master write routine */
+ // I2C1->DR = Address1;
+ // SR1Register = 0;
+ // SR2Register = 0;
+ // }
+ // /* If I2C is Master (MSL flag = 1) */
+
+ // if ((SR2Register & 0x0001) == 0x0001)
+ // {
+ // /* If ADDR = 1, EV6 */
+ // if ((SR1Register & 0x0002) == 0x0002)
+ // {
+ // /* Write the first data in case the Master is Transmitter */
+ // if (I2CDirection1 == I2C_DIRECTION_TX)
+ // {
+ // /* Initialize the Transmit counter */
+ // Tx_Idx1 = 0;
+ // /* Write the first data in the data register */
+ // I2C1->DR = Buffer_Tx1[Tx_Idx1++];
+ // /* Decrement the number of bytes to be written */
+ // NumbOfBytes1--;
+ // /* If no further data to be sent, disable the I2C BUF IT
+ // in order to not have a TxE interrupt */
+ // if (NumbOfBytes1 == 0)
+ // {
+ // I2C1->CR2 &= (uint16_t) ~I2C_IT_BUF;
+ // }
+ // }
+ // /* Master Receiver */
+ // else
+ // {
+ // /* Initialize Receive counter */
+ // Rx_Idx1 = 0;
+ // /* At this stage, ADDR is cleared because both SR1 and SR2 were read.*/
+ // /* EV6_1: used for single byte reception. The ACK disable and the STOP
+ // Programming should be done just after ADDR is cleared. */
+ // if (NumbOfBytes1 == 1)
+ // {
+ // /* Clear ACK */
+ // I2C1->CR1 &= CR1_ACK_Reset;
+ // /* Program the STOP */
+ // I2C1->CR1 |= CR1_STOP_Set;
+ // }
+ // }
+ // SR1Register = 0;
+ // SR2Register = 0;
+ // }
+ // /* Master transmits the remaing data: from data2 until the last one. */
+ // /* If TXE is set */
+ // if ((SR1Register & 0x0084) == 0x0080)
+ // {
+ // /* If there is still data to write */
+ // if (NumbOfBytes1 != 0)
+ // {
+ // /* Write the data in DR register */
+ // I2C1->DR = Buffer_Tx1[Tx_Idx1++];
+ // /* Decrment the number of data to be written */
+ // NumbOfBytes1--;
+ // /* If no data remains to write, disable the BUF IT in order
+ // to not have again a TxE interrupt. */
+ // if (NumbOfBytes1 == 0)
+ // {
+ // /* Disable the BUF IT */
+ // I2C1->CR2 &= (uint16_t) ~I2C_IT_BUF;
+ // }
+ // }
+ // SR1Register = 0;
+ // SR2Register = 0;
+ // }
+ // /* If BTF and TXE are set (EV8_2), program the STOP */
+ // if ((SR1Register & 0x0084) == 0x0084)
+ // {
+ // /* Program the STOP */
+ // I2C1->CR1 |= CR1_STOP_Set;
+ // /* Disable EVT IT In order to not have again a BTF IT */
+ // I2C1->CR2 &= (uint16_t) ~I2C_IT_EVT;
+ // SR1Register = 0;
+ // SR2Register = 0;
+ // }
+ // /* If RXNE is set */
+ // if ((SR1Register & 0x0040) == 0x0040)
+ // {
+ // /* Read the data register */
+ // Buffer_Rx1[Rx_Idx1++] = I2C1->DR;
+ // /* Decrement the number of bytes to be read */
+ // NumbOfBytes1--;
+ // /* If it remains only one byte to read, disable ACK and program the STOP (EV7_1) */
+ // if (NumbOfBytes1 == 1)
+ // {
+ // /* Clear ACK */
+ // I2C1->CR1 &= CR1_ACK_Reset;
+ // /* Program the STOP */
+ // I2C1->CR1 |= CR1_STOP_Set;
+ // }
+ // SR1Register = 0;
+ // SR2Register = 0;
+ // }
+ // }
}
/**
@@ -641,125 +645,128 @@ void __attribute__((used)) I2C1_EV_IRQHandler(void)
*/
void __attribute__((used)) I2C2_EV_IRQHandler(void)
{
- __IO uint32_t SR1Register = 0;
- __IO uint32_t SR2Register = 0;
-
- /* Read the I2C SR1 and SR2 status registers */
- SR1Register = I2C2->SR1;
- SR2Register = I2C2->SR2;
-
- /* If SB = 1, I2C master sent a START on the bus: EV5) */
- if ((SR1Register & 0x0001) == 0x0001)
- {
- /* Send the slave address for transmssion or for reception (according to the configured value
- in the write master write routine */
- I2C2->DR = Address2;
- SR1Register = 0;
- SR2Register = 0;
- }
- /* If I2C is Master (MSL flag = 1) */
-
- if ((SR2Register & 0x0001) == 0x0001)
- {
- /* If ADDR = 1, EV6 */
- if ((SR1Register & 0x0002) == 0x0002)
- {
- /* Write the first data in case the Master is Transmitter */
- if (I2CDirection2 == I2C_DIRECTION_TX)
- {
- /* Initialize the Transmit counter */
- Tx_Idx2 = 0;
- /* Write the first data in the data register */
- I2C2->DR = Buffer_Tx2[Tx_Idx2++];
- /* Decrement the number of bytes to be written */
- NumbOfBytes2--;
- /* If no further data to be sent, disable the I2C BUF IT
- in order to not have a TxE interrupt */
- if (NumbOfBytes2 == 0)
- {
- I2C2->CR2 &= (uint16_t) ~I2C_IT_BUF;
- }
- }
- /* Master Receiver */
- else
- {
- /* Initialize Receive counter */
- Rx_Idx2 = 0;
- /* At this stage, ADDR is cleared because both SR1 and SR2 were read.*/
- /* EV6_1: used for single byte reception. The ACK disable and the STOP
- Programming should be done just after ADDR is cleared. */
- if (NumbOfBytes2 == 1)
- {
- /* Clear ACK */
- I2C2->CR1 &= CR1_ACK_Reset;
- /* Program the STOP */
- I2C2->CR1 |= CR1_STOP_Set;
- }
- }
- SR1Register = 0;
- SR2Register = 0;
- }
- /* Master transmits the remaing data: from data2 until the last one. */
- /* If TXE is set */
- if ((SR1Register & 0x0084) == 0x0080)
- {
- /* If there is still data to write */
- if (NumbOfBytes2 != 0)
- {
- /* Write the data in DR register */
- I2C2->DR = Buffer_Tx2[Tx_Idx2++];
- /* Decrment the number of data to be written */
- NumbOfBytes2--;
- /* If no data remains to write, disable the BUF IT in order
- to not have again a TxE interrupt. */
- if (NumbOfBytes2 == 0)
- {
- /* Disable the BUF IT */
- I2C2->CR2 &= (uint16_t) ~I2C_IT_BUF;
- }
- }
- SR1Register = 0;
- SR2Register = 0;
- }
- /* If BTF and TXE are set (EV8_2), program the STOP */
- if ((SR1Register & 0x0084) == 0x0084)
- {
- /* Program the STOP */
- I2C2->CR1 |= CR1_STOP_Set;
- /* Disable EVT IT In order to not have again a BTF IT */
- I2C2->CR2 &= (uint16_t) ~I2C_IT_EVT;
- SR1Register = 0;
- SR2Register = 0;
- }
- /* If RXNE is set */
- if ((SR1Register & 0x0040) == 0x0040)
- {
- /* Read the data register */
- Buffer_Rx2[Rx_Idx2++] = I2C2->DR;
- /* Decrement the number of bytes to be read */
- NumbOfBytes2--;
- /* If it remains only one byte to read, disable ACK and program the STOP (EV7_1) */
- if (NumbOfBytes2 == 1)
- {
- /* Clear ACK */
- I2C2->CR1 &= CR1_ACK_Reset;
- /* Program the STOP */
- I2C2->CR1 |= CR1_STOP_Set;
- }
- SR1Register = 0;
- SR2Register = 0;
- }
- }
+ //COMMENTED FIRMWARE
+ // __IO uint32_t SR1Register = 0;
+ // __IO uint32_t SR2Register = 0;
+
+ // /* Read the I2C SR1 and SR2 status registers */
+ // SR1Register = I2C2->SR1;
+ // SR2Register = I2C2->SR2;
+
+ // /* If SB = 1, I2C master sent a START on the bus: EV5) */
+ // if ((SR1Register & 0x0001) == 0x0001)
+ // {
+ // /* Send the slave address for transmssion or for reception (according to the configured value
+ // in the write master write routine */
+ // I2C2->DR = Address2;
+ // SR1Register = 0;
+ // SR2Register = 0;
+ // }
+ // /* If I2C is Master (MSL flag = 1) */
+
+ // if ((SR2Register & 0x0001) == 0x0001)
+ // {
+ // /* If ADDR = 1, EV6 */
+ // if ((SR1Register & 0x0002) == 0x0002)
+ // {
+ // /* Write the first data in case the Master is Transmitter */
+ // if (I2CDirection2 == I2C_DIRECTION_TX)
+ // {
+ // /* Initialize the Transmit counter */
+ // Tx_Idx2 = 0;
+ // /* Write the first data in the data register */
+ // I2C2->DR = Buffer_Tx2[Tx_Idx2++];
+ // /* Decrement the number of bytes to be written */
+ // NumbOfBytes2--;
+ // /* If no further data to be sent, disable the I2C BUF IT
+ // in order to not have a TxE interrupt */
+ // if (NumbOfBytes2 == 0)
+ // {
+ // I2C2->CR2 &= (uint16_t) ~I2C_IT_BUF;
+ // }
+ // }
+ // /* Master Receiver */
+ // else
+ // {
+ // /* Initialize Receive counter */
+ // Rx_Idx2 = 0;
+ // /* At this stage, ADDR is cleared because both SR1 and SR2 were read.*/
+ // /* EV6_1: used for single byte reception. The ACK disable and the STOP
+ // Programming should be done just after ADDR is cleared. */
+ // if (NumbOfBytes2 == 1)
+ // {
+ // /* Clear ACK */
+ // I2C2->CR1 &= CR1_ACK_Reset;
+ // /* Program the STOP */
+ // I2C2->CR1 |= CR1_STOP_Set;
+ // }
+ // }
+ // SR1Register = 0;
+ // SR2Register = 0;
+ // }
+ // /* Master transmits the remaing data: from data2 until the last one. */
+ // /* If TXE is set */
+ // if ((SR1Register & 0x0084) == 0x0080)
+ // {
+ // /* If there is still data to write */
+ // if (NumbOfBytes2 != 0)
+ // {
+ // /* Write the data in DR register */
+ // I2C2->DR = Buffer_Tx2[Tx_Idx2++];
+ // /* Decrment the number of data to be written */
+ // NumbOfBytes2--;
+ // /* If no data remains to write, disable the BUF IT in order
+ // to not have again a TxE interrupt. */
+ // if (NumbOfBytes2 == 0)
+ // {
+ // /* Disable the BUF IT */
+ // I2C2->CR2 &= (uint16_t) ~I2C_IT_BUF;
+ // }
+ // }
+ // SR1Register = 0;
+ // SR2Register = 0;
+ // }
+ // /* If BTF and TXE are set (EV8_2), program the STOP */
+ // if ((SR1Register & 0x0084) == 0x0084)
+ // {
+ // /* Program the STOP */
+ // I2C2->CR1 |= CR1_STOP_Set;
+ // /* Disable EVT IT In order to not have again a BTF IT */
+ // I2C2->CR2 &= (uint16_t) ~I2C_IT_EVT;
+ // SR1Register = 0;
+ // SR2Register = 0;
+ // }
+ // /* If RXNE is set */
+ // if ((SR1Register & 0x0040) == 0x0040)
+ // {
+ // /* Read the data register */
+ // Buffer_Rx2[Rx_Idx2++] = I2C2->DR;
+ // /* Decrement the number of bytes to be read */
+ // NumbOfBytes2--;
+ // /* If it remains only one byte to read, disable ACK and program the STOP (EV7_1) */
+ // if (NumbOfBytes2 == 1)
+ // {
+ // /* Clear ACK */
+ // I2C2->CR1 &= CR1_ACK_Reset;
+ // /* Program the STOP */
+ // I2C2->CR1 |= CR1_STOP_Set;
+ // }
+ // SR1Register = 0;
+ // SR2Register = 0;
+ // }
+ // }
}
void __attribute__((used)) I2C1_ER_IRQHandler(void)
{
- i2cErrorInterruptHandler(I2C1);
+ //COMMENTED FIRMWARE
+ //i2cErrorInterruptHandler(I2C1);
}
void __attribute__((used)) I2C2_ER_IRQHandler(void)
{
- i2cErrorInterruptHandler(I2C2);
+ //COMMENTED FIRMWARE
+ //i2cErrorInterruptHandler(I2C2);
}
/**
@@ -769,35 +776,35 @@ void __attribute__((used)) I2C2_ER_IRQHandler(void)
*/
void i2cErrorInterruptHandler(I2C_TypeDef* I2Cx)
{
-
- __IO uint32_t SR1Register = 0;
-
- /* Read the I2C status register */
- SR1Register = I2Cx->SR1;
- /* If AF = 1 */
- if ((SR1Register & 0x0400) == 0x0400)
- {
- I2Cx->SR1 &= 0xFBFF;
- SR1Register = 0;
- }
- /* If ARLO = 1 */
- if ((SR1Register & 0x0200) == 0x0200)
- {
- I2Cx->SR1 &= 0xFBFF;
- SR1Register = 0;
- }
- /* If BERR = 1 */
- if ((SR1Register & 0x0100) == 0x0100)
- {
- I2Cx->SR1 &= 0xFEFF;
- SR1Register = 0;
- }
- /* If OVR = 1 */
- if ((SR1Register & 0x0800) == 0x0800)
- {
- I2Cx->SR1 &= 0xF7FF;
- SR1Register = 0;
- }
+ //COMMENTED FIRMWARE
+ // __IO uint32_t SR1Register = 0;
+
+ // /* Read the I2C status register */
+ // SR1Register = I2Cx->SR1;
+ // /* If AF = 1 */
+ // if ((SR1Register & 0x0400) == 0x0400)
+ // {
+ // I2Cx->SR1 &= 0xFBFF;
+ // SR1Register = 0;
+ // }
+ // /* If ARLO = 1 */
+ // if ((SR1Register & 0x0200) == 0x0200)
+ // {
+ // I2Cx->SR1 &= 0xFBFF;
+ // SR1Register = 0;
+ // }
+ // /* If BERR = 1 */
+ // if ((SR1Register & 0x0100) == 0x0100)
+ // {
+ // I2Cx->SR1 &= 0xFEFF;
+ // SR1Register = 0;
+ // }
+ // /* If OVR = 1 */
+ // if ((SR1Register & 0x0800) == 0x0800)
+ // {
+ // I2Cx->SR1 &= 0xF7FF;
+ // SR1Register = 0;
+ // }
}
/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/led.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/led.c
index ade1a68597595c6b6960ad18b5171b1e76ef92df..c538e7aab83c87f36d13864891328573cc838f89 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/led.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/led.c
@@ -63,85 +63,90 @@ static bool isInit = false;
//Initialize the green led pin as output
void ledInit()
{
- int i;
+ //COMMENTED FIRMWARE
+ // int i;
- if(isInit)
- return;
+ // if(isInit)
+ // return;
- GPIO_InitTypeDef GPIO_InitStructure;
+ // GPIO_InitTypeDef GPIO_InitStructure;
- // Enable GPIO
- RCC_AHB1PeriphClockCmd(LED_GPIO_PERIF, ENABLE);
+ // // Enable GPIO
+ // RCC_AHB1PeriphClockCmd(LED_GPIO_PERIF, ENABLE);
- for (i = 0; i < LED_NUM; i++)
- {
- //Initialize the LED pins as an output
- GPIO_InitStructure.GPIO_Pin = led_pin[i];
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
- GPIO_Init(led_port[i], &GPIO_InitStructure);
+ // for (i = 0; i < LED_NUM; i++)
+ // {
+ // //Initialize the LED pins as an output
+ // GPIO_InitStructure.GPIO_Pin = led_pin[i];
+ // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+ // GPIO_Init(led_port[i], &GPIO_InitStructure);
- //Turn off the LED:s
- ledSet(i, 0);
- }
+ // //Turn off the LED:s
+ // ledSet(i, 0);
+ // }
- isInit = true;
+ // isInit = true;
}
bool ledTest(void)
{
- ledSet(LED_GREEN_L, 1);
- ledSet(LED_GREEN_R, 1);
- ledSet(LED_RED_L, 0);
- ledSet(LED_RED_R, 0);
- vTaskDelay(M2T(250));
- ledSet(LED_GREEN_L, 0);
- ledSet(LED_GREEN_R, 0);
- ledSet(LED_RED_L, 1);
- ledSet(LED_RED_R, 1);
- vTaskDelay(M2T(250));
-
- // LED test end
- ledClearAll();
- ledSet(LED_BLUE_L, 1);
+ //COMMENTED FIRMWARE
+ // ledSet(LED_GREEN_L, 1);
+ // ledSet(LED_GREEN_R, 1);
+ // ledSet(LED_RED_L, 0);
+ // ledSet(LED_RED_R, 0);
+ // vTaskDelay(M2T(250));
+ // ledSet(LED_GREEN_L, 0);
+ // ledSet(LED_GREEN_R, 0);
+ // ledSet(LED_RED_L, 1);
+ // ledSet(LED_RED_R, 1);
+ // vTaskDelay(M2T(250));
+
+ // // LED test end
+ // ledClearAll();
+ // ledSet(LED_BLUE_L, 1);
return isInit;
}
void ledClearAll(void)
{
- int i;
-
- for (i = 0; i < LED_NUM; i++)
- {
- //Turn off the LED:s
- ledSet(i, 0);
- }
+ //COMMENTED FIRMWARE
+ // int i;
+
+ // for (i = 0; i < LED_NUM; i++)
+ // {
+ // //Turn off the LED:s
+ // ledSet(i, 0);
+ // }
}
void ledSetAll(void)
{
- int i;
-
- for (i = 0; i < LED_NUM; i++)
- {
- //Turn on the LED:s
- ledSet(i, 1);
- }
+ //COMMENTED FIRMWARE
+ // int i;
+
+ // for (i = 0; i < LED_NUM; i++)
+ // {
+ // //Turn on the LED:s
+ // ledSet(i, 1);
+ // }
}
void ledSet(led_t led, bool value)
{
- if (led>LED_NUM)
- return;
+ //COMMENTED FIRMWARE
+ // if (led>LED_NUM)
+ // return;
- if (led_polarity[led]==LED_POL_NEG)
- value = !value;
+ // if (led_polarity[led]==LED_POL_NEG)
+ // value = !value;
- if(value)
- GPIO_SetBits(led_port[led], led_pin[led]);
- else
- GPIO_ResetBits(led_port[led], led_pin[led]);
+ // if(value)
+ // GPIO_SetBits(led_port[led], led_pin[led]);
+ // else
+ // GPIO_ResetBits(led_port[led], led_pin[led]);
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lh_bootloader.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lh_bootloader.c
index 8bf183247b54babb7effdb4a564fd252cc229f5c..1c3715cd4bbb1f3239d7ac066f0c864ac4bee8ba 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lh_bootloader.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lh_bootloader.c
@@ -60,11 +60,12 @@ static bool lhExchange(uint16_t writeLen, uint8_t* writeData, uint16_t readLen,
{
bool status = false;
- status = i2cdevWrite(I2C1_DEV, devAddr, writeLen, writeData);
- if (status && readLen && readData)
- {
- status = i2cdevRead(I2C1_DEV, devAddr, readLen, readData);
- }
+ //COMMENTED FIRMWARE
+ // status = i2cdevWrite(I2C1_DEV, devAddr, writeLen, writeData);
+ // if (status && readLen && readData)
+ // {
+ // status = i2cdevRead(I2C1_DEV, devAddr, readLen, readData);
+ // }
return status;
}
@@ -110,27 +111,30 @@ bool lhblTest()
static bool lhblFlashReadStatus(uint8_t* status)
{
- lhblHeaderPrepare(1, 1, flashWriteBuf);
- flashWriteBuf[5] = FLASH_CMD_READ_STATUS;
+ //COMMENTED FIRMWARE
+ // lhblHeaderPrepare(1, 1, flashWriteBuf);
+ // flashWriteBuf[5] = FLASH_CMD_READ_STATUS;
return lhExchange(5+1, flashWriteBuf, 1, status);
}
static bool lhblFlashWriteEnable(void)
{
- lhblHeaderPrepare(1, 0, flashWriteBuf);
- flashWriteBuf[5] = FLASH_CMD_WRITE_EN;
+ //COMMENTED FIRMWARE
+ // lhblHeaderPrepare(1, 0, flashWriteBuf);
+ // flashWriteBuf[5] = FLASH_CMD_WRITE_EN;
return lhExchange(5 + 1, flashWriteBuf, 0, 0);
}
static bool lhblFlashEraseBlock64k(uint32_t address)
{
- lhblHeaderPrepare(4, 0, flashWriteBuf);
- flashWriteBuf[5] = FLASH_CMD_ERASE_SECTOR;
- flashWriteBuf[6] = (uint8_t)((address >> 16) & 0x000000FF);
- flashWriteBuf[7] = (uint8_t)((address >> 8) & 0x000000FF);
- flashWriteBuf[8] = (uint8_t)((address >> 0) & 0x000000FF);
+ //COMMENTED FIRMWARE
+ // lhblHeaderPrepare(4, 0, flashWriteBuf);
+ // flashWriteBuf[5] = FLASH_CMD_ERASE_SECTOR;
+ // flashWriteBuf[6] = (uint8_t)((address >> 16) & 0x000000FF);
+ // flashWriteBuf[7] = (uint8_t)((address >> 8) & 0x000000FF);
+ // flashWriteBuf[8] = (uint8_t)((address >> 0) & 0x000000FF);
return lhExchange(5 + 4, flashWriteBuf, 0, 0);
}
@@ -138,33 +142,35 @@ static bool lhblFlashEraseBlock64k(uint32_t address)
static bool lhblFlashWaitComplete(void)
{
bool status;
- uint8_t flashStatus;
+ //COMMENTED FIRMWARE
+ // uint8_t flashStatus;
- status = lhblFlashReadStatus(&flashStatus);
+ // status = lhblFlashReadStatus(&flashStatus);
- while ((flashStatus & 0x01) != 0)
- {
- vTaskDelay(M2T(1));
- status &= lhblFlashReadStatus(&flashStatus);
- }
+ // while ((flashStatus & 0x01) != 0)
+ // {
+ // vTaskDelay(M2T(1));
+ // status &= lhblFlashReadStatus(&flashStatus);
+ // }
return status;
}
bool lhblFlashWritePage(uint32_t address, uint16_t length, const uint8_t *data)
{
- ASSERT(address >= LH_FW_ADDR);
- ASSERT(length <= LH_FLASH_PAGE_SIZE);
+ //COMMENTED FIRMWARE
+ // ASSERT(address >= LH_FW_ADDR);
+ // ASSERT(length <= LH_FLASH_PAGE_SIZE);
- lhblFlashWriteEnable();
+ // lhblFlashWriteEnable();
- lhblHeaderPrepare(4 + length, 0, flashWriteBuf);
- flashWriteBuf[5] = FLASH_CMD_WRITE_PAGE;
- flashWriteBuf[6] = (uint8_t)((address >> 16) & 0x000000FF);
- flashWriteBuf[7] = (uint8_t)((address >> 8) & 0x000000FF);
- flashWriteBuf[8] = (uint8_t)((address >> 0) & 0x000000FF);
+ // lhblHeaderPrepare(4 + length, 0, flashWriteBuf);
+ // flashWriteBuf[5] = FLASH_CMD_WRITE_PAGE;
+ // flashWriteBuf[6] = (uint8_t)((address >> 16) & 0x000000FF);
+ // flashWriteBuf[7] = (uint8_t)((address >> 8) & 0x000000FF);
+ // flashWriteBuf[8] = (uint8_t)((address >> 0) & 0x000000FF);
- memcpy(&flashWriteBuf[9], data, length);
+ // memcpy(&flashWriteBuf[9], data, length);
return lhExchange(5 + 4 + length, flashWriteBuf, 0, 0);
}
@@ -191,19 +197,21 @@ bool lhblFlashGetProtocolVersion(void)
bool lhblFlashRead(uint32_t address, uint16_t length, uint8_t *data)
{
- lhblHeaderPrepare(4, length, flashWriteBuf);
- flashWriteBuf[5] = FLASH_CMD_READ;
- flashWriteBuf[6] = (uint8_t)((address >> 16) & 0x000000FF);
- flashWriteBuf[7] = (uint8_t)((address >> 8) & 0x000000FF);
- flashWriteBuf[8] = (uint8_t)((address >> 0) & 0x000000FF);
+ //COMMENTED FIRMWARE
+ // lhblHeaderPrepare(4, length, flashWriteBuf);
+ // flashWriteBuf[5] = FLASH_CMD_READ;
+ // flashWriteBuf[6] = (uint8_t)((address >> 16) & 0x000000FF);
+ // flashWriteBuf[7] = (uint8_t)((address >> 8) & 0x000000FF);
+ // flashWriteBuf[8] = (uint8_t)((address >> 0) & 0x000000FF);
return lhExchange(5 + 4, flashWriteBuf, length, data);
}
bool lhblFlashWakeup(void)
{
- lhblHeaderPrepare(1, 0, flashWriteBuf);
- flashWriteBuf[5] = FLASH_CMD_WAKEUP;
+ //COMMENTED FIRMWARE
+ // lhblHeaderPrepare(1, 0, flashWriteBuf);
+ // flashWriteBuf[5] = FLASH_CMD_WAKEUP;
return lhExchange(5 + 1, flashWriteBuf, 0, 0);
}
@@ -212,14 +220,15 @@ bool lhblFlashEraseFirmware(void)
{
bool status;
+//COMMENTED FIRMWARE
/* Erase first 64K */
- lhblFlashWriteEnable();
- status = lhblFlashEraseBlock64k(LH_FW_ADDR);
- status &= lhblFlashWaitComplete();
- /* Erase last 64K */
- lhblFlashWriteEnable();
- status &= lhblFlashEraseBlock64k(LH_FW_ADDR + 0x10000);
- status &= lhblFlashWaitComplete();
+ // lhblFlashWriteEnable();
+ // status = lhblFlashEraseBlock64k(LH_FW_ADDR);
+ // status &= lhblFlashWaitComplete();
+ // /* Erase last 64K */
+ // lhblFlashWriteEnable();
+ // status &= lhblFlashEraseBlock64k(LH_FW_ADDR + 0x10000);
+ // status &= lhblFlashWaitComplete();
return status;
}
@@ -227,44 +236,45 @@ bool lhblFlashEraseFirmware(void)
bool lhblFlashWriteFW(uint8_t *data, uint32_t length)
{
bool status = true;
- int pageCount = 0;
- int nbrPages = length / LH_FLASH_PAGE_SIZE;
- int lastPageSize = length % LH_FLASH_PAGE_SIZE;
-
- ASSERT(length <= LH_FW_SIZE);
-
- for (pageCount = 0; pageCount < nbrPages && status; pageCount++)
- {
- status = lhblFlashWritePage(LH_FW_ADDR + pageCount * LH_FLASH_PAGE_SIZE,
- LH_FLASH_PAGE_SIZE,
- &data[pageCount * LH_FLASH_PAGE_SIZE]);
- lhblFlashWaitComplete();
-
- lhblFlashRead(LH_FW_ADDR + pageCount * LH_FLASH_PAGE_SIZE,
- LH_FLASH_PAGE_SIZE,
- flashReadBuf);
-
- if (verify(&data[pageCount * LH_FLASH_PAGE_SIZE], flashReadBuf, LH_FLASH_PAGE_SIZE) == false)
- {
- return false;
- }
- }
- if (lastPageSize && status)
- {
- status = lhblFlashWritePage(LH_FW_ADDR + pageCount * LH_FLASH_PAGE_SIZE,
- lastPageSize,
- &data[pageCount * LH_FLASH_PAGE_SIZE]);
- lhblFlashWaitComplete();
-
- lhblFlashRead(LH_FW_ADDR + pageCount * LH_FLASH_PAGE_SIZE,
- lastPageSize,
- flashReadBuf);
-
- if (verify(&data[pageCount * LH_FLASH_PAGE_SIZE], flashReadBuf, lastPageSize) == false)
- {
- return false;
- }
- }
+ //COMMENTED FIRMWARE
+ // int pageCount = 0;
+ // int nbrPages = length / LH_FLASH_PAGE_SIZE;
+ // int lastPageSize = length % LH_FLASH_PAGE_SIZE;
+
+ // ASSERT(length <= LH_FW_SIZE);
+
+ // for (pageCount = 0; pageCount < nbrPages && status; pageCount++)
+ // {
+ // status = lhblFlashWritePage(LH_FW_ADDR + pageCount * LH_FLASH_PAGE_SIZE,
+ // LH_FLASH_PAGE_SIZE,
+ // &data[pageCount * LH_FLASH_PAGE_SIZE]);
+ // lhblFlashWaitComplete();
+
+ // lhblFlashRead(LH_FW_ADDR + pageCount * LH_FLASH_PAGE_SIZE,
+ // LH_FLASH_PAGE_SIZE,
+ // flashReadBuf);
+
+ // if (verify(&data[pageCount * LH_FLASH_PAGE_SIZE], flashReadBuf, LH_FLASH_PAGE_SIZE) == false)
+ // {
+ // return false;
+ // }
+ // }
+ // if (lastPageSize && status)
+ // {
+ // status = lhblFlashWritePage(LH_FW_ADDR + pageCount * LH_FLASH_PAGE_SIZE,
+ // lastPageSize,
+ // &data[pageCount * LH_FLASH_PAGE_SIZE]);
+ // lhblFlashWaitComplete();
+
+ // lhblFlashRead(LH_FW_ADDR + pageCount * LH_FLASH_PAGE_SIZE,
+ // lastPageSize,
+ // flashReadBuf);
+
+ // if (verify(&data[pageCount * LH_FLASH_PAGE_SIZE], flashReadBuf, lastPageSize) == false)
+ // {
+ // return false;
+ // }
+ // }
return status;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lh_flasher.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lh_flasher.c
index 9f18fe76974e7335d609656048ba6609556d99fe..b13fdb0e25572875c115715a575c206d03ca0516 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lh_flasher.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lh_flasher.c
@@ -93,37 +93,39 @@ static uint32_t statusCode = 0;
static bool lhExchange(uint16_t writeLen, uint8_t* writeData, uint16_t readLen, uint8_t* readData)
{
- static uint8_t wbuffer[1024];
- static uint8_t rbuffer[1024];
+ //COMMENTED FIRMWARE
+ // static uint8_t wbuffer[1024];
+ // static uint8_t rbuffer[1024];
- memcpy(wbuffer, writeData, writeLen);
+ // memcpy(wbuffer, writeData, writeLen);
- spiBeginTransaction(SPI_BAUDRATE_2MHZ);
+ // spiBeginTransaction(SPI_BAUDRATE_2MHZ);
- digitalWrite(LH_SPI_CS, 0);
+ // digitalWrite(LH_SPI_CS, 0);
- spiExchange(writeLen+readLen, wbuffer, rbuffer);
+ // spiExchange(writeLen+readLen, wbuffer, rbuffer);
- digitalWrite(LH_SPI_CS, 1);
+ // digitalWrite(LH_SPI_CS, 1);
- spiEndTransaction();
+ // spiEndTransaction();
- if (readLen > 0) {
- memcpy(readData, &rbuffer[writeLen], readLen);
- }
+ // if (readLen > 0) {
+ // memcpy(readData, &rbuffer[writeLen], readLen);
+ // }
return true;
}
static bool verify(uint8_t *dataA, uint8_t *dataB, uint16_t length)
{
- for (int i = 0; i < length; i++)
- {
- if (dataA[i] != dataB[i])
- {
- return false;
- }
- }
+ //COMMENTED FIRMWARE
+ // for (int i = 0; i < length; i++)
+ // {
+ // if (dataA[i] != dataB[i])
+ // {
+ // return false;
+ // }
+ // }
return true;
}
@@ -133,15 +135,16 @@ bool lhflashInit()
if (isInit)
return true;
- spiBegin();
+ //COMMENTED FIRMWARE
+ // spiBegin();
- pinMode(LH_FPGA_RESET, OUTPUT);
- pinMode(LH_SPI_CS, INPUT);
+ // pinMode(LH_FPGA_RESET, OUTPUT);
+ // pinMode(LH_SPI_CS, INPUT);
- digitalWrite(LH_FPGA_RESET, 1);
+ // digitalWrite(LH_FPGA_RESET, 1);
- isInit = true;
+ // isInit = true;
return true;
}
@@ -156,25 +159,27 @@ static bool flashReadStatus(uint8_t* status)
static bool flashWriteEnable(void)
{
bool status = true;
- uint8_t flash_status = 0;
+ //COMMENTED FIRMWARE
+ // uint8_t flash_status = 0;
- do {
- flashWriteBuf[0] = FLASH_CMD_WRITE_EN;
+ // do {
+ // flashWriteBuf[0] = FLASH_CMD_WRITE_EN;
- status &= lhExchange(1, flashWriteBuf, 0, 0);
+ // status &= lhExchange(1, flashWriteBuf, 0, 0);
- status &= flashReadStatus(&flash_status);
- } while ((flash_status & 0x02) == 0);
+ // status &= flashReadStatus(&flash_status);
+ // } while ((flash_status & 0x02) == 0);
return status;
}
static bool flashEraseBlock64k(uint32_t address)
{
- flashWriteBuf[0] = FLASH_CMD_ERASE_SECTOR;
- flashWriteBuf[1] = (uint8_t)((address >> 16) & 0x000000FF);
- flashWriteBuf[2] = (uint8_t)((address >> 8) & 0x000000FF);
- flashWriteBuf[3] = (uint8_t)((address >> 0) & 0x000000FF);
+ //COMMENTED FIRMWARE
+ // flashWriteBuf[0] = FLASH_CMD_ERASE_SECTOR;
+ // flashWriteBuf[1] = (uint8_t)((address >> 16) & 0x000000FF);
+ // flashWriteBuf[2] = (uint8_t)((address >> 8) & 0x000000FF);
+ // flashWriteBuf[3] = (uint8_t)((address >> 0) & 0x000000FF);
return lhExchange(4, flashWriteBuf, 0, 0);
}
@@ -182,61 +187,67 @@ static bool flashEraseBlock64k(uint32_t address)
static bool flashWaitComplete(void)
{
bool status;
- uint8_t flashStatus;
+ //COMMENTED FIRMWARE
+ // uint8_t flashStatus;
- status = flashReadStatus(&flashStatus);
+ // status = flashReadStatus(&flashStatus);
- while ((flashStatus & 0x01) != 0)
- {
- vTaskDelay(M2T(1));
- status &= flashReadStatus(&flashStatus);
- }
+ // while ((flashStatus & 0x01) != 0)
+ // {
+ // vTaskDelay(M2T(1));
+ // status &= flashReadStatus(&flashStatus);
+ // }
return status;
}
static bool flashWritePage(uint32_t address, uint16_t length, uint8_t *data)
{
- ASSERT(length <= LH_FLASH_PAGE_SIZE);
+ //COMMENTED FIRMWARE
+ // ASSERT(length <= LH_FLASH_PAGE_SIZE);
- flashWriteEnable();
+ // flashWriteEnable();
- flashWriteBuf[0] = FLASH_CMD_WRITE_PAGE;
- flashWriteBuf[1] = (uint8_t)((address >> 16) & 0x000000FF);
- flashWriteBuf[2] = (uint8_t)((address >> 8) & 0x000000FF);
- flashWriteBuf[3] = (uint8_t)((address >> 0) & 0x000000FF);
+ // flashWriteBuf[0] = FLASH_CMD_WRITE_PAGE;
+ // flashWriteBuf[1] = (uint8_t)((address >> 16) & 0x000000FF);
+ // flashWriteBuf[2] = (uint8_t)((address >> 8) & 0x000000FF);
+ // flashWriteBuf[3] = (uint8_t)((address >> 0) & 0x000000FF);
- memcpy(&flashWriteBuf[4], data, length);
+ // memcpy(&flashWriteBuf[4], data, length);
return lhExchange(4 + length, flashWriteBuf, 0, 0);
}
static void fpgaHoldReset(void)
{
- digitalWrite(LH_FPGA_RESET, 0);
- pinMode(LH_SPI_CS, OUTPUT);
- digitalWrite(LH_SPI_CS, 1);
+ //COMMENTED FIRMWARE
+ // digitalWrite(LH_FPGA_RESET, 0);
+ // pinMode(LH_SPI_CS, OUTPUT);
+ // digitalWrite(LH_SPI_CS, 1);
}
static void fpgaReleaseReset(void)
{
- pinMode(LH_SPI_CS, INPUT);
- digitalWrite(LH_FPGA_RESET, 1);
+ //COMMENTED FIRMWARE
+ // pinMode(LH_SPI_CS, INPUT);
+ // digitalWrite(LH_FPGA_RESET, 1);
}
static bool flashRead(uint32_t address, uint16_t length, uint8_t *data)
{
- flashWriteBuf[0] = FLASH_CMD_READ;
- flashWriteBuf[1] = (uint8_t)((address >> 16) & 0x000000FF);
- flashWriteBuf[2] = (uint8_t)((address >> 8) & 0x000000FF);
- flashWriteBuf[3] = (uint8_t)((address >> 0) & 0x000000FF);
+ //COMMENTED FIRMWARE
+ // flashWriteBuf[0] = FLASH_CMD_READ;
+ // flashWriteBuf[1] = (uint8_t)((address >> 16) & 0x000000FF);
+ // flashWriteBuf[2] = (uint8_t)((address >> 8) & 0x000000FF);
+ // flashWriteBuf[3] = (uint8_t)((address >> 0) & 0x000000FF);
return lhExchange(4, flashWriteBuf, length, data);
}
static bool flashWakeup(void)
{
- flashWriteBuf[0] = FLASH_CMD_WAKEUP;
+ //COMMENTED FIRMWARE
+ //flashWriteBuf[0] = FLASH_CMD_WAKEUP;
return lhExchange(1, flashWriteBuf, 0, 0);
}
@@ -252,13 +263,15 @@ static bool flashReset()
{
bool status = true;
- flashWriteBuf[0] = 0x66;
+ //COMMENTED FIRMWARE
+
+ // flashWriteBuf[0] = 0x66;
- status &= lhExchange(1, flashWriteBuf, 0, 0);
+ // status &= lhExchange(1, flashWriteBuf, 0, 0);
- flashWriteBuf[0] = 0x99;
+ // flashWriteBuf[0] = 0x99;
- status &= lhExchange(1, flashWriteBuf, 0, 0);
+ // status &= lhExchange(1, flashWriteBuf, 0, 0);
return status;
}
@@ -268,24 +281,26 @@ static bool flashErase(void)
bool status = true;
/* Disable write protection */
- flashWriteEnable();
+ //COMMENTED FIRMWARE
+ // flashWriteEnable();
- flashWriteBuf[0] = FLASH_CMD_WRITE_STATUS;
- flashWriteBuf[1] = 0;
+ // flashWriteBuf[0] = FLASH_CMD_WRITE_STATUS;
+ // flashWriteBuf[1] = 0;
- lhExchange(2, flashWriteBuf, 0, NULL);
+ // lhExchange(2, flashWriteBuf, 0, NULL);
- status &= flashWaitComplete();
+ // status &= flashWaitComplete();
- /* Erase first 128K */
- flashWriteEnable();
- status = flashEraseBlock64k(0);
- status &= flashWaitComplete();
+ // /* Erase first 128K */
+ // flashWriteEnable();
+ // status = flashEraseBlock64k(0);
+ // status &= flashWaitComplete();
- flashWriteEnable();
- status &= flashEraseBlock64k(0x10000);
- status &= flashWaitComplete();
+ // flashWriteEnable();
+ // status &= flashEraseBlock64k(0x10000);
+ // status &= flashWaitComplete();
+ //THIS WAS ALREADY COMMENTED
// flashWriteEnable();
// status = flashEraseBlock64k(0x20000);
// status &= flashWaitComplete();
@@ -299,57 +314,59 @@ static bool flashErase(void)
static void flashProtectBootloader()
{
- flashWriteEnable();
+ //COMMENTED FIRMWARE
+ // flashWriteEnable();
- flashWriteBuf[0] = FLASH_CMD_WRITE_STATUS;
- flashWriteBuf[1] = 0x28;
+ // flashWriteBuf[0] = FLASH_CMD_WRITE_STATUS;
+ // flashWriteBuf[1] = 0x28;
- lhExchange(2, flashWriteBuf, 0, NULL);
+ // lhExchange(2, flashWriteBuf, 0, NULL);
- flashWaitComplete();
+ // flashWaitComplete();
}
static bool flashWriteFW(uint8_t *data, uint32_t length)
{
bool status = true;
- int pageCount = 0;
- int nbrPages = length / LH_FLASH_PAGE_SIZE;
- int lastPageSize = length % LH_FLASH_PAGE_SIZE;
-
- for (pageCount = 0; pageCount < nbrPages && status; pageCount++)
- {
- status = flashWritePage(pageCount * LH_FLASH_PAGE_SIZE,
- LH_FLASH_PAGE_SIZE,
- &data[pageCount * LH_FLASH_PAGE_SIZE]);
- flashWaitComplete();
-
- flashRead(pageCount * LH_FLASH_PAGE_SIZE,
- LH_FLASH_PAGE_SIZE,
- flashReadBuf);
-
- if (verify(&data[pageCount * LH_FLASH_PAGE_SIZE], flashReadBuf, LH_FLASH_PAGE_SIZE) == false)
- {
- DEBUG_PRINT("Verify fail page %d!\n", pageCount);
- statusCode = 0x80000000 | pageCount;
- return false;
- }
- }
- if (lastPageSize && status)
- {
- status = flashWritePage(pageCount * LH_FLASH_PAGE_SIZE,
- lastPageSize,
- &data[pageCount * LH_FLASH_PAGE_SIZE]);
- flashWaitComplete();
-
- flashRead(pageCount * LH_FLASH_PAGE_SIZE,
- lastPageSize,
- flashReadBuf);
-
- if (verify(&data[pageCount * LH_FLASH_PAGE_SIZE], flashReadBuf, lastPageSize) == false)
- {
- return false;
- }
- }
+ //COMMENTED FIRMWARE
+ // int pageCount = 0;
+ // int nbrPages = length / LH_FLASH_PAGE_SIZE;
+ // int lastPageSize = length % LH_FLASH_PAGE_SIZE;
+
+ // for (pageCount = 0; pageCount < nbrPages && status; pageCount++)
+ // {
+ // status = flashWritePage(pageCount * LH_FLASH_PAGE_SIZE,
+ // LH_FLASH_PAGE_SIZE,
+ // &data[pageCount * LH_FLASH_PAGE_SIZE]);
+ // flashWaitComplete();
+
+ // flashRead(pageCount * LH_FLASH_PAGE_SIZE,
+ // LH_FLASH_PAGE_SIZE,
+ // flashReadBuf);
+
+ // if (verify(&data[pageCount * LH_FLASH_PAGE_SIZE], flashReadBuf, LH_FLASH_PAGE_SIZE) == false)
+ // {
+ // DEBUG_PRINT("Verify fail page %d!\n", pageCount);
+ // statusCode = 0x80000000 | pageCount;
+ // return false;
+ // }
+ // }
+ // if (lastPageSize && status)
+ // {
+ // status = flashWritePage(pageCount * LH_FLASH_PAGE_SIZE,
+ // lastPageSize,
+ // &data[pageCount * LH_FLASH_PAGE_SIZE]);
+ // flashWaitComplete();
+
+ // flashRead(pageCount * LH_FLASH_PAGE_SIZE,
+ // lastPageSize,
+ // flashReadBuf);
+
+ // if (verify(&data[pageCount * LH_FLASH_PAGE_SIZE], flashReadBuf, lastPageSize) == false)
+ // {
+ // return false;
+ // }
+ // }
return status;
}
@@ -357,35 +374,35 @@ static bool flashWriteFW(uint8_t *data, uint32_t length)
bool lhflashFlashBootloader()
{
bool pass = true;
+ //COMMENTED FIRMWARE
+ // fpgaHoldReset();
- fpgaHoldReset();
-
- vTaskDelay(M2T(10));
+ // vTaskDelay(M2T(10));
- flashWakeup();
- vTaskDelay(M2T(10));
+ // flashWakeup();
+ // vTaskDelay(M2T(10));
- flashErase();
+ // flashErase();
- pass &= flashWriteFW((uint8_t *)bootloader, bootloaderSize);
+ // pass &= flashWriteFW((uint8_t *)bootloader, bootloaderSize);
- // Freeze here if the flashing is unsuccessful
- if (pass == false) {
- statusDone = true;
- while(1) {
- vTaskDelay(portMAX_DELAY);
- }
- }
+ // // Freeze here if the flashing is unsuccessful
+ // if (pass == false) {
+ // statusDone = true;
+ // while(1) {
+ // vTaskDelay(portMAX_DELAY);
+ // }
+ // }
- flashProtectBootloader();
+ // flashProtectBootloader();
- flashReset();
+ // flashReset();
- fpgaReleaseReset();
+ // fpgaReleaseReset();
- vTaskDelay(M2T(200));
+ // vTaskDelay(M2T(200));
- statusDone = true;
+ // statusDone = true;
return pass;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lps25h.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lps25h.c
index f724fc6d669d40b47b5d02ff216fa22fcf91533a..4cb77a181f16f5cd821f50129f30ae0d25d22407 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lps25h.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/lps25h.c
@@ -44,11 +44,11 @@ bool lps25hInit(I2C_Dev *i2cPort)
{
if (isInit)
return true;
+ //COMMENTED FIRMWARE
+ // I2Cx = i2cPort;
+ // devAddr = LPS25H_I2C_ADDR;
- I2Cx = i2cPort;
- devAddr = LPS25H_I2C_ADDR;
-
- vTaskDelay(M2T(5));
+ // vTaskDelay(M2T(5));
isInit = true;
@@ -57,17 +57,18 @@ bool lps25hInit(I2C_Dev *i2cPort)
bool lps25hTestConnection(void)
{
- uint8_t id;
- bool status;
+ //COMMENTED FIRMWARE
+ // uint8_t id;
+ // bool status;
- if (!isInit)
- return false;
+ // if (!isInit)
+ // return false;
- status = i2cdevReadReg8(I2Cx, devAddr, LPS25H_WHO_AM_I, 1, &id);
+ // status = i2cdevReadReg8(I2Cx, devAddr, LPS25H_WHO_AM_I, 1, &id);
- if (status == true && id == LPS25H_WAI_ID)
- return true;
+ // if (status == true && id == LPS25H_WAI_ID)
+ // return true;
return false;
}
@@ -96,12 +97,13 @@ bool lps25hSelfTest(void)
bool lps25hEvaluateSelfTest(float min, float max, float value, char* string)
{
- if (value < min || value > max)
- {
- DEBUG_PRINT("Self test %s [FAIL]. low: %0.2f, high: %0.2f, measured: %0.2f\n",
- string, (double)min, (double)max, (double)value);
- return false;
- }
+ //COMMENTED FIRMWARE
+ // if (value < min || value > max)
+ // {
+ // DEBUG_PRINT("Self test %s [FAIL]. low: %0.2f, high: %0.2f, measured: %0.2f\n",
+ // string, (double)min, (double)max, (double)value);
+ // return false;
+ // }
return true;
}
@@ -109,28 +111,28 @@ bool lps25hSetEnabled(bool enable)
{
uint8_t enable_mask;
bool status;
-
- if (!isInit)
- return false;
-
- if (enable)
- {
- enable_mask = 0b11000110; // Power on, 25Hz, BDU, reset zero
- status = i2cdevWriteReg8(I2Cx, devAddr, LPS25H_CTRL_REG1, 1, &enable_mask);
- enable_mask = 0b00001111; // AVG-P 512, AVG-T 64
- status = i2cdevWriteReg8(I2Cx, devAddr, LPS25H_RES_CONF, 1, &enable_mask);
- // FIFO averaging. This requres temp reg to be read in different read as reg auto inc
- // wraps back to LPS25H_PRESS_OUT_L after LPS25H_PRESS_OUT_H is read.
- enable_mask = 0b11000011; // FIFO Mean mode, 4 moving average
- status = i2cdevWriteReg8(I2Cx, devAddr, LPS25H_FIFO_CTRL, 1, &enable_mask);
- enable_mask = 0b01000000; // FIFO Enable
- status = i2cdevWriteReg8(I2Cx, devAddr, LPS25H_CTRL_REG2, 1, &enable_mask);
- }
- else
- {
- enable_mask = 0x00; // Power off and default values
- status = i2cdevWriteReg8(I2Cx, devAddr, LPS25H_CTRL_REG1, 1, &enable_mask);
- }
+ //COMMENTED FIRMWARE
+ // if (!isInit)
+ // return false;
+
+ // if (enable)
+ // {
+ // enable_mask = 0b11000110; // Power on, 25Hz, BDU, reset zero
+ // status = i2cdevWriteReg8(I2Cx, devAddr, LPS25H_CTRL_REG1, 1, &enable_mask);
+ // enable_mask = 0b00001111; // AVG-P 512, AVG-T 64
+ // status = i2cdevWriteReg8(I2Cx, devAddr, LPS25H_RES_CONF, 1, &enable_mask);
+ // // FIFO averaging. This requres temp reg to be read in different read as reg auto inc
+ // // wraps back to LPS25H_PRESS_OUT_L after LPS25H_PRESS_OUT_H is read.
+ // enable_mask = 0b11000011; // FIFO Mean mode, 4 moving average
+ // status = i2cdevWriteReg8(I2Cx, devAddr, LPS25H_FIFO_CTRL, 1, &enable_mask);
+ // enable_mask = 0b01000000; // FIFO Enable
+ // status = i2cdevWriteReg8(I2Cx, devAddr, LPS25H_CTRL_REG2, 1, &enable_mask);
+ // }
+ // else
+ // {
+ // enable_mask = 0x00; // Power off and default values
+ // status = i2cdevWriteReg8(I2Cx, devAddr, LPS25H_CTRL_REG1, 1, &enable_mask);
+ // }
return status;
}
@@ -142,17 +144,18 @@ bool lps25hGetData(float* pressure, float* temperature, float* asl)
int16_t rawTemp;
bool status;
- status = i2cdevReadReg8(I2Cx, devAddr, LPS25H_PRESS_OUT_XL | LPS25H_ADDR_AUTO_INC, 3, data);
- // If LPS25H moving avg filter is activated the temp must be read out in separate read.
- status &= i2cdevReadReg8(I2Cx, devAddr, LPS25H_TEMP_OUT_L | LPS25H_ADDR_AUTO_INC, 2, &data[3]);
+ //COMMENTED FIRMWARE
+ // status = i2cdevReadReg8(I2Cx, devAddr, LPS25H_PRESS_OUT_XL | LPS25H_ADDR_AUTO_INC, 3, data);
+ // // If LPS25H moving avg filter is activated the temp must be read out in separate read.
+ // status &= i2cdevReadReg8(I2Cx, devAddr, LPS25H_TEMP_OUT_L | LPS25H_ADDR_AUTO_INC, 2, &data[3]);
- rawPressure = ((uint32_t)data[2] << 16) | ((uint32_t)data[1] << 8) | data[0];
- *pressure = (float)rawPressure / LPS25H_LSB_PER_MBAR;
+ // rawPressure = ((uint32_t)data[2] << 16) | ((uint32_t)data[1] << 8) | data[0];
+ // *pressure = (float)rawPressure / LPS25H_LSB_PER_MBAR;
- rawTemp = ((int16_t)data[4] << 8) | data[3];
- *temperature = LPS25H_TEMP_OFFSET + ((float)rawTemp / LPS25H_LSB_PER_CELSIUS);
+ // rawTemp = ((int16_t)data[4] << 8) | data[3];
+ // *temperature = LPS25H_TEMP_OFFSET + ((float)rawTemp / LPS25H_LSB_PER_CELSIUS);
- *asl = lps25hPressureToAltitude(pressure);
+ // *asl = lps25hPressureToAltitude(pressure);
return status;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/maxsonar.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/maxsonar.c
index cebebe40eb111296788f402d0bd2ef0588cdba20..4286fbb6dc985bb635c32d0413e5577979596e20 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/maxsonar.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/maxsonar.c
@@ -55,37 +55,37 @@ LOG_GROUP_STOP(maxSonar)
static uint32_t maxSonarGetAccuracyMB1040(uint32_t distance)
{
/* Specify the accuracy of the measurement from the MB1040 (LV-MaxBotix-EZ4) sensor. */
-
- if(distance <= IN2MM(6)) {
- /**
- * The datasheet for the MB1040 specifies that any distance below 6 inches is reported as 6 inches.
- * Since all measurements are given in 1 inch steps, the actual distance can be anything
- * from 7 (exclusive) to 0 (inclusive) inches.
- *
- * The accuracy is therefore set to 7(!) inches.
- */
- maxSonarAccuracy = IN2MM(7);
- }
- else if(distance >= IN2MM(20)) {
- /**
- * The datasheet for the MB1040 specifies that any distance between 6 and 20 inches may result in
- * measurement inaccuracies up to 2 inches.
- */
- maxSonarAccuracy = IN2MM(2);
- }
- else if(distance > IN2MM(254)) {
- /**
- * The datasheet for the MB1040 specifies that maximum reported distance is 254 inches. If we for
- * some reason should measure more than this, set the accuracy to 0.
- */
- maxSonarAccuracy = 0;
- }
- else {
- /**
- * Otherwise the accuracy is specified by the datasheet for the MB1040 to be 1 inch.
- */
- maxSonarAccuracy = IN2MM(1);
- }
+ //COMMENTED FIRMWARE
+ // if(distance <= IN2MM(6)) {
+ // /**
+ // * The datasheet for the MB1040 specifies that any distance below 6 inches is reported as 6 inches.
+ // * Since all measurements are given in 1 inch steps, the actual distance can be anything
+ // * from 7 (exclusive) to 0 (inclusive) inches.
+ // *
+ // * The accuracy is therefore set to 7(!) inches.
+ // */
+ // maxSonarAccuracy = IN2MM(7);
+ // }
+ // else if(distance >= IN2MM(20)) {
+ // /**
+ // * The datasheet for the MB1040 specifies that any distance between 6 and 20 inches may result in
+ // * measurement inaccuracies up to 2 inches.
+ // */
+ // maxSonarAccuracy = IN2MM(2);
+ // }
+ // else if(distance > IN2MM(254)) {
+ // /**
+ // * The datasheet for the MB1040 specifies that maximum reported distance is 254 inches. If we for
+ // * some reason should measure more than this, set the accuracy to 0.
+ // */
+ // maxSonarAccuracy = 0;
+ // }
+ // else {
+ // /**
+ // * Otherwise the accuracy is specified by the datasheet for the MB1040 to be 1 inch.
+ // */
+ // maxSonarAccuracy = IN2MM(1);
+ // }
/* Report accuracy if the caller asked for this. */
return maxSonarAccuracy;
@@ -121,12 +121,12 @@ static uint32_t maxSonarReadDistanceMB1040AN(const deckPin_t pin, uint32_t *accu
* According to the datasheet for the MB1040, the sensor draws typically 2mA, so powering it with
* the VCC pin on the deck port is safe.
*/
+ //COMMENTED FIRMWARE
+ // maxSonarDistance = (uint32_t) (IN2MM(analogRead(pin)) / 8);
- maxSonarDistance = (uint32_t) (IN2MM(analogRead(pin)) / 8);
-
- if(NULL != accuracy) {
- *accuracy = maxSonarGetAccuracyMB1040(maxSonarDistance);
- }
+ // if(NULL != accuracy) {
+ // *accuracy = maxSonarGetAccuracyMB1040(maxSonarDistance);
+ // }
return maxSonarDistance;
}
@@ -140,17 +140,18 @@ static uint32_t maxSonarReadDistanceMB1040AN(const deckPin_t pin, uint32_t *accu
*/
uint32_t maxSonarReadDistance(maxSonarSensor_t type, uint32_t *accuracy)
{
- switch(type) {
- case MAXSONAR_MB1040_AN: {
- return maxSonarReadDistanceMB1040AN(MAXSONAR_DECK_GPIO, accuracy);
- }
- }
+ //COMMENTED FIRMWARE
+ // switch(type) {
+ // case MAXSONAR_MB1040_AN: {
+ // return maxSonarReadDistanceMB1040AN(MAXSONAR_DECK_GPIO, accuracy);
+ // }
+ // }
- maxSonarDistance = 0;
- maxSonarAccuracy = 0;
- if(accuracy) {
- *accuracy = maxSonarAccuracy;
- }
+ // maxSonarDistance = 0;
+ // maxSonarAccuracy = 0;
+ // if(accuracy) {
+ // *accuracy = maxSonarAccuracy;
+ // }
return(maxSonarDistance);
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/ledseq.c b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/ledseq.c
index 94b5dffabce3595bf0f68ec848f6a70d33094c08..ff5cd881ac71872fccefdfe17cf51f015b56b48b 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/ledseq.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/ledseq.c
@@ -192,37 +192,39 @@ static bool ledseqEnabled = false;
static void lesdeqCmdTask(void* param);
void ledseqInit() {
+
if(isInit) {
return;
}
- ledInit();
-
- /* Led sequence priority */
- ledseqRegisterSequence(&seq_testPassed);
- ledseqRegisterSequence(&seq_testFailed);
- ledseqRegisterSequence(&seq_lowbat);
- ledseqRegisterSequence(&seq_charged);
- ledseqRegisterSequence(&seq_charging);
- ledseqRegisterSequence(&seq_calibrated);
- ledseqRegisterSequence(&seq_alive);
- ledseqRegisterSequence(&seq_linkUp);
- ledseqRegisterSequence(&seq_linkDown);
-
- //Initialise the sequences state
- for(int i=0; i<LED_NUM; i++) {
- activeSeq[i] = 0;
- }
-
- //Init the soft timers that runs the led sequences for each leds
- for(int i=0; i<LED_NUM; i++) {
- timer[i] = xTimerCreateStatic("ledseqTimer", M2T(1000), pdFALSE, (void*)i, runLedseq, &timerBuffer[i]);
- }
-
- ledseqMutex = xSemaphoreCreateMutex();
-
- ledseqCmdQueue = xQueueCreate(10, sizeof(struct ledseqCmd_s));
- xTaskCreate(lesdeqCmdTask, LEDSEQCMD_TASK_NAME, LEDSEQCMD_TASK_STACKSIZE, NULL, LEDSEQCMD_TASK_PRI, NULL);
+ //COMMENTED FIRMWARE
+ // ledInit();
+
+ // /* Led sequence priority */
+ // ledseqRegisterSequence(&seq_testPassed);
+ // ledseqRegisterSequence(&seq_testFailed);
+ // ledseqRegisterSequence(&seq_lowbat);
+ // ledseqRegisterSequence(&seq_charged);
+ // ledseqRegisterSequence(&seq_charging);
+ // ledseqRegisterSequence(&seq_calibrated);
+ // ledseqRegisterSequence(&seq_alive);
+ // ledseqRegisterSequence(&seq_linkUp);
+ // ledseqRegisterSequence(&seq_linkDown);
+
+ // //Initialise the sequences state
+ // for(int i=0; i<LED_NUM; i++) {
+ // activeSeq[i] = 0;
+ // }
+
+ // //Init the soft timers that runs the led sequences for each leds
+ // for(int i=0; i<LED_NUM; i++) {
+ // timer[i] = xTimerCreateStatic("ledseqTimer", M2T(1000), pdFALSE, (void*)i, runLedseq, &timerBuffer[i]);
+ // }
+
+ // ledseqMutex = xSemaphoreCreateMutex();
+
+ // ledseqCmdQueue = xQueueCreate(10, sizeof(struct ledseqCmd_s));
+ // xTaskCreate(lesdeqCmdTask, LEDSEQCMD_TASK_NAME, LEDSEQCMD_TASK_STACKSIZE, NULL, LEDSEQCMD_TASK_PRI, NULL);
isInit = true;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/pm_stm32f4.c b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/pm_stm32f4.c
index 2b2c744608eadbdbe79970bc7f73da180b363ab2..d522e268d29a14bdec7bbc8ade3d4817e56e6995 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/pm_stm32f4.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/pm_stm32f4.c
@@ -116,9 +116,9 @@ void pmInit(void)
STATIC_MEM_TASK_CREATE(pmTask, pmTask, PM_TASK_NAME, NULL, PM_TASK_PRI);
isInit = true;
-
- pmSyslinkInfo.vBat = 3.7f;
- pmSetBatteryVoltage(pmSyslinkInfo.vBat); //TODO remove
+ //COMMENTED FIRMWARE
+ // pmSyslinkInfo.vBat = 3.7f;
+ // pmSetBatteryVoltage(pmSyslinkInfo.vBat); //TODO remove
}
bool pmTest(void)
@@ -307,104 +307,105 @@ bool pmIsDischarging(void) {
void pmTask(void *param)
{
- PMStates pmStateOld = battery;
- uint32_t tickCount;
-
- vTaskSetApplicationTaskTag(0, (void*)TASK_PM_ID_NBR);
-
- tickCount = xTaskGetTickCount();
- batteryLowTimeStamp = tickCount;
- batteryCriticalLowTimeStamp = tickCount;
-
- pmSetChargeState(charge500mA);
- systemWaitStart();
-
- while(1)
- {
- vTaskDelay(100);
- tickCount = xTaskGetTickCount();
-
- extBatteryVoltage = pmMeasureExtBatteryVoltage();
- extBatteryVoltageMV = (uint16_t)(extBatteryVoltage * 1000);
- extBatteryCurrent = pmMeasureExtBatteryCurrent();
- batteryLevel = pmBatteryChargeFromVoltage(pmGetBatteryVoltage()) * 10;
-
- if (pmGetBatteryVoltage() > PM_BAT_LOW_VOLTAGE)
- {
- batteryLowTimeStamp = tickCount;
- }
- if (pmGetBatteryVoltage() > PM_BAT_CRITICAL_LOW_VOLTAGE)
- {
- batteryCriticalLowTimeStamp = tickCount;
- }
-
- pmState = pmUpdateState();
-
- if (pmState != pmStateOld)
- {
- // Actions on state change
- switch (pmState)
- {
- case charged:
- ledseqStop(&seq_charging);
- ledseqRunBlocking(&seq_charged);
- soundSetEffect(SND_BAT_FULL);
- break;
- case charging:
- ledseqStop(&seq_lowbat);
- ledseqStop(&seq_charged);
- ledseqRunBlocking(&seq_charging);
- soundSetEffect(SND_USB_CONN);
- break;
- case lowPower:
- ledseqRunBlocking(&seq_lowbat);
- soundSetEffect(SND_BAT_LOW);
- break;
- case battery:
- ledseqRunBlocking(&seq_charging);
- ledseqRun(&seq_charged);
- soundSetEffect(SND_USB_DISC);
- break;
- default:
- break;
- }
- pmStateOld = pmState;
- }
- // Actions during state
- switch (pmState)
- {
- case charged:
- break;
- case charging:
- {
- // Charge level between 0.0 and 1.0
- float chargeLevel = pmBatteryChargeFromVoltage(pmGetBatteryVoltage()) / 10.0f;
- ledseqSetChargeLevel(chargeLevel);
- }
- break;
- case lowPower:
- {
- uint32_t batteryCriticalLowTime;
-
- batteryCriticalLowTime = tickCount - batteryCriticalLowTimeStamp;
- if (batteryCriticalLowTime > PM_BAT_CRITICAL_LOW_TIMEOUT)
- {
- pmSystemShutdown();
- }
- }
- break;
- case battery:
- {
- if ((commanderGetInactivityTime() > PM_SYSTEM_SHUTDOWN_TIMEOUT))
- {
- pmSystemShutdown();
- }
- }
- break;
- default:
- break;
- }
- }
+ //COMMENTED FIRMWARE
+ // PMStates pmStateOld = battery;
+ // uint32_t tickCount;
+
+ // vTaskSetApplicationTaskTag(0, (void*)TASK_PM_ID_NBR);
+
+ // tickCount = xTaskGetTickCount();
+ // batteryLowTimeStamp = tickCount;
+ // batteryCriticalLowTimeStamp = tickCount;
+
+ // pmSetChargeState(charge500mA);
+ // systemWaitStart();
+
+ // while(1)
+ // {
+ // vTaskDelay(100);
+ // tickCount = xTaskGetTickCount();
+
+ // extBatteryVoltage = pmMeasureExtBatteryVoltage();
+ // extBatteryVoltageMV = (uint16_t)(extBatteryVoltage * 1000);
+ // extBatteryCurrent = pmMeasureExtBatteryCurrent();
+ // batteryLevel = pmBatteryChargeFromVoltage(pmGetBatteryVoltage()) * 10;
+
+ // if (pmGetBatteryVoltage() > PM_BAT_LOW_VOLTAGE)
+ // {
+ // batteryLowTimeStamp = tickCount;
+ // }
+ // if (pmGetBatteryVoltage() > PM_BAT_CRITICAL_LOW_VOLTAGE)
+ // {
+ // batteryCriticalLowTimeStamp = tickCount;
+ // }
+
+ // pmState = pmUpdateState();
+
+ // if (pmState != pmStateOld)
+ // {
+ // // Actions on state change
+ // switch (pmState)
+ // {
+ // case charged:
+ // ledseqStop(&seq_charging);
+ // ledseqRunBlocking(&seq_charged);
+ // soundSetEffect(SND_BAT_FULL);
+ // break;
+ // case charging:
+ // ledseqStop(&seq_lowbat);
+ // ledseqStop(&seq_charged);
+ // ledseqRunBlocking(&seq_charging);
+ // soundSetEffect(SND_USB_CONN);
+ // break;
+ // case lowPower:
+ // ledseqRunBlocking(&seq_lowbat);
+ // soundSetEffect(SND_BAT_LOW);
+ // break;
+ // case battery:
+ // ledseqRunBlocking(&seq_charging);
+ // ledseqRun(&seq_charged);
+ // soundSetEffect(SND_USB_DISC);
+ // break;
+ // default:
+ // break;
+ // }
+ // pmStateOld = pmState;
+ // }
+ // // Actions during state
+ // switch (pmState)
+ // {
+ // case charged:
+ // break;
+ // case charging:
+ // {
+ // // Charge level between 0.0 and 1.0
+ // float chargeLevel = pmBatteryChargeFromVoltage(pmGetBatteryVoltage()) / 10.0f;
+ // ledseqSetChargeLevel(chargeLevel);
+ // }
+ // break;
+ // case lowPower:
+ // {
+ // uint32_t batteryCriticalLowTime;
+
+ // batteryCriticalLowTime = tickCount - batteryCriticalLowTimeStamp;
+ // if (batteryCriticalLowTime > PM_BAT_CRITICAL_LOW_TIMEOUT)
+ // {
+ // pmSystemShutdown();
+ // }
+ // }
+ // break;
+ // case battery:
+ // {
+ // if ((commanderGetInactivityTime() > PM_SYSTEM_SHUTDOWN_TIMEOUT))
+ // {
+ // pmSystemShutdown();
+ // }
+ // }
+ // break;
+ // default:
+ // break;
+ // }
+ // }
}
/**
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usb.c b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usb.c
index 2a10496e6c31239e44bf70d3b8153b1086900914..1f300a9b313399342b02d2310aa664c997ae7fa6 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usb.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usb.c
@@ -402,16 +402,17 @@ void USBD_USR_DeviceDisconnected(void)
void usbInit(void)
{
- USBD_Init(&USB_OTG_dev,
- USB_OTG_FS_CORE_ID,
- &USR_desc,
- &cf_usb_cb,
- &USR_cb);
-
- usbDataRx = STATIC_MEM_QUEUE_CREATE(usbDataRx);
- DEBUG_QUEUE_MONITOR_REGISTER(usbDataRx);
- usbDataTx = STATIC_MEM_QUEUE_CREATE(usbDataTx);
- DEBUG_QUEUE_MONITOR_REGISTER(usbDataTx);
+ //COMMENTED FIRMWARE
+ // USBD_Init(&USB_OTG_dev,
+ // USB_OTG_FS_CORE_ID,
+ // &USR_desc,
+ // &cf_usb_cb,
+ // &USR_cb);
+
+ // usbDataRx = STATIC_MEM_QUEUE_CREATE(usbDataRx);
+ // DEBUG_QUEUE_MONITOR_REGISTER(usbDataRx);
+ // usbDataTx = STATIC_MEM_QUEUE_CREATE(usbDataTx);
+ // DEBUG_QUEUE_MONITOR_REGISTER(usbDataTx);
isInit = true;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usblink.c b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usblink.c
index 05958a83ec80bd345e3950b56e5076881e3b1f31..225edc6ad14b0bd167b6814109abe01a96c9838f 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usblink.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usblink.c
@@ -69,15 +69,16 @@ static USBPacket usbIn;
static CRTPPacket p;
static void usblinkTask(void *param)
{
- while(1)
- {
- // Fetch a USB packet off the queue
- usbGetDataBlocking(&usbIn);
- p.size = usbIn.size - 1;
- memcpy(&p.raw, usbIn.data, usbIn.size);
- // This queuing will copy a CRTP packet size from usbIn
- xQueueSend(crtpPacketDelivery, &p, portMAX_DELAY);
- }
+ //COMMENTED FIRMWARE
+ // while(1)
+ // {
+ // // Fetch a USB packet off the queue
+ // usbGetDataBlocking(&usbIn);
+ // p.size = usbIn.size - 1;
+ // memcpy(&p.raw, usbIn.data, usbIn.size);
+ // // This queuing will copy a CRTP packet size from usbIn
+ // xQueueSend(crtpPacketDelivery, &p, portMAX_DELAY);
+ // }
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usec_time.c b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usec_time.c
index 6971d308ea23004b613150ce3fa78da7f947cf42..73643417242091b8fd800c843029bdc8a1c2a5c4 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usec_time.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/hal/src/usec_time.c
@@ -36,39 +36,40 @@ static uint32_t usecTimerHighCount;
void initUsecTimer(void)
{
- if (isInit) {
- return;
- }
+ //COMMENTED FIRMWARE
+ // if (isInit) {
+ // return;
+ // }
- usecTimerHighCount = 0;
+ // usecTimerHighCount = 0;
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
+ // TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ // NVIC_InitTypeDef NVIC_InitStructure;
- //Enable the Timer
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
+ // //Enable the Timer
+ // RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
- //Timer configuration
- TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
- // APB1 clock is /4, but APB clock inputs to timers are doubled when
- // the APB clock runs slower than /1, so APB1 timers see a /2 clock
- TIM_TimeBaseStructure.TIM_Prescaler = SystemCoreClock / (1000 * 1000) / 2;
- TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
- TIM_TimeBaseInit(TIM7, &TIM_TimeBaseStructure);
+ // //Timer configuration
+ // TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+ // // APB1 clock is /4, but APB clock inputs to timers are doubled when
+ // // the APB clock runs slower than /1, so APB1 timers see a /2 clock
+ // TIM_TimeBaseStructure.TIM_Prescaler = SystemCoreClock / (1000 * 1000) / 2;
+ // TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
+ // TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ // TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+ // TIM_TimeBaseInit(TIM7, &TIM_TimeBaseStructure);
- NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_TRACE_TIM_PRI;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
+ // NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;
+ // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_TRACE_TIM_PRI;
+ // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ // NVIC_Init(&NVIC_InitStructure);
- DBGMCU_APB1PeriphConfig(DBGMCU_TIM7_STOP, ENABLE);
- TIM_ITConfig(TIM7, TIM_IT_Update, ENABLE);
- TIM_Cmd(TIM7, ENABLE);
+ // DBGMCU_APB1PeriphConfig(DBGMCU_TIM7_STOP, ENABLE);
+ // TIM_ITConfig(TIM7, TIM_IT_Update, ENABLE);
+ // TIM_Cmd(TIM7, ENABLE);
- isInit = true;
+ // isInit = true;
}
uint64_t usecTimestamp(void)
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/init/main.c b/crazyflie_software/crazyflie-firmware-2021.06/src/init/main.c
index 1faf554a5f99534664bd03022f6cec986397fe3c..97d331ab8d411775fc61a8328f321d681e087520 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/init/main.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/init/main.c
@@ -62,9 +62,10 @@ int main()
vTaskStartScheduler();
//TODO: Move to platform launch failed
- ledInit();
- ledSet(0, 1);
- ledSet(1, 1);
+ //COMMENTED FIRMWARE
+ // ledInit();
+ // ledSet(0, 1);
+ // ledSet(1, 1);
//Should never reach this point!
while(1);
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform.c b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform.c
index ac69b237e4b6a1f445b87814266770a29269ac14..cc8f668b4b0947c2866566157085578c36fbf99e 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform.c
@@ -43,7 +43,7 @@ int platformInit(void) {
// }
// platformInitHardware();
- // return 0;
+ return 0;
}
int platformParseDeviceTypeString(const char* deviceTypeString, char* deviceType) {
@@ -68,7 +68,7 @@ int platformParseDeviceTypeString(const char* deviceTypeString, char* deviceType
// int length = end - start;
// memcpy(deviceType, &deviceTypeString[start], length);
// deviceType[length] = '\0';
- // return 0;
+ return 0;
}
int platformInitConfiguration(const platformConfig_t* configs, const int nrOfConfigs) {
@@ -90,7 +90,7 @@ int platformInitConfiguration(const platformConfig_t* configs, const int nrOfCon
// const platformConfig_t* config = &configs[i];
// if (strcmp(config->deviceType, deviceType) == 0) {
// active_config = config;
-// return 0;
+ return 0;
// }
// }