static void test_7(void)
{
printf("\n");
struct FloatEulers ea2c;
EULERS_ASSIGN(ea2c, RadOfDeg(29.742755), RadOfDeg(-40.966522), RadOfDeg(69.467265));
DISPLAY_FLOAT_EULERS_DEG("ea2c", ea2c);
struct FloatEulers eb2c;
EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
struct FloatRMat fa2c;
float_rmat_of_eulers(&fa2c, &ea2c);
struct FloatRMat fb2c;
float_rmat_of_eulers(&fb2c, &eb2c);
printf("\n");
test_rmat_comp_inv(fa2c, fb2c, 1);
struct FloatQuat qa2c;
float_quat_of_eulers(&qa2c, &ea2c);
struct FloatQuat qb2c;
float_quat_of_eulers(&qb2c, &eb2c);
printf("\n");
test_quat_comp_inv(qa2c, qb2c, 1);
printf("\n");
}
static void test_6(void)
{
printf("\n");
struct FloatEulers ea2b;
EULERS_ASSIGN(ea2b, RadOfDeg(45.), RadOfDeg(22.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("ea2b", ea2b);
struct FloatEulers eb2c;
EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
struct FloatRMat fa2b;
float_rmat_of_eulers(&fa2b, &ea2b);
struct FloatRMat fb2c;
float_rmat_of_eulers(&fb2c, &eb2c);
printf("\n");
test_rmat_comp(fa2b, fb2c, 1);
struct FloatQuat qa2b;
float_quat_of_eulers(&qa2b, &ea2b);
struct FloatQuat qb2c;
float_quat_of_eulers(&qb2c, &eb2c);
printf("\n");
test_quat_comp(qa2b, qb2c, 1);
printf("\n");
}
float test_quat2(void)
{
struct FloatEulers eula2b;
EULERS_ASSIGN(eula2b, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.));
// DISPLAY_FLOAT_EULERS_DEG("eula2b", eula2b);
struct FloatQuat qa2b;
float_quat_of_eulers(&qa2b, &eula2b);
DISPLAY_FLOAT_QUAT("qa2b", qa2b);
struct DoubleEulers eula2b_d;
EULERS_ASSIGN(eula2b_d, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.));
struct DoubleQuat qa2b_d;
double_quat_of_eulers(&qa2b_d, &eula2b_d);
DISPLAY_FLOAT_QUAT("qa2b_d", qa2b_d);
struct FloatVect3 u = { 1., 0., 0.};
float angle = RadOfDeg(70.);
struct FloatQuat q;
FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle);
DISPLAY_FLOAT_QUAT("q ", q);
struct FloatEulers eula2c;
EULERS_ASSIGN(eula2c, RadOfDeg(80.), RadOfDeg(0.), RadOfDeg(0.));
// DISPLAY_FLOAT_EULERS_DEG("eula2c", eula2c);
struct FloatQuat qa2c;
float_quat_of_eulers(&qa2c, &eula2c);
DISPLAY_FLOAT_QUAT("qa2c", qa2c);
struct FloatQuat qb2a;
FLOAT_QUAT_INVERT(qb2a, qa2b);
DISPLAY_FLOAT_QUAT("qb2a", qb2a);
struct FloatQuat qb2c1;
float_quat_comp(&qb2c1, &qb2a, &qa2c);
DISPLAY_FLOAT_QUAT("qb2c1", qb2c1);
struct FloatQuat qb2c2;
float_quat_inv_comp(&qb2c2, &qa2b, &qa2c);
DISPLAY_FLOAT_QUAT("qb2c2", qb2c2);
return 0.;
}
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
{
// copy euler setpoint for debugging
EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy);
float_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
}
static void test_10(void)
{
struct FloatEulers euler;
EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("euler", euler);
struct FloatQuat quat;
float_quat_of_eulers(&quat, &euler);
DISPLAY_FLOAT_QUAT("####quat", quat);
struct Int32Eulers euleri;
EULERS_BFP_OF_REAL(euleri, euler);
DISPLAY_INT32_EULERS("euleri", euleri);
struct Int32Quat quati;
int32_quat_of_eulers(&quati, &euleri);
DISPLAY_INT32_QUAT("####quat", quati);
struct Int32RMat rmati;
int32_rmat_of_eulers(&rmati, &euleri);
DISPLAY_INT32_RMAT("####rmat", rmati);
struct Int32Quat quat_ltp_to_body;
struct Int32Quat body_to_imu_quat;
int32_quat_identity(&body_to_imu_quat);
int32_quat_comp_inv(&quat_ltp_to_body, &body_to_imu_quat, &quati);
DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);
}
static void traj_coordinated_circle_update(void)
{
const float speed = 15; // m/s
const float R = 100; // radius in m
float omega = speed / R;
// tan phi = v^2/Rg
float phi = atan2(speed * speed, R * 9.81);
if (aos.time > 2.*M_PI / omega) {
VECT3_ASSIGN(aos.ltp_pos, R * cos(omega * aos.time), R * sin(omega * aos.time), 0.);
VECT3_ASSIGN(aos.ltp_vel, -omega * R * sin(omega * aos.time), omega * R * cos(omega * aos.time), 0.);
VECT3_ASSIGN(aos.ltp_accel, -omega * omega * R * cos(omega * aos.time), -omega * omega * R * sin(omega * aos.time), 0.);
// float psi = atan2(aos.ltp_pos.y, aos.ltp_pos.x);
float psi = M_PI_2 + omega * aos.time;
while (psi > M_PI) { psi -= 2.*M_PI; }
EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, psi);
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
struct FloatEulers e_dot;
EULERS_ASSIGN(e_dot, 0., 0., omega);
float_rates_of_euler_dot(&aos.imu_rates, &aos.ltp_to_imu_euler, &e_dot);
}
}
static void traj_step_phi_update(void)
{
if (aos.time > 5) {
EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0);
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
}
}
static void traj_sineX_quad_update(void)
{
const float om = RadOfDeg(10);
if (aos.time > (M_PI / om)) {
const float a = 20;
struct FloatVect3 jerk;
VECT3_ASSIGN(jerk , -a * om * om * om * cos(om * aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_accel , -a * om * om * sin(om * aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_vel , a * om * cos(om * aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_pos , a * sin(om * aos.time), 0, 0);
// this is based on differential flatness of the quad
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
const struct FloatEulers e_dot = {
0.,
9.81 * jerk.x / ((9.81 * 9.81) + (aos.ltp_accel.x * aos.ltp_accel.x)),
0.
};
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}
}
static void traj_bungee_takeoff_init(void)
{
aos.traj->te = 40.;
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0, RadOfDeg(10), 0);
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
}
static void traj_coordinated_circle_init(void)
{
aos.traj->te = 120.;
const float speed = 15; // m/s
const float R = 100; // radius in m
// tan phi = v^2/Rg
float phi = atan2(speed * speed, R * 9.81);
EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, M_PI_2);
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
}
/**
* Read received data
*/
void ahrs_vectornav_propagate(void)
{
// Rates [rad/s]
static struct FloatRates body_rate;
float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ahrs_vn.body_to_imu), &ahrs_vn.vn_data.gyro); // compute body rates
stateSetBodyRates_f(&body_rate); // Set state [rad/s]
// Attitude [deg]
static struct FloatQuat imu_quat; // convert from euler to quat
float_quat_of_eulers(&imu_quat, &ahrs_vn.vn_data.attitude);
stateSetNedToBodyQuat_f(&imu_quat);
}
static void test_2(void)
{
struct Int32Vect3 v1 = { 5000, 5000, 5000 };
DISPLAY_INT32_VECT3("v1", v1);
struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
DISPLAY_FLOAT_EULERS("euler_f", euler_f);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, euler_f);
DISPLAY_INT32_EULERS("euler_i", euler_i);
struct Int32Quat quat_i;
int32_quat_of_eulers(&quat_i, &euler_i);
DISPLAY_INT32_QUAT("quat_i", quat_i);
int32_quat_normalize(&quat_i);
DISPLAY_INT32_QUAT("quat_i_n", quat_i);
struct Int32Vect3 v2;
int32_quat_vmult(&v2, &quat_i, &v1);
DISPLAY_INT32_VECT3("v2", v2);
struct Int32RMat rmat_i;
int32_rmat_of_quat(&rmat_i, &quat_i);
DISPLAY_INT32_RMAT("rmat_i", rmat_i);
struct Int32Vect3 v3;
INT32_RMAT_VMULT(v3, rmat_i, v1);
DISPLAY_INT32_VECT3("v3", v3);
struct Int32RMat rmat_i2;
int32_rmat_of_eulers(&rmat_i2, &euler_i);
DISPLAY_INT32_RMAT("rmat_i2", rmat_i2);
struct Int32Vect3 v4;
INT32_RMAT_VMULT(v4, rmat_i2, v1);
DISPLAY_INT32_VECT3("v4", v4);
struct FloatQuat quat_f;
float_quat_of_eulers(&quat_f, &euler_f);
DISPLAY_FLOAT_QUAT("quat_f", quat_f);
struct FloatVect3 v5;
VECT3_COPY(v5, v1);
DISPLAY_FLOAT_VECT3("v5", v5);
struct FloatVect3 v6;
float_quat_vmult(&v6, &quat_f, &v5);
DISPLAY_FLOAT_VECT3("v6", v6);
}
void orientationCalcQuat_f(struct OrientationReps* orientation)
{
if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
return;
}
if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i);
} else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
float_quat_of_rmat(&(orientation->quat_f), &(orientation->rmat_f));
} else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
float_quat_of_eulers(&(orientation->quat_f), &(orientation->eulers_f));
} else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
SetBit(orientation->status, ORREP_RMAT_F);
float_quat_of_rmat(&(orientation->quat_f), &(orientation->rmat_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_quat_of_eulers(&(orientation->quat_f), &(orientation->eulers_f));
}
/* set bit to indicate this representation is computed */
SetBit(orientation->status, ORREP_QUAT_F);
}
static void test_5(void)
{
struct FloatEulers fe;
EULERS_ASSIGN(fe, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("fe", fe);
printf("\n");
struct FloatQuat fq;
float_quat_of_eulers(&fq, &fe);
test_eulers_of_quat(fq, 1);
printf("\n");
struct FloatRMat frm;
float_rmat_of_eulers(&frm, &fe);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("frm", frm);
test_eulers_of_rmat(frm, 1);
printf("\n");
}
float test_quat_of_rmat(void)
{
// struct FloatEulers eul = {-0.280849, 0.613423, -1.850440};
struct FloatEulers eul = {RadOfDeg(0.131579), RadOfDeg(-62.397659), RadOfDeg(-110.470299)};
// struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)};
struct FloatRMat rm;
float_rmat_of_eulers(&rm, &eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm);
struct FloatQuat q;
float_quat_of_rmat(&q, &rm);
DISPLAY_FLOAT_QUAT("q_of_rm ", q);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm ", q);
struct FloatQuat qref;
float_quat_of_eulers(&qref, &eul);
DISPLAY_FLOAT_QUAT("q_of_euler", qref);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref);
printf("\n\n\n");
struct FloatRMat r_att;
struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi };
float_rmat_of_eulers_312(&r_att, &e312);
DISPLAY_FLOAT_RMAT("r_att ", r_att);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att ", r_att);
struct FloatQuat q_att;
float_quat_of_rmat(&q_att, &r_att);
struct FloatEulers e_att;
float_eulers_of_rmat(&e_att, &r_att);
DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att);
float_eulers_of_quat(&e_att, &q_att);
DISPLAY_FLOAT_EULERS_DEG("of quat", e_att);
return 0.;
}
static void traj_stop_stop_x_update(void)
{
const float t0 = 5.;
const float dt_jerk = 0.75;
const float dt_nojerk = 10.;
const float val_jerk = 5.;
float_vect3_integrate_fi(&aos.ltp_pos, &aos.ltp_vel, aos.dt);
float_vect3_integrate_fi(&aos.ltp_vel, &aos.ltp_accel, aos.dt);
float_vect3_integrate_fi(&aos.ltp_accel, &aos.ltp_jerk, aos.dt);
if (aos.time < t0) { return; }
else if (aos.time < t0 + dt_jerk) {
VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.);
} else if (aos.time < t0 + 2.*dt_jerk) {
VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.);
} else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.);
} else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.);
} else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.);
} else {
VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.);
}
// this is based on differential flatness of the quad
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
const struct FloatEulers e_dot = {
0.,
9.81 * aos.ltp_jerk.x / ((9.81 * 9.81) + (aos.ltp_accel.x * aos.ltp_accel.x)),
0.
};
float_rates_of_euler_dot(&aos.imu_rates, &aos.ltp_to_imu_euler, &e_dot);
}
static void test_1(void)
{
struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
DISPLAY_FLOAT_EULERS("euler_f", euler_f);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, euler_f);
DISPLAY_INT32_EULERS("euler_i", euler_i);
struct FloatQuat quat_f;
float_quat_of_eulers(&quat_f, &euler_f);
DISPLAY_FLOAT_QUAT("quat_f", quat_f);
struct Int32Quat quat_i;
int32_quat_of_eulers(&quat_i, &euler_i);
DISPLAY_INT32_QUAT("quat_i", quat_i);
struct Int32RMat rmat_i;
int32_rmat_of_quat(&rmat_i, &quat_i);
DISPLAY_INT32_RMAT("rmat_i", rmat_i);
}
static void test_4_float(void)
{
printf("euler to quat to euler - float\n");
/* initial euler angles */
struct FloatEulers e;
EULERS_ASSIGN(e, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("e", e);
/* transform to quaternion */
struct FloatQuat q;
float_quat_of_eulers(&q, &e);
// DISPLAY_FLOAT_QUAT("q", q);
float_quat_normalize(&q);
DISPLAY_FLOAT_QUAT("q_n", q);
DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q);
/* back to eulers */
struct FloatEulers e2;
float_eulers_of_quat(&e2, &q);
DISPLAY_FLOAT_EULERS_DEG("e2", e2);
}
void stabilization_attitude_set_setpoint_rp_quat_f(bool in_flight, int32_t heading)
{
struct FloatQuat q_rp_cmd;
float_quat_of_eulers(&q_rp_cmd, &guidance_euler_cmd); //TODO this is a quaternion without yaw! add the desired yaw before you use it!
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);
/* roll/pitch commands applied to to current heading */
struct FloatQuat q_rp_sp;
float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
float_quat_normalize(&q_rp_sp);
struct FloatQuat q_sp;
if (in_flight) {
/* get current heading setpoint */
struct FloatQuat q_yaw_sp;
float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(heading));
/* rotation between current yaw and yaw setpoint */
struct FloatQuat q_yaw_diff;
float_quat_comp_inv(&q_yaw_diff, &q_yaw_sp, &q_yaw);
/* compute final setpoint with yaw */
float_quat_comp_norm_shortest(&q_sp, &q_rp_sp, &q_yaw_diff);
} else {
QUAT_COPY(q_sp, q_rp_sp);
}
QUAT_BFP_OF_REAL(stab_att_sp_quat,q_sp);
}
/**
* 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 aos_init(int traj_nb)
{
aos.traj = &traj[traj_nb];
aos.time = 0;
aos.dt = 1. / AHRS_PROPAGATE_FREQUENCY;
aos.traj->ts = 0;
aos.traj->ts = 1.; // default to one second
/* default state */
EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
FLOAT_VECT3_ZERO(aos.ltp_pos);
FLOAT_VECT3_ZERO(aos.ltp_vel);
FLOAT_VECT3_ZERO(aos.ltp_accel);
FLOAT_VECT3_ZERO(aos.ltp_jerk);
aos.traj->init_fun();
imu_init();
ahrs_init();
#ifdef PERFECT_SENSORS
RATES_ASSIGN(aos.gyro_bias, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.);
aos.heading_noise = 0.;
#else
RATES_ASSIGN(aos.gyro_bias, RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.));
RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.));
VECT3_ASSIGN(aos.accel_noise, .5, .5, .5);
aos.heading_noise = RadOfDeg(3.);
#endif
#ifdef FORCE_ALIGNEMENT
// DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat);
aos_compute_sensors();
// DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias);
// DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates);
VECT3_COPY(ahrs_aligner.lp_accel, imu.accel);
VECT3_COPY(ahrs_aligner.lp_mag, imu.mag);
RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro);
// DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro);
ahrs_align();
// DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias);
#endif
#ifdef DISABLE_ALIGNEMENT
printf("# DISABLE_ALIGNEMENT\n");
#endif
#ifdef DISABLE_PROPAGATE
printf("# DISABLE_PROPAGATE\n");
#endif
#ifdef DISABLE_ACCEL_UPDATE
printf("# DISABLE_ACCEL_UPDATE\n");
#endif
#ifdef DISABLE_MAG_UPDATE
printf("# DISABLE_MAG_UPDATE\n");
#endif
printf("# AHRS_TYPE %s\n", ahrs_type_str[AHRS_TYPE]);
printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n");
#endif
#if AHRS_MAG_UPDATE_YAW_ONLY
printf("# AHRS_MAG_UPDATE_YAW_ONLY\n");
#endif
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n");
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n");
#endif
#ifdef PERFECT_SENSORS
printf("# PERFECT_SENSORS\n");
#endif
#if AHRS_USE_GPS_HEADING
printf("# AHRS_USE_GPS_HEADING\n");
#endif
#if USE_AHRS_GPS_ACCELERATIONS
printf("# USE_AHRS_GPS_ACCELERATIONS\n");
#endif
printf("# tajectory : %s\n", aos.traj->name);
};
