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
}
