void mpu6050Tool(char *args[], uint8_t argc){
int8_t r;
r=getArgNum(args, argc, "-help", 1);
if(r>=0){
printf("mpu -init gyroScale accelScale\n\r");
printf("mpu -print (anz rtime)\n\r");
return;
}
r=getArgNum(args, argc, "-init", 1);
if(r>=0){
if(argc == 3)
mpu6050Init(atoi(args[r+1]), atoi(args[r+2]));
else
mpu6050Init(0,0); //default init
mpu6050Cal(ofSet, ofSet+3, 10);
return;
}
r=getArgNum(args, argc, "-print", 1);
if(r>=0){
if(argc>=3){
uint16_t i;
for(i=atoi(args[r+1]);i; i--){
print();
sleep(atoi(args[r+2]));
}
}
else{
print();
}
return;
}
printf("??\n\r");
}
void imu6Init(void)
{
if(isInit)
return;
isHmc5883lPresent = false;
isMs5611Present = false;
// Wait for sensors to startup
while (xTaskGetTickCount() < M2T(IMU_STARTUP_TIME_MS));
i2cdevInit(I2C1);
mpu6050Init(I2C1);
if (mpu6050TestConnection() == true)
{
DEBUG_PRINT("MPU6050 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MPU6050 I2C connection [FAIL].\n");
}
mpu6050Reset();
vTaskDelay(M2T(50));
// Activate MPU6050
mpu6050SetSleepEnabled(false);
// Enable temp sensor
mpu6050SetTempSensorEnabled(true);
// Disable interrupts
mpu6050SetIntEnabled(false);
// Connect the HMC5883L to the main I2C bus
mpu6050SetI2CBypassEnabled(true);
// Set x-axis gyro as clock source
mpu6050SetClockSource(MPU6050_CLOCK_PLL_XGYRO);
// Set gyro full scale range
mpu6050SetFullScaleGyroRange(IMU_GYRO_FS_CFG);
// Set accelerometer full scale range
mpu6050SetFullScaleAccelRange(IMU_ACCEL_FS_CFG);
#ifdef IMU_MPU6050_DLPF_256HZ
// 256Hz digital low-pass filter only works with little vibrations
// Set output rate (15): 8000 / (1 + 15) = 500Hz
mpu6050SetRate(15);
// Set digital low-pass bandwidth
mpu6050SetDLPFMode(MPU6050_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
mpu6050SetRate(1);
// Set digital low-pass bandwidth
mpu6050SetDLPFMode(MPU6050_DLPF_BW_98);
#endif
#ifdef IMU_ENABLE_MAG_HMC5883
hmc5883lInit(I2C1);
if (hmc5883lTestConnection() == true)
{
isHmc5883lPresent = true;
DEBUG_PRINT("HMC5883 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("HMC5883L I2C connection [FAIL].\n");
}
#endif
#ifdef IMU_ENABLE_PRESSURE_MS5611
if (ms5611Init(I2C1) == true)
{
isMs5611Present = true;
DEBUG_PRINT("MS5611 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MS5611 I2C connection [FAIL].\n");
}
#endif
imuBiasInit(&gyroBias);
imuBiasInit(&accelBias);
varianceSampleTime = -GYRO_MIN_BIAS_TIMEOUT_MS + 1;
imuAccLpfAttFactor = IMU_ACC_IIR_LPF_ATT_FACTOR;
cosPitch = cos(configblockGetCalibPitch() * M_PI/180);
sinPitch = sin(configblockGetCalibPitch() * M_PI/180);
cosRoll = cos(configblockGetCalibRoll() * M_PI/180);
sinRoll = sin(configblockGetCalibRoll() * M_PI/180);
isInit = true;
}
/**
* @brief Application entry point.
* @details
*/
int main(void) {
/* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/* Initializes a serial-over-USB CDC driver. */
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/* Activates the USB driver and then the USB bus pull-up on D+.
* Note, a delay is inserted in order to not have to disconnect the cable
* after a reset.
*/
usbStop(serusbcfg.usbp);
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(500);
usbConnectBus(serusbcfg.usbp);
usbStart(serusbcfg.usbp, &usbcfg);
/* Activates the serial driver 4 using the driver's default configuration. */
sdStart(&SD4, NULL);
/* Activates the I2C driver 2. */
i2cInit();
i2cStart(&I2CD2, &i2cfg_d2);
/* Enables the CRC peripheral clock. */
rccEnableCRC(FALSE);
/* Loads settings from external EEPROM chip. */
if (eepromLoadSettings()) {
g_boardStatus |= EEPROM_24C02_DETECTED;
}
/* Initializes the MPU6050 sensor. */
if (mpu6050Init()) {
g_boardStatus |= MPU6050_LOW_DETECTED;
/* Creates a taken binary semaphore. */
chBSemInit(&bsemNewDataReady, TRUE);
/* Creates the MPU6050 polling thread and attitude calculation thread. */
chThdCreateStatic(waPollMPU6050Thread, sizeof(waPollMPU6050Thread),
NORMALPRIO + 1, PollMPU6050Thread, NULL);
chThdCreateStatic(waAttitudeThread, sizeof(waAttitudeThread),
HIGHPRIO, AttitudeThread, NULL);
/* Starts motor drivers. */
pwmOutputStart();
/* Starts ADC and ICU input drivers. */
mixedInputStart();
}
/* Creates the blinker thread. */
chThdCreateStatic(waBlinkerThread, sizeof(waBlinkerThread),
LOWPRIO, BlinkerThread, NULL);
/* Normal main() thread activity. */
while (TRUE) {
g_chnp = serusbcfg.usbp->state == USB_ACTIVE ? (BaseChannel *)&SDU1 : (BaseChannel *)&SD4;
telemetryReadSerialData();
if (eepromIsDataLeft()) {
eepromContinueSaving();
}
chThdSleepMilliseconds(TELEMETRY_SLEEP_MS);
}
/* This point should never be reached. */
return 0;
}