void imu_SetBodyToImuCurrent(float set) { imu.b2i_set_current = set; if (imu.b2i_set_current) { // adjust imu_to_body roll and pitch by current NedToBody roll and pitch struct FloatEulers body_to_imu_eulers; memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); if (stateIsAttitudeValid()) { // adjust imu_to_body roll and pitch by current NedToBody roll and pitch body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi; body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); #if USE_IMU_FLOAT orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); #endif } else { // indicate that we couldn't set to current roll/pitch imu.b2i_set_current = FALSE; } } else { // reset to BODY_TO_IMU as defined in airframe file struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); #if USE_IMU_FLOAT orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); #endif } }
void imu_SetBodyToImuPsi(float psi) { struct FloatEulers body_to_imu_eulers; memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); body_to_imu_eulers.psi = psi; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); #if USE_IMU_FLOAT orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); #endif }
void imu_SetBodyToImuTheta(float theta) { struct FloatEulers body_to_imu_eulers; memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); body_to_imu_eulers.theta = theta; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); #if USE_IMU_FLOAT orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); #endif }
void imu_init(void) { #ifdef IMU_POWER_GPIO gpio_setup_output(IMU_POWER_GPIO); IMU_POWER_GPIO_ON(IMU_POWER_GPIO); #endif /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); #if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); #else #if USE_MAGNETOMETER INFO("Magnetometer neutrals are set to zero, you should calibrate!") #endif INT_VECT3_ZERO(imu.mag_neutral); #endif struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); #if USE_IMU_FLOAT orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); #endif #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); #if !USE_IMU_FLOAT register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag); #endif // !USE_IMU_FLOAT #endif // DOWNLINK imu_impl_init(); }
void imu_SetBodyToImuPsi(float psi) { struct FloatEulers body_to_imu_eulers; memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); body_to_imu_eulers.psi = psi; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); }
/* Initialize the magneto and pitot */ void mag_pitot_init() { // Initialize transport protocol pprz_transport_init(&mag_pitot.transport); // Set the Imu to Magneto rotation struct FloatEulers imu_to_mag_eulers = {IMU_TO_MAG_PHI, IMU_TO_MAG_THETA, IMU_TO_MAG_PSI}; orientationSetEulers_f(&mag_pitot.imu_to_mag, &imu_to_mag_eulers); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_RAW, mag_pitot_raw_downlink); #endif }
/** * Initialize Vectornav struct */ void ins_vectornav_init(void) { // Initialize variables ins_vn.vn_status = VNNotTracking; ins_vn.vn_time = get_sys_time_float(); // Initialize packet ins_vn.vn_packet.status = VNMsgSync; ins_vn.vn_packet.msg_idx = 0; ins_vn.vn_packet.msg_available = FALSE; ins_vn.vn_packet.chksm_error = 0; ins_vn.vn_packet.hdr_error = 0; ins_vn.vn_packet.overrun_error = 0; ins_vn.vn_packet.noise_error = 0; ins_vn.vn_packet.framing_error = 0; INT32_VECT3_ZERO(ins_vn.ltp_pos_i); INT32_VECT3_ZERO(ins_vn.ltp_speed_i); INT32_VECT3_ZERO(ins_vn.ltp_accel_i); FLOAT_VECT3_ZERO(ins_vn.vel_ned); FLOAT_VECT3_ZERO(ins_vn.lin_accel); FLOAT_VECT3_ZERO(ins_vn.vel_body); #if USE_INS_NAV_INIT ins_init_origin_from_flightplan(); ins_vn.ltp_initialized = TRUE; #else ins_vn.ltp_initialized = FALSE; #endif struct FloatEulers body_to_imu_eulers = {INS_VN_BODY_TO_IMU_PHI, INS_VN_BODY_TO_IMU_THETA, INS_VN_BODY_TO_IMU_PSI}; orientationSetEulers_f(&ins_vn.body_to_imu, &body_to_imu_eulers); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "INS", send_ins); register_periodic_telemetry(DefaultPeriodic, "INS_Z", send_ins_z); register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref); register_periodic_telemetry(DefaultPeriodic, "VECTORNAV_INFO", send_vn_info); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled); #endif }
/** * Navstik IMU initializtion of the MPU-60x0 and HMC58xx */ void imu_bebop_init(void) { /* MPU-60X0 */ mpu60x0_i2c_init(&imu_bebop.mpu, &(BEBOP_MPU_I2C_DEV), MPU60X0_ADDR); imu_bebop.mpu.config.smplrt_div = BEBOP_SMPLRT_DIV; imu_bebop.mpu.config.dlpf_cfg = BEBOP_LOWPASS_FILTER; imu_bebop.mpu.config.gyro_range = BEBOP_GYRO_RANGE; imu_bebop.mpu.config.accel_range = BEBOP_ACCEL_RANGE; /* AKM8963 */ ak8963_init(&imu_bebop.ak, &(BEBOP_MAG_I2C_DEV), AK8963_ADDR); #if BEBOP_VERSION2 //the magnetometer of the bebop2 is located on the gps board, //which is under a slight angle struct FloatEulers imu_to_mag_eulers = {0.0, RadOfDeg(8.5), 0.0}; orientationSetEulers_f(&imu_to_mag_bebop, &imu_to_mag_eulers); #endif }