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