void handle_ins_msg(void) {
#if USE_INS
update_fw_estimator();
#endif
#if USE_IMU
#ifdef XSENS_BACKWARDS
if (imu_xsens.gyro_available) {
RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(ins_p), -RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
}
if (imu_xsens.accel_available) {
VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(ins_ax), -ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
}
if (imu_xsens.mag_available) {
VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(ins_mx), -MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
}
#else
if (imu_xsens.gyro_available) {
RATES_ASSIGN(imu.gyro_unscaled, RATE_BFP_OF_REAL(ins_p), RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
}
if (imu_xsens.accel_available) {
VECT3_ASSIGN(imu.accel_unscaled, ACCEL_BFP_OF_REAL(ins_ax), ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
}
if (imu_xsens.mag_available) {
VECT3_ASSIGN(imu.mag_unscaled, MAG_BFP_OF_REAL(ins_mx), MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
}
#endif /* XSENS_BACKWARDS */
#endif /* USE_IMU */
#if USE_GPS_XSENS
#ifndef ALT_KALMAN
#warning NO_VZ
#endif
// Horizontal speed
float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
if (gps.fix != GPS_FIX_3D) {
fspeed = 0;
}
gps.gspeed = fspeed * 100.;
gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
float fcourse = atan2f((float)ins_vy, (float)ins_vx);
gps.course = fcourse * 1e7;
#endif // USE_GPS_XSENS
}
//Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
void stabilization_rate_read_rc_switched_sticks(void)
{
if (ROLL_RATE_DEADBAND_EXCEEDED()) {
stabilization_rate_sp.r = (int32_t) - radio_control.values[RADIO_ROLL] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_P) / MAX_PPRZ;
} else {
stabilization_rate_sp.r = 0;
}
if (PITCH_RATE_DEADBAND_EXCEEDED()) {
stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_Q) / MAX_PPRZ;
} else {
stabilization_rate_sp.q = 0;
}
if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_R) / MAX_PPRZ;
} else {
stabilization_rate_sp.p = 0;
}
}
void stabilization_attitude_read_rc_absolute(bool_t in_flight) {
// FIXME: wtf???
#ifdef AIRPLANE_STICKS
pprz_t roll = radio_control.values[RADIO_ROLL];
pprz_t pitch = radio_control.values[RADIO_PITCH];
pprz_t yaw = radio_control.values[RADIO_YAW];
#else // QUAD STICKS
pprz_t roll = radio_control.values[RADIO_YAW];
pprz_t pitch = radio_control.values[RADIO_PITCH];
pprz_t yaw = -radio_control.values[RADIO_ROLL];
#endif
struct Int32Eulers sticks_eulers;
struct Int32Quat sticks_quat, prev_sp_quat;
// heading hold?
if (in_flight) {
// compose setpoint based on previous setpoint + pitch/roll sticks
reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), &stab_att_sp_quat);
// get commanded yaw rate from sticks
sticks_eulers.phi = RATE_BFP_OF_REAL(APPLY_DEADBAND(roll, STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF / RC_UPDATE_FREQ);
sticks_eulers.theta = 0;
sticks_eulers.psi = 0;
// convert yaw rate * dt into quaternion
INT32_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
QUAT_COPY(prev_sp_quat, stab_att_sp_quat)
// update setpoint by rotating by incremental yaw command
INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, sticks_quat);
} else { /* if not flying, use current body position + pitch/yaw from sticks to compose setpoint */
reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), stateGetNedToBodyQuat_i());
}
// update euler setpoints for telemetry
INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat);
}