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); }