From 8b0c2ea068debd16a1c871197f10dd15e5650bd1 Mon Sep 17 00:00:00 2001
From: jtkenny <jtkenny@iastate.edu>
Date: Fri, 29 Mar 2024 19:38:28 +0100
Subject: [PATCH] readding cf firmware
---
.../src/deck/api/deck_analog.c | 40 +-
.../src/drivers/src/ak8963.c | 196 +--
.../src/drivers/src/cppm.c | 130 +-
.../src/drivers/src/eeprom.c | 3 +-
.../src/drivers/src/exti.c | 209 ++-
.../src/drivers/src/fatfs_sd.c | 878 +++++------
.../src/drivers/src/hmc5883l.c | 236 ++-
.../src/drivers/src/i2c_drv.c | 538 ++++---
.../src/drivers/src/i2cdev.c | 85 +-
.../src/drivers/src/i2croutines.c | 1299 ++++++++-------
.../src/drivers/src/led.c | 113 +-
.../src/drivers/src/lh_bootloader.c | 174 +-
.../src/drivers/src/lh_flasher.c | 287 ++--
.../src/drivers/src/lps25h.c | 97 +-
.../src/drivers/src/maxsonar.c | 93 +-
.../src/drivers/src/motors.c | 404 ++---
.../src/drivers/src/mpu6050.c | 1403 ++++++++---------
.../src/drivers/src/mpu6500.c | 1246 ++++++++-------
.../src/drivers/src/ms5611.c | 431 +++--
.../src/drivers/src/nrf24l01.c | 356 ++---
.../src/drivers/src/nvic.c | 172 +-
.../src/drivers/src/pca9685.c | 75 +-
.../src/drivers/src/piezo.c | 118 +-
.../src/drivers/src/pmw3901.c | 306 ++--
.../src/drivers/src/swd.c | 179 ++-
.../src/drivers/src/uart1.c | 165 +-
.../src/drivers/src/uart2.c | 325 ++--
.../src/drivers/src/uart_syslink.c | 635 ++++----
.../src/drivers/src/vl53l0x.c | 1386 ++++++++--------
.../src/drivers/src/vl53l1x.c | 281 ++--
.../src/drivers/src/watchdog.c | 63 +-
.../src/drivers/src/ws2812_cf2.c | 327 ++--
.../src/hal/src/ledseq.c | 56 +-
.../src/hal/src/pm_stm32f4.c | 203 ++-
.../src/hal/src/usb.c | 21 +-
.../src/hal/src/usblink.c | 19 +-
.../src/hal/src/usec_time.c | 53 +-
.../src/init/main.c | 7 +-
.../src/platform/platform.c | 124 +-
.../src/platform/platform_cf2.c | 17 +-
.../src/platform/platform_stm32f4.c | 44 +-
.../src/platform/platform_tag.c | 19 +-
.../src/platform/platform_utils.c | 7 +-
43 files changed, 6274 insertions(+), 6546 deletions(-)
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 27fed2489..f1ae95f9b 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 7aa6b20c9..d248f41f8 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,57 +66,54 @@ void ak8963Init(I2C_Dev *i2cPort)
*/
bool ak8963TestConnection()
{
- //COMMENTED FIRMWARE
- // if (i2cdevReadByte(I2Cx, devAddr, AK8963_RA_WIA, buffer) == 1)
- // {
- // return (buffer[0] == 0x48);
- // }
- // return false;
- return true;
+ if (i2cdevReadByte(I2Cx, devAddr, AK8963_RA_WIA, buffer) == 1)
+ {
+ return (buffer[0] == 0x48);
+ }
+ return false;
}
bool ak8963SelfTest()
{
bool testStatus = true;
- //COMMENTED FIRMWARE
- // int16_t mx, my, mz; // positive magnetometer measurements
- // uint8_t confSave;
- // uint8_t timeout = 20;
+ 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;
}
@@ -130,13 +127,12 @@ bool ak8963SelfTest()
*/
static bool ak8963EvaluateSelfTest(int16_t min, int16_t max, int16_t value, char* string)
{
- //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;
- // }
+ 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;
}
@@ -144,8 +140,7 @@ static bool ak8963EvaluateSelfTest(int16_t min, int16_t max, int16_t value, char
uint8_t ak8963GetDeviceID()
{
- //COMMENTED FIRMWARE
- //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_WIA, buffer);
+ i2cdevReadByte(I2Cx, devAddr, AK8963_RA_WIA, buffer);
return buffer[0];
}
@@ -153,8 +148,7 @@ uint8_t ak8963GetDeviceID()
uint8_t ak8963GetInfo()
{
- //COMMENTED FIRMWARE
- //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_INFO, buffer);
+ i2cdevReadByte(I2Cx, devAddr, AK8963_RA_INFO, buffer);
return buffer[0];
}
@@ -162,8 +156,7 @@ uint8_t ak8963GetInfo()
bool ak8963GetDataReady()
{
- //COMMENTED FIRMWARE
- //i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST1, AK8963_ST1_DRDY_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST1, AK8963_ST1_DRDY_BIT, buffer);
return buffer[0];
}
@@ -172,131 +165,112 @@ void ak8963GetHeading(int16_t *x, int16_t *y, int16_t *z)
{
// i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_MODE_SINGLE);
// delay(10);
-//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];
+ 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()
{
- //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];
+ 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()
{
- //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];
+ 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()
{
- //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];
+ 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()
{
- //COMMENTED FIRMWARE
- //i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST2, AK8963_ST2_HOFL_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST2, AK8963_ST2_HOFL_BIT, buffer);
return buffer[0];
}
bool ak8963GetDataError()
{
- //COMMENTED FIRMWARE
- //i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST2, AK8963_ST2_DERR_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, AK8963_RA_ST2, AK8963_ST2_DERR_BIT, buffer);
return buffer[0];
}
// CNTL register
uint8_t ak8963GetMode()
{
- //COMMENTED FIRMWARE
- //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_CNTL, buffer);
+ i2cdevReadByte(I2Cx, devAddr, AK8963_RA_CNTL, buffer);
return buffer[0];
}
void ak8963SetMode(uint8_t mode)
{
- //COMMENTED FIRMWARE
- //i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, mode);
+ i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, mode);
}
void ak8963Reset()
{
- //COMMENTED FIRMWARE
- //i2cdevWriteBits(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_CNTL_MODE_BIT,
+ i2cdevWriteBits(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_CNTL_MODE_BIT,
AK8963_CNTL_MODE_LENGTH, AK8963_MODE_POWERDOWN);
}
// ASTC register
void ak8963SetSelfTest(bool enabled)
{
- //COMMENTED FIRMWARE
- //i2cdevWriteBit(I2Cx, devAddr, AK8963_RA_ASTC, AK8963_ASTC_SELF_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, AK8963_RA_ASTC, AK8963_ASTC_SELF_BIT, enabled);
}
// I2CDIS
void ak8963DisableI2C()
{
- //COMMENTED FIRMWARE
- //i2cdevWriteBit(I2Cx, devAddr, AK8963_RA_I2CDIS, AK8963_I2CDIS_BIT, true);
+ i2cdevWriteBit(I2Cx, devAddr, AK8963_RA_I2CDIS, AK8963_I2CDIS_BIT, true);
}
// ASA* registers
void ak8963GetAdjustment(int8_t *x, int8_t *y, int8_t *z)
{
- //COMMENTED FIRMWARE
- // i2cdevReadReg8(I2Cx, devAddr, AK8963_RA_ASAX, 3, buffer);
- // *x = buffer[0];
- // *y = buffer[1];
- // *z = buffer[2];
+ 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)
{
- //COMMENTED FIRMWARE
- // buffer[0] = x;
- // buffer[1] = y;
- // buffer[2] = z;
- // i2cdevWriteReg8(I2Cx, devAddr, AK8963_RA_ASAX, 3, buffer);
+ buffer[0] = x;
+ buffer[1] = y;
+ buffer[2] = z;
+ i2cdevWriteReg8(I2Cx, devAddr, AK8963_RA_ASAX, 3, buffer);
}
uint8_t ak8963GetAdjustmentX()
{
- //COMMENTED FIRMWARE
- //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAX, buffer);
+ i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAX, buffer);
return buffer[0];
}
void ak8963SetAdjustmentX(uint8_t x)
{
- //COMMENTED FIRMWARE
- //i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAX, x);
+ i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAX, x);
}
uint8_t ak8963GetAdjustmentY()
{
- //COMMENTED FIRMWARE
- //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAY, buffer);
+ i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAY, buffer);
return buffer[0];
}
void ak8963SetAdjustmentY(uint8_t y)
{
- //COMMENTED FIRMWARE
- //i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAY, y);
+ i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAY, y);
}
uint8_t ak8963GetAdjustmentZ()
{
- //COMMENTED FIRMWARE
- //i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAZ, buffer);
+ i2cdevReadByte(I2Cx, devAddr, AK8963_RA_ASAZ, buffer);
return buffer[0];
}
void ak8963SetAdjustmentZ(uint8_t z)
{
- //COMMENTED FIRMWARE
- //i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_ASAZ, z);
+ 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 a3cf95cee..e0ac6de67 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,43 +65,42 @@ static bool isAvailible;
void cppmInit(void)
{
- //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);
+ 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)
@@ -155,33 +154,32 @@ uint16_t cppmConvert2uint16(uint16_t timestamp)
void __attribute__((used)) TIM8_TRG_COM_TIM14_IRQHandler()
{
- //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);
- // }
+ 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 422e688eb..9aedf5cc5 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,8 +131,7 @@ bool eepromTestConnection(void)
if (!isInit)
return false;
- //COMMENTED FIRMWARE
- status = true;//i2cdevRead16(I2Cx, devAddr, 0, 1, &tmp);
+ status = 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 79810be14..86675d7a0 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);
- // // 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.
+ // 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);
- // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- // NVIC_InitStructure.NVIC_IRQChannelCmd = 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.
- // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI0_PRI;
- // NVIC_InitStructure.NVIC_IRQChannel = EXTI0_IRQn;
- // NVIC_Init(&NVIC_InitStructure);
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI1_PRI;
- // NVIC_InitStructure.NVIC_IRQChannel = EXTI1_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 = EXTI2_PRI;
- // NVIC_InitStructure.NVIC_IRQChannel = EXTI2_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 = EXTI3_PRI;
- // NVIC_InitStructure.NVIC_IRQChannel = EXTI3_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 = EXTI4_PRI;
- // NVIC_InitStructure.NVIC_IRQChannel = EXTI4_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 = EXTI9_5_PRI;
- // NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_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 = EXTI15_10_PRI;
- // NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_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);
isInit = true;
}
@@ -91,107 +91,100 @@ bool extiTest(void)
void __attribute__((used)) EXTI0_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- // NVIC_ClearPendingIRQ(EXTI0_IRQn);
- // EXTI_ClearITPendingBit(EXTI_Line0);
- // EXTI0_Callback();
+ NVIC_ClearPendingIRQ(EXTI0_IRQn);
+ EXTI_ClearITPendingBit(EXTI_Line0);
+ EXTI0_Callback();
}
void __attribute__((used)) EXTI1_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- // NVIC_ClearPendingIRQ(EXTI1_IRQn);
- // EXTI_ClearITPendingBit(EXTI_Line1);
- // EXTI1_Callback();
+ NVIC_ClearPendingIRQ(EXTI1_IRQn);
+ EXTI_ClearITPendingBit(EXTI_Line1);
+ EXTI1_Callback();
}
void __attribute__((used)) EXTI2_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- // NVIC_ClearPendingIRQ(EXTI2_IRQn);
- // EXTI_ClearITPendingBit(EXTI_Line2);
- // EXTI2_Callback();
+ NVIC_ClearPendingIRQ(EXTI2_IRQn);
+ EXTI_ClearITPendingBit(EXTI_Line2);
+ EXTI2_Callback();
}
void __attribute__((used)) EXTI3_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- // NVIC_ClearPendingIRQ(EXTI3_IRQn);
- // EXTI_ClearITPendingBit(EXTI_Line3);
- // EXTI3_Callback();
+ NVIC_ClearPendingIRQ(EXTI3_IRQn);
+ EXTI_ClearITPendingBit(EXTI_Line3);
+ EXTI3_Callback();
}
void __attribute__((used)) EXTI4_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- // NVIC_ClearPendingIRQ(EXTI4_IRQn);
- // EXTI_ClearITPendingBit(EXTI_Line4);
- // EXTI4_Callback();
+ NVIC_ClearPendingIRQ(EXTI4_IRQn);
+ EXTI_ClearITPendingBit(EXTI_Line4);
+ EXTI4_Callback();
}
void __attribute__((used)) EXTI9_5_IRQHandler(void)
{
- //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();
- // }
+ 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)
{
- //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();
- // }
+ 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 1fe8a06ee..d424cc8c9 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,15 +54,14 @@ static int waitForCardReady(sdSpiContext_t *context, UINT timeoutMs) {
BYTE d;
uint32_t timeout = timeoutMs;
- //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--;
- // }
+ 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;
}
@@ -94,37 +93,36 @@ static int select(sdSpiContext_t *context) {
/* power on */
static void powerOn(sdSpiContext_t *context)
{
- //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;
+ 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 */
@@ -141,52 +139,50 @@ static uint8_t checkPower(sdSpiContext_t *context)
static int receiveDataBlock(sdSpiContext_t *context, BYTE *data, UINT length) {
- //COMMENTED FIRMWARE
- // BYTE token;
+ 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) {
- //COMMENTED FIRMWARE
- // if (!waitForCardReady(context, 500)) {
- // return INT_TIMEOUT;
- // }
+ 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;
}
@@ -194,60 +190,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);
- // if (res > 1) {
- // return res;
- // }
- // }
+ // Send a CMD55 prior to ACMD<n>
+ if (cmd & 0x80) {
+ cmd &= 0x7F;
+ BYTE res = sendCommand(context, CMD55, 0);
+
+ 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;
@@ -257,96 +253,95 @@ static BYTE sendCommand(sdSpiContext_t *context, BYTE cmd, DWORD arg) {
DSTATUS SD_disk_initialize(void* usrOps) {
sdSpiContext_t *context = (sdSpiContext_t *)usrOps;
- //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;
- // }
+ 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;
}
@@ -360,246 +355,243 @@ DSTATUS SD_disk_status(void * usrOps) {
DRESULT SD_disk_read(BYTE *buff, DWORD sector, UINT count, void * usrOps) {
- //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);
- // }
+ 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) {
- //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);
- // }
+ 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) {
- //COMMENTED FIRMWARE
- // sdSpiContext_t *context = (sdSpiContext_t *)usrOps;
- // BYTE csd[16];
+ 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;
}
@@ -609,29 +601,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;
- //COMMENTED FIRMWARE
- // {
- // WORD n = context->timer1;
- // if (n) {
- // context->timer1 = --n;
- // }
- // }
-
- // {
- // WORD n = context->timer2;
- // if (n) {
- // context->timer2 = --n;
- // }
- // }
+
+ {
+ 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 da45f4a51..a77e22720 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;
- // // 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)));
+ I2Cx = i2cPort;
+ devAddr = HMC5883L_ADDRESS;
- // // write CONFIG_B register
- // hmc5883lSetGain(HMC5883L_GAIN_660);
+ // 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);
isInit = true;
}
@@ -79,11 +79,10 @@ void hmc5883lInit(I2C_Dev *i2cPort)
*/
bool hmc5883lTestConnection()
{
- //COMMENTED FIRMWARE
- // if (i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_ID_A, 3, buffer))
- // {
- // return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3');
- // }
+ if (i2cdevReadReg8(I2Cx, devAddr, HMC5883L_RA_ID_A, 3, buffer))
+ {
+ return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3');
+ }
return false;
}
@@ -94,67 +93,66 @@ bool hmc5883lTestConnection()
bool hmc5883lSelfTest()
{
bool testStatus = true;
- //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;
- // }
+ 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;
}
@@ -168,13 +166,12 @@ bool hmc5883lSelfTest()
*/
bool hmc5883lEvaluateSelfTest(int16_t min, int16_t max, int16_t value, char* string)
{
- //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;
- // }
+ 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;
}
@@ -189,8 +186,7 @@ bool hmc5883lEvaluateSelfTest(int16_t min, int16_t max, int16_t value, char* str
*/
uint8_t hmc5883lGetSampleAveraging()
{
- //COMMENTED FIRMWARE
- //i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, buffer);
+ 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.
@@ -201,8 +197,7 @@ uint8_t hmc5883lGetSampleAveraging()
*/
void hmc5883lSetSampleAveraging(uint8_t averaging)
{
- //COMMENTED FIRMWARE
- //i2cdevWriteBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, averaging);
+ 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
@@ -229,8 +224,7 @@ void hmc5883lSetSampleAveraging(uint8_t averaging)
*/
uint8_t hmc5883lGetDataRate()
{
- //COMMENTED FIRMWARE
- //i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, buffer);
return buffer[0];
}
/** Set data output rate value.
@@ -243,8 +237,7 @@ uint8_t hmc5883lGetDataRate()
*/
void hmc5883lSetDataRate(uint8_t rate)
{
- //COMMENTED FIRMWARE
- //i2cdevWriteBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, rate);
+ 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)
@@ -255,8 +248,7 @@ void hmc5883lSetDataRate(uint8_t rate)
*/
uint8_t hmc5883lGetMeasurementBias()
{
- //COMMENTED FIRMWARE
- //i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, buffer);
return buffer[0];
}
/** Set measurement bias value.
@@ -268,8 +260,7 @@ uint8_t hmc5883lGetMeasurementBias()
*/
void hmc5883lSetMeasurementBias(uint8_t bias)
{
- //COMMENTED FIRMWARE
- //i2cdevWriteBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, bias);
+ i2cdevWriteBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, bias);
}
// CONFIG_B register
@@ -299,8 +290,7 @@ void hmc5883lSetMeasurementBias(uint8_t bias)
*/
uint8_t hmc5883lGetGain()
{
- //COMMENTED FIRMWARE
- //i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_B, HMC5883L_CRB_GAIN_BIT, HMC5883L_CRB_GAIN_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_CONFIG_B, HMC5883L_CRB_GAIN_BIT, HMC5883L_CRB_GAIN_LENGTH, buffer);
return buffer[0];
}
/** Set magnetic field gain value.
@@ -315,8 +305,7 @@ 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
- //COMMENTED FIRMWARE
- //i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1));
+ i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1));
}
// MODE register
@@ -345,8 +334,7 @@ void hmc5883lSetGain(uint8_t gain)
*/
uint8_t hmc5883lGetMode()
{
- //COMMENTED FIRMWARE
- //i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODEREG_BIT, HMC5883L_MODEREG_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODEREG_BIT, HMC5883L_MODEREG_LENGTH, buffer);
return buffer[0];
}
/** Set measurement mode.
@@ -364,9 +352,8 @@ 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
- //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
+ 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
@@ -384,12 +371,11 @@ void hmc5883lSetMode(uint8_t newMode)
*/
void hmc5883lGetHeading(int16_t *x, int16_t *y, int16_t *z)
{
- //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];
+ 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
@@ -399,9 +385,8 @@ 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
- //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));
+ 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.
@@ -412,9 +397,8 @@ 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
- //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));
+ 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.
@@ -425,9 +409,8 @@ 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
- //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));
+ 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];
}
@@ -446,8 +429,7 @@ int16_t hmc5883lGetHeadingZ()
*/
bool hmc5883lGetLockStatus()
{
- //COMMENTED FIRMWARE
- //i2cdevReadBit(I2Cx, devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_LOCK_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_LOCK_BIT, buffer);
return buffer[0];
}
/** Get data ready status.
@@ -462,8 +444,7 @@ bool hmc5883lGetLockStatus()
*/
bool hmc5883lGetReadyStatus()
{
- //COMMENTED FIRMWARE
- //i2cdevReadBit(I2Cx, devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_READY_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_READY_BIT, buffer);
return buffer[0];
}
@@ -474,8 +455,7 @@ bool hmc5883lGetReadyStatus()
*/
uint8_t hmc5883lGetIDA()
{
- //COMMENTED FIRMWARE
- //i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_A, buffer);
+ i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_A, buffer);
return buffer[0];
}
/** Get identification byte B
@@ -483,8 +463,7 @@ uint8_t hmc5883lGetIDA()
*/
uint8_t hmc5883lGetIDB()
{
- //COMMENTED FIRMWARE
- //i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_B, buffer);
+ i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_B, buffer);
return buffer[0];
}
/** Get identification byte C
@@ -492,7 +471,6 @@ uint8_t hmc5883lGetIDB()
*/
uint8_t hmc5883lGetIDC()
{
- //COMMENTED FIRMWARE
- //i2cdevReadByte(I2Cx, devAddr, HMC5883L_RA_ID_C, buffer);
+ 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 869a29ba6..79bd7ed9e 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,62 +221,61 @@ static void i2cNotifyClient(I2cDrv* i2c)
static void i2cdrvTryToRestartBus(I2cDrv* 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);
+ 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)
@@ -402,27 +401,26 @@ bool i2cdrvMessageTransfer(I2cDrv* i2c, I2cMessage* message)
{
bool status = false;
- //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);
+ 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;
}
@@ -430,214 +428,210 @@ bool i2cdrvMessageTransfer(I2cDrv* i2c, I2cMessage* message)
static void i2cdrvEventIsrHandler(I2cDrv* i2c)
{
- //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);
-// }
-// }
-// }
+ 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)
{
- //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);
- // }
+ 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)
{
- //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);
+ 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)
{
- //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);
- // }
+ 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 81dd6ab1a..3fa753bf2 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,8 +41,7 @@
int i2cdevInit(I2C_Dev *dev)
{
- //COMMENTED FIRMWARE
- //i2cdrvInit(dev);
+ i2cdrvInit(dev);
return true;
}
@@ -58,9 +57,9 @@ bool i2cdevReadBit(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
{
uint8_t byte;
bool status;
- //COMMENTED FIRMWARE
- // status = i2cdevReadReg8(dev, devAddress, memAddress, 1, &byte);
- // *data = byte & (1 << bitNum);
+
+ status = i2cdevReadReg8(dev, devAddress, memAddress, 1, &byte);
+ *data = byte & (1 << bitNum);
return status;
}
@@ -69,16 +68,15 @@ bool i2cdevReadBits(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
uint8_t bitStart, uint8_t length, uint8_t *data)
{
bool status;
- //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;
- // }
+ 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;
}
@@ -86,8 +84,7 @@ bool i2cdevRead(I2C_Dev *dev, uint8_t devAddress, uint16_t len, uint8_t *data)
{
I2cMessage message;
- //COMMENTED FIRMWARE
- //i2cdrvCreateMessage(&message, devAddress, i2cRead, len, data);
+ i2cdrvCreateMessage(&message, devAddress, i2cRead, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
@@ -96,9 +93,9 @@ bool i2cdevReadReg8(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
uint16_t len, uint8_t *data)
{
I2cMessage message;
- //COMMENTED FIRMWARE
- // i2cdrvCreateMessageIntAddr(&message, devAddress, false, memAddress,
- // i2cRead, len, data);
+
+ i2cdrvCreateMessageIntAddr(&message, devAddress, false, memAddress,
+ i2cRead, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
@@ -106,11 +103,10 @@ 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)
{
- //COMMENTED FIRMWARE
- // I2cMessage message;
+ I2cMessage message;
- // i2cdrvCreateMessageIntAddr(&message, devAddress, true, memAddress,
- // i2cRead, len, data);
+ i2cdrvCreateMessageIntAddr(&message, devAddress, true, memAddress,
+ i2cRead, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
@@ -125,9 +121,8 @@ bool i2cdevWriteBit(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
uint8_t bitNum, uint8_t data)
{
uint8_t byte;
- //COMMENTED FIRMWARE
- // i2cdevReadByte(dev, devAddress, memAddress, &byte);
- // byte = (data != 0) ? (byte | (1 << bitNum)) : (byte & ~(1 << bitNum));
+ i2cdevReadByte(dev, devAddress, memAddress, &byte);
+ byte = (data != 0) ? (byte | (1 << bitNum)) : (byte & ~(1 << bitNum));
return i2cdevWriteByte(dev, devAddress, memAddress, byte);
}
@@ -135,18 +130,17 @@ bool i2cdevWriteBits(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
uint8_t bitStart, uint8_t length, uint8_t data)
{
bool status;
- //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);
- // }
+ 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;
}
@@ -155,8 +149,7 @@ bool i2cdevWrite(I2C_Dev *dev, uint8_t devAddress, uint16_t len, const uint8_t *
{
I2cMessage message;
- //COMMENTED FIRMWARE
- //i2cdrvCreateMessage(&message, devAddress, i2cWrite, len, data);
+ i2cdrvCreateMessage(&message, devAddress, i2cWrite, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
@@ -166,9 +159,8 @@ bool i2cdevWriteReg8(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
{
I2cMessage message;
- //COMMENTED FIRMWARE
- // i2cdrvCreateMessageIntAddr(&message, devAddress, false, memAddress,
- // i2cWrite, len, data);
+ i2cdrvCreateMessageIntAddr(&message, devAddress, false, memAddress,
+ i2cWrite, len, data);
return i2cdrvMessageTransfer(dev, &message);
}
@@ -178,9 +170,8 @@ bool i2cdevWriteReg16(I2C_Dev *dev, uint8_t devAddress, uint16_t memAddress,
{
I2cMessage message;
- //COMMENTED FIRMWARE
- // i2cdrvCreateMessageIntAddr(&message, devAddress, true, memAddress,
- // i2cWrite, len, data);
+ 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 d0171b93a..b0f8a2f52 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,112 +66,111 @@ ErrorStatus I2C_Master_BufferRead(I2C_TypeDef* I2Cx, uint8_t* pBuffer,
uint32_t timeoutMs)
{
- //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;
- // }
+ __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;
@@ -190,114 +189,114 @@ ErrorStatus I2C_Master_BufferWrite(I2C_TypeDef* I2Cx, uint8_t* pBuffer,
uint32_t NumByteToWrite, I2C_ProgrammingModel Mode, uint8_t SlaveAddress,
uint32_t timeoutMs)
{
- //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;
-// }
+
+ __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!
@@ -312,22 +311,21 @@ ErrorStatus I2C_Master_BufferWrite(I2C_TypeDef* I2Cx, uint8_t* pBuffer,
void I2C_Slave_BufferReadWrite(I2C_TypeDef* I2Cx, I2C_ProgrammingModel Mode)
{
- //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;
- // }
+ /* 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;
+ }
}
/**
@@ -337,127 +335,126 @@ void I2C_Slave_BufferReadWrite(I2C_TypeDef* I2Cx, I2C_ProgrammingModel Mode)
*/
void I2C_LowLevel_Init(I2C_TypeDef* I2Cx)
{
- //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);
-
- // }
+ 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);
+
+ }
}
/**
@@ -468,55 +465,54 @@ void I2C_LowLevel_Init(I2C_TypeDef* I2Cx)
void I2C_DMAConfig(I2C_TypeDef* I2Cx, uint8_t* pBuffer, uint32_t BufferSize,
uint32_t Direction)
{
- //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);
- // }
-
- // }
+ /* 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);
+ }
+
+ }
}
/**
@@ -526,116 +522,116 @@ void I2C_DMAConfig(I2C_TypeDef* I2Cx, uint8_t* pBuffer, uint32_t BufferSize,
*/
void __attribute__((used)) I2C1_EV_IRQHandler(void)
{
- //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;
- // }
- // }
+
+ __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;
+ }
+ }
}
/**
@@ -645,128 +641,125 @@ void __attribute__((used)) I2C1_EV_IRQHandler(void)
*/
void __attribute__((used)) I2C2_EV_IRQHandler(void)
{
- //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;
- // }
- // }
+ __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)
{
- //COMMENTED FIRMWARE
- //i2cErrorInterruptHandler(I2C1);
+ i2cErrorInterruptHandler(I2C1);
}
void __attribute__((used)) I2C2_ER_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- //i2cErrorInterruptHandler(I2C2);
+ i2cErrorInterruptHandler(I2C2);
}
/**
@@ -776,35 +769,35 @@ void __attribute__((used)) I2C2_ER_IRQHandler(void)
*/
void i2cErrorInterruptHandler(I2C_TypeDef* I2Cx)
{
- //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;
- // }
+
+ __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 c538e7aab..ade1a6859 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,90 +63,85 @@ static bool isInit = false;
//Initialize the green led pin as output
void ledInit()
{
- //COMMENTED FIRMWARE
- // int i;
+ 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)
{
- //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);
+ 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)
{
- //COMMENTED FIRMWARE
- // int i;
-
- // for (i = 0; i < LED_NUM; i++)
- // {
- // //Turn off the LED:s
- // ledSet(i, 0);
- // }
+ int i;
+
+ for (i = 0; i < LED_NUM; i++)
+ {
+ //Turn off the LED:s
+ ledSet(i, 0);
+ }
}
void ledSetAll(void)
{
- //COMMENTED FIRMWARE
- // int i;
-
- // for (i = 0; i < LED_NUM; i++)
- // {
- // //Turn on the LED:s
- // ledSet(i, 1);
- // }
+ int i;
+
+ for (i = 0; i < LED_NUM; i++)
+ {
+ //Turn on the LED:s
+ ledSet(i, 1);
+ }
}
void ledSet(led_t led, bool value)
{
- //COMMENTED FIRMWARE
- // if (led>LED_NUM)
- // return;
+ 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 1c3715cd4..8bf183247 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,12 +60,11 @@ static bool lhExchange(uint16_t writeLen, uint8_t* writeData, uint16_t readLen,
{
bool status = false;
- //COMMENTED FIRMWARE
- // status = i2cdevWrite(I2C1_DEV, devAddr, writeLen, writeData);
- // if (status && readLen && readData)
- // {
- // status = i2cdevRead(I2C1_DEV, devAddr, readLen, readData);
- // }
+ status = i2cdevWrite(I2C1_DEV, devAddr, writeLen, writeData);
+ if (status && readLen && readData)
+ {
+ status = i2cdevRead(I2C1_DEV, devAddr, readLen, readData);
+ }
return status;
}
@@ -111,30 +110,27 @@ bool lhblTest()
static bool lhblFlashReadStatus(uint8_t* status)
{
- //COMMENTED FIRMWARE
- // lhblHeaderPrepare(1, 1, flashWriteBuf);
- // flashWriteBuf[5] = FLASH_CMD_READ_STATUS;
+ lhblHeaderPrepare(1, 1, flashWriteBuf);
+ flashWriteBuf[5] = FLASH_CMD_READ_STATUS;
return lhExchange(5+1, flashWriteBuf, 1, status);
}
static bool lhblFlashWriteEnable(void)
{
- //COMMENTED FIRMWARE
- // lhblHeaderPrepare(1, 0, flashWriteBuf);
- // flashWriteBuf[5] = FLASH_CMD_WRITE_EN;
+ lhblHeaderPrepare(1, 0, flashWriteBuf);
+ flashWriteBuf[5] = FLASH_CMD_WRITE_EN;
return lhExchange(5 + 1, flashWriteBuf, 0, 0);
}
static bool lhblFlashEraseBlock64k(uint32_t address)
{
- //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);
+ 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);
}
@@ -142,35 +138,33 @@ static bool lhblFlashEraseBlock64k(uint32_t address)
static bool lhblFlashWaitComplete(void)
{
bool status;
- //COMMENTED FIRMWARE
- // uint8_t flashStatus;
+ 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)
{
- //COMMENTED FIRMWARE
- // ASSERT(address >= LH_FW_ADDR);
- // ASSERT(length <= LH_FLASH_PAGE_SIZE);
+ 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);
}
@@ -197,21 +191,19 @@ bool lhblFlashGetProtocolVersion(void)
bool lhblFlashRead(uint32_t address, uint16_t length, uint8_t *data)
{
- //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);
+ 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)
{
- //COMMENTED FIRMWARE
- // lhblHeaderPrepare(1, 0, flashWriteBuf);
- // flashWriteBuf[5] = FLASH_CMD_WAKEUP;
+ lhblHeaderPrepare(1, 0, flashWriteBuf);
+ flashWriteBuf[5] = FLASH_CMD_WAKEUP;
return lhExchange(5 + 1, flashWriteBuf, 0, 0);
}
@@ -220,15 +212,14 @@ 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;
}
@@ -236,45 +227,44 @@ bool lhblFlashEraseFirmware(void)
bool lhblFlashWriteFW(uint8_t *data, uint32_t length)
{
bool status = true;
- //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;
- // }
- // }
+ 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 b13fdb0e2..9f18fe769 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,39 +93,37 @@ static uint32_t statusCode = 0;
static bool lhExchange(uint16_t writeLen, uint8_t* writeData, uint16_t readLen, uint8_t* readData)
{
- //COMMENTED FIRMWARE
- // static uint8_t wbuffer[1024];
- // static uint8_t rbuffer[1024];
+ 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)
{
- //COMMENTED FIRMWARE
- // for (int i = 0; i < length; i++)
- // {
- // if (dataA[i] != dataB[i])
- // {
- // return false;
- // }
- // }
+ for (int i = 0; i < length; i++)
+ {
+ if (dataA[i] != dataB[i])
+ {
+ return false;
+ }
+ }
return true;
}
@@ -135,16 +133,15 @@ bool lhflashInit()
if (isInit)
return true;
- //COMMENTED FIRMWARE
- // spiBegin();
+ 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;
}
@@ -159,27 +156,25 @@ static bool flashReadStatus(uint8_t* status)
static bool flashWriteEnable(void)
{
bool status = true;
- //COMMENTED FIRMWARE
- // uint8_t flash_status = 0;
+ 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)
{
- //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);
+ 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);
}
@@ -187,67 +182,61 @@ static bool flashEraseBlock64k(uint32_t address)
static bool flashWaitComplete(void)
{
bool status;
- //COMMENTED FIRMWARE
- // uint8_t flashStatus;
+ 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)
{
- //COMMENTED FIRMWARE
- // ASSERT(length <= LH_FLASH_PAGE_SIZE);
+ 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)
{
- //COMMENTED FIRMWARE
- // digitalWrite(LH_FPGA_RESET, 0);
- // pinMode(LH_SPI_CS, OUTPUT);
- // digitalWrite(LH_SPI_CS, 1);
+ digitalWrite(LH_FPGA_RESET, 0);
+ pinMode(LH_SPI_CS, OUTPUT);
+ digitalWrite(LH_SPI_CS, 1);
}
static void fpgaReleaseReset(void)
{
- //COMMENTED FIRMWARE
- // pinMode(LH_SPI_CS, INPUT);
- // digitalWrite(LH_FPGA_RESET, 1);
+ pinMode(LH_SPI_CS, INPUT);
+ digitalWrite(LH_FPGA_RESET, 1);
}
static bool flashRead(uint32_t address, uint16_t length, uint8_t *data)
{
- //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);
+ 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)
{
- //COMMENTED FIRMWARE
- //flashWriteBuf[0] = FLASH_CMD_WAKEUP;
+ flashWriteBuf[0] = FLASH_CMD_WAKEUP;
return lhExchange(1, flashWriteBuf, 0, 0);
}
@@ -263,15 +252,13 @@ static bool flashReset()
{
bool status = true;
- //COMMENTED FIRMWARE
-
- // flashWriteBuf[0] = 0x66;
+ 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;
}
@@ -281,26 +268,24 @@ static bool flashErase(void)
bool status = true;
/* Disable write protection */
- //COMMENTED FIRMWARE
- // flashWriteEnable();
+ 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();
@@ -314,59 +299,57 @@ static bool flashErase(void)
static void flashProtectBootloader()
{
- //COMMENTED FIRMWARE
- // flashWriteEnable();
+ 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;
- //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;
- // }
- // }
+ 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;
}
@@ -374,35 +357,35 @@ static bool flashWriteFW(uint8_t *data, uint32_t length)
bool lhflashFlashBootloader()
{
bool pass = true;
- //COMMENTED FIRMWARE
- // fpgaHoldReset();
- // vTaskDelay(M2T(10));
+ fpgaHoldReset();
+
+ 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 4cb77a181..f724fc6d6 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;
- // vTaskDelay(M2T(5));
+ I2Cx = i2cPort;
+ devAddr = LPS25H_I2C_ADDR;
+
+ vTaskDelay(M2T(5));
isInit = true;
@@ -57,18 +57,17 @@ bool lps25hInit(I2C_Dev *i2cPort)
bool lps25hTestConnection(void)
{
- //COMMENTED FIRMWARE
- // uint8_t id;
- // bool status;
+ 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;
}
@@ -97,13 +96,12 @@ bool lps25hSelfTest(void)
bool lps25hEvaluateSelfTest(float min, float max, float value, char* string)
{
- //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;
- // }
+ 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;
}
@@ -111,28 +109,28 @@ bool lps25hSetEnabled(bool enable)
{
uint8_t enable_mask;
bool status;
- //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);
- // }
+
+ 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;
}
@@ -144,18 +142,17 @@ bool lps25hGetData(float* pressure, float* temperature, float* asl)
int16_t rawTemp;
bool status;
- //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]);
+ 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 4286fbb6d..cebebe40e 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. */
- //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);
- // }
+
+ 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);
- // if(NULL != accuracy) {
- // *accuracy = maxSonarGetAccuracyMB1040(maxSonarDistance);
- // }
+ maxSonarDistance = (uint32_t) (IN2MM(analogRead(pin)) / 8);
+
+ if(NULL != accuracy) {
+ *accuracy = maxSonarGetAccuracyMB1040(maxSonarDistance);
+ }
return maxSonarDistance;
}
@@ -140,18 +140,17 @@ static uint32_t maxSonarReadDistanceMB1040AN(const deckPin_t pin, uint32_t *accu
*/
uint32_t maxSonarReadDistance(maxSonarSensor_t type, uint32_t *accuracy)
{
- //COMMENTED FIRMWARE
- // switch(type) {
- // case MAXSONAR_MB1040_AN: {
- // return maxSonarReadDistanceMB1040AN(MAXSONAR_DECK_GPIO, accuracy);
- // }
- // }
+ 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/drivers/src/motors.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/motors.c
index ea2712d94..24bccfb73 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/motors.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/motors.c
@@ -67,22 +67,22 @@ static bool isInit = false;
static uint16_t motorsBLConvBitsTo16(uint16_t bits)
{
- // return (0xFFFF * (bits - MOTORS_BL_PWM_CNT_FOR_HIGH) / MOTORS_BL_PWM_CNT_FOR_HIGH);
+ return (0xFFFF * (bits - MOTORS_BL_PWM_CNT_FOR_HIGH) / MOTORS_BL_PWM_CNT_FOR_HIGH);
}
static uint16_t motorsBLConv16ToBits(uint16_t bits)
{
- // return (MOTORS_BL_PWM_CNT_FOR_HIGH + ((bits * MOTORS_BL_PWM_CNT_FOR_HIGH) / 0xFFFF));
+ return (MOTORS_BL_PWM_CNT_FOR_HIGH + ((bits * MOTORS_BL_PWM_CNT_FOR_HIGH) / 0xFFFF));
}
static uint16_t motorsConvBitsTo16(uint16_t bits)
{
- // return ((bits) << (16 - MOTORS_PWM_BITS));
+ return ((bits) << (16 - MOTORS_PWM_BITS));
}
static uint16_t motorsConv16ToBits(uint16_t bits)
{
- // return ((bits) >> (16 - MOTORS_PWM_BITS) & ((1 << MOTORS_PWM_BITS) - 1));
+ return ((bits) >> (16 - MOTORS_PWM_BITS) & ((1 << MOTORS_PWM_BITS) - 1));
}
// We have data that maps PWM to thrust at different supply voltage levels.
@@ -130,12 +130,12 @@ static uint16_t motorsConv16ToBits(uint16_t bits)
// Voltage needed with the Supply voltage.
static uint16_t motorsCompensateBatteryVoltage(uint16_t ithrust)
{
- // float thrust = ((float) ithrust / 65536.0f) * 60;
- // float volts = -0.0006239f * thrust * thrust + 0.088f * thrust;
- // float supply_voltage = pmGetBatteryVoltage();
- // float percentage = volts / supply_voltage;
- // percentage = percentage > 1.0f ? 1.0f : percentage;
- // return percentage * UINT16_MAX;
+ float thrust = ((float) ithrust / 65536.0f) * 60;
+ float volts = -0.0006239f * thrust * thrust + 0.088f * thrust;
+ float supply_voltage = pmGetBatteryVoltage();
+ float percentage = volts / supply_voltage;
+ percentage = percentage > 1.0f ? 1.0f : percentage;
+ return percentage * UINT16_MAX;
}
/* Public functions */
@@ -143,83 +143,83 @@ static uint16_t motorsCompensateBatteryVoltage(uint16_t ithrust)
//Initialization. Will set all motors ratio to 0%
void motorsInit(const MotorPerifDef** motorMapSelect)
{
- // int i;
- // //Init structures
- // GPIO_InitTypeDef GPIO_InitStructure;
- // TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- // TIM_OCInitTypeDef TIM_OCInitStructure;
-
- // if (isInit)
- // {
- // // First to init will configure it
- // // return;
- // }
-
- // // motorMap = motorMapSelect;
-
- // DEBUG_PRINT("Using %s motor driver\n", motorMap[0]->drvType == BRUSHED ? "brushed" : "brushless");
-
- // for (i = 0; i < NBR_OF_MOTORS; i++)
- // {
- // // //Clock the gpio and the timers
- // // MOTORS_RCC_GPIO_CMD(motorMap[i]->gpioPerif, ENABLE);
- // // MOTORS_RCC_GPIO_CMD(motorMap[i]->gpioPowerswitchPerif, ENABLE);
- // // MOTORS_RCC_TIM_CMD(motorMap[i]->timPerif, ENABLE);
-
- // // If there is a power switch, as on Bolt, enable power to ESC by
- // // switching on mosfet.
- // if (motorMap[i]->gpioPowerswitchPin != 0)
- // {
- // // GPIO_StructInit(&GPIO_InitStructure);
- // // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
- // // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- // // GPIO_InitStructure.GPIO_Pin = motorMap[i]->gpioPowerswitchPin;
- // // GPIO_Init(motorMap[i]->gpioPowerswitchPort, &GPIO_InitStructure);
- // // GPIO_WriteBit(motorMap[i]->gpioPowerswitchPort, motorMap[i]->gpioPowerswitchPin, 1);
- // }
-
- // // Configure the GPIO for the timer output
- // GPIO_StructInit(&GPIO_InitStructure);
- // GPIO_InitStructure.GPIO_Mode = MOTORS_GPIO_MODE;
- // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- // GPIO_InitStructure.GPIO_OType = motorMap[i]->gpioOType;
- // GPIO_InitStructure.GPIO_Pin = motorMap[i]->gpioPin;
- // GPIO_Init(motorMap[i]->gpioPort, &GPIO_InitStructure);
-
- // //Map timers to alternate functions
- // MOTORS_GPIO_AF_CFG(motorMap[i]->gpioPort, motorMap[i]->gpioPinSource, motorMap[i]->gpioAF);
-
- // //Timer configuration
- // TIM_TimeBaseStructure.TIM_Period = motorMap[i]->timPeriod;
- // TIM_TimeBaseStructure.TIM_Prescaler = motorMap[i]->timPrescaler;
- // TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- // TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- // TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
- // TIM_TimeBaseInit(motorMap[i]->tim, &TIM_TimeBaseStructure);
-
- // // PWM channels configuration (All identical!)
- // TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
- // TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
- // TIM_OCInitStructure.TIM_Pulse = 0;
- // TIM_OCInitStructure.TIM_OCPolarity = motorMap[i]->timPolarity;
- // TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
-
- // // Configure Output Compare for PWM
- // motorMap[i]->ocInit(motorMap[i]->tim, &TIM_OCInitStructure);
- // motorMap[i]->preloadConfig(motorMap[i]->tim, TIM_OCPreload_Enable);
-
- // MOTORS_TIM_DBG_CFG(motorMap[i]->timDbgStop, ENABLE);
- // //Enable the timer PWM outputs
- // TIM_CtrlPWMOutputs(motorMap[i]->tim, ENABLE);
- // }
-
- // // Start the timers
- // for (i = 0; i < NBR_OF_MOTORS; i++)
- // {
- // TIM_Cmd(motorMap[i]->tim, ENABLE);
- // }
-
- // isInit = true;
+ int i;
+ //Init structures
+ GPIO_InitTypeDef GPIO_InitStructure;
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ TIM_OCInitTypeDef TIM_OCInitStructure;
+
+ if (isInit)
+ {
+ // First to init will configure it
+ return;
+ }
+
+ motorMap = motorMapSelect;
+
+ DEBUG_PRINT("Using %s motor driver\n", motorMap[0]->drvType == BRUSHED ? "brushed" : "brushless");
+
+ for (i = 0; i < NBR_OF_MOTORS; i++)
+ {
+ //Clock the gpio and the timers
+ MOTORS_RCC_GPIO_CMD(motorMap[i]->gpioPerif, ENABLE);
+ MOTORS_RCC_GPIO_CMD(motorMap[i]->gpioPowerswitchPerif, ENABLE);
+ MOTORS_RCC_TIM_CMD(motorMap[i]->timPerif, ENABLE);
+
+ // If there is a power switch, as on Bolt, enable power to ESC by
+ // switching on mosfet.
+ if (motorMap[i]->gpioPowerswitchPin != 0)
+ {
+ GPIO_StructInit(&GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_Pin = motorMap[i]->gpioPowerswitchPin;
+ GPIO_Init(motorMap[i]->gpioPowerswitchPort, &GPIO_InitStructure);
+ GPIO_WriteBit(motorMap[i]->gpioPowerswitchPort, motorMap[i]->gpioPowerswitchPin, 1);
+ }
+
+ // Configure the GPIO for the timer output
+ GPIO_StructInit(&GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Mode = MOTORS_GPIO_MODE;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_OType = motorMap[i]->gpioOType;
+ GPIO_InitStructure.GPIO_Pin = motorMap[i]->gpioPin;
+ GPIO_Init(motorMap[i]->gpioPort, &GPIO_InitStructure);
+
+ //Map timers to alternate functions
+ MOTORS_GPIO_AF_CFG(motorMap[i]->gpioPort, motorMap[i]->gpioPinSource, motorMap[i]->gpioAF);
+
+ //Timer configuration
+ TIM_TimeBaseStructure.TIM_Period = motorMap[i]->timPeriod;
+ TIM_TimeBaseStructure.TIM_Prescaler = motorMap[i]->timPrescaler;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+ TIM_TimeBaseInit(motorMap[i]->tim, &TIM_TimeBaseStructure);
+
+ // PWM channels configuration (All identical!)
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+ TIM_OCInitStructure.TIM_Pulse = 0;
+ TIM_OCInitStructure.TIM_OCPolarity = motorMap[i]->timPolarity;
+ TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
+
+ // Configure Output Compare for PWM
+ motorMap[i]->ocInit(motorMap[i]->tim, &TIM_OCInitStructure);
+ motorMap[i]->preloadConfig(motorMap[i]->tim, TIM_OCPreload_Enable);
+
+ MOTORS_TIM_DBG_CFG(motorMap[i]->timDbgStop, ENABLE);
+ //Enable the timer PWM outputs
+ TIM_CtrlPWMOutputs(motorMap[i]->tim, ENABLE);
+ }
+
+ // Start the timers
+ for (i = 0; i < NBR_OF_MOTORS; i++)
+ {
+ TIM_Cmd(motorMap[i]->tim, ENABLE);
+ }
+
+ isInit = true;
// Output zero power
motorsSetRatio(MOTOR_M1, 0);
@@ -230,149 +230,149 @@ void motorsInit(const MotorPerifDef** motorMapSelect)
void motorsDeInit(const MotorPerifDef** motorMapSelect)
{
- // int i;
- // GPIO_InitTypeDef GPIO_InitStructure;
-
- // for (i = 0; i < NBR_OF_MOTORS; i++)
- // {
- // // Configure default
- // GPIO_StructInit(&GPIO_InitStructure);
- // GPIO_InitStructure.GPIO_Pin = motorMap[i]->gpioPin;
- // GPIO_Init(motorMap[i]->gpioPort, &GPIO_InitStructure);
-
- // //Map timers to alternate functions
- // GPIO_PinAFConfig(motorMap[i]->gpioPort, motorMap[i]->gpioPinSource, 0x00);
-
- // //Deinit timer
- // TIM_DeInit(motorMap[i]->tim);
- // }
+ int i;
+ GPIO_InitTypeDef GPIO_InitStructure;
+
+ for (i = 0; i < NBR_OF_MOTORS; i++)
+ {
+ // Configure default
+ GPIO_StructInit(&GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = motorMap[i]->gpioPin;
+ GPIO_Init(motorMap[i]->gpioPort, &GPIO_InitStructure);
+
+ //Map timers to alternate functions
+ GPIO_PinAFConfig(motorMap[i]->gpioPort, motorMap[i]->gpioPinSource, 0x00);
+
+ //Deinit timer
+ TIM_DeInit(motorMap[i]->tim);
+ }
}
bool motorsTest(void)
{
-// int i;
-
-// for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++)
-// {
-// if (motorMap[i]->drvType == BRUSHED)
-// {
-// #ifdef ACTIVATE_STARTUP_SOUND
-// motorsBeep(MOTORS[i], true, testsound[i], (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / A4)/ 20);
-// vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
-// motorsBeep(MOTORS[i], false, 0, 0);
-// vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS));
-// #else
-// motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO);
-// vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
-// motorsSetRatio(MOTORS[i], 0);
-// vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS));
-// #endif
-// }
-// }
-
-// return isInit;
+ int i;
+
+ for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++)
+ {
+ if (motorMap[i]->drvType == BRUSHED)
+ {
+#ifdef ACTIVATE_STARTUP_SOUND
+ motorsBeep(MOTORS[i], true, testsound[i], (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / A4)/ 20);
+ vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
+ motorsBeep(MOTORS[i], false, 0, 0);
+ vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS));
+#else
+ motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO);
+ vTaskDelay(M2T(MOTORS_TEST_ON_TIME_MS));
+ motorsSetRatio(MOTORS[i], 0);
+ vTaskDelay(M2T(MOTORS_TEST_DELAY_TIME_MS));
+#endif
+ }
+ }
+
+ return isInit;
}
// Ithrust is thrust mapped for 65536 <==> 60 grams
void motorsSetRatio(uint32_t id, uint16_t ithrust)
{
-// if (isInit) {
-// uint16_t ratio;
-
-// ASSERT(id < NBR_OF_MOTORS);
-
-// ratio = ithrust;
-
-// #ifdef ENABLE_THRUST_BAT_COMPENSATED
-// if (motorMap[id]->drvType == BRUSHED)
-// {
-// // To make sure we provide the correct PWM given current supply voltage
-// // from the battery, we do calculations based on measurements of PWM,
-// // voltage and thrust. See comment at function definition for details.
-// ratio = motorsCompensateBatteryVoltage(ithrust);
-// motor_ratios[id] = ratio;
-// }
-// #endif
-// if (motorMap[id]->drvType == BRUSHLESS)
-// {
-// motorMap[id]->setCompare(motorMap[id]->tim, motorsBLConv16ToBits(ratio));
-// }
-// else
-// {
-// motorMap[id]->setCompare(motorMap[id]->tim, motorsConv16ToBits(ratio));
-// }
-// }
+ if (isInit) {
+ uint16_t ratio;
+
+ ASSERT(id < NBR_OF_MOTORS);
+
+ ratio = ithrust;
+
+#ifdef ENABLE_THRUST_BAT_COMPENSATED
+ if (motorMap[id]->drvType == BRUSHED)
+ {
+ // To make sure we provide the correct PWM given current supply voltage
+ // from the battery, we do calculations based on measurements of PWM,
+ // voltage and thrust. See comment at function definition for details.
+ ratio = motorsCompensateBatteryVoltage(ithrust);
+ motor_ratios[id] = ratio;
+ }
+#endif
+ if (motorMap[id]->drvType == BRUSHLESS)
+ {
+ motorMap[id]->setCompare(motorMap[id]->tim, motorsBLConv16ToBits(ratio));
+ }
+ else
+ {
+ motorMap[id]->setCompare(motorMap[id]->tim, motorsConv16ToBits(ratio));
+ }
+ }
}
int motorsGetRatio(uint32_t id)
{
- // int ratio;
-
- // ASSERT(id < NBR_OF_MOTORS);
- // if (motorMap[id]->drvType == BRUSHLESS)
- // {
- // ratio = motorsBLConvBitsTo16(motorMap[id]->getCompare(motorMap[id]->tim));
- // }
- // else
- // {
- // ratio = motorsConvBitsTo16(motorMap[id]->getCompare(motorMap[id]->tim));
- // }
-
- // return ratio;
+ int ratio;
+
+ ASSERT(id < NBR_OF_MOTORS);
+ if (motorMap[id]->drvType == BRUSHLESS)
+ {
+ ratio = motorsBLConvBitsTo16(motorMap[id]->getCompare(motorMap[id]->tim));
+ }
+ else
+ {
+ ratio = motorsConvBitsTo16(motorMap[id]->getCompare(motorMap[id]->tim));
+ }
+
+ return ratio;
}
void motorsBeep(int id, bool enable, uint16_t frequency, uint16_t ratio)
{
- // TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
-
- // ASSERT(id < NBR_OF_MOTORS);
-
- // TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
-
- // if (enable)
- // {
- // TIM_TimeBaseStructure.TIM_Prescaler = (5 - 1);
- // TIM_TimeBaseStructure.TIM_Period = (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / frequency);
- // }
- // else
- // {
- // TIM_TimeBaseStructure.TIM_Period = motorMap[id]->timPeriod;
- // TIM_TimeBaseStructure.TIM_Prescaler = motorMap[id]->timPrescaler;
- // }
-
- // // Timer configuration
- // TIM_TimeBaseInit(motorMap[id]->tim, &TIM_TimeBaseStructure);
- // motorMap[id]->setCompare(motorMap[id]->tim, ratio);
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+
+ ASSERT(id < NBR_OF_MOTORS);
+
+ TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
+
+ if (enable)
+ {
+ TIM_TimeBaseStructure.TIM_Prescaler = (5 - 1);
+ TIM_TimeBaseStructure.TIM_Period = (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / frequency);
+ }
+ else
+ {
+ TIM_TimeBaseStructure.TIM_Period = motorMap[id]->timPeriod;
+ TIM_TimeBaseStructure.TIM_Prescaler = motorMap[id]->timPrescaler;
+ }
+
+ // Timer configuration
+ TIM_TimeBaseInit(motorMap[id]->tim, &TIM_TimeBaseStructure);
+ motorMap[id]->setCompare(motorMap[id]->tim, ratio);
}
// Play a tone with a given frequency and a specific duration in milliseconds (ms)
void motorsPlayTone(uint16_t frequency, uint16_t duration_msec)
{
- // motorsBeep(MOTOR_M1, true, frequency, (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / frequency)/ 20);
- // motorsBeep(MOTOR_M2, true, frequency, (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / frequency)/ 20);
- // motorsBeep(MOTOR_M3, true, frequency, (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / frequency)/ 20);
- // motorsBeep(MOTOR_M4, true, frequency, (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / frequency)/ 20);
- // vTaskDelay(M2T(duration_msec));
- // motorsBeep(MOTOR_M1, false, frequency, 0);
- // motorsBeep(MOTOR_M2, false, frequency, 0);
- // motorsBeep(MOTOR_M3, false, frequency, 0);
- // motorsBeep(MOTOR_M4, false, frequency, 0);
+ motorsBeep(MOTOR_M1, true, frequency, (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / frequency)/ 20);
+ motorsBeep(MOTOR_M2, true, frequency, (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / frequency)/ 20);
+ motorsBeep(MOTOR_M3, true, frequency, (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / frequency)/ 20);
+ motorsBeep(MOTOR_M4, true, frequency, (uint16_t)(MOTORS_TIM_BEEP_CLK_FREQ / frequency)/ 20);
+ vTaskDelay(M2T(duration_msec));
+ motorsBeep(MOTOR_M1, false, frequency, 0);
+ motorsBeep(MOTOR_M2, false, frequency, 0);
+ motorsBeep(MOTOR_M3, false, frequency, 0);
+ motorsBeep(MOTOR_M4, false, frequency, 0);
}
// Plays a melody from a note array
void motorsPlayMelody(uint16_t *notes)
{
- // int i = 0;
- // uint16_t note; // Note in hz
- // uint16_t duration; // Duration in ms
-
- // do
- // {
- // note = notes[i++];
- // duration = notes[i++];
- // motorsPlayTone(note, duration);
- // } while (duration != 0);
+ int i = 0;
+ uint16_t note; // Note in hz
+ uint16_t duration; // Duration in ms
+
+ do
+ {
+ note = notes[i++];
+ duration = notes[i++];
+ motorsPlayTone(note, duration);
+ } while (duration != 0);
}
/**
* Logging variables of the motors PWM output
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/mpu6050.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/mpu6050.c
index fd3ae6282..ba0aa1a14 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/mpu6050.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/mpu6050.c
@@ -50,25 +50,24 @@ static bool isInit;
*/
void mpu6050Init(I2C_Dev *i2cPort)
{
-// if (isInit)
-// return;
+ if (isInit)
+ return;
-// I2Cx = i2cPort;
-// devAddr = MPU6050_ADDRESS_AD0_HIGH;
-// //FIXME devAddr = MPU6050_ADDRESS_AD0_LOW;
+ I2Cx = i2cPort;
+ devAddr = MPU6050_ADDRESS_AD0_HIGH;
+//FIXME devAddr = MPU6050_ADDRESS_AD0_LOW;
isInit = true;
}
bool mpu6050Test(void)
{
- // bool testStatus;
- bool testStatus = true;
+ bool testStatus;
- // if (!isInit)
- // return false;
+ if (!isInit)
+ return false;
- // testStatus = mpu6050TestConnection();
+ testStatus = mpu6050TestConnection();
return testStatus;
}
@@ -79,7 +78,7 @@ bool mpu6050Test(void)
*/
bool mpu6050TestConnection()
{
- // return mpu6050GetDeviceID() == 0b110100;
+ return mpu6050GetDeviceID() == 0b110100;
}
/** Do a MPU6050 self test.
@@ -87,100 +86,99 @@ bool mpu6050TestConnection()
*/
bool mpu6050SelfTest()
{
-// bool testStatus = true;
-// int16_t axi16, ayi16, azi16;
-// int16_t gxi16, gyi16, gzi16;
-// // int8_t axfi8, ayfi8, azfi8;
-// // int8_t gxfi8, gyfi8, gzfi8;
-// float axf, ayf, azf;
-// float gxf, gyf, gzf;
-// float axfTst, ayfTst, azfTst;
-// float gxfTst, gyfTst, gzfTst;
-// float axfDiff, ayfDiff, azfDiff;
-// float gxfDiff, gyfDiff, gzfDiff;
-// float gRange, aRange;
-// uint32_t scrap;
-
-// aRange = mpu6050GetFullScaleAccelGPL();
-// gRange = mpu6050GetFullScaleGyroDPL();
-
-// // First values after startup can be read as zero. Scrap a couple to be sure.
-// for (scrap = 0; scrap < 20; scrap++)
-// {
-// mpu6050GetMotion6(&axi16, &ayi16, &azi16, &gxi16, &gyi16, &gzi16);
-// vTaskDelay(M2T(2));
-// }
-// // First measurement
-// gxf = gxi16 * gRange;
-// gyf = gyi16 * gRange;
-// gzf = gzi16 * gRange;
-// axf = axi16 * aRange;
-// ayf = ayi16 * aRange;
-// azf = azi16 * aRange;
-
-// // Enable self test
-// mpu6050SetGyroXSelfTest(true);
-// mpu6050SetGyroYSelfTest(true);
-// mpu6050SetGyroZSelfTest(true);
-// mpu6050SetAccelXSelfTest(true);
-// mpu6050SetAccelYSelfTest(true);
-// mpu6050SetAccelZSelfTest(true);
-
-// // Wait for self test to take effect
-// vTaskDelay(M2T(MPU6050_SELF_TEST_DELAY_MS));
-// // Take second measurement
-// mpu6050GetMotion6(&axi16, &ayi16, &azi16, &gxi16, &gyi16, &gzi16);
-// gxfTst = gxi16 * gRange;
-// gyfTst = gyi16 * gRange;
-// gzfTst = gzi16 * gRange;
-// axfTst = axi16 * aRange;
-// ayfTst = ayi16 * aRange;
-// azfTst = azi16 * aRange;
-
-// // Disable self test
-// mpu6050SetGyroXSelfTest(false);
-// mpu6050SetGyroYSelfTest(false);
-// mpu6050SetGyroZSelfTest(false);
-// mpu6050SetAccelXSelfTest(false);
-// mpu6050SetAccelYSelfTest(false);
-// mpu6050SetAccelZSelfTest(false);
-
-// // // Read factory values
-// // i2cdevReadByte(I2Cx, devAddr, 0x00, (uint8_t *)&gxfi8);
-// // i2cdevReadByte(I2Cx, devAddr, 0x01, (uint8_t *)&gyfi8);
-// // i2cdevReadByte(I2Cx, devAddr, 0x02, (uint8_t *)&gzfi8);
-// // i2cdevReadByte(I2Cx, devAddr, 0x0D, (uint8_t *)&axfi8);
-// // i2cdevReadByte(I2Cx, devAddr, 0x0E, (uint8_t *)&ayfi8);
-// // i2cdevReadByte(I2Cx, devAddr, 0x0F, (uint8_t *)&azfi8);
-// //
-// // DEBUG_PRINT("gxf:%i, gyf:%i, gzf:%i, axf:%i, ayf:%i, azf:%i\n",
-// // (int)gxfi8, (int)gyfi8, (int)gzfi8, (int)axfi8, (int)ayfi8, (int)azfi8);
-
-// // Calculate difference
-// gxfDiff = gxfTst - gxf;
-// gyfDiff = gyfTst - gyf;
-// gzfDiff = gzfTst - gzf;
-// axfDiff = axfTst - axf;
-// ayfDiff = ayfTst - ayf;
-// azfDiff = azfTst - azf;
-
-// // Check result
-// if (mpu6050EvaluateSelfTest(MPU6050_ST_GYRO_LOW, MPU6050_ST_GYRO_HIGH, gxfDiff, "gyro X") &&
-// mpu6050EvaluateSelfTest(-MPU6050_ST_GYRO_HIGH, -MPU6050_ST_GYRO_LOW, gyfDiff, "gyro Y") &&
-// mpu6050EvaluateSelfTest(MPU6050_ST_GYRO_LOW, MPU6050_ST_GYRO_HIGH, gzfDiff, "gyro Z") &&
-// mpu6050EvaluateSelfTest(MPU6050_ST_ACCEL_LOW, MPU6050_ST_ACCEL_HIGH, axfDiff, "acc X") &&
-// mpu6050EvaluateSelfTest(MPU6050_ST_ACCEL_LOW, MPU6050_ST_ACCEL_HIGH, ayfDiff, "acc Y") &&
-// mpu6050EvaluateSelfTest(MPU6050_ST_ACCEL_LOW, MPU6050_ST_ACCEL_HIGH, azfDiff, "acc Z"))
-// {
-// DEBUG_PRINT("Self test [OK].\n");
-// }
-// else
-// {
-// testStatus = false;
-// }
-
- // return testStatus;
- return false;
+ bool testStatus = true;
+ int16_t axi16, ayi16, azi16;
+ int16_t gxi16, gyi16, gzi16;
+// int8_t axfi8, ayfi8, azfi8;
+// int8_t gxfi8, gyfi8, gzfi8;
+ float axf, ayf, azf;
+ float gxf, gyf, gzf;
+ float axfTst, ayfTst, azfTst;
+ float gxfTst, gyfTst, gzfTst;
+ float axfDiff, ayfDiff, azfDiff;
+ float gxfDiff, gyfDiff, gzfDiff;
+ float gRange, aRange;
+ uint32_t scrap;
+
+ aRange = mpu6050GetFullScaleAccelGPL();
+ gRange = mpu6050GetFullScaleGyroDPL();
+
+ // First values after startup can be read as zero. Scrap a couple to be sure.
+ for (scrap = 0; scrap < 20; scrap++)
+ {
+ mpu6050GetMotion6(&axi16, &ayi16, &azi16, &gxi16, &gyi16, &gzi16);
+ vTaskDelay(M2T(2));
+ }
+ // First measurement
+ gxf = gxi16 * gRange;
+ gyf = gyi16 * gRange;
+ gzf = gzi16 * gRange;
+ axf = axi16 * aRange;
+ ayf = ayi16 * aRange;
+ azf = azi16 * aRange;
+
+ // Enable self test
+ mpu6050SetGyroXSelfTest(true);
+ mpu6050SetGyroYSelfTest(true);
+ mpu6050SetGyroZSelfTest(true);
+ mpu6050SetAccelXSelfTest(true);
+ mpu6050SetAccelYSelfTest(true);
+ mpu6050SetAccelZSelfTest(true);
+
+ // Wait for self test to take effect
+ vTaskDelay(M2T(MPU6050_SELF_TEST_DELAY_MS));
+ // Take second measurement
+ mpu6050GetMotion6(&axi16, &ayi16, &azi16, &gxi16, &gyi16, &gzi16);
+ gxfTst = gxi16 * gRange;
+ gyfTst = gyi16 * gRange;
+ gzfTst = gzi16 * gRange;
+ axfTst = axi16 * aRange;
+ ayfTst = ayi16 * aRange;
+ azfTst = azi16 * aRange;
+
+ // Disable self test
+ mpu6050SetGyroXSelfTest(false);
+ mpu6050SetGyroYSelfTest(false);
+ mpu6050SetGyroZSelfTest(false);
+ mpu6050SetAccelXSelfTest(false);
+ mpu6050SetAccelYSelfTest(false);
+ mpu6050SetAccelZSelfTest(false);
+
+// // Read factory values
+// i2cdevReadByte(I2Cx, devAddr, 0x00, (uint8_t *)&gxfi8);
+// i2cdevReadByte(I2Cx, devAddr, 0x01, (uint8_t *)&gyfi8);
+// i2cdevReadByte(I2Cx, devAddr, 0x02, (uint8_t *)&gzfi8);
+// i2cdevReadByte(I2Cx, devAddr, 0x0D, (uint8_t *)&axfi8);
+// i2cdevReadByte(I2Cx, devAddr, 0x0E, (uint8_t *)&ayfi8);
+// i2cdevReadByte(I2Cx, devAddr, 0x0F, (uint8_t *)&azfi8);
+//
+// DEBUG_PRINT("gxf:%i, gyf:%i, gzf:%i, axf:%i, ayf:%i, azf:%i\n",
+// (int)gxfi8, (int)gyfi8, (int)gzfi8, (int)axfi8, (int)ayfi8, (int)azfi8);
+
+ // Calculate difference
+ gxfDiff = gxfTst - gxf;
+ gyfDiff = gyfTst - gyf;
+ gzfDiff = gzfTst - gzf;
+ axfDiff = axfTst - axf;
+ ayfDiff = ayfTst - ayf;
+ azfDiff = azfTst - azf;
+
+ // Check result
+ if (mpu6050EvaluateSelfTest(MPU6050_ST_GYRO_LOW, MPU6050_ST_GYRO_HIGH, gxfDiff, "gyro X") &&
+ mpu6050EvaluateSelfTest(-MPU6050_ST_GYRO_HIGH, -MPU6050_ST_GYRO_LOW, gyfDiff, "gyro Y") &&
+ mpu6050EvaluateSelfTest(MPU6050_ST_GYRO_LOW, MPU6050_ST_GYRO_HIGH, gzfDiff, "gyro Z") &&
+ mpu6050EvaluateSelfTest(MPU6050_ST_ACCEL_LOW, MPU6050_ST_ACCEL_HIGH, axfDiff, "acc X") &&
+ mpu6050EvaluateSelfTest(MPU6050_ST_ACCEL_LOW, MPU6050_ST_ACCEL_HIGH, ayfDiff, "acc Y") &&
+ mpu6050EvaluateSelfTest(MPU6050_ST_ACCEL_LOW, MPU6050_ST_ACCEL_HIGH, azfDiff, "acc Z"))
+ {
+ DEBUG_PRINT("Self test [OK].\n");
+ }
+ else
+ {
+ testStatus = false;
+ }
+
+ return testStatus;
}
/** Evaluate the values from a MPU6050 self test.
@@ -192,12 +190,12 @@ bool mpu6050SelfTest()
*/
bool mpu6050EvaluateSelfTest(float low, float high, float value, char* string)
{
- // if (value < low || value > high)
- // {
- // DEBUG_PRINT("Self test %s [FAIL]. low: %0.2f, high: %0.2f, measured: %0.2f\n",
- // string, (double)low, (double)high, (double)value);
- // return false;
- // }
+ if (value < low || value > high)
+ {
+ DEBUG_PRINT("Self test %s [FAIL]. low: %0.2f, high: %0.2f, measured: %0.2f\n",
+ string, (double)low, (double)high, (double)value);
+ return false;
+ }
return true;
}
@@ -211,7 +209,7 @@ bool mpu6050EvaluateSelfTest(float low, float high, float value, char* string)
*/
uint8_t mpu6050GetAuxVDDIOLevel()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer);
return buffer[0];
}
/** Set the auxiliary I2C supply voltage level.
@@ -222,7 +220,7 @@ uint8_t mpu6050GetAuxVDDIOLevel()
*/
void mpu6050SetAuxVDDIOLevel(uint8_t level)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level);
}
// SMPLRT_DIV register
@@ -250,7 +248,7 @@ void mpu6050SetAuxVDDIOLevel(uint8_t level)
*/
uint8_t mpu6050GetRate()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_SMPLRT_DIV, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_SMPLRT_DIV, buffer);
return buffer[0];
}
/** Set gyroscope sample rate divider.
@@ -260,7 +258,7 @@ uint8_t mpu6050GetRate()
*/
void mpu6050SetRate(uint8_t rate)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_SMPLRT_DIV, rate);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_SMPLRT_DIV, rate);
}
// CONFIG register
@@ -294,8 +292,8 @@ void mpu6050SetRate(uint8_t rate)
*/
uint8_t mpu6050GetExternalFrameSync()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT,
- // MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT,
+ MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer);
return buffer[0];
}
/** Set external FSYNC configuration.
@@ -305,8 +303,8 @@ uint8_t mpu6050GetExternalFrameSync()
*/
void mpu6050SetExternalFrameSync(uint8_t sync)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT,
- // MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT,
+ MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync);
}
/** Get digital low-pass filter configuration.
* The DLPF_CFG parameter sets the digital low pass filter configuration. It
@@ -338,8 +336,8 @@ void mpu6050SetExternalFrameSync(uint8_t sync)
*/
uint8_t mpu6050GetDLPFMode()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT,
- // MPU6050_CFG_DLPF_CFG_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT,
+ MPU6050_CFG_DLPF_CFG_LENGTH, buffer);
return buffer[0];
}
/** Set digital low-pass filter configuration.
@@ -352,8 +350,8 @@ uint8_t mpu6050GetDLPFMode()
*/
void mpu6050SetDLPFMode(uint8_t mode)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT,
- // MPU6050_CFG_DLPF_CFG_LENGTH, mode);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT,
+ MPU6050_CFG_DLPF_CFG_LENGTH, mode);
}
// GYRO_CONFIG register
@@ -377,8 +375,8 @@ void mpu6050SetDLPFMode(uint8_t mode)
*/
uint8_t mpu6050GetFullScaleGyroRangeId()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT,
- // MPU6050_GCONFIG_FS_SEL_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT,
+ MPU6050_GCONFIG_FS_SEL_LENGTH, buffer);
return buffer[0];
}
@@ -392,30 +390,28 @@ uint8_t mpu6050GetFullScaleGyroRangeId()
*/
float mpu6050GetFullScaleGyroDPL()
{
- float range = 1.0;
-
- // int32_t rangeId;
- // float range;
-
- // rangeId = mpu6050GetFullScaleGyroRangeId();
- // switch (rangeId)
- // {
- // case MPU6050_GYRO_FS_250:
- // range = MPU6050_DEG_PER_LSB_250;
- // break;
- // case MPU6050_GYRO_FS_500:
- // range = MPU6050_DEG_PER_LSB_500;
- // break;
- // case MPU6050_GYRO_FS_1000:
- // range = MPU6050_DEG_PER_LSB_1000;
- // break;
- // case MPU6050_GYRO_FS_2000:
- // range = MPU6050_DEG_PER_LSB_2000;
- // break;
- // default:
- // range = MPU6050_DEG_PER_LSB_1000;
- // break;
- // }
+ int32_t rangeId;
+ float range;
+
+ rangeId = mpu6050GetFullScaleGyroRangeId();
+ switch (rangeId)
+ {
+ case MPU6050_GYRO_FS_250:
+ range = MPU6050_DEG_PER_LSB_250;
+ break;
+ case MPU6050_GYRO_FS_500:
+ range = MPU6050_DEG_PER_LSB_500;
+ break;
+ case MPU6050_GYRO_FS_1000:
+ range = MPU6050_DEG_PER_LSB_1000;
+ break;
+ case MPU6050_GYRO_FS_2000:
+ range = MPU6050_DEG_PER_LSB_2000;
+ break;
+ default:
+ range = MPU6050_DEG_PER_LSB_1000;
+ break;
+ }
return range;
}
@@ -430,23 +426,23 @@ float mpu6050GetFullScaleGyroDPL()
*/
void mpu6050SetFullScaleGyroRange(uint8_t range)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT,
- // MPU6050_GCONFIG_FS_SEL_LENGTH, range);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT,
+ MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}
void mpu6050SetGyroXSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_XG_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_XG_ST_BIT, enabled);
}
void mpu6050SetGyroYSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_YG_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_YG_ST_BIT, enabled);
}
void mpu6050SetGyroZSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_ZG_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_ZG_ST_BIT, enabled);
}
// ACCEL_CONFIG register
@@ -457,7 +453,7 @@ void mpu6050SetGyroZSelfTest(bool enabled)
*/
bool mpu6050GetAccelXSelfTest()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer);
return buffer[0];
}
/** Get self-test enabled setting for accelerometer X axis.
@@ -466,7 +462,7 @@ bool mpu6050GetAccelXSelfTest()
*/
void mpu6050SetAccelXSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled);
}
/** Get self-test enabled value for accelerometer Y axis.
* @return Self-test enabled value
@@ -474,7 +470,7 @@ void mpu6050SetAccelXSelfTest(bool enabled)
*/
bool mpu6050GetAccelYSelfTest()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer);
return buffer[0];
}
/** Get self-test enabled value for accelerometer Y axis.
@@ -483,7 +479,7 @@ bool mpu6050GetAccelYSelfTest()
*/
void mpu6050SetAccelYSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled);
}
/** Get self-test enabled value for accelerometer Z axis.
* @return Self-test enabled value
@@ -491,7 +487,7 @@ void mpu6050SetAccelYSelfTest(bool enabled)
*/
bool mpu6050GetAccelZSelfTest()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer);
return buffer[0];
}
/** Set self-test enabled value for accelerometer Z axis.
@@ -500,7 +496,7 @@ bool mpu6050GetAccelZSelfTest()
*/
void mpu6050SetAccelZSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled);
}
/** Get full-scale accelerometer range.
* The FS_SEL parameter allows setting the full-scale range of the accelerometer
@@ -521,8 +517,8 @@ void mpu6050SetAccelZSelfTest(bool enabled)
*/
uint8_t mpu6050GetFullScaleAccelRangeId()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT,
- // MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT,
+ MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer);
return buffer[0];
}
@@ -536,30 +532,28 @@ uint8_t mpu6050GetFullScaleAccelRangeId()
*/
float mpu6050GetFullScaleAccelGPL()
{
- float range = 1.0;
-
- // int32_t rangeId;
- // float range;
-
- // rangeId = mpu6050GetFullScaleAccelRangeId();
- // switch (rangeId)
- // {
- // case MPU6050_ACCEL_FS_2:
- // range = MPU6050_G_PER_LSB_2;
- // break;
- // case MPU6050_ACCEL_FS_4:
- // range = MPU6050_G_PER_LSB_4;
- // break;
- // case MPU6050_ACCEL_FS_8:
- // range = MPU6050_G_PER_LSB_8;
- // break;
- // case MPU6050_ACCEL_FS_16:
- // range = MPU6050_G_PER_LSB_16;
- // break;
- // default:
- // range = MPU6050_DEG_PER_LSB_1000;
- // break;
- // }
+ int32_t rangeId;
+ float range;
+
+ rangeId = mpu6050GetFullScaleAccelRangeId();
+ switch (rangeId)
+ {
+ case MPU6050_ACCEL_FS_2:
+ range = MPU6050_G_PER_LSB_2;
+ break;
+ case MPU6050_ACCEL_FS_4:
+ range = MPU6050_G_PER_LSB_4;
+ break;
+ case MPU6050_ACCEL_FS_8:
+ range = MPU6050_G_PER_LSB_8;
+ break;
+ case MPU6050_ACCEL_FS_16:
+ range = MPU6050_G_PER_LSB_16;
+ break;
+ default:
+ range = MPU6050_DEG_PER_LSB_1000;
+ break;
+ }
return range;
}
@@ -570,7 +564,7 @@ float mpu6050GetFullScaleAccelGPL()
*/
void mpu6050SetFullScaleAccelRange(uint8_t range)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT,
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT,
MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}
/** Get the high-pass filter configuration.
@@ -610,8 +604,8 @@ void mpu6050SetFullScaleAccelRange(uint8_t range)
*/
uint8_t mpu6050GetDHPFMode()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT,
- // MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT,
+ MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer);
return buffer[0];
}
/** Set the high-pass filter configuration.
@@ -622,8 +616,8 @@ uint8_t mpu6050GetDHPFMode()
*/
void mpu6050SetDHPFMode(uint8_t bandwidth)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT,
- // MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT,
+ MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);
}
// FF_THR register
@@ -645,7 +639,7 @@ void mpu6050SetDHPFMode(uint8_t bandwidth)
*/
uint8_t mpu6050GetFreefallDetectionThreshold()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_FF_THR, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_FF_THR, buffer);
return buffer[0];
}
/** Get free-fall event acceleration threshold.
@@ -655,7 +649,7 @@ uint8_t mpu6050GetFreefallDetectionThreshold()
*/
void mpu6050SetFreefallDetectionThreshold(uint8_t threshold)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_FF_THR, threshold);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_FF_THR, threshold);
}
// FF_DUR register
@@ -679,7 +673,7 @@ void mpu6050SetFreefallDetectionThreshold(uint8_t threshold)
*/
uint8_t mpu6050GetFreefallDetectionDuration()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_FF_DUR, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_FF_DUR, buffer);
return buffer[0];
}
/** Get free-fall event duration threshold.
@@ -689,7 +683,7 @@ uint8_t mpu6050GetFreefallDetectionDuration()
*/
void mpu6050SetFreefallDetectionDuration(uint8_t duration)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_FF_DUR, duration);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_FF_DUR, duration);
}
// MOT_THR register
@@ -715,7 +709,7 @@ void mpu6050SetFreefallDetectionDuration(uint8_t duration)
*/
uint8_t mpu6050GetMotionDetectionThreshold()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_MOT_THR, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_MOT_THR, buffer);
return buffer[0];
}
/** Set free-fall event acceleration threshold.
@@ -725,7 +719,7 @@ uint8_t mpu6050GetMotionDetectionThreshold()
*/
void mpu6050SetMotionDetectionThreshold(uint8_t threshold)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_MOT_THR, threshold);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_MOT_THR, threshold);
}
// MOT_DUR register
@@ -747,7 +741,7 @@ void mpu6050SetMotionDetectionThreshold(uint8_t threshold)
*/
uint8_t mpu6050GetMotionDetectionDuration()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_MOT_DUR, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_MOT_DUR, buffer);
return buffer[0];
}
/** Set motion detection event duration threshold.
@@ -757,7 +751,7 @@ uint8_t mpu6050GetMotionDetectionDuration()
*/
void mpu6050SetMotionDetectionDuration(uint8_t duration)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_MOT_DUR, duration);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_MOT_DUR, duration);
}
// ZRMOT_THR register
@@ -789,7 +783,7 @@ void mpu6050SetMotionDetectionDuration(uint8_t duration)
*/
uint8_t mpu6050GetZeroMotionDetectionThreshold()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_ZRMOT_THR, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_ZRMOT_THR, buffer);
return buffer[0];
}
/** Set zero motion detection event acceleration threshold.
@@ -799,7 +793,7 @@ uint8_t mpu6050GetZeroMotionDetectionThreshold()
*/
void mpu6050SetZeroMotionDetectionThreshold(uint8_t threshold)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_ZRMOT_THR, threshold);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_ZRMOT_THR, threshold);
}
// ZRMOT_DUR register
@@ -822,7 +816,7 @@ void mpu6050SetZeroMotionDetectionThreshold(uint8_t threshold)
*/
uint8_t mpu6050GetZeroMotionDetectionDuration()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_ZRMOT_DUR, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_ZRMOT_DUR, buffer);
return buffer[0];
}
/** Set zero motion detection event duration threshold.
@@ -832,7 +826,7 @@ uint8_t mpu6050GetZeroMotionDetectionDuration()
*/
void mpu6050SetZeroMotionDetectionDuration(uint8_t duration)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_ZRMOT_DUR, duration);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_ZRMOT_DUR, duration);
}
// FIFO_EN register
@@ -845,7 +839,7 @@ void mpu6050SetZeroMotionDetectionDuration(uint8_t duration)
*/
bool mpu6050GetTempFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set temperature FIFO enabled value.
@@ -855,7 +849,7 @@ bool mpu6050GetTempFIFOEnabled()
*/
void mpu6050SetTempFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled);
}
/** Get gyroscope X-axis FIFO enabled value.
* When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and
@@ -865,7 +859,7 @@ void mpu6050SetTempFIFOEnabled(bool enabled)
*/
bool mpu6050GetXGyroFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set gyroscope X-axis FIFO enabled value.
@@ -875,7 +869,7 @@ bool mpu6050GetXGyroFIFOEnabled()
*/
void mpu6050SetXGyroFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled);
}
/** Get gyroscope Y-axis FIFO enabled value.
* When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and
@@ -885,7 +879,7 @@ void mpu6050SetXGyroFIFOEnabled(bool enabled)
*/
bool mpu6050GetYGyroFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set gyroscope Y-axis FIFO enabled value.
@@ -895,7 +889,7 @@ bool mpu6050GetYGyroFIFOEnabled()
*/
void mpu6050SetYGyroFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled);
}
/** Get gyroscope Z-axis FIFO enabled value.
* When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and
@@ -905,7 +899,7 @@ void mpu6050SetYGyroFIFOEnabled(bool enabled)
*/
bool mpu6050GetZGyroFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set gyroscope Z-axis FIFO enabled value.
@@ -915,7 +909,7 @@ bool mpu6050GetZGyroFIFOEnabled()
*/
void mpu6050SetZGyroFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled);
}
/** Get accelerometer FIFO enabled value.
* When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H,
@@ -926,7 +920,7 @@ void mpu6050SetZGyroFIFOEnabled(bool enabled)
*/
bool mpu6050GetAccelFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set accelerometer FIFO enabled value.
@@ -946,7 +940,7 @@ void mpu6050SetAccelFIFOEnabled(bool enabled)
*/
bool mpu6050GetSlave2FIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set Slave 2 FIFO enabled value.
@@ -966,7 +960,7 @@ void mpu6050SetSlave2FIFOEnabled(bool enabled)
*/
bool mpu6050GetSlave1FIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set Slave 1 FIFO enabled value.
@@ -976,7 +970,7 @@ bool mpu6050GetSlave1FIFOEnabled()
*/
void mpu6050SetSlave1FIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled);
}
/** Get Slave 0 FIFO enabled value.
* When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -986,7 +980,7 @@ void mpu6050SetSlave1FIFOEnabled(bool enabled)
*/
bool mpu6050GetSlave0FIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set Slave 0 FIFO enabled value.
@@ -996,7 +990,7 @@ bool mpu6050GetSlave0FIFOEnabled()
*/
void mpu6050SetSlave0FIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled);
}
// I2C_MST_CTRL register
@@ -1018,7 +1012,7 @@ void mpu6050SetSlave0FIFOEnabled(bool enabled)
*/
bool mpu6050GetMultiMasterEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer);
return buffer[0];
}
/** Set multi-master enabled value.
@@ -1028,7 +1022,7 @@ bool mpu6050GetMultiMasterEnabled()
*/
void mpu6050SetMultiMasterEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled);
}
/** Get wait-for-external-sensor-data enabled value.
* When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be
@@ -1043,7 +1037,7 @@ void mpu6050SetMultiMasterEnabled(bool enabled)
*/
bool mpu6050GetWaitForExternalSensorEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer);
return buffer[0];
}
/** Set wait-for-external-sensor-data enabled value.
@@ -1053,7 +1047,7 @@ bool mpu6050GetWaitForExternalSensorEnabled()
*/
void mpu6050SetWaitForExternalSensorEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled);
}
/** Get Slave 3 FIFO enabled value.
* When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -1063,7 +1057,7 @@ void mpu6050SetWaitForExternalSensorEnabled(bool enabled)
*/
bool mpu6050GetSlave3FIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set Slave 3 FIFO enabled value.
@@ -1073,7 +1067,7 @@ bool mpu6050GetSlave3FIFOEnabled()
*/
void mpu6050SetSlave3FIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled);
}
/** Get slave read/write transition enabled value.
* The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave
@@ -1087,7 +1081,7 @@ void mpu6050SetSlave3FIFOEnabled(bool enabled)
*/
bool mpu6050GetSlaveReadWriteTransitionEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer);
return buffer[0];
}
/** Set slave read/write transition enabled value.
@@ -1130,8 +1124,8 @@ void mpu6050SetSlaveReadWriteTransitionEnabled(bool enabled)
*/
uint8_t mpu6050GetMasterClockSpeed()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT,
- // MPU6050_I2C_MST_CLK_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT,
+ MPU6050_I2C_MST_CLK_LENGTH, buffer);
return buffer[0];
}
/** Set I2C master clock speed.
@@ -1140,8 +1134,8 @@ uint8_t mpu6050GetMasterClockSpeed()
*/
void mpu6050SetMasterClockSpeed(uint8_t speed)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT,
- // MPU6050_I2C_MST_CLK_LENGTH, speed);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT,
+ MPU6050_I2C_MST_CLK_LENGTH, speed);
}
// I2C_SLV* registers (Slave 0-3)
@@ -1189,9 +1183,9 @@ void mpu6050SetMasterClockSpeed(uint8_t speed)
*/
uint8_t mpu6050GetSlaveAddress(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3, buffer);
+ if (num > 3)
+ return 0;
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3, buffer);
return buffer[0];
}
/** Set the I2C address of the specified slave (0-3).
@@ -1202,9 +1196,9 @@ uint8_t mpu6050GetSlaveAddress(uint8_t num)
*/
void mpu6050SetSlaveAddress(uint8_t num, uint8_t address)
{
- // if (num > 3)
- // return;
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3, address);
+ if (num > 3)
+ return;
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3, address);
}
/** Get the active internal register for the specified slave (0-3).
* Read/write operations for this slave will be done to whatever internal
@@ -1219,9 +1213,9 @@ void mpu6050SetSlaveAddress(uint8_t num, uint8_t address)
*/
uint8_t mpu6050GetSlaveRegister(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3, buffer);
+ if (num > 3)
+ return 0;
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3, buffer);
return buffer[0];
}
/** Set the active internal register for the specified slave (0-3).
@@ -1232,9 +1226,9 @@ uint8_t mpu6050GetSlaveRegister(uint8_t num)
*/
void mpu6050SetSlaveRegister(uint8_t num, uint8_t reg)
{
- // if (num > 3)
- // return;
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3, reg);
+ if (num > 3)
+ return;
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3, reg);
}
/** Get the enabled value for the specified slave (0-3).
* When set to 1, this bit enables Slave 0 for data transfer operations. When
@@ -1245,11 +1239,10 @@ void mpu6050SetSlaveRegister(uint8_t num, uint8_t reg)
*/
bool mpu6050GetSlaveEnabled(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, buffer);
- // return buffer[0];
- return true;
+ if (num > 3)
+ return 0;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, buffer);
+ return buffer[0];
}
/** Set the enabled value for the specified slave (0-3).
* @param num Slave number (0-3)
@@ -1259,10 +1252,10 @@ bool mpu6050GetSlaveEnabled(uint8_t num)
*/
void mpu6050SetSlaveEnabled(uint8_t num, bool enabled)
{
- // if (num > 3)
- // return;
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT,
- // enabled);
+ if (num > 3)
+ return;
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT,
+ enabled);
}
/** Get word pair byte-swapping enabled for the specified slave (0-3).
* When set to 1, this bit enables byte swapping. When byte swapping is enabled,
@@ -1277,12 +1270,11 @@ void mpu6050SetSlaveEnabled(uint8_t num, bool enabled)
*/
bool mpu6050GetSlaveWordByteSwap(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT,
- // buffer);
- // return buffer[0];
- return true;
+ if (num > 3)
+ return 0;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT,
+ buffer);
+ return buffer[0];
}
/** Set word pair byte-swapping enabled for the specified slave (0-3).
* @param num Slave number (0-3)
@@ -1292,10 +1284,10 @@ bool mpu6050GetSlaveWordByteSwap(uint8_t num)
*/
void mpu6050SetSlaveWordByteSwap(uint8_t num, bool enabled)
{
- // if (num > 3)
- // return;
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT,
- // enabled);
+ if (num > 3)
+ return;
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT,
+ enabled);
}
/** Get write mode for the specified slave (0-3).
* When set to 1, the transaction will read or write data only. When cleared to
@@ -1309,12 +1301,11 @@ void mpu6050SetSlaveWordByteSwap(uint8_t num, bool enabled)
*/
bool mpu6050GetSlaveWriteMode(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT,
- // buffer);
- // return buffer[0];
- return true;
+ if (num > 3)
+ return 0;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT,
+ buffer);
+ return buffer[0];
}
/** Set write mode for the specified slave (0-3).
* @param num Slave number (0-3)
@@ -1324,11 +1315,10 @@ bool mpu6050GetSlaveWriteMode(uint8_t num)
*/
void mpu6050SetSlaveWriteMode(uint8_t num, bool mode)
{
- // if (num > 3)
- // return;
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT,
- // mode);
-
+ if (num > 3)
+ return;
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT,
+ mode);
}
/** Get word pair grouping order offset for the specified slave (0-3).
* This sets specifies the grouping order of word pairs received from registers.
@@ -1343,11 +1333,10 @@ void mpu6050SetSlaveWriteMode(uint8_t num, bool mode)
*/
bool mpu6050GetSlaveWordGroupOffset(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, buffer);
- // return buffer[0];
- return true;
+ if (num > 3)
+ return 0;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, buffer);
+ return buffer[0];
}
/** Set word pair grouping order offset for the specified slave (0-3).
* @param num Slave number (0-3)
@@ -1357,10 +1346,10 @@ bool mpu6050GetSlaveWordGroupOffset(uint8_t num)
*/
void mpu6050SetSlaveWordGroupOffset(uint8_t num, bool enabled)
{
- // if (num > 3)
- // return;
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT,
- // enabled);
+ if (num > 3)
+ return;
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT,
+ enabled);
}
/** Get number of bytes to read for the specified slave (0-3).
* Specifies the number of bytes transferred to and from Slave 0. Clearing this
@@ -1371,12 +1360,11 @@ void mpu6050SetSlaveWordGroupOffset(uint8_t num, bool enabled)
*/
uint8_t mpu6050GetSlaveDataLength(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT,
- // MPU6050_I2C_SLV_LEN_LENGTH, buffer);
- // return buffer[0];
- return 1;
+ if (num > 3)
+ return 0;
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT,
+ MPU6050_I2C_SLV_LEN_LENGTH, buffer);
+ return buffer[0];
}
/** Set number of bytes to read for the specified slave (0-3).
* @param num Slave number (0-3)
@@ -1386,10 +1374,10 @@ uint8_t mpu6050GetSlaveDataLength(uint8_t num)
*/
void mpu6050SetSlaveDataLength(uint8_t num, uint8_t length)
{
- // if (num > 3)
- // return;
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT,
- // MPU6050_I2C_SLV_LEN_LENGTH, length);
+ if (num > 3)
+ return;
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT,
+ MPU6050_I2C_SLV_LEN_LENGTH, length);
}
// I2C_SLV* registers (Slave 4)
@@ -1405,10 +1393,8 @@ void mpu6050SetSlaveDataLength(uint8_t num, uint8_t length)
*/
uint8_t mpu6050GetSlave4Address()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer);
- // return buffer[0];
-
- return 1;
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer);
+ return buffer[0];
}
/** Set the I2C address of Slave 4.
* @param address New address for Slave 4
@@ -1438,7 +1424,7 @@ uint8_t mpu6050GetSlave4Register()
*/
void mpu6050SetSlave4Register(uint8_t reg)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_REG, reg);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_REG, reg);
}
/** Set new byte to write to Slave 4.
* This register stores the data to be written into the Slave 4. If I2C_SLV4_RW
@@ -1448,7 +1434,7 @@ void mpu6050SetSlave4Register(uint8_t reg)
*/
void mpu6050SetSlave4OutputByte(uint8_t data)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_DO, data);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_DO, data);
}
/** Get the enabled value for the Slave 4.
* When set to 1, this bit enables Slave 4 for data transfer operations. When
@@ -1458,9 +1444,8 @@ void mpu6050SetSlave4OutputByte(uint8_t data)
*/
bool mpu6050GetSlave4Enabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer);
+ return buffer[0];
}
/** Set the enabled value for Slave 4.
* @param enabled New enabled value for Slave 4
@@ -1469,7 +1454,7 @@ bool mpu6050GetSlave4Enabled()
*/
void mpu6050SetSlave4Enabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled);
}
/** Get the enabled value for Slave 4 transaction interrupts.
* When set to 1, this bit enables the generation of an interrupt signal upon
@@ -1482,9 +1467,8 @@ void mpu6050SetSlave4Enabled(bool enabled)
*/
bool mpu6050GetSlave4InterruptEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer);
+ return buffer[0];
}
/** Set the enabled value for Slave 4 transaction interrupts.
* @param enabled New enabled value for Slave 4 transaction interrupts.
@@ -1493,7 +1477,7 @@ bool mpu6050GetSlave4InterruptEnabled()
*/
void mpu6050SetSlave4InterruptEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled);
}
/** Get write mode for Slave 4.
* When set to 1, the transaction will read or write data only. When cleared to
@@ -1506,9 +1490,8 @@ void mpu6050SetSlave4InterruptEnabled(bool enabled)
*/
bool mpu6050GetSlave4WriteMode()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer);
+ return buffer[0];
}
/** Set write mode for the Slave 4.
* @param mode New write mode for Slave 4 (0 = register address + data, 1 = data only)
@@ -1517,7 +1500,7 @@ bool mpu6050GetSlave4WriteMode()
*/
void mpu6050SetSlave4WriteMode(bool mode)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode);
}
/** Get Slave 4 master delay value.
* This configures the reduced access rate of I2C slaves relative to the Sample
@@ -1536,10 +1519,9 @@ void mpu6050SetSlave4WriteMode(bool mode)
*/
uint8_t mpu6050GetSlave4MasterDelay()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT,
- // MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer);
- // return buffer[0];
- return 1;
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT,
+ MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer);
+ return buffer[0];
}
/** Set Slave 4 master delay value.
* @param delay New Slave 4 master delay value
@@ -1548,8 +1530,8 @@ uint8_t mpu6050GetSlave4MasterDelay()
*/
void mpu6050SetSlave4MasterDelay(uint8_t delay)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT,
- // MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT,
+ MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay);
}
/** Get last available byte read from Slave 4.
* This register stores the data read from Slave 4. This field is populated
@@ -1559,9 +1541,8 @@ void mpu6050SetSlave4MasterDelay(uint8_t delay)
*/
uint8_t mpu6050GetSlate4InputByte()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_DI, buffer);
- // return buffer[0];
- return 1;
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV4_DI, buffer);
+ return buffer[0];
}
// I2C_MST_STATUS register
@@ -1577,9 +1558,8 @@ uint8_t mpu6050GetSlate4InputByte()
*/
bool mpu6050GetPassthroughStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer);
+ return buffer[0];
}
/** Get Slave 4 transaction done status.
* Automatically sets to 1 when a Slave 4 transaction has completed. This
@@ -1591,9 +1571,8 @@ bool mpu6050GetPassthroughStatus()
*/
bool mpu6050GetSlave4IsDone()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer);
- // return buffer[0];
- return 1;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer);
+ return buffer[0];
}
/** Get master arbitration lost status.
* This bit automatically sets to 1 when the I2C Master has lost arbitration of
@@ -1604,9 +1583,8 @@ bool mpu6050GetSlave4IsDone()
*/
bool mpu6050GetLostArbitration()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer);
- // return buffer[0];
- return 1;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer);
+ return buffer[0];
}
/** Get Slave 4 NACK status.
* This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1617,9 +1595,8 @@ bool mpu6050GetLostArbitration()
*/
bool mpu6050GetSlave4Nack()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer);
+ return buffer[0];
}
/** Get Slave 3 NACK status.
* This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1630,9 +1607,8 @@ bool mpu6050GetSlave4Nack()
*/
bool mpu6050GetSlave3Nack()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer);
+ return buffer[0];
}
/** Get Slave 2 NACK status.
* This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1643,9 +1619,8 @@ bool mpu6050GetSlave3Nack()
*/
bool mpu6050GetSlave2Nack()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer);
+ return buffer[0];
}
/** Get Slave 1 NACK status.
* This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1656,9 +1631,8 @@ bool mpu6050GetSlave2Nack()
*/
bool mpu6050GetSlave1Nack()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer);
+ return buffer[0];
}
/** Get Slave 0 NACK status.
* This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1669,9 +1643,8 @@ bool mpu6050GetSlave1Nack()
*/
bool mpu6050GetSlave0Nack()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer);
+ return buffer[0];
}
// INT_PIN_CFG register
@@ -1684,9 +1657,8 @@ bool mpu6050GetSlave0Nack()
*/
bool mpu6050GetInterruptMode()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer);
+ return buffer[0];
}
/** Set interrupt logic level mode.
* @param mode New interrupt mode (0=active-high, 1=active-low)
@@ -1696,7 +1668,7 @@ bool mpu6050GetInterruptMode()
*/
void mpu6050SetInterruptMode(bool mode)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode);
}
/** Get interrupt drive mode.
* Will be set 0 for push-pull, 1 for open-drain.
@@ -1706,9 +1678,8 @@ void mpu6050SetInterruptMode(bool mode)
*/
bool mpu6050GetInterruptDrive()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer);
+ return buffer[0];
}
/** Set interrupt drive mode.
* @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
@@ -1718,7 +1689,7 @@ bool mpu6050GetInterruptDrive()
*/
void mpu6050SetInterruptDrive(bool drive)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive);
}
/** Get interrupt latch mode.
* Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
@@ -1728,9 +1699,8 @@ void mpu6050SetInterruptDrive(bool drive)
*/
bool mpu6050GetInterruptLatch()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer);
+ return buffer[0];
}
/** Set interrupt latch mode.
* @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
@@ -1740,7 +1710,7 @@ bool mpu6050GetInterruptLatch()
*/
void mpu6050SetInterruptLatch(bool latch)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch);
}
/** Get interrupt latch clear mode.
* Will be set 0 for status-read-only, 1 for any-register-read.
@@ -1750,9 +1720,8 @@ void mpu6050SetInterruptLatch(bool latch)
*/
bool mpu6050GetInterruptLatchClear()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer);
+ return buffer[0];
}
/** Set interrupt latch clear mode.
* @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
@@ -1762,7 +1731,7 @@ bool mpu6050GetInterruptLatchClear()
*/
void mpu6050SetInterruptLatchClear(bool clear)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
}
/** Get FSYNC interrupt logic level mode.
* @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
@@ -1772,9 +1741,8 @@ void mpu6050SetInterruptLatchClear(bool clear)
*/
bool mpu6050GetFSyncInterruptLevel()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer);
+ return buffer[0];
}
/** Set FSYNC interrupt logic level mode.
* @param mode New FSYNC interrupt mode (0=active-high, 1=active-low)
@@ -1784,7 +1752,7 @@ bool mpu6050GetFSyncInterruptLevel()
*/
void mpu6050SetFSyncInterruptLevel(bool level)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level);
}
/** Get FSYNC pin interrupt enabled setting.
* Will be set 0 for disabled, 1 for enabled.
@@ -1794,9 +1762,8 @@ void mpu6050SetFSyncInterruptLevel(bool level)
*/
bool mpu6050GetFSyncInterruptEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer);
+ return buffer[0];
}
/** Set FSYNC pin interrupt enabled setting.
* @param enabled New FSYNC pin interrupt enabled setting
@@ -1806,7 +1773,7 @@ bool mpu6050GetFSyncInterruptEnabled()
*/
void mpu6050SetFSyncInterruptEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled);
}
/** Get I2C bypass enabled status.
* When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
@@ -1821,9 +1788,8 @@ void mpu6050SetFSyncInterruptEnabled(bool enabled)
*/
bool mpu6050GetI2CBypassEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer);
+ return buffer[0];
}
/** Set I2C bypass enabled status.
* When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
@@ -1838,7 +1804,7 @@ bool mpu6050GetI2CBypassEnabled()
*/
void mpu6050SetI2CBypassEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}
/** Get reference clock output enabled status.
* When this bit is equal to 1, a reference clock output is provided at the
@@ -1851,9 +1817,8 @@ void mpu6050SetI2CBypassEnabled(bool enabled)
*/
bool mpu6050GetClockOutputEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer);
+ return buffer[0];
}
/** Set reference clock output enabled status.
* When this bit is equal to 1, a reference clock output is provided at the
@@ -1866,7 +1831,7 @@ bool mpu6050GetClockOutputEnabled()
*/
void mpu6050SetClockOutputEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
}
// INT_ENABLE register
@@ -1880,9 +1845,8 @@ void mpu6050SetClockOutputEnabled(bool enabled)
**/
uint8_t mpu6050GetIntEnabled()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, buffer);
- // return buffer[0];
- return 1;
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, buffer);
+ return buffer[0];
}
/** Set full interrupt enabled status.
* Full register byte for all interrupts, for quick reading. Each bit should be
@@ -1894,7 +1858,7 @@ uint8_t mpu6050GetIntEnabled()
**/
void mpu6050SetIntEnabled(uint8_t enabled)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, enabled);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, enabled);
}
/** Get Free Fall interrupt enabled status.
* Will be set 0 for disabled, 1 for enabled.
@@ -1904,9 +1868,8 @@ void mpu6050SetIntEnabled(uint8_t enabled)
**/
bool mpu6050GetIntFreefallEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer);
- // return buffer[0];
- return true;
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer);
+ return buffer[0];
}
/** Set Free Fall interrupt enabled status.
* @param enabled New interrupt enabled status
@@ -1926,7 +1889,7 @@ void mpu6050SetIntFreefallEnabled(bool enabled)
**/
bool mpu6050GetIntMotionEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer);
return buffer[0];
}
/** Set Motion Detection interrupt enabled status.
@@ -1937,7 +1900,7 @@ bool mpu6050GetIntMotionEnabled()
**/
void mpu6050SetIntMotionEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled);
}
/** Get Zero Motion Detection interrupt enabled status.
* Will be set 0 for disabled, 1 for enabled.
@@ -1947,7 +1910,7 @@ void mpu6050SetIntMotionEnabled(bool enabled)
**/
bool mpu6050GetIntZeroMotionEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
return buffer[0];
}
/** Set Zero Motion Detection interrupt enabled status.
@@ -1958,7 +1921,7 @@ bool mpu6050GetIntZeroMotionEnabled()
**/
void mpu6050SetIntZeroMotionEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled);
}
/** Get FIFO Buffer Overflow interrupt enabled status.
* Will be set 0 for disabled, 1 for enabled.
@@ -1968,7 +1931,7 @@ void mpu6050SetIntZeroMotionEnabled(bool enabled)
**/
bool mpu6050GetIntFIFOBufferOverflowEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
return buffer[0];
}
/** Set FIFO Buffer Overflow interrupt enabled status.
@@ -1979,7 +1942,7 @@ bool mpu6050GetIntFIFOBufferOverflowEnabled()
**/
void mpu6050SetIntFIFOBufferOverflowEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled);
}
/** Get I2C Master interrupt enabled status.
* This enables any of the I2C Master interrupt sources to generate an
@@ -1990,7 +1953,7 @@ void mpu6050SetIntFIFOBufferOverflowEnabled(bool enabled)
**/
bool mpu6050GetIntI2CMasterEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
return buffer[0];
}
/** Set I2C Master interrupt enabled status.
@@ -2001,7 +1964,7 @@ bool mpu6050GetIntI2CMasterEnabled()
**/
void mpu6050SetIntI2CMasterEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled);
}
/** Get Data Ready interrupt enabled setting.
* This event occurs each time a write operation to all of the sensor registers
@@ -2012,7 +1975,7 @@ void mpu6050SetIntI2CMasterEnabled(bool enabled)
*/
bool mpu6050GetIntDataReadyEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
return buffer[0];
}
/** Set Data Ready interrupt enabled status.
@@ -2023,7 +1986,7 @@ bool mpu6050GetIntDataReadyEnabled()
*/
void mpu6050SetIntDataReadyEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled);
}
// INT_STATUS register
@@ -2037,7 +2000,7 @@ void mpu6050SetIntDataReadyEnabled(bool enabled)
*/
uint8_t mpu6050GetIntStatus()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_INT_STATUS, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_INT_STATUS, buffer);
return buffer[0];
}
/** Get Free Fall interrupt status.
@@ -2049,7 +2012,7 @@ uint8_t mpu6050GetIntStatus()
*/
bool mpu6050GetIntFreefallStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer);
return buffer[0];
}
/** Get Motion Detection interrupt status.
@@ -2061,7 +2024,7 @@ bool mpu6050GetIntFreefallStatus()
*/
bool mpu6050GetIntMotionStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer);
return buffer[0];
}
/** Get Zero Motion Detection interrupt status.
@@ -2073,7 +2036,7 @@ bool mpu6050GetIntMotionStatus()
*/
bool mpu6050GetIntZeroMotionStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
return buffer[0];
}
/** Get FIFO Buffer Overflow interrupt status.
@@ -2085,7 +2048,7 @@ bool mpu6050GetIntZeroMotionStatus()
*/
bool mpu6050GetIntFIFOBufferOverflowStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
return buffer[0];
}
/** Get I2C Master interrupt status.
@@ -2098,7 +2061,7 @@ bool mpu6050GetIntFIFOBufferOverflowStatus()
*/
bool mpu6050GetIntI2CMasterStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
return buffer[0];
}
/** Get Data Ready interrupt status.
@@ -2110,7 +2073,7 @@ bool mpu6050GetIntI2CMasterStatus()
*/
bool mpu6050GetIntDataReadyStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
return buffer[0];
}
@@ -2135,7 +2098,7 @@ bool mpu6050GetIntDataReadyStatus()
void mpu6050GetMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz,
int16_t* mx, int16_t* my, int16_t* mz)
{
- // mpu6050GetMotion6(ax, ay, az, gx, gy, gz);
+ mpu6050GetMotion6(ax, ay, az, gx, gy, gz);
// TODO: magnetometer integration
}
/** Get raw 6-axis motion sensor readings (accel/gyro).
@@ -2152,13 +2115,13 @@ void mpu6050GetMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16
*/
void mpu6050GetMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
- // *ax = (((int16_t) buffer[0]) << 8) | buffer[1];
- // *ay = (((int16_t) buffer[2]) << 8) | buffer[3];
- // *az = (((int16_t) buffer[4]) << 8) | buffer[5];
- // *gx = (((int16_t) buffer[8]) << 8) | buffer[9];
- // *gy = (((int16_t) buffer[10]) << 8) | buffer[11];
- // *gz = (((int16_t) buffer[12]) << 8) | buffer[13];
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
+ *ax = (((int16_t) buffer[0]) << 8) | buffer[1];
+ *ay = (((int16_t) buffer[2]) << 8) | buffer[3];
+ *az = (((int16_t) buffer[4]) << 8) | buffer[5];
+ *gx = (((int16_t) buffer[8]) << 8) | buffer[9];
+ *gy = (((int16_t) buffer[10]) << 8) | buffer[11];
+ *gz = (((int16_t) buffer[12]) << 8) | buffer[13];
}
/** Get 3-axis accelerometer readings.
* These registers store the most recent accelerometer measurements.
@@ -2198,10 +2161,10 @@ void mpu6050GetMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16
*/
void mpu6050GetAcceleration(int16_t* x, int16_t* y, int16_t* z)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer);
- // *x = (((int16_t) buffer[0]) << 8) | buffer[1];
- // *y = (((int16_t) buffer[2]) << 8) | buffer[3];
- // *z = (((int16_t) buffer[4]) << 8) | buffer[5];
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer);
+ *x = (((int16_t) buffer[0]) << 8) | buffer[1];
+ *y = (((int16_t) buffer[2]) << 8) | buffer[3];
+ *z = (((int16_t) buffer[4]) << 8) | buffer[5];
}
/** Get X-axis accelerometer reading.
* @return X-axis acceleration measurement in 16-bit 2's complement format
@@ -2210,7 +2173,7 @@ void mpu6050GetAcceleration(int16_t* x, int16_t* y, int16_t* z)
*/
int16_t mpu6050GetAccelerationX()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
/** Get Y-axis accelerometer reading.
@@ -2220,7 +2183,7 @@ int16_t mpu6050GetAccelerationX()
*/
int16_t mpu6050GetAccelerationY()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
/** Get Z-axis accelerometer reading.
@@ -2230,7 +2193,7 @@ int16_t mpu6050GetAccelerationY()
*/
int16_t mpu6050GetAccelerationZ()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
@@ -2242,7 +2205,7 @@ int16_t mpu6050GetAccelerationZ()
*/
int16_t mpu6050GetTemperature()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
@@ -2282,10 +2245,10 @@ int16_t mpu6050GetTemperature()
*/
void mpu6050GetRotation(int16_t* x, int16_t* y, int16_t* z)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer);
- // *x = (((int16_t) buffer[0]) << 8) | buffer[1];
- // *y = (((int16_t) buffer[2]) << 8) | buffer[3];
- // *z = (((int16_t) buffer[4]) << 8) | buffer[5];
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer);
+ *x = (((int16_t) buffer[0]) << 8) | buffer[1];
+ *y = (((int16_t) buffer[2]) << 8) | buffer[3];
+ *z = (((int16_t) buffer[4]) << 8) | buffer[5];
}
/** Get X-axis gyroscope reading.
* @return X-axis rotation measurement in 16-bit 2's complement format
@@ -2294,7 +2257,7 @@ void mpu6050GetRotation(int16_t* x, int16_t* y, int16_t* z)
*/
int16_t mpu6050GetRotationX()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
/** Get Y-axis gyroscope reading.
@@ -2304,7 +2267,7 @@ int16_t mpu6050GetRotationX()
*/
int16_t mpu6050GetRotationY()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
/** Get Z-axis gyroscope reading.
@@ -2314,7 +2277,7 @@ int16_t mpu6050GetRotationY()
*/
int16_t mpu6050GetRotationZ()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
@@ -2396,7 +2359,7 @@ int16_t mpu6050GetRotationZ()
*/
uint8_t mpu6050GetExternalSensorByte(int position)
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer);
return buffer[0];
}
/** Read word (2 bytes) from external sensor data registers.
@@ -2406,7 +2369,7 @@ uint8_t mpu6050GetExternalSensorByte(int position)
*/
uint16_t mpu6050GetExternalSensorWord(int position)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer);
return (((uint16_t) buffer[0]) << 8) | buffer[1];
}
/** Read double word (4 bytes) from external sensor data registers.
@@ -2416,7 +2379,7 @@ uint16_t mpu6050GetExternalSensorWord(int position)
*/
uint32_t mpu6050GetExternalSensorDWord(int position)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer);
return (((uint32_t) buffer[0]) << 24) | (((uint32_t) buffer[1]) << 16)
| (((uint16_t) buffer[2]) << 8) | buffer[3];
}
@@ -2430,7 +2393,7 @@ uint32_t mpu6050GetExternalSensorDWord(int position)
*/
bool mpu6050GetXNegMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer);
return buffer[0];
}
/** Get X-axis positive motion detection interrupt status.
@@ -2440,7 +2403,7 @@ bool mpu6050GetXNegMotionDetected()
*/
bool mpu6050GetXPosMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer);
return buffer[0];
}
/** Get Y-axis negative motion detection interrupt status.
@@ -2450,7 +2413,7 @@ bool mpu6050GetXPosMotionDetected()
*/
bool mpu6050GetYNegMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer);
return buffer[0];
}
/** Get Y-axis positive motion detection interrupt status.
@@ -2460,7 +2423,7 @@ bool mpu6050GetYNegMotionDetected()
*/
bool mpu6050GetYPosMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer);
return buffer[0];
}
/** Get Z-axis negative motion detection interrupt status.
@@ -2470,7 +2433,7 @@ bool mpu6050GetYPosMotionDetected()
*/
bool mpu6050GetZNegMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer);
return buffer[0];
}
/** Get Z-axis positive motion detection interrupt status.
@@ -2480,7 +2443,7 @@ bool mpu6050GetZNegMotionDetected()
*/
bool mpu6050GetZPosMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer);
return buffer[0];
}
/** Get zero motion detection interrupt status.
@@ -2490,7 +2453,7 @@ bool mpu6050GetZPosMotionDetected()
*/
bool mpu6050GetZeroMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer);
return buffer[0];
}
@@ -2506,9 +2469,9 @@ bool mpu6050GetZeroMotionDetected()
*/
void mpu6050SetSlaveOutputByte(uint8_t num, uint8_t data)
{
- // if (num > 3)
- // return;
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_DO + num, data);
+ if (num > 3)
+ return;
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_I2C_SLV0_DO + num, data);
}
// I2C_MST_DELAY_CTRL register
@@ -2523,8 +2486,8 @@ void mpu6050SetSlaveOutputByte(uint8_t num, uint8_t data)
*/
bool mpu6050GetExternalShadowDelayEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT,
- // buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT,
+ buffer);
return buffer[0];
}
/** Set external data shadow delay enabled status.
@@ -2535,8 +2498,8 @@ bool mpu6050GetExternalShadowDelayEnabled()
*/
void mpu6050SetExternalShadowDelayEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL,
- // MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL,
+ MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled);
}
/** Get slave delay enabled status.
* When a particular slave delay is enabled, the rate of access for the that
@@ -2561,7 +2524,7 @@ bool mpu6050GetSlaveDelayEnabled(uint8_t num)
// MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
if (num > 4)
return 0;
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer);
return buffer[0];
}
/** Set slave delay enabled status.
@@ -2572,7 +2535,7 @@ bool mpu6050GetSlaveDelayEnabled(uint8_t num)
*/
void mpu6050SetSlaveDelayEnabled(uint8_t num, bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled);
}
// SIGNAL_PATH_RESET register
@@ -2585,7 +2548,7 @@ void mpu6050SetSlaveDelayEnabled(uint8_t num, bool enabled)
*/
void mpu6050ResetGyroscopePath()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, 1);
}
/** Reset accelerometer signal path.
* The reset will revert the signal path analog to digital converters and
@@ -2595,7 +2558,7 @@ void mpu6050ResetGyroscopePath()
*/
void mpu6050ResetAccelerometerPath()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, 1);
}
/** Reset temperature sensor signal path.
* The reset will revert the signal path analog to digital converters and
@@ -2605,7 +2568,7 @@ void mpu6050ResetAccelerometerPath()
*/
void mpu6050ResetTemperaturePath()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, 1);
}
// MOT_DETECT_CTRL register
@@ -2626,8 +2589,8 @@ void mpu6050ResetTemperaturePath()
*/
uint8_t mpu6050GetAccelerometerPowerOnDelay()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
- // MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
+ MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer);
return buffer[0];
}
/** Set accelerometer power-on delay.
@@ -2638,8 +2601,8 @@ uint8_t mpu6050GetAccelerometerPowerOnDelay()
*/
void mpu6050SetAccelerometerPowerOnDelay(uint8_t delay)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
- // MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
+ MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
}
/** Get Free Fall detection counter decrement configuration.
* Detection is registered by the Free Fall detection module after accelerometer
@@ -2669,8 +2632,8 @@ void mpu6050SetAccelerometerPowerOnDelay(uint8_t delay)
*/
uint8_t mpu6050GetFreefallDetectionCounterDecrement()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT,
- // MPU6050_DETECT_FF_COUNT_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT,
+ MPU6050_DETECT_FF_COUNT_LENGTH, buffer);
return buffer[0];
}
/** Set Free Fall detection counter decrement configuration.
@@ -2681,8 +2644,8 @@ uint8_t mpu6050GetFreefallDetectionCounterDecrement()
*/
void mpu6050SetFreefallDetectionCounterDecrement(uint8_t decrement)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT,
- // MPU6050_DETECT_FF_COUNT_LENGTH, decrement);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT,
+ MPU6050_DETECT_FF_COUNT_LENGTH, decrement);
}
/** Get Motion detection counter decrement configuration.
* Detection is registered by the Motion detection module after accelerometer
@@ -2709,8 +2672,8 @@ void mpu6050SetFreefallDetectionCounterDecrement(uint8_t decrement)
*/
uint8_t mpu6050GetMotionDetectionCounterDecrement()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT,
- // MPU6050_DETECT_MOT_COUNT_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT,
+ MPU6050_DETECT_MOT_COUNT_LENGTH, buffer);
return buffer[0];
}
/** Set Motion detection counter decrement configuration.
@@ -2721,8 +2684,8 @@ uint8_t mpu6050GetMotionDetectionCounterDecrement()
*/
void mpu6050SetMotionDetectionCounterDecrement(uint8_t decrement)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT,
- // MPU6050_DETECT_MOT_COUNT_LENGTH, decrement);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT,
+ MPU6050_DETECT_MOT_COUNT_LENGTH, decrement);
}
// USER_CTRL register
@@ -2737,7 +2700,7 @@ void mpu6050SetMotionDetectionCounterDecrement(uint8_t decrement)
*/
bool mpu6050GetFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set FIFO enabled status.
@@ -2748,7 +2711,7 @@ bool mpu6050GetFIFOEnabled()
*/
void mpu6050SetFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled);
}
/** Get I2C Master Mode enabled status.
* When this mode is enabled, the MPU-60X0 acts as the I2C Master to the
@@ -2763,7 +2726,7 @@ void mpu6050SetFIFOEnabled(bool enabled)
*/
bool mpu6050GetI2CMasterModeEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer);
return buffer[0];
}
/** Set I2C Master Mode enabled status.
@@ -2774,7 +2737,7 @@ bool mpu6050GetI2CMasterModeEnabled()
*/
void mpu6050SetI2CMasterModeEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}
/** Switch from I2C to SPI mode (MPU-6000 only)
* If this is set, the primary SPI interface will be enabled in place of the
@@ -2782,7 +2745,7 @@ void mpu6050SetI2CMasterModeEnabled(bool enabled)
*/
void mpu6050SwitchSPIEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
}
/** Reset the FIFO.
* This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
@@ -2792,7 +2755,7 @@ void mpu6050SwitchSPIEnabled(bool enabled)
*/
void mpu6050ResetFIFO()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1);
}
/** Reset the I2C Master.
* This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0.
@@ -2802,7 +2765,7 @@ void mpu6050ResetFIFO()
*/
void mpu6050ResetI2CMaster()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1);
}
/** Reset all sensor registers and signal paths.
* When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
@@ -2818,7 +2781,7 @@ void mpu6050ResetI2CMaster()
*/
void mpu6050ResetSensors()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, 1);
}
// PWR_MGMT_1 register
@@ -2830,7 +2793,7 @@ void mpu6050ResetSensors()
*/
void mpu6050Reset()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1);
}
/** Get sleep mode status.
* Setting the SLEEP bit in the register puts the device into very low power
@@ -2845,7 +2808,7 @@ void mpu6050Reset()
*/
bool mpu6050GetSleepEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer);
return buffer[0];
}
/** Set sleep mode status.
@@ -2856,7 +2819,7 @@ bool mpu6050GetSleepEnabled()
*/
void mpu6050SetSleepEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}
/** Get wake cycle enabled status.
* When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
@@ -2868,7 +2831,7 @@ void mpu6050SetSleepEnabled(bool enabled)
*/
bool mpu6050GetWakeCycleEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer);
return buffer[0];
}
/** Set wake cycle enabled status.
@@ -2879,7 +2842,7 @@ bool mpu6050GetWakeCycleEnabled()
*/
void mpu6050SetWakeCycleEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
}
/** Get temperature sensor enabled status.
* Control the usage of the internal temperature sensor.
@@ -2894,7 +2857,7 @@ void mpu6050SetWakeCycleEnabled(bool enabled)
*/
bool mpu6050GetTempSensorEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer);
return buffer[0] == 0; // 1 is actually disabled here
}
/** Set temperature sensor enabled status.
@@ -2910,7 +2873,7 @@ bool mpu6050GetTempSensorEnabled()
void mpu6050SetTempSensorEnabled(bool enabled)
{
// 1 is actually disabled here
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
}
/** Get clock source setting.
* @return Current clock source setting
@@ -2920,8 +2883,8 @@ void mpu6050SetTempSensorEnabled(bool enabled)
*/
uint8_t mpu6050GetClockSource()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT,
- // MPU6050_PWR1_CLKSEL_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT,
+ MPU6050_PWR1_CLKSEL_LENGTH, buffer);
return buffer[0];
}
/** Set clock source setting.
@@ -2956,8 +2919,8 @@ uint8_t mpu6050GetClockSource()
*/
void mpu6050SetClockSource(uint8_t source)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT,
- // MPU6050_PWR1_CLKSEL_LENGTH, source);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT,
+ MPU6050_PWR1_CLKSEL_LENGTH, source);
}
// PWR_MGMT_2 register
@@ -2987,8 +2950,8 @@ void mpu6050SetClockSource(uint8_t source)
*/
uint8_t mpu6050GetWakeFrequency()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT,
- // MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT,
+ MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer);
return buffer[0];
}
/** Set wake frequency in Accel-Only Low Power Mode.
@@ -2997,8 +2960,8 @@ uint8_t mpu6050GetWakeFrequency()
*/
void mpu6050SetWakeFrequency(uint8_t frequency)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT,
- // MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT,
+ MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
}
/** Get X-axis accelerometer standby enabled status.
@@ -3009,7 +2972,7 @@ void mpu6050SetWakeFrequency(uint8_t frequency)
*/
bool mpu6050GetStandbyXAccelEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer);
return buffer[0];
}
/** Set X-axis accelerometer standby enabled status.
@@ -3020,7 +2983,7 @@ bool mpu6050GetStandbyXAccelEnabled()
*/
void mpu6050SetStandbyXAccelEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled);
}
/** Get Y-axis accelerometer standby enabled status.
* If enabled, the Y-axis will not gather or report data (or use power).
@@ -3030,7 +2993,7 @@ void mpu6050SetStandbyXAccelEnabled(bool enabled)
*/
bool mpu6050GetStandbyYAccelEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer);
return buffer[0];
}
/** Set Y-axis accelerometer standby enabled status.
@@ -3041,7 +3004,7 @@ bool mpu6050GetStandbyYAccelEnabled()
*/
void mpu6050SetStandbyYAccelEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled);
}
/** Get Z-axis accelerometer standby enabled status.
* If enabled, the Z-axis will not gather or report data (or use power).
@@ -3051,7 +3014,7 @@ void mpu6050SetStandbyYAccelEnabled(bool enabled)
*/
bool mpu6050GetStandbyZAccelEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer);
return buffer[0];
}
/** Set Z-axis accelerometer standby enabled status.
@@ -3062,7 +3025,7 @@ bool mpu6050GetStandbyZAccelEnabled()
*/
void mpu6050SetStandbyZAccelEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled);
}
/** Get X-axis gyroscope standby enabled status.
* If enabled, the X-axis will not gather or report data (or use power).
@@ -3072,7 +3035,7 @@ void mpu6050SetStandbyZAccelEnabled(bool enabled)
*/
bool mpu6050GetStandbyXGyroEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer);
return buffer[0];
}
/** Set X-axis gyroscope standby enabled status.
@@ -3083,7 +3046,7 @@ bool mpu6050GetStandbyXGyroEnabled()
*/
void mpu6050SetStandbyXGyroEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled);
}
/** Get Y-axis gyroscope standby enabled status.
* If enabled, the Y-axis will not gather or report data (or use power).
@@ -3093,7 +3056,7 @@ void mpu6050SetStandbyXGyroEnabled(bool enabled)
*/
bool mpu6050GetStandbyYGyroEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer);
return buffer[0];
}
/** Set Y-axis gyroscope standby enabled status.
@@ -3104,7 +3067,7 @@ bool mpu6050GetStandbyYGyroEnabled()
*/
void mpu6050SetStandbyYGyroEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled);
}
/** Get Z-axis gyroscope standby enabled status.
* If enabled, the Z-axis will not gather or report data (or use power).
@@ -3114,7 +3077,7 @@ void mpu6050SetStandbyYGyroEnabled(bool enabled)
*/
bool mpu6050GetStandbyZGyroEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer);
return buffer[0];
}
/** Set Z-axis gyroscope standby enabled status.
@@ -3125,7 +3088,7 @@ bool mpu6050GetStandbyZGyroEnabled()
*/
void mpu6050SetStandbyZGyroEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
}
// FIFO_COUNT* registers
@@ -3139,7 +3102,7 @@ void mpu6050SetStandbyZGyroEnabled(bool enabled)
*/
uint16_t mpu6050GetFIFOCount()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
return (((uint16_t) buffer[0]) << 8) | buffer[1];
}
@@ -3172,12 +3135,12 @@ uint16_t mpu6050GetFIFOCount()
*/
uint8_t mpu6050GetFIFOByte()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_FIFO_R_W, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_FIFO_R_W, buffer);
return buffer[0];
}
void mpu6050GetFIFOBytes(uint8_t *data, uint8_t length)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_FIFO_R_W, length, data);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_FIFO_R_W, length, data);
}
/** Write byte to FIFO buffer.
* @see getFIFOByte()
@@ -3185,7 +3148,7 @@ void mpu6050GetFIFOBytes(uint8_t *data, uint8_t length)
*/
void mpu6050SetFIFOByte(uint8_t data)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_FIFO_R_W, data);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_FIFO_R_W, data);
}
// WHO_AM_I register
@@ -3199,8 +3162,8 @@ void mpu6050SetFIFOByte(uint8_t data)
*/
uint8_t mpu6050GetDeviceID()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH,
- // buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH,
+ buffer);
return buffer[0];
}
/** Set Device ID.
@@ -3214,8 +3177,8 @@ uint8_t mpu6050GetDeviceID()
*/
void mpu6050SetDeviceID(uint8_t id)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH,
- // id);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH,
+ id);
}
// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
@@ -3224,212 +3187,212 @@ void mpu6050SetDeviceID(uint8_t id)
uint8_t mpu6050GetOTPBankValid()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer);
return buffer[0];
}
void mpu6050SetOTPBankValid(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
}
int8_t mpu6050GetXGyroOffset()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
- // MPU6050_TC_OFFSET_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
+ MPU6050_TC_OFFSET_LENGTH, buffer);
return buffer[0];
}
void mpu6050SetXGyroOffset(int8_t offset)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
- // MPU6050_TC_OFFSET_LENGTH, offset);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
+ MPU6050_TC_OFFSET_LENGTH, offset);
}
// YG_OFFS_TC register
int8_t mpu6050GetYGyroOffset()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
- // MPU6050_TC_OFFSET_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
+ MPU6050_TC_OFFSET_LENGTH, buffer);
return buffer[0];
}
void mpu6050SetYGyroOffset(int8_t offset)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
- // MPU6050_TC_OFFSET_LENGTH, offset);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
+ MPU6050_TC_OFFSET_LENGTH, offset);
}
// ZG_OFFS_TC register
int8_t mpu6050GetZGyroOffset()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
- // MPU6050_TC_OFFSET_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
+ MPU6050_TC_OFFSET_LENGTH, buffer);
return buffer[0];
}
void mpu6050SetZGyroOffset(int8_t offset)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
- // MPU6050_TC_OFFSET_LENGTH, offset);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
+ MPU6050_TC_OFFSET_LENGTH, offset);
}
// X_FINE_GAIN register
int8_t mpu6050GetXFineGain()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_X_FINE_GAIN, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_X_FINE_GAIN, buffer);
return buffer[0];
}
void mpu6050SetXFineGain(int8_t gain)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_X_FINE_GAIN, gain);
}
// Y_FINE_GAIN register
int8_t mpu6050GetYFineGain()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_Y_FINE_GAIN, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_Y_FINE_GAIN, buffer);
return buffer[0];
}
void mpu6050SetYFineGain(int8_t gain)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
}
// Z_FINE_GAIN register
int8_t mpu6050GetZFineGain()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_Z_FINE_GAIN, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_Z_FINE_GAIN, buffer);
return buffer[0];
}
void mpu6050SetZFineGain(int8_t gain)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
}
// XA_OFFS_* registers
int16_t mpu6050GetXAccelOffset()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_XA_OFFS_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_XA_OFFS_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
void mpu6050SetXAccelOffset(int16_t offset)
{
- // i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_XA_OFFS_H, 2, (uint8_t *)&offset);
+ i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_XA_OFFS_H, 2, (uint8_t *)&offset);
}
// YA_OFFS_* register
int16_t mpu6050GetYAccelOffset()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_YA_OFFS_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_YA_OFFS_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
void mpu6050SetYAccelOffset(int16_t offset)
{
- // i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_YA_OFFS_H, 2, (uint8_t *)&offset);
+ i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_YA_OFFS_H, 2, (uint8_t *)&offset);
}
// ZA_OFFS_* register
int16_t mpu6050GetZAccelOffset()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ZA_OFFS_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ZA_OFFS_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
void mpu6050SetZAccelOffset(int16_t offset)
{
- // i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_ZA_OFFS_H, 2, (uint8_t *)&offset);
+ i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_ZA_OFFS_H, 2, (uint8_t *)&offset);
}
// XG_OFFS_USR* registers
int16_t mpu6050GetXGyroOffsetUser()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
void mpu6050SetXGyroOffsetUser(int16_t offset)
{
- // i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_XG_OFFS_USRH, 2, (uint8_t *)&offset);
+ i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_XG_OFFS_USRH, 2, (uint8_t *)&offset);
}
// YG_OFFS_USR* register
int16_t mpu6050GetYGyroOffsetUser()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
void mpu6050SetYGyroOffsetUser(int16_t offset)
{
- // i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_YG_OFFS_USRH, 2, (uint8_t *)&offset);
+ i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_YG_OFFS_USRH, 2, (uint8_t *)&offset);
}
// ZG_OFFS_USR* register
int16_t mpu6050GetZGyroOffsetUser()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
void mpu6050SetZGyroOffsetUser(int16_t offset)
{
- // i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, (uint8_t *)&offset);
+ i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, (uint8_t *)&offset);
}
// INT_ENABLE register (DMP functions)
bool mpu6050GetIntPLLReadyEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
return buffer[0];
}
void mpu6050SetIntPLLReadyEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
}
bool mpu6050GetIntDMPEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
return buffer[0];
}
void mpu6050SetIntDMPEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
}
// DMP_INT_STATUS
bool mpu6050GetDMPInt5Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer);
return buffer[0];
}
bool mpu6050GetDMPInt4Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer);
return buffer[0];
}
bool mpu6050GetDMPInt3Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer);
return buffer[0];
}
bool mpu6050GetDMPInt2Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer);
return buffer[0];
}
bool mpu6050GetDMPInt1Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer);
return buffer[0];
}
bool mpu6050GetDMPInt0Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer);
return buffer[0];
}
@@ -3437,12 +3400,12 @@ bool mpu6050GetDMPInt0Status()
bool mpu6050GetIntPLLReadyStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
return buffer[0];
}
bool mpu6050GetIntDMPStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
return buffer[0];
}
@@ -3450,165 +3413,165 @@ bool mpu6050GetIntDMPStatus()
bool mpu6050GetDMPEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer);
return buffer[0];
}
void mpu6050SetDMPEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
}
void mpu6050ResetDMP()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1);
}
// BANK_SEL register
void mpu6050SetMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank)
{
- // bank &= 0x1F;
- // if (userBank)
- // bank |= 0x20;
- // if (prefetchEnabled)
- // bank |= 0x40;
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_BANK_SEL, bank);
+ bank &= 0x1F;
+ if (userBank)
+ bank |= 0x20;
+ if (prefetchEnabled)
+ bank |= 0x40;
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_BANK_SEL, bank);
}
// MEM_START_ADDR register
void mpu6050SetMemoryStartAddress(uint8_t address)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_MEM_START_ADDR, address);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_MEM_START_ADDR, address);
}
// MEM_R_W register
uint8_t mpu6050ReadMemoryByte()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_MEM_R_W, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_MEM_R_W, buffer);
return buffer[0];
}
void mpu6050WriteMemoryByte(uint8_t data)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_MEM_R_W, data);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_MEM_R_W, data);
}
void mpu6050ReadMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address)
{
- // mpu6050SetMemoryBank(bank, true, true);
- // mpu6050SetMemoryStartAddress(address);
- // uint8_t chunkSize;
- // uint16_t i;
+ mpu6050SetMemoryBank(bank, true, true);
+ mpu6050SetMemoryStartAddress(address);
+ uint8_t chunkSize;
+ uint16_t i;
- // for (i = 0; i < dataSize;)
- // {
- // // determine correct chunk size according to bank position and data size
- // chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
+ for (i = 0; i < dataSize;)
+ {
+ // determine correct chunk size according to bank position and data size
+ chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
- // // make sure we don't go past the data size
- // if (i + chunkSize > dataSize)
- // chunkSize = dataSize - i;
+ // make sure we don't go past the data size
+ if (i + chunkSize > dataSize)
+ chunkSize = dataSize - i;
- // // make sure this chunk doesn't go past the bank boundary (256 bytes)
- // if (chunkSize > 256 - address)
- // chunkSize = 256 - address;
+ // make sure this chunk doesn't go past the bank boundary (256 bytes)
+ if (chunkSize > 256 - address)
+ chunkSize = 256 - address;
- // // read the chunk of data as specified
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i);
+ // read the chunk of data as specified
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i);
- // // increase byte index by [chunkSize]
- // i += chunkSize;
+ // increase byte index by [chunkSize]
+ i += chunkSize;
- // // uint8_t automatically wraps to 0 at 256
- // address += chunkSize;
+ // uint8_t automatically wraps to 0 at 256
+ address += chunkSize;
- // // if we aren't done, update bank (if necessary) and address
- // if (i < dataSize)
- // {
- // if (address == 0)
- // bank++;
- // mpu6050SetMemoryBank(bank, true, true);
- // mpu6050SetMemoryStartAddress(address);
- // }
- // }
+ // if we aren't done, update bank (if necessary) and address
+ if (i < dataSize)
+ {
+ if (address == 0)
+ bank++;
+ mpu6050SetMemoryBank(bank, true, true);
+ mpu6050SetMemoryStartAddress(address);
+ }
+ }
}
bool mpu6050WriteMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address,
bool verify)
{
- // static uint8_t verifyBuffer[MPU6050_DMP_MEMORY_CHUNK_SIZE];
- // uint8_t chunkSize;
- // uint8_t *progBuffer;
- // uint16_t i;
-
- // mpu6050SetMemoryBank(bank, true, true);
- // mpu6050SetMemoryStartAddress(address);
-
- // for (i = 0; i < dataSize;)
- // {
- // // determine correct chunk size according to bank position and data size
- // chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
-
- // // make sure we don't go past the data size
- // if (i + chunkSize > dataSize)
- // chunkSize = dataSize - i;
-
- // // make sure this chunk doesn't go past the bank boundary (256 bytes)
- // if (chunkSize > 256 - address)
- // chunkSize = 256 - address;
-
- // // write the chunk of data as specified
- // progBuffer = (uint8_t *) data + i;
-
- // i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
-
- // // verify data if needed
- // if (verify)
- // {
- // uint32_t j;
- // mpu6050SetMemoryBank(bank, true, true);
- // mpu6050SetMemoryStartAddress(address);
- // i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
-
- // for (j = 0; j < chunkSize; j++)
- // {
- // if (progBuffer[j] != verifyBuffer[j])
- // {
- // /*Serial.print("Block write verification error, bank ");
- // Serial.print(bank, DEC);
- // Serial.print(", address ");
- // Serial.print(address, DEC);
- // Serial.print("!\nExpected:");
- // for (j = 0; j < chunkSize; j++) {
- // Serial.print(" 0x");
- // if (progBuffer[j] < 16) Serial.print("0");
- // Serial.print(progBuffer[j], HEX);
- // }
- // Serial.print("\nReceived:");
- // for (uint8_t j = 0; j < chunkSize; j++) {
- // Serial.print(" 0x");
- // if (verifyBuffer[i + j] < 16) Serial.print("0");
- // Serial.print(verifyBuffer[i + j], HEX);
- // }
- // Serial.print("\n");*/
- // return false;
- // }
- // }
- // }
-
- // // increase byte index by [chunkSize]
- // i += chunkSize;
-
- // // uint8_t automatically wraps to 0 at 256
- // address += chunkSize;
-
- // // if we aren't done, update bank (if necessary) and address
- // if (i < dataSize)
- // {
- // if (address == 0)
- // bank++;
- // mpu6050SetMemoryBank(bank, true, true);
- // mpu6050SetMemoryStartAddress(address);
- // }
- // }
+ static uint8_t verifyBuffer[MPU6050_DMP_MEMORY_CHUNK_SIZE];
+ uint8_t chunkSize;
+ uint8_t *progBuffer;
+ uint16_t i;
+
+ mpu6050SetMemoryBank(bank, true, true);
+ mpu6050SetMemoryStartAddress(address);
+
+ for (i = 0; i < dataSize;)
+ {
+ // determine correct chunk size according to bank position and data size
+ chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
+
+ // make sure we don't go past the data size
+ if (i + chunkSize > dataSize)
+ chunkSize = dataSize - i;
+
+ // make sure this chunk doesn't go past the bank boundary (256 bytes)
+ if (chunkSize > 256 - address)
+ chunkSize = 256 - address;
+
+ // write the chunk of data as specified
+ progBuffer = (uint8_t *) data + i;
+
+ i2cdevWriteReg8(I2Cx, devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
+
+ // verify data if needed
+ if (verify)
+ {
+ uint32_t j;
+ mpu6050SetMemoryBank(bank, true, true);
+ mpu6050SetMemoryStartAddress(address);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
+
+ for (j = 0; j < chunkSize; j++)
+ {
+ if (progBuffer[j] != verifyBuffer[j])
+ {
+ /*Serial.print("Block write verification error, bank ");
+ Serial.print(bank, DEC);
+ Serial.print(", address ");
+ Serial.print(address, DEC);
+ Serial.print("!\nExpected:");
+ for (j = 0; j < chunkSize; j++) {
+ Serial.print(" 0x");
+ if (progBuffer[j] < 16) Serial.print("0");
+ Serial.print(progBuffer[j], HEX);
+ }
+ Serial.print("\nReceived:");
+ for (uint8_t j = 0; j < chunkSize; j++) {
+ Serial.print(" 0x");
+ if (verifyBuffer[i + j] < 16) Serial.print("0");
+ Serial.print(verifyBuffer[i + j], HEX);
+ }
+ Serial.print("\n");*/
+ return false;
+ }
+ }
+ }
+
+ // increase byte index by [chunkSize]
+ i += chunkSize;
+
+ // uint8_t automatically wraps to 0 at 256
+ address += chunkSize;
+
+ // if we aren't done, update bank (if necessary) and address
+ if (i < dataSize)
+ {
+ if (address == 0)
+ bank++;
+ mpu6050SetMemoryBank(bank, true, true);
+ mpu6050SetMemoryStartAddress(address);
+ }
+ }
return true;
}
bool mpu6050WriteProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank,
@@ -3619,66 +3582,66 @@ bool mpu6050WriteProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t
#define MPU6050_DMP_CONFIG_BLOCK_SIZE 6
bool mpu6050WriteDMPConfigurationSet(const uint8_t *data, uint16_t dataSize)
{
- // uint8_t *progBuffer, success, special;
- // uint16_t i;
-
- // // config set data is a long string of blocks with the following structure:
- // // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
- // uint8_t bank=0;
- // uint8_t offset=0;
- // uint8_t length=0;
- // for (i = 0; i < dataSize;)
- // {
- // bank = data[i++];
- // offset = data[i++];
- // length = data[i++];
- // }
-
- // // write data or perform special action
- // if (length > 0)
- // {
- // // regular block of data to write
- // /*Serial.print("Writing config block to bank ");
- // Serial.print(bank);
- // Serial.print(", offset ");
- // Serial.print(offset);
- // Serial.print(", length=");
- // Serial.println(length);*/
- // progBuffer = (uint8_t *) data + i;
- // success = mpu6050WriteMemoryBlock(progBuffer, length, bank, offset, true);
- // i += length;
- // }
- // else
- // {
- // // special instruction
- // // NOTE: this kind of behavior (what and when to do certain things)
- // // is totally undocumented. This code is in here based on observed
- // // behavior only, and exactly why (or even whether) it has to be here
- // // is anybody's guess for now.
- // special = data[i++];
- // /*Serial.print("Special command code ");
- // Serial.print(special, HEX);
- // Serial.println(" found...");*/
- // if (special == 0x01)
- // {
- // // enable DMP-related interrupts
- // mpu6050SetIntZeroMotionEnabled(true);
- // mpu6050SetIntFIFOBufferOverflowEnabled(true);
- // mpu6050SetIntDMPEnabled(true);
- // //i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, 0x32);
- // success = true;
- // }
- // else
- // {
- // // unknown special command
- // success = false;
- // }
- // }
-
- // if (!success)
- // {
- // return false; // uh oh
- // }
+ uint8_t *progBuffer, success, special;
+ uint16_t i;
+
+ // config set data is a long string of blocks with the following structure:
+ // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
+ uint8_t bank=0;
+ uint8_t offset=0;
+ uint8_t length=0;
+ for (i = 0; i < dataSize;)
+ {
+ bank = data[i++];
+ offset = data[i++];
+ length = data[i++];
+ }
+
+ // write data or perform special action
+ if (length > 0)
+ {
+ // regular block of data to write
+ /*Serial.print("Writing config block to bank ");
+ Serial.print(bank);
+ Serial.print(", offset ");
+ Serial.print(offset);
+ Serial.print(", length=");
+ Serial.println(length);*/
+ progBuffer = (uint8_t *) data + i;
+ success = mpu6050WriteMemoryBlock(progBuffer, length, bank, offset, true);
+ i += length;
+ }
+ else
+ {
+ // special instruction
+ // NOTE: this kind of behavior (what and when to do certain things)
+ // is totally undocumented. This code is in here based on observed
+ // behavior only, and exactly why (or even whether) it has to be here
+ // is anybody's guess for now.
+ special = data[i++];
+ /*Serial.print("Special command code ");
+ Serial.print(special, HEX);
+ Serial.println(" found...");*/
+ if (special == 0x01)
+ {
+ // enable DMP-related interrupts
+ mpu6050SetIntZeroMotionEnabled(true);
+ mpu6050SetIntFIFOBufferOverflowEnabled(true);
+ mpu6050SetIntDMPEnabled(true);
+ //i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_INT_ENABLE, 0x32);
+ success = true;
+ }
+ else
+ {
+ // unknown special command
+ success = false;
+ }
+ }
+
+ if (!success)
+ {
+ return false; // uh oh
+ }
return true;
}
@@ -3691,22 +3654,22 @@ bool mpu6050WriteProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize)
uint8_t mpu6050GetDMPConfig1()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_DMP_CFG_1, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_DMP_CFG_1, buffer);
return buffer[0];
}
void mpu6050SetDMPConfig1(uint8_t config)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_DMP_CFG_1, config);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_DMP_CFG_1, config);
}
// DMP_CFG_2 register
uint8_t mpu6050GetDMPConfig2()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_DMP_CFG_2, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6050_RA_DMP_CFG_2, buffer);
return buffer[0];
}
void mpu6050SetDMPConfig2(uint8_t config)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_DMP_CFG_2, config);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6050_RA_DMP_CFG_2, config);
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/mpu6500.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/mpu6500.c
index 9a29f7930..e54b85c95 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/mpu6500.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/mpu6500.c
@@ -121,131 +121,129 @@ bool mpu6500TestConnection()
*/
bool mpu6500SelfTest()
{
-// uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
-// uint8_t saveReg[5];
-// uint8_t selfTest[6];
-// int32_t gAvg[3]={0}, aAvg[3]={0}, aSTAvg[3]={0}, gSTAvg[3]={0};
-// int32_t factoryTrim[6];
-// float aDiff[3], gDiff[3];
-// uint8_t FS = 0;
-// int i;
-
-// // Save old configuration
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_SMPLRT_DIV, &saveReg[0]);
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_CONFIG, &saveReg[1]);
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, &saveReg[2]);
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG_2, &saveReg[3]);
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, &saveReg[4]);
-// // Write test configuration
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG_2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g
-
-// for(i = 0; i < 200; i++)
-// {
-// // get average current values of gyro and acclerometer
-// i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
-// aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
-// aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
-// aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
-
-// i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
-// gAvg[0] += (int16_t)((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a signed 16-bit value
-// gAvg[1] += (int16_t)((int16_t)rawData[2] << 8) | rawData[3];
-// gAvg[2] += (int16_t)((int16_t)rawData[4] << 8) | rawData[5];
-// }
-
-// for (i = 0; i < 3; i++)
-// { // Get average of 200 values and store as average current readings
-// aAvg[i] /= 200;
-// gAvg[i] /= 200;
-// }
-
-// // Configure the accelerometer for self-test
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
-// vTaskDelay(M2T(25)); // Delay a while to let the device stabilize
-
-// for(i = 0; i < 200; i++)
-// {
-// // get average self-test values of gyro and acclerometer
-// i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
-// aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
-// aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
-// aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
-
-// i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
-// gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
-// gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
-// gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
-// }
-
-// for (i =0; i < 3; i++)
-// { // Get average of 200 values and store as average self-test readings
-// aSTAvg[i] /= 200;
-// gSTAvg[i] /= 200;
-// }
-
-// // Configure the gyro and accelerometer for normal operation
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, 0x00);
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, 0x00);
-// vTaskDelay(M2T(25)); // Delay a while to let the device stabilize
-
-// // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_X_ACCEL, &selfTest[0]); // X-axis accel self-test results
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_Y_ACCEL, &selfTest[1]); // Y-axis accel self-test results
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_Z_ACCEL, &selfTest[2]); // Z-axis accel self-test results
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_X_GYRO, &selfTest[3]); // X-axis gyro self-test results
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_Y_GYRO, &selfTest[4]); // Y-axis gyro self-test results
-// i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_Z_GYRO, &selfTest[5]); // Z-axis gyro self-test results
-
-// for (i = 0; i < 6; i++)
-// {
-// if (selfTest[i] != 0)
-// {
-// factoryTrim[i] = mpu6500StTb[selfTest[i] - 1];
-// }
-// else
-// {
-// factoryTrim[i] = 0;
-// }
-// }
-
-// // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
-// // To get percent, must multiply by 100
-// for (i = 0; i < 3; i++)
-// {
-// aDiff[i] = 100.0f*((float)((aSTAvg[i] - aAvg[i]) - factoryTrim[i]))/factoryTrim[i]; // Report percent differences
-// gDiff[i] = 100.0f*((float)((gSTAvg[i] - gAvg[i]) - factoryTrim[i+3]))/factoryTrim[i+3]; // Report percent differences
-// // DEBUG_PRINT("a[%d] Avg:%d, StAvg:%d, Shift:%d, FT:%d, Diff:%0.2f\n", i, aAvg[i], aSTAvg[i], aSTAvg[i] - aAvg[i], factoryTrim[i], aDiff[i]);
-// // DEBUG_PRINT("g[%d] Avg:%d, StAvg:%d, Shift:%d, FT:%d, Diff:%0.2f\n", i, gAvg[i], gSTAvg[i], gSTAvg[i] - gAvg[i], factoryTrim[i+3], gDiff[i]);
-// }
-
-// // Restore old configuration
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_SMPLRT_DIV, saveReg[0]);
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_CONFIG, saveReg[1]);
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, saveReg[2]);
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG_2, saveReg[3]);
-// i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, saveReg[4]);
-
-// // Check result
-// if (mpu6500EvaluateSelfTest(MPU6500_ST_GYRO_LOW, MPU6500_ST_GYRO_HIGH, gDiff[0], "gyro X") &&
-// mpu6500EvaluateSelfTest(MPU6500_ST_GYRO_LOW, MPU6500_ST_GYRO_HIGH, gDiff[1], "gyro Y") &&
-// mpu6500EvaluateSelfTest(MPU6500_ST_GYRO_LOW, MPU6500_ST_GYRO_HIGH, gDiff[2], "gyro Z") &&
-// mpu6500EvaluateSelfTest(MPU6500_ST_ACCEL_LOW, MPU6500_ST_ACCEL_HIGH, aDiff[0], "acc X") &&
-// mpu6500EvaluateSelfTest(MPU6500_ST_ACCEL_LOW, MPU6500_ST_ACCEL_HIGH, aDiff[1], "acc Y") &&
-// mpu6500EvaluateSelfTest(MPU6500_ST_ACCEL_LOW, MPU6500_ST_ACCEL_HIGH, aDiff[2], "acc Z"))
-// {
-// return true;
-// }
-// else
-// {
-// return false;
-// }
-
-return true;
+ uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
+ uint8_t saveReg[5];
+ uint8_t selfTest[6];
+ int32_t gAvg[3]={0}, aAvg[3]={0}, aSTAvg[3]={0}, gSTAvg[3]={0};
+ int32_t factoryTrim[6];
+ float aDiff[3], gDiff[3];
+ uint8_t FS = 0;
+ int i;
+
+ // Save old configuration
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_SMPLRT_DIV, &saveReg[0]);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_CONFIG, &saveReg[1]);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, &saveReg[2]);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG_2, &saveReg[3]);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, &saveReg[4]);
+ // Write test configuration
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG_2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g
+
+ for(i = 0; i < 200; i++)
+ {
+ // get average current values of gyro and acclerometer
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
+ aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+ aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+ aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+ gAvg[0] += (int16_t)((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a signed 16-bit value
+ gAvg[1] += (int16_t)((int16_t)rawData[2] << 8) | rawData[3];
+ gAvg[2] += (int16_t)((int16_t)rawData[4] << 8) | rawData[5];
+ }
+
+ for (i = 0; i < 3; i++)
+ { // Get average of 200 values and store as average current readings
+ aAvg[i] /= 200;
+ gAvg[i] /= 200;
+ }
+
+ // Configure the accelerometer for self-test
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
+ vTaskDelay(M2T(25)); // Delay a while to let the device stabilize
+
+ for(i = 0; i < 200; i++)
+ {
+ // get average self-test values of gyro and acclerometer
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
+ aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+ aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+ aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+ gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+ gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+ gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+ }
+
+ for (i =0; i < 3; i++)
+ { // Get average of 200 values and store as average self-test readings
+ aSTAvg[i] /= 200;
+ gSTAvg[i] /= 200;
+ }
+
+ // Configure the gyro and accelerometer for normal operation
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, 0x00);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, 0x00);
+ vTaskDelay(M2T(25)); // Delay a while to let the device stabilize
+
+ // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_X_ACCEL, &selfTest[0]); // X-axis accel self-test results
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_Y_ACCEL, &selfTest[1]); // Y-axis accel self-test results
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_Z_ACCEL, &selfTest[2]); // Z-axis accel self-test results
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_X_GYRO, &selfTest[3]); // X-axis gyro self-test results
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_Y_GYRO, &selfTest[4]); // Y-axis gyro self-test results
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_ST_Z_GYRO, &selfTest[5]); // Z-axis gyro self-test results
+
+ for (i = 0; i < 6; i++)
+ {
+ if (selfTest[i] != 0)
+ {
+ factoryTrim[i] = mpu6500StTb[selfTest[i] - 1];
+ }
+ else
+ {
+ factoryTrim[i] = 0;
+ }
+ }
+
+ // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
+ // To get percent, must multiply by 100
+ for (i = 0; i < 3; i++)
+ {
+ aDiff[i] = 100.0f*((float)((aSTAvg[i] - aAvg[i]) - factoryTrim[i]))/factoryTrim[i]; // Report percent differences
+ gDiff[i] = 100.0f*((float)((gSTAvg[i] - gAvg[i]) - factoryTrim[i+3]))/factoryTrim[i+3]; // Report percent differences
+// DEBUG_PRINT("a[%d] Avg:%d, StAvg:%d, Shift:%d, FT:%d, Diff:%0.2f\n", i, aAvg[i], aSTAvg[i], aSTAvg[i] - aAvg[i], factoryTrim[i], aDiff[i]);
+// DEBUG_PRINT("g[%d] Avg:%d, StAvg:%d, Shift:%d, FT:%d, Diff:%0.2f\n", i, gAvg[i], gSTAvg[i], gSTAvg[i] - gAvg[i], factoryTrim[i+3], gDiff[i]);
+ }
+
+ // Restore old configuration
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_SMPLRT_DIV, saveReg[0]);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_CONFIG, saveReg[1]);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, saveReg[2]);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG_2, saveReg[3]);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, saveReg[4]);
+
+ // Check result
+ if (mpu6500EvaluateSelfTest(MPU6500_ST_GYRO_LOW, MPU6500_ST_GYRO_HIGH, gDiff[0], "gyro X") &&
+ mpu6500EvaluateSelfTest(MPU6500_ST_GYRO_LOW, MPU6500_ST_GYRO_HIGH, gDiff[1], "gyro Y") &&
+ mpu6500EvaluateSelfTest(MPU6500_ST_GYRO_LOW, MPU6500_ST_GYRO_HIGH, gDiff[2], "gyro Z") &&
+ mpu6500EvaluateSelfTest(MPU6500_ST_ACCEL_LOW, MPU6500_ST_ACCEL_HIGH, aDiff[0], "acc X") &&
+ mpu6500EvaluateSelfTest(MPU6500_ST_ACCEL_LOW, MPU6500_ST_ACCEL_HIGH, aDiff[1], "acc Y") &&
+ mpu6500EvaluateSelfTest(MPU6500_ST_ACCEL_LOW, MPU6500_ST_ACCEL_HIGH, aDiff[2], "acc Z"))
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
}
/** Evaluate the values from a MPU6500 self test.
@@ -257,12 +255,12 @@ return true;
*/
bool mpu6500EvaluateSelfTest(float low, float high, float value, char* string)
{
- // if (value < low || value > high)
- // {
- // DEBUG_PRINT("Self test %s [FAIL]. low: %0.2f, high: %0.2f, measured: %0.2f\n",
- // string, (double)low, (double)high, (double)value);
- // return false;
- // }
+ if (value < low || value > high)
+ {
+ DEBUG_PRINT("Self test %s [FAIL]. low: %0.2f, high: %0.2f, measured: %0.2f\n",
+ string, (double)low, (double)high, (double)value);
+ return false;
+ }
return true;
}
@@ -291,7 +289,7 @@ bool mpu6500EvaluateSelfTest(float low, float high, float value, char* string)
*/
uint8_t mpu6500GetRate()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_SMPLRT_DIV, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_SMPLRT_DIV, buffer);
return buffer[0];
}
/** Set gyroscope sample rate divider.
@@ -301,7 +299,7 @@ uint8_t mpu6500GetRate()
*/
void mpu6500SetRate(uint8_t rate)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_SMPLRT_DIV, rate);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_SMPLRT_DIV, rate);
}
// CONFIG register
@@ -335,8 +333,8 @@ void mpu6500SetRate(uint8_t rate)
*/
uint8_t mpu6500GetExternalFrameSync()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_CONFIG, MPU6500_CFG_EXT_SYNC_SET_BIT,
- // MPU6500_CFG_EXT_SYNC_SET_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_CONFIG, MPU6500_CFG_EXT_SYNC_SET_BIT,
+ MPU6500_CFG_EXT_SYNC_SET_LENGTH, buffer);
return buffer[0];
}
/** Set external FSYNC configuration.
@@ -346,8 +344,8 @@ uint8_t mpu6500GetExternalFrameSync()
*/
void mpu6500SetExternalFrameSync(uint8_t sync)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_CONFIG, MPU6500_CFG_EXT_SYNC_SET_BIT,
- // MPU6500_CFG_EXT_SYNC_SET_LENGTH, sync);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_CONFIG, MPU6500_CFG_EXT_SYNC_SET_BIT,
+ MPU6500_CFG_EXT_SYNC_SET_LENGTH, sync);
}
/** Get digital low-pass filter configuration.
* The DLPF_CFG parameter sets the digital low pass filter configuration. It
@@ -379,8 +377,8 @@ void mpu6500SetExternalFrameSync(uint8_t sync)
*/
uint8_t mpu6500GetDLPFMode()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_CONFIG, MPU6500_CFG_DLPF_CFG_BIT,
- // MPU6500_CFG_DLPF_CFG_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_CONFIG, MPU6500_CFG_DLPF_CFG_BIT,
+ MPU6500_CFG_DLPF_CFG_LENGTH, buffer);
return buffer[0];
}
/** Set digital low-pass filter configuration.
@@ -393,8 +391,8 @@ uint8_t mpu6500GetDLPFMode()
*/
void mpu6500SetDLPFMode(uint8_t mode)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_CONFIG, MPU6500_CFG_DLPF_CFG_BIT,
- // MPU6500_CFG_DLPF_CFG_LENGTH, mode);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_CONFIG, MPU6500_CFG_DLPF_CFG_BIT,
+ MPU6500_CFG_DLPF_CFG_LENGTH, mode);
}
// GYRO_CONFIG register
@@ -418,8 +416,8 @@ void mpu6500SetDLPFMode(uint8_t mode)
*/
uint8_t mpu6500GetFullScaleGyroRangeId()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, MPU6500_GCONFIG_FS_SEL_BIT,
- // MPU6500_GCONFIG_FS_SEL_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, MPU6500_GCONFIG_FS_SEL_BIT,
+ MPU6500_GCONFIG_FS_SEL_LENGTH, buffer);
return buffer[0];
}
@@ -436,25 +434,25 @@ float mpu6500GetFullScaleGyroDPL()
int32_t rangeId;
float range;
- // rangeId = mpu6500GetFullScaleGyroRangeId();
- // switch (rangeId)
- // {
- // case MPU6500_GYRO_FS_250:
- // range = MPU6500_DEG_PER_LSB_250;
- // break;
- // case MPU6500_GYRO_FS_500:
- // range = MPU6500_DEG_PER_LSB_500;
- // break;
- // case MPU6500_GYRO_FS_1000:
- // range = MPU6500_DEG_PER_LSB_1000;
- // break;
- // case MPU6500_GYRO_FS_2000:
- // range = MPU6500_DEG_PER_LSB_2000;
- // break;
- // default:
- // range = MPU6500_DEG_PER_LSB_1000;
- // break;
- // }
+ rangeId = mpu6500GetFullScaleGyroRangeId();
+ switch (rangeId)
+ {
+ case MPU6500_GYRO_FS_250:
+ range = MPU6500_DEG_PER_LSB_250;
+ break;
+ case MPU6500_GYRO_FS_500:
+ range = MPU6500_DEG_PER_LSB_500;
+ break;
+ case MPU6500_GYRO_FS_1000:
+ range = MPU6500_DEG_PER_LSB_1000;
+ break;
+ case MPU6500_GYRO_FS_2000:
+ range = MPU6500_DEG_PER_LSB_2000;
+ break;
+ default:
+ range = MPU6500_DEG_PER_LSB_1000;
+ break;
+ }
return range;
}
@@ -469,23 +467,23 @@ float mpu6500GetFullScaleGyroDPL()
*/
void mpu6500SetFullScaleGyroRange(uint8_t range)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, MPU6500_GCONFIG_FS_SEL_BIT,
- // MPU6500_GCONFIG_FS_SEL_LENGTH, range);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, MPU6500_GCONFIG_FS_SEL_BIT,
+ MPU6500_GCONFIG_FS_SEL_LENGTH, range);
}
void mpu6500SetGyroXSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, MPU6500_GCONFIG_XG_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, MPU6500_GCONFIG_XG_ST_BIT, enabled);
}
void mpu6500SetGyroYSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, MPU6500_GCONFIG_YG_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, MPU6500_GCONFIG_YG_ST_BIT, enabled);
}
void mpu6500SetGyroZSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, MPU6500_GCONFIG_ZG_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_GYRO_CONFIG, MPU6500_GCONFIG_ZG_ST_BIT, enabled);
}
// ACCEL_CONFIG register
@@ -496,7 +494,7 @@ void mpu6500SetGyroZSelfTest(bool enabled)
*/
bool mpu6500GetAccelXSelfTest()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_XA_ST_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_XA_ST_BIT, buffer);
return buffer[0];
}
/** Get self-test enabled setting for accelerometer X axis.
@@ -505,7 +503,7 @@ bool mpu6500GetAccelXSelfTest()
*/
void mpu6500SetAccelXSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_XA_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_XA_ST_BIT, enabled);
}
/** Get self-test enabled value for accelerometer Y axis.
* @return Self-test enabled value
@@ -513,7 +511,7 @@ void mpu6500SetAccelXSelfTest(bool enabled)
*/
bool mpu6500GetAccelYSelfTest()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_YA_ST_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_YA_ST_BIT, buffer);
return buffer[0];
}
/** Get self-test enabled value for accelerometer Y axis.
@@ -522,7 +520,7 @@ bool mpu6500GetAccelYSelfTest()
*/
void mpu6500SetAccelYSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_YA_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_YA_ST_BIT, enabled);
}
/** Get self-test enabled value for accelerometer Z axis.
* @return Self-test enabled value
@@ -530,7 +528,7 @@ void mpu6500SetAccelYSelfTest(bool enabled)
*/
bool mpu6500GetAccelZSelfTest()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_ZA_ST_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_ZA_ST_BIT, buffer);
return buffer[0];
}
/** Set self-test enabled value for accelerometer Z axis.
@@ -539,7 +537,7 @@ bool mpu6500GetAccelZSelfTest()
*/
void mpu6500SetAccelZSelfTest(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_ZA_ST_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_ZA_ST_BIT, enabled);
}
/** Get full-scale accelerometer range.
* The FS_SEL parameter allows setting the full-scale range of the accelerometer
@@ -560,8 +558,8 @@ void mpu6500SetAccelZSelfTest(bool enabled)
*/
uint8_t mpu6500GetFullScaleAccelRangeId()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_AFS_SEL_BIT,
- // MPU6500_ACONFIG_AFS_SEL_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_AFS_SEL_BIT,
+ MPU6500_ACONFIG_AFS_SEL_LENGTH, buffer);
return buffer[0];
}
@@ -578,25 +576,25 @@ float mpu6500GetFullScaleAccelGPL()
int32_t rangeId;
float range;
- // rangeId = mpu6500GetFullScaleAccelRangeId();
- // switch (rangeId)
- // {
- // case MPU6500_ACCEL_FS_2:
- // range = MPU6500_G_PER_LSB_2;
- // break;
- // case MPU6500_ACCEL_FS_4:
- // range = MPU6500_G_PER_LSB_4;
- // break;
- // case MPU6500_ACCEL_FS_8:
- // range = MPU6500_G_PER_LSB_8;
- // break;
- // case MPU6500_ACCEL_FS_16:
- // range = MPU6500_G_PER_LSB_16;
- // break;
- // default:
- // range = MPU6500_DEG_PER_LSB_1000;
- // break;
- // }
+ rangeId = mpu6500GetFullScaleAccelRangeId();
+ switch (rangeId)
+ {
+ case MPU6500_ACCEL_FS_2:
+ range = MPU6500_G_PER_LSB_2;
+ break;
+ case MPU6500_ACCEL_FS_4:
+ range = MPU6500_G_PER_LSB_4;
+ break;
+ case MPU6500_ACCEL_FS_8:
+ range = MPU6500_G_PER_LSB_8;
+ break;
+ case MPU6500_ACCEL_FS_16:
+ range = MPU6500_G_PER_LSB_16;
+ break;
+ default:
+ range = MPU6500_DEG_PER_LSB_1000;
+ break;
+ }
return range;
}
@@ -607,8 +605,8 @@ float mpu6500GetFullScaleAccelGPL()
*/
void mpu6500SetFullScaleAccelRange(uint8_t range)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_AFS_SEL_BIT,
- // MPU6500_ACONFIG_AFS_SEL_LENGTH, range);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_AFS_SEL_BIT,
+ MPU6500_ACONFIG_AFS_SEL_LENGTH, range);
}
/** Set accelerometer digital low pass filter.
@@ -623,8 +621,8 @@ void mpu6500SetFullScaleAccelRange(uint8_t range)
*/
void mpu6500SetAccelDLPF(uint8_t range)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG_2, MPU6500_ACONFIG2_DLPF_BIT,
- // MPU6500_ACONFIG2_DLPF_LENGTH, range);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG_2, MPU6500_ACONFIG2_DLPF_BIT,
+ MPU6500_ACONFIG2_DLPF_LENGTH, range);
}
/** Get the high-pass filter configuration.
@@ -664,8 +662,8 @@ void mpu6500SetAccelDLPF(uint8_t range)
*/
uint8_t mpu6500GetDHPFMode()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_ACCEL_HPF_BIT,
- // MPU6500_ACONFIG_ACCEL_HPF_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_ACCEL_HPF_BIT,
+ MPU6500_ACONFIG_ACCEL_HPF_LENGTH, buffer);
return buffer[0];
}
/** Set the high-pass filter configuration.
@@ -676,8 +674,8 @@ uint8_t mpu6500GetDHPFMode()
*/
void mpu6500SetDHPFMode(uint8_t bandwidth)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_ACCEL_HPF_BIT,
- // MPU6500_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_ACCEL_CONFIG, MPU6500_ACONFIG_ACCEL_HPF_BIT,
+ MPU6500_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);
}
// FIFO_EN register
@@ -690,7 +688,7 @@ void mpu6500SetDHPFMode(uint8_t bandwidth)
*/
bool mpu6500GetTempFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_TEMP_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_TEMP_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set temperature FIFO enabled value.
@@ -700,7 +698,7 @@ bool mpu6500GetTempFIFOEnabled()
*/
void mpu6500SetTempFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_TEMP_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_TEMP_FIFO_EN_BIT, enabled);
}
/** Get gyroscope X-axis FIFO enabled value.
* When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and
@@ -710,7 +708,7 @@ void mpu6500SetTempFIFOEnabled(bool enabled)
*/
bool mpu6500GetXGyroFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_XG_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_XG_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set gyroscope X-axis FIFO enabled value.
@@ -720,7 +718,7 @@ bool mpu6500GetXGyroFIFOEnabled()
*/
void mpu6500SetXGyroFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_XG_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_XG_FIFO_EN_BIT, enabled);
}
/** Get gyroscope Y-axis FIFO enabled value.
* When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and
@@ -730,7 +728,7 @@ void mpu6500SetXGyroFIFOEnabled(bool enabled)
*/
bool mpu6500GetYGyroFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_YG_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_YG_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set gyroscope Y-axis FIFO enabled value.
@@ -740,7 +738,7 @@ bool mpu6500GetYGyroFIFOEnabled()
*/
void mpu6500SetYGyroFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_YG_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_YG_FIFO_EN_BIT, enabled);
}
/** Get gyroscope Z-axis FIFO enabled value.
* When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and
@@ -750,7 +748,7 @@ void mpu6500SetYGyroFIFOEnabled(bool enabled)
*/
bool mpu6500GetZGyroFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_ZG_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_ZG_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set gyroscope Z-axis FIFO enabled value.
@@ -760,7 +758,7 @@ bool mpu6500GetZGyroFIFOEnabled()
*/
void mpu6500SetZGyroFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_ZG_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_ZG_FIFO_EN_BIT, enabled);
}
/** Get accelerometer FIFO enabled value.
* When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H,
@@ -771,7 +769,7 @@ void mpu6500SetZGyroFIFOEnabled(bool enabled)
*/
bool mpu6500GetAccelFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_ACCEL_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_ACCEL_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set accelerometer FIFO enabled value.
@@ -781,7 +779,7 @@ bool mpu6500GetAccelFIFOEnabled()
*/
void mpu6500SetAccelFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_ACCEL_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_ACCEL_FIFO_EN_BIT, enabled);
}
/** Get Slave 2 FIFO enabled value.
* When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -791,7 +789,7 @@ void mpu6500SetAccelFIFOEnabled(bool enabled)
*/
bool mpu6500GetSlave2FIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV2_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV2_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set Slave 2 FIFO enabled value.
@@ -801,7 +799,7 @@ bool mpu6500GetSlave2FIFOEnabled()
*/
void mpu6500SetSlave2FIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV2_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV2_FIFO_EN_BIT, enabled);
}
/** Get Slave 1 FIFO enabled value.
* When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -811,7 +809,7 @@ void mpu6500SetSlave2FIFOEnabled(bool enabled)
*/
bool mpu6500GetSlave1FIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV1_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV1_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set Slave 1 FIFO enabled value.
@@ -821,7 +819,7 @@ bool mpu6500GetSlave1FIFOEnabled()
*/
void mpu6500SetSlave1FIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV1_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV1_FIFO_EN_BIT, enabled);
}
/** Get Slave 0 FIFO enabled value.
* When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -831,7 +829,7 @@ void mpu6500SetSlave1FIFOEnabled(bool enabled)
*/
bool mpu6500GetSlave0FIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV0_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV0_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set Slave 0 FIFO enabled value.
@@ -841,7 +839,7 @@ bool mpu6500GetSlave0FIFOEnabled()
*/
void mpu6500SetSlave0FIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV0_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_FIFO_EN, MPU6500_SLV0_FIFO_EN_BIT, enabled);
}
// I2C_MST_CTRL register
@@ -863,7 +861,7 @@ void mpu6500SetSlave0FIFOEnabled(bool enabled)
*/
bool mpu6500GetMultiMasterEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_MULT_MST_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_MULT_MST_EN_BIT, buffer);
return buffer[0];
}
/** Set multi-master enabled value.
@@ -873,7 +871,7 @@ bool mpu6500GetMultiMasterEnabled()
*/
void mpu6500SetMultiMasterEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_MULT_MST_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_MULT_MST_EN_BIT, enabled);
}
/** Get wait-for-external-sensor-data enabled value.
* When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be
@@ -888,7 +886,7 @@ void mpu6500SetMultiMasterEnabled(bool enabled)
*/
bool mpu6500GetWaitForExternalSensorEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_WAIT_FOR_ES_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_WAIT_FOR_ES_BIT, buffer);
return buffer[0];
}
/** Set wait-for-external-sensor-data enabled value.
@@ -898,7 +896,7 @@ bool mpu6500GetWaitForExternalSensorEnabled()
*/
void mpu6500SetWaitForExternalSensorEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_WAIT_FOR_ES_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_WAIT_FOR_ES_BIT, enabled);
}
/** Get Slave 3 FIFO enabled value.
* When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -908,7 +906,7 @@ void mpu6500SetWaitForExternalSensorEnabled(bool enabled)
*/
bool mpu6500GetSlave3FIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_SLV_3_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_SLV_3_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set Slave 3 FIFO enabled value.
@@ -918,7 +916,7 @@ bool mpu6500GetSlave3FIFOEnabled()
*/
void mpu6500SetSlave3FIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_SLV_3_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_SLV_3_FIFO_EN_BIT, enabled);
}
/** Get slave read/write transition enabled value.
* The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave
@@ -932,7 +930,7 @@ void mpu6500SetSlave3FIFOEnabled(bool enabled)
*/
bool mpu6500GetSlaveReadWriteTransitionEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_I2C_MST_P_NSR_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_I2C_MST_P_NSR_BIT, buffer);
return buffer[0];
}
/** Set slave read/write transition enabled value.
@@ -942,7 +940,7 @@ bool mpu6500GetSlaveReadWriteTransitionEnabled()
*/
void mpu6500SetSlaveReadWriteTransitionEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_I2C_MST_P_NSR_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_I2C_MST_P_NSR_BIT, enabled);
}
/** Get I2C master clock speed.
* I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the
@@ -975,8 +973,8 @@ void mpu6500SetSlaveReadWriteTransitionEnabled(bool enabled)
*/
uint8_t mpu6500GetMasterClockSpeed()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_I2C_MST_CLK_BIT,
- // MPU6500_I2C_MST_CLK_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_I2C_MST_CLK_BIT,
+ MPU6500_I2C_MST_CLK_LENGTH, buffer);
return buffer[0];
}
/** Set I2C master clock speed.
@@ -985,8 +983,8 @@ uint8_t mpu6500GetMasterClockSpeed()
*/
void mpu6500SetMasterClockSpeed(uint8_t speed)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_I2C_MST_CLK_BIT,
- // MPU6500_I2C_MST_CLK_LENGTH, speed);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_I2C_MST_CTRL, MPU6500_I2C_MST_CLK_BIT,
+ MPU6500_I2C_MST_CLK_LENGTH, speed);
}
// I2C_SLV* registers (Slave 0-3)
@@ -1034,9 +1032,9 @@ void mpu6500SetMasterClockSpeed(uint8_t speed)
*/
uint8_t mpu6500GetSlaveAddress(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_ADDR + num * 3, buffer);
+ if (num > 3)
+ return 0;
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_ADDR + num * 3, buffer);
return buffer[0];
}
/** Set the I2C address of the specified slave (0-3).
@@ -1047,9 +1045,9 @@ uint8_t mpu6500GetSlaveAddress(uint8_t num)
*/
void mpu6500SetSlaveAddress(uint8_t num, uint8_t address)
{
- // if (num > 3)
- // return;
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_ADDR + num * 3, address);
+ if (num > 3)
+ return;
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_ADDR + num * 3, address);
}
/** Get the active internal register for the specified slave (0-3).
* Read/write operations for this slave will be done to whatever internal
@@ -1064,9 +1062,9 @@ void mpu6500SetSlaveAddress(uint8_t num, uint8_t address)
*/
uint8_t mpu6500GetSlaveRegister(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_REG + num * 3, buffer);
+ if (num > 3)
+ return 0;
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_REG + num * 3, buffer);
return buffer[0];
}
/** Set the active internal register for the specified slave (0-3).
@@ -1077,9 +1075,9 @@ uint8_t mpu6500GetSlaveRegister(uint8_t num)
*/
void mpu6500SetSlaveRegister(uint8_t num, uint8_t reg)
{
- // if (num > 3)
- // return;
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_REG + num * 3, reg);
+ if (num > 3)
+ return;
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_REG + num * 3, reg);
}
/** Get the enabled value for the specified slave (0-3).
* When set to 1, this bit enables Slave 0 for data transfer operations. When
@@ -1090,9 +1088,9 @@ void mpu6500SetSlaveRegister(uint8_t num, uint8_t reg)
*/
bool mpu6500GetSlaveEnabled(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_EN_BIT, buffer);
+ if (num > 3)
+ return 0;
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_EN_BIT, buffer);
return buffer[0];
}
/** Set the enabled value for the specified slave (0-3).
@@ -1103,10 +1101,10 @@ bool mpu6500GetSlaveEnabled(uint8_t num)
*/
void mpu6500SetSlaveEnabled(uint8_t num, bool enabled)
{
- // if (num > 3)
- // return;
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_EN_BIT,
- // enabled);
+ if (num > 3)
+ return;
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_EN_BIT,
+ enabled);
}
/** Get word pair byte-swapping enabled for the specified slave (0-3).
* When set to 1, this bit enables byte swapping. When byte swapping is enabled,
@@ -1121,10 +1119,10 @@ void mpu6500SetSlaveEnabled(uint8_t num, bool enabled)
*/
bool mpu6500GetSlaveWordByteSwap(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_BYTE_SW_BIT,
- // buffer);
+ if (num > 3)
+ return 0;
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_BYTE_SW_BIT,
+ buffer);
return buffer[0];
}
/** Set word pair byte-swapping enabled for the specified slave (0-3).
@@ -1135,10 +1133,10 @@ bool mpu6500GetSlaveWordByteSwap(uint8_t num)
*/
void mpu6500SetSlaveWordByteSwap(uint8_t num, bool enabled)
{
- // if (num > 3)
- // return;
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_BYTE_SW_BIT,
- // enabled);
+ if (num > 3)
+ return;
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_BYTE_SW_BIT,
+ enabled);
}
/** Get write mode for the specified slave (0-3).
* When set to 1, the transaction will read or write data only. When cleared to
@@ -1152,10 +1150,10 @@ void mpu6500SetSlaveWordByteSwap(uint8_t num, bool enabled)
*/
bool mpu6500GetSlaveWriteMode(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_REG_DIS_BIT,
- // buffer);
+ if (num > 3)
+ return 0;
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_REG_DIS_BIT,
+ buffer);
return buffer[0];
}
/** Set write mode for the specified slave (0-3).
@@ -1166,10 +1164,10 @@ bool mpu6500GetSlaveWriteMode(uint8_t num)
*/
void mpu6500SetSlaveWriteMode(uint8_t num, bool mode)
{
- // if (num > 3)
- // return;
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_REG_DIS_BIT,
- // mode);
+ if (num > 3)
+ return;
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_REG_DIS_BIT,
+ mode);
}
/** Get word pair grouping order offset for the specified slave (0-3).
* This sets specifies the grouping order of word pairs received from registers.
@@ -1184,9 +1182,9 @@ void mpu6500SetSlaveWriteMode(uint8_t num, bool mode)
*/
bool mpu6500GetSlaveWordGroupOffset(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_GRP_BIT, buffer);
+ if (num > 3)
+ return 0;
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_GRP_BIT, buffer);
return buffer[0];
}
/** Set word pair grouping order offset for the specified slave (0-3).
@@ -1197,10 +1195,10 @@ bool mpu6500GetSlaveWordGroupOffset(uint8_t num)
*/
void mpu6500SetSlaveWordGroupOffset(uint8_t num, bool enabled)
{
- // if (num > 3)
- // return;
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_GRP_BIT,
- // enabled);
+ if (num > 3)
+ return;
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_GRP_BIT,
+ enabled);
}
/** Get number of bytes to read for the specified slave (0-3).
* Specifies the number of bytes transferred to and from Slave 0. Clearing this
@@ -1211,10 +1209,10 @@ void mpu6500SetSlaveWordGroupOffset(uint8_t num, bool enabled)
*/
uint8_t mpu6500GetSlaveDataLength(uint8_t num)
{
- // if (num > 3)
- // return 0;
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_LEN_BIT,
- // MPU6500_I2C_SLV_LEN_LENGTH, buffer);
+ if (num > 3)
+ return 0;
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_LEN_BIT,
+ MPU6500_I2C_SLV_LEN_LENGTH, buffer);
return buffer[0];
}
/** Set number of bytes to read for the specified slave (0-3).
@@ -1225,10 +1223,10 @@ uint8_t mpu6500GetSlaveDataLength(uint8_t num)
*/
void mpu6500SetSlaveDataLength(uint8_t num, uint8_t length)
{
- // if (num > 3)
- // return;
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_LEN_BIT,
- // MPU6500_I2C_SLV_LEN_LENGTH, length);
+ if (num > 3)
+ return;
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_CTRL + num * 3, MPU6500_I2C_SLV_LEN_BIT,
+ MPU6500_I2C_SLV_LEN_LENGTH, length);
}
// I2C_SLV* registers (Slave 4)
@@ -1244,7 +1242,7 @@ void mpu6500SetSlaveDataLength(uint8_t num, uint8_t length)
*/
uint8_t mpu6500GetSlave4Address()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_ADDR, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_ADDR, buffer);
return buffer[0];
}
/** Set the I2C address of Slave 4.
@@ -1254,7 +1252,7 @@ uint8_t mpu6500GetSlave4Address()
*/
void mpu6500SetSlave4Address(uint8_t address)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_ADDR, address);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_ADDR, address);
}
/** Get the active internal register for the Slave 4.
* Read/write operations for this slave will be done to whatever internal
@@ -1265,7 +1263,7 @@ void mpu6500SetSlave4Address(uint8_t address)
*/
uint8_t mpu6500GetSlave4Register()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_REG, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_REG, buffer);
return buffer[0];
}
/** Set the active internal register for Slave 4.
@@ -1275,7 +1273,7 @@ uint8_t mpu6500GetSlave4Register()
*/
void mpu6500SetSlave4Register(uint8_t reg)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_REG, reg);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_REG, reg);
}
/** Set new byte to write to Slave 4.
* This register stores the data to be written into the Slave 4. If I2C_SLV4_RW
@@ -1285,7 +1283,7 @@ void mpu6500SetSlave4Register(uint8_t reg)
*/
void mpu6500SetSlave4OutputByte(uint8_t data)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_DO, data);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_DO, data);
}
/** Get the enabled value for the Slave 4.
* When set to 1, this bit enables Slave 4 for data transfer operations. When
@@ -1295,7 +1293,7 @@ void mpu6500SetSlave4OutputByte(uint8_t data)
*/
bool mpu6500GetSlave4Enabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_EN_BIT, buffer);
return buffer[0];
}
/** Set the enabled value for Slave 4.
@@ -1305,7 +1303,7 @@ bool mpu6500GetSlave4Enabled()
*/
void mpu6500SetSlave4Enabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_EN_BIT, enabled);
}
/** Get the enabled value for Slave 4 transaction interrupts.
* When set to 1, this bit enables the generation of an interrupt signal upon
@@ -1318,7 +1316,7 @@ void mpu6500SetSlave4Enabled(bool enabled)
*/
bool mpu6500GetSlave4InterruptEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_INT_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_INT_EN_BIT, buffer);
return buffer[0];
}
/** Set the enabled value for Slave 4 transaction interrupts.
@@ -1328,7 +1326,7 @@ bool mpu6500GetSlave4InterruptEnabled()
*/
void mpu6500SetSlave4InterruptEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_INT_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_INT_EN_BIT, enabled);
}
/** Get write mode for Slave 4.
* When set to 1, the transaction will read or write data only. When cleared to
@@ -1341,7 +1339,7 @@ void mpu6500SetSlave4InterruptEnabled(bool enabled)
*/
bool mpu6500GetSlave4WriteMode()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_REG_DIS_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_REG_DIS_BIT, buffer);
return buffer[0];
}
/** Set write mode for the Slave 4.
@@ -1351,7 +1349,7 @@ bool mpu6500GetSlave4WriteMode()
*/
void mpu6500SetSlave4WriteMode(bool mode)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_REG_DIS_BIT, mode);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_REG_DIS_BIT, mode);
}
/** Get Slave 4 master delay value.
* This configures the reduced access rate of I2C slaves relative to the Sample
@@ -1370,8 +1368,8 @@ void mpu6500SetSlave4WriteMode(bool mode)
*/
uint8_t mpu6500GetSlave4MasterDelay()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_MST_DLY_BIT,
- // MPU6500_I2C_SLV4_MST_DLY_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_MST_DLY_BIT,
+ MPU6500_I2C_SLV4_MST_DLY_LENGTH, buffer);
return buffer[0];
}
/** Set Slave 4 master delay value.
@@ -1381,8 +1379,8 @@ uint8_t mpu6500GetSlave4MasterDelay()
*/
void mpu6500SetSlave4MasterDelay(uint8_t delay)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_MST_DLY_BIT,
- // MPU6500_I2C_SLV4_MST_DLY_LENGTH, delay);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_CTRL, MPU6500_I2C_SLV4_MST_DLY_BIT,
+ MPU6500_I2C_SLV4_MST_DLY_LENGTH, delay);
}
/** Get last available byte read from Slave 4.
* This register stores the data read from Slave 4. This field is populated
@@ -1392,7 +1390,7 @@ void mpu6500SetSlave4MasterDelay(uint8_t delay)
*/
uint8_t mpu6500GetSlate4InputByte()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_DI, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV4_DI, buffer);
return buffer[0];
}
@@ -1409,7 +1407,7 @@ uint8_t mpu6500GetSlate4InputByte()
*/
bool mpu6500GetPassthroughStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_PASS_THROUGH_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_PASS_THROUGH_BIT, buffer);
return buffer[0];
}
/** Get Slave 4 transaction done status.
@@ -1422,7 +1420,7 @@ bool mpu6500GetPassthroughStatus()
*/
bool mpu6500GetSlave4IsDone()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV4_DONE_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV4_DONE_BIT, buffer);
return buffer[0];
}
/** Get master arbitration lost status.
@@ -1434,7 +1432,7 @@ bool mpu6500GetSlave4IsDone()
*/
bool mpu6500GetLostArbitration()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_LOST_ARB_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_LOST_ARB_BIT, buffer);
return buffer[0];
}
/** Get Slave 4 NACK status.
@@ -1446,7 +1444,7 @@ bool mpu6500GetLostArbitration()
*/
bool mpu6500GetSlave4Nack()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV4_NACK_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV4_NACK_BIT, buffer);
return buffer[0];
}
/** Get Slave 3 NACK status.
@@ -1458,7 +1456,7 @@ bool mpu6500GetSlave4Nack()
*/
bool mpu6500GetSlave3Nack()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV3_NACK_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV3_NACK_BIT, buffer);
return buffer[0];
}
/** Get Slave 2 NACK status.
@@ -1470,7 +1468,7 @@ bool mpu6500GetSlave3Nack()
*/
bool mpu6500GetSlave2Nack()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV2_NACK_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV2_NACK_BIT, buffer);
return buffer[0];
}
/** Get Slave 1 NACK status.
@@ -1482,7 +1480,7 @@ bool mpu6500GetSlave2Nack()
*/
bool mpu6500GetSlave1Nack()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV1_NACK_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV1_NACK_BIT, buffer);
return buffer[0];
}
/** Get Slave 0 NACK status.
@@ -1494,7 +1492,7 @@ bool mpu6500GetSlave1Nack()
*/
bool mpu6500GetSlave0Nack()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV0_NACK_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_STATUS, MPU6500_MST_I2C_SLV0_NACK_BIT, buffer);
return buffer[0];
}
@@ -1508,7 +1506,7 @@ bool mpu6500GetSlave0Nack()
*/
bool mpu6500GetInterruptMode()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_LEVEL_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_LEVEL_BIT, buffer);
return buffer[0];
}
/** Set interrupt logic level mode.
@@ -1519,7 +1517,7 @@ bool mpu6500GetInterruptMode()
*/
void mpu6500SetInterruptMode(bool mode)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_LEVEL_BIT, mode);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_LEVEL_BIT, mode);
}
/** Get interrupt drive mode.
* Will be set 0 for push-pull, 1 for open-drain.
@@ -1529,7 +1527,7 @@ void mpu6500SetInterruptMode(bool mode)
*/
bool mpu6500GetInterruptDrive()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_OPEN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_OPEN_BIT, buffer);
return buffer[0];
}
/** Set interrupt drive mode.
@@ -1540,7 +1538,7 @@ bool mpu6500GetInterruptDrive()
*/
void mpu6500SetInterruptDrive(bool drive)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_OPEN_BIT, drive);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_OPEN_BIT, drive);
}
/** Get interrupt latch mode.
* Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
@@ -1550,7 +1548,7 @@ void mpu6500SetInterruptDrive(bool drive)
*/
bool mpu6500GetInterruptLatch()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_LATCH_INT_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_LATCH_INT_EN_BIT, buffer);
return buffer[0];
}
/** Set interrupt latch mode.
@@ -1561,7 +1559,7 @@ bool mpu6500GetInterruptLatch()
*/
void mpu6500SetInterruptLatch(bool latch)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_LATCH_INT_EN_BIT, latch);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_LATCH_INT_EN_BIT, latch);
}
/** Get interrupt latch clear mode.
* Will be set 0 for status-read-only, 1 for any-register-read.
@@ -1571,7 +1569,7 @@ void mpu6500SetInterruptLatch(bool latch)
*/
bool mpu6500GetInterruptLatchClear()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_RD_CLEAR_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_RD_CLEAR_BIT, buffer);
return buffer[0];
}
/** Set interrupt latch clear mode.
@@ -1582,7 +1580,7 @@ bool mpu6500GetInterruptLatchClear()
*/
void mpu6500SetInterruptLatchClear(bool clear)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_RD_CLEAR_BIT, clear);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_INT_RD_CLEAR_BIT, clear);
}
/** Get FSYNC interrupt logic level mode.
* @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
@@ -1592,7 +1590,7 @@ void mpu6500SetInterruptLatchClear(bool clear)
*/
bool mpu6500GetFSyncInterruptLevel()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_FSYNC_INT_LEVEL_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_FSYNC_INT_LEVEL_BIT, buffer);
return buffer[0];
}
/** Set FSYNC interrupt logic level mode.
@@ -1603,7 +1601,7 @@ bool mpu6500GetFSyncInterruptLevel()
*/
void mpu6500SetFSyncInterruptLevel(bool level)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_FSYNC_INT_LEVEL_BIT, level);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_FSYNC_INT_LEVEL_BIT, level);
}
/** Get FSYNC pin interrupt enabled setting.
* Will be set 0 for disabled, 1 for enabled.
@@ -1613,7 +1611,7 @@ void mpu6500SetFSyncInterruptLevel(bool level)
*/
bool mpu6500GetFSyncInterruptEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_FSYNC_INT_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_FSYNC_INT_EN_BIT, buffer);
return buffer[0];
}
/** Set FSYNC pin interrupt enabled setting.
@@ -1624,7 +1622,7 @@ bool mpu6500GetFSyncInterruptEnabled()
*/
void mpu6500SetFSyncInterruptEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_FSYNC_INT_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_FSYNC_INT_EN_BIT, enabled);
}
/** Get I2C bypass enabled status.
* When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
@@ -1639,7 +1637,7 @@ void mpu6500SetFSyncInterruptEnabled(bool enabled)
*/
bool mpu6500GetI2CBypassEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_I2C_BYPASS_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_I2C_BYPASS_EN_BIT, buffer);
return buffer[0];
}
/** Set I2C bypass enabled status.
@@ -1655,7 +1653,7 @@ bool mpu6500GetI2CBypassEnabled()
*/
void mpu6500SetI2CBypassEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_I2C_BYPASS_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}
/** Get reference clock output enabled status.
* When this bit is equal to 1, a reference clock output is provided at the
@@ -1668,7 +1666,7 @@ void mpu6500SetI2CBypassEnabled(bool enabled)
*/
bool mpu6500GetClockOutputEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_CLKOUT_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_CLKOUT_EN_BIT, buffer);
return buffer[0];
}
/** Set reference clock output enabled status.
@@ -1682,7 +1680,7 @@ bool mpu6500GetClockOutputEnabled()
*/
void mpu6500SetClockOutputEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_CLKOUT_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_PIN_CFG, MPU6500_INTCFG_CLKOUT_EN_BIT, enabled);
}
// INT_ENABLE register
@@ -1696,7 +1694,7 @@ void mpu6500SetClockOutputEnabled(bool enabled)
**/
uint8_t mpu6500GetIntEnabled()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, buffer);
return buffer[0];
}
/** Set full interrupt enabled status.
@@ -1709,7 +1707,7 @@ uint8_t mpu6500GetIntEnabled()
**/
void mpu6500SetIntEnabled(uint8_t enabled)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, enabled);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, enabled);
}
/** Get Free Fall interrupt enabled status.
* Will be set 0 for disabled, 1 for enabled.
@@ -1719,7 +1717,7 @@ void mpu6500SetIntEnabled(uint8_t enabled)
**/
bool mpu6500GetIntFreefallEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_FF_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_FF_BIT, buffer);
return buffer[0];
}
/** Set Free Fall interrupt enabled status.
@@ -1740,7 +1738,7 @@ void mpu6500SetIntFreefallEnabled(bool enabled)
**/
bool mpu6500GetIntMotionEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_MOT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_MOT_BIT, buffer);
return buffer[0];
}
/** Set Motion Detection interrupt enabled status.
@@ -1751,7 +1749,7 @@ bool mpu6500GetIntMotionEnabled()
**/
void mpu6500SetIntMotionEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_MOT_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_MOT_BIT, enabled);
}
/** Get Zero Motion Detection interrupt enabled status.
* Will be set 0 for disabled, 1 for enabled.
@@ -1761,7 +1759,7 @@ void mpu6500SetIntMotionEnabled(bool enabled)
**/
bool mpu6500GetIntZeroMotionEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_ZMOT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_ZMOT_BIT, buffer);
return buffer[0];
}
/** Set Zero Motion Detection interrupt enabled status.
@@ -1772,7 +1770,7 @@ bool mpu6500GetIntZeroMotionEnabled()
**/
void mpu6500SetIntZeroMotionEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_ZMOT_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_ZMOT_BIT, enabled);
}
/** Get FIFO Buffer Overflow interrupt enabled status.
* Will be set 0 for disabled, 1 for enabled.
@@ -1782,7 +1780,7 @@ void mpu6500SetIntZeroMotionEnabled(bool enabled)
**/
bool mpu6500GetIntFIFOBufferOverflowEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_FIFO_OFLOW_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_FIFO_OFLOW_BIT, buffer);
return buffer[0];
}
/** Set FIFO Buffer Overflow interrupt enabled status.
@@ -1793,7 +1791,7 @@ bool mpu6500GetIntFIFOBufferOverflowEnabled()
**/
void mpu6500SetIntFIFOBufferOverflowEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_FIFO_OFLOW_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_FIFO_OFLOW_BIT, enabled);
}
/** Get I2C Master interrupt enabled status.
* This enables any of the I2C Master interrupt sources to generate an
@@ -1804,7 +1802,7 @@ void mpu6500SetIntFIFOBufferOverflowEnabled(bool enabled)
**/
bool mpu6500GetIntI2CMasterEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_I2C_MST_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_I2C_MST_INT_BIT, buffer);
return buffer[0];
}
/** Set I2C Master interrupt enabled status.
@@ -1815,7 +1813,7 @@ bool mpu6500GetIntI2CMasterEnabled()
**/
void mpu6500SetIntI2CMasterEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_I2C_MST_INT_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_I2C_MST_INT_BIT, enabled);
}
/** Get Data Ready interrupt enabled setting.
* This event occurs each time a write operation to all of the sensor registers
@@ -1826,7 +1824,7 @@ void mpu6500SetIntI2CMasterEnabled(bool enabled)
*/
bool mpu6500GetIntDataReadyEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_DATA_RDY_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_DATA_RDY_BIT, buffer);
return buffer[0];
}
/** Set Data Ready interrupt enabled status.
@@ -1837,7 +1835,7 @@ bool mpu6500GetIntDataReadyEnabled()
*/
void mpu6500SetIntDataReadyEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_DATA_RDY_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_DATA_RDY_BIT, enabled);
}
// INT_STATUS register
@@ -1851,7 +1849,7 @@ void mpu6500SetIntDataReadyEnabled(bool enabled)
*/
uint8_t mpu6500GetIntStatus()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_INT_STATUS, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_INT_STATUS, buffer);
return buffer[0];
}
/** Get Free Fall interrupt status.
@@ -1863,7 +1861,7 @@ uint8_t mpu6500GetIntStatus()
*/
bool mpu6500GetIntFreefallStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_FF_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_FF_BIT, buffer);
return buffer[0];
}
/** Get Motion Detection interrupt status.
@@ -1875,7 +1873,7 @@ bool mpu6500GetIntFreefallStatus()
*/
bool mpu6500GetIntMotionStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_MOT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_MOT_BIT, buffer);
return buffer[0];
}
/** Get Zero Motion Detection interrupt status.
@@ -1887,7 +1885,7 @@ bool mpu6500GetIntMotionStatus()
*/
bool mpu6500GetIntZeroMotionStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_ZMOT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_ZMOT_BIT, buffer);
return buffer[0];
}
/** Get FIFO Buffer Overflow interrupt status.
@@ -1899,7 +1897,7 @@ bool mpu6500GetIntZeroMotionStatus()
*/
bool mpu6500GetIntFIFOBufferOverflowStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_FIFO_OFLOW_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_FIFO_OFLOW_BIT, buffer);
return buffer[0];
}
/** Get I2C Master interrupt status.
@@ -1912,7 +1910,7 @@ bool mpu6500GetIntFIFOBufferOverflowStatus()
*/
bool mpu6500GetIntI2CMasterStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_I2C_MST_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_I2C_MST_INT_BIT, buffer);
return buffer[0];
}
/** Get Data Ready interrupt status.
@@ -1924,7 +1922,7 @@ bool mpu6500GetIntI2CMasterStatus()
*/
bool mpu6500GetIntDataReadyStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_DATA_RDY_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_DATA_RDY_BIT, buffer);
return buffer[0];
}
@@ -1966,13 +1964,13 @@ void mpu6500GetMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16
*/
void mpu6500GetMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_XOUT_H, 14, buffer);
- // *ax = (((int16_t) buffer[0]) << 8) | buffer[1];
- // *ay = (((int16_t) buffer[2]) << 8) | buffer[3];
- // *az = (((int16_t) buffer[4]) << 8) | buffer[5];
- // *gx = (((int16_t) buffer[8]) << 8) | buffer[9];
- // *gy = (((int16_t) buffer[10]) << 8) | buffer[11];
- // *gz = (((int16_t) buffer[12]) << 8) | buffer[13];
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_XOUT_H, 14, buffer);
+ *ax = (((int16_t) buffer[0]) << 8) | buffer[1];
+ *ay = (((int16_t) buffer[2]) << 8) | buffer[3];
+ *az = (((int16_t) buffer[4]) << 8) | buffer[5];
+ *gx = (((int16_t) buffer[8]) << 8) | buffer[9];
+ *gy = (((int16_t) buffer[10]) << 8) | buffer[11];
+ *gz = (((int16_t) buffer[12]) << 8) | buffer[13];
}
/** Get 3-axis accelerometer readings.
* These registers store the most recent accelerometer measurements.
@@ -2012,10 +2010,10 @@ void mpu6500GetMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16
*/
void mpu6500GetAcceleration(int16_t* x, int16_t* y, int16_t* z)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_XOUT_H, 6, buffer);
- // *x = (((int16_t) buffer[0]) << 8) | buffer[1];
- // *y = (((int16_t) buffer[2]) << 8) | buffer[3];
- // *z = (((int16_t) buffer[4]) << 8) | buffer[5];
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_XOUT_H, 6, buffer);
+ *x = (((int16_t) buffer[0]) << 8) | buffer[1];
+ *y = (((int16_t) buffer[2]) << 8) | buffer[3];
+ *z = (((int16_t) buffer[4]) << 8) | buffer[5];
}
/** Get X-axis accelerometer reading.
* @return X-axis acceleration measurement in 16-bit 2's complement format
@@ -2024,7 +2022,7 @@ void mpu6500GetAcceleration(int16_t* x, int16_t* y, int16_t* z)
*/
int16_t mpu6500GetAccelerationX()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_XOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_XOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
/** Get Y-axis accelerometer reading.
@@ -2034,7 +2032,7 @@ int16_t mpu6500GetAccelerationX()
*/
int16_t mpu6500GetAccelerationY()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_YOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_YOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
/** Get Z-axis accelerometer reading.
@@ -2044,7 +2042,7 @@ int16_t mpu6500GetAccelerationY()
*/
int16_t mpu6500GetAccelerationZ()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_ZOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_ACCEL_ZOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
@@ -2056,7 +2054,7 @@ int16_t mpu6500GetAccelerationZ()
*/
int16_t mpu6500GetTemperature()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_TEMP_OUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_TEMP_OUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
@@ -2096,10 +2094,10 @@ int16_t mpu6500GetTemperature()
*/
void mpu6500GetRotation(int16_t* x, int16_t* y, int16_t* z)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_XOUT_H, 6, buffer);
- // *x = (((int16_t) buffer[0]) << 8) | buffer[1];
- // *y = (((int16_t) buffer[2]) << 8) | buffer[3];
- // *z = (((int16_t) buffer[4]) << 8) | buffer[5];
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_XOUT_H, 6, buffer);
+ *x = (((int16_t) buffer[0]) << 8) | buffer[1];
+ *y = (((int16_t) buffer[2]) << 8) | buffer[3];
+ *z = (((int16_t) buffer[4]) << 8) | buffer[5];
}
/** Get X-axis gyroscope reading.
* @return X-axis rotation measurement in 16-bit 2's complement format
@@ -2108,7 +2106,7 @@ void mpu6500GetRotation(int16_t* x, int16_t* y, int16_t* z)
*/
int16_t mpu6500GetRotationX()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_XOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_XOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
/** Get Y-axis gyroscope reading.
@@ -2118,7 +2116,7 @@ int16_t mpu6500GetRotationX()
*/
int16_t mpu6500GetRotationY()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_YOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_YOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
/** Get Z-axis gyroscope reading.
@@ -2128,7 +2126,7 @@ int16_t mpu6500GetRotationY()
*/
int16_t mpu6500GetRotationZ()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_ZOUT_H, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_GYRO_ZOUT_H, 2, buffer);
return (((int16_t) buffer[0]) << 8) | buffer[1];
}
@@ -2210,7 +2208,7 @@ int16_t mpu6500GetRotationZ()
*/
uint8_t mpu6500GetExternalSensorByte(int position)
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_EXT_SENS_DATA_00 + position, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_EXT_SENS_DATA_00 + position, buffer);
return buffer[0];
}
/** Read word (2 bytes) from external sensor data registers.
@@ -2220,7 +2218,7 @@ uint8_t mpu6500GetExternalSensorByte(int position)
*/
uint16_t mpu6500GetExternalSensorWord(int position)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_EXT_SENS_DATA_00 + position, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_EXT_SENS_DATA_00 + position, 2, buffer);
return (((uint16_t) buffer[0]) << 8) | buffer[1];
}
/** Read double word (4 bytes) from external sensor data registers.
@@ -2230,7 +2228,7 @@ uint16_t mpu6500GetExternalSensorWord(int position)
*/
uint32_t mpu6500GetExternalSensorDWord(int position)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_EXT_SENS_DATA_00 + position, 4, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_EXT_SENS_DATA_00 + position, 4, buffer);
return (((uint32_t) buffer[0]) << 24) | (((uint32_t) buffer[1]) << 16)
| (((uint16_t) buffer[2]) << 8) | buffer[3];
}
@@ -2244,7 +2242,7 @@ uint32_t mpu6500GetExternalSensorDWord(int position)
*/
bool mpu6500GetXNegMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_XNEG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_XNEG_BIT, buffer);
return buffer[0];
}
/** Get X-axis positive motion detection interrupt status.
@@ -2254,7 +2252,7 @@ bool mpu6500GetXNegMotionDetected()
*/
bool mpu6500GetXPosMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_XPOS_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_XPOS_BIT, buffer);
return buffer[0];
}
/** Get Y-axis negative motion detection interrupt status.
@@ -2264,7 +2262,7 @@ bool mpu6500GetXPosMotionDetected()
*/
bool mpu6500GetYNegMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_YNEG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_YNEG_BIT, buffer);
return buffer[0];
}
/** Get Y-axis positive motion detection interrupt status.
@@ -2274,7 +2272,7 @@ bool mpu6500GetYNegMotionDetected()
*/
bool mpu6500GetYPosMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_YPOS_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_YPOS_BIT, buffer);
return buffer[0];
}
/** Get Z-axis negative motion detection interrupt status.
@@ -2284,7 +2282,7 @@ bool mpu6500GetYPosMotionDetected()
*/
bool mpu6500GetZNegMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_ZNEG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_ZNEG_BIT, buffer);
return buffer[0];
}
/** Get Z-axis positive motion detection interrupt status.
@@ -2294,7 +2292,7 @@ bool mpu6500GetZNegMotionDetected()
*/
bool mpu6500GetZPosMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_ZPOS_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_ZPOS_BIT, buffer);
return buffer[0];
}
/** Get zero motion detection interrupt status.
@@ -2304,7 +2302,7 @@ bool mpu6500GetZPosMotionDetected()
*/
bool mpu6500GetZeroMotionDetected()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_ZRMOT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_STATUS, MPU6500_MOTION_MOT_ZRMOT_BIT, buffer);
return buffer[0];
}
@@ -2320,9 +2318,9 @@ bool mpu6500GetZeroMotionDetected()
*/
void mpu6500SetSlaveOutputByte(uint8_t num, uint8_t data)
{
- // if (num > 3)
- // return;
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_DO + num, data);
+ if (num > 3)
+ return;
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_I2C_SLV0_DO + num, data);
}
// I2C_MST_DELAY_CTRL register
@@ -2337,8 +2335,8 @@ void mpu6500SetSlaveOutputByte(uint8_t num, uint8_t data)
*/
bool mpu6500GetExternalShadowDelayEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_DELAY_CTRL, MPU6500_DELAYCTRL_DELAY_ES_SHADOW_BIT,
- // buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_DELAY_CTRL, MPU6500_DELAYCTRL_DELAY_ES_SHADOW_BIT,
+ buffer);
return buffer[0];
}
/** Set external data shadow delay enabled status.
@@ -2349,8 +2347,8 @@ bool mpu6500GetExternalShadowDelayEnabled()
*/
void mpu6500SetExternalShadowDelayEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_DELAY_CTRL,
- // MPU6500_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_DELAY_CTRL,
+ MPU6500_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled);
}
/** Get slave delay enabled status.
* When a particular slave delay is enabled, the rate of access for the that
@@ -2373,9 +2371,9 @@ void mpu6500SetExternalShadowDelayEnabled(bool enabled)
bool mpu6500GetSlaveDelayEnabled(uint8_t num)
{
// MPU6500_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
- // if (num > 4)
- // return 0;
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_DELAY_CTRL, num, buffer);
+ if (num > 4)
+ return 0;
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_DELAY_CTRL, num, buffer);
return buffer[0];
}
/** Set slave delay enabled status.
@@ -2386,7 +2384,7 @@ bool mpu6500GetSlaveDelayEnabled(uint8_t num)
*/
void mpu6500SetSlaveDelayEnabled(uint8_t num, bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_DELAY_CTRL, num, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_I2C_MST_DELAY_CTRL, num, enabled);
}
// SIGNAL_PATH_RESET register
@@ -2399,7 +2397,7 @@ void mpu6500SetSlaveDelayEnabled(uint8_t num, bool enabled)
*/
void mpu6500ResetGyroscopePath()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_SIGNAL_PATH_RESET, MPU6500_PATHRESET_GYRO_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_SIGNAL_PATH_RESET, MPU6500_PATHRESET_GYRO_RESET_BIT, 1);
}
/** Reset accelerometer signal path.
* The reset will revert the signal path analog to digital converters and
@@ -2409,7 +2407,7 @@ void mpu6500ResetGyroscopePath()
*/
void mpu6500ResetAccelerometerPath()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_SIGNAL_PATH_RESET, MPU6500_PATHRESET_ACCEL_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_SIGNAL_PATH_RESET, MPU6500_PATHRESET_ACCEL_RESET_BIT, 1);
}
/** Reset temperature sensor signal path.
* The reset will revert the signal path analog to digital converters and
@@ -2419,7 +2417,7 @@ void mpu6500ResetAccelerometerPath()
*/
void mpu6500ResetTemperaturePath()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_SIGNAL_PATH_RESET, MPU6500_PATHRESET_TEMP_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_SIGNAL_PATH_RESET, MPU6500_PATHRESET_TEMP_RESET_BIT, 1);
}
// MOT_DETECT_CTRL register
@@ -2440,8 +2438,8 @@ void mpu6500ResetTemperaturePath()
*/
uint8_t mpu6500GetAccelerometerPowerOnDelay()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_ACCEL_ON_DELAY_BIT,
- // MPU6500_DETECT_ACCEL_ON_DELAY_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_ACCEL_ON_DELAY_BIT,
+ MPU6500_DETECT_ACCEL_ON_DELAY_LENGTH, buffer);
return buffer[0];
}
/** Set accelerometer power-on delay.
@@ -2452,8 +2450,8 @@ uint8_t mpu6500GetAccelerometerPowerOnDelay()
*/
void mpu6500SetAccelerometerPowerOnDelay(uint8_t delay)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_ACCEL_ON_DELAY_BIT,
- // MPU6500_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_ACCEL_ON_DELAY_BIT,
+ MPU6500_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
}
/** Get Free Fall detection counter decrement configuration.
* Detection is registered by the Free Fall detection module after accelerometer
@@ -2483,8 +2481,8 @@ void mpu6500SetAccelerometerPowerOnDelay(uint8_t delay)
*/
uint8_t mpu6500GetFreefallDetectionCounterDecrement()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_FF_COUNT_BIT,
- // MPU6500_DETECT_FF_COUNT_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_FF_COUNT_BIT,
+ MPU6500_DETECT_FF_COUNT_LENGTH, buffer);
return buffer[0];
}
/** Set Free Fall detection counter decrement configuration.
@@ -2495,8 +2493,8 @@ uint8_t mpu6500GetFreefallDetectionCounterDecrement()
*/
void mpu6500SetFreefallDetectionCounterDecrement(uint8_t decrement)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_FF_COUNT_BIT,
- // MPU6500_DETECT_FF_COUNT_LENGTH, decrement);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_FF_COUNT_BIT,
+ MPU6500_DETECT_FF_COUNT_LENGTH, decrement);
}
/** Get Motion detection counter decrement configuration.
* Detection is registered by the Motion detection module after accelerometer
@@ -2523,8 +2521,8 @@ void mpu6500SetFreefallDetectionCounterDecrement(uint8_t decrement)
*/
uint8_t mpu6500GetMotionDetectionCounterDecrement()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_MOT_COUNT_BIT,
- // MPU6500_DETECT_MOT_COUNT_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_MOT_COUNT_BIT,
+ MPU6500_DETECT_MOT_COUNT_LENGTH, buffer);
return buffer[0];
}
/** Set Motion detection counter decrement configuration.
@@ -2535,8 +2533,8 @@ uint8_t mpu6500GetMotionDetectionCounterDecrement()
*/
void mpu6500SetMotionDetectionCounterDecrement(uint8_t decrement)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_MOT_COUNT_BIT,
- // MPU6500_DETECT_MOT_COUNT_LENGTH, decrement);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_MOT_DETECT_CTRL, MPU6500_DETECT_MOT_COUNT_BIT,
+ MPU6500_DETECT_MOT_COUNT_LENGTH, decrement);
}
// USER_CTRL register
@@ -2551,7 +2549,7 @@ void mpu6500SetMotionDetectionCounterDecrement(uint8_t decrement)
*/
bool mpu6500GetFIFOEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_FIFO_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_FIFO_EN_BIT, buffer);
return buffer[0];
}
/** Set FIFO enabled status.
@@ -2562,7 +2560,7 @@ bool mpu6500GetFIFOEnabled()
*/
void mpu6500SetFIFOEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_FIFO_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_FIFO_EN_BIT, enabled);
}
/** Get I2C Master Mode enabled status.
* When this mode is enabled, the MPU-60X0 acts as the I2C Master to the
@@ -2577,7 +2575,7 @@ void mpu6500SetFIFOEnabled(bool enabled)
*/
bool mpu6500GetI2CMasterModeEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_I2C_MST_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_I2C_MST_EN_BIT, buffer);
return buffer[0];
}
/** Set I2C Master Mode enabled status.
@@ -2588,7 +2586,7 @@ bool mpu6500GetI2CMasterModeEnabled()
*/
void mpu6500SetI2CMasterModeEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_I2C_MST_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_I2C_MST_EN_BIT, enabled);
}
/** Switch from I2C to SPI mode (MPU-6000 only)
* If this is set, the primary SPI interface will be enabled in place of the
@@ -2596,7 +2594,7 @@ void mpu6500SetI2CMasterModeEnabled(bool enabled)
*/
void mpu6500SwitchSPIEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_I2C_IF_DIS_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_I2C_IF_DIS_BIT, enabled);
}
/** Reset the FIFO.
* This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
@@ -2606,7 +2604,7 @@ void mpu6500SwitchSPIEnabled(bool enabled)
*/
void mpu6500ResetFIFO()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_FIFO_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_FIFO_RESET_BIT, 1);
}
/** Reset the I2C Master.
* This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0.
@@ -2616,7 +2614,7 @@ void mpu6500ResetFIFO()
*/
void mpu6500ResetI2CMaster()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_I2C_MST_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_I2C_MST_RESET_BIT, 1);
}
/** Reset all sensor registers and signal paths.
* When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
@@ -2632,7 +2630,7 @@ void mpu6500ResetI2CMaster()
*/
void mpu6500ResetSensors()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_SIG_COND_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_SIG_COND_RESET_BIT, 1);
}
// PWR_MGMT_1 register
@@ -2644,7 +2642,7 @@ void mpu6500ResetSensors()
*/
void mpu6500Reset()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_DEVICE_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_DEVICE_RESET_BIT, 1);
}
/** Get sleep mode status.
* Setting the SLEEP bit in the register puts the device into very low power
@@ -2659,7 +2657,7 @@ void mpu6500Reset()
*/
bool mpu6500GetSleepEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_SLEEP_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_SLEEP_BIT, buffer);
return buffer[0];
}
/** Set sleep mode status.
@@ -2670,7 +2668,7 @@ bool mpu6500GetSleepEnabled()
*/
void mpu6500SetSleepEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_SLEEP_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_SLEEP_BIT, enabled);
}
/** Get wake cycle enabled status.
* When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
@@ -2682,7 +2680,7 @@ void mpu6500SetSleepEnabled(bool enabled)
*/
bool mpu6500GetWakeCycleEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_CYCLE_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_CYCLE_BIT, buffer);
return buffer[0];
}
/** Set wake cycle enabled status.
@@ -2693,7 +2691,7 @@ bool mpu6500GetWakeCycleEnabled()
*/
void mpu6500SetWakeCycleEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_CYCLE_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_CYCLE_BIT, enabled);
}
/** Get temperature sensor enabled status.
* Control the usage of the internal temperature sensor.
@@ -2708,7 +2706,7 @@ void mpu6500SetWakeCycleEnabled(bool enabled)
*/
bool mpu6500GetTempSensorEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_TEMP_DIS_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_TEMP_DIS_BIT, buffer);
return buffer[0] == 0; // 1 is actually disabled here
}
/** Set temperature sensor enabled status.
@@ -2724,7 +2722,7 @@ bool mpu6500GetTempSensorEnabled()
void mpu6500SetTempSensorEnabled(bool enabled)
{
// 1 is actually disabled here
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_TEMP_DIS_BIT, !enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_TEMP_DIS_BIT, !enabled);
}
/** Get clock source setting.
* @return Current clock source setting
@@ -2734,8 +2732,8 @@ void mpu6500SetTempSensorEnabled(bool enabled)
*/
uint8_t mpu6500GetClockSource()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_CLKSEL_BIT,
- // MPU6500_PWR1_CLKSEL_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_CLKSEL_BIT,
+ MPU6500_PWR1_CLKSEL_LENGTH, buffer);
return buffer[0];
}
/** Set clock source setting.
@@ -2770,8 +2768,8 @@ uint8_t mpu6500GetClockSource()
*/
void mpu6500SetClockSource(uint8_t source)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_CLKSEL_BIT,
- // MPU6500_PWR1_CLKSEL_LENGTH, source);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_1, MPU6500_PWR1_CLKSEL_BIT,
+ MPU6500_PWR1_CLKSEL_LENGTH, source);
}
// PWR_MGMT_2 register
@@ -2801,8 +2799,8 @@ void mpu6500SetClockSource(uint8_t source)
*/
uint8_t mpu6500GetWakeFrequency()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_LP_WAKE_CTRL_BIT,
- // MPU6500_PWR2_LP_WAKE_CTRL_LENGTH, buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_LP_WAKE_CTRL_BIT,
+ MPU6500_PWR2_LP_WAKE_CTRL_LENGTH, buffer);
return buffer[0];
}
/** Set wake frequency in Accel-Only Low Power Mode.
@@ -2811,8 +2809,8 @@ uint8_t mpu6500GetWakeFrequency()
*/
void mpu6500SetWakeFrequency(uint8_t frequency)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_LP_WAKE_CTRL_BIT,
- // MPU6500_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_LP_WAKE_CTRL_BIT,
+ MPU6500_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
}
/** Get X-axis accelerometer standby enabled status.
@@ -2823,7 +2821,7 @@ void mpu6500SetWakeFrequency(uint8_t frequency)
*/
bool mpu6500GetStandbyXAccelEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_XA_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_XA_BIT, buffer);
return buffer[0];
}
/** Set X-axis accelerometer standby enabled status.
@@ -2834,7 +2832,7 @@ bool mpu6500GetStandbyXAccelEnabled()
*/
void mpu6500SetStandbyXAccelEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_XA_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_XA_BIT, enabled);
}
/** Get Y-axis accelerometer standby enabled status.
* If enabled, the Y-axis will not gather or report data (or use power).
@@ -2844,7 +2842,7 @@ void mpu6500SetStandbyXAccelEnabled(bool enabled)
*/
bool mpu6500GetStandbyYAccelEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_YA_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_YA_BIT, buffer);
return buffer[0];
}
/** Set Y-axis accelerometer standby enabled status.
@@ -2855,7 +2853,7 @@ bool mpu6500GetStandbyYAccelEnabled()
*/
void mpu6500SetStandbyYAccelEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_YA_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_YA_BIT, enabled);
}
/** Get Z-axis accelerometer standby enabled status.
* If enabled, the Z-axis will not gather or report data (or use power).
@@ -2865,7 +2863,7 @@ void mpu6500SetStandbyYAccelEnabled(bool enabled)
*/
bool mpu6500GetStandbyZAccelEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_ZA_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_ZA_BIT, buffer);
return buffer[0];
}
/** Set Z-axis accelerometer standby enabled status.
@@ -2876,7 +2874,7 @@ bool mpu6500GetStandbyZAccelEnabled()
*/
void mpu6500SetStandbyZAccelEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_ZA_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_ZA_BIT, enabled);
}
/** Get X-axis gyroscope standby enabled status.
* If enabled, the X-axis will not gather or report data (or use power).
@@ -2886,7 +2884,7 @@ void mpu6500SetStandbyZAccelEnabled(bool enabled)
*/
bool mpu6500GetStandbyXGyroEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_XG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_XG_BIT, buffer);
return buffer[0];
}
/** Set X-axis gyroscope standby enabled status.
@@ -2897,7 +2895,7 @@ bool mpu6500GetStandbyXGyroEnabled()
*/
void mpu6500SetStandbyXGyroEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_XG_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_XG_BIT, enabled);
}
/** Get Y-axis gyroscope standby enabled status.
* If enabled, the Y-axis will not gather or report data (or use power).
@@ -2907,7 +2905,7 @@ void mpu6500SetStandbyXGyroEnabled(bool enabled)
*/
bool mpu6500GetStandbyYGyroEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_YG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_YG_BIT, buffer);
return buffer[0];
}
/** Set Y-axis gyroscope standby enabled status.
@@ -2918,7 +2916,7 @@ bool mpu6500GetStandbyYGyroEnabled()
*/
void mpu6500SetStandbyYGyroEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_YG_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_YG_BIT, enabled);
}
/** Get Z-axis gyroscope standby enabled status.
* If enabled, the Z-axis will not gather or report data (or use power).
@@ -2928,7 +2926,7 @@ void mpu6500SetStandbyYGyroEnabled(bool enabled)
*/
bool mpu6500GetStandbyZGyroEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_ZG_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_ZG_BIT, buffer);
return buffer[0];
}
/** Set Z-axis gyroscope standby enabled status.
@@ -2939,7 +2937,7 @@ bool mpu6500GetStandbyZGyroEnabled()
*/
void mpu6500SetStandbyZGyroEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_ZG_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_PWR_MGMT_2, MPU6500_PWR2_STBY_ZG_BIT, enabled);
}
// FIFO_COUNT* registers
@@ -2953,7 +2951,7 @@ void mpu6500SetStandbyZGyroEnabled(bool enabled)
*/
uint16_t mpu6500GetFIFOCount()
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_FIFO_COUNTH, 2, buffer);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_FIFO_COUNTH, 2, buffer);
return (((uint16_t) buffer[0]) << 8) | buffer[1];
}
@@ -2986,12 +2984,12 @@ uint16_t mpu6500GetFIFOCount()
*/
uint8_t mpu6500GetFIFOByte()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_FIFO_R_W, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_FIFO_R_W, buffer);
return buffer[0];
}
void mpu6500GetFIFOBytes(uint8_t *data, uint8_t length)
{
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_FIFO_R_W, length, data);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_FIFO_R_W, length, data);
}
/** Write byte to FIFO buffer.
* @see getFIFOByte()
@@ -2999,7 +2997,7 @@ void mpu6500GetFIFOBytes(uint8_t *data, uint8_t length)
*/
void mpu6500SetFIFOByte(uint8_t data)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_FIFO_R_W, data);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_FIFO_R_W, data);
}
// WHO_AM_I register
@@ -3013,8 +3011,8 @@ void mpu6500SetFIFOByte(uint8_t data)
*/
uint8_t mpu6500GetDeviceID()
{
- // i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_WHO_AM_I, MPU6500_WHO_AM_I_BIT, MPU6500_WHO_AM_I_LENGTH,
- // buffer);
+ i2cdevReadBits(I2Cx, devAddr, MPU6500_RA_WHO_AM_I, MPU6500_WHO_AM_I_BIT, MPU6500_WHO_AM_I_LENGTH,
+ buffer);
return buffer[0];
}
/** Set Device ID.
@@ -3028,8 +3026,8 @@ uint8_t mpu6500GetDeviceID()
*/
void mpu6500SetDeviceID(uint8_t id)
{
- // i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_WHO_AM_I, MPU6500_WHO_AM_I_BIT, MPU6500_WHO_AM_I_LENGTH,
- // id);
+ i2cdevWriteBits(I2Cx, devAddr, MPU6500_RA_WHO_AM_I, MPU6500_WHO_AM_I_BIT, MPU6500_WHO_AM_I_LENGTH,
+ id);
}
// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
@@ -3040,53 +3038,53 @@ void mpu6500SetDeviceID(uint8_t id)
bool mpu6500GetIntPLLReadyEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_PLL_RDY_INT_BIT, buffer);
return buffer[0];
}
void mpu6500SetIntPLLReadyEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_PLL_RDY_INT_BIT, enabled);
}
bool mpu6500GetIntDMPEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_DMP_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_DMP_INT_BIT, buffer);
return buffer[0];
}
void mpu6500SetIntDMPEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_DMP_INT_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, MPU6500_INTERRUPT_DMP_INT_BIT, enabled);
}
// DMP_INT_STATUS
bool mpu6500GetDMPInt5Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_5_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_5_BIT, buffer);
return buffer[0];
}
bool mpu6500GetDMPInt4Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_4_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_4_BIT, buffer);
return buffer[0];
}
bool mpu6500GetDMPInt3Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_3_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_3_BIT, buffer);
return buffer[0];
}
bool mpu6500GetDMPInt2Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_2_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_2_BIT, buffer);
return buffer[0];
}
bool mpu6500GetDMPInt1Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_1_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_1_BIT, buffer);
return buffer[0];
}
bool mpu6500GetDMPInt0Status()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_0_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_DMP_INT_STATUS, MPU6500_DMPINT_0_BIT, buffer);
return buffer[0];
}
@@ -3094,12 +3092,12 @@ bool mpu6500GetDMPInt0Status()
bool mpu6500GetIntPLLReadyStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_PLL_RDY_INT_BIT, buffer);
return buffer[0];
}
bool mpu6500GetIntDMPStatus()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_DMP_INT_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_INT_STATUS, MPU6500_INTERRUPT_DMP_INT_BIT, buffer);
return buffer[0];
}
@@ -3107,47 +3105,47 @@ bool mpu6500GetIntDMPStatus()
bool mpu6500GetDMPEnabled()
{
- // i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_DMP_EN_BIT, buffer);
+ i2cdevReadBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_DMP_EN_BIT, buffer);
return buffer[0];
}
void mpu6500SetDMPEnabled(bool enabled)
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_DMP_EN_BIT, enabled);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_DMP_EN_BIT, enabled);
}
void mpu6500ResetDMP()
{
- // i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_DMP_RESET_BIT, 1);
+ i2cdevWriteBit(I2Cx, devAddr, MPU6500_RA_USER_CTRL, MPU6500_USERCTRL_DMP_RESET_BIT, 1);
}
// BANK_SEL register
void mpu6500SetMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank)
{
- // bank &= 0x1F;
- // if (userBank)
- // bank |= 0x20;
- // if (prefetchEnabled)
- // bank |= 0x40;
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_BANK_SEL, bank);
+ bank &= 0x1F;
+ if (userBank)
+ bank |= 0x20;
+ if (prefetchEnabled)
+ bank |= 0x40;
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_BANK_SEL, bank);
}
// MEM_START_ADDR register
void mpu6500SetMemoryStartAddress(uint8_t address)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_MEM_START_ADDR, address);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_MEM_START_ADDR, address);
}
// MEM_R_W register
uint8_t mpu6500ReadMemoryByte()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_MEM_R_W, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_MEM_R_W, buffer);
return buffer[0];
}
void mpu6500WriteMemoryByte(uint8_t data)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_MEM_R_W, data);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_MEM_R_W, data);
}
void mpu6500ReadMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address)
{
@@ -3156,37 +3154,37 @@ void mpu6500ReadMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint
uint8_t chunkSize;
uint16_t i;
- // for (i = 0; i < dataSize;)
- // {
- // // determine correct chunk size according to bank position and data size
- // chunkSize = MPU6500_DMP_MEMORY_CHUNK_SIZE;
+ for (i = 0; i < dataSize;)
+ {
+ // determine correct chunk size according to bank position and data size
+ chunkSize = MPU6500_DMP_MEMORY_CHUNK_SIZE;
- // // make sure we don't go past the data size
- // if (i + chunkSize > dataSize)
- // chunkSize = dataSize - i;
+ // make sure we don't go past the data size
+ if (i + chunkSize > dataSize)
+ chunkSize = dataSize - i;
- // // make sure this chunk doesn't go past the bank boundary (256 bytes)
- // if (chunkSize > 256 - address)
- // chunkSize = 256 - address;
+ // make sure this chunk doesn't go past the bank boundary (256 bytes)
+ if (chunkSize > 256 - address)
+ chunkSize = 256 - address;
- // // read the chunk of data as specified
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_MEM_R_W, chunkSize, data + i);
+ // read the chunk of data as specified
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_MEM_R_W, chunkSize, data + i);
- // // increase byte index by [chunkSize]
- // i += chunkSize;
+ // increase byte index by [chunkSize]
+ i += chunkSize;
- // // uint8_t automatically wraps to 0 at 256
- // address += chunkSize;
+ // uint8_t automatically wraps to 0 at 256
+ address += chunkSize;
- // // if we aren't done, update bank (if necessary) and address
- // if (i < dataSize)
- // {
- // if (address == 0)
- // bank++;
- // mpu6500SetMemoryBank(bank, true, true);
- // mpu6500SetMemoryStartAddress(address);
- // }
- // }
+ // if we aren't done, update bank (if necessary) and address
+ if (i < dataSize)
+ {
+ if (address == 0)
+ bank++;
+ mpu6500SetMemoryBank(bank, true, true);
+ mpu6500SetMemoryStartAddress(address);
+ }
+ }
}
bool mpu6500WriteMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address,
bool verify)
@@ -3196,76 +3194,76 @@ bool mpu6500WriteMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t ban
uint8_t *progBuffer;
uint16_t i;
- // mpu6500SetMemoryBank(bank, true, true);
- // mpu6500SetMemoryStartAddress(address);
-
- // for (i = 0; i < dataSize;)
- // {
- // // determine correct chunk size according to bank position and data size
- // chunkSize = MPU6500_DMP_MEMORY_CHUNK_SIZE;
-
- // // make sure we don't go past the data size
- // if (i + chunkSize > dataSize)
- // chunkSize = dataSize - i;
-
- // // make sure this chunk doesn't go past the bank boundary (256 bytes)
- // if (chunkSize > 256 - address)
- // chunkSize = 256 - address;
-
- // // write the chunk of data as specified
- // progBuffer = (uint8_t *) data + i;
-
- // i2cdevWriteReg8(I2Cx, devAddr, MPU6500_RA_MEM_R_W, chunkSize, progBuffer);
-
- // // verify data if needed
- // if (verify)
- // {
- // uint32_t j;
- // mpu6500SetMemoryBank(bank, true, true);
- // mpu6500SetMemoryStartAddress(address);
- // i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_MEM_R_W, chunkSize, verifyBuffer);
-
- // for (j = 0; j < chunkSize; j++)
- // {
- // if (progBuffer[j] != verifyBuffer[j])
- // {
- // /*Serial.print("Block write verification error, bank ");
- // Serial.print(bank, DEC);
- // Serial.print(", address ");
- // Serial.print(address, DEC);
- // Serial.print("!\nExpected:");
- // for (j = 0; j < chunkSize; j++) {
- // Serial.print(" 0x");
- // if (progBuffer[j] < 16) Serial.print("0");
- // Serial.print(progBuffer[j], HEX);
- // }
- // Serial.print("\nReceived:");
- // for (uint8_t j = 0; j < chunkSize; j++) {
- // Serial.print(" 0x");
- // if (verifyBuffer[i + j] < 16) Serial.print("0");
- // Serial.print(verifyBuffer[i + j], HEX);
- // }
- // Serial.print("\n");*/
- // return false;
- // }
- // }
- // }
-
- // // increase byte index by [chunkSize]
- // i += chunkSize;
-
- // // uint8_t automatically wraps to 0 at 256
- // address += chunkSize;
-
- // // if we aren't done, update bank (if necessary) and address
- // if (i < dataSize)
- // {
- // if (address == 0)
- // bank++;
- // mpu6500SetMemoryBank(bank, true, true);
- // mpu6500SetMemoryStartAddress(address);
- // }
- // }
+ mpu6500SetMemoryBank(bank, true, true);
+ mpu6500SetMemoryStartAddress(address);
+
+ for (i = 0; i < dataSize;)
+ {
+ // determine correct chunk size according to bank position and data size
+ chunkSize = MPU6500_DMP_MEMORY_CHUNK_SIZE;
+
+ // make sure we don't go past the data size
+ if (i + chunkSize > dataSize)
+ chunkSize = dataSize - i;
+
+ // make sure this chunk doesn't go past the bank boundary (256 bytes)
+ if (chunkSize > 256 - address)
+ chunkSize = 256 - address;
+
+ // write the chunk of data as specified
+ progBuffer = (uint8_t *) data + i;
+
+ i2cdevWriteReg8(I2Cx, devAddr, MPU6500_RA_MEM_R_W, chunkSize, progBuffer);
+
+ // verify data if needed
+ if (verify)
+ {
+ uint32_t j;
+ mpu6500SetMemoryBank(bank, true, true);
+ mpu6500SetMemoryStartAddress(address);
+ i2cdevReadReg8(I2Cx, devAddr, MPU6500_RA_MEM_R_W, chunkSize, verifyBuffer);
+
+ for (j = 0; j < chunkSize; j++)
+ {
+ if (progBuffer[j] != verifyBuffer[j])
+ {
+ /*Serial.print("Block write verification error, bank ");
+ Serial.print(bank, DEC);
+ Serial.print(", address ");
+ Serial.print(address, DEC);
+ Serial.print("!\nExpected:");
+ for (j = 0; j < chunkSize; j++) {
+ Serial.print(" 0x");
+ if (progBuffer[j] < 16) Serial.print("0");
+ Serial.print(progBuffer[j], HEX);
+ }
+ Serial.print("\nReceived:");
+ for (uint8_t j = 0; j < chunkSize; j++) {
+ Serial.print(" 0x");
+ if (verifyBuffer[i + j] < 16) Serial.print("0");
+ Serial.print(verifyBuffer[i + j], HEX);
+ }
+ Serial.print("\n");*/
+ return false;
+ }
+ }
+ }
+
+ // increase byte index by [chunkSize]
+ i += chunkSize;
+
+ // uint8_t automatically wraps to 0 at 256
+ address += chunkSize;
+
+ // if we aren't done, update bank (if necessary) and address
+ if (i < dataSize)
+ {
+ if (address == 0)
+ bank++;
+ mpu6500SetMemoryBank(bank, true, true);
+ mpu6500SetMemoryStartAddress(address);
+ }
+ }
return true;
}
bool mpu6500WriteProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank,
@@ -3276,66 +3274,66 @@ bool mpu6500WriteProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t
#define MPU6500_DMP_CONFIG_BLOCK_SIZE 6
bool mpu6500WriteDMPConfigurationSet(const uint8_t *data, uint16_t dataSize)
{
- // uint8_t *progBuffer, success, special;
- // uint16_t i;
-
- // // config set data is a long string of blocks with the following structure:
- // // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
- // uint8_t bank=0;
- // uint8_t offset=0;
- // uint8_t length=0;
- // for (i = 0; i < dataSize;)
- // {
- // bank = data[i++];
- // offset = data[i++];
- // length = data[i++];
- // }
-
- // // write data or perform special action
- // if (length > 0)
- // {
- // // regular block of data to write
- // /*Serial.print("Writing config block to bank ");
- // Serial.print(bank);
- // Serial.print(", offset ");
- // Serial.print(offset);
- // Serial.print(", length=");
- // Serial.println(length);*/
- // progBuffer = (uint8_t *) data + i;
- // success = mpu6500WriteMemoryBlock(progBuffer, length, bank, offset, true);
- // i += length;
- // }
- // else
- // {
- // // special instruction
- // // NOTE: this kind of behavior (what and when to do certain things)
- // // is totally undocumented. This code is in here based on observed
- // // behavior only, and exactly why (or even whether) it has to be here
- // // is anybody's guess for now.
- // special = data[i++];
- // /*Serial.print("Special command code ");
- // Serial.print(special, HEX);
- // Serial.println(" found...");*/
- // if (special == 0x01)
- // {
- // // enable DMP-related interrupts
- // mpu6500SetIntZeroMotionEnabled(true);
- // mpu6500SetIntFIFOBufferOverflowEnabled(true);
- // mpu6500SetIntDMPEnabled(true);
- // //i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, 0x32);
- // success = true;
- // }
- // else
- // {
- // // unknown special command
- // success = false;
- // }
- // }
-
- // if (!success)
- // {
- // return false; // uh oh
- // }
+ uint8_t *progBuffer, success, special;
+ uint16_t i;
+
+ // config set data is a long string of blocks with the following structure:
+ // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
+ uint8_t bank=0;
+ uint8_t offset=0;
+ uint8_t length=0;
+ for (i = 0; i < dataSize;)
+ {
+ bank = data[i++];
+ offset = data[i++];
+ length = data[i++];
+ }
+
+ // write data or perform special action
+ if (length > 0)
+ {
+ // regular block of data to write
+ /*Serial.print("Writing config block to bank ");
+ Serial.print(bank);
+ Serial.print(", offset ");
+ Serial.print(offset);
+ Serial.print(", length=");
+ Serial.println(length);*/
+ progBuffer = (uint8_t *) data + i;
+ success = mpu6500WriteMemoryBlock(progBuffer, length, bank, offset, true);
+ i += length;
+ }
+ else
+ {
+ // special instruction
+ // NOTE: this kind of behavior (what and when to do certain things)
+ // is totally undocumented. This code is in here based on observed
+ // behavior only, and exactly why (or even whether) it has to be here
+ // is anybody's guess for now.
+ special = data[i++];
+ /*Serial.print("Special command code ");
+ Serial.print(special, HEX);
+ Serial.println(" found...");*/
+ if (special == 0x01)
+ {
+ // enable DMP-related interrupts
+ mpu6500SetIntZeroMotionEnabled(true);
+ mpu6500SetIntFIFOBufferOverflowEnabled(true);
+ mpu6500SetIntDMPEnabled(true);
+ //i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_INT_ENABLE, 0x32);
+ success = true;
+ }
+ else
+ {
+ // unknown special command
+ success = false;
+ }
+ }
+
+ if (!success)
+ {
+ return false; // uh oh
+ }
return true;
}
@@ -3348,22 +3346,22 @@ bool mpu6500WriteProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize)
uint8_t mpu6500GetDMPConfig1()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_DMP_CFG_1, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_DMP_CFG_1, buffer);
return buffer[0];
}
void mpu6500SetDMPConfig1(uint8_t config)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_DMP_CFG_1, config);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_DMP_CFG_1, config);
}
// DMP_CFG_2 register
uint8_t mpu6500GetDMPConfig2()
{
- // i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_DMP_CFG_2, buffer);
+ i2cdevReadByte(I2Cx, devAddr, MPU6500_RA_DMP_CFG_2, buffer);
return buffer[0];
}
void mpu6500SetDMPConfig2(uint8_t config)
{
- // i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_DMP_CFG_2, config);
+ i2cdevWriteByte(I2Cx, devAddr, MPU6500_RA_DMP_CFG_2, config);
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ms5611.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ms5611.c
index 129c1b056..33dabcffe 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ms5611.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ms5611.c
@@ -70,109 +70,106 @@ static int32_t tempDeltaT;
bool ms5611Init(I2C_Dev *i2cPort)
{
- // if (isInit)
- // return true;
+ if (isInit)
+ return true;
- // I2Cx = i2cPort;
- // devAddr = MS5611_ADDR_CSB_LOW;
+ I2Cx = i2cPort;
+ devAddr = MS5611_ADDR_CSB_LOW;
- // ms5611Reset(); // reset the device to populate its internal PROM registers
- // vTaskDelay(M2T(5));
- // if (ms5611ReadPROM() == false) // reads the PROM into object variables for later use
- // {
- // return false;
- // }
+ ms5611Reset(); // reset the device to populate its internal PROM registers
+ vTaskDelay(M2T(5));
+ if (ms5611ReadPROM() == false) // reads the PROM into object variables for later use
+ {
+ return false;
+ }
- // isInit = true;
+ isInit = true;
return true;
}
bool ms5611SelfTest(void)
{
- // bool testStatus = true;
- // int32_t rawPress;
- // int32_t rawTemp;
- // int32_t deltaT;
- // float pressure;
- // float temperature;
-
- // if (!isInit)
- // return false;
-
- // ms5611StartConversion(MS5611_D1 + MS5611_OSR_4096);
- // vTaskDelay(M2T(CONVERSION_TIME_MS));
- // rawPress = ms5611GetConversion(MS5611_D1 + MS5611_OSR_4096);
-
- // ms5611StartConversion(MS5611_D2 + MS5611_OSR_4096);
- // vTaskDelay(M2T(CONVERSION_TIME_MS));
- // rawTemp = ms5611GetConversion(MS5611_D2 + MS5611_OSR_4096);
-
- // deltaT = ms5611CalcDeltaTemp(rawTemp);
- // temperature = ms5611CalcTemp(deltaT);
- // pressure = ms5611CalcPressure(rawPress, deltaT);
-
- // if (ms5611EvaluateSelfTest(MS5611_ST_PRESS_MIN, MS5611_ST_PRESS_MAX, pressure, "pressure") &&
- // ms5611EvaluateSelfTest(MS5611_ST_TEMP_MIN, MS5611_ST_TEMP_MAX, temperature, "temperature"))
- // {
- // DEBUG_PRINT("Self test [OK].\n");
- // }
- // else
- // {
- // testStatus = false;
- // }
-
- // return testStatus;
- return true;
+ bool testStatus = true;
+ int32_t rawPress;
+ int32_t rawTemp;
+ int32_t deltaT;
+ float pressure;
+ float temperature;
+
+ if (!isInit)
+ return false;
+
+ ms5611StartConversion(MS5611_D1 + MS5611_OSR_4096);
+ vTaskDelay(M2T(CONVERSION_TIME_MS));
+ rawPress = ms5611GetConversion(MS5611_D1 + MS5611_OSR_4096);
+
+ ms5611StartConversion(MS5611_D2 + MS5611_OSR_4096);
+ vTaskDelay(M2T(CONVERSION_TIME_MS));
+ rawTemp = ms5611GetConversion(MS5611_D2 + MS5611_OSR_4096);
+
+ deltaT = ms5611CalcDeltaTemp(rawTemp);
+ temperature = ms5611CalcTemp(deltaT);
+ pressure = ms5611CalcPressure(rawPress, deltaT);
+
+ if (ms5611EvaluateSelfTest(MS5611_ST_PRESS_MIN, MS5611_ST_PRESS_MAX, pressure, "pressure") &&
+ ms5611EvaluateSelfTest(MS5611_ST_TEMP_MIN, MS5611_ST_TEMP_MAX, temperature, "temperature"))
+ {
+ DEBUG_PRINT("Self test [OK].\n");
+ }
+ else
+ {
+ testStatus = false;
+ }
+
+ return testStatus;
}
bool ms5611EvaluateSelfTest(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;
- // }
+ 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;
}
float ms5611GetPressure(uint8_t osr)
{
- // // see datasheet page 7 for formulas
- // int32_t rawPress = ms5611RawPressure(osr);
- // int64_t dT = (int64_t)ms5611GetDeltaTemp(osr);
- // if (dT == 0)
- // {
- // return 0;
- // }
- // int64_t off = (((int64_t)calReg.off) << 16) + ((calReg.tco * dT) >> 7);
- // int64_t sens = (((int64_t)calReg.psens) << 15) + ((calReg.tcs * dT) >> 8);
- // if (rawPress != 0)
- // {
- // return ((((rawPress * sens) >> 21) - off) >> (15 - EXTRA_PRECISION))
- // / ((1 << EXTRA_PRECISION) * 100.0);
- // }
- // else
- // {
- // return 0;
- // }
-
- return 1.0;
+ // see datasheet page 7 for formulas
+ int32_t rawPress = ms5611RawPressure(osr);
+ int64_t dT = (int64_t)ms5611GetDeltaTemp(osr);
+ if (dT == 0)
+ {
+ return 0;
+ }
+ int64_t off = (((int64_t)calReg.off) << 16) + ((calReg.tco * dT) >> 7);
+ int64_t sens = (((int64_t)calReg.psens) << 15) + ((calReg.tcs * dT) >> 8);
+ if (rawPress != 0)
+ {
+ return ((((rawPress * sens) >> 21) - off) >> (15 - EXTRA_PRECISION))
+ / ((1 << EXTRA_PRECISION) * 100.0);
+ }
+ else
+ {
+ return 0;
+ }
}
float ms5611CalcPressure(int32_t rawPress, int32_t dT)
{
- // int64_t off;
- // int64_t sens;
+ int64_t off;
+ int64_t sens;
- // if (rawPress == 0 || dT == 0)
- // {
- // return 0;
- // }
+ if (rawPress == 0 || dT == 0)
+ {
+ return 0;
+ }
- // off = (((int64_t)calReg.off) << 16) + ((calReg.tco * (int64_t)dT) >> 7);
- // sens = (((int64_t)calReg.psens) << 15) + ((calReg.tcs * (int64_t)dT) >> 8);
+ off = (((int64_t)calReg.off) << 16) + ((calReg.tco * (int64_t)dT) >> 7);
+ sens = (((int64_t)calReg.psens) << 15) + ((calReg.tcs * (int64_t)dT) >> 8);
return ((((rawPress * sens) >> 21) - off) >> (15 - EXTRA_PRECISION))
/ ((1 << EXTRA_PRECISION) * 100.0);
@@ -180,10 +177,10 @@ float ms5611CalcPressure(int32_t rawPress, int32_t dT)
float ms5611GetTemperature(uint8_t osr)
{
- // // see datasheet page 7 for formulas
- // int32_t dT;
+ // see datasheet page 7 for formulas
+ int32_t dT;
- // dT = ms5611GetDeltaTemp(osr);
+ dT = ms5611GetDeltaTemp(osr);
if (dT != 0)
{
return ms5611CalcTemp(dT);
@@ -196,7 +193,7 @@ float ms5611GetTemperature(uint8_t osr)
int32_t ms5611GetDeltaTemp(uint8_t osr)
{
- // int32_t rawTemp = ms5611RawTemperature(osr);
+ int32_t rawTemp = ms5611RawTemperature(osr);
if (rawTemp != 0)
{
return ms5611CalcDeltaTemp(rawTemp);
@@ -209,92 +206,87 @@ int32_t ms5611GetDeltaTemp(uint8_t osr)
float ms5611CalcTemp(int32_t deltaT)
{
- // if (deltaT == 0)
- // {
- // return 0;
- // }
- // else
- // {
- // return (float)(((1 << EXTRA_PRECISION) * 2000)
- // + (((int64_t)deltaT * calReg.tsens) >> (23 - EXTRA_PRECISION)))
- // / ((1 << EXTRA_PRECISION)* 100.0f);
- // }
- return 1.0;
+ if (deltaT == 0)
+ {
+ return 0;
+ }
+ else
+ {
+ return (float)(((1 << EXTRA_PRECISION) * 2000)
+ + (((int64_t)deltaT * calReg.tsens) >> (23 - EXTRA_PRECISION)))
+ / ((1 << EXTRA_PRECISION)* 100.0f);
+ }
}
int32_t ms5611CalcDeltaTemp(int32_t rawTemp)
{
- // if (rawTemp == 0)
- // {
- // return 0;
- // }
- // else
- // {
- // return rawTemp - (((int32_t)calReg.tref) << 8);
- // }
- return 1.0;
+ if (rawTemp == 0)
+ {
+ return 0;
+ }
+ else
+ {
+ return rawTemp - (((int32_t)calReg.tref) << 8);
+ }
}
int32_t ms5611RawPressure(uint8_t osr)
{
- // uint32_t now = xTaskGetTickCount();
- // if (lastPresConv != 0 && (now - lastPresConv) >= CONVERSION_TIME_MS)
- // {
- // lastPresConv = 0;
- // return ms5611GetConversion(MS5611_D1 + osr);
- // }
- // else
- // {
- // if (lastPresConv == 0 && lastTempConv == 0)
- // {
- // ms5611StartConversion(MS5611_D1 + osr);
- // lastPresConv = now;
- // }
- // return 0;
- // }
- return 1.0;
+ uint32_t now = xTaskGetTickCount();
+ if (lastPresConv != 0 && (now - lastPresConv) >= CONVERSION_TIME_MS)
+ {
+ lastPresConv = 0;
+ return ms5611GetConversion(MS5611_D1 + osr);
+ }
+ else
+ {
+ if (lastPresConv == 0 && lastTempConv == 0)
+ {
+ ms5611StartConversion(MS5611_D1 + osr);
+ lastPresConv = now;
+ }
+ return 0;
+ }
}
int32_t ms5611RawTemperature(uint8_t osr)
{
- // uint32_t now = xTaskGetTickCount();
- // if (lastTempConv != 0 && (now - lastTempConv) >= CONVERSION_TIME_MS)
- // {
- // lastTempConv = 0;
- // tempCache = ms5611GetConversion(MS5611_D2 + osr);
- // return tempCache;
- // }
- // else
- // {
- // if (lastTempConv == 0 && lastPresConv == 0)
- // {
- // ms5611StartConversion(MS5611_D2 + osr);
- // lastTempConv = now;
- // }
- // return tempCache;
- // }
- return 1;
-
+ uint32_t now = xTaskGetTickCount();
+ if (lastTempConv != 0 && (now - lastTempConv) >= CONVERSION_TIME_MS)
+ {
+ lastTempConv = 0;
+ tempCache = ms5611GetConversion(MS5611_D2 + osr);
+ return tempCache;
+ }
+ else
+ {
+ if (lastTempConv == 0 && lastPresConv == 0)
+ {
+ ms5611StartConversion(MS5611_D2 + osr);
+ lastTempConv = now;
+ }
+ return tempCache;
+ }
}
// see page 11 of the datasheet
void ms5611StartConversion(uint8_t command)
{
// initialize pressure conversion
- // i2cdevWriteByte(I2Cx, devAddr, I2CDEV_NO_MEM_ADDR, command);
+ i2cdevWriteByte(I2Cx, devAddr, I2CDEV_NO_MEM_ADDR, command);
}
int32_t ms5611GetConversion(uint8_t command)
{
- // int32_t conversion = 0;
- // uint8_t buffer[MS5611_D1D2_SIZE];
+ int32_t conversion = 0;
+ uint8_t buffer[MS5611_D1D2_SIZE];
- // // start read sequence
- // i2cdevWriteByte(I2Cx, devAddr, I2CDEV_NO_MEM_ADDR, 0);
- // // Read conversion
- // i2cdevRead(I2Cx, devAddr, MS5611_D1D2_SIZE, buffer);
- // conversion = ((int32_t)buffer[0] << 16) |
- // ((int32_t)buffer[1] << 8) | buffer[2];
+ // start read sequence
+ i2cdevWriteByte(I2Cx, devAddr, I2CDEV_NO_MEM_ADDR, 0);
+ // Read conversion
+ i2cdevRead(I2Cx, devAddr, MS5611_D1D2_SIZE, buffer);
+ conversion = ((int32_t)buffer[0] << 16) |
+ ((int32_t)buffer[1] << 8) | buffer[2];
return conversion;
}
@@ -304,23 +296,23 @@ int32_t ms5611GetConversion(uint8_t command)
*/
bool ms5611ReadPROM()
{
- // uint8_t buffer[MS5611_PROM_REG_SIZE];
- // uint16_t* pCalRegU16 = (uint16_t*)&calReg;
- // int32_t i = 0;
- // bool status = false;
-
- // for (i = 0; i < MS5611_PROM_REG_COUNT; i++)
- // {
- // // start read sequence
- // status = i2cdevWriteByte(I2Cx, devAddr, I2CDEV_NO_MEM_ADDR,
- // MS5611_PROM_BASE_ADDR + (i * MS5611_PROM_REG_SIZE));
- // // Read conversion
- // if (status)
- // {
- // status = i2cdevRead(I2Cx, devAddr, MS5611_PROM_REG_SIZE, buffer);
- // pCalRegU16[i] = ((uint16_t)buffer[0] << 8) | buffer[1];
- // }
- // }
+ uint8_t buffer[MS5611_PROM_REG_SIZE];
+ uint16_t* pCalRegU16 = (uint16_t*)&calReg;
+ int32_t i = 0;
+ bool status = false;
+
+ for (i = 0; i < MS5611_PROM_REG_COUNT; i++)
+ {
+ // start read sequence
+ status = i2cdevWriteByte(I2Cx, devAddr, I2CDEV_NO_MEM_ADDR,
+ MS5611_PROM_BASE_ADDR + (i * MS5611_PROM_REG_SIZE));
+ // Read conversion
+ if (status)
+ {
+ status = i2cdevRead(I2Cx, devAddr, MS5611_PROM_REG_SIZE, buffer);
+ pCalRegU16[i] = ((uint16_t)buffer[0] << 8) | buffer[1];
+ }
+ }
return status;
}
@@ -331,7 +323,7 @@ bool ms5611ReadPROM()
*/
void ms5611Reset()
{
- // i2cdevWriteByte(I2Cx, devAddr, I2CDEV_NO_MEM_ADDR, MS5611_RESET);
+ i2cdevWriteByte(I2Cx, devAddr, I2CDEV_NO_MEM_ADDR, MS5611_RESET);
}
@@ -344,51 +336,51 @@ void ms5611Reset()
*/
void ms5611GetData(float* pressure, float* temperature, float* asl)
{
- // int32_t tempPressureRaw, tempTemperatureRaw;
- // static float savedPress, savedTemp;
-
- // // Dont reader faster than we can
- // uint32_t now = xTaskGetTickCount();
- // if ((now - lastConv) < CONVERSION_TIME_MS)
- // {
- // *pressure = savedPress;
- // *temperature = savedTemp;
- // return;
- // }
- // lastConv = now;
-
- // if (readState == 0)
- // {
- // // read temp
- // ++readState;
- // tempTemperatureRaw = ms5611GetConversion(MS5611_D2 + MS5611_OSR_DEFAULT);
- // tempDeltaT = ms5611CalcDeltaTemp(tempTemperatureRaw);
- // *temperature = ms5611CalcTemp(tempDeltaT);
- // savedTemp = *temperature;
- // *pressure = savedPress;
- // // cmd to read pressure
- // ms5611StartConversion(MS5611_D1 + MS5611_OSR_DEFAULT);
- // }
- // else
- // {
- // // read pressure
- // ++readState;
- // tempPressureRaw = ms5611GetConversion(MS5611_D1 + MS5611_OSR_DEFAULT);
- // *pressure = ms5611CalcPressure(tempPressureRaw, tempDeltaT);
- // savedPress = *pressure;
- // *asl = ms5611PressureToAltitude(pressure);
- // *temperature = savedTemp;
- // if (readState == PRESSURE_PER_TEMP){
- // // cmd to read temp
- // ms5611StartConversion(MS5611_D2 + MS5611_OSR_DEFAULT);
- // readState = 0;
- // }
- // else
- // {
- // // cmd to read pressure
- // ms5611StartConversion(MS5611_D1 + MS5611_OSR_DEFAULT);
- // }
- // }
+ int32_t tempPressureRaw, tempTemperatureRaw;
+ static float savedPress, savedTemp;
+
+ // Dont reader faster than we can
+ uint32_t now = xTaskGetTickCount();
+ if ((now - lastConv) < CONVERSION_TIME_MS)
+ {
+ *pressure = savedPress;
+ *temperature = savedTemp;
+ return;
+ }
+ lastConv = now;
+
+ if (readState == 0)
+ {
+ // read temp
+ ++readState;
+ tempTemperatureRaw = ms5611GetConversion(MS5611_D2 + MS5611_OSR_DEFAULT);
+ tempDeltaT = ms5611CalcDeltaTemp(tempTemperatureRaw);
+ *temperature = ms5611CalcTemp(tempDeltaT);
+ savedTemp = *temperature;
+ *pressure = savedPress;
+ // cmd to read pressure
+ ms5611StartConversion(MS5611_D1 + MS5611_OSR_DEFAULT);
+ }
+ else
+ {
+ // read pressure
+ ++readState;
+ tempPressureRaw = ms5611GetConversion(MS5611_D1 + MS5611_OSR_DEFAULT);
+ *pressure = ms5611CalcPressure(tempPressureRaw, tempDeltaT);
+ savedPress = *pressure;
+ *asl = ms5611PressureToAltitude(pressure);
+ *temperature = savedTemp;
+ if (readState == PRESSURE_PER_TEMP){
+ // cmd to read temp
+ ms5611StartConversion(MS5611_D2 + MS5611_OSR_DEFAULT);
+ readState = 0;
+ }
+ else
+ {
+ // cmd to read pressure
+ ms5611StartConversion(MS5611_D1 + MS5611_OSR_DEFAULT);
+ }
+ }
}
//TODO: pretty expensive function. Rather smooth the pressure estimates and only call this when needed
@@ -398,15 +390,14 @@ void ms5611GetData(float* pressure, float* temperature, float* asl)
*/
float ms5611PressureToAltitude(float* pressure/*, float* ground_pressure, float* ground_temp*/)
{
- // if (*pressure > 0)
- // {
- // //return (1.f - powf(*pressure / CONST_SEA_PRESSURE, CONST_PF)) * CONST_PF2;
- // //return ((powf((1015.7f / *pressure), CONST_PF) - 1.0f) * (25.f + 273.15f)) / 0.0065f;
- // return ((powf((1015.7f / *pressure), CONST_PF) - 1.0f) * (FIX_TEMP + 273.15f)) / 0.0065f;
- // }
- // else
- // {
- // return 0;
- // }
- return 1.0;
+ if (*pressure > 0)
+ {
+ //return (1.f - powf(*pressure / CONST_SEA_PRESSURE, CONST_PF)) * CONST_PF2;
+ //return ((powf((1015.7f / *pressure), CONST_PF) - 1.0f) * (25.f + 273.15f)) / 0.0065f;
+ return ((powf((1015.7f / *pressure), CONST_PF) - 1.0f) * (FIX_TEMP + 273.15f)) / 0.0065f;
+ }
+ else
+ {
+ return 0;
+ }
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nrf24l01.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nrf24l01.c
index eb6cec301..c50d3fa5c 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nrf24l01.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nrf24l01.c
@@ -105,14 +105,14 @@ static void (*interruptCb)(void) = NULL;
***********************/
static char spiSendByte(char byte)
{
- // /* Loop while DR register in not emplty */
- // while (SPI_I2S_GetFlagStatus(RADIO_SPI, SPI_I2S_FLAG_TXE) == RESET);
+ /* Loop while DR register in not emplty */
+ while (SPI_I2S_GetFlagStatus(RADIO_SPI, SPI_I2S_FLAG_TXE) == RESET);
- // /* Send byte through the SPI1 peripheral */
- // SPI_I2S_SendData(RADIO_SPI, byte);
+ /* Send byte through the SPI1 peripheral */
+ SPI_I2S_SendData(RADIO_SPI, byte);
- // /* Wait to receive a byte */
- // while (SPI_I2S_GetFlagStatus(RADIO_SPI, SPI_I2S_FLAG_RXNE) == RESET);
+ /* Wait to receive a byte */
+ while (SPI_I2S_GetFlagStatus(RADIO_SPI, SPI_I2S_FLAG_RXNE) == RESET);
/* Return the byte read from the SPI bus */
return SPI_I2S_ReceiveData(RADIO_SPI);
@@ -130,18 +130,18 @@ static char spiReceiveByte()
/* Read len bytes from a nRF24L register. 5 Bytes max */
unsigned char nrfReadReg(unsigned char address, char *buffer, int len)
{
- // unsigned char status;
- // int i;
+ unsigned char status;
+ int i;
- // RADIO_EN_CS();
+ RADIO_EN_CS();
- // /* Send the read command with the address */
- // status = spiSendByte( CMD_R_REG | (address&0x1F) );
- // /* Read LEN bytes */
- // for(i=0; i<len; i++)
- // buffer[i]=spiReceiveByte();
+ /* Send the read command with the address */
+ status = spiSendByte( CMD_R_REG | (address&0x1F) );
+ /* Read LEN bytes */
+ for(i=0; i<len; i++)
+ buffer[i]=spiReceiveByte();
- // RADIO_DIS_CS();
+ RADIO_DIS_CS();
return status;
}
@@ -149,18 +149,18 @@ unsigned char nrfReadReg(unsigned char address, char *buffer, int len)
/* Write len bytes a nRF24L register. 5 Bytes max */
unsigned char nrfWriteReg(unsigned char address, char *buffer, int len)
{
- // unsigned char status;
- // int i;
+ unsigned char status;
+ int i;
- // RADIO_EN_CS();
+ RADIO_EN_CS();
- // /* Send the write command with the address */
- // status = spiSendByte( CMD_W_REG | (address&0x1F) );
- // /* Write LEN bytes */
- // for(i=0; i<len; i++)
- // spiSendByte(buffer[i]);
+ /* Send the write command with the address */
+ status = spiSendByte( CMD_W_REG | (address&0x1F) );
+ /* Write LEN bytes */
+ for(i=0; i<len; i++)
+ spiSendByte(buffer[i]);
- // RADIO_DIS_CS();
+ RADIO_DIS_CS();
return status;
}
@@ -173,9 +173,9 @@ unsigned char nrfWrite1Reg(unsigned char address, char byte)
/* Read only one byte (useful for most of the reg.) */
unsigned char nrfRead1Reg(unsigned char address) {
- // char byte;
+ char byte;
- // nrfReadReg(address, &byte, 1);
+ nrfReadReg(address, &byte, 1);
return byte;
}
@@ -183,33 +183,33 @@ unsigned char nrfRead1Reg(unsigned char address) {
/* Sent the NOP command. Used to get the status byte */
unsigned char nrfNop()
{
- // unsigned char status;
+ unsigned char status;
- // RADIO_EN_CS();
- // status = spiSendByte(CMD_NOP);
- // RADIO_DIS_CS();
+ RADIO_EN_CS();
+ status = spiSendByte(CMD_NOP);
+ RADIO_DIS_CS();
return status;
}
unsigned char nrfFlushRx()
{
- // unsigned char status;
+ unsigned char status;
- // RADIO_EN_CS();
- // status = spiSendByte(CMD_FLUSH_RX);
- // RADIO_DIS_CS();
+ RADIO_EN_CS();
+ status = spiSendByte(CMD_FLUSH_RX);
+ RADIO_DIS_CS();
return status;
}
unsigned char nrfFlushTx()
{
- // unsigned char status;
+ unsigned char status;
- // RADIO_EN_CS();
- // status = spiSendByte(CMD_FLUSH_TX);
- // RADIO_DIS_CS();
+ RADIO_EN_CS();
+ status = spiSendByte(CMD_FLUSH_TX);
+ RADIO_DIS_CS();
return status;
}
@@ -217,24 +217,24 @@ unsigned char nrfFlushTx()
// Return the payload length
unsigned char nrfRxLength(unsigned int pipe)
{
- // unsigned char length;
+ unsigned char length;
- // RADIO_EN_CS();
- // spiSendByte(CMD_RX_PL_WID);
- // length = spiReceiveByte();
- // RADIO_DIS_CS();
+ RADIO_EN_CS();
+ spiSendByte(CMD_RX_PL_WID);
+ length = spiReceiveByte();
+ RADIO_DIS_CS();
return length;
}
unsigned char nrfActivate()
{
- // unsigned char status;
+ unsigned char status;
- // RADIO_EN_CS();
- // status = spiSendByte(CMD_ACTIVATE);
- // spiSendByte(ACTIVATE_DATA);
- // RADIO_DIS_CS();
+ RADIO_EN_CS();
+ status = spiSendByte(CMD_ACTIVATE);
+ spiSendByte(ACTIVATE_DATA);
+ RADIO_DIS_CS();
return status;
}
@@ -242,20 +242,20 @@ unsigned char nrfActivate()
// Write the ack payload of the pipe 0
unsigned char nrfWriteAck(unsigned int pipe, char *buffer, int len)
{
- // unsigned char status;
- // int i;
+ unsigned char status;
+ int i;
- // ASSERT(pipe<6);
+ ASSERT(pipe<6);
- // RADIO_EN_CS();
+ RADIO_EN_CS();
- // /* Send the read command with the address */
- // status = spiSendByte(CMD_W_ACK_PAYLOAD(pipe));
- // /* Read LEN bytes */
- // for(i=0; i<len; i++)
- // spiSendByte(buffer[i]);
+ /* Send the read command with the address */
+ status = spiSendByte(CMD_W_ACK_PAYLOAD(pipe));
+ /* Read LEN bytes */
+ for(i=0; i<len; i++)
+ spiSendByte(buffer[i]);
- // RADIO_DIS_CS();
+ RADIO_DIS_CS();
return status;
}
@@ -263,18 +263,18 @@ unsigned char nrfWriteAck(unsigned int pipe, char *buffer, int len)
// Read the RX payload
unsigned char nrfReadRX(char *buffer, int len)
{
- // unsigned char status;
- // int i;
+ unsigned char status;
+ int i;
- // RADIO_EN_CS();
+ RADIO_EN_CS();
- // /* Send the read command with the address */
- // status = spiSendByte(CMD_R_RX_PAYLOAD);
- // /* Read LEN bytes */
- // for(i=0; i<len; i++)
- // buffer[i]=spiReceiveByte();
+ /* Send the read command with the address */
+ status = spiSendByte(CMD_R_RX_PAYLOAD);
+ /* Read LEN bytes */
+ for(i=0; i<len; i++)
+ buffer[i]=spiReceiveByte();
- // RADIO_DIS_CS();
+ RADIO_DIS_CS();
return status;
}
@@ -282,150 +282,150 @@ unsigned char nrfReadRX(char *buffer, int len)
/* Interrupt service routine, call the interrupt callback */
void __attribute__((used)) EXTI9_Callback(void)
{
- // if (interruptCb)
- // {
- // interruptCb();
- // }
+ if (interruptCb)
+ {
+ interruptCb();
+ }
}
void nrfSetInterruptCallback(void (*cb)(void))
{
- // interruptCb = cb;
+ interruptCb = cb;
}
void nrfSetChannel(unsigned int channel)
{
- // if (channel<126)
- // nrfWrite1Reg(REG_RF_CH, channel);
+ if (channel<126)
+ nrfWrite1Reg(REG_RF_CH, channel);
}
void nrfSetDatarate(int datarate)
{
- // switch(datarate)
- // {
- // case RADIO_RATE_250K:
- // nrfWrite1Reg(REG_RF_SETUP, VAL_RF_SETUP_250K);
- // break;
- // case RADIO_RATE_1M:
- // nrfWrite1Reg(REG_RF_SETUP, VAL_RF_SETUP_1M);
- // break;
- // case RADIO_RATE_2M:
- // nrfWrite1Reg(REG_RF_SETUP, VAL_RF_SETUP_2M);
- // break;
- // }
+ switch(datarate)
+ {
+ case RADIO_RATE_250K:
+ nrfWrite1Reg(REG_RF_SETUP, VAL_RF_SETUP_250K);
+ break;
+ case RADIO_RATE_1M:
+ nrfWrite1Reg(REG_RF_SETUP, VAL_RF_SETUP_1M);
+ break;
+ case RADIO_RATE_2M:
+ nrfWrite1Reg(REG_RF_SETUP, VAL_RF_SETUP_2M);
+ break;
+ }
}
void nrfSetAddress(unsigned int pipe, char* address)
{
- // int len = 5;
+ int len = 5;
- // ASSERT(pipe<6);
+ ASSERT(pipe<6);
- // if (pipe > 1)
- // len = 1;
+ if (pipe > 1)
+ len = 1;
- // nrfWriteReg(REG_RX_ADDR_P0 + pipe, address, len);
+ nrfWriteReg(REG_RX_ADDR_P0 + pipe, address, len);
}
void nrfSetEnable(bool enable)
{
- // if (enable)
- // {
- // RADIO_EN_CE();
- // }
- // else
- // {
- // RADIO_DIS_CE();
- // }
+ if (enable)
+ {
+ RADIO_EN_CE();
+ }
+ else
+ {
+ RADIO_DIS_CE();
+ }
}
unsigned char nrfGetStatus()
{
- // return nrfNop();
+ return nrfNop();
}
/* Initialisation */
void nrfInit(void)
{
- // SPI_InitTypeDef SPI_InitStructure;
- // EXTI_InitTypeDef EXTI_InitStructure;
- // GPIO_InitTypeDef GPIO_InitStructure;
-
- // if (isInit)
- // return;
-
- // /* Enable SPI and GPIO clocks */
- // RCC_APB2PeriphClockCmd(RADIO_GPIO_SPI_CLK | RADIO_GPIO_CS_PERIF |
- // RADIO_GPIO_CE_PERIF | RADIO_GPIO_IRQ_PERIF, ENABLE);
-
- // /* Enable SPI and GPIO clocks */
- // RCC_APB1PeriphClockCmd(RADIO_SPI_CLK, ENABLE);
-
- // /* Configure main clock */
- // GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_CLK;
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
- // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- // GPIO_Init(RADIO_GPIO_CLK_PORT, &GPIO_InitStructure);
-
- // /* Configure SPI pins: SCK, MISO and MOSI */
- // GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_SPI_SCK | RADIO_GPIO_SPI_MOSI;
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
- // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- // GPIO_Init(RADIO_GPIO_SPI_PORT, &GPIO_InitStructure);
-
- // //* Configure MISO */
- // GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_SPI_MISO;
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
- // GPIO_Init(RADIO_GPIO_SPI_PORT, &GPIO_InitStructure);
-
- // /* Configure I/O for the Chip select */
- // GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_CS;
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- // GPIO_Init(RADIO_GPIO_CS_PORT, &GPIO_InitStructure);
-
- // /* Configure the interruption (EXTI Source) */
- // GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_IRQ;
- // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
- // GPIO_Init(RADIO_GPIO_IRQ_PORT, &GPIO_InitStructure);
-
- // GPIO_EXTILineConfig(RADIO_GPIO_IRQ_SRC_PORT, RADIO_GPIO_IRQ_SRC);
- // EXTI_InitStructure.EXTI_Line = RADIO_GPIO_IRQ_LINE;
- // EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
- // EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
- // EXTI_InitStructure.EXTI_LineCmd = ENABLE;
- // EXTI_Init(&EXTI_InitStructure);
-
- // // Clock the radio with 16MHz
- // RCC_MCOConfig(RCC_MCO_HSE);
-
- // /* disable the chip select */
- // RADIO_DIS_CS();
-
- // /* Configure I/O for the Chip Enable */
- // GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_CE;
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- // GPIO_Init(RADIO_GPIO_CE_PORT, &GPIO_InitStructure);
-
- // /* disable the chip enable */
- // RADIO_DIS_CE();
-
- // /* SPI configuration */
- // SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
- // SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
- // SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
- // SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
- // SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
- // SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
- // SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
- // SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
- // SPI_InitStructure.SPI_CRCPolynomial = 7;
- // SPI_Init(RADIO_SPI, &SPI_InitStructure);
-
- // /* Enable the SPI */
- // SPI_Cmd(RADIO_SPI, ENABLE);
+ SPI_InitTypeDef SPI_InitStructure;
+ EXTI_InitTypeDef EXTI_InitStructure;
+ GPIO_InitTypeDef GPIO_InitStructure;
+
+ if (isInit)
+ return;
+
+ /* Enable SPI and GPIO clocks */
+ RCC_APB2PeriphClockCmd(RADIO_GPIO_SPI_CLK | RADIO_GPIO_CS_PERIF |
+ RADIO_GPIO_CE_PERIF | RADIO_GPIO_IRQ_PERIF, ENABLE);
+
+ /* Enable SPI and GPIO clocks */
+ RCC_APB1PeriphClockCmd(RADIO_SPI_CLK, ENABLE);
+
+ /* Configure main clock */
+ GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_CLK;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(RADIO_GPIO_CLK_PORT, &GPIO_InitStructure);
+
+ /* Configure SPI pins: SCK, MISO and MOSI */
+ GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_SPI_SCK | RADIO_GPIO_SPI_MOSI;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(RADIO_GPIO_SPI_PORT, &GPIO_InitStructure);
+
+ //* Configure MISO */
+ GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_SPI_MISO;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_Init(RADIO_GPIO_SPI_PORT, &GPIO_InitStructure);
+
+ /* Configure I/O for the Chip select */
+ GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_CS;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+ GPIO_Init(RADIO_GPIO_CS_PORT, &GPIO_InitStructure);
+
+ /* Configure the interruption (EXTI Source) */
+ GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_IRQ;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_Init(RADIO_GPIO_IRQ_PORT, &GPIO_InitStructure);
+
+ GPIO_EXTILineConfig(RADIO_GPIO_IRQ_SRC_PORT, RADIO_GPIO_IRQ_SRC);
+ EXTI_InitStructure.EXTI_Line = RADIO_GPIO_IRQ_LINE;
+ EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+ EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
+ EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+ EXTI_Init(&EXTI_InitStructure);
+
+ // Clock the radio with 16MHz
+ RCC_MCOConfig(RCC_MCO_HSE);
+
+ /* disable the chip select */
+ RADIO_DIS_CS();
+
+ /* Configure I/O for the Chip Enable */
+ GPIO_InitStructure.GPIO_Pin = RADIO_GPIO_CE;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+ GPIO_Init(RADIO_GPIO_CE_PORT, &GPIO_InitStructure);
+
+ /* disable the chip enable */
+ RADIO_DIS_CE();
+
+ /* SPI configuration */
+ SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
+ SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
+ SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
+ SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
+ SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
+ SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
+ SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
+ SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
+ SPI_InitStructure.SPI_CRCPolynomial = 7;
+ SPI_Init(RADIO_SPI, &SPI_InitStructure);
+
+ /* Enable the SPI */
+ SPI_Cmd(RADIO_SPI, ENABLE);
- // isInit = true;
+ isInit = true;
}
bool nrfTest(void)
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nvic.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nvic.c
index 9cc83b178..a5c777c0e 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nvic.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/nvic.c
@@ -36,8 +36,7 @@
void nvicInit(void)
{
- //COMMENTED FIRMWARE
- //NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
+ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
}
/**
@@ -86,84 +85,81 @@ void DONT_DISCARD HardFault_Handler(void)
// asm (" MOVS r0, #1 \n"
// " LDM r0,{r1-r2} \n"
// " BX LR; \n");
- //COMMENTED FIRMWARE
- // asm( "TST LR, #4 \n"
- // "ITE EQ \n"
- // "MRSEQ R0, MSP \n"
- // "MRSNE R0, PSP \n"
- // "B printHardFault");
+ asm( "TST LR, #4 \n"
+ "ITE EQ \n"
+ "MRSEQ R0, MSP \n"
+ "MRSNE R0, PSP \n"
+ "B printHardFault");
}
void DONT_DISCARD printHardFault(uint32_t* hardfaultArgs)
{
- //COMMENTED FIRMWARE
- // unsigned int stacked_r0;
- // unsigned int stacked_r1;
- // unsigned int stacked_r2;
- // unsigned int stacked_r3;
- // unsigned int stacked_r12;
- // unsigned int stacked_lr;
- // unsigned int stacked_pc;
- // unsigned int stacked_psr;
-
- // stacked_r0 = ((unsigned long) hardfaultArgs[0]);
- // stacked_r1 = ((unsigned long) hardfaultArgs[1]);
- // stacked_r2 = ((unsigned long) hardfaultArgs[2]);
- // stacked_r3 = ((unsigned long) hardfaultArgs[3]);
-
- // stacked_r12 = ((unsigned long) hardfaultArgs[4]);
- // stacked_lr = ((unsigned long) hardfaultArgs[5]);
- // stacked_pc = ((unsigned long) hardfaultArgs[6]);
- // stacked_psr = ((unsigned long) hardfaultArgs[7]);
-
-
- // UART_PRINT("[Hard fault handler]\n");
- // UART_PRINT("R0 = %x\n", stacked_r0);
- // UART_PRINT("R1 = %x\n", stacked_r1);
- // UART_PRINT("R2 = %x\n", stacked_r2);
- // UART_PRINT("R3 = %x\n", stacked_r3);
- // UART_PRINT("R12 = %x\n", stacked_r12);
- // UART_PRINT("LR = %x\n", stacked_lr);
- // UART_PRINT("PC = %x\n", stacked_pc);
- // UART_PRINT("PSR = %x\n", stacked_psr);
- // UART_PRINT("BFAR = %x\n", (*((volatile unsigned int *)(0xE000ED38))));
- // UART_PRINT("CFSR = %x\n", (*((volatile unsigned int *)(0xE000ED28))));
- // UART_PRINT("HFSR = %x\n", (*((volatile unsigned int *)(0xE000ED2C))));
- // UART_PRINT("DFSR = %x\n", (*((volatile unsigned int *)(0xE000ED30))));
- // UART_PRINT("AFSR = %x\n", (*((volatile unsigned int *)(0xE000ED3C))));
-
- // ledClearAll();
- // ledSet(ERR_LED1, 1);
- // ledSet(ERR_LED2, 1);
- // powerStop();
-
- // storeAssertHardfaultData(
- // stacked_r0,
- // stacked_r1,
- // stacked_r2,
- // stacked_r3,
- // stacked_r12,
- // stacked_lr,
- // stacked_pc,
- // stacked_psr);
- // while (1)
- // {}
+ unsigned int stacked_r0;
+ unsigned int stacked_r1;
+ unsigned int stacked_r2;
+ unsigned int stacked_r3;
+ unsigned int stacked_r12;
+ unsigned int stacked_lr;
+ unsigned int stacked_pc;
+ unsigned int stacked_psr;
+
+ stacked_r0 = ((unsigned long) hardfaultArgs[0]);
+ stacked_r1 = ((unsigned long) hardfaultArgs[1]);
+ stacked_r2 = ((unsigned long) hardfaultArgs[2]);
+ stacked_r3 = ((unsigned long) hardfaultArgs[3]);
+
+ stacked_r12 = ((unsigned long) hardfaultArgs[4]);
+ stacked_lr = ((unsigned long) hardfaultArgs[5]);
+ stacked_pc = ((unsigned long) hardfaultArgs[6]);
+ stacked_psr = ((unsigned long) hardfaultArgs[7]);
+
+
+ UART_PRINT("[Hard fault handler]\n");
+ UART_PRINT("R0 = %x\n", stacked_r0);
+ UART_PRINT("R1 = %x\n", stacked_r1);
+ UART_PRINT("R2 = %x\n", stacked_r2);
+ UART_PRINT("R3 = %x\n", stacked_r3);
+ UART_PRINT("R12 = %x\n", stacked_r12);
+ UART_PRINT("LR = %x\n", stacked_lr);
+ UART_PRINT("PC = %x\n", stacked_pc);
+ UART_PRINT("PSR = %x\n", stacked_psr);
+ UART_PRINT("BFAR = %x\n", (*((volatile unsigned int *)(0xE000ED38))));
+ UART_PRINT("CFSR = %x\n", (*((volatile unsigned int *)(0xE000ED28))));
+ UART_PRINT("HFSR = %x\n", (*((volatile unsigned int *)(0xE000ED2C))));
+ UART_PRINT("DFSR = %x\n", (*((volatile unsigned int *)(0xE000ED30))));
+ UART_PRINT("AFSR = %x\n", (*((volatile unsigned int *)(0xE000ED3C))));
+
+ ledClearAll();
+ ledSet(ERR_LED1, 1);
+ ledSet(ERR_LED2, 1);
+ powerStop();
+
+ storeAssertHardfaultData(
+ stacked_r0,
+ stacked_r1,
+ stacked_r2,
+ stacked_r3,
+ stacked_r12,
+ stacked_lr,
+ stacked_pc,
+ stacked_psr);
+ while (1)
+ {}
}
/**
* @brief This function handles Memory Manage exception.
*/
void DONT_DISCARD MemManage_Handler(void)
{
- //COMMENTED FIRMWARE
- // /* Go to infinite loop when Memory Manage exception occurs */
- // ledClearAll();
- // ledSet(ERR_LED1, 1);
- // ledSet(ERR_LED2, 1);
- // powerStop();
-
- // storeAssertTextData("MemManage");
- // while (1)
- // {}
+ /* Go to infinite loop when Memory Manage exception occurs */
+ ledClearAll();
+ ledSet(ERR_LED1, 1);
+ ledSet(ERR_LED2, 1);
+ powerStop();
+
+ storeAssertTextData("MemManage");
+ while (1)
+ {}
}
/**
@@ -171,16 +167,15 @@ void DONT_DISCARD MemManage_Handler(void)
*/
void DONT_DISCARD BusFault_Handler(void)
{
- //COMMENTED FIRMWARE
/* Go to infinite loop when Bus Fault exception occurs */
- // ledClearAll();
- // ledSet(ERR_LED1, 1);
- // ledSet(ERR_LED2, 1);
- // powerStop();
-
- // storeAssertTextData("BusFault");
- // while (1)
- // {}
+ ledClearAll();
+ ledSet(ERR_LED1, 1);
+ ledSet(ERR_LED2, 1);
+ powerStop();
+
+ storeAssertTextData("BusFault");
+ while (1)
+ {}
}
/**
@@ -188,16 +183,15 @@ void DONT_DISCARD BusFault_Handler(void)
*/
void DONT_DISCARD UsageFault_Handler(void)
{
- //COMMENTED FIRMWARE
- // /* Go to infinite loop when Usage Fault exception occurs */
- // ledClearAll();
- // ledSet(ERR_LED1, 1);
- // ledSet(ERR_LED2, 1);
- // powerStop();
-
- // storeAssertTextData("UsageFault");
- // while (1)
- // {}
+ /* Go to infinite loop when Usage Fault exception occurs */
+ ledClearAll();
+ ledSet(ERR_LED1, 1);
+ ledSet(ERR_LED2, 1);
+ powerStop();
+
+ storeAssertTextData("UsageFault");
+ while (1)
+ {}
}
/**
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pca9685.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pca9685.c
index 92a19eeee..abab44e8d 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pca9685.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pca9685.c
@@ -92,9 +92,8 @@ static void durationToBytes(uint16_t duration, uint8_t *bytes)
static uint16_t dutyToDuration(float duty)
{
- //COMMENTED FIRMWARE
- // duty = fmax(duty, 0.0f);
- // duty = fmin(duty, 1.0f);
+ duty = fmax(duty, 0.0f);
+ duty = fmin(duty, 1.0f);
return duty * 4096.0f;
}
@@ -178,59 +177,55 @@ STATIC_MEM_QUEUE_ALLOC(queue, 1, sizeof(struct asyncRequest));
static void asyncTask(__attribute__((unused)) void *param)
{
-
- // while (true) {
- // BaseType_t ok = xQueueReceive(queue, &reqPop, portMAX_DELAY);
- // if (ok == pdTRUE) {
- // // blocking message send.
- // pca9685setDurations(
- // reqPop.addr, reqPop.chanBegin, reqPop.nChan, reqPop.durations);
- // }
- // }
+ while (true) {
+ BaseType_t ok = xQueueReceive(queue, &reqPop, portMAX_DELAY);
+ if (ok == pdTRUE) {
+ // blocking message send.
+ pca9685setDurations(
+ reqPop.addr, reqPop.chanBegin, reqPop.nChan, reqPop.durations);
+ }
+ }
}
bool pca9685startAsyncTask()
{
- //COMMENTED FIRMWARE
- // queue = STATIC_MEM_QUEUE_CREATE(queue);
- // if (queue == 0) {
- // return false;
- // }
+ queue = STATIC_MEM_QUEUE_CREATE(queue);
+ if (queue == 0) {
+ return false;
+ }
- // task = STATIC_MEM_TASK_CREATE(task, asyncTask, PCA9685_TASK_NAME, NULL, PCA9685_TASK_PRI);
+ task = STATIC_MEM_TASK_CREATE(task, asyncTask, PCA9685_TASK_NAME, NULL, PCA9685_TASK_PRI);
return true;
}
bool pca9685setDutiesAsync(
int addr, int chanBegin, int nChan, float const *duties)
{
- //COMMENTED FIRMWARE
- // struct asyncRequest reqPush = {
- // .addr = addr,
- // .chanBegin = chanBegin,
- // .nChan = nChan,
- // };
- // for (int i = 0; i < nChan; ++i) {
- // reqPush.durations[i] = dutyToDuration(duties[i]);
- // }
- // // drop message unless queue is empty!
- // xQueueSend(queue, &reqPush, 0);
+ struct asyncRequest reqPush = {
+ .addr = addr,
+ .chanBegin = chanBegin,
+ .nChan = nChan,
+ };
+ for (int i = 0; i < nChan; ++i) {
+ reqPush.durations[i] = dutyToDuration(duties[i]);
+ }
+ // drop message unless queue is empty!
+ xQueueSend(queue, &reqPush, 0);
return true;
}
bool pca9685setDurationsAsync(
int addr, int chanBegin, int nChan, uint16_t const *durations)
{
- //COMMENTED FIRMWARE
- // struct asyncRequest reqPush = {
- // .addr = addr,
- // .chanBegin = chanBegin,
- // .nChan = nChan,
- // };
- // for (int i = 0; i < nChan; ++i) {
- // reqPush.durations[i] = durations[i];
- // }
- // // drop message unless queue is empty!
- // xQueueSend(queue, &reqPush, 0);
+ struct asyncRequest reqPush = {
+ .addr = addr,
+ .chanBegin = chanBegin,
+ .nChan = nChan,
+ };
+ for (int i = 0; i < nChan; ++i) {
+ reqPush.durations[i] = durations[i];
+ }
+ // drop message unless queue is empty!
+ xQueueSend(queue, &reqPush, 0);
return true;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/piezo.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/piezo.c
index 78acc40f6..e28ef7129 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/piezo.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/piezo.c
@@ -71,61 +71,61 @@ void piezoInit()
{
if (isInit)
return;
- //COMMENTED FIRMWARE
- // //Init structures
- // GPIO_InitTypeDef GPIO_InitStructure;
- // TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- // TIM_OCInitTypeDef TIM_OCInitStructure;
-
- // //Clock the gpio and the timers
- // RCC_AHB1PeriphClockCmd(PIEZO_GPIO_POS_PERIF | PIEZO_GPIO_NEG_PERIF, ENABLE);
- // RCC_APB1PeriphClockCmd(PIEZO_TIM_PERIF, ENABLE);
-
- // // Configure the GPIO for the timer output
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
- // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
- // GPIO_InitStructure.GPIO_Pin = PIEZO_GPIO_POS_PIN;
- // GPIO_Init(PIEZO_GPIO_POS_PORT, &GPIO_InitStructure);
- // GPIO_InitStructure.GPIO_Pin = PIEZO_GPIO_NEG_PIN;
- // GPIO_Init(PIEZO_GPIO_NEG_PORT, &GPIO_InitStructure);
-
- // //Map timers to alternate functions
- // GPIO_PinAFConfig(PIEZO_GPIO_POS_PORT, PIEZO_GPIO_AF_POS_PIN, PIEZO_GPIO_AF_POS);
- // GPIO_PinAFConfig(PIEZO_GPIO_NEG_PORT, PIEZO_GPIO_AF_NEG_PIN, PIEZO_GPIO_AF_NEG);
-
- // //Timer configuration
- // TIM_TimeBaseStructure.TIM_Period = PIEZO_PWM_PERIOD;
- // TIM_TimeBaseStructure.TIM_Prescaler = PIEZO_PWM_PRESCALE;
- // TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- // TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- // TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
- // TIM_TimeBaseInit(PIEZO_TIM, &TIM_TimeBaseStructure);
-
- // // PWM channels configuration
- // TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
- // TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
- // TIM_OCInitStructure.TIM_Pulse = 0;
- // TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
- // TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
-
- // // Configure OC3
- // TIM_OC3Init(PIEZO_TIM, &TIM_OCInitStructure);
- // TIM_OC3PreloadConfig(PIEZO_TIM, TIM_OCPreload_Enable);
-
- // // Configure OC4 inverted
- // TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
- // TIM_OC4Init(PIEZO_TIM, &TIM_OCInitStructure);
- // TIM_OC4PreloadConfig(PIEZO_TIM, TIM_OCPreload_Enable);
-
- // //Enable the timer PWM outputs
- // TIM_CtrlPWMOutputs(PIEZO_TIM, ENABLE);
- // TIM_SetCompare3(PIEZO_TIM, 0x00);
- // TIM_SetCompare4(PIEZO_TIM, 0x00);
-
- // //Enable the timer
- // TIM_Cmd(PIEZO_TIM, ENABLE);
+
+ //Init structures
+ GPIO_InitTypeDef GPIO_InitStructure;
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ TIM_OCInitTypeDef TIM_OCInitStructure;
+
+ //Clock the gpio and the timers
+ RCC_AHB1PeriphClockCmd(PIEZO_GPIO_POS_PERIF | PIEZO_GPIO_NEG_PERIF, ENABLE);
+ RCC_APB1PeriphClockCmd(PIEZO_TIM_PERIF, ENABLE);
+
+ // Configure the GPIO for the timer output
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+ GPIO_InitStructure.GPIO_Pin = PIEZO_GPIO_POS_PIN;
+ GPIO_Init(PIEZO_GPIO_POS_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = PIEZO_GPIO_NEG_PIN;
+ GPIO_Init(PIEZO_GPIO_NEG_PORT, &GPIO_InitStructure);
+
+ //Map timers to alternate functions
+ GPIO_PinAFConfig(PIEZO_GPIO_POS_PORT, PIEZO_GPIO_AF_POS_PIN, PIEZO_GPIO_AF_POS);
+ GPIO_PinAFConfig(PIEZO_GPIO_NEG_PORT, PIEZO_GPIO_AF_NEG_PIN, PIEZO_GPIO_AF_NEG);
+
+ //Timer configuration
+ TIM_TimeBaseStructure.TIM_Period = PIEZO_PWM_PERIOD;
+ TIM_TimeBaseStructure.TIM_Prescaler = PIEZO_PWM_PRESCALE;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+ TIM_TimeBaseInit(PIEZO_TIM, &TIM_TimeBaseStructure);
+
+ // PWM channels configuration
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+ TIM_OCInitStructure.TIM_Pulse = 0;
+ TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
+ TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
+
+ // Configure OC3
+ TIM_OC3Init(PIEZO_TIM, &TIM_OCInitStructure);
+ TIM_OC3PreloadConfig(PIEZO_TIM, TIM_OCPreload_Enable);
+
+ // Configure OC4 inverted
+ TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+ TIM_OC4Init(PIEZO_TIM, &TIM_OCInitStructure);
+ TIM_OC4PreloadConfig(PIEZO_TIM, TIM_OCPreload_Enable);
+
+ //Enable the timer PWM outputs
+ TIM_CtrlPWMOutputs(PIEZO_TIM, ENABLE);
+ TIM_SetCompare3(PIEZO_TIM, 0x00);
+ TIM_SetCompare4(PIEZO_TIM, 0x00);
+
+ //Enable the timer
+ TIM_Cmd(PIEZO_TIM, ENABLE);
isInit = true;
}
@@ -137,13 +137,11 @@ bool piezoTest(void)
void piezoSetRatio(uint8_t ratio)
{
- //COMMENTED FIRMWARE
- // TIM_SetCompare3(PIEZO_TIM, ratio);
- // TIM_SetCompare4(PIEZO_TIM, ratio);
+ TIM_SetCompare3(PIEZO_TIM, ratio);
+ TIM_SetCompare4(PIEZO_TIM, ratio);
}
void piezoSetFreq(uint16_t freq)
{
- //COMMENTED FIRMWARE
- //TIM_PrescalerConfig(PIEZO_TIM, (PIEZO_BASE_FREQ/freq), TIM_PSCReloadMode_Update);
+ TIM_PrescalerConfig(PIEZO_TIM, (PIEZO_BASE_FREQ/freq), TIM_PSCReloadMode_Update);
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pmw3901.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pmw3901.c
index 4777db247..111af0eca 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pmw3901.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/pmw3901.c
@@ -41,136 +41,133 @@ static bool isInit = false;
static void registerWrite(const deckPin_t csPin, uint8_t reg, uint8_t value)
{
- //COMMENTED FIRMWARE
- // // Set MSB to 1 for write
- // reg |= 0x80u;
+ // Set MSB to 1 for write
+ reg |= 0x80u;
- // spiBeginTransaction(SPI_BAUDRATE_2MHZ);
- // digitalWrite(csPin, LOW);
+ spiBeginTransaction(SPI_BAUDRATE_2MHZ);
+ digitalWrite(csPin, LOW);
- // sleepus(50);
+ sleepus(50);
- // spiExchange(1, ®, ®);
- // sleepus(50);
- // spiExchange(1, &value, &value);
+ spiExchange(1, ®, ®);
+ sleepus(50);
+ spiExchange(1, &value, &value);
- // sleepus(50);
+ sleepus(50);
- // digitalWrite(csPin, HIGH);
- // spiEndTransaction();
- // sleepus(200);
+ digitalWrite(csPin, HIGH);
+ spiEndTransaction();
+ sleepus(200);
}
static uint8_t registerRead(const deckPin_t csPin, uint8_t reg)
{
uint8_t data = 0;
- //COMMENTED FIRMWARE
- // uint8_t dummy = 0;
+ uint8_t dummy = 0;
- // // Set MSB to 0 for read
- // reg &= ~0x80u;
+ // Set MSB to 0 for read
+ reg &= ~0x80u;
- // spiBeginTransaction(SPI_BAUDRATE_2MHZ);
- // digitalWrite(csPin, LOW);
+ spiBeginTransaction(SPI_BAUDRATE_2MHZ);
+ digitalWrite(csPin, LOW);
- // sleepus(50);
+ sleepus(50);
- // spiExchange(1, ®, ®);
- // sleepus(500);
- // spiExchange(1, &dummy, &data);
+ spiExchange(1, ®, ®);
+ sleepus(500);
+ spiExchange(1, &dummy, &data);
- // sleepus(50);
+ sleepus(50);
- // digitalWrite(csPin, HIGH);
- // spiEndTransaction();
- // sleepus(200);
+ digitalWrite(csPin, HIGH);
+ spiEndTransaction();
+ sleepus(200);
return data;
}
static void InitRegisters(const deckPin_t csPin)
{
- //COMMENTED FIRMWARE
- // registerWrite(csPin, 0x7F, 0x00);
- // registerWrite(csPin, 0x61, 0xAD);
- // registerWrite(csPin, 0x7F, 0x03);
- // registerWrite(csPin, 0x40, 0x00);
- // registerWrite(csPin, 0x7F, 0x05);
- // registerWrite(csPin, 0x41, 0xB3);
- // registerWrite(csPin, 0x43, 0xF1);
- // registerWrite(csPin, 0x45, 0x14);
- // registerWrite(csPin, 0x5B, 0x32);
- // registerWrite(csPin, 0x5F, 0x34);
- // registerWrite(csPin, 0x7B, 0x08);
- // registerWrite(csPin, 0x7F, 0x06);
- // registerWrite(csPin, 0x44, 0x1B);
- // registerWrite(csPin, 0x40, 0xBF);
- // registerWrite(csPin, 0x4E, 0x3F);
- // registerWrite(csPin, 0x7F, 0x08);
- // registerWrite(csPin, 0x65, 0x20);
- // registerWrite(csPin, 0x6A, 0x18);
- // registerWrite(csPin, 0x7F, 0x09);
- // registerWrite(csPin, 0x4F, 0xAF);
- // registerWrite(csPin, 0x5F, 0x40);
- // registerWrite(csPin, 0x48, 0x80);
- // registerWrite(csPin, 0x49, 0x80);
- // registerWrite(csPin, 0x57, 0x77);
- // registerWrite(csPin, 0x60, 0x78);
- // registerWrite(csPin, 0x61, 0x78);
- // registerWrite(csPin, 0x62, 0x08);
- // registerWrite(csPin, 0x63, 0x50);
- // registerWrite(csPin, 0x7F, 0x0A);
- // registerWrite(csPin, 0x45, 0x60);
- // registerWrite(csPin, 0x7F, 0x00);
- // registerWrite(csPin, 0x4D, 0x11);
- // registerWrite(csPin, 0x55, 0x80);
- // registerWrite(csPin, 0x74, 0x1F);
- // registerWrite(csPin, 0x75, 0x1F);
- // registerWrite(csPin, 0x4A, 0x78);
- // registerWrite(csPin, 0x4B, 0x78);
- // registerWrite(csPin, 0x44, 0x08);
- // registerWrite(csPin, 0x45, 0x50);
- // registerWrite(csPin, 0x64, 0xFF);
- // registerWrite(csPin, 0x65, 0x1F);
- // registerWrite(csPin, 0x7F, 0x14);
- // registerWrite(csPin, 0x65, 0x67);
- // registerWrite(csPin, 0x66, 0x08);
- // registerWrite(csPin, 0x63, 0x70);
- // registerWrite(csPin, 0x7F, 0x15);
- // registerWrite(csPin, 0x48, 0x48);
- // registerWrite(csPin, 0x7F, 0x07);
- // registerWrite(csPin, 0x41, 0x0D);
- // registerWrite(csPin, 0x43, 0x14);
- // registerWrite(csPin, 0x4B, 0x0E);
- // registerWrite(csPin, 0x45, 0x0F);
- // registerWrite(csPin, 0x44, 0x42);
- // registerWrite(csPin, 0x4C, 0x80);
- // registerWrite(csPin, 0x7F, 0x10);
- // registerWrite(csPin, 0x5B, 0x02);
- // registerWrite(csPin, 0x7F, 0x07);
- // registerWrite(csPin, 0x40, 0x41);
- // registerWrite(csPin, 0x70, 0x00);
-
- // vTaskDelay(M2T(10)); // delay 10ms
-
- // registerWrite(csPin, 0x32, 0x44);
- // registerWrite(csPin, 0x7F, 0x07);
- // registerWrite(csPin, 0x40, 0x40);
- // registerWrite(csPin, 0x7F, 0x06);
- // registerWrite(csPin, 0x62, 0xF0);
- // registerWrite(csPin, 0x63, 0x00);
- // registerWrite(csPin, 0x7F, 0x0D);
- // registerWrite(csPin, 0x48, 0xC0);
- // registerWrite(csPin, 0x6F, 0xD5);
- // registerWrite(csPin, 0x7F, 0x00);
- // registerWrite(csPin, 0x5B, 0xA0);
- // registerWrite(csPin, 0x4E, 0xA8);
- // registerWrite(csPin, 0x5A, 0x50);
- // registerWrite(csPin, 0x40, 0x80);
-
- // registerWrite(csPin, 0x7F, 0x00);
- // registerWrite(csPin, 0x5A, 0x10);
- // registerWrite(csPin, 0x54, 0x00);
+ registerWrite(csPin, 0x7F, 0x00);
+ registerWrite(csPin, 0x61, 0xAD);
+ registerWrite(csPin, 0x7F, 0x03);
+ registerWrite(csPin, 0x40, 0x00);
+ registerWrite(csPin, 0x7F, 0x05);
+ registerWrite(csPin, 0x41, 0xB3);
+ registerWrite(csPin, 0x43, 0xF1);
+ registerWrite(csPin, 0x45, 0x14);
+ registerWrite(csPin, 0x5B, 0x32);
+ registerWrite(csPin, 0x5F, 0x34);
+ registerWrite(csPin, 0x7B, 0x08);
+ registerWrite(csPin, 0x7F, 0x06);
+ registerWrite(csPin, 0x44, 0x1B);
+ registerWrite(csPin, 0x40, 0xBF);
+ registerWrite(csPin, 0x4E, 0x3F);
+ registerWrite(csPin, 0x7F, 0x08);
+ registerWrite(csPin, 0x65, 0x20);
+ registerWrite(csPin, 0x6A, 0x18);
+ registerWrite(csPin, 0x7F, 0x09);
+ registerWrite(csPin, 0x4F, 0xAF);
+ registerWrite(csPin, 0x5F, 0x40);
+ registerWrite(csPin, 0x48, 0x80);
+ registerWrite(csPin, 0x49, 0x80);
+ registerWrite(csPin, 0x57, 0x77);
+ registerWrite(csPin, 0x60, 0x78);
+ registerWrite(csPin, 0x61, 0x78);
+ registerWrite(csPin, 0x62, 0x08);
+ registerWrite(csPin, 0x63, 0x50);
+ registerWrite(csPin, 0x7F, 0x0A);
+ registerWrite(csPin, 0x45, 0x60);
+ registerWrite(csPin, 0x7F, 0x00);
+ registerWrite(csPin, 0x4D, 0x11);
+ registerWrite(csPin, 0x55, 0x80);
+ registerWrite(csPin, 0x74, 0x1F);
+ registerWrite(csPin, 0x75, 0x1F);
+ registerWrite(csPin, 0x4A, 0x78);
+ registerWrite(csPin, 0x4B, 0x78);
+ registerWrite(csPin, 0x44, 0x08);
+ registerWrite(csPin, 0x45, 0x50);
+ registerWrite(csPin, 0x64, 0xFF);
+ registerWrite(csPin, 0x65, 0x1F);
+ registerWrite(csPin, 0x7F, 0x14);
+ registerWrite(csPin, 0x65, 0x67);
+ registerWrite(csPin, 0x66, 0x08);
+ registerWrite(csPin, 0x63, 0x70);
+ registerWrite(csPin, 0x7F, 0x15);
+ registerWrite(csPin, 0x48, 0x48);
+ registerWrite(csPin, 0x7F, 0x07);
+ registerWrite(csPin, 0x41, 0x0D);
+ registerWrite(csPin, 0x43, 0x14);
+ registerWrite(csPin, 0x4B, 0x0E);
+ registerWrite(csPin, 0x45, 0x0F);
+ registerWrite(csPin, 0x44, 0x42);
+ registerWrite(csPin, 0x4C, 0x80);
+ registerWrite(csPin, 0x7F, 0x10);
+ registerWrite(csPin, 0x5B, 0x02);
+ registerWrite(csPin, 0x7F, 0x07);
+ registerWrite(csPin, 0x40, 0x41);
+ registerWrite(csPin, 0x70, 0x00);
+
+ vTaskDelay(M2T(10)); // delay 10ms
+
+ registerWrite(csPin, 0x32, 0x44);
+ registerWrite(csPin, 0x7F, 0x07);
+ registerWrite(csPin, 0x40, 0x40);
+ registerWrite(csPin, 0x7F, 0x06);
+ registerWrite(csPin, 0x62, 0xF0);
+ registerWrite(csPin, 0x63, 0x00);
+ registerWrite(csPin, 0x7F, 0x0D);
+ registerWrite(csPin, 0x48, 0xC0);
+ registerWrite(csPin, 0x6F, 0xD5);
+ registerWrite(csPin, 0x7F, 0x00);
+ registerWrite(csPin, 0x5B, 0xA0);
+ registerWrite(csPin, 0x4E, 0xA8);
+ registerWrite(csPin, 0x5A, 0x50);
+ registerWrite(csPin, 0x40, 0x80);
+
+ registerWrite(csPin, 0x7F, 0x00);
+ registerWrite(csPin, 0x5A, 0x10);
+ registerWrite(csPin, 0x54, 0x00);
}
bool pmw3901Init(const deckPin_t csPin)
@@ -178,65 +175,64 @@ bool pmw3901Init(const deckPin_t csPin)
if (isInit) {
return true;
}
- //COMMENTED FIRMWARE
- // // Initialize CS Pin
- // pinMode(csPin, OUTPUT);
- // digitalWrite(csPin, HIGH);
- // spiBegin();
- // vTaskDelay(M2T(40));
+ // Initialize CS Pin
+ pinMode(csPin, OUTPUT);
+ digitalWrite(csPin, HIGH);
- // digitalWrite(csPin, HIGH);
- // vTaskDelay(M2T(2));
- // digitalWrite(csPin, LOW);
- // vTaskDelay(M2T(2));
- // digitalWrite(csPin, HIGH);
- // vTaskDelay(M2T(2));
+ spiBegin();
+ vTaskDelay(M2T(40));
- // uint8_t chipId = registerRead(csPin, 0x00);
- // uint8_t invChipId = registerRead(csPin, 0x5f);
+ digitalWrite(csPin, HIGH);
+ vTaskDelay(M2T(2));
+ digitalWrite(csPin, LOW);
+ vTaskDelay(M2T(2));
+ digitalWrite(csPin, HIGH);
+ vTaskDelay(M2T(2));
- // DEBUG_PRINT("Motion chip id: 0x%x:0x%x\n", chipId, invChipId);
+ uint8_t chipId = registerRead(csPin, 0x00);
+ uint8_t invChipId = registerRead(csPin, 0x5f);
- // if (chipId == 0x49 && invChipId == 0xB6)
- // {
- // // Power on reset
- // registerWrite(csPin, 0x3a, 0x5a);
- // vTaskDelay(M2T(5));
+ DEBUG_PRINT("Motion chip id: 0x%x:0x%x\n", chipId, invChipId);
- // // Reading the motion registers one time
- // registerRead(csPin, 0x02);
- // registerRead(csPin, 0x03);
- // registerRead(csPin, 0x04);
- // registerRead(csPin, 0x05);
- // registerRead(csPin, 0x06);
- // vTaskDelay(M2T(1));
+ if (chipId == 0x49 && invChipId == 0xB6)
+ {
+ // Power on reset
+ registerWrite(csPin, 0x3a, 0x5a);
+ vTaskDelay(M2T(5));
- // InitRegisters(csPin);
+ // Reading the motion registers one time
+ registerRead(csPin, 0x02);
+ registerRead(csPin, 0x03);
+ registerRead(csPin, 0x04);
+ registerRead(csPin, 0x05);
+ registerRead(csPin, 0x06);
+ vTaskDelay(M2T(1));
- // isInit = true;
- // }
+ InitRegisters(csPin);
+
+ isInit = true;
+ }
return isInit;
}
void pmw3901ReadMotion(const deckPin_t csPin, motionBurst_t * motion)
{
- //COMMENTED FIRMWARE
- // uint8_t address = 0x16;
-
- // spiBeginTransaction(SPI_BAUDRATE_2MHZ);
- // digitalWrite(csPin,LOW);
- // sleepus(50);
- // spiExchange(1, &address, &address);
- // sleepus(50);
- // spiExchange(sizeof(motionBurst_t), (uint8_t*)motion, (uint8_t*)motion);
- // sleepus(50);
- // digitalWrite(csPin, HIGH);
- // spiEndTransaction();
- // sleepus(50);
-
- // uint16_t realShutter = (motion->shutter >> 8) & 0x0FF;
- // realShutter |= (motion->shutter & 0x0ff) << 8;
- // motion->shutter = realShutter;
+ uint8_t address = 0x16;
+
+ spiBeginTransaction(SPI_BAUDRATE_2MHZ);
+ digitalWrite(csPin,LOW);
+ sleepus(50);
+ spiExchange(1, &address, &address);
+ sleepus(50);
+ spiExchange(sizeof(motionBurst_t), (uint8_t*)motion, (uint8_t*)motion);
+ sleepus(50);
+ digitalWrite(csPin, HIGH);
+ spiEndTransaction();
+ sleepus(50);
+
+ uint16_t realShutter = (motion->shutter >> 8) & 0x0FF;
+ realShutter |= (motion->shutter & 0x0ff) << 8;
+ motion->shutter = realShutter;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/swd.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/swd.c
index a553065d7..bccba0424 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/swd.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/swd.c
@@ -53,50 +53,49 @@ static bool isInit = false;
static void initGPIO(void) {
- //COMMENTED FIRMWARE
- // GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_InitTypeDef GPIO_InitStructure;
- // SWD_SPI_CLK_INIT(SWD_SPI_CLK, ENABLE);
+ SWD_SPI_CLK_INIT(SWD_SPI_CLK, ENABLE);
- // RCC_AHB1PeriphClockCmd(SWD_SPI_SCK_GPIO_CLK | SWD_SPI_MISO_GPIO_CLK, ENABLE);
+ RCC_AHB1PeriphClockCmd(SWD_SPI_SCK_GPIO_CLK | SWD_SPI_MISO_GPIO_CLK, ENABLE);
- // GPIO_PinAFConfig(SWD_SPI_SCK_GPIO_PORT, SWD_SPI_SCK_SOURCE, SWD_SPI_SCK_AF);
- // GPIO_PinAFConfig(SWD_SPI_MISO_GPIO_PORT, SWD_SPI_MISO_SOURCE, SWD_SPI_MISO_AF);
+ GPIO_PinAFConfig(SWD_SPI_SCK_GPIO_PORT, SWD_SPI_SCK_SOURCE, SWD_SPI_SCK_AF);
+ GPIO_PinAFConfig(SWD_SPI_MISO_GPIO_PORT, SWD_SPI_MISO_SOURCE, SWD_SPI_MISO_AF);
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
- // GPIO_InitStructure.GPIO_Pin = SWD_SPI_SCK_PIN;
- // GPIO_Init(SWD_SPI_SCK_GPIO_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = SWD_SPI_SCK_PIN;
+ GPIO_Init(SWD_SPI_SCK_GPIO_PORT, &GPIO_InitStructure);
- // /* In bi-directional mode only MISO is used */
- // GPIO_InitStructure.GPIO_Pin = SWD_SPI_MISO_PIN;
- // GPIO_Init(SWD_SPI_MISO_GPIO_PORT, &GPIO_InitStructure);
+ /* In bi-directional mode only MISO is used */
+ GPIO_InitStructure.GPIO_Pin = SWD_SPI_MISO_PIN;
+ GPIO_Init(SWD_SPI_MISO_GPIO_PORT, &GPIO_InitStructure);
}
static void initSPI(void) {
- //COMMENTED FIRMWARE
- // SPI_InitTypeDef SPI_InitStructure;
- // initGPIO();
+ SPI_InitTypeDef SPI_InitStructure;
- // /* Set up SPI in bi-directional mode */
- // SPI_InitStructure.SPI_Direction = SPI_Direction_1Line_Tx;
- // SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
- // SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
- // SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
- // SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
- // SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
- // SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256;
+ initGPIO();
- // SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
- // SPI_InitStructure.SPI_CRCPolynomial = 7;
- // SPI_Init(SWD_SPI, &SPI_InitStructure);
+ /* Set up SPI in bi-directional mode */
+ SPI_InitStructure.SPI_Direction = SPI_Direction_1Line_Tx;
+ SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
+ SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
+ SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
+ SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
+ SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
+ SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256;
- // SPI_Cmd(SWD_SPI, ENABLE);
+ SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
+ SPI_InitStructure.SPI_CRCPolynomial = 7;
+ SPI_Init(SWD_SPI, &SPI_InitStructure);
+
+ SPI_Cmd(SWD_SPI, ENABLE);
}
//Initialize the green led pin as output
@@ -104,8 +103,8 @@ void swdInit()
{
if(isInit)
return;
- //COMMENTED FIRMWARE
- // initSPI();
+
+ initSPI();
isInit = true;
}
@@ -142,64 +141,64 @@ bool swdTest(void) {
// Should read out (and does):0x0BB11477 (according to OpenOCD)
//T ACK |------------DATA--------------|P
//? 100 111011100010100010001101110100001
-//COMMENTED FIRMWARE
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// #if 0
-// tx_data[idx++] = 0x9E;
-// tx_data[idx++] = 0xE7;
-// #else
-// tx_data[idx++] = 0x79;
-// tx_data[idx++] = 0xE7;
-// #endif
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFF;
-// tx_data[idx++] = 0xFC;
-// tx_data[idx++] = header;
-// // Rest is 0x00
-
-// /* As long as the SPI is enabled in bi-di mode the clock is enabled. So
-// * we write data as normal, but when reading there's no need to output any
-// * data to trigger the clock. Direction needs to be set accordingly.
-// */
-
-// while(number_of_txd_bytes < transfer_size)
-// {
-// if (number_of_txd_bytes < idx)
-// {
-// SPI_BiDirectionalLineConfig(SWD_SPI, SPI_Direction_Tx);
-
-// /*!< Loop while DR register in not emplty */
-// while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_TXE) == RESET);
-
-// SPI_I2S_SendData(SWD_SPI, tx_data[number_of_txd_bytes]);
-
-// // Make sure that we have sent data in case next loop will be RX, the
-// // mode is switched before we manage to send the data!
-// while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_BSY) == SET);
-// }
-// else
-// {
-// SPI_BiDirectionalLineConfig(SWD_SPI, SPI_Direction_Rx);
-// while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_RXNE) == RESET);
-
-// /*!< Return the byte read from the SPI bus */
-// rx_data[number_of_txd_bytes] = (uint8_t) SPI_I2S_ReceiveData(SWD_SPI);
-// }
-// number_of_txd_bytes++;
-// }
-// SPI_Cmd(SWD_SPI, DISABLE);
+
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+#if 0
+ tx_data[idx++] = 0x9E;
+ tx_data[idx++] = 0xE7;
+#else
+ tx_data[idx++] = 0x79;
+ tx_data[idx++] = 0xE7;
+#endif
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFF;
+ tx_data[idx++] = 0xFC;
+ tx_data[idx++] = header;
+ // Rest is 0x00
+
+ /* As long as the SPI is enabled in bi-di mode the clock is enabled. So
+ * we write data as normal, but when reading there's no need to output any
+ * data to trigger the clock. Direction needs to be set accordingly.
+ */
+
+ while(number_of_txd_bytes < transfer_size)
+ {
+ if (number_of_txd_bytes < idx)
+ {
+ SPI_BiDirectionalLineConfig(SWD_SPI, SPI_Direction_Tx);
+
+ /*!< Loop while DR register in not emplty */
+ while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_TXE) == RESET);
+
+ SPI_I2S_SendData(SWD_SPI, tx_data[number_of_txd_bytes]);
+
+ // Make sure that we have sent data in case next loop will be RX, the
+ // mode is switched before we manage to send the data!
+ while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_BSY) == SET);
+ }
+ else
+ {
+ SPI_BiDirectionalLineConfig(SWD_SPI, SPI_Direction_Rx);
+ while (SPI_I2S_GetFlagStatus(SWD_SPI, SPI_I2S_FLAG_RXNE) == RESET);
+
+ /*!< Return the byte read from the SPI bus */
+ rx_data[number_of_txd_bytes] = (uint8_t) SPI_I2S_ReceiveData(SWD_SPI);
+ }
+ number_of_txd_bytes++;
+ }
+ SPI_Cmd(SWD_SPI, DISABLE);
return isInit;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart1.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart1.c
index dabca42bb..213cce235 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart1.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart1.c
@@ -109,76 +109,74 @@ static void uart1DmaInit(void)
}
void uart1Init(const uint32_t baudrate) {
- //COMMENTED FIRMWARE
- //uart1InitWithParity(baudrate, uart1ParityNone);
+ uart1InitWithParity(baudrate, uart1ParityNone);
}
void uart1InitWithParity(const uint32_t baudrate, const uart1Parity_t parity)
{
- //COMMENTED FIRMWARE
- // USART_InitTypeDef USART_InitStructure;
- // GPIO_InitTypeDef GPIO_InitStructure;
- // NVIC_InitTypeDef NVIC_InitStructure;
-
- // /* Enable GPIO and USART clock */
- // RCC_AHB1PeriphClockCmd(UART1_GPIO_PERIF, ENABLE);
- // ENABLE_UART1_RCC(UART1_PERIF, ENABLE);
-
- // /* Configure USART Rx as input floating */
- // GPIO_InitStructure.GPIO_Pin = UART1_GPIO_RX_PIN;
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
- // GPIO_Init(UART1_GPIO_PORT, &GPIO_InitStructure);
-
- // /* Configure USART Tx as alternate function */
- // GPIO_InitStructure.GPIO_Pin = UART1_GPIO_TX_PIN;
- // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
- // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- // GPIO_Init(UART1_GPIO_PORT, &GPIO_InitStructure);
-
- // //Map uart to alternate functions
- // GPIO_PinAFConfig(UART1_GPIO_PORT, UART1_GPIO_AF_TX_PIN, UART1_GPIO_AF_TX);
- // GPIO_PinAFConfig(UART1_GPIO_PORT, UART1_GPIO_AF_RX_PIN, UART1_GPIO_AF_RX);
+ USART_InitTypeDef USART_InitStructure;
+ GPIO_InitTypeDef GPIO_InitStructure;
+ NVIC_InitTypeDef NVIC_InitStructure;
- // USART_InitStructure.USART_BaudRate = baudrate;
- // USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
- // if (parity == uart1ParityEven || parity == uart1ParityOdd) {
- // USART_InitStructure.USART_WordLength = USART_WordLength_9b;
- // } else {
- // USART_InitStructure.USART_WordLength = USART_WordLength_8b;
- // }
+ /* Enable GPIO and USART clock */
+ RCC_AHB1PeriphClockCmd(UART1_GPIO_PERIF, ENABLE);
+ ENABLE_UART1_RCC(UART1_PERIF, ENABLE);
+
+ /* Configure USART Rx as input floating */
+ GPIO_InitStructure.GPIO_Pin = UART1_GPIO_RX_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+ GPIO_Init(UART1_GPIO_PORT, &GPIO_InitStructure);
+
+ /* Configure USART Tx as alternate function */
+ GPIO_InitStructure.GPIO_Pin = UART1_GPIO_TX_PIN;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_Init(UART1_GPIO_PORT, &GPIO_InitStructure);
+
+ //Map uart to alternate functions
+ GPIO_PinAFConfig(UART1_GPIO_PORT, UART1_GPIO_AF_TX_PIN, UART1_GPIO_AF_TX);
+ GPIO_PinAFConfig(UART1_GPIO_PORT, UART1_GPIO_AF_RX_PIN, UART1_GPIO_AF_RX);
+
+ USART_InitStructure.USART_BaudRate = baudrate;
+ USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+ if (parity == uart1ParityEven || parity == uart1ParityOdd) {
+ USART_InitStructure.USART_WordLength = USART_WordLength_9b;
+ } else {
+ USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+ }
- // USART_InitStructure.USART_StopBits = USART_StopBits_1;
+ USART_InitStructure.USART_StopBits = USART_StopBits_1;
- // if (parity == uart1ParityEven) {
- // USART_InitStructure.USART_Parity = USART_Parity_Even;
- // } else if (parity == uart1ParityOdd) {
- // USART_InitStructure.USART_Parity = USART_Parity_Odd;
- // } else {
- // USART_InitStructure.USART_Parity = USART_Parity_No;
- // }
+ if (parity == uart1ParityEven) {
+ USART_InitStructure.USART_Parity = USART_Parity_Even;
+ } else if (parity == uart1ParityOdd) {
+ USART_InitStructure.USART_Parity = USART_Parity_Odd;
+ } else {
+ USART_InitStructure.USART_Parity = USART_Parity_No;
+ }
- // USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
- // USART_Init(UART1_TYPE, &USART_InitStructure);
+ USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+ USART_Init(UART1_TYPE, &USART_InitStructure);
- // uart1DmaInit();
+ uart1DmaInit();
- // NVIC_InitStructure.NVIC_IRQChannel = UART1_IRQ;
- // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
- // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- // NVIC_Init(&NVIC_InitStructure);
+ NVIC_InitStructure.NVIC_IRQChannel = UART1_IRQ;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
- // uart1queue = STATIC_MEM_QUEUE_CREATE(uart1queue);
+ uart1queue = STATIC_MEM_QUEUE_CREATE(uart1queue);
- // USART_ITConfig(UART1_TYPE, USART_IT_RXNE, ENABLE);
+ USART_ITConfig(UART1_TYPE, USART_IT_RXNE, ENABLE);
- // //Enable UART
- // USART_Cmd(UART1_TYPE, ENABLE);
+ //Enable UART
+ USART_Cmd(UART1_TYPE, ENABLE);
- // USART_ITConfig(UART1_TYPE, USART_IT_RXNE, ENABLE);
+ USART_ITConfig(UART1_TYPE, USART_IT_RXNE, ENABLE);
isInit = true;
}
@@ -206,17 +204,16 @@ bool uart1GetDataWithDefaultTimeout(uint8_t *c)
void uart1SendData(uint32_t size, uint8_t* data)
{
- //COMMENTED FIRMWARE
- // uint32_t i;
+ uint32_t i;
- // if (!isInit)
- // return;
+ if (!isInit)
+ return;
- // for(i = 0; i < size; i++)
- // {
- // while (!(UART1_TYPE->SR & USART_FLAG_TXE));
- // UART1_TYPE->DR = (data[i] & 0x00FF);
- // }
+ for(i = 0; i < size; i++)
+ {
+ while (!(UART1_TYPE->SR & USART_FLAG_TXE));
+ UART1_TYPE->DR = (data[i] & 0x00FF);
+ }
}
#ifdef ENABLE_UART1_DMA
@@ -249,8 +246,7 @@ void uart1SendDataDmaBlocking(uint32_t size, uint8_t* data)
int uart1Putchar(int ch)
{
- //COMMENTED FIRMWARE
- //uart1SendData(1, (uint8_t *)&ch);
+ uart1SendData(1, (uint8_t *)&ch);
return (unsigned char)ch;
}
@@ -285,23 +281,22 @@ void __attribute__((used)) DMA1_Stream3_IRQHandler(void)
void __attribute__((used)) USART3_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- // uint8_t rxData;
- // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-
- // if (USART_GetITStatus(UART1_TYPE, USART_IT_RXNE))
- // {
- // rxData = USART_ReceiveData(UART1_TYPE) & 0x00FF;
- // xQueueSendFromISR(uart1queue, &rxData, &xHigherPriorityTaskWoken);
- // } else {
- // /** if we get here, the error is most likely caused by an overrun!
- // * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
- // * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
- // * - reading USART_SR register followed reading the USART_DR register.
- // */
- // asm volatile ("" : "=m" (UART1_TYPE->SR) : "r" (UART1_TYPE->SR)); // force non-optimizable reads
- // asm volatile ("" : "=m" (UART1_TYPE->DR) : "r" (UART1_TYPE->DR)); // of these two registers
-
- // hasOverrun = true;
- // }
+ uint8_t rxData;
+ portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+
+ if (USART_GetITStatus(UART1_TYPE, USART_IT_RXNE))
+ {
+ rxData = USART_ReceiveData(UART1_TYPE) & 0x00FF;
+ xQueueSendFromISR(uart1queue, &rxData, &xHigherPriorityTaskWoken);
+ } else {
+ /** if we get here, the error is most likely caused by an overrun!
+ * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
+ * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
+ * - reading USART_SR register followed reading the USART_DR register.
+ */
+ asm volatile ("" : "=m" (UART1_TYPE->SR) : "r" (UART1_TYPE->SR)); // force non-optimizable reads
+ asm volatile ("" : "=m" (UART1_TYPE->DR) : "r" (UART1_TYPE->DR)); // of these two registers
+
+ hasOverrun = true;
+ }
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart2.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart2.c
index 3eca14c49..53419a3ae 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart2.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart2.c
@@ -83,113 +83,111 @@ static bool hasOverrun = false;
*/
static void uart2DmaInit(void)
{
- //COMMENTED FIRMWARE
- // NVIC_InitTypeDef NVIC_InitStructure;
-
- // RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
-
- // // USART TX DMA Channel Config
- // DMA_InitStructureShare.DMA_PeripheralBaseAddr = (uint32_t)&UART2_TYPE->DR;
- // DMA_InitStructureShare.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
- // DMA_InitStructureShare.DMA_MemoryInc = DMA_MemoryInc_Enable;
- // DMA_InitStructureShare.DMA_MemoryBurst = DMA_MemoryBurst_Single;
- // DMA_InitStructureShare.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
- // DMA_InitStructureShare.DMA_BufferSize = 0;
- // DMA_InitStructureShare.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- // DMA_InitStructureShare.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
- // DMA_InitStructureShare.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
- // DMA_InitStructureShare.DMA_DIR = DMA_DIR_MemoryToPeripheral;
- // DMA_InitStructureShare.DMA_Mode = DMA_Mode_Normal;
- // DMA_InitStructureShare.DMA_FIFOMode = DMA_FIFOMode_Disable;
- // DMA_InitStructureShare.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
- // DMA_InitStructureShare.DMA_Channel = UART2_DMA_CH;
- // #ifdef UART2_LINK_COMM
- // DMA_InitStructureShare.DMA_Priority = DMA_Priority_High;
- // #else
- // DMA_InitStructureShare.DMA_Priority = DMA_Priority_Low;
- // #endif
-
- // NVIC_InitStructure.NVIC_IRQChannel = UART2_DMA_IRQ;
- // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- // #ifdef UART2_LINK_COMM
- // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
- // #else
- // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
- // #endif
- // NVIC_Init(&NVIC_InitStructure);
-
- // isUartDmaInitialized = true;
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
+
+ // USART TX DMA Channel Config
+ DMA_InitStructureShare.DMA_PeripheralBaseAddr = (uint32_t)&UART2_TYPE->DR;
+ DMA_InitStructureShare.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
+ DMA_InitStructureShare.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ DMA_InitStructureShare.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+ DMA_InitStructureShare.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ DMA_InitStructureShare.DMA_BufferSize = 0;
+ DMA_InitStructureShare.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ DMA_InitStructureShare.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+ DMA_InitStructureShare.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+ DMA_InitStructureShare.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+ DMA_InitStructureShare.DMA_Mode = DMA_Mode_Normal;
+ DMA_InitStructureShare.DMA_FIFOMode = DMA_FIFOMode_Disable;
+ DMA_InitStructureShare.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
+ DMA_InitStructureShare.DMA_Channel = UART2_DMA_CH;
+ #ifdef UART2_LINK_COMM
+ DMA_InitStructureShare.DMA_Priority = DMA_Priority_High;
+ #else
+ DMA_InitStructureShare.DMA_Priority = DMA_Priority_Low;
+ #endif
+
+ NVIC_InitStructure.NVIC_IRQChannel = UART2_DMA_IRQ;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ #ifdef UART2_LINK_COMM
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
+ #else
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
+ #endif
+ NVIC_Init(&NVIC_InitStructure);
+
+ isUartDmaInitialized = true;
}
void uart2Init(const uint32_t baudrate)
{
- //COMMENTED FIRMWARE
-
- // USART_InitTypeDef USART_InitStructure;
- // GPIO_InitTypeDef GPIO_InitStructure;
- // NVIC_InitTypeDef NVIC_InitStructure;
-
- // // initialize the FreeRTOS structures first, to prevent null pointers in interrupts
- // waitUntilSendDone = xSemaphoreCreateBinaryStatic(&waitUntilSendDoneBuffer); // initialized as blocking
- // uartBusy = xSemaphoreCreateBinaryStatic(&uartBusyBuffer); // initialized as blocking
- // xSemaphoreGive(uartBusy); // but we give it because the uart isn't busy at initialization
-
- // /* Enable GPIO and USART clock */
- // RCC_AHB1PeriphClockCmd(UART2_GPIO_PERIF, ENABLE);
- // ENABLE_UART2_RCC(UART2_PERIF, ENABLE);
-
- // /* Configure USART Rx as input floating */
- // GPIO_InitStructure.GPIO_Pin = UART2_GPIO_RX_PIN;
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
- // GPIO_Init(UART2_GPIO_PORT, &GPIO_InitStructure);
-
- // /* Configure USART Tx as alternate function */
- // GPIO_InitStructure.GPIO_Pin = UART2_GPIO_TX_PIN;
- // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
- // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
- // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
- // GPIO_Init(UART2_GPIO_PORT, &GPIO_InitStructure);
-
- // //Map uart to alternate functions
- // GPIO_PinAFConfig(UART2_GPIO_PORT, UART2_GPIO_AF_TX_PIN, UART2_GPIO_AF_TX);
- // GPIO_PinAFConfig(UART2_GPIO_PORT, UART2_GPIO_AF_RX_PIN, UART2_GPIO_AF_RX);
-
- // USART_InitStructure.USART_BaudRate = baudrate;
- // USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
- // USART_InitStructure.USART_WordLength = USART_WordLength_8b;
- // USART_InitStructure.USART_StopBits = USART_StopBits_1;
- // USART_InitStructure.USART_Parity = USART_Parity_No ;
- // USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
- // USART_Init(UART2_TYPE, &USART_InitStructure);
-
- // uart2DmaInit();
-
- // // Configure Rx buffer not empty interrupt
- // NVIC_InitStructure.NVIC_IRQChannel = UART2_IRQ;
- // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- // #ifdef UART2_LINK_COMM
- // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SYSLINK_PRI;
- // #else
- // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
- // #endif
- // NVIC_Init(&NVIC_InitStructure);
-
- // #ifdef UART2_LINK_COMM
- // uart2PacketDelivery = STATIC_MEM_QUEUE_CREATE(uart2PacketDelivery);
- // DEBUG_QUEUE_MONITOR_REGISTER(uart2PacketDelivery);
- // #else
- // uart2queue = STATIC_MEM_QUEUE_CREATE(uart2queue);
- // #endif
-
- // USART_ITConfig(UART2_TYPE, USART_IT_RXNE, ENABLE);
-
- // //Enable UART
- // USART_Cmd(UART2_TYPE, ENABLE);
-
- // USART_ITConfig(UART2_TYPE, USART_IT_RXNE, ENABLE);
+
+ USART_InitTypeDef USART_InitStructure;
+ GPIO_InitTypeDef GPIO_InitStructure;
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ // initialize the FreeRTOS structures first, to prevent null pointers in interrupts
+ waitUntilSendDone = xSemaphoreCreateBinaryStatic(&waitUntilSendDoneBuffer); // initialized as blocking
+ uartBusy = xSemaphoreCreateBinaryStatic(&uartBusyBuffer); // initialized as blocking
+ xSemaphoreGive(uartBusy); // but we give it because the uart isn't busy at initialization
+
+ /* Enable GPIO and USART clock */
+ RCC_AHB1PeriphClockCmd(UART2_GPIO_PERIF, ENABLE);
+ ENABLE_UART2_RCC(UART2_PERIF, ENABLE);
+
+ /* Configure USART Rx as input floating */
+ GPIO_InitStructure.GPIO_Pin = UART2_GPIO_RX_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+ GPIO_Init(UART2_GPIO_PORT, &GPIO_InitStructure);
+
+ /* Configure USART Tx as alternate function */
+ GPIO_InitStructure.GPIO_Pin = UART2_GPIO_TX_PIN;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_Init(UART2_GPIO_PORT, &GPIO_InitStructure);
+
+ //Map uart to alternate functions
+ GPIO_PinAFConfig(UART2_GPIO_PORT, UART2_GPIO_AF_TX_PIN, UART2_GPIO_AF_TX);
+ GPIO_PinAFConfig(UART2_GPIO_PORT, UART2_GPIO_AF_RX_PIN, UART2_GPIO_AF_RX);
+
+ USART_InitStructure.USART_BaudRate = baudrate;
+ USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+ USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+ USART_InitStructure.USART_StopBits = USART_StopBits_1;
+ USART_InitStructure.USART_Parity = USART_Parity_No ;
+ USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+ USART_Init(UART2_TYPE, &USART_InitStructure);
+
+ uart2DmaInit();
+
+ // Configure Rx buffer not empty interrupt
+ NVIC_InitStructure.NVIC_IRQChannel = UART2_IRQ;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ #ifdef UART2_LINK_COMM
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SYSLINK_PRI;
+ #else
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_MID_PRI;
+ #endif
+ NVIC_Init(&NVIC_InitStructure);
+
+ #ifdef UART2_LINK_COMM
+ uart2PacketDelivery = STATIC_MEM_QUEUE_CREATE(uart2PacketDelivery);
+ DEBUG_QUEUE_MONITOR_REGISTER(uart2PacketDelivery);
+ #else
+ uart2queue = STATIC_MEM_QUEUE_CREATE(uart2queue);
+ #endif
+
+ USART_ITConfig(UART2_TYPE, USART_IT_RXNE, ENABLE);
+
+ //Enable UART
+ USART_Cmd(UART2_TYPE, ENABLE);
+
+ USART_ITConfig(UART2_TYPE, USART_IT_RXNE, ENABLE);
isInit = true;
}
@@ -201,50 +199,47 @@ bool uart2Test(void)
void uart2SendData(uint32_t size, uint8_t* data)
{
- //COMMENTED FIRMWARE
- // uint32_t i;
+ uint32_t i;
- // if (!isInit)
- // return;
+ if (!isInit)
+ return;
- // for(i = 0; i < size; i++)
- // {
- // while (!(UART2_TYPE->SR & USART_FLAG_TXE));
- // UART2_TYPE->DR = (data[i] & 0x00FF);
- // }
+ for(i = 0; i < size; i++)
+ {
+ while (!(UART2_TYPE->SR & USART_FLAG_TXE));
+ UART2_TYPE->DR = (data[i] & 0x00FF);
+ }
}
void uart2SendDataDmaBlocking(uint32_t size, uint8_t* data)
{
- //COMMENTED FIRMWARE
- // if (isUartDmaInitialized)
- // {
- // xSemaphoreTake(uartBusy, portMAX_DELAY);
- // // Wait for DMA to be free
- // while(DMA_GetCmdStatus(UART2_DMA_STREAM) != DISABLE);
- // //Copy data in DMA buffer
- // memcpy(dmaBuffer, data, size);
- // DMA_InitStructureShare.DMA_BufferSize = size;
- // initialDMACount = size;
- // // Init new DMA stream
- // DMA_Init(UART2_DMA_STREAM, &DMA_InitStructureShare);
- // // Enable the Transfer Complete interrupt
- // DMA_ITConfig(UART2_DMA_STREAM, DMA_IT_TC, ENABLE);
- // /* Enable USART DMA TX Requests */
- // USART_DMACmd(UART2_TYPE, USART_DMAReq_Tx, ENABLE);
- // /* Clear transfer complete */
- // USART_ClearFlag(UART2_TYPE, USART_FLAG_TC);
- // /* Enable DMA USART TX Stream */
- // DMA_Cmd(UART2_DMA_STREAM, ENABLE);
- // xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
- // xSemaphoreGive(uartBusy);
- // }
+ if (isUartDmaInitialized)
+ {
+ xSemaphoreTake(uartBusy, portMAX_DELAY);
+ // Wait for DMA to be free
+ while(DMA_GetCmdStatus(UART2_DMA_STREAM) != DISABLE);
+ //Copy data in DMA buffer
+ memcpy(dmaBuffer, data, size);
+ DMA_InitStructureShare.DMA_BufferSize = size;
+ initialDMACount = size;
+ // Init new DMA stream
+ DMA_Init(UART2_DMA_STREAM, &DMA_InitStructureShare);
+ // Enable the Transfer Complete interrupt
+ DMA_ITConfig(UART2_DMA_STREAM, DMA_IT_TC, ENABLE);
+ /* Enable USART DMA TX Requests */
+ USART_DMACmd(UART2_TYPE, USART_DMAReq_Tx, ENABLE);
+ /* Clear transfer complete */
+ USART_ClearFlag(UART2_TYPE, USART_FLAG_TC);
+ /* Enable DMA USART TX Stream */
+ DMA_Cmd(UART2_DMA_STREAM, ENABLE);
+ xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
+ xSemaphoreGive(uartBusy);
+ }
}
int uart2Putchar(int ch)
{
- //COMMENTED FIRMWARE
- //uart2SendData(1, (uint8_t *)&ch);
+ uart2SendData(1, (uint8_t *)&ch);
return (unsigned char)ch;
}
@@ -368,16 +363,15 @@ bool uart2DidOverrun()
void __attribute__((used)) DMA1_Stream6_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+ portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
- // // Stop and cleanup DMA stream
- // DMA_ITConfig(UART2_DMA_STREAM, DMA_IT_TC, DISABLE);
- // DMA_ClearITPendingBit(UART2_DMA_STREAM, UART2_DMA_FLAG_TCIF);
- // USART_DMACmd(UART2_TYPE, USART_DMAReq_Tx, DISABLE);
- // DMA_Cmd(UART2_DMA_STREAM, DISABLE);
+ // Stop and cleanup DMA stream
+ DMA_ITConfig(UART2_DMA_STREAM, DMA_IT_TC, DISABLE);
+ DMA_ClearITPendingBit(UART2_DMA_STREAM, UART2_DMA_FLAG_TCIF);
+ USART_DMACmd(UART2_TYPE, USART_DMAReq_Tx, DISABLE);
+ DMA_Cmd(UART2_DMA_STREAM, DISABLE);
- // xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
+ xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
}
#ifdef UART2_LINK_COMM
@@ -409,27 +403,26 @@ void __attribute__((used)) USART2_IRQHandler(void)
void __attribute__((used)) USART2_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- // uint8_t rxData;
- // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-
- // if ((UART2_TYPE->SR & (1<<5)) != 0) // fast check if the RXNE interrupt has occurred
- // {
- // rxData = USART_ReceiveData(UART2_TYPE) & 0x00FF;
- // xQueueSendFromISR(uart2queue, &rxData, &xHigherPriorityTaskWoken);
- // }
- // else
- // {
- // /** if we get here, the error is most likely caused by an overrun!
- // * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
- // * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
- // * - reading USART_SR register followed reading the USART_DR register.
- // */
- // asm volatile ("" : "=m" (UART2_TYPE->SR) : "r" (UART2_TYPE->SR)); // force non-optimizable reads
- // asm volatile ("" : "=m" (UART2_TYPE->DR) : "r" (UART2_TYPE->DR)); // of these two registers
-
- // hasOverrun = true;
- // }
+ uint8_t rxData;
+ portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+
+ if ((UART2_TYPE->SR & (1<<5)) != 0) // fast check if the RXNE interrupt has occurred
+ {
+ rxData = USART_ReceiveData(UART2_TYPE) & 0x00FF;
+ xQueueSendFromISR(uart2queue, &rxData, &xHigherPriorityTaskWoken);
+ }
+ else
+ {
+ /** if we get here, the error is most likely caused by an overrun!
+ * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
+ * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
+ * - reading USART_SR register followed reading the USART_DR register.
+ */
+ asm volatile ("" : "=m" (UART2_TYPE->SR) : "r" (UART2_TYPE->SR)); // force non-optimizable reads
+ asm volatile ("" : "=m" (UART2_TYPE->DR) : "r" (UART2_TYPE->DR)); // of these two registers
+
+ hasOverrun = true;
+ }
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart_syslink.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart_syslink.c
index c960da880..fd5df3e31 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart_syslink.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/uart_syslink.c
@@ -83,118 +83,116 @@ static void uartslkResumeDma();
*/
void uartslkDmaInit(void)
{
- //COMMENTED FIRMWARE
- // NVIC_InitTypeDef NVIC_InitStructure;
-
- // RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
-
- // // USART TX DMA Channel Config
- // DMA_InitStructureShare.DMA_PeripheralBaseAddr = (uint32_t)&UARTSLK_TYPE->DR;
- // DMA_InitStructureShare.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
- // DMA_InitStructureShare.DMA_MemoryInc = DMA_MemoryInc_Enable;
- // DMA_InitStructureShare.DMA_MemoryBurst = DMA_MemoryBurst_Single;
- // DMA_InitStructureShare.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
- // DMA_InitStructureShare.DMA_BufferSize = 0;
- // DMA_InitStructureShare.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- // DMA_InitStructureShare.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
- // DMA_InitStructureShare.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
- // DMA_InitStructureShare.DMA_DIR = DMA_DIR_MemoryToPeripheral;
- // DMA_InitStructureShare.DMA_Mode = DMA_Mode_Normal;
- // DMA_InitStructureShare.DMA_Priority = DMA_Priority_High;
- // DMA_InitStructureShare.DMA_FIFOMode = DMA_FIFOMode_Disable;
- // DMA_InitStructureShare.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
- // DMA_InitStructureShare.DMA_Channel = UARTSLK_DMA_CH;
-
- // NVIC_InitStructure.NVIC_IRQChannel = UARTSLK_DMA_IRQ;
- // NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
- // NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- // NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- // NVIC_Init(&NVIC_InitStructure);
-
- // isUartDmaInitialized = true;
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
+
+ // USART TX DMA Channel Config
+ DMA_InitStructureShare.DMA_PeripheralBaseAddr = (uint32_t)&UARTSLK_TYPE->DR;
+ DMA_InitStructureShare.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
+ DMA_InitStructureShare.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ DMA_InitStructureShare.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+ DMA_InitStructureShare.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ DMA_InitStructureShare.DMA_BufferSize = 0;
+ DMA_InitStructureShare.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ DMA_InitStructureShare.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+ DMA_InitStructureShare.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+ DMA_InitStructureShare.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+ DMA_InitStructureShare.DMA_Mode = DMA_Mode_Normal;
+ DMA_InitStructureShare.DMA_Priority = DMA_Priority_High;
+ DMA_InitStructureShare.DMA_FIFOMode = DMA_FIFOMode_Disable;
+ DMA_InitStructureShare.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
+ DMA_InitStructureShare.DMA_Channel = UARTSLK_DMA_CH;
+
+ NVIC_InitStructure.NVIC_IRQChannel = UARTSLK_DMA_IRQ;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_HIGH_PRI;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ isUartDmaInitialized = true;
}
void uartslkInit(void)
{
- //COMMENTED FIRMWARE
-// // initialize the FreeRTOS structures first, to prevent null pointers in interrupts
-// waitUntilSendDone = xSemaphoreCreateBinaryStatic(&waitUntilSendDoneBuffer); // initialized as blocking
-// uartBusy = xSemaphoreCreateBinaryStatic(&uartBusyBuffer); // initialized as blocking
-// xSemaphoreGive(uartBusy); // but we give it because the uart isn't busy at initialization
-
-// syslinkPacketDelivery = STATIC_MEM_QUEUE_CREATE(syslinkPacketDelivery);
-// DEBUG_QUEUE_MONITOR_REGISTER(syslinkPacketDelivery);
-
-// USART_InitTypeDef USART_InitStructure;
-// GPIO_InitTypeDef GPIO_InitStructure;
-// NVIC_InitTypeDef NVIC_InitStructure;
-// EXTI_InitTypeDef extiInit;
-
-// /* Enable GPIO and USART clock */
-// RCC_AHB1PeriphClockCmd(UARTSLK_GPIO_PERIF, ENABLE);
-// ENABLE_UARTSLK_RCC(UARTSLK_PERIF, ENABLE);
-
-// /* Configure USART Rx as input floating */
-// GPIO_InitStructure.GPIO_Pin = UARTSLK_GPIO_RX_PIN;
-// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
-// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
-// GPIO_Init(UARTSLK_GPIO_PORT, &GPIO_InitStructure);
-
-// /* Configure USART Tx as alternate function */
-// GPIO_InitStructure.GPIO_Pin = UARTSLK_GPIO_TX_PIN;
-// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
-// GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
-// GPIO_Init(UARTSLK_GPIO_PORT, &GPIO_InitStructure);
-
-// //Map uartslk to alternate functions
-// GPIO_PinAFConfig(UARTSLK_GPIO_PORT, UARTSLK_GPIO_AF_TX_PIN, UARTSLK_GPIO_AF_TX);
-// GPIO_PinAFConfig(UARTSLK_GPIO_PORT, UARTSLK_GPIO_AF_RX_PIN, UARTSLK_GPIO_AF_RX);
-
-// #if defined(UARTSLK_OUTPUT_TRACE_DATA) || defined(ADC_OUTPUT_RAW_DATA) || defined(IMU_OUTPUT_RAW_DATA_ON_UART)
-// USART_InitStructure.USART_BaudRate = 2000000;
-// USART_InitStructure.USART_Mode = USART_Mode_Tx;
-// #else
-// USART_InitStructure.USART_BaudRate = 1000000;
-// USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
-// #endif
-// USART_InitStructure.USART_WordLength = USART_WordLength_8b;
-// USART_InitStructure.USART_StopBits = USART_StopBits_1;
-// USART_InitStructure.USART_Parity = USART_Parity_No ;
-// USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
-// USART_Init(UARTSLK_TYPE, &USART_InitStructure);
-
-// uartslkDmaInit();
-
-// // Configure Rx buffer not empty interrupt
-// NVIC_InitStructure.NVIC_IRQChannel = UARTSLK_IRQ;
-// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SYSLINK_PRI;
-// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
-// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-// NVIC_Init(&NVIC_InitStructure);
-
-// USART_ITConfig(UARTSLK_TYPE, USART_IT_RXNE, ENABLE);
-
-// //Setting up TXEN pin (NRF flow control)
-// RCC_AHB1PeriphClockCmd(UARTSLK_TXEN_PERIF, ENABLE);
-
-// bzero(&GPIO_InitStructure, sizeof(GPIO_InitStructure));
-// GPIO_InitStructure.GPIO_Pin = UARTSLK_TXEN_PIN;
-// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
-// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
-// GPIO_Init(UARTSLK_TXEN_PORT, &GPIO_InitStructure);
-
-// extiInit.EXTI_Line = UARTSLK_TXEN_EXTI;
-// extiInit.EXTI_Mode = EXTI_Mode_Interrupt;
-// extiInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
-// extiInit.EXTI_LineCmd = ENABLE;
-// EXTI_Init(&extiInit);
-// EXTI_ClearITPendingBit(UARTSLK_TXEN_EXTI);
-
-// NVIC_EnableIRQ(EXTI4_IRQn);
-
-// //Enable UART
-// USART_Cmd(UARTSLK_TYPE, ENABLE);
+ // initialize the FreeRTOS structures first, to prevent null pointers in interrupts
+ waitUntilSendDone = xSemaphoreCreateBinaryStatic(&waitUntilSendDoneBuffer); // initialized as blocking
+ uartBusy = xSemaphoreCreateBinaryStatic(&uartBusyBuffer); // initialized as blocking
+ xSemaphoreGive(uartBusy); // but we give it because the uart isn't busy at initialization
+
+ syslinkPacketDelivery = STATIC_MEM_QUEUE_CREATE(syslinkPacketDelivery);
+ DEBUG_QUEUE_MONITOR_REGISTER(syslinkPacketDelivery);
+
+ USART_InitTypeDef USART_InitStructure;
+ GPIO_InitTypeDef GPIO_InitStructure;
+ NVIC_InitTypeDef NVIC_InitStructure;
+ EXTI_InitTypeDef extiInit;
+
+ /* Enable GPIO and USART clock */
+ RCC_AHB1PeriphClockCmd(UARTSLK_GPIO_PERIF, ENABLE);
+ ENABLE_UARTSLK_RCC(UARTSLK_PERIF, ENABLE);
+
+ /* Configure USART Rx as input floating */
+ GPIO_InitStructure.GPIO_Pin = UARTSLK_GPIO_RX_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+ GPIO_Init(UARTSLK_GPIO_PORT, &GPIO_InitStructure);
+
+ /* Configure USART Tx as alternate function */
+ GPIO_InitStructure.GPIO_Pin = UARTSLK_GPIO_TX_PIN;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_Init(UARTSLK_GPIO_PORT, &GPIO_InitStructure);
+
+ //Map uartslk to alternate functions
+ GPIO_PinAFConfig(UARTSLK_GPIO_PORT, UARTSLK_GPIO_AF_TX_PIN, UARTSLK_GPIO_AF_TX);
+ GPIO_PinAFConfig(UARTSLK_GPIO_PORT, UARTSLK_GPIO_AF_RX_PIN, UARTSLK_GPIO_AF_RX);
+
+#if defined(UARTSLK_OUTPUT_TRACE_DATA) || defined(ADC_OUTPUT_RAW_DATA) || defined(IMU_OUTPUT_RAW_DATA_ON_UART)
+ USART_InitStructure.USART_BaudRate = 2000000;
+ USART_InitStructure.USART_Mode = USART_Mode_Tx;
+#else
+ USART_InitStructure.USART_BaudRate = 1000000;
+ USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+#endif
+ USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+ USART_InitStructure.USART_StopBits = USART_StopBits_1;
+ USART_InitStructure.USART_Parity = USART_Parity_No ;
+ USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+ USART_Init(UARTSLK_TYPE, &USART_InitStructure);
+
+ uartslkDmaInit();
+
+ // Configure Rx buffer not empty interrupt
+ NVIC_InitStructure.NVIC_IRQChannel = UARTSLK_IRQ;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_SYSLINK_PRI;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ USART_ITConfig(UARTSLK_TYPE, USART_IT_RXNE, ENABLE);
+
+ //Setting up TXEN pin (NRF flow control)
+ RCC_AHB1PeriphClockCmd(UARTSLK_TXEN_PERIF, ENABLE);
+
+ bzero(&GPIO_InitStructure, sizeof(GPIO_InitStructure));
+ GPIO_InitStructure.GPIO_Pin = UARTSLK_TXEN_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+ GPIO_Init(UARTSLK_TXEN_PORT, &GPIO_InitStructure);
+
+ extiInit.EXTI_Line = UARTSLK_TXEN_EXTI;
+ extiInit.EXTI_Mode = EXTI_Mode_Interrupt;
+ extiInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
+ extiInit.EXTI_LineCmd = ENABLE;
+ EXTI_Init(&extiInit);
+ EXTI_ClearITPendingBit(UARTSLK_TXEN_EXTI);
+
+ NVIC_EnableIRQ(EXTI4_IRQn);
+
+ //Enable UART
+ USART_Cmd(UARTSLK_TYPE, ENABLE);
isInit = true;
}
@@ -210,273 +208,260 @@ void uartslkGetPacketBlocking(SyslinkPacket* packet)
void uartslkSendData(uint32_t size, uint8_t* data)
{
- //COMMENTED FIRMWARE
-// uint32_t i;
-
-// if (!isInit)
-// return;
-
-// for(i = 0; i < size; i++)
-// {
-// #ifdef UARTSLK_SPINLOOP_FLOWCTRL
-// while(GPIO_ReadInputDataBit(UARTSLK_TXEN_PORT, UARTSLK_TXEN_PIN) == Bit_SET);
-// #endif
-// while (!(UARTSLK_TYPE->SR & USART_FLAG_TXE));
-// UARTSLK_TYPE->DR = (data[i] & 0x00FF);
-// }
+ uint32_t i;
+
+ if (!isInit)
+ return;
+
+ for(i = 0; i < size; i++)
+ {
+#ifdef UARTSLK_SPINLOOP_FLOWCTRL
+ while(GPIO_ReadInputDataBit(UARTSLK_TXEN_PORT, UARTSLK_TXEN_PIN) == Bit_SET);
+#endif
+ while (!(UARTSLK_TYPE->SR & USART_FLAG_TXE));
+ UARTSLK_TYPE->DR = (data[i] & 0x00FF);
+ }
}
void uartslkSendDataIsrBlocking(uint32_t size, uint8_t* data)
{
- //COMMENTED FIRMWARE
- // xSemaphoreTake(uartBusy, portMAX_DELAY);
- // outDataIsr = data;
- // dataSizeIsr = size;
- // dataIndexIsr = 1;
- // uartslkSendData(1, &data[0]);
- // USART_ITConfig(UARTSLK_TYPE, USART_IT_TXE, ENABLE);
- // xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
- // outDataIsr = 0;
- // xSemaphoreGive(uartBusy);
+ xSemaphoreTake(uartBusy, portMAX_DELAY);
+ outDataIsr = data;
+ dataSizeIsr = size;
+ dataIndexIsr = 1;
+ uartslkSendData(1, &data[0]);
+ USART_ITConfig(UARTSLK_TYPE, USART_IT_TXE, ENABLE);
+ xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
+ outDataIsr = 0;
+ xSemaphoreGive(uartBusy);
}
int uartslkPutchar(int ch)
{
- //COMMENTED FIRMWARE
- //uartslkSendData(1, (uint8_t *)&ch);
+ uartslkSendData(1, (uint8_t *)&ch);
return (unsigned char)ch;
}
void uartslkSendDataDmaBlocking(uint32_t size, uint8_t* data)
{
- //COMMENTED FIRMWARE
- // if (isUartDmaInitialized)
- // {
- // xSemaphoreTake(uartBusy, portMAX_DELAY);
- // // Wait for DMA to be free
- // while(DMA_GetCmdStatus(UARTSLK_DMA_STREAM) != DISABLE);
- // //Copy data in DMA buffer
- // memcpy(dmaBuffer, data, size);
- // DMA_InitStructureShare.DMA_BufferSize = size;
- // initialDMACount = size;
- // // Init new DMA stream
- // DMA_Init(UARTSLK_DMA_STREAM, &DMA_InitStructureShare);
- // // Enable the Transfer Complete interrupt
- // DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, ENABLE);
- // /* Enable USART DMA TX Requests */
- // USART_DMACmd(UARTSLK_TYPE, USART_DMAReq_Tx, ENABLE);
- // /* Clear transfer complete */
- // USART_ClearFlag(UARTSLK_TYPE, USART_FLAG_TC);
- // /* Enable DMA USART TX Stream */
- // DMA_Cmd(UARTSLK_DMA_STREAM, ENABLE);
- // xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
- // xSemaphoreGive(uartBusy);
- // }
+ if (isUartDmaInitialized)
+ {
+ xSemaphoreTake(uartBusy, portMAX_DELAY);
+ // Wait for DMA to be free
+ while(DMA_GetCmdStatus(UARTSLK_DMA_STREAM) != DISABLE);
+ //Copy data in DMA buffer
+ memcpy(dmaBuffer, data, size);
+ DMA_InitStructureShare.DMA_BufferSize = size;
+ initialDMACount = size;
+ // Init new DMA stream
+ DMA_Init(UARTSLK_DMA_STREAM, &DMA_InitStructureShare);
+ // Enable the Transfer Complete interrupt
+ DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, ENABLE);
+ /* Enable USART DMA TX Requests */
+ USART_DMACmd(UARTSLK_TYPE, USART_DMAReq_Tx, ENABLE);
+ /* Clear transfer complete */
+ USART_ClearFlag(UARTSLK_TYPE, USART_FLAG_TC);
+ /* Enable DMA USART TX Stream */
+ DMA_Cmd(UARTSLK_DMA_STREAM, ENABLE);
+ xSemaphoreTake(waitUntilSendDone, portMAX_DELAY);
+ xSemaphoreGive(uartBusy);
+ }
}
static void uartslkPauseDma()
{
- //COMMENTED FIRMWARE
- // if (DMA_GetCmdStatus(UARTSLK_DMA_STREAM) == ENABLE)
- // {
- // // Disable transfer complete interrupt
- // DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, DISABLE);
- // // Disable stream to pause it
- // DMA_Cmd(UARTSLK_DMA_STREAM, DISABLE);
- // // Wait for it to be disabled
- // while(DMA_GetCmdStatus(UARTSLK_DMA_STREAM) != DISABLE);
- // // Disable transfer complete
- // DMA_ClearITPendingBit(UARTSLK_DMA_STREAM, UARTSLK_DMA_FLAG_TCIF);
- // // Read remaining data count
- // remainingDMACount = DMA_GetCurrDataCounter(UARTSLK_DMA_STREAM);
- // dmaIsPaused = true;
- // }
+ if (DMA_GetCmdStatus(UARTSLK_DMA_STREAM) == ENABLE)
+ {
+ // Disable transfer complete interrupt
+ DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, DISABLE);
+ // Disable stream to pause it
+ DMA_Cmd(UARTSLK_DMA_STREAM, DISABLE);
+ // Wait for it to be disabled
+ while(DMA_GetCmdStatus(UARTSLK_DMA_STREAM) != DISABLE);
+ // Disable transfer complete
+ DMA_ClearITPendingBit(UARTSLK_DMA_STREAM, UARTSLK_DMA_FLAG_TCIF);
+ // Read remaining data count
+ remainingDMACount = DMA_GetCurrDataCounter(UARTSLK_DMA_STREAM);
+ dmaIsPaused = true;
+ }
}
static void uartslkResumeDma()
{
- //COMMENTED FIRMWARE
- // if (dmaIsPaused)
- // {
- // // Update DMA counter
- // DMA_SetCurrDataCounter(UARTSLK_DMA_STREAM, remainingDMACount);
- // // Update memory read address
- // UARTSLK_DMA_STREAM->M0AR = (uint32_t)&dmaBuffer[initialDMACount - remainingDMACount];
- // // Enable the Transfer Complete interrupt
- // DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, ENABLE);
- // /* Clear transfer complete */
- // USART_ClearFlag(UARTSLK_TYPE, USART_FLAG_TC);
- // /* Enable DMA USART TX Stream */
- // DMA_Cmd(UARTSLK_DMA_STREAM, ENABLE);
- // dmaIsPaused = false;
- // }
+ if (dmaIsPaused)
+ {
+ // Update DMA counter
+ DMA_SetCurrDataCounter(UARTSLK_DMA_STREAM, remainingDMACount);
+ // Update memory read address
+ UARTSLK_DMA_STREAM->M0AR = (uint32_t)&dmaBuffer[initialDMACount - remainingDMACount];
+ // Enable the Transfer Complete interrupt
+ DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, ENABLE);
+ /* Clear transfer complete */
+ USART_ClearFlag(UARTSLK_TYPE, USART_FLAG_TC);
+ /* Enable DMA USART TX Stream */
+ DMA_Cmd(UARTSLK_DMA_STREAM, ENABLE);
+ dmaIsPaused = false;
+ }
}
void uartslkDmaIsr(void)
{
- //COMMENTED FIRMWARE
- // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+ portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
- // // Stop and cleanup DMA stream
- // DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, DISABLE);
- // DMA_ClearITPendingBit(UARTSLK_DMA_STREAM, UARTSLK_DMA_FLAG_TCIF);
- // USART_DMACmd(UARTSLK_TYPE, USART_DMAReq_Tx, DISABLE);
- // DMA_Cmd(UARTSLK_DMA_STREAM, DISABLE);
+ // Stop and cleanup DMA stream
+ DMA_ITConfig(UARTSLK_DMA_STREAM, DMA_IT_TC, DISABLE);
+ DMA_ClearITPendingBit(UARTSLK_DMA_STREAM, UARTSLK_DMA_FLAG_TCIF);
+ USART_DMACmd(UARTSLK_TYPE, USART_DMAReq_Tx, DISABLE);
+ DMA_Cmd(UARTSLK_DMA_STREAM, DISABLE);
- // remainingDMACount = 0;
- // xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
+ remainingDMACount = 0;
+ xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
}
void uartslkHandleDataFromISR(uint8_t c, BaseType_t * const pxHigherPriorityTaskWoken)
{
- //COMMENTED FIRMWARE
- // switch (rxState)
- // {
- // case waitForFirstStart:
- // rxState = (c == SYSLINK_START_BYTE1) ? waitForSecondStart : waitForFirstStart;
- // break;
- // case waitForSecondStart:
- // rxState = (c == SYSLINK_START_BYTE2) ? waitForType : waitForFirstStart;
- // break;
- // case waitForType:
- // cksum[0] = c;
- // cksum[1] = c;
- // slp.type = c;
- // rxState = waitForLength;
- // break;
- // case waitForLength:
- // if (c <= SYSLINK_MTU)
- // {
- // slp.length = c;
- // cksum[0] += c;
- // cksum[1] += cksum[0];
- // dataIndex = 0;
- // rxState = (c > 0) ? waitForData : waitForChksum1;
- // }
- // else
- // {
- // rxState = waitForFirstStart;
- // }
- // break;
- // case waitForData:
- // slp.data[dataIndex] = c;
- // cksum[0] += c;
- // cksum[1] += cksum[0];
- // dataIndex++;
- // if (dataIndex == slp.length)
- // {
- // rxState = waitForChksum1;
- // }
- // break;
- // case waitForChksum1:
- // if (cksum[0] == c)
- // {
- // rxState = waitForChksum2;
- // }
- // else
- // {
- // rxState = waitForFirstStart; //Checksum error
- // ASSERT(0);
- // }
- // break;
- // case waitForChksum2:
- // if (cksum[1] == c)
- // {
- // // Post the packet to the queue if there's room
- // if (!xQueueIsQueueFullFromISR(syslinkPacketDelivery))
- // {
- // xQueueSendFromISR(syslinkPacketDelivery, (void *)&slp, pxHigherPriorityTaskWoken);
- // }
- // else
- // {
- // ASSERT(0); // Queue overflow
- // }
- // }
- // else
- // {
- // rxState = waitForFirstStart; //Checksum error
- // ASSERT(0);
- // }
- // rxState = waitForFirstStart;
- // break;
- // default:
- // ASSERT(0);
- // break;
- // }
+ switch (rxState)
+ {
+ case waitForFirstStart:
+ rxState = (c == SYSLINK_START_BYTE1) ? waitForSecondStart : waitForFirstStart;
+ break;
+ case waitForSecondStart:
+ rxState = (c == SYSLINK_START_BYTE2) ? waitForType : waitForFirstStart;
+ break;
+ case waitForType:
+ cksum[0] = c;
+ cksum[1] = c;
+ slp.type = c;
+ rxState = waitForLength;
+ break;
+ case waitForLength:
+ if (c <= SYSLINK_MTU)
+ {
+ slp.length = c;
+ cksum[0] += c;
+ cksum[1] += cksum[0];
+ dataIndex = 0;
+ rxState = (c > 0) ? waitForData : waitForChksum1;
+ }
+ else
+ {
+ rxState = waitForFirstStart;
+ }
+ break;
+ case waitForData:
+ slp.data[dataIndex] = c;
+ cksum[0] += c;
+ cksum[1] += cksum[0];
+ dataIndex++;
+ if (dataIndex == slp.length)
+ {
+ rxState = waitForChksum1;
+ }
+ break;
+ case waitForChksum1:
+ if (cksum[0] == c)
+ {
+ rxState = waitForChksum2;
+ }
+ else
+ {
+ rxState = waitForFirstStart; //Checksum error
+ ASSERT(0);
+ }
+ break;
+ case waitForChksum2:
+ if (cksum[1] == c)
+ {
+ // Post the packet to the queue if there's room
+ if (!xQueueIsQueueFullFromISR(syslinkPacketDelivery))
+ {
+ xQueueSendFromISR(syslinkPacketDelivery, (void *)&slp, pxHigherPriorityTaskWoken);
+ }
+ else
+ {
+ ASSERT(0); // Queue overflow
+ }
+ }
+ else
+ {
+ rxState = waitForFirstStart; //Checksum error
+ ASSERT(0);
+ }
+ rxState = waitForFirstStart;
+ break;
+ default:
+ ASSERT(0);
+ break;
+ }
}
void uartslkIsr(void)
{
- //COMMENTED FIRMWARE
- // portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-
- // // the following if statement replaces:
- // // if (USART_GetITStatus(UARTSLK_TYPE, USART_IT_RXNE) == SET)
- // // we do this check as fast as possible to minimize the chance of an overrun,
- // // which occasionally cause problems and cause packet loss at high CPU usage
- // if ((UARTSLK_TYPE->SR & (1<<5)) != 0) // if the RXNE interrupt has occurred
- // {
- // uint8_t rxDataInterrupt = (uint8_t)(UARTSLK_TYPE->DR & 0xFF);
- // uartslkHandleDataFromISR(rxDataInterrupt, &xHigherPriorityTaskWoken);
- // }
- // else if (USART_GetITStatus(UARTSLK_TYPE, USART_IT_TXE) == SET)
- // {
- // if (outDataIsr && (dataIndexIsr < dataSizeIsr))
- // {
- // USART_SendData(UARTSLK_TYPE, outDataIsr[dataIndexIsr] & 0x00FF);
- // dataIndexIsr++;
- // }
- // else
- // {
- // USART_ITConfig(UARTSLK_TYPE, USART_IT_TXE, DISABLE);
- // xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
- // }
- // }
- // else
- // {
- // /** if we get here, the error is most likely caused by an overrun!
- // * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
- // * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
- // * - reading USART_SR register followed reading the USART_DR register.
- // */
- // asm volatile ("" : "=m" (UARTSLK_TYPE->SR) : "r" (UARTSLK_TYPE->SR)); // force non-optimizable reads
- // asm volatile ("" : "=m" (UARTSLK_TYPE->DR) : "r" (UARTSLK_TYPE->DR)); // of these two registers
- // }
-
- // portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+ portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+
+ // the following if statement replaces:
+ // if (USART_GetITStatus(UARTSLK_TYPE, USART_IT_RXNE) == SET)
+ // we do this check as fast as possible to minimize the chance of an overrun,
+ // which occasionally cause problems and cause packet loss at high CPU usage
+ if ((UARTSLK_TYPE->SR & (1<<5)) != 0) // if the RXNE interrupt has occurred
+ {
+ uint8_t rxDataInterrupt = (uint8_t)(UARTSLK_TYPE->DR & 0xFF);
+ uartslkHandleDataFromISR(rxDataInterrupt, &xHigherPriorityTaskWoken);
+ }
+ else if (USART_GetITStatus(UARTSLK_TYPE, USART_IT_TXE) == SET)
+ {
+ if (outDataIsr && (dataIndexIsr < dataSizeIsr))
+ {
+ USART_SendData(UARTSLK_TYPE, outDataIsr[dataIndexIsr] & 0x00FF);
+ dataIndexIsr++;
+ }
+ else
+ {
+ USART_ITConfig(UARTSLK_TYPE, USART_IT_TXE, DISABLE);
+ xSemaphoreGiveFromISR(waitUntilSendDone, &xHigherPriorityTaskWoken);
+ }
+ }
+ else
+ {
+ /** if we get here, the error is most likely caused by an overrun!
+ * - PE (Parity error), FE (Framing error), NE (Noise error), ORE (OverRun error)
+ * - and IDLE (Idle line detected) pending bits are cleared by software sequence:
+ * - reading USART_SR register followed reading the USART_DR register.
+ */
+ asm volatile ("" : "=m" (UARTSLK_TYPE->SR) : "r" (UARTSLK_TYPE->SR)); // force non-optimizable reads
+ asm volatile ("" : "=m" (UARTSLK_TYPE->DR) : "r" (UARTSLK_TYPE->DR)); // of these two registers
+ }
+
+ portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
void uartslkTxenFlowctrlIsr()
{
- //COMMENTED FIRMWARE
- // EXTI_ClearFlag(UARTSLK_TXEN_EXTI);
- // if (GPIO_ReadInputDataBit(UARTSLK_TXEN_PORT, UARTSLK_TXEN_PIN) == Bit_SET)
- // {
- // uartslkPauseDma();
- // //ledSet(LED_GREEN_R, 1);
- // }
- // else
- // {
- // uartslkResumeDma();
- // //ledSet(LED_GREEN_R, 0);
- // }
+ EXTI_ClearFlag(UARTSLK_TXEN_EXTI);
+ if (GPIO_ReadInputDataBit(UARTSLK_TXEN_PORT, UARTSLK_TXEN_PIN) == Bit_SET)
+ {
+ uartslkPauseDma();
+ //ledSet(LED_GREEN_R, 1);
+ }
+ else
+ {
+ uartslkResumeDma();
+ //ledSet(LED_GREEN_R, 0);
+ }
}
void __attribute__((used)) EXTI4_Callback(void)
{
- //COMMENTED FIRMWARE
- //uartslkTxenFlowctrlIsr();
+ uartslkTxenFlowctrlIsr();
}
void __attribute__((used)) USART6_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- //uartslkIsr();
+ uartslkIsr();
}
void __attribute__((used)) DMA2_Stream7_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- //uartslkDmaIsr();
+ uartslkDmaIsr();
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l0x.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l0x.c
index 49b006c39..b28be495c 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l0x.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l0x.c
@@ -104,27 +104,26 @@ static int nextI2CAddress = VL53L0X_DEFAULT_ADDRESS+1;
bool vl53l0xInit(VL53L0xDev* dev, I2C_Dev *I2Cx, bool io_2V8)
{
- //COMMENTED FIRMWARE
- // dev->I2Cx = I2Cx;
- // dev->devAddr = VL53L0X_DEFAULT_ADDRESS;
-
- // dev->io_timeout = 0;
- // dev->did_timeout = 0;
- // dev->timeout_start_ms = 0;
- // dev->stop_variable = 0;
- // dev->measurement_timing_budget_us = 0;
- // dev->measurement_timing_budget_ms = 0;
-
- // if (!vl53l0xInitSensor(dev, io_2V8)) {
- // return false;
- // }
+ dev->I2Cx = I2Cx;
+ dev->devAddr = VL53L0X_DEFAULT_ADDRESS;
+
+ dev->io_timeout = 0;
+ dev->did_timeout = 0;
+ dev->timeout_start_ms = 0;
+ dev->stop_variable = 0;
+ dev->measurement_timing_budget_us = 0;
+ dev->measurement_timing_budget_ms = 0;
+
+ if (!vl53l0xInitSensor(dev, io_2V8)) {
+ return false;
+ }
- // /* Move initialized sensor to a new I2C address */
- // int newAddress;
+ /* Move initialized sensor to a new I2C address */
+ int newAddress;
- // taskENTER_CRITICAL();
- // newAddress = nextI2CAddress++;
- // taskEXIT_CRITICAL();
+ taskENTER_CRITICAL();
+ newAddress = nextI2CAddress++;
+ taskEXIT_CRITICAL();
return vl53l0xSetI2CAddress(dev, newAddress);
}
@@ -136,9 +135,8 @@ bool vl53l0xInit(VL53L0xDev* dev, I2C_Dev *I2Cx, bool io_2V8)
bool vl53l0xTestConnection(VL53L0xDev* dev)
{
bool ret = true;
- //COMMENTED FIRMWARE
- // ret &= vl53l0xGetModelID(dev) == VL53L0X_IDENTIFICATION_MODEL_ID;
- // ret &= vl53l0xGetRevisionID(dev) == VL53L0X_IDENTIFICATION_REVISION_ID;
+ ret &= vl53l0xGetModelID(dev) == VL53L0X_IDENTIFICATION_MODEL_ID;
+ ret &= vl53l0xGetRevisionID(dev) == VL53L0X_IDENTIFICATION_REVISION_ID;
return ret;
}
@@ -164,8 +162,7 @@ uint16_t vl53l0xGetModelID(VL53L0xDev* dev)
uint8_t vl53l0xGetRevisionID(VL53L0xDev* dev)
{
uint8_t output = 0;
- //COMMENTED FIRMWARE
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_IDENTIFICATION_REVISION_ID, &output);
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_IDENTIFICATION_REVISION_ID, &output);
return output;
}
@@ -176,8 +173,7 @@ uint8_t vl53l0xGetRevisionID(VL53L0xDev* dev)
*/
bool vl53l0xSetI2CAddress(VL53L0xDev* dev, uint8_t address) {
bool pass = i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS, address);
- //COMMENTED FIRMWARE
- //dev->devAddr = address;
+ dev->devAddr = address;
return pass;
}
@@ -191,226 +187,225 @@ bool vl53l0xSetI2CAddress(VL53L0xDev* dev, uint8_t address) {
// mode.
bool vl53l0xInitSensor(VL53L0xDev* dev, bool io_2v8)
{
- //COMMENTED FIRMWARE
- // uint8_t temp;
- // // VL53L0X_DataInit() begin
-
- // // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
- // if (io_2v8)
- // {
- // i2cdevWriteBit(dev->I2Cx, dev->devAddr, VL53L0X_RA_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0, 0x01);
- // }
-
- // // "Set I2C standard mode"
- // if (!i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x88, 0x00)) {
- // return false;
- // }
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x91, &dev->stop_variable);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
-
- // // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_CONTROL, &temp);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_CONTROL, temp | 0x12);
-
- // // set final range signal rate limit to 0.25 MCPS (million counts per second)
- // vl53l0xSetSignalRateLimit(dev, 0.25);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xFF);
-
- // // VL53L0X_DataInit() end
-
- // // VL53L0X_StaticInit() begin
-
- // uint8_t spad_count;
- // bool spad_type_is_aperture;
- // if (!vl53l0xGetSpadInfo(dev, &spad_count, &spad_type_is_aperture)) { return false; }
-
- // // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
- // // the API, but the same data seems to be more easily readable from
- // // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
- // uint8_t ref_spad_map[6];
- // i2cdevReadReg8(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6, ref_spad_map);
-
- // // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
-
- // uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad
- // uint8_t spads_enabled = 0;
-
- // for (uint8_t i = 0; i < 48; i++)
- // {
- // if (i < first_spad_to_enable || spads_enabled == spad_count)
- // {
- // // This bit is lower than the first one that should be enabled, or
- // // (reference_spad_count) bits have already been enabled, so zero this bit
- // ref_spad_map[i / 8] &= ~(1 << (i % 8));
- // }
- // else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1)
- // {
- // spads_enabled++;
- // }
- // }
-
- // i2cdevWriteReg8(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6, ref_spad_map);
-
- // // -- VL53L0X_set_reference_spads() end
-
- // // -- VL53L0X_load_tuning_settings() begin
- // // DefaultTuningSettings from vl53l0x_tuning.h
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x09, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x10, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x11, 0x00);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x24, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x25, 0xFF);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x75, 0x00);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4E, 0x2C);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x48, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x30, 0x20);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x30, 0x09);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x54, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x31, 0x04);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x32, 0x03);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x40, 0x83);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x46, 0x25);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x60, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x27, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x50, 0x06);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x51, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x52, 0x96);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x56, 0x08);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x57, 0x30);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x61, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x62, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x64, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x65, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x66, 0xA0);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x22, 0x32);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x47, 0x14);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x49, 0xFF);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4A, 0x00);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x7A, 0x0A);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x7B, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x78, 0x21);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x23, 0x34);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x42, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x44, 0xFF);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x45, 0x26);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x46, 0x05);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x40, 0x40);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x0E, 0x06);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x20, 0x1A);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x43, 0x40);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x34, 0x03);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x35, 0x44);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x31, 0x04);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4B, 0x09);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4C, 0x05);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4D, 0x04);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x44, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x45, 0x20);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x47, 0x08);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x48, 0x28);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x67, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x70, 0x04);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x71, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x72, 0xFE);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x76, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x77, 0x00);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x0D, 0x01);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x01, 0xF8);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x8E, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
-
- // // -- VL53L0X_load_tuning_settings() end
-
- // // "Set interrupt config to new sample ready"
- // // -- VL53L0X_SetGpioConfig() begin
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
- // i2cdevWriteBit(dev->I2Cx, dev->devAddr, VL53L0X_RA_GPIO_HV_MUX_ACTIVE_HIGH, 4, 0); // active low
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
-
- // // -- VL53L0X_SetGpioConfig() end
-
- // dev->measurement_timing_budget_us = vl53l0xGetMeasurementTimingBudget(dev);
- // dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
-
- // // "Disable MSRC and TCC by default"
- // // MSRC = Minimum Signal Rate Check
- // // TCC = Target CentreCheck
- // // -- VL53L0X_SetSequenceStepEnable() begin
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xE8);
-
- // // -- VL53L0X_SetSequenceStepEnable() end
-
- // // "Recalculate timing budget"
- // vl53l0xSetMeasurementTimingBudget(dev, dev->measurement_timing_budget_us);
-
- // // VL53L0X_StaticInit() end
-
- // // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration())
-
- // // -- VL53L0X_perform_vhv_calibration() begin
+ uint8_t temp;
+ // VL53L0X_DataInit() begin
+
+ // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
+ if (io_2v8)
+ {
+ i2cdevWriteBit(dev->I2Cx, dev->devAddr, VL53L0X_RA_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0, 0x01);
+ }
+
+ // "Set I2C standard mode"
+ if (!i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x88, 0x00)) {
+ return false;
+ }
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x91, &dev->stop_variable);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
+
+ // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_CONTROL, &temp);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_CONTROL, temp | 0x12);
+
+ // set final range signal rate limit to 0.25 MCPS (million counts per second)
+ vl53l0xSetSignalRateLimit(dev, 0.25);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xFF);
+
+ // VL53L0X_DataInit() end
+
+ // VL53L0X_StaticInit() begin
+
+ uint8_t spad_count;
+ bool spad_type_is_aperture;
+ if (!vl53l0xGetSpadInfo(dev, &spad_count, &spad_type_is_aperture)) { return false; }
+
+ // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
+ // the API, but the same data seems to be more easily readable from
+ // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
+ uint8_t ref_spad_map[6];
+ i2cdevReadReg8(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6, ref_spad_map);
+
+ // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
+
+ uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad
+ uint8_t spads_enabled = 0;
+
+ for (uint8_t i = 0; i < 48; i++)
+ {
+ if (i < first_spad_to_enable || spads_enabled == spad_count)
+ {
+ // This bit is lower than the first one that should be enabled, or
+ // (reference_spad_count) bits have already been enabled, so zero this bit
+ ref_spad_map[i / 8] &= ~(1 << (i % 8));
+ }
+ else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1)
+ {
+ spads_enabled++;
+ }
+ }
+
+ i2cdevWriteReg8(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 6, ref_spad_map);
+
+ // -- VL53L0X_set_reference_spads() end
+
+ // -- VL53L0X_load_tuning_settings() begin
+ // DefaultTuningSettings from vl53l0x_tuning.h
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x09, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x10, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x11, 0x00);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x24, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x25, 0xFF);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x75, 0x00);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4E, 0x2C);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x48, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x30, 0x20);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x30, 0x09);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x54, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x31, 0x04);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x32, 0x03);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x40, 0x83);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x46, 0x25);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x60, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x27, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x50, 0x06);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x51, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x52, 0x96);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x56, 0x08);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x57, 0x30);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x61, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x62, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x64, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x65, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x66, 0xA0);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x22, 0x32);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x47, 0x14);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x49, 0xFF);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4A, 0x00);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x7A, 0x0A);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x7B, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x78, 0x21);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x23, 0x34);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x42, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x44, 0xFF);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x45, 0x26);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x46, 0x05);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x40, 0x40);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x0E, 0x06);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x20, 0x1A);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x43, 0x40);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x34, 0x03);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x35, 0x44);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x31, 0x04);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4B, 0x09);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4C, 0x05);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x4D, 0x04);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x44, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x45, 0x20);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x47, 0x08);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x48, 0x28);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x67, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x70, 0x04);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x71, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x72, 0xFE);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x76, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x77, 0x00);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x0D, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x01);
- // if (!vl53l0xPerformSingleRefCalibration(dev, 0x40)) { DEBUG_PRINT("Failed VHV calibration\n"); return false; }
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x01, 0xF8);
- // // -- VL53L0X_perform_vhv_calibration() end
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x8E, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
+
+ // -- VL53L0X_load_tuning_settings() end
+
+ // "Set interrupt config to new sample ready"
+ // -- VL53L0X_SetGpioConfig() begin
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
+ i2cdevWriteBit(dev->I2Cx, dev->devAddr, VL53L0X_RA_GPIO_HV_MUX_ACTIVE_HIGH, 4, 0); // active low
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
+
+ // -- VL53L0X_SetGpioConfig() end
+
+ dev->measurement_timing_budget_us = vl53l0xGetMeasurementTimingBudget(dev);
+ dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
+
+ // "Disable MSRC and TCC by default"
+ // MSRC = Minimum Signal Rate Check
+ // TCC = Target CentreCheck
+ // -- VL53L0X_SetSequenceStepEnable() begin
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xE8);
+
+ // -- VL53L0X_SetSequenceStepEnable() end
+
+ // "Recalculate timing budget"
+ vl53l0xSetMeasurementTimingBudget(dev, dev->measurement_timing_budget_us);
+
+ // VL53L0X_StaticInit() end
+
+ // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration())
+
+ // -- VL53L0X_perform_vhv_calibration() begin
- // // -- VL53L0X_perform_phase_calibration() begin
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x01);
+ if (!vl53l0xPerformSingleRefCalibration(dev, 0x40)) { DEBUG_PRINT("Failed VHV calibration\n"); return false; }
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x02);
- // if (!vl53l0xPerformSingleRefCalibration(dev, 0x00)) { DEBUG_PRINT("Failed phase calibration\n"); return false; }
+ // -- VL53L0X_perform_vhv_calibration() end
- // // -- VL53L0X_perform_phase_calibration() end
+ // -- VL53L0X_perform_phase_calibration() begin
- // // "restore the previous Sequence Config"
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xE8);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x02);
+ if (!vl53l0xPerformSingleRefCalibration(dev, 0x00)) { DEBUG_PRINT("Failed phase calibration\n"); return false; }
- // // VL53L0X_PerformRefCalibration() end
+ // -- VL53L0X_perform_phase_calibration() end
+
+ // "restore the previous Sequence Config"
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0xE8);
+
+ // VL53L0X_PerformRefCalibration() end
return true;
}
@@ -425,11 +420,10 @@ bool vl53l0xInitSensor(VL53L0xDev* dev, bool io_2v8)
// Defaults to 0.25 MCPS as initialized by the ST API and this library.
bool vl53l0xSetSignalRateLimit(VL53L0xDev* dev, float limit_Mcps)
{
- //COMMENTED FIRMWARE
- // if (limit_Mcps < 0 || limit_Mcps > 511.99f) { return false; }
+ if (limit_Mcps < 0 || limit_Mcps > 511.99f) { return false; }
- // // Q9.7 fixed point format (9 integer bits, 7 fractional bits)
- // uint16_t fixed_pt = limit_Mcps * (1 << 7);
+ // Q9.7 fixed point format (9 integer bits, 7 fractional bits)
+ uint16_t fixed_pt = limit_Mcps * (1 << 7);
return vl53l0xWriteReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, fixed_pt);
}
@@ -442,89 +436,88 @@ bool vl53l0xSetSignalRateLimit(VL53L0xDev* dev, float limit_Mcps)
// based on VL53L0X_set_measurement_timing_budget_micro_seconds()
bool vl53l0xSetMeasurementTimingBudget(VL53L0xDev* dev, uint32_t budget_us)
{
- //COMMENTED FIRMWARE
- // SequenceStepEnables enables;
- // SequenceStepTimeouts timeouts;
-
- // uint16_t const StartOverhead = 1320; // note that this is different than the value in get_
- // uint16_t const EndOverhead = 960;
- // uint16_t const MsrcOverhead = 660;
- // uint16_t const TccOverhead = 590;
- // uint16_t const DssOverhead = 690;
- // uint16_t const PreRangeOverhead = 660;
- // uint16_t const FinalRangeOverhead = 550;
-
- // uint32_t const MinTimingBudget = 20000;
-
- // if (budget_us < MinTimingBudget) { return false; }
-
- // uint32_t used_budget_us = StartOverhead + EndOverhead;
-
- // vl53l0xGetSequenceStepEnables(dev, &enables);
- // vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
-
- // if (enables.tcc)
- // {
- // used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
- // }
-
- // if (enables.dss)
- // {
- // used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
- // }
- // else if (enables.msrc)
- // {
- // used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
- // }
-
- // if (enables.pre_range)
- // {
- // used_budget_us += (timeouts.pre_range_us + PreRangeOverhead);
- // }
-
- // if (enables.final_range)
- // {
- // used_budget_us += FinalRangeOverhead;
-
- // // "Note that the final range timeout is determined by the timing
- // // budget and the sum of all other timeouts within the sequence.
- // // If there is no room for the final range timeout, then an error
- // // will be set. Otherwise the remaining time will be applied to
- // // the final range."
-
- // if (used_budget_us > budget_us)
- // {
- // // "Requested timeout too big."
- // return false;
- // }
-
- // uint32_t final_range_timeout_us = budget_us - used_budget_us;
-
- // // set_sequence_step_timeout() begin
- // // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
-
- // // "For the final range timeout, the pre-range timeout
- // // must be added. To do this both final and pre-range
- // // timeouts must be expressed in macro periods MClks
- // // because they have different vcsel periods."
-
- // uint16_t final_range_timeout_mclks =
- // vl53l0xTimeoutMicrosecondsToMclks(final_range_timeout_us,
- // timeouts.final_range_vcsel_period_pclks);
-
- // if (enables.pre_range)
- // {
- // final_range_timeout_mclks += timeouts.pre_range_mclks;
- // }
-
- // uint16_t temp = vl53l0xEncodeTimeout(final_range_timeout_mclks);
- // vl53l0xWriteReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, temp);
-
- // // set_sequence_step_timeout() end
-
- // dev->measurement_timing_budget_us = budget_us; // store for internal reuse
- // dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
- // }
+ SequenceStepEnables enables;
+ SequenceStepTimeouts timeouts;
+
+ uint16_t const StartOverhead = 1320; // note that this is different than the value in get_
+ uint16_t const EndOverhead = 960;
+ uint16_t const MsrcOverhead = 660;
+ uint16_t const TccOverhead = 590;
+ uint16_t const DssOverhead = 690;
+ uint16_t const PreRangeOverhead = 660;
+ uint16_t const FinalRangeOverhead = 550;
+
+ uint32_t const MinTimingBudget = 20000;
+
+ if (budget_us < MinTimingBudget) { return false; }
+
+ uint32_t used_budget_us = StartOverhead + EndOverhead;
+
+ vl53l0xGetSequenceStepEnables(dev, &enables);
+ vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
+
+ if (enables.tcc)
+ {
+ used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
+ }
+
+ if (enables.dss)
+ {
+ used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
+ }
+ else if (enables.msrc)
+ {
+ used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
+ }
+
+ if (enables.pre_range)
+ {
+ used_budget_us += (timeouts.pre_range_us + PreRangeOverhead);
+ }
+
+ if (enables.final_range)
+ {
+ used_budget_us += FinalRangeOverhead;
+
+ // "Note that the final range timeout is determined by the timing
+ // budget and the sum of all other timeouts within the sequence.
+ // If there is no room for the final range timeout, then an error
+ // will be set. Otherwise the remaining time will be applied to
+ // the final range."
+
+ if (used_budget_us > budget_us)
+ {
+ // "Requested timeout too big."
+ return false;
+ }
+
+ uint32_t final_range_timeout_us = budget_us - used_budget_us;
+
+ // set_sequence_step_timeout() begin
+ // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
+
+ // "For the final range timeout, the pre-range timeout
+ // must be added. To do this both final and pre-range
+ // timeouts must be expressed in macro periods MClks
+ // because they have different vcsel periods."
+
+ uint16_t final_range_timeout_mclks =
+ vl53l0xTimeoutMicrosecondsToMclks(final_range_timeout_us,
+ timeouts.final_range_vcsel_period_pclks);
+
+ if (enables.pre_range)
+ {
+ final_range_timeout_mclks += timeouts.pre_range_mclks;
+ }
+
+ uint16_t temp = vl53l0xEncodeTimeout(final_range_timeout_mclks);
+ vl53l0xWriteReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, temp);
+
+ // set_sequence_step_timeout() end
+
+ dev->measurement_timing_budget_us = budget_us; // store for internal reuse
+ dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
+ }
return true;
}
@@ -546,36 +539,36 @@ uint32_t vl53l0xGetMeasurementTimingBudget(VL53L0xDev* dev)
// "Start and end overhead times always present"
uint32_t budget_us = StartOverhead + EndOverhead;
- //COMMENTED FIRMWARE
- // vl53l0xGetSequenceStepEnables(dev, &enables);
- // vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
-
- // if (enables.tcc)
- // {
- // budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
- // }
-
- // if (enables.dss)
- // {
- // budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
- // }
- // else if (enables.msrc)
- // {
- // budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
- // }
-
- // if (enables.pre_range)
- // {
- // budget_us += (timeouts.pre_range_us + PreRangeOverhead);
- // }
-
- // if (enables.final_range)
- // {
- // budget_us += (timeouts.final_range_us + FinalRangeOverhead);
- // }
-
- // dev->measurement_timing_budget_us = budget_us; // store for internal reuse
- // dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
+
+ vl53l0xGetSequenceStepEnables(dev, &enables);
+ vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
+
+ if (enables.tcc)
+ {
+ budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
+ }
+
+ if (enables.dss)
+ {
+ budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
+ }
+ else if (enables.msrc)
+ {
+ budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
+ }
+
+ if (enables.pre_range)
+ {
+ budget_us += (timeouts.pre_range_us + PreRangeOverhead);
+ }
+
+ if (enables.final_range)
+ {
+ budget_us += (timeouts.final_range_us + FinalRangeOverhead);
+ }
+
+ dev->measurement_timing_budget_us = budget_us; // store for internal reuse
+ dev->measurement_timing_budget_ms = (uint16_t)(dev->measurement_timing_budget_us / 1000.0f);
return budget_us;
}
@@ -589,177 +582,176 @@ uint32_t vl53l0xGetMeasurementTimingBudget(VL53L0xDev* dev)
// based on VL53L0X_set_vcsel_pulse_period()
bool vl53l0xSetVcselPulsePeriod(VL53L0xDev* dev, vcselPeriodType type, uint8_t period_pclks)
{
- //COMMENTED FIRMWARE
- // uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks);
-
- // SequenceStepEnables enables;
- // SequenceStepTimeouts timeouts;
-
- // vl53l0xGetSequenceStepEnables(dev, &enables);
- // vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
-
- // // "Apply specific settings for the requested clock period"
- // // "Re-calculate and apply timeouts, in macro periods"
-
- // // "When the VCSEL period for the pre or final range is changed,
- // // the corresponding timeout must be read from the device using
- // // the current VCSEL period, then the new VCSEL period can be
- // // applied. The timeout then must be written back to the device
- // // using the new VCSEL period.
- // //
- // // For the MSRC timeout, the same applies - this timeout being
- // // dependant on the pre-range vcsel period."
-
-
- // if (type == VcselPeriodPreRange)
- // {
- // // "Set phase check limits"
- // switch (period_pclks)
- // {
- // case 12:
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
- // break;
-
- // case 14:
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
- // break;
-
- // case 16:
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
- // break;
-
- // case 18:
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
- // break;
-
- // default:
- // // invalid period
- // return false;
- // }
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
-
- // // apply new VCSEL period
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
-
- // // update timeouts
-
- // // set_sequence_step_timeout() begin
- // // (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE)
-
- // uint16_t new_pre_range_timeout_mclks =
- // vl53l0xTimeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks);
-
- // uint16_t new_pre_range_timeout_encoded = vl53l0xEncodeTimeout(new_pre_range_timeout_mclks);
- // vl53l0xWriteReg16Bit(dev, VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, new_pre_range_timeout_encoded);
-
- // // set_sequence_step_timeout() end
-
- // // set_sequence_step_timeout() begin
- // // (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)
-
- // uint16_t new_msrc_timeout_mclks =
- // vl53l0xTimeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP,
- // (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1));
-
- // // set_sequence_step_timeout() end
- // }
- // else if (type == VcselPeriodFinalRange)
- // {
- // switch (period_pclks)
- // {
- // case 8:
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x30);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // break;
-
- // case 10:
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // break;
-
- // case 12:
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // break;
-
- // case 14:
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // break;
-
- // default:
- // // invalid period
- // return false;
- // }
-
- // // apply new VCSEL period
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
-
- // // update timeouts
-
- // // set_sequence_step_timeout() begin
- // // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
-
- // // "For the final range timeout, the pre-range timeout
- // // must be added. To do this both final and pre-range
- // // timeouts must be expressed in macro periods MClks
- // // because they have different vcsel periods."
-
- // uint16_t new_final_range_timeout_mclks =
- // vl53l0xTimeoutMicrosecondsToMclks(timeouts.final_range_us, period_pclks);
-
- // if (enables.pre_range)
- // {
- // new_final_range_timeout_mclks += timeouts.pre_range_mclks;
- // }
-
- // uint16_t new_final_range_timeout_encoded = vl53l0xEncodeTimeout(new_final_range_timeout_mclks);
- // vl53l0xWriteReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, new_final_range_timeout_encoded);
-
- // // set_sequence_step_timeout end
- // }
- // else
- // {
- // // invalid type
- // return false;
- // }
-
- // // "Finally, the timing budget must be re-applied"
-
- // vl53l0xSetMeasurementTimingBudget(dev, dev->measurement_timing_budget_us);
-
- // // "Perform the phase calibration. This is needed after changing on vcsel period."
- // // VL53L0X_perform_phase_calibration() begin
-
- // uint8_t sequence_config = 0;
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x02);
+ uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks);
+
+ SequenceStepEnables enables;
+ SequenceStepTimeouts timeouts;
+
+ vl53l0xGetSequenceStepEnables(dev, &enables);
+ vl53l0xGetSequenceStepTimeouts(dev, &enables, &timeouts);
+
+ // "Apply specific settings for the requested clock period"
+ // "Re-calculate and apply timeouts, in macro periods"
+
+ // "When the VCSEL period for the pre or final range is changed,
+ // the corresponding timeout must be read from the device using
+ // the current VCSEL period, then the new VCSEL period can be
+ // applied. The timeout then must be written back to the device
+ // using the new VCSEL period.
+ //
+ // For the MSRC timeout, the same applies - this timeout being
+ // dependant on the pre-range vcsel period."
+
+
+ if (type == VcselPeriodPreRange)
+ {
+ // "Set phase check limits"
+ switch (period_pclks)
+ {
+ case 12:
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
+ break;
+
+ case 14:
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
+ break;
+
+ case 16:
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
+ break;
+
+ case 18:
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
+ break;
+
+ default:
+ // invalid period
+ return false;
+ }
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+
+ // apply new VCSEL period
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
+
+ // update timeouts
+
+ // set_sequence_step_timeout() begin
+ // (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE)
+
+ uint16_t new_pre_range_timeout_mclks =
+ vl53l0xTimeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks);
+
+ uint16_t new_pre_range_timeout_encoded = vl53l0xEncodeTimeout(new_pre_range_timeout_mclks);
+ vl53l0xWriteReg16Bit(dev, VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, new_pre_range_timeout_encoded);
+
+ // set_sequence_step_timeout() end
+
+ // set_sequence_step_timeout() begin
+ // (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)
+
+ uint16_t new_msrc_timeout_mclks =
+ vl53l0xTimeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP,
+ (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1));
+
+ // set_sequence_step_timeout() end
+ }
+ else if (type == VcselPeriodFinalRange)
+ {
+ switch (period_pclks)
+ {
+ case 8:
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x30);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ break;
+
+ case 10:
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ break;
+
+ case 12:
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ break;
+
+ case 14:
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_ALGO_PHASECAL_LIM, 0x20);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ break;
+
+ default:
+ // invalid period
+ return false;
+ }
+
+ // apply new VCSEL period
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
+
+ // update timeouts
+
+ // set_sequence_step_timeout() begin
+ // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
+
+ // "For the final range timeout, the pre-range timeout
+ // must be added. To do this both final and pre-range
+ // timeouts must be expressed in macro periods MClks
+ // because they have different vcsel periods."
+
+ uint16_t new_final_range_timeout_mclks =
+ vl53l0xTimeoutMicrosecondsToMclks(timeouts.final_range_us, period_pclks);
+
+ if (enables.pre_range)
+ {
+ new_final_range_timeout_mclks += timeouts.pre_range_mclks;
+ }
+
+ uint16_t new_final_range_timeout_encoded = vl53l0xEncodeTimeout(new_final_range_timeout_mclks);
+ vl53l0xWriteReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, new_final_range_timeout_encoded);
+
+ // set_sequence_step_timeout end
+ }
+ else
+ {
+ // invalid type
+ return false;
+ }
+
+ // "Finally, the timing budget must be re-applied"
+
+ vl53l0xSetMeasurementTimingBudget(dev, dev->measurement_timing_budget_us);
+
+ // "Perform the phase calibration. This is needed after changing on vcsel period."
+ // VL53L0X_perform_phase_calibration() begin
+
+ uint8_t sequence_config = 0;
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, 0x02);
bool ret = vl53l0xPerformSingleRefCalibration(dev, 0x0);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, sequence_config);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, sequence_config);
- // // VL53L0X_perform_phase_calibration() end
+ // VL53L0X_perform_phase_calibration() end
return ret;
}
@@ -768,21 +760,19 @@ bool vl53l0xSetVcselPulsePeriod(VL53L0xDev* dev, vcselPeriodType type, uint8_t p
// based on VL53L0X_get_vcsel_pulse_period()
uint8_t vl53l0xGetVcselPulsePeriod(VL53L0xDev* dev, vcselPeriodType type)
{
- //COMMENTED FIRMWARE
- // if (type == VcselPeriodPreRange)
- // {
- // uint8_t temp = 0;
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD, &temp);
- // return decodeVcselPeriod(temp);
- // }
- // else if (type == VcselPeriodFinalRange)
- // {
- // uint8_t temp = 0;
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD, &temp);
- // return decodeVcselPeriod(temp);
- // }
- // else { return 255; }
- return 255;
+ if (type == VcselPeriodPreRange)
+ {
+ uint8_t temp = 0;
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_PRE_RANGE_CONFIG_VCSEL_PERIOD, &temp);
+ return decodeVcselPeriod(temp);
+ }
+ else if (type == VcselPeriodFinalRange)
+ {
+ uint8_t temp = 0;
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_FINAL_RANGE_CONFIG_VCSEL_PERIOD, &temp);
+ return decodeVcselPeriod(temp);
+ }
+ else { return 255; }
}
// Start continuous ranging measurements. If period_ms (optional) is 0 or not
@@ -793,53 +783,51 @@ uint8_t vl53l0xGetVcselPulsePeriod(VL53L0xDev* dev, vcselPeriodType type)
// based on VL53L0X_StartMeasurement()
void vl53l0xStartContinuous(VL53L0xDev* dev, uint32_t period_ms)
{
- //COMMENTED FIRMWARE
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, dev->stop_variable);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
-
- // if (period_ms != 0)
- // {
- // // continuous timed mode
-
- // // VL53L0X_SetInterMeasurementPeriodMilliSeconds() begin
-
- // uint16_t osc_calibrate_val = vl53l0xReadReg16Bit(dev, VL53L0X_RA_OSC_CALIBRATE_VAL);
-
- // if (osc_calibrate_val != 0)
- // {
- // period_ms *= osc_calibrate_val;
- // }
-
- // vl53l0xWriteReg32Bit(dev, VL53L0X_RA_SYSTEM_INTERMEASUREMENT_PERIOD, period_ms);
-
- // // VL53L0X_SetInterMeasurementPeriodMilliSeconds() end
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x04); // VL53L0X_REG_SYSRANGE_MODE_TIMED
- // }
- // else
- // {
- // // continuous back-to-back mode
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
- // }
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, dev->stop_variable);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
+
+ if (period_ms != 0)
+ {
+ // continuous timed mode
+
+ // VL53L0X_SetInterMeasurementPeriodMilliSeconds() begin
+
+ uint16_t osc_calibrate_val = vl53l0xReadReg16Bit(dev, VL53L0X_RA_OSC_CALIBRATE_VAL);
+
+ if (osc_calibrate_val != 0)
+ {
+ period_ms *= osc_calibrate_val;
+ }
+
+ vl53l0xWriteReg32Bit(dev, VL53L0X_RA_SYSTEM_INTERMEASUREMENT_PERIOD, period_ms);
+
+ // VL53L0X_SetInterMeasurementPeriodMilliSeconds() end
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x04); // VL53L0X_REG_SYSRANGE_MODE_TIMED
+ }
+ else
+ {
+ // continuous back-to-back mode
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
+ }
}
// Stop continuous measurements
// based on VL53L0X_StopMeasurement()
void vl53l0xStopContinuous(VL53L0xDev* dev)
{
- //COMMENTED FIRMWARE
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
}
// Returns a range reading in millimeters when continuous mode is active
@@ -847,30 +835,28 @@ void vl53l0xStopContinuous(VL53L0xDev* dev)
// single-shot range measurement)
uint16_t vl53l0xReadRangeContinuousMillimeters(VL53L0xDev* dev)
{
- //COMMENTED FIRMWARE
- // startTimeout();
- // uint8_t val = 0;
- // while ((val & 0x07) == 0)
- // {
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &val);
- // if ((val & 0x07) == 0)
- // {
- // // Relaxation delay when polling interrupt
- // vTaskDelay(M2T(1));
- // }
- // if (checkTimeoutExpired())
- // {
- // dev->did_timeout = true;
- // return 65535;
- // }
- // }
+ startTimeout();
+ uint8_t val = 0;
+ while ((val & 0x07) == 0)
+ {
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &val);
+ if ((val & 0x07) == 0)
+ {
+ // Relaxation delay when polling interrupt
+ vTaskDelay(M2T(1));
+ }
+ if (checkTimeoutExpired())
+ {
+ dev->did_timeout = true;
+ return 65535;
+ }
+ }
// assumptions: Linearity Corrective Gain is 1000 (default);
// fractional ranging is not enabled
uint16_t range = vl53l0xReadReg16Bit(dev, VL53L0X_RA_RESULT_RANGE_STATUS + 10);
- //COMMENTED FIRMWARE
- //i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
return range;
}
@@ -880,29 +866,28 @@ uint16_t vl53l0xReadRangeContinuousMillimeters(VL53L0xDev* dev)
// based on VL53L0X_PerformSingleRangingMeasurement()
uint16_t vl53l0xReadRangeSingleMillimeters(VL53L0xDev* dev)
{
- //COMMENTED FIRMWARE
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, dev->stop_variable);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
-
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01);
-
- // // "Wait until start bit has been cleared"
- // startTimeout();
- // uint8_t val = 0x01;
- // while (val & 0x01)
- // {
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, &val);
- // if (checkTimeoutExpired())
- // {
- // dev->did_timeout = true;
- // return 65535;
- // }
- // }
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x91, dev->stop_variable);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
+
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01);
+
+ // "Wait until start bit has been cleared"
+ startTimeout();
+ uint8_t val = 0x01;
+ while (val & 0x01)
+ {
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, &val);
+ if (checkTimeoutExpired())
+ {
+ dev->did_timeout = true;
+ return 65535;
+ }
+ }
return vl53l0xReadRangeContinuousMillimeters(dev);
}
@@ -912,43 +897,42 @@ uint16_t vl53l0xReadRangeSingleMillimeters(VL53L0xDev* dev)
// but only gets reference SPAD count and type
bool vl53l0xGetSpadInfo(VL53L0xDev* dev, uint8_t * count, bool * type_is_aperture)
{
- //COMMENTED FIRMWARE
- // uint8_t tmp;
+ uint8_t tmp;
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x06);
- // i2cdevWriteBit(dev->I2Cx, dev->devAddr, 0x83, 2, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x07);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x81, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x06);
+ i2cdevWriteBit(dev->I2Cx, dev->devAddr, 0x83, 2, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x07);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x81, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x94, 0x6b);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x83, 0x00);
- // startTimeout();
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x94, 0x6b);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x83, 0x00);
+ startTimeout();
- // uint8_t val = 0x00;
- // while (val == 0x00) {
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x83, &val);
- // if (checkTimeoutExpired()) { return false; }
- // };
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x83, 0x01);
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x92, &tmp);
+ uint8_t val = 0x00;
+ while (val == 0x00) {
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x83, &val);
+ if (checkTimeoutExpired()) { return false; }
+ };
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x83, 0x01);
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, 0x92, &tmp);
- // *count = tmp & 0x7f;
- // *type_is_aperture = (tmp >> 7) & 0x01;
+ *count = tmp & 0x7f;
+ *type_is_aperture = (tmp >> 7) & 0x01;
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x81, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x06);
- // i2cdevWriteBit(dev->I2Cx, dev->devAddr, 0x83, 2, 0);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x81, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x06);
+ i2cdevWriteBit(dev->I2Cx, dev->devAddr, 0x83, 2, 0);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x00, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0xFF, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, 0x80, 0x00);
return true;
}
@@ -957,15 +941,14 @@ bool vl53l0xGetSpadInfo(VL53L0xDev* dev, uint8_t * count, bool * type_is_apertur
// based on VL53L0X_GetSequenceStepEnables()
void vl53l0xGetSequenceStepEnables(VL53L0xDev* dev, SequenceStepEnables * enables)
{
- //COMMENTED FIRMWARE
- // uint8_t sequence_config = 0;
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
-
- // enables->tcc = (sequence_config >> 4) & 0x1;
- // enables->dss = (sequence_config >> 3) & 0x1;
- // enables->msrc = (sequence_config >> 2) & 0x1;
- // enables->pre_range = (sequence_config >> 6) & 0x1;
- // enables->final_range = (sequence_config >> 7) & 0x1;
+ uint8_t sequence_config = 0;
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_SEQUENCE_CONFIG, &sequence_config);
+
+ enables->tcc = (sequence_config >> 4) & 0x1;
+ enables->dss = (sequence_config >> 3) & 0x1;
+ enables->msrc = (sequence_config >> 2) & 0x1;
+ enables->pre_range = (sequence_config >> 6) & 0x1;
+ enables->final_range = (sequence_config >> 7) & 0x1;
}
// Get sequence step timeouts
@@ -974,35 +957,34 @@ void vl53l0xGetSequenceStepEnables(VL53L0xDev* dev, SequenceStepEnables * enable
// intermediate values
void vl53l0xGetSequenceStepTimeouts(VL53L0xDev* dev, SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts)
{
- //COMMENTED FIRMWARE
- // timeouts->pre_range_vcsel_period_pclks = vl53l0xGetVcselPulsePeriod(dev, VcselPeriodPreRange);
-
- // uint8_t temp = 0;
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP, &temp);
- // timeouts->msrc_dss_tcc_mclks = temp + 1;
- // timeouts->msrc_dss_tcc_us =
- // vl53l0xTimeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks,
- // timeouts->pre_range_vcsel_period_pclks);
-
- // uint16_t pre_range_encoded = vl53l0xReadReg16Bit(dev, VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
- // timeouts->pre_range_mclks = vl53l0xDecodeTimeout(pre_range_encoded);
- // timeouts->pre_range_us =
- // vl53l0xTimeoutMclksToMicroseconds(timeouts->pre_range_mclks,
- // timeouts->pre_range_vcsel_period_pclks);
-
- // timeouts->final_range_vcsel_period_pclks = vl53l0xGetVcselPulsePeriod(dev, VcselPeriodFinalRange);
-
- // uint16_t final_range_encoded = vl53l0xReadReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI);
- // timeouts->final_range_mclks = vl53l0xDecodeTimeout(final_range_encoded);
-
- // if (enables->pre_range)
- // {
- // timeouts->final_range_mclks -= timeouts->pre_range_mclks;
- // }
-
- // timeouts->final_range_us =
- // vl53l0xTimeoutMclksToMicroseconds(timeouts->final_range_mclks,
- // timeouts->final_range_vcsel_period_pclks);
+ timeouts->pre_range_vcsel_period_pclks = vl53l0xGetVcselPulsePeriod(dev, VcselPeriodPreRange);
+
+ uint8_t temp = 0;
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_MSRC_CONFIG_TIMEOUT_MACROP, &temp);
+ timeouts->msrc_dss_tcc_mclks = temp + 1;
+ timeouts->msrc_dss_tcc_us =
+ vl53l0xTimeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks,
+ timeouts->pre_range_vcsel_period_pclks);
+
+ uint16_t pre_range_encoded = vl53l0xReadReg16Bit(dev, VL53L0X_RA_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
+ timeouts->pre_range_mclks = vl53l0xDecodeTimeout(pre_range_encoded);
+ timeouts->pre_range_us =
+ vl53l0xTimeoutMclksToMicroseconds(timeouts->pre_range_mclks,
+ timeouts->pre_range_vcsel_period_pclks);
+
+ timeouts->final_range_vcsel_period_pclks = vl53l0xGetVcselPulsePeriod(dev, VcselPeriodFinalRange);
+
+ uint16_t final_range_encoded = vl53l0xReadReg16Bit(dev, VL53L0X_RA_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI);
+ timeouts->final_range_mclks = vl53l0xDecodeTimeout(final_range_encoded);
+
+ if (enables->pre_range)
+ {
+ timeouts->final_range_mclks -= timeouts->pre_range_mclks;
+ }
+
+ timeouts->final_range_us =
+ vl53l0xTimeoutMclksToMicroseconds(timeouts->final_range_mclks,
+ timeouts->final_range_vcsel_period_pclks);
}
// Decode sequence step timeout in MCLKs from register value
@@ -1063,20 +1045,19 @@ uint32_t vl53l0xTimeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t v
// based on VL53L0X_perform_single_ref_calibration()
bool vl53l0xPerformSingleRefCalibration(VL53L0xDev* dev, uint8_t vhv_init_byte)
{
- //COMMENTED FIRMWARE
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
- // startTimeout();
- // uint8_t temp = 0x00;
- // while ((temp & 0x07) == 0)
- // {
- // i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &temp);
- // if (checkTimeoutExpired()) { return false; }
- // }
+ startTimeout();
+ uint8_t temp = 0x00;
+ while ((temp & 0x07) == 0)
+ {
+ i2cdevReadByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_RESULT_INTERRUPT_STATUS, &temp);
+ if (checkTimeoutExpired()) { return false; }
+ }
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSTEM_INTERRUPT_CLEAR, 0x01);
- // i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x00);
+ i2cdevWriteByte(dev->I2Cx, dev->devAddr, VL53L0X_RA_SYSRANGE_START, 0x00);
return true;
}
@@ -1084,8 +1065,7 @@ bool vl53l0xPerformSingleRefCalibration(VL53L0xDev* dev, uint8_t vhv_init_byte)
uint16_t vl53l0xReadReg16Bit(VL53L0xDev* dev, uint8_t reg)
{
uint8_t buffer[2] = {};
- //COMMENTED FIRMWARE
- // i2cdevReadReg8(dev->I2Cx, dev->devAddr, reg, 2, (uint8_t *)&buffer);
+ i2cdevReadReg8(dev->I2Cx, dev->devAddr, reg, 2, (uint8_t *)&buffer);
return ((uint16_t)(buffer[0]) << 8) | buffer[1];
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l1x.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l1x.c
index e8b39e0f1..ace2d8a2b 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l1x.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/vl53l1x.c
@@ -55,25 +55,25 @@ static int nextI2CAddress = VL53L1X_DEFAULT_ADDRESS+8;
bool vl53l1xInit(VL53L1_Dev_t *pdev, I2C_Dev *I2Cx)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
-//COMMENTED FIRMWARE
-// pdev->I2Cx = I2Cx;
-// pdev->devAddr = VL53L1X_DEFAULT_ADDRESS;
-// /* Move initialized sensor to a new I2C address */
-// int newAddress;
+ pdev->I2Cx = I2Cx;
+ pdev->devAddr = VL53L1X_DEFAULT_ADDRESS;
-// taskENTER_CRITICAL();
-// newAddress = nextI2CAddress++;
-// taskEXIT_CRITICAL();
+ /* Move initialized sensor to a new I2C address */
+ int newAddress;
-// vl53l1xSetI2CAddress(pdev, newAddress);
+ taskENTER_CRITICAL();
+ newAddress = nextI2CAddress++;
+ taskEXIT_CRITICAL();
-// status = VL53L1_DataInit(pdev);
+ vl53l1xSetI2CAddress(pdev, newAddress);
-// if (status == VL53L1_ERROR_NONE)
-// {
-// status = VL53L1_StaticInit(pdev);
-// }
+ status = VL53L1_DataInit(pdev);
+
+ if (status == VL53L1_ERROR_NONE)
+ {
+ status = VL53L1_StaticInit(pdev);
+ }
return status == VL53L1_ERROR_NONE;
}
@@ -82,8 +82,8 @@ bool vl53l1xTestConnection(VL53L1_Dev_t* pdev)
{
VL53L1_DeviceInfo_t info;
VL53L1_Error status = VL53L1_ERROR_NONE;
- //COMMENTED FIRMWARE
-// status = VL53L1_GetDeviceInfo(pdev, &info);
+
+ status = VL53L1_GetDeviceInfo(pdev, &info);
return status == VL53L1_ERROR_NONE;
}
@@ -96,9 +96,9 @@ bool vl53l1xTestConnection(VL53L1_Dev_t* pdev)
VL53L1_Error vl53l1xSetI2CAddress(VL53L1_Dev_t* pdev, uint8_t address)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
-//COMMENTED FIRMWARE
-// status = VL53L1_SetDeviceAddress(pdev, address);
-// pdev->devAddr = address;
+
+ status = VL53L1_SetDeviceAddress(pdev, address);
+ pdev->devAddr = address;
return status;
}
@@ -114,11 +114,11 @@ VL53L1_Error VL53L1_WriteMulti(
uint32_t count)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
-//COMMENTED FIRMWARE
-// if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, count, pdata))
-// {
-// status = VL53L1_ERROR_CONTROL_INTERFACE;
-// }
+
+ if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, count, pdata))
+ {
+ status = VL53L1_ERROR_CONTROL_INTERFACE;
+ }
return status;
}
@@ -130,11 +130,11 @@ VL53L1_Error VL53L1_ReadMulti(
uint32_t count)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- //COMMENTED FIRMWARE
-// if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, count, pdata))
-// {
-// status = VL53L1_ERROR_CONTROL_INTERFACE;
-// }
+
+ if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, count, pdata))
+ {
+ status = VL53L1_ERROR_CONTROL_INTERFACE;
+ }
return status;
}
@@ -146,11 +146,11 @@ VL53L1_Error VL53L1_WrByte(
uint8_t data)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
-//COMMENTED FIRMWARE
- // if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 1, &data))
- // {
- // status = VL53L1_ERROR_CONTROL_INTERFACE;
- // }
+
+ if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 1, &data))
+ {
+ status = VL53L1_ERROR_CONTROL_INTERFACE;
+ }
return status;
}
@@ -162,11 +162,11 @@ VL53L1_Error VL53L1_WrWord(
uint16_t data)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- //COMMENTED FIRMWARE
-// if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 2, (uint8_t *)&data))
-// {
-// status = VL53L1_ERROR_CONTROL_INTERFACE;
-// }
+
+ if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 2, (uint8_t *)&data))
+ {
+ status = VL53L1_ERROR_CONTROL_INTERFACE;
+ }
return status;
}
@@ -178,11 +178,11 @@ VL53L1_Error VL53L1_WrDWord(
uint32_t data)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- //COMMENTED FIRMWARE
-// if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 4, (uint8_t *)&data))
-// {
-// status = VL53L1_ERROR_CONTROL_INTERFACE;
-// }
+
+ if (!i2cdevWrite16(pdev->I2Cx, pdev->devAddr, index, 4, (uint8_t *)&data))
+ {
+ status = VL53L1_ERROR_CONTROL_INTERFACE;
+ }
return status;
}
@@ -194,14 +194,13 @@ VL53L1_Error VL53L1_RdByte(
uint8_t *pdata)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- //COMMENTED FIRMWARE
-// static uint8_t r8data;
+ static uint8_t r8data;
-// if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 1, &r8data))
-// {
-// status = VL53L1_ERROR_CONTROL_INTERFACE;
-// }
-// *pdata = r8data;
+ if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 1, &r8data))
+ {
+ status = VL53L1_ERROR_CONTROL_INTERFACE;
+ }
+ *pdata = r8data;
return status;
}
@@ -213,14 +212,13 @@ VL53L1_Error VL53L1_RdWord(
uint16_t *pdata)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- //COMMENTED FIRMWARE
-// static uint16_t r16data;
-
-// if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 2, (uint8_t *)&r16data))
-// {
-// status = VL53L1_ERROR_CONTROL_INTERFACE;
-// }
-// *pdata = r16data;
+ static uint16_t r16data;
+
+ if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 2, (uint8_t *)&r16data))
+ {
+ status = VL53L1_ERROR_CONTROL_INTERFACE;
+ }
+ *pdata = r16data;
return status;
}
@@ -232,14 +230,13 @@ VL53L1_Error VL53L1_RdDWord(
uint32_t *pdata)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- //COMMENTED FIRMWARE
-// static uint32_t r32data;
+ static uint32_t r32data;
-// if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 4, (uint8_t *)&r32data))
-// {
-// status = VL53L1_ERROR_CONTROL_INTERFACE;
-// }
-// *pdata = r32data;
+ if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 4, (uint8_t *)&r32data))
+ {
+ status = VL53L1_ERROR_CONTROL_INTERFACE;
+ }
+ *pdata = r32data;
return status;
}
@@ -253,15 +250,14 @@ VL53L1_Error VL53L1_WaitUs(
int32_t wait_us)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
- //COMMENTED FIRMWARE
- // uint32_t delay_ms = (wait_us + 900) / 1000;
+ uint32_t delay_ms = (wait_us + 900) / 1000;
- // if(delay_ms == 0)
- // {
- // delay_ms = 1;
- // }
+ if(delay_ms == 0)
+ {
+ delay_ms = 1;
+ }
- // vTaskDelay(M2T(delay_ms));
+ vTaskDelay(M2T(delay_ms));
return status;
}
@@ -271,8 +267,7 @@ VL53L1_Error VL53L1_WaitMs(
VL53L1_Dev_t *pdev,
int32_t wait_ms)
{
- //COMMENTED FIRMWARE
- //vTaskDelay(M2T(wait_ms));
+ vTaskDelay(M2T(wait_ms));
return VL53L1_ERROR_NONE;
}
@@ -285,8 +280,7 @@ VL53L1_Error VL53L1_GetTickCount(
uint32_t *ptick_count_ms)
{
/* Returns current tick count in [ms] */
- //COMMENTED FIRMWARE
- //*ptick_count_ms = xTaskGetTickCount();
+ *ptick_count_ms = xTaskGetTickCount();
return VL53L1_ERROR_NONE;
}
@@ -311,73 +305,72 @@ VL53L1_Error VL53L1_WaitValueMaskEx(
* poll_delay_ms);
*/
- VL53L1_Error status = VL53L1_ERROR_NONE;
- //COMMENTED FIRMWARE
-// uint32_t start_time_ms = 0;
-// uint32_t current_time_ms = 0;
-// uint8_t byte_value = 0;
-// uint8_t found = 0;
-// #ifdef VL53L1_LOG_ENABLE
-// uint32_t trace_functions = 0;
-// #endif
-
-// SUPPRESS_UNUSED_WARNING(poll_delay_ms);
-
-// #ifdef VL53L1_LOG_ENABLE
-// /* look up register name */
-// VL53L1_get_register_name(
-// index,
-// register_name);
-
-// /* Output to I2C logger for FMT/DFT */
-// trace_i2c("WaitValueMaskEx(%5d, %s, 0x%02X, 0x%02X, %5d);\n",
-// timeout_ms, register_name, value, mask, poll_delay_ms);
-// #endif // VL53L1_LOG_ENABLE
-
-// /* calculate time limit in absolute time */
-
-// VL53L1_GetTickCount(&start_time_ms);
-// pdev->new_data_ready_poll_duration_ms = 0;
-
-// /* remember current trace functions and temporarily disable
-// * function logging
-// */
-
-// #ifdef VL53L1_LOG_ENABLE
-// trace_functions = _LOG_GET_TRACE_FUNCTIONS();
-// #endif
-
-// /* wait until value is found, timeout reached on error occurred */
-
-// while ((status == VL53L1_ERROR_NONE) &&
-// (pdev->new_data_ready_poll_duration_ms < timeout_ms) &&
-// (found == 0))
-// {
-// status = VL53L1_RdByte(
-// pdev,
-// index,
-// &byte_value);
-
-// if ((byte_value & mask) == value)
-// {
-// found = 1;
-// }
-
-// if (status == VL53L1_ERROR_NONE &&
-// found == 0 &&
-// poll_delay_ms > 0)
-// status = VL53L1_WaitMs(
-// pdev,
-// poll_delay_ms);
-
-// /* Update polling time (Compare difference rather than absolute to
-// negate 32bit wrap around issue) */
-// VL53L1_GetTickCount(¤t_time_ms);
-// pdev->new_data_ready_poll_duration_ms = current_time_ms - start_time_ms;
-// }
-
-// if (found == 0 && status == VL53L1_ERROR_NONE)
-// status = VL53L1_ERROR_TIME_OUT;
+ VL53L1_Error status = VL53L1_ERROR_NONE;
+ uint32_t start_time_ms = 0;
+ uint32_t current_time_ms = 0;
+ uint8_t byte_value = 0;
+ uint8_t found = 0;
+#ifdef VL53L1_LOG_ENABLE
+ uint32_t trace_functions = 0;
+#endif
+
+ SUPPRESS_UNUSED_WARNING(poll_delay_ms);
+
+#ifdef VL53L1_LOG_ENABLE
+ /* look up register name */
+ VL53L1_get_register_name(
+ index,
+ register_name);
+
+ /* Output to I2C logger for FMT/DFT */
+ trace_i2c("WaitValueMaskEx(%5d, %s, 0x%02X, 0x%02X, %5d);\n",
+ timeout_ms, register_name, value, mask, poll_delay_ms);
+#endif // VL53L1_LOG_ENABLE
+
+ /* calculate time limit in absolute time */
+
+ VL53L1_GetTickCount(&start_time_ms);
+ pdev->new_data_ready_poll_duration_ms = 0;
+
+ /* remember current trace functions and temporarily disable
+ * function logging
+ */
+
+#ifdef VL53L1_LOG_ENABLE
+ trace_functions = _LOG_GET_TRACE_FUNCTIONS();
+#endif
+
+ /* wait until value is found, timeout reached on error occurred */
+
+ while ((status == VL53L1_ERROR_NONE) &&
+ (pdev->new_data_ready_poll_duration_ms < timeout_ms) &&
+ (found == 0))
+ {
+ status = VL53L1_RdByte(
+ pdev,
+ index,
+ &byte_value);
+
+ if ((byte_value & mask) == value)
+ {
+ found = 1;
+ }
+
+ if (status == VL53L1_ERROR_NONE &&
+ found == 0 &&
+ poll_delay_ms > 0)
+ status = VL53L1_WaitMs(
+ pdev,
+ poll_delay_ms);
+
+ /* Update polling time (Compare difference rather than absolute to
+ negate 32bit wrap around issue) */
+ VL53L1_GetTickCount(¤t_time_ms);
+ pdev->new_data_ready_poll_duration_ms = current_time_ms - start_time_ms;
+ }
+
+ if (found == 0 && status == VL53L1_ERROR_NONE)
+ status = VL53L1_ERROR_TIME_OUT;
return status;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/watchdog.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/watchdog.c
index 46089f6fe..2bb339487 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/watchdog.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/watchdog.c
@@ -33,12 +33,12 @@
bool watchdogNormalStartTest(void)
{
bool wasNormalStart = true;
- //COMMENTED FIRMWARE
- // if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST)) {
- // RCC_ClearFlag();
- // wasNormalStart = false;
- // DEBUG_PRINT("The system resumed after watchdog timeout [WARNING]\n");
- // }
+
+ if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST)) {
+ RCC_ClearFlag();
+ wasNormalStart = false;
+ DEBUG_PRINT("The system resumed after watchdog timeout [WARNING]\n");
+ }
return wasNormalStart;
}
@@ -46,32 +46,31 @@ bool watchdogNormalStartTest(void)
void watchdogInit(void)
{
- //COMMENTED FIRMWARE
- // IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
+ IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
- // // The watchdog uses the LSI oscillator for checking the timeout. The LSI
- // // oscillator frequency is not very exact and the range is fairly large
- // // and also differs between the Crazyflie 1.0 and 2.0:
- // // MIN TYP MAX
- // // Crazyflie 1.0 30 40 60 (in kHz)
- // // Crazyflie 2.0 17 32 47 (in kHz)
- // //
- // IWDG_SetPrescaler(IWDG_Prescaler_32);
- // // Divide the clock with 32 which gives
- // // an interval of 17kHz/32 (CF2 min) to 60kHz/32 (CF1 max) =>
- // // 1875 Hz to 531 Hz for the watchdog timer.
- // //
- // // The goal timeout is >100 ms, but it's acceptable
- // // that the max timeout is a bit higher. Scaling the
- // // reload counter for the fastest LSI then gives a
- // // timeout of 100ms, which in turn gives a timeout
- // // of 353ms for the slowest LSI. So the watchdog timeout
- // // will be between 100ms and 353ms on all platforms.
- // //
- // // At prescaler 32 each bit is 1 ms this gives:
- // // 1875 Hz * 0.1 s / 1 => 188
- // IWDG_SetReload(188);
+ // The watchdog uses the LSI oscillator for checking the timeout. The LSI
+ // oscillator frequency is not very exact and the range is fairly large
+ // and also differs between the Crazyflie 1.0 and 2.0:
+ // MIN TYP MAX
+ // Crazyflie 1.0 30 40 60 (in kHz)
+ // Crazyflie 2.0 17 32 47 (in kHz)
+ //
+ IWDG_SetPrescaler(IWDG_Prescaler_32);
+ // Divide the clock with 32 which gives
+ // an interval of 17kHz/32 (CF2 min) to 60kHz/32 (CF1 max) =>
+ // 1875 Hz to 531 Hz for the watchdog timer.
+ //
+ // The goal timeout is >100 ms, but it's acceptable
+ // that the max timeout is a bit higher. Scaling the
+ // reload counter for the fastest LSI then gives a
+ // timeout of 100ms, which in turn gives a timeout
+ // of 353ms for the slowest LSI. So the watchdog timeout
+ // will be between 100ms and 353ms on all platforms.
+ //
+ // At prescaler 32 each bit is 1 ms this gives:
+ // 1875 Hz * 0.1 s / 1 => 188
+ IWDG_SetReload(188);
- // watchdogReset();
- // IWDG_Enable();
+ watchdogReset();
+ IWDG_Enable();
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ws2812_cf2.c b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ws2812_cf2.c
index c3f217631..34e5bd7b9 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ws2812_cf2.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/drivers/src/ws2812_cf2.c
@@ -64,109 +64,107 @@ static union {
void ws2812Init(void)
{
- //COMMENTED FIRMWARE
-// uint16_t PrescalerValue;
-
-// RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
-// RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
-// /* GPIOB Configuration: TIM3 Channel 1 as alternate function push-pull */
-// // Configure the GPIO PB4 for the timer output
-// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
-// GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
-// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
-// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
-// GPIO_Init(GPIOB, &GPIO_InitStructure);
-
-// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
-// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
-// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
-// GPIO_Init(GPIOB, &GPIO_InitStructure);
-
-// //Map timer to alternate functions
-// GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3);
-
-// /* Compute the prescaler value */
-// PrescalerValue = 0;
-// /* Time base configuration */
-// TIM_TimeBaseStructure.TIM_Period = (105 - 1); // 800kHz
-// TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
-// TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-// TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-// TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
-// TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
-
-// /* PWM1 Mode configuration: Channel1 */
-// TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
-// TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
-// TIM_OCInitStructure.TIM_Pulse = 0;
-// TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
-// TIM_OC2Init(TIM3, &TIM_OCInitStructure);
-
-// // TIM_Cmd(TIM3, ENABLE); // Go!!!
-// TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
-// TIM_CtrlPWMOutputs(TIM3, ENABLE); // enable Timer 3
-
-// /* configure DMA */
-// /* DMA clock enable */
-// RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
-
-// /* DMA1 Channel5 Config TM */
-// DMA_DeInit(DMA1_Stream5);
-
-// ASSERT_DMA_SAFE(led_dma.buffer);
-// // USART TX DMA Channel Config
-// DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR2;
-// DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)led_dma.buffer; // this is the buffer memory
-// DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
-// DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
-// DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
-// DMA_InitStructure.DMA_BufferSize = 0;
-// DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
-// DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
-// DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
-// DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
-// DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
-// DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
-// DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
-// DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
-// DMA_InitStructure.DMA_Channel = DMA_Channel_5;
-// DMA_Init(DMA1_Stream5, &DMA_InitStructure);
-
-// NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream5_IRQn;
-// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_LOW_PRI;
-// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
-// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-// NVIC_Init(&NVIC_InitStructure);
-
-// vSemaphoreCreateBinary(allLedDone);
-
-// DMA_ITConfig(DMA1_Stream5, DMA_IT_TC, ENABLE);
-// DMA_ITConfig(DMA1_Stream5, DMA_IT_HT, ENABLE);
-
-// /* TIM3 CC2 DMA Request enable */
-// TIM_DMACmd(TIM3, TIM_DMA_CC2, ENABLE);
+ uint16_t PrescalerValue;
+
+ RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+ /* GPIOB Configuration: TIM3 Channel 1 as alternate function push-pull */
+ // Configure the GPIO PB4 for the timer output
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ //Map timer to alternate functions
+ GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3);
+
+ /* Compute the prescaler value */
+ PrescalerValue = 0;
+ /* Time base configuration */
+ TIM_TimeBaseStructure.TIM_Period = (105 - 1); // 800kHz
+ TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+ TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
+
+ /* PWM1 Mode configuration: Channel1 */
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+ TIM_OCInitStructure.TIM_Pulse = 0;
+ TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+ TIM_OC2Init(TIM3, &TIM_OCInitStructure);
+
+// TIM_Cmd(TIM3, ENABLE); // Go!!!
+ TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
+ TIM_CtrlPWMOutputs(TIM3, ENABLE); // enable Timer 3
+
+ /* configure DMA */
+ /* DMA clock enable */
+ RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
+
+ /* DMA1 Channel5 Config TM */
+ DMA_DeInit(DMA1_Stream5);
+
+ ASSERT_DMA_SAFE(led_dma.buffer);
+ // USART TX DMA Channel Config
+ DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR2;
+ DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)led_dma.buffer; // this is the buffer memory
+ DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+ DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
+ DMA_InitStructure.DMA_BufferSize = 0;
+ DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
+ DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+ DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+ DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
+ DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
+ DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
+ DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
+ DMA_InitStructure.DMA_Channel = DMA_Channel_5;
+ DMA_Init(DMA1_Stream5, &DMA_InitStructure);
+
+ NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream5_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_LOW_PRI;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ vSemaphoreCreateBinary(allLedDone);
+
+ DMA_ITConfig(DMA1_Stream5, DMA_IT_TC, ENABLE);
+ DMA_ITConfig(DMA1_Stream5, DMA_IT_HT, ENABLE);
+
+ /* TIM3 CC2 DMA Request enable */
+ TIM_DMACmd(TIM3, TIM_DMA_CC2, ENABLE);
}
static void fillLed(uint16_t *buffer, uint8_t *color)
{
- //COMMENTED FIRMWARE
- // int i;
-
- // for(i=0; i<8; i++) // GREEN data
- // {
- // buffer[i] = ((color[1]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
- // }
- // for(i=0; i<8; i++) // RED
- // {
- // buffer[8+i] = ((color[0]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
- // }
- // for(i=0; i<8; i++) // BLUE
- // {
- // buffer[16+i] = ((color[2]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
- // }
+ int i;
+
+ for(i=0; i<8; i++) // GREEN data
+ {
+ buffer[i] = ((color[1]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
+ }
+ for(i=0; i<8; i++) // RED
+ {
+ buffer[8+i] = ((color[0]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
+ }
+ for(i=0; i<8; i++) // BLUE
+ {
+ buffer[16+i] = ((color[2]<<i) & 0x0080) ? TIMING_ONE:TIMING_ZERO;
+ }
}
static int current_led = 0;
@@ -175,83 +173,80 @@ static uint8_t (*color_led)[3] = NULL;
void ws2812Send(uint8_t (*color)[3], uint16_t len)
{
- //COMMENTED FIRMWARE
- // int i;
- // if(len<1) return;
-
- // //Wait for previous transfer to be finished
- // xSemaphoreTake(allLedDone, portMAX_DELAY);
-
- // // Set interrupt context ...
- // current_led = 0;
- // total_led = len;
- // color_led = color;
-
- // for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
- // if (current_led<total_led)
- // fillLed(led_dma.begin+(24*i), color_led[current_led]);
- // else
- // bzero(led_dma.begin+(24*i), sizeof(led_dma.begin));
- // }
-
- // for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
- // if (current_led<total_led)
- // fillLed(led_dma.end+(24*i), color_led[current_led]);
- // else
- // bzero(led_dma.end+(24*i), sizeof(led_dma.end));
- // }
-
- // DMA1_Stream5->NDTR = sizeof(led_dma.buffer) / sizeof(led_dma.buffer[0]); // load number of bytes to be transferred
- // DMA_Cmd(DMA1_Stream5, ENABLE); // enable DMA channel 2
- // TIM_Cmd(TIM3, ENABLE); // Go!!!
+ int i;
+ if(len<1) return;
+
+ //Wait for previous transfer to be finished
+ xSemaphoreTake(allLedDone, portMAX_DELAY);
+
+ // Set interrupt context ...
+ current_led = 0;
+ total_led = len;
+ color_led = color;
+
+ for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
+ if (current_led<total_led)
+ fillLed(led_dma.begin+(24*i), color_led[current_led]);
+ else
+ bzero(led_dma.begin+(24*i), sizeof(led_dma.begin));
+ }
+
+ for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
+ if (current_led<total_led)
+ fillLed(led_dma.end+(24*i), color_led[current_led]);
+ else
+ bzero(led_dma.end+(24*i), sizeof(led_dma.end));
+ }
+
+ DMA1_Stream5->NDTR = sizeof(led_dma.buffer) / sizeof(led_dma.buffer[0]); // load number of bytes to be transferred
+ DMA_Cmd(DMA1_Stream5, ENABLE); // enable DMA channel 2
+ TIM_Cmd(TIM3, ENABLE); // Go!!!
}
void ws2812DmaIsr(void)
{
- //COMMENTED FIRMWARE
- // portBASE_TYPE xHigherPriorityTaskWoken;
- // uint16_t * buffer;
- // int i;
-
- // if (total_led == 0)
- // {
- // TIM_Cmd(TIM3, DISABLE);
- // DMA_Cmd(DMA1_Stream5, DISABLE);
- // }
-
- // if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_HTIF5))
- // {
- // DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_HTIF5);
- // buffer = led_dma.begin;
- // }
-
- // if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_TCIF5))
- // {
- // DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_TCIF5);
- // buffer = led_dma.end;
- // }
-
- // for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
- // if (current_led<total_led)
- // fillLed(buffer+(24*i), color_led[current_led]);
- // else
- // bzero(buffer+(24*i), sizeof(led_dma.end));
- // }
-
- // if (current_led >= total_led+2) {
- // xSemaphoreGiveFromISR(allLedDone, &xHigherPriorityTaskWoken);
-
- // TIM_Cmd(TIM3, DISABLE); // disable Timer 3
- // DMA_Cmd(DMA1_Stream5, DISABLE); // disable DMA stream4
-
- // total_led = 0;
- // }
+ portBASE_TYPE xHigherPriorityTaskWoken;
+ uint16_t * buffer;
+ int i;
+
+ if (total_led == 0)
+ {
+ TIM_Cmd(TIM3, DISABLE);
+ DMA_Cmd(DMA1_Stream5, DISABLE);
+ }
+
+ if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_HTIF5))
+ {
+ DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_HTIF5);
+ buffer = led_dma.begin;
+ }
+
+ if (DMA_GetITStatus(DMA1_Stream5, DMA_IT_TCIF5))
+ {
+ DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_TCIF5);
+ buffer = led_dma.end;
+ }
+
+ for(i=0; (i<LED_PER_HALF) && (current_led<total_led+2); i++, current_led++) {
+ if (current_led<total_led)
+ fillLed(buffer+(24*i), color_led[current_led]);
+ else
+ bzero(buffer+(24*i), sizeof(led_dma.end));
+ }
+
+ if (current_led >= total_led+2) {
+ xSemaphoreGiveFromISR(allLedDone, &xHigherPriorityTaskWoken);
+
+ TIM_Cmd(TIM3, DISABLE); // disable Timer 3
+ DMA_Cmd(DMA1_Stream5, DISABLE); // disable DMA stream4
+
+ total_led = 0;
+ }
}
#ifndef USDDECK_USE_ALT_PINS_AND_SPI
void __attribute__((used)) DMA1_Stream5_IRQHandler(void)
{
- //COMMENTED FIRMWARE
- //ws2812DmaIsr();
+ ws2812DmaIsr();
}
#endif
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 ff5cd881a..94b5dffab 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,39 +192,37 @@ static bool ledseqEnabled = false;
static void lesdeqCmdTask(void* param);
void ledseqInit() {
-
if(isInit) {
return;
}
- //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);
+ 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 d522e268d..2b2c74460 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;
- //COMMENTED FIRMWARE
- // pmSyslinkInfo.vBat = 3.7f;
- // pmSetBatteryVoltage(pmSyslinkInfo.vBat); //TODO remove
+
+ pmSyslinkInfo.vBat = 3.7f;
+ pmSetBatteryVoltage(pmSyslinkInfo.vBat); //TODO remove
}
bool pmTest(void)
@@ -307,105 +307,104 @@ bool pmIsDischarging(void) {
void pmTask(void *param)
{
- //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;
- // }
- // }
+ 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 1f300a9b3..2a10496e6 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,17 +402,16 @@ void USBD_USR_DeviceDisconnected(void)
void usbInit(void)
{
- //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);
+ 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 225edc6ad..05958a83e 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,16 +69,15 @@ static USBPacket usbIn;
static CRTPPacket p;
static void usblinkTask(void *param)
{
- //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);
- // }
+ 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 736434172..6971d308e 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,40 +36,39 @@ static uint32_t usecTimerHighCount;
void initUsecTimer(void)
{
- //COMMENTED FIRMWARE
- // if (isInit) {
- // return;
- // }
+ 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 97d331ab8..1faf554a5 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/init/main.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/init/main.c
@@ -62,10 +62,9 @@ int main()
vTaskStartScheduler();
//TODO: Move to platform launch failed
- //COMMENTED FIRMWARE
- // ledInit();
- // ledSet(0, 1);
- // ledSet(1, 1);
+ 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 cc8f668b4..afb09ba94 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform.c
@@ -31,94 +31,86 @@
static const platformConfig_t* active_config = 0;
int platformInit(void) {
- //COMMENTED FIRMWARE
- // int nrOfConfigs = 0;
- // const platformConfig_t* configs = platformGetListOfConfigurations(&nrOfConfigs);
-
- // int err = platformInitConfiguration(configs, nrOfConfigs);
- // if (err != 0)
- // {
- // // This firmware is not compatible, abort init
- // return 1;
- // }
-
- // platformInitHardware();
+ int nrOfConfigs = 0;
+ const platformConfig_t* configs = platformGetListOfConfigurations(&nrOfConfigs);
+
+ int err = platformInitConfiguration(configs, nrOfConfigs);
+ if (err != 0)
+ {
+ // This firmware is not compatible, abort init
+ return 1;
+ }
+
+ platformInitHardware();
return 0;
}
int platformParseDeviceTypeString(const char* deviceTypeString, char* deviceType) {
- //COMMENTED FIRMWARE
- // if (deviceTypeString[0] != '0' || deviceTypeString[1] != ';') {
- // return 1;
- // }
-
- // const int start = 2;
- // const int last = start + PLATFORM_DEVICE_TYPE_MAX_LEN - 1;
- // int end = 0;
- // for (end = start; end <= last; end++) {
- // if (deviceTypeString[end] == '\0' || deviceTypeString[end] == ';') {
- // break;
- // }
- // }
-
- // if (end > last) {
- // return 1;
- // }
-
- // int length = end - start;
- // memcpy(deviceType, &deviceTypeString[start], length);
- // deviceType[length] = '\0';
+ if (deviceTypeString[0] != '0' || deviceTypeString[1] != ';') {
+ return 1;
+ }
+
+ const int start = 2;
+ const int last = start + PLATFORM_DEVICE_TYPE_MAX_LEN - 1;
+ int end = 0;
+ for (end = start; end <= last; end++) {
+ if (deviceTypeString[end] == '\0' || deviceTypeString[end] == ';') {
+ break;
+ }
+ }
+
+ if (end > last) {
+ return 1;
+ }
+
+ int length = end - start;
+ memcpy(deviceType, &deviceTypeString[start], length);
+ deviceType[length] = '\0';
return 0;
}
int platformInitConfiguration(const platformConfig_t* configs, const int nrOfConfigs) {
-//COMMENTED FIRMWARE
-// #ifndef DEVICE_TYPE_STRING_FORCE
-// char deviceTypeString[PLATFORM_DEVICE_TYPE_STRING_MAX_LEN];
-// char deviceType[PLATFORM_DEVICE_TYPE_MAX_LEN];
-
-// platformGetDeviceTypeString(deviceTypeString);
-// platformParseDeviceTypeString(deviceTypeString, deviceType);
-// #else
-// #define xstr(s) str(s)
-// #define str(s) #s
-
-// char* deviceType = xstr(DEVICE_TYPE_STRING_FORCE);
-// #endif
-
-// for (int i = 0; i < nrOfConfigs; i++) {
-// const platformConfig_t* config = &configs[i];
-// if (strcmp(config->deviceType, deviceType) == 0) {
-// active_config = config;
- return 0;
-// }
-// }
-
-// return 1;
+#ifndef DEVICE_TYPE_STRING_FORCE
+ char deviceTypeString[PLATFORM_DEVICE_TYPE_STRING_MAX_LEN];
+ char deviceType[PLATFORM_DEVICE_TYPE_MAX_LEN];
+
+ platformGetDeviceTypeString(deviceTypeString);
+ platformParseDeviceTypeString(deviceTypeString, deviceType);
+#else
+ #define xstr(s) str(s)
+ #define str(s) #s
+
+ char* deviceType = xstr(DEVICE_TYPE_STRING_FORCE);
+#endif
+
+ for (int i = 0; i < nrOfConfigs; i++) {
+ const platformConfig_t* config = &configs[i];
+ if (strcmp(config->deviceType, deviceType) == 0) {
+ active_config = config;
+ return 0;
+ }
+ }
+
+ return 1;
}
const char* platformConfigGetDeviceType() {
- //COMMENTED FIRMWARE
- // return active_config->deviceType;
+ return active_config->deviceType;
}
const char* platformConfigGetDeviceTypeName() {
- //COMMENTED FIRMWARE
- // return active_config->deviceTypeName;
+ return active_config->deviceTypeName;
}
SensorImplementation_t platformConfigGetSensorImplementation() {
- //COMMENTED FIRMWARE
- // return active_config->sensorImplementation;
+ return active_config->sensorImplementation;
}
bool platformConfigPhysicalLayoutAntennasAreClose() {
- //COMMENTED FIRMWARE
- // return active_config->physicalLayoutAntennasAreClose;
+ return active_config->physicalLayoutAntennasAreClose;
}
const MotorPerifDef** platformConfigGetMotorMapping() {
- //COMMENTED FIRMWARE
- // return active_config->motorMap;
+ return active_config->motorMap;
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_cf2.c b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_cf2.c
index 76c5299bd..fb250dad5 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_cf2.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_cf2.c
@@ -37,28 +37,28 @@ static platformConfig_t configs[] = {
{
.deviceType = "CF20",
.deviceTypeName = "Crazyflie 2.0",
- // .sensorImplementation = SensorImplementation_mpu9250_lps25h,
+ .sensorImplementation = SensorImplementation_mpu9250_lps25h,
.physicalLayoutAntennasAreClose = true,
.motorMap = motorMapDefaultBrushed,
},
{
.deviceType = "CF21",
.deviceTypeName = "Crazyflie 2.1",
- // .sensorImplementation = SensorImplementation_bmi088_bmp388,
+ .sensorImplementation = SensorImplementation_bmi088_bmp388,
.physicalLayoutAntennasAreClose = false,
.motorMap = motorMapDefaultBrushed,
},
{ // Old ID of Crzyflie Bolt
.deviceType = "RZ10",
.deviceTypeName = "Crazyflie Bolt",
- // .sensorImplementation = SensorImplementation_bmi088_spi_bmp388,
+ .sensorImplementation = SensorImplementation_bmi088_spi_bmp388,
.physicalLayoutAntennasAreClose = false,
.motorMap = motorMapBoltBrushless,
},
{
.deviceType = "CB10",
.deviceTypeName = "Crazyflie Bolt",
- // .sensorImplementation = SensorImplementation_bmi088_spi_bmp388,
+ .sensorImplementation = SensorImplementation_bmi088_spi_bmp388,
.physicalLayoutAntennasAreClose = false,
.motorMap = motorMapBoltBrushless,
}
@@ -70,12 +70,11 @@ const platformConfig_t* platformGetListOfConfigurations(int* nrOfConfigs) {
}
void platformInitHardware() {
- //COMMENTED FIRMWARE
- // //Low level init: Clock and Interrupt controller
- // nvicInit();
+ //Low level init: Clock and Interrupt controller
+ nvicInit();
- // //EXTI interrupts
- // extiInit();
+ //EXTI interrupts
+ extiInit();
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_stm32f4.c b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_stm32f4.c
index b4bc0bb04..77f5072b0 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_stm32f4.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_stm32f4.c
@@ -38,35 +38,33 @@
#define DEFAULT_PLATFORM_STRING "0;CF20"
-//COMMENTED FIRMWARE
-// #ifndef UNIT_TEST_MODE
-// static char* getAddressOfOtpMemoryBlock(int blockNr) {
-// return (char*)(0x1fff7800 + blockNr * 0x20);
-// }
-// #else
-// // This function is replaced by a mock in unit tests
-// char* getAddressOfOtpMemoryBlock(const int blockNr);
-// #endif
+#ifndef UNIT_TEST_MODE
+static char* getAddressOfOtpMemoryBlock(int blockNr) {
+ return (char*)(0x1fff7800 + blockNr * 0x20);
+}
+#else
+ // This function is replaced by a mock in unit tests
+ char* getAddressOfOtpMemoryBlock(const int blockNr);
+#endif
void platformGetDeviceTypeString(char* deviceTypeString) {
- //COMMENTED FIRMWARE
- // char* block = 0;
+ char* block = 0;
- // for (int i = 0; i < PLATFORM_INFO_OTP_NR_OF_BLOCKS; i++) {
- // char* candidateBlock = getAddressOfOtpMemoryBlock(i);
- // if (candidateBlock[0] != 0) {
- // block = candidateBlock;
- // break;
- // }
- // }
+ for (int i = 0; i < PLATFORM_INFO_OTP_NR_OF_BLOCKS; i++) {
+ char* candidateBlock = getAddressOfOtpMemoryBlock(i);
+ if (candidateBlock[0] != 0) {
+ block = candidateBlock;
+ break;
+ }
+ }
- // if (!block || ((unsigned char)block[0]) == 0xff) {
- // block = DEFAULT_PLATFORM_STRING;
- // }
+ if (!block || ((unsigned char)block[0]) == 0xff) {
+ block = DEFAULT_PLATFORM_STRING;
+ }
- // strncpy(deviceTypeString, block, PLATFORM_INFO_OTP_BLOCK_LEN);
- // deviceTypeString[PLATFORM_INFO_OTP_BLOCK_LEN] = '\0';
+ strncpy(deviceTypeString, block, PLATFORM_INFO_OTP_BLOCK_LEN);
+ deviceTypeString[PLATFORM_INFO_OTP_BLOCK_LEN] = '\0';
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_tag.c b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_tag.c
index 7cb8eaa62..cc15b5445 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_tag.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_tag.c
@@ -37,32 +37,29 @@ static platformConfig_t configs[] = {
{
.deviceType = "RR10",
.deviceTypeName = "Roadrunner 1.0",
- // .sensorImplementation = SensorImplementation_bmi088_bmp388,
+ .sensorImplementation = SensorImplementation_bmi088_bmp388,
.physicalLayoutAntennasAreClose = false,
.motorMap = motorMapNoMotors,
},
};
const platformConfig_t* platformGetListOfConfigurations(int* nrOfConfigs) {
- //COMMENTED FIRMWARE
- // *nrOfConfigs = sizeof(configs) / sizeof(platformConfig_t);
- // return configs;
+ *nrOfConfigs = sizeof(configs) / sizeof(platformConfig_t);
+ return configs;
}
void platformInitHardware() {
- //COMMENTED FIRMWARE
- // //Low level init: Clock and Interrupt controller
- // nvicInit();
+ //Low level init: Clock and Interrupt controller
+ nvicInit();
- // //EXTI interrupts
- // extiInit();
+ //EXTI interrupts
+ extiInit();
}
// Config functions ------------------------
const char* platformConfigGetPlatformName() {
- //COMMENTED FIRMWARE
- // return "tag";
+ return "tag";
}
diff --git a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_utils.c b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_utils.c
index e2d275fa9..3f136b00e 100644
--- a/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_utils.c
+++ b/crazyflie_software/crazyflie-firmware-2021.06/src/platform/platform_utils.c
@@ -38,9 +38,8 @@
void platformSetLowInterferenceRadioMode(void) {
- //COMMENTED FIRMWARE
-// // Decrease the nRF51 Tx power to reduce interference
-// radiolinkSetPowerDbm(PLATFORM_NRF51_LOW_INTERFERENCE_TX_POWER_DBM);
-// DEBUG_PRINT("Low interference mode. NRF51 TX power offset by %ddb.\r\n", PLATFORM_NRF51_LOW_INTERFERENCE_TX_POWER_DBM);
+ // Decrease the nRF51 Tx power to reduce interference
+ radiolinkSetPowerDbm(PLATFORM_NRF51_LOW_INTERFERENCE_TX_POWER_DBM);
+ DEBUG_PRINT("Low interference mode. NRF51 TX power offset by %ddb.\r\n", PLATFORM_NRF51_LOW_INTERFERENCE_TX_POWER_DBM);
}
--
GitLab