void test1234(void) { struct FloatEulers eul = {RadOfDeg(33.), RadOfDeg(25.), RadOfDeg(26.)}; 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_tmp; float_rmat_comp(&r_tmp, &r_yaw, &r_roll); struct FloatRMat r_att; float_rmat_comp(&r_att, &r_tmp, &r_pitch); DISPLAY_FLOAT_RMAT("r_att_ref ", r_att); float_rmat_of_eulers_312(&r_att, &eul); DISPLAY_FLOAT_RMAT("r_att312 ", r_att); }
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); }
float test_rmat_comp(struct FloatRMat ma2b_f, struct FloatRMat mb2c_f, int display) { struct FloatRMat ma2c_f; float_rmat_comp(&ma2c_f, &ma2b_f, &mb2c_f); struct Int32RMat ma2b_i; RMAT_BFP_OF_REAL(ma2b_i, ma2b_f); struct Int32RMat mb2c_i; RMAT_BFP_OF_REAL(mb2c_i, mb2c_f); struct Int32RMat ma2c_i; int32_rmat_comp(&ma2c_i, &ma2b_i, &mb2c_i); struct FloatRMat err; RMAT_DIFF(err, ma2c_f, ma2c_i); float norm_err = FLOAT_RMAT_NORM(err); if (display) { printf("rmap comp\n"); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("a2cf", ma2c_f); DISPLAY_INT32_RMAT_AS_EULERS_DEG("a2ci", ma2c_i); } return norm_err; }
/** * 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); }