void imu_init(void) { /* initialises neutrals */ RATES_ASSIGN(booz_imu.gyro_neutral, IMU_NEUTRAL_GYRO_P, IMU_NEUTRAL_GYRO_Q, IMU_NEUTRAL_GYRO_R); VECT3_ASSIGN(booz_imu.accel_neutral, IMU_NEUTRAL_ACCEL_X, IMU_NEUTRAL_ACCEL_Y, IMU_NEUTRAL_ACCEL_Z); VECT3_ASSIGN(booz_imu.mag_neutral, IMU_NEUTRAL_MAG_X, IMU_NEUTRAL_MAG_Y, IMU_NEUTRAL_MAG_Z); /* initialise IMU alignment */ imu_adjust_alignment(IMU_ALIGNMENT_BODY_TO_IMU_PHI, IMU_ALIGNMENT_BODY_TO_IMU_THETA, IMU_ALIGNMENT_BODY_TO_IMU_PSI); imu_spi_selected = SPI_NONE; do_max1168_read = FALSE; /* setup pins for SSP (SCK, MISO, MOSI) */ PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI; /* setup SSP */ SSPCR0 = SSPCR0_VAL;; SSPCR1 = SSPCR1_VAL; SSPCPSR = 0x02; /* initialize interrupt vector */ VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */ VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */ _VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1; _VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */ max1168_init(); micromag_init(); }
void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); VECT3_ASSIGN(stabilization_gains.p, STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN); VECT3_ASSIGN(stabilization_gains.d, STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN); VECT3_ASSIGN(stabilization_gains.i, STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN); VECT3_ASSIGN(stabilization_gains.dd, STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN); INT_EULERS_ZERO( stabilization_att_sum_err ); }
static void traj_sineX_quad_update(void) { const float om = RadOfDeg(10); if (aos.time > (M_PI / om)) { const float a = 20; struct FloatVect3 jerk; VECT3_ASSIGN(jerk , -a * om * om * om * cos(om * aos.time), 0, 0); VECT3_ASSIGN(aos.ltp_accel , -a * om * om * sin(om * aos.time), 0, 0); VECT3_ASSIGN(aos.ltp_vel , a * om * cos(om * aos.time), 0, 0); VECT3_ASSIGN(aos.ltp_pos , a * sin(om * aos.time), 0, 0); // this is based on differential flatness of the quad EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.); float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler); const struct FloatEulers e_dot = { 0., 9.81 * jerk.x / ((9.81 * 9.81) + (aos.ltp_accel.x * aos.ltp_accel.x)), 0. }; FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot); } }
void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); VECT3_ASSIGN(stabilization_gains.p, STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN); VECT3_ASSIGN(stabilization_gains.d, STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN); VECT3_ASSIGN(stabilization_gains.i, STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN); VECT3_ASSIGN(stabilization_gains.dd, STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN); FLOAT_EULERS_ZERO( stabilization_att_sum_err ); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref); #endif }
void imu_navgo_event( void ) { // If the itg3200 I2C transaction has succeeded: convert the data itg3200_event(); if (itg3200_data_available) { RATES_COPY(imu.gyro_unscaled, itg3200_data); itg3200_data_available = FALSE; gyr_valid = TRUE; } // If the adxl345 I2C transaction has succeeded: convert the data adxl345_event(); if (adxl345_data_available) { VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z); adxl345_data_available = FALSE; acc_valid = TRUE; } // HMC58XX event task hmc58xx_event(); if (hmc58xx_data_available) { VECT3_ASSIGN(imu.mag_unscaled, -hmc58xx_data.x, -hmc58xx_data.y, hmc58xx_data.z); hmc58xx_data_available = FALSE; mag_valid = TRUE; } }
void imu_init(void) { /* 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); VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); /* Compute quaternion and rotation matrix for conversions between body and imu frame */ #if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALISE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); #else INT32_QUAT_ZERO(imu.body_to_imu_quat); INT32_RMAT_ZERO(imu.body_to_imu_rmat); #endif imu_impl_init(); }
void imu_init(void) { /* 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 #pragma message "Info: Magnetomter neutrals are set to zero!" #endif INT_VECT3_ZERO(imu.mag_neutral); #endif /* Compute quaternion and rotation matrix for conversions between body and imu frame */ struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALIZE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); imu_impl_init(); }
/** * Handle all the events of the Navstik IMU components. * When there is data available convert it to the correct axis and save it in the imu structure. */ void imu_bebop_event(void) { /* MPU-60x0 event taks */ mpu60x0_i2c_event(&imu_bebop.mpu); if (imu_bebop.mpu.data_available) { /* default orientation of the MPU is upside down sor corrigate this here */ RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q, -imu_bebop.mpu.data_rates.rates.r); VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y, -imu_bebop.mpu.data_accel.vect.z); imu_bebop.mpu.data_available = FALSE; imu_bebop.gyro_valid = TRUE; imu_bebop.accel_valid = TRUE; } /* AKM8963 event task */ ak8963_event(&imu_bebop.ak); if (imu_bebop.ak.data_available) { //32760 to -32760 VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z); imu_bebop.ak.data_available = FALSE; imu_bebop.mag_valid = TRUE; } }
static void traj_coordinated_circle_update(void) { const float speed = 15; // m/s const float R = 100; // radius in m float omega = speed / R; // tan phi = v^2/Rg float phi = atan2(speed * speed, R * 9.81); if (aos.time > 2.*M_PI / omega) { VECT3_ASSIGN(aos.ltp_pos, R * cos(omega * aos.time), R * sin(omega * aos.time), 0.); VECT3_ASSIGN(aos.ltp_vel, -omega * R * sin(omega * aos.time), omega * R * cos(omega * aos.time), 0.); VECT3_ASSIGN(aos.ltp_accel, -omega * omega * R * cos(omega * aos.time), -omega * omega * R * sin(omega * aos.time), 0.); // float psi = atan2(aos.ltp_pos.y, aos.ltp_pos.x); float psi = M_PI_2 + omega * aos.time; while (psi > M_PI) { psi -= 2.*M_PI; } EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, psi); float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler); struct FloatEulers e_dot; EULERS_ASSIGN(e_dot, 0., 0., omega); float_rates_of_euler_dot(&aos.imu_rates, &aos.ltp_to_imu_euler, &e_dot); } }
void imu_periodic( void ) { // Start reading the latest gyroscope data if (!imu_krooz.mpu.config.initialized) mpu60x0_i2c_start_configure(&imu_krooz.mpu); if (!imu_krooz.hmc.initialized) hmc58xx_start_configure(&imu_krooz.hmc); if (imu_krooz.meas_nb) { RATES_ASSIGN(imu.gyro_unscaled, imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb); #if KROOZ_USE_MEDIAN_FILTER UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled); #endif VECT3_ASSIGN(imu.accel_unscaled, imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb); #if KROOZ_USE_MEDIAN_FILTER UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); #endif RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0); VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0); imu_krooz.meas_nb = 0; imu_krooz.gyr_valid = TRUE; imu_krooz.acc_valid = TRUE; } //RunOnceEvery(10,imu_krooz_downlink_raw()); }
void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]); VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]); #ifdef HAS_SURFACE_COMMANDS VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]); #endif } FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err ); FLOAT_RATES_ZERO( last_body_rate ); FLOAT_RATES_ZERO( body_rate_d ); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref); #endif }
void stabilization_attitude_init(void) { /* setpoints */ FLOAT_EULERS_ZERO(stab_att_sp_euler); float_quat_identity(&stab_att_sp_quat); /* reference */ attitude_ref_quat_float_init(&att_ref_quat_f); attitude_ref_quat_float_schedule(&att_ref_quat_f, STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT); for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]); VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]); #ifdef HAS_SURFACE_COMMANDS VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]); #endif } float_quat_identity(&stabilization_att_sum_err_quat); FLOAT_RATES_ZERO(last_body_rate); FLOAT_RATES_ZERO(body_rate_d); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_FLOAT, send_att); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_FLOAT, send_att_ref); #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 /* Compute quaternion and rotation matrix for conversions between body and imu frame */ struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALIZE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); #if USE_IMU_FLOAT #else // !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 nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) { FLOAT_VECT3_ZERO(accel->value); accel->resolution = NPS_ACCEL_RESOLUTION; FLOAT_MAT33_DIAG(accel->sensitivity, NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ); VECT3_ASSIGN(accel->neutral, NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z); VECT3_ASSIGN(accel->noise_std_dev, NPS_ACCEL_NOISE_STD_DEV_X, NPS_ACCEL_NOISE_STD_DEV_Y, NPS_ACCEL_NOISE_STD_DEV_Z); VECT3_ASSIGN(accel->bias, NPS_ACCEL_BIAS_X, NPS_ACCEL_BIAS_Y, NPS_ACCEL_BIAS_Z); accel->next_update = time; accel->data_available = FALSE; }
void nps_sensor_mag_init(struct NpsSensorMag* mag, double time) { VECT3_ASSIGN(mag->value, 0., 0., 0.); // mag->resolution = NPS_MAG_RESOLUTION; FLOAT_MAT33_DIAG(mag->sensitivity, NPS_MAG_SENSITIVITY_XX, NPS_MAG_SENSITIVITY_YY, NPS_MAG_SENSITIVITY_ZZ); VECT3_ASSIGN(mag->neutral, NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z); VECT3_ASSIGN(mag->noise_std_dev, NPS_MAG_NOISE_STD_DEV_X, NPS_MAG_NOISE_STD_DEV_Y, NPS_MAG_NOISE_STD_DEV_Z); struct DoubleEulers imu_to_sensor_eulers = { NPS_MAG_IMU_TO_SENSOR_PHI, NPS_MAG_IMU_TO_SENSOR_THETA, NPS_MAG_IMU_TO_SENSOR_PSI }; DOUBLE_RMAT_OF_EULERS(mag->imu_to_sensor_rmat, imu_to_sensor_eulers); mag->next_update = time; mag->data_available = FALSE; }
void imu_aspirin2_event(void) { mpu60x0_spi_event(&imu_aspirin2.mpu); if (imu_aspirin2.mpu.data_available) { /* HMC5883 has xzy order of axes in returned data */ struct Int32Vect3 mag; mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0); mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2); mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4); #ifdef LISA_M_LONGITUDINAL_X RATES_ASSIGN(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates.q, -imu_aspirin2.mpu.data_rates.rates.p, imu_aspirin2.mpu.data_rates.rates.r); VECT3_ASSIGN(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect.y, -imu_aspirin2.mpu.data_accel.vect.x, imu_aspirin2.mpu.data_accel.vect.z); VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z); #else #ifdef LISA_S #ifdef LISA_S_UPSIDE_DOWN RATES_ASSIGN(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates.p, -imu_aspirin2.mpu.data_rates.rates.q, -imu_aspirin2.mpu.data_rates.rates.r); VECT3_ASSIGN(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect.x, -imu_aspirin2.mpu.data_accel.vect.y, -imu_aspirin2.mpu.data_accel.vect.z); VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z); #else RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); VECT3_COPY(imu.mag_unscaled, mag); #endif #else RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z) #endif #endif imu_aspirin2.mpu.data_available = FALSE; imu_aspirin2.gyro_valid = TRUE; imu_aspirin2.accel_valid = TRUE; imu_aspirin2.mag_valid = TRUE; } }
void imu_px4fmu_event(void) { mpu60x0_spi_event(&imu_px4fmu.mpu); if (imu_px4fmu.mpu.data_available) { RATES_ASSIGN(imu.gyro_unscaled, imu_px4fmu.mpu.data_rates.rates.q, imu_px4fmu.mpu.data_rates.rates.p, -imu_px4fmu.mpu.data_rates.rates.r); VECT3_ASSIGN(imu.accel_unscaled, imu_px4fmu.mpu.data_accel.vect.y, imu_px4fmu.mpu.data_accel.vect.x, -imu_px4fmu.mpu.data_accel.vect.z); imu_px4fmu.mpu.data_available = FALSE; imu_px4fmu.gyro_valid = TRUE; imu_px4fmu.accel_valid = TRUE; } /* HMC58XX event task */ hmc58xx_event(&imu_px4fmu.hmc); if (imu_px4fmu.hmc.data_available) { imu.mag_unscaled.x = imu_px4fmu.hmc.data.vect.y; imu.mag_unscaled.y = imu_px4fmu.hmc.data.vect.x; imu.mag_unscaled.z = -imu_px4fmu.hmc.data.vect.z; imu_px4fmu.hmc.data_available = FALSE; imu_px4fmu.mag_valid = TRUE; } }
void imu_krooz_event( void ) { if (imu_krooz.mpu_eoc) { mpu60x0_i2c_read(&imu_krooz.mpu); imu_krooz.mpu_eoc = FALSE; } // If the MPU6050 I2C transaction has succeeded: convert the data mpu60x0_i2c_event(&imu_krooz.mpu); if (imu_krooz.mpu.data_available) { RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates); VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect); imu_krooz.meas_nb++; imu_krooz.mpu.data_available = FALSE; } if (imu_krooz.hmc_eoc) { hmc58xx_read(&imu_krooz.hmc); imu_krooz.hmc_eoc = FALSE; } // If the HMC5883 I2C transaction has succeeded: convert the data hmc58xx_event(&imu_krooz.hmc); if (imu_krooz.hmc.data_available) { VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z); UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled); imu_krooz.hmc.data_available = FALSE; imu_krooz.mag_valid = TRUE; } }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE; /* set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); INT_RATES_ZERO(ahrs_impl.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.high_rez_bias); #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; #else ahrs_impl.correct_gravity = FALSE; #endif #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC ahrs_impl.use_gravity_heuristic = TRUE; #else ahrs_impl.use_gravity_heuristic = FALSE; #endif VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); //INT32_VECT3_ZERO(imu_accel_local); //This is part of the grav correction hack }
void imu_feed_mag(void) { VECT3_ASSIGN(imu.mag_unscaled, sensors.mag.value.x, sensors.mag.value.y, sensors.mag.value.z); imu_nps.mag_available = TRUE; }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT;//AHRS状态未初始化 ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE;//未航姿校准 /* set ltp_to_imu so that body is zero */ //设置ltp_to_imu ,imu速度,陀螺仪的偏差,速度矫正, QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); INT_RATES_ZERO(ahrs_impl.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.high_rez_bias); #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; #else ahrs_impl.correct_gravity = FALSE; #endif #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC ahrs_impl.use_gravity_heuristic = TRUE; #else ahrs_impl.use_gravity_heuristic = FALSE; #endif VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); }
void imu_px4fmu_event(void) { uint32_t now_ts = get_sys_time_usec(); mpu60x0_spi_event(&imu_px4fmu.mpu); if (imu_px4fmu.mpu.data_available) { RATES_ASSIGN(imu.gyro_unscaled, imu_px4fmu.mpu.data_rates.rates.q, imu_px4fmu.mpu.data_rates.rates.p, -imu_px4fmu.mpu.data_rates.rates.r); VECT3_ASSIGN(imu.accel_unscaled, imu_px4fmu.mpu.data_accel.vect.y, imu_px4fmu.mpu.data_accel.vect.x, -imu_px4fmu.mpu.data_accel.vect.z); imu_px4fmu.mpu.data_available = FALSE; imu_scale_gyro(&imu); imu_scale_accel(&imu); AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); } /* HMC58XX event task */ hmc58xx_event(&imu_px4fmu.hmc); if (imu_px4fmu.hmc.data_available) { imu.mag_unscaled.x = imu_px4fmu.hmc.data.vect.y; imu.mag_unscaled.y = imu_px4fmu.hmc.data.vect.x; imu.mag_unscaled.z = -imu_px4fmu.hmc.data.vect.z; imu_px4fmu.hmc.data_available = FALSE; imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); } }
void imu_impl_init( void ) { ///////////////////////////////////////////////////////////////////// // MPU-60X0 mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR); // change the default configuration imu_krooz.mpu.config.smplrt_div = KROOZ_SMPLRT_DIV; imu_krooz.mpu.config.dlpf_cfg = KROOZ_LOWPASS_FILTER; imu_krooz.mpu.config.gyro_range = KROOZ_GYRO_RANGE; imu_krooz.mpu.config.accel_range = KROOZ_ACCEL_RANGE; imu_krooz.mpu.config.drdy_int_enable = TRUE; hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR); #if KROOZ_USE_MEDIAN_FILTER // Init median filters InitMedianFilterRatesInt(median_gyro); InitMedianFilterVect3Int(median_accel); InitMedianFilterVect3Int(median_mag); #endif RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0); VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0); imu_krooz.meas_nb = 0; imu_krooz.gyr_valid = FALSE; imu_krooz.acc_valid = FALSE; imu_krooz.mag_valid = FALSE; imu_krooz_sd_arch_init(); }
void ahrs_init(void) { QUAT_ASSIGN(ned_to_body_orientation_quat_i, 1, 0, 0, 0); INT32_QUAT_NORMALIZE(ned_to_body_orientation_quat_i); RATES_ASSIGN(body_rates_i, 0, 0, 0); ahrs.status = AHRS_UNINIT; ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE; /* set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); INT_RATES_ZERO(ahrs_impl.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.high_rez_bias); #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; #else ahrs_impl.correct_gravity = FALSE; #endif #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC ahrs_impl.use_gravity_heuristic = TRUE; #else ahrs_impl.use_gravity_heuristic = FALSE; #endif VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; /* * Initialises our IMU alignement variables * This should probably done in the IMU code instead */ struct FloatEulers body_to_imu_euler = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); /* Set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); FLOAT_RATES_ZERO(ahrs_impl.imu_rate); VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); /* * Initialises our state */ FLOAT_RATES_ZERO(ahrs_impl.gyro_bias); const float P0_a = 1.; const float P0_b = 1e-4; float P0[6][6] = {{ P0_a, 0., 0., 0., 0., 0. }, { 0., P0_a, 0., 0., 0., 0. }, { 0., 0., P0_a, 0., 0., 0. }, { 0., 0., 0., P0_b, 0., 0. }, { 0., 0., 0., 0., P0_b, 0. }, { 0., 0., 0., 0., 0., P0_b}}; memcpy(ahrs_impl.P, P0, sizeof(P0)); }
void imu_krooz_event(void) { if (imu_krooz.mpu_eoc) { mpu60x0_i2c_read(&imu_krooz.mpu); imu_krooz.mpu_eoc = false; } // If the MPU6050 I2C transaction has succeeded: convert the data mpu60x0_i2c_event(&imu_krooz.mpu); if (imu_krooz.mpu.data_available) { RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates); VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect); imu_krooz.meas_nb++; imu_krooz.mpu.data_available = false; } if (imu_krooz.hmc_eoc) { hmc58xx_read(&imu_krooz.hmc); imu_krooz.hmc_eoc = false; } // If the HMC5883 I2C transaction has succeeded: convert the data hmc58xx_event(&imu_krooz.hmc); if (imu_krooz.hmc.data_available) { VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z); UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled); imu_krooz.hmc.data_available = false; imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, get_sys_time_usec(), &imu.mag); } }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE; /* Initialises IMU alignement */ struct FloatEulers body_to_imu_euler = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); /* Set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); FLOAT_RATES_ZERO(ahrs_impl.imu_rate); #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; #else ahrs_impl.correct_gravity = FALSE; #endif VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); }
void ahrs_fc_init(void) { ahrs_fc.status = AHRS_FC_UNINIT; ahrs_fc.is_aligned = FALSE; ahrs_fc.ltp_vel_norm_valid = FALSE; ahrs_fc.heading_aligned = FALSE; /* init ltp_to_imu quaternion as zero/identity rotation */ float_quat_identity(&ahrs_fc.ltp_to_imu_quat); FLOAT_RATES_ZERO(ahrs_fc.imu_rate); /* set default filter cut-off frequency and damping */ ahrs_fc.accel_omega = AHRS_ACCEL_OMEGA; ahrs_fc.accel_zeta = AHRS_ACCEL_ZETA; ahrs_fc.mag_omega = AHRS_MAG_OMEGA; ahrs_fc.mag_zeta = AHRS_MAG_ZETA; #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_fc.correct_gravity = TRUE; #else ahrs_fc.correct_gravity = FALSE; #endif ahrs_fc.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; VECT3_ASSIGN(ahrs_fc.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); ahrs_fc.accel_cnt = 0; ahrs_fc.mag_cnt = 0; }
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 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 }