void stateCalcAccelNed_i(void) { if (bit_is_set(state.accel_status, ACCEL_NED_I)) return; int errno = 0; if (state.ned_initialized_i) { if (bit_is_set(state.accel_status, ACCEL_NED_F)) { ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f); } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i); } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { /* transform ecef_f -> ecef_i -> ned_i , set status bits */ ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f); SetBit(state.accel_status, ACCEL_ECEF_I); ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i); } else { /* could not get this representation, set errno */ errno = 1; } } else { /* ned coordinate system not initialized, set errno */ errno = 2; } if (errno) { //struct NedCoor_i _ned_zero = {0}; //return _ned_zero; } /* set bit to indicate this representation is computed */ SetBit(state.accel_status, ACCEL_NED_I); }
static void feed_imu(int i) { if (i>0) { RATES_COPY(imu.gyro_prev, imu.gyro); } else { RATES_BFP_OF_REAL(imu.gyro_prev, samples[0].gyro); } RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro); ACCELS_BFP_OF_REAL(imu.accel, samples[i].accel); MAGS_BFP_OF_REAL(imu.mag, samples[i].mag); }
void aos_compute_sensors(void) { struct FloatRates gyro; RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias); // printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r)); float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise); RATES_BFP_OF_REAL(imu.gyro, gyro); RATES_BFP_OF_REAL(imu.gyro_prev, gyro); struct FloatVect3 g_ltp = {0., 0., 9.81}; struct FloatVect3 accelero_ltp; VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp); struct FloatVect3 accelero_imu; float_quat_vmult(&accelero_imu, &aos.ltp_to_imu_quat, &accelero_ltp); float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise); ACCELS_BFP_OF_REAL(imu.accel, accelero_imu); #ifndef DISABLE_MAG_UPDATE struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; struct FloatVect3 h_imu; float_quat_vmult(&h_imu, &aos.ltp_to_imu_quat, &h_earth); MAGS_BFP_OF_REAL(imu.mag, h_imu); #endif aos.heading_meas = aos.ltp_to_imu_euler.psi + get_gaussian_noise() * aos.heading_noise; #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN #if AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel); ahrs_impl.ltp_vel_norm_valid = true; #endif #if AHRS_TYPE == AHRS_TYPE_FCR2 ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel); ahrs_impl.ltp_vel_norm_valid = true; #endif #if AHRS_TYPE == AHRS_TYPE_FCR ahrs_impl.gps_speed = float_vect3_norm(&aos.ltp_vel); ahrs_impl.gps_age = 0; ahrs_update_gps(); //RunOnceEvery(100,printf("# gps accel: %f\n", ahrs_impl.gps_acceleration)); #endif #if AHRS_TYPE == AHRS_TYPE_ICQ ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(float_vect3_norm(&aos.ltp_vel)); ahrs_impl.ltp_vel_norm_valid = true; #endif #endif }
void stateCalcAccelEcef_i(void) { if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) return; if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f); } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) { ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i); } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) { /* transform ned_f -> ned_i -> ecef_i , set status bits */ ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f); SetBit(state.accel_status, ACCEL_NED_I); ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i); } else { /* could not get this representation, set errno */ //struct EcefCoor_i _ecef_zero = {0}; //return _ecef_zero; } /* set bit to indicate this representation is computed */ SetBit(state.accel_status, ACCEL_ECEF_I); }
/** * Event handling for Vectornav */ void ahrs_vectornav_event(void) { // receive data vn200_event(&(ahrs_vn.vn_packet)); // read message if (ahrs_vn.vn_packet.msg_available) { vn200_read_message(&(ahrs_vn.vn_packet),&(ahrs_vn.vn_data)); ahrs_vn.vn_packet.msg_available = false; if (ahrs_vectornav_is_enabled()) { ahrs_vectornav_propagate(); } // send ABI messages uint32_t now_ts = get_sys_time_usec(); // in fixed point for sending as ABI and telemetry msgs RATES_BFP_OF_REAL(ahrs_vn.gyro_i, ahrs_vn.vn_data.gyro); AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i); ACCELS_BFP_OF_REAL(ahrs_vn.accel_i, ahrs_vn.vn_data.accel); AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i); } }
void aos_compute_sensors(void) { struct FloatRates gyro; RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias); // printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r)); float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise); RATES_BFP_OF_REAL(imu.gyro, gyro); RATES_BFP_OF_REAL(imu.gyro_prev, gyro); struct FloatVect3 g_ltp = {0., 0., 9.81}; struct FloatVect3 accelero_ltp; VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp); struct FloatVect3 accelero_imu; FLOAT_QUAT_VMULT(accelero_imu, aos.ltp_to_imu_quat, accelero_ltp); float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise); ACCELS_BFP_OF_REAL(imu.accel, accelero_imu); struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; struct FloatVect3 h_imu; FLOAT_QUAT_VMULT(h_imu, aos.ltp_to_imu_quat, h_earth); MAGS_BFP_OF_REAL(imu.mag, h_imu); #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN #if AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ VECT3_COPY(ahrs_impl.est_ltp_speed, aos.ltp_vel); #endif #if AHRS_TYPE == AHRS_TYPE_ICQ ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel)); #endif #endif }
/** * 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 gx3_packet_read_message(void) { ahrs_impl.gx3_accel.x = bef(&ahrs_impl.gx3_packet.msg_buf[1]); ahrs_impl.gx3_accel.y = bef(&ahrs_impl.gx3_packet.msg_buf[5]); ahrs_impl.gx3_accel.z = bef(&ahrs_impl.gx3_packet.msg_buf[9]); ahrs_impl.gx3_rate.p = bef(&ahrs_impl.gx3_packet.msg_buf[13]); ahrs_impl.gx3_rate.q = bef(&ahrs_impl.gx3_packet.msg_buf[17]); ahrs_impl.gx3_rate.r = bef(&ahrs_impl.gx3_packet.msg_buf[21]); ahrs_impl.gx3_rmat.m[0] = bef(&ahrs_impl.gx3_packet.msg_buf[25]); ahrs_impl.gx3_rmat.m[1] = bef(&ahrs_impl.gx3_packet.msg_buf[29]); ahrs_impl.gx3_rmat.m[2] = bef(&ahrs_impl.gx3_packet.msg_buf[33]); ahrs_impl.gx3_rmat.m[3] = bef(&ahrs_impl.gx3_packet.msg_buf[37]); ahrs_impl.gx3_rmat.m[4] = bef(&ahrs_impl.gx3_packet.msg_buf[41]); ahrs_impl.gx3_rmat.m[5] = bef(&ahrs_impl.gx3_packet.msg_buf[45]); ahrs_impl.gx3_rmat.m[6] = bef(&ahrs_impl.gx3_packet.msg_buf[49]); ahrs_impl.gx3_rmat.m[7] = bef(&ahrs_impl.gx3_packet.msg_buf[53]); ahrs_impl.gx3_rmat.m[8] = bef(&ahrs_impl.gx3_packet.msg_buf[57]); ahrs_impl.gx3_time = (uint32_t)(ahrs_impl.gx3_packet.msg_buf[61] << 24 | ahrs_impl.gx3_packet.msg_buf[62] << 16 | ahrs_impl.gx3_packet.msg_buf[63] << 8 | ahrs_impl.gx3_packet.msg_buf[64]); ahrs_impl.gx3_chksm = GX3_CHKSM(ahrs_impl.gx3_packet.msg_buf); ahrs_impl.gx3_freq = 62500.0 / (float)(ahrs_impl.gx3_time - ahrs_impl.gx3_ltime); ahrs_impl.gx3_ltime = ahrs_impl.gx3_time; // Acceleration VECT3_SMUL(ahrs_impl.gx3_accel, ahrs_impl.gx3_accel, 9.80665); // Convert g into m/s2 ACCELS_BFP_OF_REAL(imu.accel, ahrs_impl.gx3_accel); // for backwards compatibility with fixed point interface imuf.accel = ahrs_impl.gx3_accel; // Rates struct FloatRates body_rate; imuf.gyro = ahrs_impl.gx3_rate; /* compute body rates */ struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imuf.body_to_imu); FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, imuf.gyro); /* Set state */ stateSetBodyRates_f(&body_rate); // Attitude struct FloatRMat ltp_to_body_rmat; FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, *body_to_imu_rmat); #if AHRS_USE_GPS_HEADING && USE_GPS struct FloatEulers ltp_to_body_eulers; float_eulers_of_rmat(<p_to_body_eulers, <p_to_body_rmat); float course_f = (float)DegOfRad(gps.course / 1e7); if (course_f > 180.0) { course_f -= 360.0; } ltp_to_body_eulers.psi = (float)RadOfDeg(course_f); stateSetNedToBodyEulers_f(<p_to_body_eulers); #else // !AHRS_USE_GPS_HEADING #ifdef IMU_MAG_OFFSET struct FloatEulers ltp_to_body_eulers; float_eulers_of_rmat(<p_to_body_eulers, <p_to_body_rmat); ltp_to_body_eulers.psi -= ahrs_impl.mag_offset; stateSetNedToBodyEulers_f(<p_to_body_eulers); #else stateSetNedToBodyRMat_f(<p_to_body_rmat); #endif // IMU_MAG_OFFSET #endif // !AHRS_USE_GPS_HEADING }
void UM6_packet_read_message(void) { if ((UM6_status == UM6Running) && PacketLength > 11) { switch (PacketAddr) { case IMU_UM6_GYRO_PROC: UM6_rate.p = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0610352; UM6_rate.q = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0610352; UM6_rate.r = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0610352; RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec RATES_BFP_OF_REAL(imu.gyro, UM6_rate); break; case IMU_UM6_ACCEL_PROC: UM6_accel.x = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000183105; UM6_accel.y = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000183105; UM6_accel.z = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000183105; VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2 ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int break; case IMU_UM6_MAG_PROC: UM6_mag.x = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000305176; UM6_mag.y = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000305176; UM6_mag.z = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000305176; // Assume the same units for magnetic field // Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss). MAGS_BFP_OF_REAL(imu.mag, UM6_mag); break; case IMU_UM6_EULER: UM6_eulers.phi = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0109863; UM6_eulers.theta = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0109863; UM6_eulers.psi = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0109863; EULERS_SMUL(UM6_eulers, UM6_eulers, 0.0174532925); //Convert deg to rad EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers); break; case IMU_UM6_QUAT: UM6_quat.qi = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0000335693; UM6_quat.qx = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0000335693; UM6_quat.qy = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0000335693; UM6_quat.qz = ((float)((int16_t) ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 6] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 7]))) * 0.0000335693; QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat); break; default: break; } } }