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);
}
