static void stabilizerTask(void* param) { uint32_t tick = 0; uint32_t lastWakeTime; vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR); //Wait for the system to be fully started to start stabilization loop systemWaitStart(); // Wait for sensors to be calibrated lastWakeTime = xTaskGetTickCount (); while(!sensorsAreCalibrated()) { vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP)); } while(1) { vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP)); #ifdef ESTIMATOR_TYPE_kalman stateEstimatorUpdate(&state, &sensorData, &control); #else sensorsAcquire(&sensorData, tick); stateEstimator(&state, &sensorData, tick); #endif commanderGetSetpoint(&setpoint, &state); sitAwUpdateSetpoint(&setpoint, &sensorData, &state); stateController(&control, &sensorData, &state, &setpoint, tick); powerDistribution(&control); tick++; } }
/** * Proximity task running at PROXIMITY_TASK_FREQ Hz. * * @param param Currently unused. */ static void proximityTask(void* param) { uint32_t lastWakeTime; vTaskSetApplicationTaskTag(0, (void*)TASK_PROXIMITY_ID_NBR); //Wait for the system to be fully started to start stabilization loop systemWaitStart(); lastWakeTime = xTaskGetTickCount(); while(1) { vTaskDelayUntil(&lastWakeTime, F2T(PROXIMITY_TASK_FREQ)); #if defined(MAXSONAR_ENABLED) /* Read the MaxBotix sensor. */ proximityDistance = maxSonarReadDistance(MAXSONAR_MB1040_AN, &proximityAccuracy); #endif /* Get the latest average value calculated. */ proximityDistanceAvg = proximitySWinAdd(proximityDistance); /* Get the latest median value calculated. */ proximityDistanceMedian = proximitySWinMedian(proximitySWin); } }
static void poseCommanderTask(void* param) { uint32_t lastWakeTime; //Wait for the system to be fully started to start pose controller loop systemWaitStart(); lastWakeTime = xTaskGetTickCount (); while(1) { vTaskDelayUntil(&lastWakeTime, F2T(POSECOMMANDERFREQUENCY)); // 100Hz // read actual pose value actualPoseGetPose(&actualPose); // get desired pose value if(desiredPoses.front != 0) { desiredPose = *(Pose*)(desiredPoses.front->item); } // update data for logging updateErrors(&desiredPose, &actualPose); // test for goal reached and shift if(reached(&desiredPose, &actualPose)) { if(desiredPoses.front != 0) { if(desiredPoses.front->next != 0) { linkedlistItem front = *linkedListPopFront(&desiredPoses); deleteLinkedlistItem(&front); } } } } }
static void stabilizerTask(void* param) { 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)); // Magnetometer not yet used more then for logging. imu9Read(&gyro, &acc, &mag); if (imu6IsCalibrated()) { commanderGetRPY(&rollDesired, &pitchDesired, &yawDesired); sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT); sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual); if (horizonMode) { horizonPID(eulerRollActual, eulerPitchActual, -gyro.z, rollDesired, pitchDesired, yawDesired); } else { ratePID(gyro.x, -gyro.y, -gyro.z, rollDesired, pitchDesired, yawDesired); } controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw); commanderGetThrust(&actuatorThrust); /* Call out before performing thrust updates, if any functions would like to influence the thrust. */ if (armed) { distributePower(actuatorThrust, actuatorRoll, actuatorPitch, actuatorYaw); } else { distributePower(0, 0, 0, 0); controllerResetAllPID(); } } } }
static void stabilizerTask(void* param) { uint32_t attitudeCounter = 0; uint32_t altHoldCounter = 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)); // 500Hz // Magnetometer not yet used more then for logging. imu9Read(&gyro, &acc, &mag); if (imu6IsCalibrated()) { commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired); commanderGetRPYType(&rollType, &pitchType, &yawType); // 250HZ 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); accWZ = sensfusion6GetAccZWithoutGravity(acc.x, acc.y, acc.z); accMAG = (acc.x * acc.x) + (acc.y * acc.y) + (acc.z * acc.z); // Estimate speed from acc (drifts) vSpeed += deadband(accWZ, vAccDeadband) * FUSION_UPDATE_DT; controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual, eulerRollDesired, eulerPitchDesired, -eulerYawDesired, &rollRateDesired, &pitchRateDesired, &yawRateDesired); attitudeCounter = 0; } // 100HZ if (imuHasBarometer() && (++altHoldCounter >= ALTHOLD_UPDATE_RATE_DIVIDER)) { stabilizerAltHoldUpdate(); altHoldCounter = 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); if (!altHold || !imuHasBarometer()) { // Use thrust from controller if not in altitude hold mode commanderGetThrust(&actuatorThrust); } else { // Added so thrust can be set to 0 while in altitude hold mode after disconnect commanderWatchdog(); } 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(); } } } }
static void sensorsTask(void *param) { systemWaitStart(); uint32_t lastWakeTime = xTaskGetTickCount(); static BiasObj bmi160GyroBias; static BiasObj bmi055GyroBias; #ifdef SENSORS_TAKE_ACCEL_BIAS static BiasObj bmi160AccelBias; static BiasObj bmi055AccelBias; #endif Axis3i16 gyroPrim; Axis3i16 accelPrim; Axis3f accelPrimScaled; Axis3i16 accelPrimLPF; Axis3i32 accelPrimStoredFilterValues; #ifdef LOG_SEC_IMU Axis3i16 gyroSec; Axis3i16 accelSec; Axis3f accelSecScaled; Axis3i16 accelSecLPF; Axis3i32 accelSecStoredFilterValues; #endif /* LOG_SEC_IMU */ /* wait an additional second the keep bus free * this is only required by the z-ranger, since the * configuration will be done after system start-up */ //vTaskDelayUntil(&lastWakeTime, M2T(1500)); while (1) { vTaskDelayUntil(&lastWakeTime, F2T(SENSORS_READ_RATE_HZ)); /* calibrate if necessary */ if (!allSensorsAreCalibrated) { if (!bmi160GyroBias.found) { sensorsGyroCalibrate(&bmi160GyroBias, SENSORS_BMI160); #ifdef SENSORS_TAKE_ACCEL_BIAS sensorsAccelCalibrate(&bmi160AccelBias, &bmi160GyroBias, SENSORS_BMI160); #endif } if (!bmi055GyroBias.found) { sensorsGyroCalibrate(&bmi055GyroBias, SENSORS_BMI055); #ifdef SENSORS_TAKE_ACCEL_BIAS sensorsAccelCalibrate(&bmi055AccelBias, &bmi055GyroBias, SENSORS_BMI055); #endif } if ( bmi160GyroBias.found && bmi055GyroBias.found #ifdef SENSORS_TAKE_ACCEL_BIAS && bmi160AccelBias.found && bmi055AccelBias.found #endif ) { // soundSetEffect(SND_CALIB); DEBUG_PRINT("Sensor calibration [OK].\n"); ledseqRun(SYS_LED, seq_calibrated); allSensorsAreCalibrated= true; } } else { /* get data from chosen sensors */ sensorsGyroGet(&gyroPrim, gyroPrimInUse); sensorsAccelGet(&accelPrim, accelPrimInUse); #ifdef LOG_SEC_IMU sensorsGyroGet(&gyroSec, gyroSecInUse); sensorsAccelGet(&accelSec, accelSecInUse); #endif /* FIXME: for sensor deck v1 realignment has to be added her */ switch(gyroPrimInUse) { case SENSORS_BMI160: sensorsApplyBiasAndScale(&sensors.gyro, &gyroPrim, &bmi160GyroBias.value, SENSORS_BMI160_DEG_PER_LSB_CFG); break; case SENSORS_BMI055: sensorsApplyBiasAndScale(&sensors.gyro, &gyroPrim, &bmi055GyroBias.value, SENSORS_BMI055_DEG_PER_LSB_CFG); break; } sensorsAccIIRLPFilter(&accelPrim, &accelPrimLPF, &accelPrimStoredFilterValues, (int32_t)sensorsAccLpfAttFactor); switch(accelPrimInUse) { case SENSORS_BMI160: sensorsApplyBiasAndScale(&accelPrimScaled, &accelPrimLPF, &bmi160AccelBias.value, SENSORS_BMI160_G_PER_LSB_CFG); break; case SENSORS_BMI055: sensorsApplyBiasAndScale(&accelPrimScaled, &accelPrimLPF, &bmi055AccelBias.value, SENSORS_BMI055_G_PER_LSB_CFG); break; } sensorsAccAlignToGravity(&accelPrimScaled, &sensors.acc); #ifdef LOG_SEC_IMU switch(gyroSecInUse) { case SENSORS_BMI160: sensorsApplyBiasAndScale(&sensors.gyroSec, &gyroSec, &bmi160GyroBias.value, SENSORS_BMI160_DEG_PER_LSB_CFG); break; case SENSORS_BMI055: sensorsApplyBiasAndScale(&sensors.gyroSec, &gyroSec, &bmi055GyroBias.value, SENSORS_BMI055_DEG_PER_LSB_CFG); break; } sensorsAccIIRLPFilter(&accelSec, &accelSecLPF, &accelSecStoredFilterValues, (int32_t)sensorsAccLpfAttFactor); switch(accelSecInUse) { case SENSORS_BMI160: sensorsApplyBiasAndScale(&accelSecScaled, &accelSecLPF, &bmi160AccelBias.value, SENSORS_BMI160_G_PER_LSB_CFG); break; case SENSORS_BMI055: sensorsApplyBiasAndScale(&accelSecScaled, &accelSecLPF, &bmi055AccelBias.value, SENSORS_BMI055_G_PER_LSB_CFG); break; } sensorsAccAlignToGravity(&accelSecScaled, &sensors.accSec); #endif } if (isMagnetometerPresent) { static uint8_t magMeasDelay = SENSORS_DELAY_MAG; if (--magMeasDelay == 0) { bmm150_read_mag_data(&bmm150Dev); sensors.mag.x = bmm150Dev.data.x; sensors.mag.y = bmm150Dev.data.y; sensors.mag.z = bmm150Dev.data.z; magMeasDelay = SENSORS_DELAY_MAG; } } if (isBarometerPresent) { static uint8_t baroMeasDelay = SENSORS_DELAY_BARO; static int32_t v_temp_s32; static uint32_t v_pres_u32; static baro_t* baro280 = &sensors.baro; if (--baroMeasDelay == 0) { bmp280_read_pressure_temperature(&v_pres_u32, &v_temp_s32); sensorsScaleBaro(baro280, (float)v_pres_u32, (float)v_temp_s32/100.0f); baroMeasDelay = baroMeasDelayMin; } } /* ensure all queues are populated at the same time */ vTaskSuspendAll(); xQueueOverwrite(accelPrimDataQueue, &sensors.acc); xQueueOverwrite(gyroPrimDataQueue, &sensors.gyro); #ifdef LOG_SEC_IMU xQueueOverwrite(gyroSecDataQueue, &sensors.gyroSec); xQueueOverwrite(accelSecDataQueue, &sensors.accSec); #endif if (isBarometerPresent) { xQueueOverwrite(baroPrimDataQueue, &sensors.baro); } if (isMagnetometerPresent) { xQueueOverwrite(magPrimDataQueue, &sensors.mag); } xTaskResumeAll(); } }
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 } } }
static void stabilizerTask(void* param) { uint32_t lastWakeTime; //uint32_t tempTime; uint16_t heartbCounter = 0; uint16_t attitudeCounter = 0; uint16_t altHoldCounter = 0; //uint32_t data[6]; //Wait for the system to be fully started to start stabilization loop systemWaitStart(); lastWakeTime = xTaskGetTickCount (); for( ; ;) { //tempTime = lastWakeTime; vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz heartbCounter ++; /* if (lastWakeTime < tempTime) { tempTime = (0 - tempTime) + lastWakeTime; } else { tempTime = lastWakeTime - tempTime; } */ while (heartbCounter >= HEART_UPDATE_RATE_DIVIDER) { // 1Hz MAVLINK(mavlink_msg_heartbeat_send(MAVLINK_COMM_0, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_PREFLIGHT, 0, MAV_STATE_STANDBY);) heartbCounter = 0; } imuRead(&gyro, &acc, &mag); if (imu6IsCalibrated()) { // 250HZ if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER) { MahonyAHRSupdateIMU(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z); //filterUpdate_mars(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z); //MahonyAHRSupdate(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z); //MahonyAHRSupdate(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z,mag.y,mag.x,mag.z); //filterUpdate_mars(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z); //MahonyAHRSupdate(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z,mag.y,mag.x,mag.z); sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual); radRollActual = eulerRollActual * M_PI / 180.0f; radPitchActual = eulerPitchActual * M_PI / 180.0f; radYawActual = eulerYawActual * M_PI / 180.0f; //float yh, xh; #define yh (mag.y * cos(radRollActual) - mag.z * sin(radRollActual)) #define xh (mag.x*cos(radPitchActual) + mag.y*sin(radRollActual)*sin(radPitchActual) + mag.z * cos(radRollActual)*sin(radPitchActual)) radYawActual = atan2(-yh,xh); MAVLINK(mavlink_msg_attitude_send(MAVLINK_COMM_0, lastWakeTime, \ radRollActual, radPitchActual, radYawActual, \ gyro.x, gyro.y, gyro.z);) attitudeCounter = 0; }