From d00c7fff655d404e492df8c843e2ddf1a87f0800 Mon Sep 17 00:00:00 2001 From: andrealaffly <36086921+andrealaffly@users.noreply.github.com> Date: Thu, 29 Mar 2018 10:44:00 -0500 Subject: [PATCH] Add files via upload Adaptive sliding mode control law for quadrotors. This control law guarantees satisfactory trajectory tracking also in case one of the propellers experiences 50% efficiency loss. --- .../firmware/AdaptiveController.hpp | 1371 +++++++++-------- 1 file changed, 713 insertions(+), 658 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp index 1f3b8baa01..155b7b57f2 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp @@ -18,664 +18,719 @@ namespace simple_flight { -class AdaptiveController : public IController { -public: - - virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) override - { - //TODO: goal is never used! - goal_ = goal; - state_estimator_ = state_estimator; - } - - virtual const Axis4r& getOutput() override - { - // Create vectors to store state variables - Axis3r angle = 0; - Axis3r position = 0; - Axis3r angle_rate = 0; - Axis3r velocity = 0; - - // Assign state variables to placeholder variables - angle = state_estimator_->getAngles(); - angle_rate = state_estimator_->getAngulerVelocity(); - position = state_estimator_->getPosition(); - velocity = state_estimator_->getLinearVelocity(); - - // Send state variables to the adaptive controller - update_state_vals(position[0], velocity[0], position[1], velocity[1], position[2], velocity[2], - angle[0], angle_rate[0], angle[1], angle_rate[1], angle[2], angle_rate[2]); - - run(); - - // Replace PID control variables with Adaptive control variables - output_controls_[0] = get_U1(); - output_controls_[1] = get_U2(); - output_controls_[2] = get_U3(); - output_controls_[3] = get_U4(); - - return output_controls_; - } - -private: - const IGoal* goal_; - const IStateEstimator* state_estimator_; - Axis4r output_controls_; - -private: - //inertia parameters - const float Ix = 0.02f; - const float Iy = 0.01f; - const float Iz = 0.03f; - - const float l = 0.11f; //arm length, can make more accurate by being specific about lx, ly - const float m = 0.916f; // mass in kg - const float grav = 9.81f; // gravity - const float Jr = 0.00006f; // inertia of rotor, currently an estimate; make more accurate by getting a measured value - - // Static Gain Variables - const float k_phi = -16.75f; // roll angle - const float k_theta = -26.75f; // pitch angle - const float k_psi = -13.0f; // yaw angle - const float k_roll = -450.0f; // roll rate - const float k_pitch = -450.0f; // pitch rate - const float k_yaw = -400.0f; // yaw rate - - //input saturation - const float U1_sat = 1.0f; - const float U2_sat = .95f; - const float U3_sat = .95f; - const float U4_sat = .95f; - - //trajectory parameters - const float pi = 3.14159265359f; - const float period = 45.0f; - const float radius = 2.5f; // input radius of the circle - const float alt = 5.0f; // height used for circle/square - - // other constants - const float SIZE_BUF_FLOAT = 30.0f; - const float NEQN = 7.0f; - -private: - void update_state_vals(float x_val, float vx, float y_val, float vy, float z_val, float vz, float roll, float roll_rate, float pitch, float pitch_rate, float yaw, float yaw_rate) - { - x_in = x_val; - xdot_in = vx; - // negative conventions in the y and z directions are a result of vicon space coordinate needs - y_in = -y_val; - ydot_in = -vy; - z_in = -z_val; - zdot_in = -vz; - phi_in = roll; - P_in = roll_rate; - theta_in = -pitch; - Q_in = -pitch_rate; - psi_in = yaw; - R_in = yaw_rate; - - - // bias modification for level imu implementing deadband - - if (std::abs(phi_in) <= 0.0001) - phi_in = 0; - - if (std::abs(theta_in) <= 0.00001) - theta_in = 0; - - if (std::abs(psi_in) <= 0.0001) - psi_in = 0; - } - - void run() - { - rungeKutta(adaptive_y, adaptive_output, getTimeU(), 0.003f, array_length); - } - - float get_U1() //pitch - { - return static_cast(U1); - } - - float get_U2() //roll - { - return static_cast(U2); - } - - float get_U3() //thrust - { - return static_cast(U3); - } - - float get_U4() //yaw - { - return static_cast(U4); - } - - /******* Rotate Body velocities to global frame *********/ - void Rotate_uvw_xyz(double u, double v, double w, double phi, double theta, double psi, double result[3][1]) - { - result[0][0] = cos(theta)*cos(psi)*u + (-cos(phi)*sin(theta) + sin(phi)*sin(theta)*cos(phi))*v + (sin(phi)*sin(psi) + cos(phi)*sin(theta)*cos(psi))*w; - result[1][0] = -cos(psi)*u + (cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi))*v + (-sin(phi)*cos(psi) + cos(phi)*sin(theta)*sin(psi))*w; - result[2][0] = -sin(theta)*u + sin(phi)*cos(theta)*v + cos(phi)*cos(theta)*w; - } - void matrix_multiply_4_4_4_1(double mat1[4][4], double mat2[4][1], double result[4][1]) - { - int i, j, k; - double sum; - for (i = 0; i<4; i++) - { - for (j = 0; j<1; j++) - { - sum = 0; - for (k = 0; k<4; k++) - { - sum += mat1[i][k] * mat2[k][j]; - } - result[i][j] = sum; - } - } - } - - - void Sliding_Iconfig(double zddot, double x_desired, double x_current, double xdot, double y_desired, double y_current, double ydot, double yaw_current, double result[2][1]) - { - double Fz, xddot, yddot; - int i; - Fz = (zddot + 9.81)*0.9116; - xddot = -2.0 * xdot - 2.0 * (x_current - x_desired); - yddot = -2.0 * ydot - 3.0 * (y_current - y_desired); - if (Fz == 0) { - Fz = 9.81*0.9116; - } - result[0][0] = (xddot*sin(yaw_current) - yddot*cos(yaw_current))*0.9116 / Fz; - result[1][0] = (xddot*cos(yaw_current) + yddot*sin(yaw_current))*0.9116 / Fz; - // Limit the angle reference values - for (i = 0; i < 2; i++) { - if (result[i][0] > 0.10) { - result[i][0] = 0.10; - } - else if (result[i][0] < -0.10) { - result[i][0] = -0.10; - } - - } - } - - void square_trajectory(double t_val, double result[4][1], double height) - { - t_val = t_val - 50; - if (t_val <= 120) - { - - if (t_val > 0) { - if (t_val < 10) { - result[0][0] = 0; - } - if ((t_val >= 10) && (t_val <= 14)) { - result[0][0] = 2 * (t_val - 10) / size_square; - } - if ((t_val > 14) && (t_val < 18)) { - result[0][0] = 2; - } - if ((t_val >= 18) && (t_val <= 26)) { - result[0][0] = 2 - 2 * (t_val - 18) / size_square; - } - if ((t_val > 26) && (t_val < 34)) { - result[0][0] = -2; - } - if ((t_val >= 34) && (t_val <= 42)) { - result[0][0] = -2 + 2 * (t_val - 34) / size_square; - } - if ((t_val > 42) && (t_val < 50)) { - result[0][0] = 2; - } - if ((t_val >= 50) && (t_val <= 58)) { - result[0][0] = 2 - 2 * (t_val - 50) / size_square; - } - if ((t_val > 58) && (t_val < 66)) { - result[0][0] = -2; - } - if ((t_val >= 66) && (t_val <= 74)) { - result[0][0] = -2 + 2 * (t_val - 66) / size_square; - } - if ((t_val > 74) && (t_val < 82)) { - result[0][0] = 2; - } - if ((t_val >= 82) && (t_val <= 90)) { - result[0][0] = 2 - 2 * (t_val - 82) / size_square; - } - if ((t_val > 90) && (t_val < 98)) { - result[0][0] = -2; - } - if ((t_val >= 98) && (t_val <= 106)) { - result[0][0] = -2 + 2 * (t_val - 98) / size_square; - } - if ((t_val > 106) && (t_val < 114)) { - result[0][0] = 2; - } - } - if (t_val < 10) { - result[1][0] = 0; - } - if ((t_val >= 10) && (t_val <= 14)) { - result[1][0] = 0; - } - if ((t_val > 14) && (t_val < 18)) { - result[1][0] = 2 * (t_val - 14) / size_square; - } - if ((t_val >= 18) && (t_val <= 26)) { - result[1][0] = 2; - } - if ((t_val > 26) && (t_val < 34)) { - result[1][0] = 2 - 2 * (t_val - 26) / size_square; - } - if ((t_val >= 34) && (t_val <= 42)) { - result[1][0] = -2; - } - if ((t_val > 42) && (t_val < 50)) { - result[1][0] = -2 + 2 * (t_val - 42) / size_square; - } - if ((t_val >= 50) && (t_val <= 58)) { - result[1][0] = 2; - } - if ((t_val > 58) && (t_val < 66)) { - result[1][0] = 2 - 2 * (t_val - 58) / size_square; - } - if ((t_val >= 66) && (t_val <= 74)) { - result[1][0] = -2; - } - if ((t_val > 74) && (t_val < 82)) { - result[1][0] = -2 + 2 * (t_val - 74) / size_square; - } - if ((t_val >= 82) && (t_val <= 90)) { - result[1][0] = 2; - } - if ((t_val > 90) && (t_val < 98)) { - result[1][0] = 2 - 2 * (t_val - 90) / size_square; - } - if ((t_val >= 98) && (t_val <= 106)) { - result[1][0] = -2; - } - if ((t_val > 106) && (t_val < 110)) { - result[1][0] = -2 + 2 * (t_val - 106) / size_square; - } - result[0][0] = result[0][0] + 6.91; // add x offset for center of vicon space - result[1][0] = result[1][0] + 4.01; // add y offset for center of vicon space - result[2][0] = height; // z_ref - result[3][0] = 0; // yaw ref - - } - else - { - result[0][0] = 6.91; - result[1][0] = 4.01; - result[2][0] = height; - result[3][0] = 0; - } - t_val = t_val + 50; - - } - - void circle_trajectory(double t_val, double circle_radius, double height, double result[4][1]) - { - if (t_val < 20) { - result[0][0] = 2.5; - result[1][0] = 0; - result[2][0] = height; - result[3][0] = 0; - } - else if (t_val > 20) { - result[0][0] = circle_radius * cos(.22*(t_val - 20)) ; - result[1][0] = circle_radius * sin(.22*(t_val - 20)) ; - result[2][0] = height; - result[3][0] = 0; - } - } - - void step_response_3D(double t_val, double result[4][1]) - { - if (t_val < 30) { - result[0][0] = 0; - result[1][0] = 0; - result[2][0] = 1; - result[3][0] = 0; - } - else if (t_val > 30) { - result[0][0] = 10.5; - result[1][0] = 0; - result[2][0] = 2.5; - result[3][0] = 0; - } - } - - void inverse_3x3(double A[3][3], double result[3][3]) - { - double det_A; // dummy variable - det_A = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) + A[0][1] * (A[1][2] * A[2][0] - A[1][0] * A[2][2])*A[2][0] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); - if (det_A == 0) { - result[0][0] = 0; - result[0][1] = 0; - result[0][2] = 0; - result[1][0] = 0; - result[1][1] = 0; - result[1][2] = 0; - result[2][0] = 0; - result[2][1] = 0; - result[2][2] = 0; - } - else { - result[0][0] = (1 / det_A)*(A[1][1] * A[2][2] - A[1][2] * A[2][1]); - result[0][1] = (1 / det_A)*(A[0][2] * A[2][1] - A[0][1] * A[2][2]); - result[0][2] = (1 / det_A)*(A[0][1] * A[1][2] - A[0][2] * A[1][1]); - result[1][0] = (1 / det_A)*(A[1][2] * A[2][0] - A[1][0] * A[2][2]); - result[1][1] = (1 / det_A)*(A[0][0] * A[2][2] - A[0][2] * A[2][0]); - result[1][2] = (1 / det_A)*(A[0][2] * A[1][0] - A[0][0] * A[1][2]); - result[2][0] = (1 / det_A)*(A[1][0] * A[2][1] - A[1][1] * A[2][0]); - result[2][1] = (1 / det_A)*(A[0][1] * A[2][0] - A[0][0] * A[2][1]); - result[2][2] = (1 / det_A)*(A[0][0] * A[1][1] - A[0][1] * A[1][0]); - } - } - - void PQR_generation(double states[12][1], double result[3][1]) - { - result[0][0] = states[7][0] - sin(states[6][0])*states[10][0]; - result[1][0] = states[9][0] * cos(states[6][0]) + states[10][0] * (cos(states[8][0])*sin(states[6][0])); - result[2][0] = states[11][0] * (cos(states[8][0])*cos(states[6][0])) - states[9][0] * sin(states[6][0]); - } - - void Angular_velocities_from_PQR(double PQR_val[3][1], double Angles_val[3][1], double result[3][1]) - { - result[0][0] = PQR_val[0][0] + PQR_val[1][0] * sin(Angles_val[0][0])*tan(Angles_val[1][0]) + PQR_val[2][0] * cos(Angles_val[0][0])*tan(Angles_val[1][0]); - result[1][0] = PQR_val[1][0] * cos(Angles_val[0][0]) - PQR_val[2][0] * sin(Angles_val[0][0]); - result[2][0] = PQR_val[1][0] * (sin(Angles_val[0][0]) / cos(Angles_val[1][0])) + PQR_val[2][0] * (cos(Angles_val[0][0]) / cos(Angles_val[1][0])); - } - - - // --------------------------------------------------------------------------- - // Local Support Functions - // --------------------------------------------------------------------------- - - uint64_t getTimeU() - { - uint64_t last_time_ = clock_ == nullptr ? 0 : clock_->millis(); - return last_time_; - } - - - void remapU(double control_u1, double control_u2, double control_u3, double control_u4) - { - // Map to px4 U outputs - U1 = control_u2; // roll - U2 = -control_u3; // pitch - U3 = control_u4; // yaw - U4 = control_u1; // throttle - U_vec[0] = static_cast(U1); - U_vec[1] = static_cast(U2); - U_vec[2] = static_cast(U3); - U_vec[3] = static_cast(U4); - } - - void rungeKutta(float* y, float* yp, uint64_t t_val, float dt, int size) - { - float zero_vec[array_length] = { 0 }; - float k1[array_length] = { 0 }; - float k2[array_length] = { 0 }; - float k3[array_length] = { 0 }; - float k4[array_length] = { 0 }; - float* y_temp; - float* y_out = zero_vec; - y_temp = y; - model(y_temp, static_cast(t_val), y_out); - for (int n = 0; n < size; n++) - { - k1[n] = dt*y_out[n]; - y_temp[n] = y[n] + k1[n] / 2; - } - model(y_temp, t_val + dt / 2, y_out); - for (int n = 0; n < size; n++) - { - k2[n] = dt*y_out[n]; - y_temp[n] = y[n] + k2[n] / 2; - } - model(y_temp, t_val + dt / 2, y_out); - for (int n = 0; n < size; n++) - { - k3[n] = dt*y_out[n]; - y_temp[n] = y[n] + k3[n]; - } - model(y_temp, t_val + dt / 2, y_out); - for (int n = 0; n < size; n++) - { - k4[n] = dt*y_out[n]; - yp[n] = y[n] + k1[n] / 6 + k2[n] / 3 + k3[n] / 4 + k4[n] / 6; - } - } - - // --------------------------------------------------------------------------- - // SlidingModeModel method - // --------------------------------------------------------------------------- - void model(float* y, float last_time_, float* y_out) - { - unused(last_time_); - - /**********desired trajectory************************/ - - - circle_trajectory(t, radius, alt, refs_temp); - //square_trajectory(t, refs_temp, alt); - //step_response_3D(t, refs_temp); - - x_des = refs_temp[0][0]; - y_des = refs_temp[1][0]; - r[0][0] = 2.5; //z/ - if (t < 10) - { - r[0][0] = 0.25 * t; - } - r[3][0] = 0; //yaw (rad) - - float lambda_theta = 1; - float lambda_theta_rate = 0.06f; - float lambda_phi = 0.5f; - float lambda_phi_rate = 0.05f; - float lambda_psi = 0.4f; - float lambda_psi_rate = 0.05f; - float lambda_z = .5f; - - - /********** 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; - Angles[0][0] = phi_in; - PQR[0][0] = P_in; - x[8][0] = theta_in; - Angles[1][0] = theta_in; - PQR[1][0] = Q_in; - x[10][0] = psi_in; - Angles[2][0] = psi_in; - PQR[2][0] = R_in; - - - 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]; - - // -4, -10 are adjustable gains for the z controller - zdotdot = -6 * x[5][0] - 15* (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 - r[2][0] = ref_angles[1][0]; //theta ref - - /************ 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] = static_cast(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]; - - //cout << x[0][0] << " " << x[1][0] << " " << x[2][0] << " " << x[3][0] << " " << x[4][0] << " " << x[5][0] << " " << x[6][0] << " " << x[7][0] << " " << x[8][0] << " " << x[9][0] << " " << x[10][0] << " " << x[11][0] << " " << "\n"; - - // generate delta_dot which goes to integrator variable, sliding surface for 3 euler angles, can adjust sliding surface slope as desired - y_out[0] = static_cast(lambda_phi*S2_phi); - y_out[1] = static_cast(lambda_theta*S2_theta); - y_out[2] = static_cast(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; - - 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] = static_cast(lambda_phi_rate*S3_P); - y_out[4] = static_cast(lambda_theta_rate*S3_Q); - y_out[5] = static_cast(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 = sqrt(std::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 - + class AdaptiveController : public IController { + public: + + virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) override + { + goal_ = goal; + state_estimator_ = state_estimator; + } + + virtual const Axis4r& getOutput() override + { + // Create vectors to store state variables + Axis3r angle = 0; + Axis3r position = 0; + Axis3r angle_rate = 0; + Axis3r velocity = 0; + + // Assign state variables to placeholder variables + angle = state_estimator_->getAngles(); + angle_rate = state_estimator_->getAngulerVelocity(); + position = state_estimator_->getPosition(); + velocity = state_estimator_->getLinearVelocity(); + + // Send state variables to the adaptive controller + update_state_vals(position[0], velocity[0], position[1], velocity[1], position[2], velocity[2], + angle[0], angle_rate[0], angle[1], angle_rate[1], angle[2], angle_rate[2]); + + update_goals(); + + run(); + + // Replace PID control variables with Adaptive control variables + output_controls_[0] = get_U1(); + output_controls_[1] = get_U2(); + output_controls_[2] = get_U3(); + output_controls_[3] = get_U4(); + + return output_controls_; + } + + private: + const IBoardClock* clock_; + const IGoal* goal_; + const IStateEstimator* state_estimator_; + Axis4r output_controls_; + + //inertia parameters + const float Ix = 0.02f; + const float Iy = 0.01f; + const float Iz = 0.03f; + + const float l = 0.11f; //arm length, can make more accurate by being specific about lx, ly + const float m = 0.916f; // mass in kg + const float grav = 9.81f; // gravity + const float Jr = 0.00006f; // inertia of rotor, currently an estimate; make more accurate by getting a measured value + + // Static Gain Variables + const float k_phi = -16.75f; // roll angle //16.75 + const float k_theta = -26.75f; // pitch angle //26.75 + const float k_psi = -1.0f; // yaw angle //13 + const float k_roll = -450.0f; // roll rate //450 + const float k_pitch = -450.0f; // pitch rate //450 + const float k_yaw = -40000.0f; // yaw rate //400 + + //input saturation + const float U1_sat = 1.0f; + const float U2_sat = .95f; + const float U3_sat = .95f; + const float U4_sat = .95f; + + //trajectory parameters + const float pi = 3.14159265359f; + const float period = 45.0f; + const float radius = 2.5f; // input radius of the circle + const float alt = 5.0f; // height used for circle/square + + // other constants + const float SIZE_BUF_FLOAT = 30.0f; + const float NEQN = 7.0f; + + + bool reset = true; + float x_0[12]; + GoalMode last_mode_; + //float error[3] = { 0 }; + float ref_vec[10][3] = { 0 }; + float ref_sum[3] = { 0 }; + float velocity_integrator[3] = { 0 }; + static constexpr int array_length = 7; + float zero[array_length] = { 0 }; + float* adaptive_y = zero; + float* adaptive_output = zero; + float last_yaw = 0.0f; + + //********************** SlidingModeModel Variables ******************************************/ + + // state values + float x_in, xdot_in, y_in, ydot_in, z_in, zdot_in, phi_in, P_in, theta_in, Q_in, psi_in, R_in; + + double x_des; + double y_des; + + + // State Vector + double x[12][1]; + double reference[12] = { 0 }; + + // References and trajectory values + double refs_temp[4][1]; //temp vector for storing x,y,z,yaw refs + double size_square = 4; // one side of the square is size_square/2 + double r[4][1]; // z, phi, theta, psi references into controller + + // update for angle states in SlidingModeModel + double PQR[3][1], Angles[3][1], Angular_rates[3][1]; + double rollrate_ref, pitchrate_ref, yawrate_ref; //pc, qc, rc + double delta_roll, delta_pitch, delta_yaw; // uncertainty parameters + double S3_P, S3_Q, S3_R; //error in body frame angular rates + + + // Iconfig Adaptive Sliding Variables + double S2_phi, S2_theta, S2_psi; // error in euler angles + double delta_z, zdotdot; // uncertainty in z and calculated desired acceleration + double delta_phi, delta_theta, delta_psi; // uncertainty in euler angles + double R_matrix[3][3], R_inverse[3][3]; + + // Iconfig Sliding Variables + double refs[2][1], ref_angles[2][1]; // reference angles output from outer loop control + + Axis4r U_vec = 0; + + double U1, U2, U3, U4; + + + void update_state_vals(float x_val, float vx, float y_val, float vy, float z_val, float vz, float roll, float roll_rate, float pitch, float pitch_rate, float yaw, float yaw_rate) + { + x_in = x_val; + xdot_in = vx; + y_in = -y_val; + ydot_in = -vy; + z_in = -z_val; + zdot_in = -vz; + phi_in = roll; + P_in = roll_rate; + theta_in = -pitch; + Q_in = -pitch_rate; + psi_in = yaw; + R_in = yaw_rate; + + + // bias modification for level imu implementing deadband + + if (abs(phi_in) <= 0.0001) + phi_in = 0; + + if (abs(theta_in) <= 0.00001) + theta_in = 0; + + if (abs(psi_in) <= 0.0001) + psi_in = 0; + } + + void update_goals() + { + const auto& mode = goal_->getGoalMode(); + const auto& value = goal_->getGoalValue(); + + for (int i = 0; i < 12; i++) + { + reference[i] = 10001.0f; + } + int count1 = 0, count2 = 0, count3 = 0, count4 = 0; + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) + { + switch (mode[axis]) + { + case GoalModeType::AngleRate: + + switch (count1) + { + case 0: + reference[11] = value[axis]; + break; + case 1: + reference[9] = value[axis]; + break; + case 2: + reference[10] = value[axis]; + break; + } + count1++; + break; + case GoalModeType::AngleLevel: + + switch (count2) + { + case 0: + if (axis == 2) + { + reference[8] = value[axis]; + } + else + { + reference[6] = value[axis]; + } + break; + case 1: + reference[7] = -value[axis]; + break; + case 2: + reference[8] = value[axis]; + break; + } + count2++; + break; + case GoalModeType::VelocityWorld: + + switch (count3) + { + case 0: + reference[3] = value[axis]; + break; + case 1: + reference[4] = value[axis]; + break; + case 2: + reference[5] = value[axis]; + break; + } + count3++; + break; + case GoalModeType::PositionWorld: + + switch (count4) + { + case 0: + if (axis != 0) + { + reference[2] = value[axis]; + } + else + { + reference[0] = value[axis]; + } + break; + case 1: + reference[1] = value[axis]; + break; + case 2: + reference[2] = value[axis]; + break; + } + count4++; + break; + default: + + break; + + + } + if (mode[axis] != last_mode_[axis]) + { + reset = true; + } + last_mode_[axis] = mode[axis]; + } + if (reference[8] < 10000) + { + last_yaw = reference[8]; + } + } + + void run() + { + rungeKutta(adaptive_y, adaptive_output, getTimeU(), 0.003f, array_length); + } + + float get_U1() //pitch + { + return static_cast(U1); + } + + float get_U2() //roll + { + return static_cast(U2); + } + + float get_U3() //thrust + { + return static_cast(U3); + } + + float get_U4() //yaw + { + return static_cast(U4); + } + + void Sliding_Iconfig(double zddot, double x_desired, double x_current, double xdot, double y_desired, double y_current, double ydot, double yaw_current, double result[2][1]) + { + double Fz, xddot, yddot; + int i; + Fz = (zddot + 9.81)*0.9116; + xddot = -2.0 * xdot - 2.0 * (x_current - x_desired); + yddot = -2.0 * ydot - 3.0 * (y_current - y_desired); + if (Fz == 0) { + Fz = 9.81*0.9116; + } + result[0][0] = (xddot*sin(yaw_current) - yddot*cos(yaw_current))*0.9116 / Fz; + result[1][0] = (xddot*cos(yaw_current) + yddot*sin(yaw_current))*0.9116 / Fz; + // Limit the angle reference values + for (i = 0; i < 2; i++) { + if (result[i][0] > 0.3) { + result[i][0] = 0.3; + } + else if (result[i][0] < -0.3) { + result[i][0] = -0.3; + } + + } + } + + void inverse_3x3(double A[3][3], double result[3][3]) + { + double det_A; // dummy variable + det_A = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) + A[0][1] * (A[1][2] * A[2][0] - A[1][0] * A[2][2])*A[2][0] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); + if (det_A == 0) { + result[0][0] = 0; + result[0][1] = 0; + result[0][2] = 0; + result[1][0] = 0; + result[1][1] = 0; + result[1][2] = 0; + result[2][0] = 0; + result[2][1] = 0; + result[2][2] = 0; + } + else { + result[0][0] = (1 / det_A)*(A[1][1] * A[2][2] - A[1][2] * A[2][1]); + result[0][1] = (1 / det_A)*(A[0][2] * A[2][1] - A[0][1] * A[2][2]); + result[0][2] = (1 / det_A)*(A[0][1] * A[1][2] - A[0][2] * A[1][1]); + result[1][0] = (1 / det_A)*(A[1][2] * A[2][0] - A[1][0] * A[2][2]); + result[1][1] = (1 / det_A)*(A[0][0] * A[2][2] - A[0][2] * A[2][0]); + result[1][2] = (1 / det_A)*(A[0][2] * A[1][0] - A[0][0] * A[1][2]); + result[2][0] = (1 / det_A)*(A[1][0] * A[2][1] - A[1][1] * A[2][0]); + result[2][1] = (1 / det_A)*(A[0][1] * A[2][0] - A[0][0] * A[2][1]); + result[2][2] = (1 / det_A)*(A[0][0] * A[1][1] - A[0][1] * A[1][0]); + } + } + + void PQR_generation(double states[12][1], double result[3][1]) + { + result[0][0] = states[7][0] - sin(states[6][0])*states[10][0]; + result[1][0] = states[9][0] * cos(states[6][0]) + states[10][0] * (cos(states[8][0])*sin(states[6][0])); + result[2][0] = states[11][0] * (cos(states[8][0])*cos(states[6][0])) - states[9][0] * sin(states[6][0]); + } + + void Angular_velocities_from_PQR(double PQR_val[3][1], double Angles_val[3][1], double result[3][1]) + { + result[0][0] = PQR_val[0][0] + PQR_val[1][0] * sin(Angles_val[0][0])*tan(Angles_val[1][0]) + PQR_val[2][0] * cos(Angles_val[0][0])*tan(Angles_val[1][0]); + result[1][0] = PQR_val[1][0] * cos(Angles_val[0][0]) - PQR_val[2][0] * sin(Angles_val[0][0]); + result[2][0] = PQR_val[1][0] * (sin(Angles_val[0][0]) / cos(Angles_val[1][0])) + PQR_val[2][0] * (cos(Angles_val[0][0]) / cos(Angles_val[1][0])); + } + + + // --------------------------------------------------------------------------- + // Local Support Functions + // --------------------------------------------------------------------------- + + uint64_t getTimeU() + { + uint64_t last_time_ = clock_ == nullptr ? 0 : clock_->millis(); + return last_time_; + } + + + void remapU(double control_u1, double control_u2, double control_u3, double control_u4) + { + // Map to px4 U outputs + U1 = control_u2; // roll + U2 = -control_u3; // pitch + U3 = control_u4; // yaw + U4 = control_u1; // throttle + U_vec[0] = static_cast(U1); + U_vec[1] = static_cast(U2); + U_vec[2] = static_cast(U3); + U_vec[3] = static_cast(U4); + } + + void rungeKutta(float* y, float* yp, uint64_t t_val, float dt, int size) + { + float zero_vec[array_length] = { 0 }; + float k1[array_length] = { 0 }; + float k2[array_length] = { 0 }; + float k3[array_length] = { 0 }; + float k4[array_length] = { 0 }; + float* y_temp; + float* y_out = zero_vec; + y = yp; + y_temp = y; + model(y_temp, static_cast(t_val), y_out); + for (int n = 0; n < size; n++) + { + k1[n] = dt*y_out[n]; + y_temp[n] = y[n] + k1[n] / 2; + } + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) + { + k2[n] = dt*y_out[n]; + y_temp[n] = y[n] + k2[n] / 2; + } + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) + { + k3[n] = dt*y_out[n]; + y_temp[n] = y[n] + k3[n]; + } + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) + { + k4[n] = dt*y_out[n]; + yp[n] = y[n] + k1[n] / 6 + k2[n] / 3 + k3[n] / 4 + k4[n] / 6; + } + } + + // --------------------------------------------------------------------------- + // SlidingModeModel method + // --------------------------------------------------------------------------- + void model(float* y, float last_time_, float* 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; + } + + float velocity_real[3] = { x[1][0],x[3][0],x[5][0] }; + float 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; + } + + + float lambda_theta = 0.5f; + float lambda_theta_rate = 0.5f; + float lambda_phi = 0.95f; + float lambda_phi_rate = 0.95f; + float lambda_psi = 0.0004f; + float lambda_psi_rate = 0.05f; + float 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 > PI) + { + S2_psi -= 2*PI; + } + if (S2_psi < -PI) + { + S2_psi += 2 * 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 */ + + }; - } // SlidingModeModel */ - -private: - const IBoardClock* clock_; - static constexpr int array_length = 7; - float zero[array_length] = { 0 }; - float* adaptive_y = zero; - float* adaptive_output = zero; - - //********************** SlidingModeModel Variables ******************************************/ - - // state values - float x_in, xdot_in, y_in, ydot_in, z_in, zdot_in, phi_in, P_in, theta_in, Q_in, psi_in, R_in; - - double x_des; - double y_des; - - // State Vector - double x[12][1]; - - // References and trajectory values - double refs_temp[4][1]; //temp vector for storing x,y,z,yaw refs - double size_square = 4; // one side of the square is size_square/2 - double r[4][1]; // z, phi, theta, psi references into controller - - // update for angle states in SlidingModeModel - double PQR[3][1], Angles[3][1], Angular_rates[3][1]; - double rollrate_ref, pitchrate_ref, yawrate_ref; //pc, qc, rc - double delta_roll, delta_pitch, delta_yaw; // uncertainty parameters - double S3_P, S3_Q, S3_R; //error in body frame angular rates - - - // Iconfig Adaptive Sliding Variables - double S2_phi, S2_theta, S2_psi; // error in euler angles - double delta_z, zdotdot; // uncertainty in z and calculated desired acceleration - double delta_phi, delta_theta, delta_psi; // uncertainty in euler angles - double R_matrix[3][3], R_inverse[3][3]; - - // Iconfig Sliding Variables - double refs[2][1], ref_angles[2][1]; // reference angles output from outer loop control - - Axis4r U_vec = 0; - - double U1, U2, U3, U4; - - double t = 0.0; //t variable - -}; - } -#endif +#endif \ No newline at end of file