static void sensorsDeviceInit(void) {
isMagnetometerPresent = false;
isBarometerPresent = false;
// Wait for sensors to startup
while (xTaskGetTickCount() < 1000);
i2cdevInit(I2C3_DEV);
mpu6500Init(I2C3_DEV);
if (mpu6500TestConnection() == true)
{
DEBUG_PRINT("MPU9250 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MPU9250 I2C connection [FAIL].\n");
}
mpu6500Reset();
vTaskDelay(M2T(50));
// Activate MPU6500
mpu6500SetSleepEnabled(false);
// Enable temp sensor
mpu6500SetTempSensorEnabled(true);
// Disable interrupts
mpu6500SetIntEnabled(false);
// Connect the HMC5883L to the main I2C bus
mpu6500SetI2CBypassEnabled(true);
// Set x-axis gyro as clock source
mpu6500SetClockSource(MPU6500_CLOCK_PLL_XGYRO);
// Set gyro full scale range
mpu6500SetFullScaleGyroRange(IMU_GYRO_FS_CFG);
// Set accelerometer full scale range
mpu6500SetFullScaleAccelRange(IMU_ACCEL_FS_CFG);
#ifdef IMU_MPU6500_DLPF_256HZ
// 256Hz digital low-pass filter only works with little vibrations
// Set output rate (15): 8000 / (1 + 15) = 500Hz
mpu6500SetRate(15);
// Set digital low-pass bandwidth
mpu6500SetDLPFMode(MPU6500_DLPF_BW_256);
#else
// To low DLPF bandwidth might cause instability and decrease agility
// but it works well for handling vibrations and unbalanced propellers
// Set output rate (1): 1000 / (1 + 1) = 500Hz
mpu6500SetRate(1);
// Set digital low-pass bandwidth
mpu6500SetDLPFMode(MPU6500_DLPF_BW_98);
#endif
// delay 3 seconds until the quad has stabilized enough to pass the test
bool mpu6500SelfTestPassed = false;
for (int i=0; i<300; i++)
{
if(mpu6500SelfTest() == true)
{
mpu6500SelfTestPassed = true;
break;
}
else
{
vTaskDelay(M2T(10));
}
}
configASSERT(mpu6500SelfTestPassed);
// Now begin to set up the slaves
mpu6500SetSlave4MasterDelay(4); // read slaves at 100Hz = (500Hz / (1 + 4))
#ifdef IMU_ENABLE_MAG_AK8963
ak8963Init(I2C3_DEV);
if (ak8963TestConnection() == true)
{
isMagnetometerPresent = true;
ak8963SetMode(AK8963_MODE_16BIT | AK8963_MODE_CONT2); // 16bit 100Hz
configASSERT(ak8963SelfTest());
DEBUG_PRINT("AK8963 I2C connection [OK].\n");
mpu6500SetSlaveAddress(0, 0x80 | AK8963_ADDRESS_00); // set the magnetometer to Slave 0, enable read
mpu6500SetSlaveRegister(0, AK8963_RA_HXL); // read the magnetometer heading register
mpu6500SetSlaveDataLength(0, 6); // read 6 bytes (x, y, z heading)
mpu6500SetSlaveDelayEnabled(0, true);
mpu6500SetSlaveEnabled(0, true);
}
else
{
DEBUG_PRINT("AK8963 I2C connection [FAIL].\n");
}
#endif
#ifdef IMU_ENABLE_PRESSURE_LPS25H
lps25hInit(I2C3_DEV);
if (lps25hTestConnection() == true)
{
lps25hSetEnabled(true);
isBarometerPresent = true;
configASSERT(lps25hSelfTest());
DEBUG_PRINT("LPS25H I2C connection [OK].\n");
mpu6500SetSlaveAddress(1, 0x80 | LPS25H_I2C_ADDR); // set the barometer to Slave 1, enable read
mpu6500SetSlaveRegister(1, LPS25H_PRESS_OUT_XL | LPS25H_ADDR_AUTO_INC);
mpu6500SetSlaveDataLength(1, 5);
mpu6500SetSlaveDelayEnabled(1, true);
mpu6500SetSlaveEnabled(1, true);
}
else
{
//TODO: Should sensor test fail hard if no connection
DEBUG_PRINT("LPS25H I2C connection [FAIL].\n");
}
#endif
mpu6500SetI2CBypassEnabled(false);
mpu6500SetI2CMasterModeEnabled(true);
mpu6500SetWaitForExternalSensorEnabled(false); // the slave data isn't so important for the state estimation
mpu6500SetInterruptMode(0); // active high
mpu6500SetInterruptDrive(0); // push pull
mpu6500SetInterruptLatch(0); // latched until clear
mpu6500SetInterruptLatchClear(1); // cleared on any register read
mpu6500SetIntDataReadyEnabled(true);
}
void imu6Init(void)
{
if(isInit)
return;
isMagPresent = false;
isBaroPresent = false;
// Wait for sensors to startup
while (xTaskGetTickCount() < M2T(IMU_STARTUP_TIME_MS));
i2cdevInit(I2C3_DEV);
mpu6500Init(I2C3_DEV);
if (mpu6500TestConnection() == true)
{
DEBUG_PRINT("MPU9250 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MPU9250 I2C connection [FAIL].\n");
}
mpu6500Reset();
vTaskDelay(M2T(50));
// Activate MPU6500
mpu6500SetSleepEnabled(false);
// Enable temp sensor
mpu6500SetTempSensorEnabled(true);
// Disable interrupts
mpu6500SetIntEnabled(false);
// Connect the HMC5883L to the main I2C bus
mpu6500SetI2CBypassEnabled(true);
// Set x-axis gyro as clock source
mpu6500SetClockSource(MPU6500_CLOCK_PLL_XGYRO);
// Set gyro full scale range
mpu6500SetFullScaleGyroRange(IMU_GYRO_FS_CFG);
// Set accelerometer full scale range
mpu6500SetFullScaleAccelRange(IMU_ACCEL_FS_CFG);
#ifdef IMU_MPU6500_DLPF_256HZ
// 256Hz digital low-pass filter only works with little vibrations
// Set output rate (15): 8000 / (1 + 15) = 500Hz
mpu6500SetRate(15);
// Set digital low-pass bandwidth
mpu6500SetDLPFMode(MPU6500_DLPF_BW_256);
#else
// To low DLPF bandwidth might cause instability and decrease agility
// but it works well for handling vibrations and unbalanced propellers
// Set output rate (1): 1000 / (1 + 1) = 500Hz
mpu6500SetRate(1);
// Set digital low-pass bandwidth
mpu6500SetDLPFMode(MPU6500_DLPF_BW_98);
#endif
#ifdef IMU_ENABLE_MAG_AK8963
ak8963Init(I2C3_DEV);
if (ak8963TestConnection() == true)
{
isMagPresent = true;
ak8963SetMode(AK8963_MODE_16BIT | AK8963_MODE_CONT2); // 16bit 100Hz
DEBUG_PRINT("AK8963 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("AK8963 I2C connection [FAIL].\n");
}
#endif
#ifdef IMU_ENABLE_PRESSURE_LPS25H
lps25hInit(I2C3_DEV);
if (lps25hTestConnection() == true)
{
lps25hSetEnabled(true);
isBaroPresent = true;
DEBUG_PRINT("LPS25H I2C connection [OK].\n");
}
else
{
//TODO: Should sensor test fail hard if no connection
DEBUG_PRINT("LPS25H I2C connection [FAIL].\n");
}
#endif
imuBiasInit(&gyroBias);
#ifdef IMU_TAKE_ACCEL_BIAS
imuBiasInit(&accelBias);
#endif
varianceSampleTime = -GYRO_MIN_BIAS_TIMEOUT_MS + 1;
imuAccLpfAttFactor = IMU_ACC_IIR_LPF_ATT_FACTOR;
cosPitch = cosf(configblockGetCalibPitch() * (float) M_PI/180);
sinPitch = sinf(configblockGetCalibPitch() * (float) M_PI/180);
cosRoll = cosf(configblockGetCalibRoll() * (float) M_PI/180);
sinRoll = sinf(configblockGetCalibRoll() * (float) M_PI/180);
isInit = true;
}
static void main_task(void *pvParameters) {
int i;
char ch;
bool selftestPasses = true;
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_I2C1_Init();
MX_USART1_UART_Init();
MX_SPI1_Init();
MX_USB_DEVICE_Init();
// Light up all LEDs to test
ledOn(ledRanging);
ledOn(ledSync);
ledOn(ledMode);
printf("\r\n\r\n====================\r\n");
printf("SYSTEM\t: CPU-ID: ");
for (i=0; i<12; i++) {
printf("%02x", uid[i]);
}
printf("\r\n");
// Initializing pressure sensor (if present ...)
lps25hInit(&hi2c1);
testSupportPrintStart("Initializing pressure sensor");
if (lps25hTestConnection()) {
printf("[OK]\r\n");
lps25hSetEnabled(true);
} else {
printf("[FAIL] (%u)\r\n", (unsigned int)hi2c1.ErrorCode);
selftestPasses = false;
}
testSupportPrintStart("Pressure sensor self-test");
testSupportReport(&selftestPasses, lps25hSelfTest());
// Initializing i2c eeprom
eepromInit(&hi2c1);
testSupportPrintStart("EEPROM self-test");
testSupportReport(&selftestPasses, eepromTest());
cfgInit();
// Initialising radio
testSupportPrintStart("Initialize UWB ");
uwbInit();
if (uwbTest()) {
printf("[OK]\r\n");
} else {
printf("[ERROR]: %s\r\n", uwbStrError());
selftestPasses = false;
}
if (!selftestPasses) {
printf("TEST\t: One or more self-tests failed, blocking startup!\r\n");
usbcommSetSystemStarted(true);
}
// Printing UWB configuration
struct uwbConfig_s * uwbConfig = uwbGetConfig();
printf("CONFIG\t: Address is 0x%X\r\n", uwbConfig->address[0]);
printf("CONFIG\t: Mode is %s\r\n", uwbAlgorithmName(uwbConfig->mode));
printf("CONFIG\t: Tag mode anchor list (%i): ", uwbConfig->anchorListSize);
for (i = 0; i < uwbConfig->anchorListSize; i++) {
printf("0x%02X ", uwbConfig->anchors[i]);
}
printf("\r\n");
HAL_Delay(500);
ledOff(ledRanging);
ledOff(ledSync);
ledOff(ledMode);
printf("SYSTEM\t: Node started ...\r\n");
printf("SYSTEM\t: Press 'h' for help.\r\n");
usbcommSetSystemStarted(true);
// Starts UWB protocol
uwbStart();
// Main loop ...
while(1) {
usbcommPrintWelcomeMessage();
ledTick();
// // Measure pressure
// if (uwbConfig.mode != modeSniffer) {
// if(lps25hGetData(&pressure, &temperature, &asl)) {
// pressure_ok = true;
// } else {
// printf("Fail reading pressure\r\n");
// printf("pressure not ok\r\n");
// }
// }
// Accepts serial commands
#ifdef USE_FTDI_UART
if (HAL_UART_Receive(&huart1, (uint8_t*)&ch, 1, 0) == HAL_OK) {
#else
if(usbcommRead(&ch, 1)) {
#endif
handleInput(ch);
}
}
}
/* Function required to use "printf" to print on serial console */
int _write (int fd, const void *buf, size_t count)
{
// stdout
if (fd == 1) {
#ifdef USE_FTDI_UART
HAL_UART_Transmit(&huart1, (uint8_t *)buf, count, HAL_MAX_DELAY);
#else
usbcommWrite(buf, count);
#endif
}
// stderr
if (fd == 2) {
HAL_UART_Transmit(&huart1, (uint8_t *)buf, count, HAL_MAX_DELAY);
}
return count;
}
static void handleInput(char ch) {
bool configChanged = true;
static enum menu_e {mainMenu, modeMenu} currentMenu = mainMenu;
switch (currentMenu) {
case mainMenu:
switch (ch) {
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
changeAddress(ch - '0');
break;
case 'a': changeMode(MODE_ANCHOR); break;
case 't': changeMode(MODE_TAG); break;
case 's': changeMode(MODE_SNIFFER); break;
case 'm':
printModeList();
printf("Type 0-9 to choose new mode...\r\n");
currentMenu = modeMenu;
configChanged = false;
break;
case 'd': restConfig(); break;
case 'h':
help();
configChanged = false;
break;
case '#':
productionTestsRun();
printf("System halted, reset to continue\r\n");
while(true){}
break;
default:
configChanged = false;
break;
}
break;
case modeMenu:
switch(ch) {
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
changeMode(ch - '0');
currentMenu = mainMenu;
break;
default:
printf("Incorrect mode '%c'\r\n", ch);
currentMenu = mainMenu;
configChanged = false;
break;
}
break;
}
if (configChanged) {
printf("EEPROM configuration changed, restart for it to take effect!\r\n");
}
}
static void sensorsDeviceInit(void)
{
isMagnetometerPresent = false;
isBarometerPresent = false;
// Wait for sensors to startup
while (xTaskGetTickCount() < 1000);
i2cdevInit(I2C3_DEV);
mpu6500Init(I2C3_DEV);
if (mpu6500TestConnection() == true)
{
DEBUG_PRINT("MPU9250 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MPU9250 I2C connection [FAIL].\n");
}
mpu6500Reset();
vTaskDelay(M2T(50));
// Activate MPU6500
mpu6500SetSleepEnabled(false);
// Delay until registers are reset
vTaskDelay(M2T(100));
// Set x-axis gyro as clock source
mpu6500SetClockSource(MPU6500_CLOCK_PLL_XGYRO);
// Delay until clock is set and stable
vTaskDelay(M2T(200));
// Enable temp sensor
mpu6500SetTempSensorEnabled(true);
// Disable interrupts
mpu6500SetIntEnabled(false);
// Connect the MAG and BARO to the main I2C bus
mpu6500SetI2CBypassEnabled(true);
// Set gyro full scale range
mpu6500SetFullScaleGyroRange(SENSORS_GYRO_FS_CFG);
// Set accelerometer full scale range
mpu6500SetFullScaleAccelRange(SENSORS_ACCEL_FS_CFG);
// Set accelerometer digital low-pass bandwidth
mpu6500SetAccelDLPF(MPU6500_ACCEL_DLPF_BW_41);
#if SENSORS_MPU6500_DLPF_256HZ
// 256Hz digital low-pass filter only works with little vibrations
// Set output rate (15): 8000 / (1 + 7) = 1000Hz
mpu6500SetRate(7);
// Set digital low-pass bandwidth
mpu6500SetDLPFMode(MPU6500_DLPF_BW_256);
#else
// To low DLPF bandwidth might cause instability and decrease agility
// but it works well for handling vibrations and unbalanced propellers
// Set output rate (1): 1000 / (1 + 0) = 1000Hz
mpu6500SetRate(0);
// Set digital low-pass bandwidth for gyro
mpu6500SetDLPFMode(MPU6500_DLPF_BW_98);
// Init second order filer for accelerometer
for (uint8_t i = 0; i < 3; i++)
{
lpf2pInit(&gyroLpf[i], 1000, GYRO_LPF_CUTOFF_FREQ);
lpf2pInit(&accLpf[i], 1000, ACCEL_LPF_CUTOFF_FREQ);
}
#endif
#ifdef SENSORS_ENABLE_MAG_AK8963
ak8963Init(I2C3_DEV);
if (ak8963TestConnection() == true)
{
isMagnetometerPresent = true;
ak8963SetMode(AK8963_MODE_16BIT | AK8963_MODE_CONT2); // 16bit 100Hz
DEBUG_PRINT("AK8963 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("AK8963 I2C connection [FAIL].\n");
}
#endif
#ifdef SENSORS_ENABLE_PRESSURE_LPS25H
lps25hInit(I2C3_DEV);
if (lps25hTestConnection() == true)
{
lps25hSetEnabled(true);
isBarometerPresent = true;
DEBUG_PRINT("LPS25H I2C connection [OK].\n");
}
else
{
//TODO: Should sensor test fail hard if no connection
DEBUG_PRINT("LPS25H I2C connection [FAIL].\n");
}
#endif
cosPitch = cosf(configblockGetCalibPitch() * (float) M_PI/180);
sinPitch = sinf(configblockGetCalibPitch() * (float) M_PI/180);
cosRoll = cosf(configblockGetCalibRoll() * (float) M_PI/180);
sinRoll = sinf(configblockGetCalibRoll() * (float) M_PI/180);
}
