static void steeringHandleISR() { int steeringFeedback; //Could be yaw OR yaw rate float relativeYaw = 0.0; if((steeringMode == STEERMODE_YAW_DEC) || (steeringMode == STEERMODE_YAW_SPLIT)){ relativeYaw = imuGetBodyZPositionDeg() - steeringInitialYaw; steeringFeedback = (int)(14.375*relativeYaw); } else{ steeringFeedback = imuGetGyroZValueAvg(); } //Threshold filter on gyro to account for minor drift //if (ABS(wz) < GYRO_DRIFT_THRESH) { // wz = 0; //} //Update the setpoints //if((currentMove->inputL != 0) && (currentMove->inputR != 0)){ if ((currentMove != idleMove) || (inMotion == 1) ) { //Only update steering controller if we are in motion #ifdef PID_SOFTWARE pidUpdate(&steeringPID, steeringFeedback); #elif defined PID_HARDWARE int temp = 0; temp = steeringPID.input; //Save unscaled input val steeringPID.input *= STEERING_PID_ERR_SCALER; //Scale input pidUpdate(&steeringPID, STEERING_PID_ERR_SCALER * steeringFeedback); //Update with scaled feedback steeringPID.input = temp; //Reset unscaled input #endif //PID_SOFTWWARE vs PID_HARDWARE } //Now the output correction is stored in steeringPID.output, //which will be read later when the steering mixing is done. }
void controllerCorrectRatePID( float rollRateActual, float pitchRateActual, float yawRateActual, float rollRateDesired, float pitchRateDesired, float yawRateDesired) { pidSetDesired(&pidRollRate, rollRateDesired); TRUNCATE_SINT16(rollOutput, pidUpdate(&pidRollRate, rollRateActual, TRUE)); pidSetDesired(&pidPitchRate, pitchRateDesired); TRUNCATE_SINT16(pitchOutput, pidUpdate(&pidPitchRate, pitchRateActual, TRUE)); pidSetDesired(&pidYawRate, yawRateDesired); TRUNCATE_SINT16(yawOutput, pidUpdate(&pidYawRate, yawRateActual, TRUE)); }
void attitudeControllerCorrectRatePID( float rollRateActual, float pitchRateActual, float yawRateActual, float rollRateDesired, float pitchRateDesired, float yawRateDesired) { pidSetDesired(&pidRollRate, rollRateDesired); rollOutput = saturateSignedInt16(pidUpdate(&pidRollRate, rollRateActual, true)); pidSetDesired(&pidPitchRate, pitchRateDesired); pitchOutput = saturateSignedInt16(pidUpdate(&pidPitchRate, pitchRateActual, true)); pidSetDesired(&pidYawRate, yawRateDesired); yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true)); }
void Roll_Pitch_Yaw_AnglePID(float Angle_roll,float Angle_pitch,float Angle_yaw){ float RateTarget ,yaw_error; static U8 add=0; add++; //ROLL if(add==2) { pidSetTarget_Measure(&Stabilize_Roll, Angle_roll*100.0f,ypr[2]*100.0f); //目标角度 Stabilize_Roll.merror = Math_fConstrain(Stabilize_Roll.merror,-4500.0f,+4500.0f); RateTarget = pidUpdate(&Stabilize_Roll ,ypr[2]*100.0f , PID_dt); pidSetTarget(&RollRate, RateTarget); //Math_fConstrain(RollRate.RateTarget,-150.0f,+150.0f)//角度限幅据说可以减少震荡 } pidUpdate(&RollRate ,IMU_GYROx*100.0f , PID_dt); RollRate.PID_out *= 0.1f; RollRate.PID_out = Math_fConstrain(RollRate.PID_out,-150.0f,+150.0f); //限制控制PWM信号的幅度,限幅的大小可以适当的改变 //PITCH if(add==2) { pidSetTarget_Measure(&Stabilize_Pitch, Angle_pitch*100.0f,-ypr[1]*100.0f); Stabilize_Pitch.merror = Math_fConstrain(Stabilize_Pitch.merror,-4500.0f,+4500.0f); RateTarget = pidUpdate(&Stabilize_Pitch ,-ypr[1]*100.0f , PID_dt); pidSetTarget(&PitchRate, RateTarget); //Math_fConstrain(PitchRate.RateTarget,-150.0f,+150.0f)//角度限幅据说可以减少震荡 } pidUpdate(&PitchRate ,IMU_GYROy*100.0f , PID_dt); PitchRate.PID_out *= 0.1f; PitchRate.PID_out = Math_fConstrain(PitchRate.PID_out,-150.0f,+150.0f); //限制控制PWM信号的幅度,限幅的大小可以适当的改变 //YAW if(add==2) { pidSetTarget(&Stabilize_Yaw, Angle_yaw*100.0f); pidSetMeasured(&Stabilize_Yaw, ypr[0]*100.0f); yaw_error =Get_Yaw_Error(Angle_yaw , ypr[0]); //目标角度 yaw_error *= 100.0f; yaw_error = Math_fConstrain(yaw_error,-4500.0f,+4500.0f); RateTarget = pidUpdate_err(&Stabilize_Yaw ,yaw_error, PID_dt); pidSetTarget(&YawRate, RateTarget); //Math_fConstrain(YawRate.RateTarget,-150.0f,+150.0f)//角度限幅据说可以减少震荡 add=0; } pidUpdate(&YawRate ,-IMU_GYROz*100.0f , PID_dt); YawRate.PID_out *= 0.1f; YawRate.PID_out = Math_fConstrain(YawRate.PID_out,-150.0f,+150.0f); //限制控制PWM信号的幅度,限幅的大小可以适当的改变 }
// Runs the PID controllers for the legs void serviceMotionPID() { //Apply steering mixing, without overwriting anything int presteer[2] = {motor_pidObjs[0].input, motor_pidObjs[1].input}; int poststeer[2] = {0, 0}; steeringApplyCorrection(presteer, poststeer); motor_pidObjs[0].input = poststeer[0]; motor_pidObjs[1].input = poststeer[1]; updateBEMF(); /////////// PID Section ////////// int j; for (j = 0; j < NUM_MOTOR_PIDS; j++) { //We are now measuring battery voltage directly via AN0, // so the input offset to each PID loop can actually be tracked, and needs // to be updated. This should compensate for battery voltage drooping over time. motor_pidObjs[j].inputOffset = adcGetVBatt(); //pidobjs[0] : Left side //pidobjs[0] : Right side if (motor_pidObjs[j].onoff) { //TODO: Do we want to add provisions to track error, even when //the output is switched off? #ifdef PID_SOFTWARE //Update values pidUpdate(&(motor_pidObjs[j]), bemf[j]); #elif defined PID_HARDWARE //Apply scaling, update, remove scaling for consistency int temp; temp = motor_pidObjs[j].input; //Save unscaled input val motor_pidObjs[j].input *= MOTOR_PID_SCALER; //Scale input pidUpdate(&(motor_pidObjs[j]), MOTOR_PID_SCALER* bemf[j]); motor_pidObjs[j].input = temp; //Reset unscaled input #endif //PID_SOFTWWARE vs PID_HARDWARE //Set PWM duty cycle SetDCMCPWM(legCtrlOutputChannels[j], motor_pidObjs[j].output, 0); }//end of if (on / off) else if (PID_ZEROING_ENABLE) { //if PID loop is off SetDCMCPWM(legCtrlOutputChannels[j], 0, 0); } } // end of for(j) }
void attitudeControllerCorrectAttitudePID( float eulerRollActual, float eulerPitchActual, float eulerYawActual, float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired, float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired) { pidSetDesired(&pidRoll, eulerRollDesired); *rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, true); // Update PID for pitch axis pidSetDesired(&pidPitch, eulerPitchDesired); *pitchRateDesired = pidUpdate(&pidPitch, eulerPitchActual, true); // Update PID for yaw axis float yawError; yawError = eulerYawDesired - eulerYawActual; if (yawError > 180.0f) yawError -= 360.0f; else if (yawError < -180.0f) yawError += 360.0f; pidSetError(&pidYaw, yawError); *yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, false); }
void controllerUpdate(Flywheel *flywheel, float timeChange) { switch (flywheel->controllerType) { case CONTROLLER_TYPE_PID: pidUpdate(flywheel, timeChange); break; case CONTROLLER_TYPE_TBH: tbhUpdate(flywheel, timeChange); break; case CONTROLLER_TYPE_BANG_BANG: bangBangUpdate(flywheel, timeChange); break; } if (flywheel->action > 127) { flywheel->action = 127; } if (flywheel->action < -127) { flywheel->action = -127; } }
void control_quadrotor_attitude( const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, const struct vehicle_rates_setpoint_s *rate_sp, const struct attitude_control_quat_params *control, struct actuator_controls_s *actuators) { float pitchCommand, rollCommand, ruddCommand, throttleCommand; if(fabsf(rate_sp->yaw) < FLT_MIN) { // hold heading float yawRateTarget = pidUpdate(controlData.yawAngle, 0.0f, compassDifferenceRad(controlData.yawSetpoint, att->yaw)); // seek a 0 deg difference between hold heading and actual yaw ruddCommand = constrainFloat(pidUpdate(controlData.yawRate, yawRateTarget, att->yawspeed), -control->controlMax, control->controlMax); } else { // rate controls ruddCommand = constrainFloat(pidUpdate(controlData.yawRate, rate_sp->yaw, att->yawspeed), -control->controlMax, control->controlMax); control_quadrotor_set_yaw(att->yaw); } // smooth float rollTarget = utilFilter3(controlData.rollFilter, att_sp->roll_body); // roll angle rollCommand = pidUpdate(controlData.rollAngle, rollTarget, att->roll); // rate rollCommand += pidUpdate(controlData.rollRate, 0.0f, att->rollspeed); rollCommand = constrainFloat(rollCommand, -control->controlMax, control->controlMax); // smooth float pitchTarget = utilFilter3(controlData.pitchFilter, att_sp->pitch_body); // pitch angle pitchCommand = pidUpdate(controlData.pitchAngle, pitchTarget, att->pitch); // rate pitchCommand += pidUpdate(controlData.pitchRate, 0.0f, att->pitchspeed); pitchCommand = constrainFloat(pitchCommand, -control->controlMax, control->controlMax); throttleCommand = att_sp->thrust; actuators->control[0] = rollCommand; actuators->control[1] = pitchCommand; actuators->control[2] = ruddCommand; actuators->control[3] = throttleCommand; }
static void stabilizerAltHoldUpdate() { // Get the time float altitudeError = 0; static float altitudeError_i = 0; float instAcceleration = 0; float deltaVertSpeed = 0; static uint32_t timeStart = 0; static uint32_t timeCurrent = 0; // Get altitude hold commands from pilot commanderGetAltHold(&altHold, &setAltHold, &altHoldChange); // Get barometer height estimates //TODO do the smoothing within getData ms5611GetData(&pressure, &temperature, &aslRaw); // Compute the altitude altitudeError = aslRaw - estimatedAltitude; altitudeError_i = fmin(50.0, fmax(-50.0, altitudeError_i + altitudeError)); instAcceleration = deadband(accWZ, vAccDeadband) * 9.80665 + altitudeError_i * altEstKi; deltaVertSpeed = instAcceleration * ALTHOLD_UPDATE_DT + (altEstKp1 * ALTHOLD_UPDATE_DT) * altitudeError; estimatedAltitude += (vSpeedComp * 2.0 + deltaVertSpeed) * (ALTHOLD_UPDATE_DT / 2) + (altEstKp2 * ALTHOLD_UPDATE_DT) * altitudeError; vSpeedComp += deltaVertSpeed; aslLong = estimatedAltitude; // Override aslLong // Estimate vertical speed based on Acc - fused with baro to reduce drift vSpeedComp = constrain(vSpeedComp, -vSpeedLimit, vSpeedLimit); // Reset Integral gain of PID controller if being charged if (!pmIsDischarging()) { altHoldPID.integ = 0.0; } // Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point if (setAltHold) { // Set to current altitude altHoldTarget = estimatedAltitude; // Set the start time timeStart = xTaskGetTickCount(); timeCurrent = 0; // Reset PID controller pidInit(&altHoldPID, estimatedAltitude, altHoldKp, altHoldKi, altHoldKd, ALTHOLD_UPDATE_DT); // Cache last integral term for reuse after pid init // const float pre_integral = hoverPID.integ; // Reset the PID controller for the hover controller. We want zero vertical velocity pidInit(&hoverPID, 0, hoverKp, hoverKi, hoverKd, ALTHOLD_UPDATE_DT); pidSetIntegralLimit(&hoverPID, 3); // TODO set low and high limits depending on voltage // TODO for now just use previous I value and manually set limits for whole voltage range // pidSetIntegralLimit(&altHoldPID, 12345); // pidSetIntegralLimitLow(&altHoldPID, 12345); / // hoverPID.integ = pre_integral; } // In altitude hold mode if (altHold) { // Get current time timeCurrent = xTaskGetTickCount(); // Update target altitude from joy controller input altHoldTarget += altHoldChange / altHoldChange_SENS; pidSetDesired(&altHoldPID, altHoldTarget); // Compute error (current - target), limit the error altHoldErr = constrain(deadband(estimatedAltitude - altHoldTarget, errDeadband), -altHoldErrMax, altHoldErrMax); pidSetError(&altHoldPID, -altHoldErr); // Get control from PID controller, dont update the error (done above) float altHoldPIDVal = pidUpdate(&altHoldPID, estimatedAltitude, false); // Get the PID value for the hover float hoverPIDVal = pidUpdate(&hoverPID, vSpeedComp, true); float thrustValFloat; // Use different weights depending on time into altHold mode if (timeCurrent > 150) { // Compute the mixture between the alt hold and the hover PID thrustValFloat = 0.5*hoverPIDVal + 0.5*altHoldPIDVal; } else { // Compute the mixture between the alt hold and the hover PID thrustValFloat = 0.1*hoverPIDVal + 0.9*altHoldPIDVal; } // float thrustVal = 0.5*hoverPIDVal + 0.5*altHoldPIDVal; uint32_t thrustVal = altHoldBaseThrust + (int32_t)(thrustValFloat*pidAslFac); // compute new thrust actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust, limitThrust( thrustVal ))); // i part should compensate for voltage drop } else { altHoldTarget = 0.0; altHoldErr = 0.0; } }
static void stabilizerAltHoldUpdate(void) { // Get altitude hold commands from pilot commanderGetAltHold(&altHold, &setAltHold, &altHoldChange); // Get barometer height estimates //TODO do the smoothing within getData ms5611GetData(&pressure, &temperature, &aslRaw); asl = asl * aslAlpha + aslRaw * (1 - aslAlpha); aslLong = aslLong * aslAlphaLong + aslRaw * (1 - aslAlphaLong); // Estimate vertical speed based on successive barometer readings. This is ugly :) vSpeedASL = deadband(asl - aslLong, vSpeedASLDeadband); // Estimate vertical speed based on Acc - fused with baro to reduce drift vSpeed = constrain(vSpeed, -vSpeedLimit, vSpeedLimit); vSpeed = vSpeed * vBiasAlpha + vSpeedASL * (1.f - vBiasAlpha); vSpeedAcc = vSpeed; // Reset Integral gain of PID controller if being charged if (!pmIsDischarging()) { altHoldPID.integ = 0.0; } // Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point if (setAltHold) { // Set to current altitude altHoldTarget = asl; // Cache last integral term for reuse after pid init const float pre_integral = altHoldPID.integ; // Reset PID controller pidInit(&altHoldPID, asl, altHoldKp, altHoldKi, altHoldKd, ALTHOLD_UPDATE_DT); // TODO set low and high limits depending on voltage // TODO for now just use previous I value and manually set limits for whole voltage range // pidSetIntegralLimit(&altHoldPID, 12345); // pidSetIntegralLimitLow(&altHoldPID, 12345); / altHoldPID.integ = pre_integral; // Reset altHoldPID altHoldPIDVal = pidUpdate(&altHoldPID, asl, false); } // In altitude hold mode if (altHold) { // Update target altitude from joy controller input altHoldTarget += altHoldChange / altHoldChange_SENS; pidSetDesired(&altHoldPID, altHoldTarget); // Compute error (current - target), limit the error altHoldErr = constrain(deadband(asl - altHoldTarget, errDeadband), -altHoldErrMax, altHoldErrMax); pidSetError(&altHoldPID, -altHoldErr); // Get control from PID controller, dont update the error (done above) // Smooth it and include barometer vspeed // TODO same as smoothing the error?? altHoldPIDVal = (pidAlpha) * altHoldPIDVal + (1.f - pidAlpha) * ((vSpeedAcc * vSpeedAccFac) + (vSpeedASL * vSpeedASLFac) + pidUpdate(&altHoldPID, asl, false)); // compute new thrust actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust, limitThrust( altHoldBaseThrust + (int32_t)(altHoldPIDVal*pidAslFac)))); // i part should compensate for voltage drop } else { altHoldTarget = 0.0; altHoldErr = 0.0; altHoldPIDVal = 0.0; } }
void navNavigate(void) { unsigned long currentTime; unsigned char leg = navData.missionLeg; navMission_t *curLeg = &navData.missionLegs[leg]; float tmp; currentTime = IMU_LASTUPD; navSetFixType(); // is there sufficient position accuracy to navigate? if (navData.fixType == 3) navData.navCapable = 1; // do not drop out of mission due to (hopefully) temporary GPS degradation else if (navData.mode < NAV_STATUS_POSHOLD) navData.navCapable = 0; // Can we navigate && do we want to be in mission mode? if ((supervisorData.state & STATE_ARMED) && navData.navCapable && RADIO_FLAPS > 250) { // are we currently in position hold mode && do we have a clear mission ahead of us? if ((navData.mode == NAV_STATUS_POSHOLD || navData.mode == NAV_STATUS_DVH) && leg < NAV_MAX_MISSION_LEGS && curLeg->type > 0) { curLeg = navLoadLeg(leg); navData.mode = NAV_STATUS_MISSION; } } // do we want to be in position hold mode? else if ((supervisorData.state & STATE_ARMED) && RADIO_FLAPS > -250) { // always allow alt hold if (navData.mode < NAV_STATUS_ALTHOLD) { // record this altitude as the hold altitude navSetHoldAlt(ALTITUDE, 0); // set integral to current RC throttle setting pidZeroIntegral(navData.altSpeedPID, -VELOCITYD, motorsData.throttle * RADIO_MID_THROTTLE / MOTORS_SCALE); pidZeroIntegral(navData.altPosPID, ALTITUDE, 0.0f); navData.holdSpeedAlt = navData.targetHoldSpeedAlt = -VELOCITYD; navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM]; navData.mode = NAV_STATUS_ALTHOLD; // notify ground AQ_NOTICE("Altitude Hold engaged\n"); } // are we not in position hold mode now? if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && navData.mode != NAV_STATUS_POSHOLD && navData.mode != NAV_STATUS_DVH) { // only zero bias if coming from lower mode if (navData.mode < NAV_STATUS_POSHOLD) { navData.holdTiltN = 0.0f; navData.holdTiltE = 0.0f; // speed pidZeroIntegral(navData.speedNPID, UKF_VELN, 0.0f); pidZeroIntegral(navData.speedEPID, UKF_VELE, 0.0f); // pos pidZeroIntegral(navData.distanceNPID, UKF_POSN, 0.0f); pidZeroIntegral(navData.distanceEPID, UKF_POSE, 0.0f); } // store this position as hold position navUkfSetHereAsPositionTarget(); if (navData.navCapable) { // set this position as home if we have none if (navData.homeLeg.targetLat == (double)0.0 || navData.homeLeg.targetLon == (double)0.0) navSetHomeCurrent(); } // activate pos hold navData.mode = NAV_STATUS_POSHOLD; navData.holdSpeedN = 0.0f; navData.holdSpeedE = 0.0f; navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED]; navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM]; // notify ground AQ_NOTICE("Position Hold engaged\n"); } // DVH else if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && ( RADIO_PITCH > p[CTRL_DEAD_BAND] || RADIO_PITCH < -p[CTRL_DEAD_BAND] || RADIO_ROLL > p[CTRL_DEAD_BAND] || RADIO_ROLL < -p[CTRL_DEAD_BAND])) { navData.mode = NAV_STATUS_DVH; } else if (navData.mode == NAV_STATUS_DVH) { // allow speed to drop before holding position (or if RTH engaged) if ((UKF_VELN < +0.1f && UKF_VELN > -0.1f && UKF_VELE < +0.1f && UKF_VELE > -0.1f) || RADIO_AUX2 < -250) { navUkfSetHereAsPositionTarget(); navData.mode = NAV_STATUS_POSHOLD; } } } else { // switch to manual mode navData.mode = NAV_STATUS_MANUAL; // reset mission legs navData.missionLeg = leg = 0; // keep up with changing altitude navSetHoldAlt(ALTITUDE, 0); } // ceiling set ?, 0 is disable if (navData.ceilingAlt) { // ceiling reached 1st time if (ALTITUDE > navData.ceilingAlt && !navData.setCeilingFlag) { navData.setCeilingFlag = 1; navData.ceilingTimer = timerMicros(); } // ceiling still reached for 5 seconds else if (navData.setCeilingFlag && ALTITUDE > navData.ceilingAlt && (timerMicros() - navData.ceilingTimer) > (1e6 * 5) ) { navData.ceilingTimer = timerMicros(); if (!navData.setCeilingReached) AQ_NOTICE("Warning: Altitude ceiling reached."); navData.setCeilingReached = 1; } else if ((navData.setCeilingFlag || navData.setCeilingReached) && ALTITUDE <= navData.ceilingAlt) { if (navData.setCeilingReached) AQ_NOTICE("Notice: Altitude returned to within ceiling."); navData.setCeilingFlag = 0; navData.setCeilingReached = 0; } } // home set if ((supervisorData.state & STATE_ARMED) && RADIO_AUX2 > 250) { if (!navData.homeActionFlag) { navSetHomeCurrent(); navData.homeActionFlag = 1; } } // recall home else if ((supervisorData.state & STATE_ARMED) && RADIO_AUX2 < -250) { if (!navData.homeActionFlag) { navRecallHome(); AQ_NOTICE("Returning to home position\n"); navData.homeActionFlag = 1; } } // switch to middle, clear action flag else { navData.homeActionFlag = 0; } // heading-free mode if ((int)p[NAV_HDFRE_CHAN] > 0 && (int)p[NAV_HDFRE_CHAN] <= RADIO_MAX_CHANNELS) { navSetHeadFreeMode(); // set/maintain headfree frame reference if (!navData.homeActionFlag && ( navData.headFreeMode == NAV_HEADFREE_SETTING || (navData.headFreeMode == NAV_HEADFREE_DYNAMIC && navData.mode == NAV_STATUS_DVH) )) { uint8_t dfRefTyp = 0; if ((supervisorData.state & STATE_FLYING) && navData.homeLeg.targetLat != (double)0.0f && navData.homeLeg.targetLon != (double)0.0f) { if (NAV_HF_HOME_DIST_D_MIN && NAV_HF_HOME_DIST_FREQ && (currentTime - navData.homeDistanceLastUpdate) > (AQ_US_PER_SEC / NAV_HF_HOME_DIST_FREQ)) { navData.distanceToHome = navCalcDistance(gpsData.lat, gpsData.lon, navData.homeLeg.targetLat, navData.homeLeg.targetLon); navData.homeDistanceLastUpdate = currentTime; } if (!NAV_HF_HOME_DIST_D_MIN || navData.distanceToHome > NAV_HF_HOME_DIST_D_MIN) dfRefTyp = 1; } navSetHfReference(dfRefTyp); } } if (UKF_POSN != 0.0f || UKF_POSE != 0.0f) { navData.holdCourse = compassNormalize(atan2f(-UKF_POSE, -UKF_POSN) * RAD_TO_DEG); navData.holdDistance = __sqrtf(UKF_POSN*UKF_POSN + UKF_POSE*UKF_POSE); } else { navData.holdCourse = 0.0f; navData.holdDistance = 0.0f; } if (navData.mode == NAV_STATUS_MISSION) { // have we arrived yet? if (navData.loiterCompleteTime == 0) { // are we close enough (distance and altitude)? // goto/home test if (((curLeg->type == NAV_LEG_GOTO || curLeg->type == NAV_LEG_HOME) && navData.holdDistance < curLeg->targetRadius && fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) || // orbit test (curLeg->type == NAV_LEG_ORBIT && fabsf(navData.holdDistance - curLeg->targetRadius) + fabsf(navData.holdAlt - ALTITUDE) < 2.0f) || // takeoff test (curLeg->type == NAV_LEG_TAKEOFF && navData.holdDistance < curLeg->targetRadius && fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) ) { // freeze heading unless orbiting if (curLeg->type != NAV_LEG_ORBIT) navSetHoldHeading(AQ_YAW); // start the loiter clock navData.loiterCompleteTime = currentTime + curLeg->loiterTime; #ifdef USE_MAVLINK // notify ground mavlinkWpReached(leg); #endif } } // have we loitered long enough? else if (currentTime > navData.loiterCompleteTime && curLeg->type != NAV_LEG_LAND) { // next leg if (++leg < NAV_MAX_MISSION_LEGS && navData.missionLegs[leg].type > 0) { curLeg = navLoadLeg(leg); } else { navData.mode = NAV_STATUS_POSHOLD; } } } // DVH if (navData.mode == NAV_STATUS_DVH) { float factor = (1.0f / 500.0f) * navData.holdMaxHorizSpeed; float x = 0.0f; float y = 0.0f; if (RADIO_PITCH > p[CTRL_DEAD_BAND]) x = -(RADIO_PITCH - p[CTRL_DEAD_BAND]) * factor; if (RADIO_PITCH < -p[CTRL_DEAD_BAND]) x = -(RADIO_PITCH + p[CTRL_DEAD_BAND]) * factor; if (RADIO_ROLL > p[CTRL_DEAD_BAND]) y = +(RADIO_ROLL - p[CTRL_DEAD_BAND]) * factor; if (RADIO_ROLL < -p[CTRL_DEAD_BAND]) y = +(RADIO_ROLL + p[CTRL_DEAD_BAND]) * factor; // do we want to ignore rotation of craft (headfree/carefree mode)? if (navData.headFreeMode > NAV_HEADFREE_OFF) { if (navData.hfUseStoredReference) { // rotate to stored frame navData.holdSpeedN = x * navData.hfReferenceCos - y * navData.hfReferenceSin; navData.holdSpeedE = y * navData.hfReferenceCos + x * navData.hfReferenceSin; } else { // don't rotate to any frame (pitch/roll move to N/S/E/W) navData.holdSpeedN = x; navData.holdSpeedE = y; } } else { // rotate to earth frame navData.holdSpeedN = x * navUkfData.yawCos - y * navUkfData.yawSin; navData.holdSpeedE = y * navUkfData.yawCos + x * navUkfData.yawSin; } } // orbit POI else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_ORBIT) { float velX, velY; // maintain orbital radius velX = -pidUpdate(navData.distanceNPID, curLeg->targetRadius, navData.holdDistance); // maintain orbital velocity (clock wise) velY = -curLeg->maxHorizSpeed; // rotate to earth frame navData.holdSpeedN = velX * navUkfData.yawCos - velY * navUkfData.yawSin; navData.holdSpeedE = velY * navUkfData.yawCos + velX * navUkfData.yawSin; } else { // distance => velocity navData.holdSpeedN = pidUpdate(navData.distanceNPID, 0.0f, UKF_POSN); navData.holdSpeedE = pidUpdate(navData.distanceEPID, 0.0f, UKF_POSE); } // normalize N/E speed requests to fit below max nav speed tmp = __sqrtf(navData.holdSpeedN*navData.holdSpeedN + navData.holdSpeedE*navData.holdSpeedE); if (tmp > navData.holdMaxHorizSpeed) { navData.holdSpeedN = (navData.holdSpeedN / tmp) * navData.holdMaxHorizSpeed; navData.holdSpeedE = (navData.holdSpeedE / tmp) * navData.holdMaxHorizSpeed; } // velocity => tilt navData.holdTiltN = -pidUpdate(navData.speedNPID, navData.holdSpeedN, UKF_VELN); navData.holdTiltE = +pidUpdate(navData.speedEPID, navData.holdSpeedE, UKF_VELE); if (navData.mode > NAV_STATUS_MANUAL) { float vertStick; // Throttle controls vertical speed vertStick = RADIO_THROT - RADIO_MID_THROTTLE; if ((vertStick > p[CTRL_DBAND_THRO] && !navData.setCeilingReached) || vertStick < -p[CTRL_DBAND_THRO]) { // altitude velocity proportional to throttle stick navData.targetHoldSpeedAlt = (vertStick - p[CTRL_DBAND_THRO] * (vertStick > 0.0f ? 1.0f : -1.0f)) * p[NAV_ALT_POS_OM] * (1.0f / RADIO_MID_THROTTLE); navData.verticalOverride = 1; } // are we trying to land? else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_LAND) { navData.targetHoldSpeedAlt = -navData.holdMaxVertSpeed; } // coming out of vertical override? else if (navData.verticalOverride) { navData.targetHoldSpeedAlt = 0.0f; // slow down before trying to hold altitude if (fabsf(VELOCITYD) < 0.025f) navData.verticalOverride = 0; // set new hold altitude to wherever we are while still in override if (navData.mode != NAV_STATUS_MISSION) navSetHoldAlt(ALTITUDE, 0); } // PID has the throttle else { navData.targetHoldSpeedAlt = pidUpdate(navData.altPosPID, navData.holdAlt, ALTITUDE); } // constrain vertical velocity navData.targetHoldSpeedAlt = constrainFloat(navData.targetHoldSpeedAlt, (navData.holdMaxVertSpeed < p[NAV_MAX_DECENT]) ? -navData.holdMaxVertSpeed : -p[NAV_MAX_DECENT], navData.holdMaxVertSpeed); // smooth vertical velocity changes navData.holdSpeedAlt += (navData.targetHoldSpeedAlt - navData.holdSpeedAlt) * 0.05f; } else { navData.verticalOverride = 0; } // calculate POI angle (used for tilt in gimbal function) if (navData.mode == NAV_STATUS_MISSION && curLeg->poiAltitude != 0.0f) { float a, b, c; a = navData.holdDistance; b = ALTITUDE - curLeg->poiAltitude; c = __sqrtf(a*a + b*b); navData.poiAngle = asinf(a/c) * RAD_TO_DEG - 90.0f; } else { navData.poiAngle = 0.0f; } if (navData.mode == NAV_STATUS_MISSION) { // recalculate autonomous heading navSetHoldHeading(navData.targetHeading); // wait for low throttle if landing if (curLeg->type == NAV_LEG_LAND && motorsData.throttle <= 1) // shut everything down (sure hope we are really on the ground :) supervisorDisarm(); } navData.lastUpdate = currentTime; }
void navNavigate(void) { unsigned long currentTime = IMU_LASTUPD; unsigned char leg = navData.missionLeg; navMission_t *curLeg = &navData.missionLegs[leg]; int reqFlightMode; // requested flight mode based on user controls or other factors float tmp; navSetFixType(); // is there sufficient position accuracy to navigate? if (navData.fixType == 3) navData.navCapable = 1; // do not drop out of mission due to (hopefully) temporary GPS degradation else if (navData.mode < NAV_STATUS_POSHOLD) navData.navCapable = 0; bool hasViableMission = (navData.navCapable && ((navData.mode != NAV_STATUS_MISSION && leg < NAV_MAX_MISSION_LEGS && curLeg->type) || (navData.mode == NAV_STATUS_MISSION && navData.hasMissionLeg))); // this defines the hierarchy of available flight modes in case of failsafe override or conflicting controls being active if (navData.spvrModeOverride) reqFlightMode = navData.spvrModeOverride; else if (rcIsSwitchActive(NAV_CTRL_MISN)) if (hasViableMission) reqFlightMode = NAV_STATUS_MISSION; else reqFlightMode = NAV_STATUS_POSHOLD; else if (rcIsSwitchActive(NAV_CTRL_PH)) { reqFlightMode = NAV_STATUS_POSHOLD; } else if (rcIsSwitchActive(NAV_CTRL_AH)) reqFlightMode = NAV_STATUS_ALTHOLD; // always default to manual else reqFlightMode = NAV_STATUS_MANUAL; // Can we navigate && do we want to be in mission mode? if ((supervisorData.state & STATE_ARMED) && reqFlightMode == NAV_STATUS_MISSION && hasViableMission) { // are we currently in position hold mode && do we have a clear mission ahead of us? if (navData.mode == NAV_STATUS_POSHOLD || navData.mode == NAV_STATUS_DVH) { curLeg = navLoadLeg(leg); navData.mode = NAV_STATUS_MISSION; AQ_NOTICE("Mission mode active\n"); } } // do we want to be in position hold mode? else if ((supervisorData.state & STATE_ARMED) && reqFlightMode > NAV_STATUS_MANUAL) { // check for DVH if (reqFlightMode == NAV_STATUS_POSHOLD && (RADIO_PITCH > p[CTRL_DEAD_BAND] || RADIO_PITCH < -p[CTRL_DEAD_BAND] || RADIO_ROLL > p[CTRL_DEAD_BAND] || RADIO_ROLL < -p[CTRL_DEAD_BAND])) reqFlightMode = NAV_STATUS_DVH; // allow alt hold regardless of GPS or flow quality if (navData.mode < NAV_STATUS_ALTHOLD || (navData.mode != NAV_STATUS_ALTHOLD && reqFlightMode == NAV_STATUS_ALTHOLD)) { // record this altitude as the hold altitude navSetHoldAlt(ALTITUDE, 0); // set integral to current RC throttle setting pidZeroIntegral(navData.altSpeedPID, -VELOCITYD, motorsData.throttle * RADIO_MID_THROTTLE / MOTORS_SCALE); pidZeroIntegral(navData.altPosPID, ALTITUDE, 0.0f); navData.holdSpeedAlt = navData.targetHoldSpeedAlt = -VELOCITYD; navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED; navData.mode = NAV_STATUS_ALTHOLD; // notify ground AQ_NOTICE("Altitude Hold engaged\n"); } // are we not in position hold mode now? if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && reqFlightMode > NAV_STATUS_ALTHOLD && navData.mode != NAV_STATUS_POSHOLD && navData.mode != NAV_STATUS_DVH) { // only zero bias if coming from lower mode if (navData.mode < NAV_STATUS_POSHOLD) { navData.holdTiltN = 0.0f; navData.holdTiltE = 0.0f; // speed pidZeroIntegral(navData.speedNPID, UKF_VELN, 0.0f); pidZeroIntegral(navData.speedEPID, UKF_VELE, 0.0f); // pos pidZeroIntegral(navData.distanceNPID, UKF_POSN, 0.0f); pidZeroIntegral(navData.distanceEPID, UKF_POSE, 0.0f); } // store this position as hold position navUkfSetHereAsPositionTarget(); if (navData.navCapable) { // set this position as home if we have none if (navData.homeLeg.targetLat == (double)0.0 || navData.homeLeg.targetLon == (double)0.0) navSetHomeCurrent(); } // activate pos hold navData.mode = NAV_STATUS_POSHOLD; navData.holdSpeedN = 0.0f; navData.holdSpeedE = 0.0f; navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED; navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED; // notify ground AQ_NOTICE("Position Hold engaged\n"); } // DVH else if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && reqFlightMode == NAV_STATUS_DVH) { navData.mode = NAV_STATUS_DVH; } // coming out of DVH mode? else if (navData.mode == NAV_STATUS_DVH) { // allow speed to drop before holding position (or if RTH engaged) // FIXME: RTH switch may no longer be engaged but craft is still returning to home? if ((UKF_VELN < +0.1f && UKF_VELN > -0.1f && UKF_VELE < +0.1f && UKF_VELE > -0.1f) || rcIsSwitchActive(NAV_CTRL_HOM_GO)) { navUkfSetHereAsPositionTarget(); navData.mode = NAV_STATUS_POSHOLD; } } } // default to manual mode else { if (navData.mode != NAV_STATUS_MANUAL) AQ_NOTICE("Manual mode active.\n"); navData.mode = NAV_STATUS_MANUAL; // reset mission legs navData.missionLeg = leg = 0; // keep up with changing altitude navSetHoldAlt(ALTITUDE, 0); } // ceiling set ?, 0 is disable if (navData.ceilingAlt) { // ceiling reached 1st time if (ALTITUDE > navData.ceilingAlt && !navData.setCeilingFlag) { navData.setCeilingFlag = 1; navData.ceilingTimer = timerMicros(); } // ceiling still reached for 5 seconds else if (navData.setCeilingFlag && ALTITUDE > navData.ceilingAlt && (timerMicros() - navData.ceilingTimer) > (1e6 * 5) ) { navData.ceilingTimer = timerMicros(); if (!navData.setCeilingReached) AQ_NOTICE("Warning: Altitude ceiling reached."); navData.setCeilingReached = 1; } else if ((navData.setCeilingFlag || navData.setCeilingReached) && ALTITUDE <= navData.ceilingAlt) { if (navData.setCeilingReached) AQ_NOTICE("Notice: Altitude returned to within ceiling."); navData.setCeilingFlag = 0; navData.setCeilingReached = 0; } } if (!(supervisorData.state & STATE_RADIO_LOSS1)) navDoUserCommands(&leg, curLeg); if (UKF_POSN != 0.0f || UKF_POSE != 0.0f) { navData.holdCourse = compassNormalize(atan2f(-UKF_POSE, -UKF_POSN) * RAD_TO_DEG); navData.holdDistance = __sqrtf(UKF_POSN*UKF_POSN + UKF_POSE*UKF_POSE); } else { navData.holdCourse = 0.0f; navData.holdDistance = 0.0f; } if (navData.mode == NAV_STATUS_MISSION) { // have we arrived yet? if (navData.loiterCompleteTime == 0) { // are we close enough (distance and altitude)? // goto/home test if (((curLeg->type == NAV_LEG_GOTO || curLeg->type == NAV_LEG_HOME) && navData.holdDistance < curLeg->targetRadius && fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) || // orbit test (curLeg->type == NAV_LEG_ORBIT && fabsf(navData.holdDistance - curLeg->targetRadius) <= 2.0f && fabsf(navData.holdAlt - ALTITUDE) <= 1.0f) || // takeoff test (curLeg->type == NAV_LEG_TAKEOFF && navData.holdDistance < curLeg->targetRadius && fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) ) { // freeze heading if relative, unless orbiting if (curLeg->type != NAV_LEG_ORBIT && signbit(navData.targetHeading)) navData.targetHeading = AQ_YAW; // start the loiter clock navData.loiterCompleteTime = currentTime + curLeg->loiterTime; AQ_PRINTF("NAV: Reached waypoint %d.\n", leg); #ifdef USE_SIGNALING signalingOnetimeEvent(SIG_EVENT_OT_WP_REACHED); #endif #ifdef USE_MAVLINK // notify ground mavlinkWpReached(leg); #endif } } // have we loitered long enough? else if (currentTime > navData.loiterCompleteTime && curLeg->type != NAV_LEG_LAND) { // next leg leg = ++navData.missionLeg; curLeg = navLoadLeg(leg); if (!navData.hasMissionLeg) navData.mode = NAV_STATUS_POSHOLD; } } // DVH if (navData.mode == NAV_STATUS_DVH) { float factor = (1.0f / 500.0f) * navData.holdMaxHorizSpeed; float x = 0.0f; float y = 0.0f; if (RADIO_PITCH > p[CTRL_DEAD_BAND]) x = -(RADIO_PITCH - p[CTRL_DEAD_BAND]) * factor; if (RADIO_PITCH < -p[CTRL_DEAD_BAND]) x = -(RADIO_PITCH + p[CTRL_DEAD_BAND]) * factor; if (RADIO_ROLL > p[CTRL_DEAD_BAND]) y = +(RADIO_ROLL - p[CTRL_DEAD_BAND]) * factor; if (RADIO_ROLL < -p[CTRL_DEAD_BAND]) y = +(RADIO_ROLL + p[CTRL_DEAD_BAND]) * factor; // do we want to ignore rotation of craft (headfree/carefree mode)? if (navData.headFreeMode > NAV_HEADFREE_OFF) { if (navData.hfUseStoredReference) { // rotate to stored frame navData.holdSpeedN = x * navData.hfReferenceCos - y * navData.hfReferenceSin; navData.holdSpeedE = y * navData.hfReferenceCos + x * navData.hfReferenceSin; } else { // don't rotate to any frame (pitch/roll move to N/S/E/W) navData.holdSpeedN = x; navData.holdSpeedE = y; } } else { // rotate to earth frame navData.holdSpeedN = x * navUkfData.yawCos - y * navUkfData.yawSin; navData.holdSpeedE = y * navUkfData.yawCos + x * navUkfData.yawSin; } } // orbit POI else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_ORBIT) { float velX, velY; // maintain orbital radius velX = -pidUpdate(navData.distanceNPID, curLeg->targetRadius, navData.holdDistance); // maintain orbital velocity (clock wise) velY = -curLeg->maxHorizSpeed; // rotate to earth frame navData.holdSpeedN = velX * navUkfData.yawCos - velY * navUkfData.yawSin; navData.holdSpeedE = velY * navUkfData.yawCos + velX * navUkfData.yawSin; } else { // distance => velocity navData.holdSpeedN = pidUpdate(navData.distanceNPID, 0.0f, UKF_POSN); navData.holdSpeedE = pidUpdate(navData.distanceEPID, 0.0f, UKF_POSE); } // normalize N/E speed requests to fit below max nav speed tmp = __sqrtf(navData.holdSpeedN*navData.holdSpeedN + navData.holdSpeedE*navData.holdSpeedE); if (tmp > navData.holdMaxHorizSpeed) { navData.holdSpeedN = (navData.holdSpeedN / tmp) * navData.holdMaxHorizSpeed; navData.holdSpeedE = (navData.holdSpeedE / tmp) * navData.holdMaxHorizSpeed; } // velocity => tilt navData.holdTiltN = -pidUpdate(navData.speedNPID, navData.holdSpeedN, UKF_VELN); navData.holdTiltE = +pidUpdate(navData.speedEPID, navData.holdSpeedE, UKF_VELE); if (navData.mode > NAV_STATUS_MANUAL) { float vertStick; // Throttle controls vertical speed vertStick = RADIO_THROT - RADIO_MID_THROTTLE; if ((vertStick > p[CTRL_DBAND_THRO] && !navData.setCeilingReached) || vertStick < -p[CTRL_DBAND_THRO]) { // altitude velocity proportional to throttle stick navData.targetHoldSpeedAlt = (vertStick - p[CTRL_DBAND_THRO] * (vertStick > 0.0f ? 1.0f : -1.0f)) * NAV_DFLT_VRT_SPEED * (1.0f / RADIO_MID_THROTTLE); navData.verticalOverride = 1; } // are we trying to land? else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_LAND) { navData.targetHoldSpeedAlt = -navData.holdMaxVertSpeed; } // coming out of vertical override? else if (navData.verticalOverride) { navData.targetHoldSpeedAlt = 0.0f; // slow down before trying to hold altitude if (fabsf(VELOCITYD) < 0.025f) navData.verticalOverride = 0; // set new hold altitude to wherever we are while still in override if (navData.mode != NAV_STATUS_MISSION) navSetHoldAlt(ALTITUDE, 0); } // PID has the throttle else { navData.targetHoldSpeedAlt = pidUpdate(navData.altPosPID, navData.holdAlt, ALTITUDE); } // constrain vertical velocity tmp = configGetParamValue(NAV_MAX_DECENT); tmp = (navData.holdMaxVertSpeed < tmp) ? -navData.holdMaxVertSpeed : -tmp; navData.targetHoldSpeedAlt = constrainFloat(navData.targetHoldSpeedAlt, tmp, navData.holdMaxVertSpeed); // smooth vertical velocity changes navData.holdSpeedAlt += (navData.targetHoldSpeedAlt - navData.holdSpeedAlt) * 0.05f; } else { navData.verticalOverride = 0; } // calculate POI angle (used for tilt in gimbal function) if (navData.mode == NAV_STATUS_MISSION && curLeg->poiAltitude != 0.0f) { float a, b, c; a = navData.holdDistance; b = ALTITUDE - curLeg->poiAltitude; c = __sqrtf(a*a + b*b); navData.poiAngle = asinf(a/c) * RAD_TO_DEG - 90.0f; } else { navData.poiAngle = 0.0f; } if (navData.mode == NAV_STATUS_MISSION) { // recalculate autonomous heading navSetHoldHeading(navData.targetHeading); // wait for low throttle if landing if (curLeg->type == NAV_LEG_LAND && motorsData.throttle <= 1) // shut everything down (sure hope we are really on the ground :) supervisorDisarm(); } navData.lastUpdate = currentTime; }
void pidControl() { angleFiltered = kalmanPitch.getAngle(imu.euler.pitch, imu.gyro.y/131.0, (double)1/FREQ); gyroFiltered = LPF(gyroFiltered, imu.gyro.y); //kalman_filter(imu.euler.pitch, imu.gyro.y/131.0, &angleFiltered, &gyroFiltered); #if 0 balanceControl.pidPitch.desired = pidUpdate(&balanceControl.pidSpeed, balanceControl.PwmLeft) + angleOffset; balanceControl.PwmLeft = pidUpdate(&balanceControl.pidPitch, angleFiltered); currendSpeed = (currendSpeed + balanceControl.PwmLeft * 0.004) * 0.999; balanceControl.PwmLeft += currendSpeed; #endif #if 0 balanceControl.pidPitch.desired = pidUpdate(&balanceControl.pidSpeed, balanceControl.PwmLeft) + angleOffset; balanceControl.PwmLeft = pidUpdate(&balanceControl.pidPitch, angleFiltered); #endif #if 0 //Kp:5 Kd:1 offset:-3 20/10/-6 0.5/0.2/06 balanceControl.PwmLeft = -(1000*(balanceControl.pidPitch.Kp * ((angleFiltered + angleOffset) /90)) + balanceControl.Kd * imu.gyro.y); #endif #if 0 currendSpeed *= 0.7; currendSpeed = currendSpeed + balanceControl.PwmLeft * 0.3; position += currendSpeed; position -= speed_need; if(position<-6000000) position = -6000000; if(position> 6000000) position = 6000000; balanceControl.PwmLeft = balanceControl.pidPitch.Kp * (angleOffset - angleFiltered) +balanceControl.Kd * gyroFiltered -balanceControl.pidSpeed.Ki * position -balanceControl.pidSpeed.Kd * currendSpeed; balanceControl.PwmLeft = -balanceControl.PwmLeft; if(balanceControl.PwmLeft<-60000) balanceControl.PwmLeft = -60000; if(balanceControl.PwmLeft> 60000) balanceControl.PwmLeft = 60000; if(balanceControl.PwmLeft > - 100 && balanceControl.PwmLeft < 100) { balanceControl.PwmLeft = 0; } balanceControl.PwmRight = balanceControl.PwmLeft; #endif #if 0 float gap = abs(balanceControl.pidPitch.desired - imu.euler.pitch); if(gap > 150) { if(flag == 0) { flag = 1; balanceControl.pidPitch.Kp *= 10; balanceControl.pidPitch.Ki *= 10; balanceControl.pidPitch.Kd *= 10; //balanceControl.Kd *= 2; } } else { if(flag == 1) { flag = 0; balanceControl.pidPitch.Kp /= 10; balanceControl.pidPitch.Ki /= 10; balanceControl.pidPitch.Kd /= 10; //balanceControl.Kd /= 2; } } #endif #if 0 //works balanceControl.speed = balanceControl.speed * 0.05 + pidUpdate(&balanceControl.pidSpeed, balanceControl.speed) * 0.95; gyroFiltered = 0.05 * imu.gyro.y + gyroFiltered * 0.95; //angleFiltered = 0.2 * imu.euler.pitch + angleFiltered * 0.8; //angleFiltered = kalmanPitch.getAngle(imu.euler.pitch, imu.gyro.y/131.0, (double)1/250); //angleFiltered = kalman(imu.euler.pitch, imu.gyro.y, (double)1/FREQ); balanceControl.pidPitch.desired = balanceControl.speed + angleOffset; balanceControl.speed = pidUpdate(&balanceControl.pidPitch, angleFiltered) + gyroFiltered * balanceControl.Kd; //balanceControl.speed += pidUpdate(&balanceControl.pidPitch, angleFiltered) + gyroFiltered * balanceControl.Kd; //balanceControl.PwmLeft = pidUpdate(&balanceControl.pidPitch, imu.euler.pitch); if(balanceControl.speed < 0.100 && balanceControl.speed > -0.100) { balanceControl.speed = 0; } //dead-band of PWM #endif #if 1 balanceControl.speed = LPF((float)(balanceControl.PwmLeft + balanceControl.PwmRight)/5600, balanceControl.speed); balanceControl.speed = pidUpdate(&balanceControl.pidSpeed, balanceControl.speed); balanceControl.pidPitch.desired = balanceControl.speed + angleOffset; balanceControl.speed = pidUpdate(&balanceControl.pidPitch, angleFiltered) + gyroFiltered * balanceControl.Kd; if(balanceControl.speed < 0.050 && balanceControl.speed > -0.050) { balanceControl.speed = 0; } //dead-band of PWM #endif //printf("dev:%6.4f ", balanceControl.pidPitch.derivative); printf("AngleSet:%4.2f AngleRef:%4.2f Angle:%4.2f error:%6.4f ", angleOffset, balanceControl.pidPitch.desired, angleFiltered, balanceControl.pidPitch.error); printf("\t| Kp:%3.2f Kd:%3.3f sumerror:%6.2f", balanceControl.pidPitch.Kp, balanceControl.Kd, balanceControl.pidPitch.sumError); printf("\t| Kp:%3.2f Ki:%3.3f sumerror:%6.2f", balanceControl.pidSpeed.Kp, balanceControl.pidSpeed.Ki, balanceControl.pidSpeed.sumError); printf("\t| speedref:%6.2f speed%6.2f error:%6.2f\r\n", balanceControl.pidSpeed.desired, balanceControl.speed, balanceControl.pidSpeed.error); /* printf("\t| Kp:%4.2f Ki:%4.2f Kd:%4.2f Speed:%4.2f angle:%4.2f |\t error:%6.4f sumerror:%6.4f Iterm:%6.4f | PWM:%6.3f\r\n", balanceControl.pidSpeed.Kp, balanceControl.pidSpeed.Ki, balanceControl.pidSpeed.Kd, balanceControl.pidSpeed.desired, balanceControl.pidPitch.desired, balanceControl.pidSpeed.error, balanceControl.pidSpeed.sumError, balanceControl.pidSpeed.intergal, balanceControl.speed); */ balanceControl.PwmLeft = 2800 * (balanceControl.speed * balanceControl.factorL - balanceControl.spinSpeed); balanceControl.PwmRight = 2800 * (balanceControl.speed * balanceControl.factorR + balanceControl.spinSpeed); }
void controlTaskCode(void *unused) { float yaw; float throttle; float ratesDesired[3]; uint16_t overrides[3]; #ifdef USE_QUATOS float quatDesired[4]; float ratesActual[3]; #else float pitch, roll; float pitchCommand, rollCommand, ruddCommand; #endif // USE_QUATOS AQ_NOTICE("Control task started\n"); // disable all axes' rate overrides overrides[0] = 0; overrides[1] = 0; overrides[2] = 0; while (1) { // wait for work CoWaitForSingleFlag(imuData.dRateFlag, 0); // this needs to be done ASAP with the freshest of data if (supervisorData.state & STATE_ARMED) { if (RADIO_THROT > p[CTRL_MIN_THROT] || navData.mode > NAV_STATUS_MANUAL) { supervisorThrottleUp(1); // are we in altitude hold mode? if (navData.mode > NAV_STATUS_MANUAL) { // override throttle with nav's request throttle = pidUpdate(navData.altSpeedPID, navData.holdSpeedAlt, -VELOCITYD) * MOTORS_SCALE / RADIO_MID_THROTTLE; // don't allow negative throttle to be built up if (navData.altSpeedPID->iState < 0.0f) { navData.altSpeedPID->iState = 0.0f; } } else { throttle = ((uint32_t)RADIO_THROT - p[CTRL_MIN_THROT]) * MOTORS_SCALE / RADIO_MID_THROTTLE * p[CTRL_FACT_THRO]; } // limit throttle = constrainInt(throttle, 1, MOTORS_SCALE); // if motors are not yet running, use this heading as hold heading if (motorsData.throttle == 0) { navData.holdHeading = AQ_YAW; controlData.yaw = navData.holdHeading; // Reset all PIDs pidZeroIntegral(controlData.pitchRatePID, 0.0f, 0.0f); pidZeroIntegral(controlData.rollRatePID, 0.0f, 0.0f); pidZeroIntegral(controlData.yawRatePID, 0.0f, 0.0f); pidZeroIntegral(controlData.pitchAnglePID, 0.0f, 0.0f); pidZeroIntegral(controlData.rollAnglePID, 0.0f, 0.0f); pidZeroIntegral(controlData.yawAnglePID, 0.0f, 0.0f); // also set this position as hold position if (navData.mode == NAV_STATUS_POSHOLD) { navUkfSetHereAsPositionTarget(); } } // constrict nav (only) yaw rates yaw = compassDifference(controlData.yaw, navData.holdHeading); yaw = constrainFloat(yaw, -p[CTRL_NAV_YAW_RT]/400.0f, +p[CTRL_NAV_YAW_RT]/400.0f); controlData.yaw = compassNormalize(controlData.yaw + yaw); // DVH overrides direct user pitch / roll requests if (navData.mode != NAV_STATUS_DVH) { controlData.userPitchTarget = RADIO_PITCH * p[CTRL_FACT_PITC]; controlData.userRollTarget = RADIO_ROLL * p[CTRL_FACT_ROLL]; } else { controlData.userPitchTarget = 0.0f; controlData.userRollTarget = 0.0f; } // navigation requests if (navData.mode > NAV_STATUS_ALTHOLD) { controlData.navPitchTarget = navData.holdTiltN; controlData.navRollTarget = navData.holdTiltE; } else { controlData.navPitchTarget = 0.0f; controlData.navRollTarget = 0.0f; } // manual rate cut through for yaw if (RADIO_RUDD > p[CTRL_DEAD_BAND] || RADIO_RUDD < -p[CTRL_DEAD_BAND]) { // fisrt remove dead band if (RADIO_RUDD > p[CTRL_DEAD_BAND]) { ratesDesired[2] = (RADIO_RUDD - p[CTRL_DEAD_BAND]); } else { ratesDesired[2] = (RADIO_RUDD + p[CTRL_DEAD_BAND]); } // calculate desired rate based on full stick scale ratesDesired[2] = ratesDesired[2] * p[CTRL_MAN_YAW_RT] * DEG_TO_RAD * (1.0f / 700.0f); // keep up with actual craft heading controlData.yaw = AQ_YAW; navData.holdHeading = AQ_YAW; // request override overrides[2] = CONTROL_MIN_YAW_OVERRIDE; } else { // currently overriding? if (overrides[2] > 0) { // request zero rate ratesDesired[2] = 0.0f; // follow actual craft heading controlData.yaw = AQ_YAW; navData.holdHeading = AQ_YAW; // decrease override timer overrides[2]--; } } #ifdef USE_QUATOS // determine which frame of reference to control from if (navData.mode <= NAV_STATUS_ALTHOLD) // craft frame - manual { eulerToQuatYPR(quatDesired, controlData.yaw, controlData.userPitchTarget, controlData.userRollTarget); } else // world frame - autonomous { eulerToQuatRPY(quatDesired, controlData.navRollTarget, controlData.navPitchTarget, controlData.yaw); } // reset controller on startup if (motorsData.throttle == 0) { quatDesired[0] = UKF_Q1; quatDesired[1] = UKF_Q2; quatDesired[2] = UKF_Q3; quatDesired[3] = UKF_Q4; quatosReset(quatDesired); } ratesActual[0] = IMU_DRATEX + UKF_GYO_BIAS_X; ratesActual[1] = IMU_DRATEY + UKF_GYO_BIAS_Y; ratesActual[2] = IMU_DRATEZ + UKF_GYO_BIAS_Z; quatos(&UKF_Q1, quatDesired, ratesActual, ratesDesired, overrides); quatosPowerDistribution(throttle); motorsSendThrust(); motorsData.throttle = throttle; #else // smooth controlData.userPitchTarget = utilFilter3(controlData.userPitchFilter, controlData.userPitchTarget); controlData.userRollTarget = utilFilter3(controlData.userRollFilter, controlData.userRollTarget); // smooth controlData.navPitchTarget = utilFilter3(controlData.navPitchFilter, controlData.navPitchTarget); controlData.navRollTarget = utilFilter3(controlData.navRollFilter, controlData.navRollTarget); // rotate nav's NE frame of reference to our craft's local frame of reference pitch = controlData.navPitchTarget * navUkfData.yawCos - controlData.navRollTarget * navUkfData.yawSin; roll = controlData.navRollTarget * navUkfData.yawCos + controlData.navPitchTarget * navUkfData.yawSin; // combine nav & user requests (both are already smoothed) controlData.pitch = pitch + controlData.userPitchTarget; controlData.roll = roll + controlData.userRollTarget; if (p[CTRL_PID_TYPE] == 0) { // pitch angle pitchCommand = pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH); // rate pitchCommand += pidUpdate(controlData.pitchRatePID, 0.0f, IMU_DRATEY); // roll angle rollCommand = pidUpdate(controlData.rollAnglePID, controlData.roll, AQ_ROLL); // rate rollCommand += pidUpdate(controlData.rollRatePID, 0.0f, IMU_DRATEX); } else if (p[CTRL_PID_TYPE] == 1) { // pitch rate from angle pitchCommand = pidUpdate(controlData.pitchRatePID, pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH), IMU_DRATEY); // roll rate from angle rollCommand = pidUpdate(controlData.rollRatePID, pidUpdate(controlData.rollAnglePID, controlData.roll, AQ_ROLL), IMU_DRATEX); } else if (p[CTRL_PID_TYPE] == 2) { // pitch angle pitchCommand = pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH); // rate pitchCommand += pidUpdate(controlData.pitchRatePID, 0.0f, IMU_DRATEY); int axis = 0; // ROLL float PID_P = 3.7; float PID_I = 0.031; float PID_D = 23.0; float error, errorAngle, AngleRateTmp, RateError, delta, deltaSum; float PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; static int16_t lastGyro[3] = { 0, 0, 0 }; static float delta1[3], delta2[3]; static float errorGyroI[3] = { 0, 0, 0 }, errorAngleI[2] = { 0, 0 }; static float lastError[3] = { 0, 0, 0 }, lastDTerm[3] = { 0, 0, 0 }; // pt1 element http://www.multiwii.com/forum/viewtopic.php?f=23&t=2624; static int16_t axisPID[3]; static float rollPitchRate = 0.0; static float newpidimax = 0.0; float dT; uint8_t ANGLE_MODE = 0; uint8_t HORIZON_MODE = 0; uint16_t cycleTime = IMU_LASTUPD - controlData.lastUpdate; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop if ((ANGLE_MODE || HORIZON_MODE)) { // MODE relying on ACC errorAngle = constrainFloat(2.0f * (float)controlData.roll, -500.0f, +500.0f) - AQ_ROLL; } if (!ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID AngleRateTmp = (float)(rollPitchRate + 27) * (float)controlData.roll * 0.0625f; // AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4; if (HORIZON_MODE) { AngleRateTmp += PID_I * errorAngle * 0.0390625f; //increased by x10 //0.00390625f AngleRateTmp += (errorAngle * (float)cfg.I8[PIDLEVEL]) >> 8; } } else { // it's the ANGLE mode - control is angle based, so control loop is needed AngleRateTmp = PID_P * errorAngle * 0.0223214286f; // AngleRateTmp = (errorAngle * (float)cfg.P8[PIDLEVEL]) >> 4; * LevelPprescale; } RateError = AngleRateTmp - IMU_DRATEX; PTerm = PID_P * RateError * 0.0078125f; errorGyroI[axis] += PID_I * RateError * (float)cycleTime / 2048.0f; errorGyroI[axis] = constrainFloat(errorGyroI[axis], -newpidimax, newpidimax); ITerm = errorGyroI[axis] / 8192.0f; delta = RateError - lastError[axis]; lastError[axis] = RateError; delta = delta * 16383.75f / (float)cycleTime; deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; DTerm = PID_D * deltaSum * 0.00390625f; axisPID[axis] = PTerm + ITerm + DTerm; rollCommand = AngleRateTmp; } else { pitchCommand = 0.0f; rollCommand = 0.0f; ruddCommand = 0.0f; } // yaw rate override? if (overrides[2] > 0) // manual yaw rate { ruddCommand = pidUpdate(controlData.yawRatePID, ratesDesired[2], IMU_DRATEZ); } else // seek a 0 deg difference between hold heading and actual yaw { ruddCommand = pidUpdate(controlData.yawRatePID, pidUpdate(controlData.yawAnglePID, 0.0f, compassDifference(controlData.yaw, AQ_YAW)), IMU_DRATEZ); } rollCommand = constrainFloat(rollCommand, -p[CTRL_MAX], p[CTRL_MAX]); pitchCommand = constrainFloat(pitchCommand, -p[CTRL_MAX], p[CTRL_MAX]); ruddCommand = constrainFloat(ruddCommand, -p[CTRL_MAX], p[CTRL_MAX]); motorsCommands(throttle, pitchCommand, rollCommand, ruddCommand); #endif } // no throttle input else { supervisorThrottleUp(0); motorsOff(); } } // not armed else { motorsOff(); } controlData.lastUpdate = IMU_LASTUPD; controlData.loops++; } }