void stabilization_attitude_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct FloatQuat att_err; FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(att_err); /* rate error */ struct FloatRates rate_err; RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.body_rate); /* integrated error */ if (enable_integrator) { struct FloatQuat new_sum_err, scaled_att_err; /* update accumulator */ scaled_att_err.qi = att_err.qi; scaled_att_err.qx = att_err.qx / IERROR_SCALE; scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); } else { /* reset accumulator */ FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); } attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel); attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat); // FIXME: this is very dangerous! only works if this really includes all commands for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) { stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i]; } /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); }
static void ahrs_do_update_mag(void) { int i, j, k; #ifdef BAFL_DEBUG2 printf("Mag update.\n"); #endif MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag); /* P_prio = P */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < BAFL_SSIZE; j++) { bafl_Pprio[i][j] = bafl_P[i][j]; } } /* * set up measurement matrix * * h = [236.; -2.; 396.]; * hx = [ h(2); -h(1); 0]; //only psi (phi and theta = 0) * Hm = Cnb * hx; * H = [ 0 0 0 0 0 * 0 0 Cnb*hx 0 0 0 * 0 0 0 0 0 ]; * */ /*bafl_H[0][2] = RMAT_ELMT(bafl_dcm, 0, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 0, 1) * bafl_h.x; bafl_H[1][2] = RMAT_ELMT(bafl_dcm, 1, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 1, 1) * bafl_h.x; bafl_H[2][2] = RMAT_ELMT(bafl_dcm, 2, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 2, 1) * bafl_h.x;*/ bafl_H[0][2] = -RMAT_ELMT(bafl_dcm, 0, 1); bafl_H[1][2] = -RMAT_ELMT(bafl_dcm, 1, 1); bafl_H[2][2] = -RMAT_ELMT(bafl_dcm, 2, 1); //rest is zero /********************************************** * compute Kalman gain K * * K = P_prio * H_T * inv(S) * S = H * P_prio * HT + R * * K = P_prio * HT * inv(H * P_prio * HT + R) * **********************************************/ /* covariance residual S = H * P_prio * HT + R */ /* temp_S(3x6) = H(3x6) * P_prio(6x6) * * only third column of H is non-zero */ for (i = 0; i < 3; i++) { for (j = 0; j < BAFL_SSIZE; j++) { bafl_tempS[i][j] = bafl_H[i][2] * bafl_Pprio[2][j]; } } /* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3) * * only third row of HT is non-zero */ for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { bafl_S[i][j] = bafl_tempS[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */ } bafl_S[i][i] += bafl_R_mag; } /* invert S */ FLOAT_MAT33_INVERT(bafl_invS, bafl_S); /* temp_K(6x3) = P_prio(6x6) * HT(6x3) * * only third row of HT is non-zero */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < 3; j++) { /* not computing zero elements */ bafl_tempK[i][j] = bafl_Pprio[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */ } } /* K(6x3) = temp_K(6x3) * invS(3x3) */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < 3; j++) { bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j]; bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j]; bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j]; } } /********************************************** * Update filter state. * * The a priori filter state is zero since the errors are corrected after each update. * * X = X_prio + K (y - H * X_prio) * X = K * y **********************************************/ /* innovation * y = Cnb * [hx; hy; hz] - mag */ FLOAT_RMAT_VECT3_MUL(bafl_ym, bafl_dcm, bafl_h); //can be optimized FLOAT_VECT3_SUB(bafl_ym, bafl_mag); /* X(6) = K(6x3) * y(3) */ for (i = 0; i < BAFL_SSIZE; i++) { bafl_X[i] = bafl_K[i][0] * bafl_ym.x; bafl_X[i] += bafl_K[i][1] * bafl_ym.y; bafl_X[i] += bafl_K[i][2] * bafl_ym.z; } /********************************************** * Update the filter covariance. * * P = P_prio - K * H * P_prio * P = ( I - K * H ) * P_prio * **********************************************/ /* temp(6x6) = I(6x6) - K(6x3)*H(3x6) * * last 3 columns of H are zero */ for (i = 0; i < 6; i++) { for (j = 0; j < 6; j++) { if (i == j) { bafl_tempP[i][j] = 1.; } else { bafl_tempP[i][j] = 0.; } if (j == 2) { /* omit the parts where H is zero */ for (k = 0; k < 3; k++) { bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j]; } } } } /* P(6x6) = temp(6x6) * P_prio(6x6) */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < BAFL_SSIZE; j++) { bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j]; for (k = 1; k < BAFL_SSIZE; k++) { bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j]; } } } #ifdef LKF_PRINT_P printf("Pum="); for (i = 0; i < 6; i++) { printf("["); for (j = 0; j < 6; j++) { printf("%f\t", bafl_P[i][j]); } printf("]\n "); } printf("\n"); #endif /********************************************** * Correct errors. * * **********************************************/ /* Error quaternion. */ QUAT_ASSIGN(bafl_q_m_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2); FLOAT_QUAT_INVERT(bafl_q_m_err, bafl_q_m_err); /* normalize */ float q_sq; q_sq = bafl_q_m_err.qx * bafl_q_m_err.qx + bafl_q_m_err.qy * bafl_q_m_err.qy + bafl_q_m_err.qz * bafl_q_m_err.qz; if (q_sq > 1) { /* this should actually never happen */ FLOAT_QUAT_SMUL(bafl_q_m_err, bafl_q_m_err, 1 / sqrtf(1 + q_sq)); printf("mag error quaternion q_sq > 1!!\n"); } else { bafl_q_m_err.qi = sqrtf(1 - q_sq); } /* correct attitude */ FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_m_err, bafl_quat); FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp); /* correct gyro bias */ RATES_ASSIGN(bafl_b_m_err, bafl_X[3], bafl_X[4], bafl_X[5]); RATES_SUB(bafl_bias, bafl_b_m_err); /* * compute all representations */ /* maintain rotation matrix representation */ FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat); /* maintain euler representation */ FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm); AHRS_TO_BFP(); AHRS_LTP_TO_BODY(); }
/* * Propagate our dynamic system in time * * Run the strapdown computation and the predict step of the filter * * * quat_dot = Wxq(pqr) * quat * bias_dot = 0 * * Wxq is the quaternion omega matrix: * * [ 0, -p, -q, -r ] * 1/2 * [ p, 0, r, -q ] * [ q, -r, 0, p ] * [ r, q, -p, 0 ] * */ void ahrs_propagate(void) { int i, j, k; ahrs_lowpass_accel(); /* compute unbiased rates */ RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro); RATES_SUB(bafl_rates, bafl_bias); /* run strapdown computation * */ /* multiplicative quaternion update */ /* compute correction quaternion */ QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT / 2, bafl_rates.q * BAFL_DT / 2, bafl_rates.r * BAFL_DT / 2); /* normalize it. maybe not necessary?? */ float q_sq; q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz * bafl_qr.qz) / 4; if (q_sq > 1) { /* this should actually never happen */ FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq)); } else { bafl_qr.qi = sqrtf(1 - q_sq); } /* propagate correction quaternion */ FLOAT_QUAT_COMP(bafl_qtemp, bafl_quat, bafl_qr); FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp); bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat); //TODO check if broot force normalization is good, use lagrange normalization? FLOAT_QUAT_NORMALISE(bafl_quat); /* * compute all representations */ /* maintain rotation matrix representation */ FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat); /* maintain euler representation */ FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm); AHRS_TO_BFP(); AHRS_LTP_TO_BODY(); /* error KF-Filter * propagate only the error covariance matrix since error is corrected after each measurement * * F = [ 0 0 0 ] * [ 0 0 0 -Cbn ] * [ 0 0 0 ] * [ 0 0 0 0 0 0 ] * [ 0 0 0 0 0 0 ] * [ 0 0 0 0 0 0 ] * * T = e^(dt * F) * * T = [ 1 0 0 ] * [ 0 1 0 -Cbn*dt ] * [ 0 0 1 ] * [ 0 0 0 1 0 0 ] * [ 0 0 0 0 1 0 ] * [ 0 0 0 0 0 1 ] * * P_prio = T * P * T_T + Q * */ /* * compute state transition matrix T * * diagonal elements of T are always one */ for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */ } } /* * estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T + Q */ /* Temporary multiplication temp(6x6) = T(6x6) * P(6x6) */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < BAFL_SSIZE; j++) { bafl_tempP[i][j] = 0.; for (k = 0; k < BAFL_SSIZE; k++) { bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j]; } } } /* P_k(6x6) = temp(6x6) * T_T(6x6) + Q */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < BAFL_SSIZE; j++) { bafl_P[i][j] = 0.; for (k = 0; k < BAFL_SSIZE; k++) { bafl_P[i][j] += bafl_tempP[i][k] * bafl_T[j][k]; //T[j][k] = T_T[k][j] } } if (i<3) { bafl_P[i][i] += bafl_Q_att; } else { bafl_P[i][i] += bafl_Q_gyro; } } #ifdef LKF_PRINT_P printf("Pp ="); for (i = 0; i < 6; i++) { printf("["); for (j = 0; j < 6; j++) { printf("%f\t", bafl_P[i][j]); } printf("]\n "); } printf("\n"); #endif }
static void ahrs_do_update_accel(void) { int i, j, k; #ifdef BAFL_DEBUG2 printf("Accel update.\n"); #endif /* P_prio = P */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < BAFL_SSIZE; j++) { bafl_Pprio[i][j] = bafl_P[i][j]; } } /* * set up measurement matrix * * g = 9.81 * gx = [ 0 -g 0 * g 0 0 * 0 0 0 ] * H = [ 0 0 0 ] * [ -Cnb*gx 0 0 0 ] * [ 0 0 0 ] * * */ bafl_H[0][0] = -RMAT_ELMT(bafl_dcm, 0, 1) * BAFL_g; bafl_H[0][1] = RMAT_ELMT(bafl_dcm, 0, 0) * BAFL_g; bafl_H[1][0] = -RMAT_ELMT(bafl_dcm, 1, 1) * BAFL_g; bafl_H[1][1] = RMAT_ELMT(bafl_dcm, 1, 0) * BAFL_g; bafl_H[2][0] = -RMAT_ELMT(bafl_dcm, 2, 1) * BAFL_g; bafl_H[2][1] = RMAT_ELMT(bafl_dcm, 2, 0) * BAFL_g; /* rest is zero bafl_H[0][2] = 0.; bafl_H[1][2] = 0.; bafl_H[2][2] = 0.;*/ /********************************************** * compute Kalman gain K * * K = P_prio * H_T * inv(S) * S = H * P_prio * HT + R * * K = P_prio * HT * inv(H * P_prio * HT + R) * **********************************************/ /* covariance residual S = H * P_prio * HT + R */ /* temp_S(3x6) = H(3x6) * P_prio(6x6) * last 4 columns of H are zero */ for (i = 0; i < 3; i++) { for (j = 0; j < BAFL_SSIZE; j++) { bafl_tempS[i][j] = bafl_H[i][0] * bafl_Pprio[0][j]; bafl_tempS[i][j] += bafl_H[i][1] * bafl_Pprio[1][j]; } } /* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3) * * bottom 4 rows of HT are zero */ for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { bafl_S[i][j] = bafl_tempS[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */ bafl_S[i][j] += bafl_tempS[i][1] * bafl_H[j][1]; /* H[j][0] = HT[0][j] */ } bafl_S[i][i] += bafl_R_accel; } /* invert S */ FLOAT_MAT33_INVERT(bafl_invS, bafl_S); /* temp_K(6x3) = P_prio(6x6) * HT(6x3) * * bottom 4 rows of HT are zero */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < 3; j++) { /* not computing zero elements */ bafl_tempK[i][j] = bafl_Pprio[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */ bafl_tempK[i][j] += bafl_Pprio[i][1] * bafl_H[j][1]; /* H[j][1] = HT[1][j] */ } } /* K(6x3) = temp_K(6x3) * invS(3x3) */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < 3; j++) { bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j]; bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j]; bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j]; } } /********************************************** * Update filter state. * * The a priori filter state is zero since the errors are corrected after each update. * * X = X_prio + K (y - H * X_prio) * X = K * y **********************************************/ /*printf("R = "); for (i = 0; i < 3; i++) { printf("["); for (j = 0; j < 3; j++) { printf("%f\t", RMAT_ELMT(bafl_dcm, i, j)); } printf("]\n "); } printf("\n");*/ /* innovation * y = Cnb * -[0; 0; g] - accel */ bafl_ya.x = -RMAT_ELMT(bafl_dcm, 0, 2) * BAFL_g - bafl_accel_measure.x; bafl_ya.y = -RMAT_ELMT(bafl_dcm, 1, 2) * BAFL_g - bafl_accel_measure.y; bafl_ya.z = -RMAT_ELMT(bafl_dcm, 2, 2) * BAFL_g - bafl_accel_measure.z; /* X(6) = K(6x3) * y(3) */ for (i = 0; i < BAFL_SSIZE; i++) { bafl_X[i] = bafl_K[i][0] * bafl_ya.x; bafl_X[i] += bafl_K[i][1] * bafl_ya.y; bafl_X[i] += bafl_K[i][2] * bafl_ya.z; } /********************************************** * Update the filter covariance. * * P = P_prio - K * H * P_prio * P = ( I - K * H ) * P_prio * **********************************************/ /* temp(6x6) = I(6x6) - K(6x3)*H(3x6) * * last 4 columns of H are zero */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < BAFL_SSIZE; j++) { if (i == j) { bafl_tempP[i][j] = 1.; } else { bafl_tempP[i][j] = 0.; } if (j < 2) { /* omit the parts where H is zero */ for (k = 0; k < 3; k++) { bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j]; } } } } /* P(6x6) = temp(6x6) * P_prio(6x6) */ for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < BAFL_SSIZE; j++) { bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j]; for (k = 1; k < BAFL_SSIZE; k++) { bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j]; } } } #ifdef LKF_PRINT_P printf("Pua="); for (i = 0; i < 6; i++) { printf("["); for (j = 0; j < 6; j++) { printf("%f\t", bafl_P[i][j]); } printf("]\n "); } printf("\n"); #endif /********************************************** * Correct errors. * * **********************************************/ /* Error quaternion. */ QUAT_ASSIGN(bafl_q_a_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2); FLOAT_QUAT_INVERT(bafl_q_a_err, bafl_q_a_err); /* normalize * Is this needed? Or is the approximation good enough?*/ float q_sq; q_sq = bafl_q_a_err.qx * bafl_q_a_err.qx + bafl_q_a_err.qy * bafl_q_a_err.qy + bafl_q_a_err.qz * bafl_q_a_err.qz; if (q_sq > 1) { /* this should actually never happen */ FLOAT_QUAT_SMUL(bafl_q_a_err, bafl_q_a_err, 1 / sqrtf(1 + q_sq)); printf("Accel error quaternion q_sq > 1!!\n"); } else { bafl_q_a_err.qi = sqrtf(1 - q_sq); } /* correct attitude */ FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_a_err, bafl_quat); FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp); /* correct gyro bias */ RATES_ASSIGN(bafl_b_a_err, bafl_X[3], bafl_X[4], bafl_X[5]); RATES_SUB(bafl_bias, bafl_b_a_err); /* * compute all representations */ /* maintain rotation matrix representation */ FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat); /* maintain euler representation */ FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm); AHRS_TO_BFP(); AHRS_LTP_TO_BODY(); }
void stabilization_attitude_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct FloatQuat att_err; struct FloatQuat* att_quat = stateGetNedToBodyQuat_f(); FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(att_err); /* rate error */ struct FloatRates rate_err; struct FloatRates* body_rate = stateGetBodyRates_f(); RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate); /* rate_d error */ struct FloatRates body_rate_d; RATES_DIFF(body_rate_d, *body_rate, last_body_rate); RATES_COPY(last_body_rate, *body_rate); /* integrated error */ if (enable_integrator) { struct FloatQuat new_sum_err, scaled_att_err; /* update accumulator */ scaled_att_err.qi = att_err.qi; scaled_att_err.qx = att_err.qx / IERROR_SCALE; scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); } else { /* reset accumulator */ FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); } attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel); attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d, &stabilization_att_sum_err_quat); stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL]; stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH]; stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]; #ifdef HAS_SURFACE_COMMANDS stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE]; stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE]; stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE]; #endif /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); }