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
}
static void send_bias(struct transport_tx *trans, struct link_device *dev)
{
struct Int32Rates gyro_bias;
RATES_BFP_OF_REAL(gyro_bias, ahrs_mlkf.gyro_bias);
pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
&gyro_bias.p, &gyro_bias.q, &gyro_bias.r, &ahrs_mlkf_id);
}
void stateCalcBodyRates_i(void) {
if (bit_is_set(state.rate_status, RATE_I))
return;
if (bit_is_set(state.rate_status, RATE_F)) {
RATES_BFP_OF_REAL(state.body_rates_i, state.body_rates_f);
}
/* set bit to indicate this representation is computed */
SetBit(state.rate_status, RATE_I);
}
gboolean timeout_callback(gpointer data) {
for (int i=0; i<20; i++) {
aos_compute_state();
aos_compute_sensors();
#ifndef DISABLE_PROPAGATE
ahrs_propagate();
#endif
#ifndef DISABLE_ACCEL_UPDATE
ahrs_update_accel();
#endif
#ifndef DISABLE_MAG_UPDATE
if (!(i%5)) ahrs_update_mag();
#endif
}
#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ
EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler);
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
ahrs_impl.gyro_bias.p,
ahrs_impl.gyro_bias.q,
ahrs_impl.gyro_bias.r);
#endif
#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2
struct Int32Rates bias_i;
RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias);
IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
bias_i.p,
bias_i.q,
bias_i.r);
#endif
IvySendMsg("183 AHRS_EULER %f %f %f",
ahrs_float.ltp_to_imu_euler.phi,
ahrs_float.ltp_to_imu_euler.theta,
ahrs_float.ltp_to_imu_euler.psi);
IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
DegOfRad(aos.imu_rates.p),
DegOfRad(aos.imu_rates.q),
DegOfRad(aos.imu_rates.r),
DegOfRad(aos.ltp_to_imu_euler.phi),
DegOfRad(aos.ltp_to_imu_euler.theta),
DegOfRad(aos.ltp_to_imu_euler.psi));
IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f",
DegOfRad(aos.gyro_bias.p),
DegOfRad(aos.gyro_bias.q),
DegOfRad(aos.gyro_bias.r));
return TRUE;
}
void sim_overwrite_ahrs(void) {
EULERS_BFP_OF_REAL(ahrs.ltp_to_body_euler, fdm.ltp_to_body_eulers);
QUAT_BFP_OF_REAL(ahrs.ltp_to_body_quat, fdm.ltp_to_body_quat);
RATES_BFP_OF_REAL(ahrs.body_rate, fdm.body_ecef_rotvel);
INT32_RMAT_OF_QUAT(ahrs.ltp_to_body_rmat, ahrs.ltp_to_body_quat);
}
void sim_overwrite_ahrs(void) {
struct Int32Quat quat;
QUAT_BFP_OF_REAL(quat, fdm.ltp_to_body_quat);
stateSetNedToBodyQuat_i(&quat);
struct Int32Rates rates;
RATES_BFP_OF_REAL(rates, fdm.body_ecef_rotvel);
stateSetBodyRates_i(&rates);
}
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
}
/**
* 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);
}
}
/**
* 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 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;
}
}
}
