TEST_F(BoundSym, NonZeroRange) { float range = 10.0f; // Below Lower Bound EXPECT_EQ(-range, bound_sym(-range - 1.0f, range)); // At Lower Bound EXPECT_EQ(-range, bound_sym(-range, range)); // In Bounds EXPECT_EQ(0.0f, bound_sym(0.0f, range)); // At Upper Bound EXPECT_EQ(range, bound_sym(range, range)); // Above Upper Bound EXPECT_EQ(range, bound_sym(range + 1.0f, range)); };
/** * Update the PID computation with setpoint weighting on the derivative * @param[in] pid The PID struture which stores temporary information * @param[in] setpoint The setpoint to use * @param[in] measured The measured value of output * @param[in] dT The time step * @returns Output the computed controller value * * This version of apply uses setpoint weighting for the derivative component so the gain * on the gyro derivative can be different than the gain on the setpoint derivative */ float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT) { float err = setpoint - measured; if (pid->i == 0) { // If Ki is zero, reset the integrator pid->iAccumulator = 0; } else { // Scale up accumulator by 1000 while computing to avoid losing precision pid->iAccumulator += err * (pid->i * dT * 1000.0f); pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim * 1000.0f); } // Calculate DT1 term, float dterm = 0; float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr); pid->lastErr = (deriv_gamma * setpoint - measured); if(pid->d && dT) { dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) } // 7.9577e-3 means 20 Hz f_cutoff return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); }
void PID_apply_setpoint(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { if (Param[0]->Val->Pointer == NULL) return; struct pid *pid = Param[0]->Val->Pointer; double setpoint = Param[1]->Val->FP; double measured = Param[2]->Val->FP; double dT = Param[3]->Val->FP; double err = setpoint - measured; if (pid->i != 0) { pid->iAccumulator += err * (pid->i * dT); pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim); } double dterm = 0; double diff = setpoint - measured - pid->lastErr; pid->lastErr = setpoint - measured; if(pid->d && dT) { dterm = pid->lastDer + dT / ( dT + pid->dTau) * ((diff * pid->d / dT) - pid->lastDer); pid->lastDer = dterm; } ReturnValue->Val->FP = (err * pid->p) + pid->iAccumulator + dterm; }
/** * Set servo position * \param[in] channel The brushless output channel * \param[in] speed The desired speed (integrated by internal task) * \ */ int32_t PIOS_Brushless_SetSpeed(uint32_t channel, float speed, float dT) { if (channel >= NUM_BGC_CHANNELS) return -1; float diff; // Limit the slew rate if (accel_limit[channel]) diff = bound_sym(speed - speeds[channel], accel_limit[channel] * dT); else diff = speed - speeds[channel]; speeds[channel] += diff; return 0; }
/** * Update the PID computation * @param[in] pid The PID struture which stores temporary information * @param[in] err The error term * @param[in] dT The time step * @returns Output the computed controller value */ float pid_apply(struct pid *pid, const float err, float dT) { // Scale up accumulator by 1000 while computing to avoid losing precision pid->iAccumulator += err * (pid->i * dT * 1000.0f); pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim * 1000.0f); // Calculate DT1 term float diff = (err - pid->lastErr); float dterm = 0; pid->lastErr = err; if(pid->d && dT) { dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) } // 7.9577e-3 means 20 Hz f_cutoff return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); }
void PID_apply_antiwindup(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { if (Param[0]->Val->Pointer == NULL) return; struct pid *pid = Param[0]->Val->Pointer; double err = Param[1]->Val->FP; double min_bound = Param[2]->Val->FP; double max_bound = Param[3]->Val->FP; double dT = Param[4]->Val->FP; if (pid->i != 0) { pid->iAccumulator += err * (pid->i * dT); } double diff = (err - pid->lastErr); double dterm = 0; pid->lastErr = err; if(pid->d && dT) { dterm = pid->lastDer + dT / ( dT + pid->dTau) * ((diff * pid->d / dT) - pid->lastDer); pid->lastDer = dterm; } double ideal_output = ((err * pid->p) + pid->iAccumulator + dterm); double saturation = 0; if (ideal_output > max_bound) { saturation = max_bound - ideal_output; ideal_output = max_bound; } else if (ideal_output < min_bound) { saturation = min_bound - ideal_output; ideal_output = min_bound; } // Use Kt 10x Ki pid->iAccumulator += saturation * (pid->i * 10.0 * dT); pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim); ReturnValue->Val->FP = ideal_output; }
/** * Update the PID computation and apply anti windup limit * @param[in] pid The PID struture which stores temporary information * @param[in] err The error term * @param[in] min_bound The minimum output * @param[in] max_bound The maximum output * @param[in] dT The time step * @returns Output the computed controller value * * @Note based on "Feedback Systems" by Astrom and Murray, PID control * chapter. */ float pid_apply_antiwindup(struct pid *pid, const float err, float min_bound, float max_bound, float dT) { if (pid->i == 0) { // If Ki is zero, reset the integrator pid->iAccumulator = 0; } else { // Scale up accumulator by 1000 while computing to avoid losing precision pid->iAccumulator += err * (pid->i * dT * 1000.0f); } // Calculate DT1 term float diff = (err - pid->lastErr); float dterm = 0; pid->lastErr = err; if(pid->d && dT) { dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer); pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff) } // 7.9577e-3 means 20 Hz f_cutoff // Compute how much (if at all) the output is saturating float ideal_output = ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm); float saturation = 0; if (ideal_output > max_bound) { saturation = max_bound - ideal_output; ideal_output = max_bound; } else if (ideal_output < min_bound) { saturation = min_bound - ideal_output; ideal_output = min_bound; } // Use Kt 10x Ki pid->iAccumulator += saturation * (pid->i * 10.0f * dT * 1000.0f); pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim * 1000.0f); return ideal_output; }
/** * Module task */ static void stabilizationTask(void* parameters) { UAVObjEvent ev; uint32_t timeval = PIOS_DELAY_GetRaw(); ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; RateDesiredData rateDesired; AttitudeActualData attitudeActual; GyrosData gyrosData; FlightStatusData flightStatus; float *stabDesiredAxis = &stabDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; float horizonRateFraction = 0.0f; // Force refresh of all settings immediately before entering main task loop SettingsUpdatedCb((UAVObjEvent *) NULL); // Settings for system identification uint32_t iteration = 0; const uint32_t SYSTEM_IDENT_PERIOD = 75; uint32_t system_ident_timeval = PIOS_DELAY_GetRaw(); float dT_filtered = 0; // Main task loop zero_pids(); while(1) { iteration++; PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if (PIOS_Queue_Receive(queue, &ev, FAILSAFE_TIMEOUT_MS) != true) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); continue; } calculate_pids(); float dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; timeval = PIOS_DELAY_GetRaw(); // exponential moving averaging (EMA) of dT to reduce jitter; ~200points // to have more or less equivalent noise reduction to a normal N point moving averaging: alpha = 2 / (N + 1) // run it only at the beginning for the first samples, to reduce CPU load, and the value should converge to a constant value if (iteration < 100) { dT_filtered = dT; } else if (iteration < 2000) { dT_filtered = 0.01f * dT + (1.0f - 0.01f) * dT_filtered; } else if (iteration == 2000) { gyro_filter_updated = true; } if (gyro_filter_updated) { if (settings.GyroCutoff < 1.0f) { gyro_alpha = 0; } else { gyro_alpha = expf(-2.0f * (float)(M_PI) * settings.GyroCutoff * dT_filtered); } // Compute time constant for vbar decay term if (settings.VbarTau < 0.001f) { vbar_decay = 0; } else { vbar_decay = expf(-dT_filtered / settings.VbarTau); } gyro_filter_updated = false; } FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); GyrosGet(&gyrosData); ActuatorDesiredGet(&actuatorDesired); #if defined(RATEDESIRED_DIAGNOSTICS) RateDesiredGet(&rateDesired); #endif struct TrimmedAttitudeSetpoint { float Roll; float Pitch; float Yaw; } trimmedAttitudeSetpoint; // Mux in level trim values, and saturate the trimmed attitude setpoint. trimmedAttitudeSetpoint.Roll = bound_min_max( stabDesired.Roll + trimAngles.Roll, -settings.RollMax + trimAngles.Roll, settings.RollMax + trimAngles.Roll); trimmedAttitudeSetpoint.Pitch = bound_min_max( stabDesired.Pitch + trimAngles.Pitch, -settings.PitchMax + trimAngles.Pitch, settings.PitchMax + trimAngles.Pitch); trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw; // For horizon mode we need to compute the desire attitude from an unscaled value and apply the // trim offset. Also track the stick with the most deflection to choose rate blending. horizonRateFraction = 0.0f; if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) { trimmedAttitudeSetpoint.Roll = bound_min_max( stabDesired.Roll * settings.RollMax + trimAngles.Roll, -settings.RollMax + trimAngles.Roll, settings.RollMax + trimAngles.Roll); horizonRateFraction = fabsf(stabDesired.Roll); } if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) { trimmedAttitudeSetpoint.Pitch = bound_min_max( stabDesired.Pitch * settings.PitchMax + trimAngles.Pitch, -settings.PitchMax + trimAngles.Pitch, settings.PitchMax + trimAngles.Pitch); horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Pitch)); } if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) { trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw * settings.YawMax; horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Yaw)); } // For weak leveling mode the attitude setpoint is the trim value (drifts back towards "0") if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) { trimmedAttitudeSetpoint.Roll = trimAngles.Roll; } if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) { trimmedAttitudeSetpoint.Pitch = trimAngles.Pitch; } if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) { trimmedAttitudeSetpoint.Yaw = 0; } // Note we divide by the maximum limit here so the fraction ranges from 0 to 1 depending on // how much is requested. horizonRateFraction = bound_sym(horizonRateFraction, HORIZON_MODE_MAX_BLEND) / HORIZON_MODE_MAX_BLEND; // Calculate the errors in each axis. The local error is used in the following modes: // ATTITUDE, HORIZON, WEAKLEVELING float local_attitude_error[3]; local_attitude_error[0] = trimmedAttitudeSetpoint.Roll - attitudeActual.Roll; local_attitude_error[1] = trimmedAttitudeSetpoint.Pitch - attitudeActual.Pitch; local_attitude_error[2] = trimmedAttitudeSetpoint.Yaw - attitudeActual.Yaw; // Wrap yaw error to [-180,180] local_attitude_error[2] = circular_modulus_deg(local_attitude_error[2]); static float gyro_filtered[3]; gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha); gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha); // A flag to track which stabilization mode each axis is in static uint8_t previous_mode[MAX_AXES] = {255,255,255}; bool error = false; //Run the selected stabilization algorithm on each axis: for(uint8_t i=0; i< MAX_AXES; i++) { // Check whether this axis mode needs to be reinitialized bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); // The unscaled input (-1,1) float *raw_input = &stabDesired.Roll; previous_mode[i] = stabDesired.StabilizationMode[i]; // Apply the selected control law switch(stabDesired.StabilizationMode[i]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: if(reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; // Store to rate desired variable for storing to UAVO rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_ACROPLUS: // this implementation is based on the Openpilot/Librepilot Acro+ flightmode // and our existing rate & MWRate flightmodes if(reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; // The factor for gyro suppression / mixing raw stick input into the output; scaled by raw stick input float factor = fabsf(raw_input[i]) * settings.AcroInsanityFactor / 100; // Store to rate desired variable for storing to UAVO rateDesiredAxis[i] = bound_sym(raw_input[i] * settings.ManualRate[i], settings.ManualRate[i]); // Zero integral for aggressive maneuvers, like it is done for MWRate if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) || (i == 0 && fabsf(raw_input[i]) > 0.2f)) { pids[PID_GROUP_RATE + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].i = 0; } // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = factor * raw_input[i] + (1.0f - factor) * actuatorDesiredAxis[i]; actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: if(reinit) { pids[PID_GROUP_ATT + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].iAccumulator = 0; } // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: // Store for debugging output rateDesiredAxis[i] = stabDesiredAxis[i]; // Run a virtual flybar stabilization algorithm on this axis stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &pids[PID_GROUP_VBAR + i], &settings); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { if (reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; float weak_leveling = local_attitude_error[i] * weak_leveling_kp; weak_leveling = bound_sym(weak_leveling, weak_leveling_max); // Compute desired rate as input biased towards leveling rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling; actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: if (reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) { // While getting strong commands act like rate mode rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]); // Reset accumulator axis_lock_accum[i] = 0; } else { // For weaker commands or no command simply lock (almost) on no gyro change axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT; axis_lock_accum[i] = bound_sym(axis_lock_accum[i], max_axis_lock); // Compute the inner loop float tmpRateDesired = pid_apply(&pids[PID_GROUP_ATT + i], axis_lock_accum[i], dT); rateDesiredAxis[i] = bound_sym(tmpRateDesired, settings.MaximumRate[i]); } actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON: if(reinit) { pids[PID_GROUP_RATE + i].iAccumulator = 0; } // Do not allow outer loop integral to wind up in this mode since the controller // is often disengaged. pids[PID_GROUP_ATT + i].iAccumulator = 0; // Compute the outer loop for the attitude control float rateDesiredAttitude = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT); // Compute the desire rate for a rate control float rateDesiredRate = raw_input[i] * settings.ManualRate[i]; // Blend from one rate to another. The maximum of all stick positions is used for the // amount so that when one axis goes completely to rate the other one does too. This // prevents doing flips while one axis tries to stay in attitude mode. rateDesiredAxis[i] = rateDesiredAttitude * (1.0f-horizonRateFraction) + rateDesiredRate * horizonRateFraction; rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_MWRATE: { if(reinit) { pids[PID_GROUP_MWR + i].iAccumulator = 0; } /* Conversion from MultiWii PID settings to our units. Kp = Kp_mw * 4 / 80 / 500 Kd = Kd_mw * looptime * 1e-6 * 4 * 3 / 32 / 500 Ki = Ki_mw * 4 / 125 / 64 / (looptime * 1e-6) / 500 These values will just be approximate and should help you get started. */ // The unscaled input (-1,1) - note in MW this is from (-500,500) float *raw_input = &stabDesired.Roll; // dynamic PIDs are scaled both by throttle and stick position float scale = (i == 0 || i == 1) ? mwrate_settings.RollPitchRate : mwrate_settings.YawRate; float pid_scale = (100.0f - scale * fabsf(raw_input[i])) / 100.0f; float dynP8 = pids[PID_GROUP_MWR + i].p * pid_scale; float dynD8 = pids[PID_GROUP_MWR + i].d * pid_scale; // these terms are used by the integral loop this proportional term is scaled by throttle (this is different than MW // that does not apply scale float cfgP8 = pids[PID_GROUP_MWR + i].p; float cfgI8 = pids[PID_GROUP_MWR + i].i; // Dynamically adjust PID settings struct pid mw_pid; mw_pid.p = 0; // use zero Kp here because of strange setpoint. applied later. mw_pid.d = dynD8; mw_pid.i = cfgI8; mw_pid.iLim = pids[PID_GROUP_MWR + i].iLim; mw_pid.iAccumulator = pids[PID_GROUP_MWR + i].iAccumulator; mw_pid.lastErr = pids[PID_GROUP_MWR + i].lastErr; mw_pid.lastDer = pids[PID_GROUP_MWR + i].lastDer; // Zero integral for aggressive maneuvers if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) || (i == 0 && fabsf(raw_input[i]) > 0.2f)) { mw_pid.iAccumulator = 0; mw_pid.i = 0; } // Apply controller as if we want zero change, then add stick input afterwards actuatorDesiredAxis[i] = pid_apply_setpoint(&mw_pid, raw_input[i] / cfgP8, gyro_filtered[i], dT); actuatorDesiredAxis[i] += raw_input[i]; // apply input actuatorDesiredAxis[i] -= dynP8 * gyro_filtered[i]; // apply Kp term actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); // Store PID accumulators for next cycle pids[PID_GROUP_MWR + i].iAccumulator = mw_pid.iAccumulator; pids[PID_GROUP_MWR + i].lastErr = mw_pid.lastErr; pids[PID_GROUP_MWR + i].lastDer = mw_pid.lastDer; } break; case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT: if(reinit) { pids[PID_GROUP_ATT + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].iAccumulator = 0; } static uint32_t ident_iteration = 0; static float ident_offsets[3] = {0}; if (PIOS_DELAY_DiffuS(system_ident_timeval) / 1000.0f > SYSTEM_IDENT_PERIOD && SystemIdentHandle()) { ident_iteration++; system_ident_timeval = PIOS_DELAY_GetRaw(); SystemIdentData systemIdent; SystemIdentGet(&systemIdent); const float SCALE_BIAS = 7.1f; float roll_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_ROLL]); float pitch_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_PITCH]); float yaw_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_YAW]); if (roll_scale > 0.25f) roll_scale = 0.25f; if (pitch_scale > 0.25f) pitch_scale = 0.25f; if (yaw_scale > 0.25f) yaw_scale = 0.2f; switch(ident_iteration & 0x07) { case 0: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = yaw_scale; break; case 1: ident_offsets[0] = roll_scale; ident_offsets[1] = 0; ident_offsets[2] = 0; break; case 2: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = -yaw_scale; break; case 3: ident_offsets[0] = -roll_scale; ident_offsets[1] = 0; ident_offsets[2] = 0; break; case 4: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = yaw_scale; break; case 5: ident_offsets[0] = 0; ident_offsets[1] = pitch_scale; ident_offsets[2] = 0; break; case 6: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = -yaw_scale; break; case 7: ident_offsets[0] = 0; ident_offsets[1] = -pitch_scale; ident_offsets[2] = 0; break; } } if (i == ROLL || i == PITCH) { // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] += ident_offsets[i]; actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); } else { // Get the desired rate. yaw is always in rate mode in system ident. rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop only for yaw actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] += ident_offsets[i]; actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); } break; case STABILIZATIONDESIRED_STABILIZATIONMODE_COORDINATEDFLIGHT: switch (i) { case YAW: if (reinit) { pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; pids[PID_RATE_YAW].iAccumulator = 0; axis_lock_accum[YAW] = 0; } //If we are not in roll attitude mode, trigger an error if (stabDesired.StabilizationMode[ROLL] != STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { error = true; break ; } if (fabsf(stabDesired.Yaw) < COORDINATED_FLIGHT_MAX_YAW_THRESHOLD) { //If yaw is within the deadband... if (fabsf(stabDesired.Roll) > COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting more roll than the threshold float accelsDataY; AccelsyGet(&accelsDataY); //Reset integral if we have changed roll to opposite direction from rudder. This implies that we have changed desired turning direction. if ((stabDesired.Roll > 0 && actuatorDesiredAxis[YAW] < 0) || (stabDesired.Roll < 0 && actuatorDesiredAxis[YAW] > 0)){ pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; } // Coordinate flight can simply be seen as ensuring that there is no lateral acceleration in the // body frame. As such, we use the (noisy) accelerometer data as our measurement. Ideally, at // some point in the future we will estimate acceleration and then we can use the estimated value // instead of the measured value. float errorSlip = -accelsDataY; float command = pid_apply(&pids[PID_COORDINATED_FLIGHT_YAW], errorSlip, dT); actuatorDesiredAxis[YAW] = bound_sym(command ,1.0); // Reset axis-lock integrals pids[PID_RATE_YAW].iAccumulator = 0; axis_lock_accum[YAW] = 0; } else if (fabsf(stabDesired.Roll) <= COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting less roll than the threshold // Axis lock on no gyro change axis_lock_accum[YAW] += (0 - gyro_filtered[YAW]) * dT; rateDesiredAxis[YAW] = pid_apply(&pids[PID_ATT_YAW], axis_lock_accum[YAW], dT); rateDesiredAxis[YAW] = bound_sym(rateDesiredAxis[YAW], settings.MaximumRate[YAW]); actuatorDesiredAxis[YAW] = pid_apply_setpoint(&pids[PID_RATE_YAW], rateDesiredAxis[YAW], gyro_filtered[YAW], dT); actuatorDesiredAxis[YAW] = bound_sym(actuatorDesiredAxis[YAW],1.0f); // Reset coordinated-flight integral pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; } } else { //... yaw is outside the deadband. Pass the manual input directly to the actuator. actuatorDesiredAxis[YAW] = bound_sym(stabDesiredAxis[YAW], 1.0); // Reset all integrals pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; pids[PID_RATE_YAW].iAccumulator = 0; axis_lock_accum[YAW] = 0; } break; case ROLL: case PITCH: default: //Coordinated Flight has no effect in these modes. Trigger a configuration error. error = true; break; } break; case STABILIZATIONDESIRED_STABILIZATIONMODE_POI: // The sanity check enforces this is only selectable for Yaw // for a gimbal you can select pitch too. if(reinit) { pids[PID_GROUP_ATT + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].iAccumulator = 0; } float error; float angle; if (CameraDesiredHandle()) { switch(i) { case PITCH: CameraDesiredDeclinationGet(&angle); error = circular_modulus_deg(angle - attitudeActual.Pitch); break; case ROLL: { uint8_t roll_fraction = 0; #ifdef GIMBAL if (BrushlessGimbalSettingsHandle()) { BrushlessGimbalSettingsRollFractionGet(&roll_fraction); } #endif /* GIMBAL */ // For ROLL POI mode we track the FC roll angle (scaled) to // allow keeping some motion CameraDesiredRollGet(&angle); angle *= roll_fraction / 100.0f; error = circular_modulus_deg(angle - attitudeActual.Roll); } break; case YAW: CameraDesiredBearingGet(&angle); error = circular_modulus_deg(angle - attitudeActual.Yaw); break; default: error = true; } } else error = true; // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], error, dT); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.PoiMaximumRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: actuatorDesiredAxis[i] = bound_sym(stabDesiredAxis[i],1.0f); break; default: error = true; break; } } if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); #if defined(RATEDESIRED_DIAGNOSTICS) RateDesiredSet(&rateDesired); #endif // Save dT actuatorDesired.UpdateTime = dT * 1000; actuatorDesired.Throttle = stabDesired.Throttle; if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_MANUAL) { ActuatorDesiredSet(&actuatorDesired); } else { // Force all axes to reinitialize when engaged for(uint8_t i=0; i< MAX_AXES; i++) previous_mode[i] = 255; } if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) { // Force all axes to reinitialize when engaged for(uint8_t i=0; i< MAX_AXES; i++) previous_mode[i] = 255; } // Clear or set alarms. Done like this to prevent toggling each cycle // and hammering system alarms if (error) AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR); else AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } }
/** * Compute desired attitude from the desired velocity * @param[in] dT the time since last evaluation * @param[in] att_adj an adjustment to the attitude for loiter mode * * Takes in @ref NedActual which has the acceleration in the * NED frame as the feedback term and then compares the * @ref VelocityActual against the @ref VelocityDesired */ int32_t vtol_follower_control_attitude(float dT, const float *att_adj) { vtol_follower_control_accel(dT); float default_adj[2] = {0,0}; if (!att_adj) { att_adj = default_adj; } AccelDesiredData accelDesired; AccelDesiredGet(&accelDesired); StabilizationSettingsData stabSet; StabilizationSettingsGet(&stabSet); float northCommand = accelDesired.North; float eastCommand = accelDesired.East; // Project the north and east acceleration signals into body frame float yaw; AttitudeActualYawGet(&yaw); float forward_accel_desired = -northCommand * cosf(yaw * DEG2RAD) + -eastCommand * sinf(yaw * DEG2RAD); float right_accel_desired = -northCommand * sinf(yaw * DEG2RAD) + eastCommand * cosf(yaw * DEG2RAD); StabilizationDesiredData stabDesired; // Set the angle that would achieve the desired acceleration given the thrust is enough for a hover stabDesired.Pitch = bound_sym(RAD2DEG * atanf(forward_accel_desired / GRAVITY), guidanceSettings.MaxRollPitch) + att_adj[1]; stabDesired.Roll = bound_sym(RAD2DEG * atanf(right_accel_desired / GRAVITY), guidanceSettings.MaxRollPitch) + att_adj[0]; // Re-bound based on maximum attitude settings stabDesired.Pitch = bound_sym(stabDesired.Pitch, stabSet.PitchMax); stabDesired.Roll = bound_sym(stabDesired.Roll, stabSet.RollMax); stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; // Calculate the throttle setting or use pass through from transmitter if (guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { ManualControlCommandThrottleGet(&stabDesired.Throttle); } else { float downCommand = vtol_follower_control_altitude(accelDesired.Down); stabDesired.Throttle = bound_min_max(downCommand, 0, 1); } // Various ways to control the yaw that are essentially manual passthrough. However, because we do not have a fine // grained mechanism of manual setting the yaw as it normally would we need to duplicate that code here switch(guidanceSettings.YawMode) { case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE: /* This is awkward. This allows the transmitter to control the yaw while flying navigation */ ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSet.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK: ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSet.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE: { ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSet.YawMax * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_PATH: { // Face forward on the path VelocityDesiredData velocityDesired; VelocityDesiredGet(&velocityDesired); float total_vel2 = velocityDesired.East*velocityDesired.East + velocityDesired.North*velocityDesired.North; float path_direction = atan2f(velocityDesired.East, velocityDesired.North) * RAD2DEG; if (total_vel2 > 1) { stabDesired.Yaw = path_direction; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } else { stabDesired.Yaw = 0; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } } break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI: stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI; break; } StabilizationDesiredSet(&stabDesired); return 0; }