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); }