void ahrs_fc_realign_heading(float heading) { FLOAT_ANGLE_NORMALIZE(heading); /* quaternion representing only the heading rotation from ltp to body */ struct FloatQuat q_h_new; q_h_new.qx = 0.0; q_h_new.qy = 0.0; q_h_new.qz = sinf(heading / 2.0); q_h_new.qi = cosf(heading / 2.0); struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); struct FloatQuat ltp_to_body_quat; float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); /* quaternion representing current heading only */ struct FloatQuat q_h; QUAT_COPY(q_h, ltp_to_body_quat); q_h.qx = 0.; q_h.qy = 0.; float_quat_normalize(&q_h); /* quaternion representing rotation from current to new heading */ struct FloatQuat q_c; float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new); /* correct current heading in body frame */ struct FloatQuat q; float_quat_comp_norm_shortest(&q, &q_c, <p_to_body_quat); /* compute ltp to imu rotation quaternion and matrix */ float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); ahrs_fc.heading_aligned = TRUE; }
bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ahrs_fc.ltp_to_imu_quat, lp_accel, lp_mag); ahrs_fc.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ ahrs_float_get_quat_from_accel(&ahrs_fc.ltp_to_imu_quat, lp_accel); ahrs_fc.heading_aligned = FALSE; #endif /* Convert initial orientation from quat to rotation matrix representations. */ float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, *lp_gyro); RATES_FLOAT_OF_BFP(ahrs_fc.gyro_bias, bias0); ahrs_fc.status = AHRS_FC_RUNNING; ahrs_fc.is_aligned = TRUE; return TRUE; }
void test_of_axis_angle(void) { struct FloatVect3 axis = { 0., 1., 0.}; FLOAT_VECT3_NORMALIZE(axis); DISPLAY_FLOAT_VECT3("axis", axis); const float angle = RadOfDeg(30.); printf("angle %f\n", DegOfRad(angle)); struct FloatQuat my_q; FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q); struct FloatRMat my_r1; float_rmat_of_quat(&my_r1, &my_q); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1); DISPLAY_FLOAT_RMAT("rmat1", my_r1); struct FloatRMat my_r; FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r); DISPLAY_FLOAT_RMAT("rmat", my_r); printf("\n"); struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.}; struct FloatVect3 uz = { 0., 0., 1.}; struct FloatRMat r_yaw; FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi); struct FloatVect3 uy = { 0., 1., 0.}; struct FloatRMat r_pitch; FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta); struct FloatVect3 ux = { 1., 0., 0.}; struct FloatRMat r_roll; FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi); struct FloatRMat r_yaw_pitch; float_rmat_comp(&r_yaw_pitch, &r_yaw, &r_pitch); struct FloatRMat r_yaw_pitch_roll; float_rmat_comp(&r_yaw_pitch_roll, &r_yaw_pitch, &r_roll); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll); DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll); DISPLAY_FLOAT_EULERS_DEG("eul", eul); struct FloatRMat rmat1; float_rmat_of_eulers(&rmat1, &eul); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1); DISPLAY_FLOAT_RMAT("rmat1", rmat1); }
void orientationCalcRMat_f(struct OrientationReps* orientation) { if (bit_is_set(orientation->status, ORREP_RMAT_F)) { return; } if (bit_is_set(orientation->status, ORREP_RMAT_I)) { RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i); } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { float_rmat_of_quat(&(orientation->rmat_f), &(orientation->quat_f)); } else if (bit_is_set(orientation->status, ORREP_EULER_F)) { float_rmat_of_eulers(&(orientation->rmat_f), &(orientation->eulers_f)); } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i); SetBit(orientation->status, ORREP_QUAT_F); float_rmat_of_quat(&(orientation->rmat_f), &(orientation->quat_f)); } else if (bit_is_set(orientation->status, ORREP_EULER_I)) { EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i); SetBit(orientation->status, ORREP_EULER_F); float_rmat_of_eulers(&(orientation->rmat_f), &(orientation->eulers_f)); } /* set bit to indicate this representation is computed */ SetBit(orientation->status, ORREP_RMAT_F); }
void ahrs_fc_update_heading(float heading) { FLOAT_ANGLE_NORMALIZE(heading); struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); struct FloatQuat ltp_to_body_quat; float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); struct FloatRMat ltp_to_body_rmat; float_rmat_of_quat(<p_to_body_rmat, <p_to_body_quat); // row 0 of ltp_to_body_rmat = body x-axis in ltp frame // we only consider x and y struct FloatVect2 expected_ltp = { RMAT_ELMT(ltp_to_body_rmat, 0, 0), RMAT_ELMT(ltp_to_body_rmat, 0, 1) }; // expected_heading cross measured_heading struct FloatVect3 residual_ltp = { 0, 0, expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading) }; struct FloatVect3 residual_imu; float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp); const float heading_rate_update_gain = 2.5; RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain); float heading_bias_update_gain; /* crude attempt to only update bias if deviation is small * e.g. needed when you only have gps providing heading * and the inital heading is totally different from * the gps course information you get once you have a gps fix. * Otherwise the bias will be falsely "corrected". */ if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) { heading_bias_update_gain = -2.5e-4; } else { heading_bias_update_gain = 0.0; } RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, heading_bias_update_gain); }
void ahrs_fc_propagate(struct Int32Rates *gyro, float dt) { /* converts gyro to floating point */ struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, *gyro); /* unbias measurement */ RATES_SUB(gyro_float, ahrs_fc.gyro_bias); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES const float alpha = 0.1; FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), gyro_float, alpha); #else RATES_COPY(ahrs_fc.imu_rate, gyro_float); #endif /* add correction */ struct FloatRates omega; RATES_SUM(omega, gyro_float, ahrs_fc.rate_correction); /* and zeros it */ FLOAT_RATES_ZERO(ahrs_fc.rate_correction); #if AHRS_PROPAGATE_RMAT float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt); float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat); float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat); #endif #if AHRS_PROPAGATE_QUAT float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt); float_quat_normalize(&ahrs_fc.ltp_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); #endif // increase accel and mag propagation counters ahrs_fc.accel_cnt++; ahrs_fc.mag_cnt++; }
void ahrs_align(void) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); ahrs_impl.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ ahrs_float_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel); ahrs_impl.heading_aligned = FALSE; #endif /* Convert initial orientation from quat to rotation matrix representations. */ float_rmat_of_quat(&ahrs_impl.ltp_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); /* Compute initial body orientation */ compute_body_orientation_and_rates(); /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, ahrs_aligner.lp_gyro); RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); ahrs.status = AHRS_RUNNING; }
/** * Propagate the received states into the vehicle * state machine */ void ins_vectornav_propagate() { // Acceleration [m/s^2] // in fixed point for sending as ABI and telemetry msgs ACCELS_BFP_OF_REAL(ins_vn.accel_i, ins_vn.accel); // Rates [rad/s] static struct FloatRates body_rate; // in fixed point for sending as ABI and telemetry msgs RATES_BFP_OF_REAL(ins_vn.gyro_i, ins_vn.gyro); float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ins_vn.body_to_imu), &ins_vn.gyro); // compute body rates stateSetBodyRates_f(&body_rate); // Set state [rad/s] // Attitude [deg] ins_vectornav_yaw_pitch_roll_to_attitude(&ins_vn.attitude); // convert to correct units and axis [rad] static struct FloatQuat imu_quat; // convert from euler to quat float_quat_of_eulers(&imu_quat, &ins_vn.attitude); static struct FloatRMat imu_rmat; // convert from quat to rmat float_rmat_of_quat(&imu_rmat, &imu_quat); static struct FloatRMat ltp_to_body_rmat; // rotate to body frame float_rmat_comp(<p_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu)); stateSetNedToBodyRMat_f(<p_to_body_rmat); // set body states [rad] // NED (LTP) velocity [m/s] // North east down (NED), also known as local tangent plane (LTP), // is a geographical coordinate system for representing state vectors that is commonly used in aviation. // It consists of three numbers: one represents the position along the northern axis, // one along the eastern axis, and one represents vertical position. Down is chosen as opposed to // up in order to comply with the right-hand rule. // The origin of this coordinate system is usually chosen to be the aircraft's center of gravity. // x = North // y = East // z = Down stateSetSpeedNed_f(&ins_vn.vel_ned); // set state // NED (LTP) acceleration [m/s^2] static struct FloatVect3 accel_meas_ltp;// first we need to rotate linear acceleration from imu-frame to body-frame float_rmat_transp_vmult(&accel_meas_ltp, orientationGetRMat_f(&ins_vn.body_to_imu), &(ins_vn.lin_accel)); static struct NedCoor_f ltp_accel; // assign to NedCoord_f struct VECT3_ASSIGN(ltp_accel, accel_meas_ltp.x, accel_meas_ltp.y, accel_meas_ltp.z); stateSetAccelNed_f(<p_accel); // then set the states ins_vn.ltp_accel_f = ltp_accel; // LLA position [rad, rad, m] //static struct LlaCoor_f lla_pos; // convert from deg to rad, and from double to float ins_vn.lla_pos.lat = RadOfDeg((float)ins_vn.pos_lla[0]); // ins_impl.pos_lla[0] = lat ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.pos_lla[1]); // ins_impl.pos_lla[1] = lon ins_vn.lla_pos.alt = ((float)ins_vn.pos_lla[2]); // ins_impl.pos_lla[2] = alt LLA_BFP_OF_REAL(gps.lla_pos, ins_vn.lla_pos); stateSetPositionLla_i(&gps.lla_pos); // ECEF position struct LtpDef_f def; ltp_def_from_lla_f(&def, &ins_vn.lla_pos); struct EcefCoor_f ecef_vel; ecef_of_ned_point_f(&ecef_vel, &def, &ins_vn.vel_ned); ECEF_BFP_OF_REAL(gps.ecef_vel, ecef_vel); // ECEF velocity gps.ecef_pos.x = stateGetPositionEcef_i()->x; gps.ecef_pos.y = stateGetPositionEcef_i()->y; gps.ecef_pos.z = stateGetPositionEcef_i()->z; #if GPS_USE_LATLONG // GPS UTM /* Computes from (lat, long) in the referenced UTM zone */ struct UtmCoor_f utm_f; utm_f.zone = nav_utm_zone0; /* convert to utm */ //utm_of_lla_f(&utm_f, &lla_f); utm_of_lla_f(&utm_f, &ins_vn.lla_pos); /* copy results of utm conversion */ gps.utm_pos.east = (int32_t)(utm_f.east * 100); gps.utm_pos.north = (int32_t)(utm_f.north * 100); gps.utm_pos.alt = (int32_t)(utm_f.alt * 1000); gps.utm_pos.zone = (uint8_t)nav_utm_zone0; #endif // GPS Ground speed float speed = sqrt(ins_vn.vel_ned.x * ins_vn.vel_ned.x + ins_vn.vel_ned.y * ins_vn.vel_ned.y); gps.gspeed = ((uint16_t)(speed * 100)); // GPS course gps.course = (int32_t)(1e7 * (atan2(ins_vn.vel_ned.y, ins_vn.vel_ned.x))); // Because we have not HMSL data from Vectornav, we are using LLA-Altitude // as a workaround gps.hmsl = (uint32_t)(gps.lla_pos.alt); // set position uncertainty ins_vectornav_set_pacc(); // set velocity uncertainty ins_vectornav_set_sacc(); // check GPS status gps.last_msg_time = sys_time.nb_sec; gps.last_msg_ticks = sys_time.nb_sec_rem; if (gps.fix == GPS_FIX_3D) { gps.last_3dfix_time = sys_time.nb_sec; gps.last_3dfix_ticks = sys_time.nb_sec_rem; } // read INS status ins_vectornav_check_status(); // update internal states for telemetry purposes // TODO: directly convert vectornav output instead of using state interface // to support multiple INS running at the same time ins_vn.ltp_pos_i = *stateGetPositionNed_i(); ins_vn.ltp_speed_i = *stateGetSpeedNed_i(); ins_vn.ltp_accel_i = *stateGetAccelNed_i(); // send ABI messages uint32_t now_ts = get_sys_time_usec(); AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps); AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.gyro_i); AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.accel_i); }
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) { /* cmd_x is positive to north = negative pitch * cmd_y is positive to east = positive roll * * orientation vector describing simultaneous rotation of roll/pitch */ const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0}; /* quaternion from that orientation vector */ struct FloatQuat q_rp; float_quat_of_orientation_vect(&q_rp, &ov); /* as rotation matrix */ struct FloatRMat R_rp; float_rmat_of_quat(&R_rp, &q_rp); /* body x-axis (before heading command) is first column */ struct FloatVect3 b_x; VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]); /* body z-axis (thrust vect) is last column */ struct FloatVect3 thrust_vect; VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]); /// @todo optimize yaw angle calculation /* * Instead of using the psi setpoint angle to rotate around the body z-axis, * calculate the real angle needed to align the projection of the body x-axis * onto the horizontal plane with the psi setpoint. * * angle between two vectors a and b: * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) * where the normal n is the thrust vector (i.e. both a and b lie in that plane) */ // desired heading vect in earth x-y plane const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0}; /* projection of desired heading onto body x-y plane * b = v - dot(v,n)*n */ float dot = VECT3_DOT_PRODUCT(psi_vect, thrust_vect); struct FloatVect3 dotn; VECT3_SMUL(dotn, thrust_vect, dot); // b = v - dot(v,n)*n struct FloatVect3 b; VECT3_DIFF(b, psi_vect, dotn); dot = VECT3_DOT_PRODUCT(b_x, b); struct FloatVect3 cross; VECT3_CROSS_PRODUCT(cross, b_x, b); // norm of the cross product float nc = FLOAT_VECT3_NORM(cross); // angle = atan2(norm(cross(a,b)), dot(a,b)) float yaw2 = atan2(nc, dot) / 2.0; // negative angle if needed // sign(dot(cross(a,b), n) float dot_cross_ab = VECT3_DOT_PRODUCT(cross, thrust_vect); if (dot_cross_ab < 0) { yaw2 = -yaw2; } /* quaternion with yaw command */ struct FloatQuat q_yaw; QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ float_quat_comp(quat, &q_rp, &q_yaw); float_quat_normalize(quat); float_quat_wrap_shortest(quat); }