/* ********************************************************************************************************* * 函 数 名: bsp_StartAutoTimer * 功能说明: 启动一个自动定时器,并设置定时周期。 * 形 参: _id : 定时器ID,值域【0,TMR_COUNT-1】。用户必须自行维护定时器ID,以避免定时器ID冲突。 * _period : 定时周期,单位10ms * 返 回 值: 无 ********************************************************************************************************* */ void timerStartAuto ( uint8_t _id, uint32_t _period ) { if ( _id >= TMR_COUNT ) { /* 打印出错的源代码文件名、函数名称 */ uartPrintf ( "Error: file %s, function %s()\r\n", __FILE__, __FUNCTION__ ); while ( 1 ); /* 参数异常,死机等待看门狗复位 */ } DISABLE_INT(); /* 关中断 */ s_tTmr[_id].Count = _period; /* 实时计数器初值 */ s_tTmr[_id].PreLoad = _period; /* 计数器自动重装值,仅自动模式起作用 */ s_tTmr[_id].Flag = 0; /* 定时时间到标志 */ s_tTmr[_id].Mode = TMR_AUTO_MODE; /* 自动工作模式 */ ENABLE_INT(); /* 开中断 */ }
static void stabilizerTask(void* param) { uint32_t attitudeCounter = 0; uint32_t lastWakeTime; vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR); //Wait for the system to be fully started to start stabilization loop systemWaitStart(); lastWakeTime = xTaskGetTickCount (); while(1) { vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); imu6Read(&gyro, &acc); hmc5883lGetHeading(&magHeadingX, &magHeadingY, &magHeadingZ); if(altMode == ALTIMETER_MODE_PRESSURE) { altimeterTmp = ms5611GetPressure(MS5611_OSR_4096); if(altimeterTmp > 0.0f) { pressure = altimeterTmp; altMode = ALTIMETER_MODE_TEMPERATURE; } } if(altMode == ALTIMETER_MODE_TEMPERATURE) { altimeterTmp = ms5611GetTemperature(MS5611_OSR_4096); if(altimeterTmp > 0.0f) { temperature = altimeterTmp; altMode = ALTIMETER_MODE_PRESSURE; } } if (imu6IsCalibrated()) { commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired); commanderGetRPYType(&rollType, &pitchType, &yawType); if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER) { //sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT); //sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual); //controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual, // eulerRollDesired, eulerPitchDesired, -eulerYawDesired, // &rollRateDesired, &pitchRateDesired, &yawRateDesired); //controllerCorrectAttitudePID(0, 0, 0, 0, 0, 0, &rollRateDesired, &pitchRateDesired, &yawRateDesired); attitudeCounter = 0; } if (rollType == RATE) { rollRateDesired = eulerRollDesired; } if (pitchType == RATE) { pitchRateDesired = eulerPitchDesired; } if (yawType == RATE) { yawRateDesired = -eulerYawDesired; } /* // TODO: Investigate possibility to subtract gyro drift. controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z, rollRateDesired, pitchRateDesired, yawRateDesired); controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw); */ commanderGetTrust(&actuatorThrust); distributePower(actuatorThrust, 0, 0, 0); if (actuatorThrust > 0) { #if defined(TUNE_ROLL) distributePower(actuatorThrust, actuatorRoll, 0, 0); #elif defined(TUNE_PITCH) distributePower(actuatorThrust, 0, actuatorPitch, 0); #elif defined(TUNE_YAW) distributePower(actuatorThrust, 0, 0, -actuatorYaw); #else distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw); #endif } else { distributePower(0, 0, 0, 0); controllerResetAllPID(); } #if 0 static int i = 0; if (++i > 19) { uartPrintf("%i, %i, %i\n", (int32_t)(eulerRollActual*100), (int32_t)(eulerPitchActual*100), (int32_t)(eulerYawActual*100)); i = 0; } #endif } } }
void imu6Read(Axis3f* gyroOut, Axis3f* accOut) { mpu6050GetMotion6(&accelMpu.x, &accelMpu.y, &accelMpu.z, &gyroMpu.x, &gyroMpu.y, &gyroMpu.z); imuAddBiasValue(&gyroBias, &gyroMpu); if (!accelBias.isBiasValueFound) { imuAddBiasValue(&accelBias, &accelMpu); } if (!gyroBias.isBiasValueFound) { imuFindBiasValue(&gyroBias); if (gyroBias.isBiasValueFound) { ledseqRun(LED_RED, seq_calibrated); // uartPrintf("Gyro bias: %i, %i, %i\n", // gyroBias.bias.x, gyroBias.bias.y, gyroBias.bias.z); } } #ifdef IMU_TAKE_ACCEL_BIAS if (gyroBias.isBiasValueFound && !accelBias.isBiasValueFound) { Axis3i32 mean; imuCalculateBiasMean(&accelBias, &mean); accelBias.bias.x = mean.x; accelBias.bias.y = mean.y; accelBias.bias.z = mean.z - IMU_1G_RAW; accelBias.isBiasValueFound = TRUE; //uartPrintf("Accel bias: %i, %i, %i\n", // accelBias.bias.x, accelBias.bias.y, accelBias.bias.z); } #endif imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues, (int32_t)imuAccLpfAttFactor); imuAccAlignToGravity(&accelLPF, &accelLPFAligned); // Re-map outputs #if 1 gyroOut->y = -((gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG); gyroOut->x = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG; gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG; accOut->y = -((accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG); accOut->x = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG; accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG; #else gyroOut->x = (gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG; gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG; gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG; accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG; accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG; accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG; #endif // uartSendData(sizeof(Axis3f), (uint8_t*)gyroOut); // uartSendData(sizeof(Axis3f), (uint8_t*)accOut); #if 0 static uint32_t count = 0; if (++count >= 19) { count = 0; uartPrintf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n", (int32_t)(gyroOut->x * 10), (int32_t)(gyroOut->y * 10), (int32_t)(gyroOut->z * 10), (int32_t)(accOut->x * 1000), (int32_t)(accOut->y * 1000), (int32_t)(accOut->z * 1000), mag.x, mag.y, mag.z); } #endif }