in AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp [432:671]
void model(double* y, double last_time_, double* y_out)
{
unused(last_time_);
/********** Input the State Vector***********/
x[0][0] = x_in;
x[1][0] = xdot_in;
x[2][0] = y_in;
x[3][0] = ydot_in;
x[4][0] = z_in;
x[5][0] = zdot_in;
x[6][0] = phi_in;
PQR[0][0] = P_in;
x[8][0] = theta_in;
PQR[1][0] = Q_in;
x[10][0] = -psi_in;
PQR[2][0] = R_in;
if (reset) {
for (int i = 0; i < 12; i++) {
x_0[i] = x[i][0];
}
velocity_integrator[2] = 0.0f;
reset = false;
}
double velocity_real[3] = { x[1][0], x[3][0], x[5][0] };
double velocity_goal[3] = { reference[4], reference[3], reference[5] };
for (int i = 0; i < 3; i++) {
ref_sum[i] = 0;
for (int j = 0; j < 9; j++) {
ref_vec[j][i] = ref_vec[j + 1][i];
}
if (abs(velocity_goal[i]) < 10000) {
if (i == 0) {
velocity_integrator[i] += (velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.004f;
ref_vec[9][i] = -x[2 * i][0] - velocity_integrator[i];
}
if (i == 1) {
velocity_integrator[i] += (velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.0007f;
ref_vec[9][i] = x[2 * i][0] - velocity_integrator[i];
}
if (i == 2) {
velocity_integrator[i] += (-velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.0005f;
ref_vec[9][i] = (x[2 * i][0] + velocity_integrator[i]);
}
}
if (abs(velocity_goal[i]) > 10000) {
ref_vec[9][i] = reference[i];
if (i == 0) {
ref_vec[9][0] = reference[1];
}
else if (i == 1) {
ref_vec[9][1] = reference[0];
}
if (i == 2) {
ref_vec[9][i] = -ref_vec[9][i];
}
if (abs(ref_vec[9][i]) > 10000) {
ref_vec[9][i] = x[2 * i][0];
if (i == 0) {
ref_vec[9][i] = x[0][0];
}
if (i == 1) {
ref_vec[9][i] = x[2][0];
}
}
}
if (i > 0) {
ref_vec[9][i] = -ref_vec[9][i];
}
for (int j = 0; j < 10; j++) {
ref_sum[i] += ref_vec[j][i];
}
ref_sum[i] = ref_sum[i] /= 10; //ref_vec[9][i];//
}
x_des = ref_sum[0];
if (x_des > 10000) {
x_des = x[0][0];
}
y_des = ref_sum[1];
if (y_des > 10000) {
y_des = x[2][0];
}
r[0][0] = -ref_sum[2];
if (abs(r[0][0]) > 10000) {
r[0][0] = x[4][0];
}
r[3][0] = reference[8];
if (r[3][0] > 10000) {
r[3][0] = -last_yaw;
}
double lambda_theta = 0.5f;
double lambda_theta_rate = 0.5f;
double lambda_phi = 0.95f;
double lambda_phi_rate = 0.95f;
double lambda_psi = 0.0004f;
double lambda_psi_rate = 0.05f;
double lambda_z = 0.5f;
Angular_velocities_from_PQR(PQR, Angles, Angular_rates);
x[7][0] = Angular_rates[0][0];
x[9][0] = Angular_rates[1][0];
x[11][0] = Angular_rates[2][0];
// -9, -21 are adjustable gains for the z controller
zdotdot = -9 * x[5][0] - 21 * (x[4][0] - r[0][0]);
/**********Iconfig Sliding Mode***********************/
Sliding_Iconfig(zdotdot, x_des, x[0][0], x[1][0], y_des, x[2][0], x[3][0], x[10][0], refs);
ref_angles[0][0] = refs[0][0];
ref_angles[1][0] = refs[1][0];
/* Reference angles to be sent to inner loop */
r[1][0] = ref_angles[0][0]; //phi ref
if (reference[6] < 10000 && reference[6] != 0.0f) {
r[1][0] = reference[6];
}
r[2][0] = ref_angles[1][0]; //theta ref
if (reference[7] < 10000 && reference[7] != 0.0f) {
r[2][0] = reference[7];
}
/************ Iconfig Control Law *********************/
// First get integrated uncertainty parameters
delta_z = y[6];
delta_phi = y[0];
delta_theta = y[1];
delta_psi = y[2];
delta_roll = y[3];
delta_pitch = y[4];
delta_yaw = y[5];
U1 = (zdotdot + grav) * m + delta_z;
y_out[6] = lambda_z * zdotdot; // generate sliding surface in z, .015 is adjustable slope for sliding surface
// error in euler angles
S2_phi = x[6][0] - r[1][0];
S2_theta = x[8][0] - r[2][0];
S2_psi = -x[10][0] - r[3][0];
if (S2_psi > M_PI) {
S2_psi -= 2 * M_PI;
}
if (S2_psi < -M_PI) {
S2_psi += 2 * M_PI;
}
// generate delta_dot which goes to integrator variable, sliding surface for 3 euler angles, can adjust sliding surface slope as desired
y_out[0] = lambda_phi * S2_phi;
y_out[1] = lambda_theta * S2_theta;
y_out[2] = lambda_psi * S2_psi;
R_matrix[0][0] = 1;
R_matrix[1][0] = sin(x[6][0] * tan(x[8][0]));
R_matrix[2][0] = cos(x[6][0]) * tan(x[8][0]);
R_matrix[1][0] = 0;
R_matrix[1][1] = cos(x[6][0]);
R_matrix[1][2] = -1 * sin(x[6][0]);
R_matrix[2][0] = 0;
R_matrix[2][1] = sin(x[6][0]) / cos(x[8][0]);
R_matrix[2][2] = cos(x[6][0]) / cos(x[8][0]);
inverse_3x3(R_matrix, R_inverse);
rollrate_ref = R_inverse[0][0] * S2_phi * k_phi + R_inverse[0][1] * S2_theta * k_theta + R_inverse[0][2] * S2_psi * k_psi - delta_phi;
pitchrate_ref = R_inverse[1][0] * S2_phi * k_phi + R_inverse[1][1] * S2_theta * k_theta + R_inverse[1][2] * S2_psi * k_psi - delta_theta;
yawrate_ref = R_inverse[2][0] * S2_phi * k_phi + R_inverse[2][1] * S2_theta * k_theta + R_inverse[2][2] * S2_psi * k_psi - delta_psi;
if (reference[9] < 10000 && reference[9] != 0.0f) {
rollrate_ref = reference[9];
}
if (reference[10] < 10000 && reference[10] != 0.0f) {
pitchrate_ref = reference[10];
}
if (reference[11] < 10000) {
yawrate_ref = reference[11];
}
PQR_generation(x, PQR);
S3_P = PQR[0][0] - rollrate_ref;
S3_Q = PQR[1][0] - pitchrate_ref;
S3_R = PQR[2][0] - yawrate_ref;
// Sliding surface for the body frame angular rates to be integrated
y_out[3] = lambda_phi_rate * S3_P;
y_out[4] = lambda_theta_rate * S3_Q;
y_out[5] = lambda_psi_rate * S3_R;
// Calculate controls for roll, pitch, and yaw
U2 = k_roll * S3_P * Ix + (Iz - Iy) * PQR[1][0] * PQR[2][0] - delta_roll;
U3 = k_pitch * S3_Q * Iy + (Ix - Iz) * PQR[0][0] * PQR[2][0] - delta_pitch;
U4 = k_yaw * S3_R * Iz + (Iy - Ix) * PQR[0][0] * PQR[1][0] - delta_yaw;
// Rescale such that the outputs normalize from -1,1
U1 = U1 / 80; //sqrt(abs(U1)) / 6.20; // I used sqrt to try and allow for smoother signal
U2 = U2 / 80;
U3 = U3 / 80;
U4 = U4 / 80;
// Saturations: U1->.35,1 : U2,U3,U4 -> -.2,.2
if (U1 > 1) {
U1 = 1;
}
else if (U1 < 0.35) {
U1 = 0.35;
}
if (U2 > U2_sat) {
U2 = U2_sat;
}
else if (U2 < -U2_sat) {
U2 = -U2_sat;
}
if (U3 > U3_sat) {
U3 = U3_sat;
}
else if (U3 < -U3_sat) {
U3 = -U3_sat;
}
if (U4 > U4_sat) {
U4 = U4_sat;
}
else if (U4 < -U4_sat) {
U4 = -U4_sat;
}
remapU(U1, U2, U3, U4); //remap to axis4r
} // SlidingModeModel */