void stabilization_attitude_ref_init(void) {
FLOAT_EULERS_ZERO(stab_att_sp_euler);
FLOAT_QUAT_ZERO( stab_att_sp_quat);
FLOAT_EULERS_ZERO(stab_att_ref_euler);
FLOAT_QUAT_ZERO( stab_att_ref_quat);
FLOAT_RATES_ZERO( stab_att_ref_rate);
FLOAT_RATES_ZERO( stab_att_ref_accel);
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
}
}
void ahrs_init(void) {
ahrs_float.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
#ifdef HAS_SURFACE_COMMANDS
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
#endif
}
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
FLOAT_RATES_ZERO( last_body_rate );
FLOAT_RATES_ZERO( body_rate_d );
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
#endif
}
void stabilization_attitude_enter(void) {
stabilization_attitude_ref_enter();
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
}
static void set_reference_direction(void){
struct NedCoor_d ref_dir_ned;
struct EcefCoor_d pos_0_ecef_pprz,
ref_dir_ecef;
EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned);
struct LtpDef_d current_ltp;
VECTOR_AS_VECT3(pos_0_ecef_pprz, pos_0_ecef);
ltp_def_from_ecef_d(¤t_ltp, &pos_0_ecef_pprz);
ecef_of_ned_vect_d(&ref_dir_ecef, ¤t_ltp, &ref_dir_ned);
// THIS SOMEWHERE ELSE!
DoubleQuat initial_orientation;
FLOAT_QUAT_ZERO(initial_orientation);
ENU_NED_transformation(current_ltp.ltp_of_ecef);
DOUBLE_QUAT_OF_RMAT(initial_orientation, current_ltp.ltp_of_ecef);
ins.avg_state.orientation = DOUBLEQUAT_AS_QUATERNIOND(initial_orientation);
// THIS SOMEWHERE ELSE! (END)
// old transformation:
//struct DoubleRMat ned2ecef;
//NED_TO_ECEF_MAT(pos_0_lla, ned2ecef.m);
//RMAT_VECT3_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned);
reference_direction = VECT3_AS_VECTOR3D(ref_dir_ecef).normalized();
//reference_direction = Vector3d(1, 0, 0);
std::cout <<"reference direction NED : " << VECT3_AS_VECTOR3D(ref_dir_ned).transpose() << std::endl;
std::cout <<"reference direction ECEF: " << reference_direction.transpose() << std::endl;
}
void stabilization_attitude_enter(void) {
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_f();
stabilization_attitude_ref_enter();
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
}
/**
* Incorporate errors to reference and zeros state
*/
static inline void reset_state(void) {
ahrs_impl.gibbs_cor.qi = 2.;
struct FloatQuat q_tmp;
FLOAT_QUAT_COMP(q_tmp, ahrs_impl.ltp_to_imu_quat, ahrs_impl.gibbs_cor);
FLOAT_QUAT_NORMALIZE(q_tmp);
memcpy(&ahrs_impl.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_impl.ltp_to_imu_quat));
FLOAT_QUAT_ZERO(ahrs_impl.gibbs_cor);
}
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);
}
/* init state and measurements */
static inline void init_invariant_state(void) {
// init state
FLOAT_QUAT_ZERO(ins_impl.state.quat);
FLOAT_RATES_ZERO(ins_impl.state.bias);
FLOAT_VECT3_ZERO(ins_impl.state.pos);
FLOAT_VECT3_ZERO(ins_impl.state.speed);
ins_impl.state.as = 1.0f;
ins_impl.state.hb = 0.0f;
// init measures
FLOAT_VECT3_ZERO(ins_impl.meas.pos_gps);
FLOAT_VECT3_ZERO(ins_impl.meas.speed_gps);
ins_impl.meas.baro_alt = 0.0f;
// init baro
ins_baro_initialized = FALSE;
}
/* the following solver uses the Power Iteration
*
* It is rather simple:
* 1. You choose a starting vektor x_0 (shouldn't be zero)
* 2. apply it on the Matrix
* x_(k+1) = A * x_k
* 3. Repeat this very often.
*
* The vector converges to the dominat eigenvector, which belongs to the eigenvalue with the biggest absolute value.
*
* But there is a problem:
* With every step, the norm of vector grows. Therefore it's necessary to scale it with every step.
*
* Important warnings:
* I. This function does not converge if the Matrix is singular
* II. Pay attention to the loop-condition! It does not end if it's close to the eigenvector!
* It ends if the steps are getting too close to each other.
*
*/
DoubleVect4 dominant_Eigenvector(struct DoubleMat44 A, unsigned int maximum_iterations, double precision,
struct DoubleMat44 sigma_A, DoubleVect4 *sigma_x)
{
unsigned int k;
DoubleVect4 x_k,
x_kp1;
double delta = 1,
scale;
FLOAT_QUAT_ZERO(x_k);
//for(k=0; (k<maximum_iterations) && (delta>precision); k++){
for (k = 0; k < maximum_iterations; k++) {
// Next step
DOUBLE_MAT_VMULT4(x_kp1, A, x_k);
// Scale the vector
scale = 1 / INFTY_NORM4(x_kp1);
QUAT_SMUL(x_kp1, x_kp1, scale);
// Calculate the difference between to steps for the loop condition. Store temporarily in x_k
QUAT_DIFF(x_k, x_k, x_kp1);
delta = INFTY_NORM4(x_k);
// Update the next step
x_k = x_kp1;
if (delta <= precision) {
DOUBLE_MAT_VMULT4(*sigma_x, sigma_A, x_k);
QUAT_SMUL(*sigma_x, *sigma_x, scale);
break;
}
}
#ifdef EKNAV_FROM_LOG_DEBUG
printf("Number of iterations: %4i\n", k);
#endif
if (k == maximum_iterations) {
printf("Orientation did not converge. Using maximum uncertainty\n");
//FLOAT_QUAT_ZERO(x_k);
QUAT_ASSIGN(*sigma_x, 0, M_PI_2, M_PI_2, M_PI_2);
}
return x_k;
}
void imu_float_init(struct ImuFloat* imuf) {
/*
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI
EULERS_ASSIGN(imuf->body_to_imu_eulers,
IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI);
FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers);
FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat);
FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers);
#else
EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.);
FLOAT_QUAT_ZERO(imuf->body_to_imu_quat);
FLOAT_RMAT_ZERO(imuf->body_to_imu_rmat);
#endif
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
}
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
}
void ahrs_init(void) {
//ahrs_float.status = AHRS_UNINIT;
// set to running for now and only use ahrs.status, not ahrs_float.status
ahrs.status = AHRS_RUNNING;
ahrs_sim_available = FALSE;
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu to same as ltp_to_body, currently no difference simulated */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_body_quat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_body_euler);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_body_rmat);
RATES_COPY(ahrs_float.imu_rate, ahrs_float.body_rate);
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
#if USE_HIGH_ACCEL_FLAG
high_accel_done = FALSE;
high_accel_flag = FALSE;
#endif
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* Set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
}
void ahrs_init(void) {
int i, j;
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_T[i][j] = 0.;
bafl_P[i][j] = 0.;
}
/* Insert the diagonal elements of the T-Matrix. These will never change. */
bafl_T[i][i] = 1.0;
/* initial covariance values */
if (i<3) {
bafl_P[i][i] = 1.0;
} else {
bafl_P[i][i] = 0.1;
}
}
FLOAT_QUAT_ZERO(bafl_quat);
ahrs.status = AHRS_UNINIT;
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
INT_RATES_ZERO(ahrs.body_rate);
INT_RATES_ZERO(ahrs.imu_rate);
ahrs_float_lkf_SetRaccel(BAFL_SIGMA_ACCEL);
ahrs_float_lkf_SetRmag(BAFL_SIGMA_MAG);
bafl_Q_att = BAFL_Q_ATT;
bafl_Q_gyro = BAFL_Q_GYRO;
FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz);
}
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);
}
