void nav_init(void) { waypoints_init(); nav_block = 0; nav_stage = 0; nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT); nav_flight_altitude = nav_altitude; flight_altitude = SECURITY_ALT; VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i); VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i); horizontal_mode = HORIZONTAL_MODE_WAYPOINT; vertical_mode = VERTICAL_MODE_ALT; nav_roll = 0; nav_pitch = 0; nav_heading = 0; nav_radius = DEFAULT_CIRCLE_RADIUS; nav_throttle = 0; nav_climb = 0; nav_leg_progress = 0; nav_leg_length = 1; too_far_from_home = FALSE; dist2_to_home = 0; dist2_to_wp = 0; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status); register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved); #endif }
void ins_update_gps(void) { if (gps_state.fix == GPS_FIX_3D) { if (!ins.ltp_initialised) { ltp_def_from_ecef_i(&ins.ltp_def, &gps_state.ecef_pos); ins.ltp_initialised = TRUE; } ned_of_ecef_point_i(&ins.gps_pos_cm_ned, &ins.ltp_def, &gps_state.ecef_pos); ned_of_ecef_vect_i(&ins.gps_speed_cm_s_ned, &ins.ltp_def, &gps_state.ecef_speed); #ifdef USE_HFF b2ins_update_gps(); VECT2_SDIV(ins.ltp_pos, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)), b2ins_pos_ltp); VECT2_SDIV(ins.ltp_speed, (1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)), b2ins_speed_ltp); #else INT32_VECT3_SCALE_2(b2ins_meas_gps_pos_ned, ins.gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, ins.gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); VECT3_COPY(ins.ltp_pos, b2ins_meas_gps_pos_ned); VECT3_COPY(ins.ltp_speed, b2ins_meas_gps_speed_ned); #endif INT32_VECT3_ENU_OF_NED(ins.enu_pos, ins.ltp_pos); INT32_VECT3_ENU_OF_NED(ins.enu_speed, ins.ltp_speed); INT32_VECT3_ENU_OF_NED(ins.enu_accel, ins.ltp_accel); } }
void imu_aspirin_event(void) { adxl345_spi_event(&imu_aspirin.acc_adxl); if (imu_aspirin.acc_adxl.data_available) { VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect); imu_aspirin.acc_adxl.data_available = FALSE; imu_aspirin.accel_valid = TRUE; } /* If the itg3200 I2C transaction has succeeded: convert the data */ itg3200_event(&imu_aspirin.gyro_itg); if (imu_aspirin.gyro_itg.data_available) { RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates); imu_aspirin.gyro_itg.data_available = FALSE; imu_aspirin.gyro_valid = TRUE; } /* HMC58XX event task */ hmc58xx_event(&imu_aspirin.mag_hmc); if (imu_aspirin.mag_hmc.data_available) { #ifdef IMU_ASPIRIN_VERSION_1_0 VECT3_COPY(imu.mag_unscaled, imu_aspirin.mag_hmc.data.vect); #else // aspirin 1.5 with hmc5883 imu.mag_unscaled.x = imu_aspirin.mag_hmc.data.vect.y; imu.mag_unscaled.y = -imu_aspirin.mag_hmc.data.vect.x; imu.mag_unscaled.z = imu_aspirin.mag_hmc.data.vect.z; #endif imu_aspirin.mag_hmc.data_available = FALSE; imu_aspirin.mag_valid = TRUE; } }
void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) { if (time < gps->next_update) return; /* * simulate speed sensor */ struct DoubleVect3 cur_speed_reading; VECT3_COPY(cur_speed_reading, fdm.ecef_ecef_vel); /* add a gaussian noise */ double_vect3_add_gaussian_noise(&cur_speed_reading, &gps->speed_noise_std_dev); /* store that for later and retrieve a previously stored data */ UpdateSensorLatency(time, &cur_speed_reading, &gps->speed_history, gps->speed_latency, &gps->ecef_vel); /* * simulate position sensor */ /* compute gps error readings */ struct DoubleVect3 pos_error; VECT3_COPY(pos_error, gps->pos_bias_initial); /* add a gaussian noise */ double_vect3_add_gaussian_noise(&pos_error, &gps->pos_noise_std_dev); /* update random walk bias and add it to error*/ double_vect3_update_random_walk(&gps->pos_bias_random_walk_value, &gps->pos_bias_random_walk_std_dev, NPS_GPS_DT, 5.); VECT3_ADD(pos_error, gps->pos_bias_random_walk_value); /* add error to current pos reading */ struct DoubleVect3 cur_pos_reading; VECT3_COPY(cur_pos_reading, fdm.ecef_pos); VECT3_ADD(cur_pos_reading, pos_error); /* store that for later and retrieve a previously stored data */ UpdateSensorLatency(time, &cur_pos_reading, &gps->pos_history, gps->pos_latency, &gps->ecef_pos); /* * simulate lla pos */ /* convert current ecef reading to lla */ struct LlaCoor_d cur_lla_reading; lla_of_ecef_d(&cur_lla_reading, (EcefCoor_d*) &cur_pos_reading); /* store that for later and retrieve a previously stored data */ UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos); double cur_hmsl_reading = fdm.hmsl; UpdateSensorLatency_Single(time, &cur_hmsl_reading, &gps->hmsl_history, gps->pos_latency, &gps->hmsl); gps->next_update += NPS_GPS_DT; gps->data_available = TRUE; }
void sim_overwrite_ins(void) { struct NedCoor_f ltp_pos; VECT3_COPY(ltp_pos, fdm.ltpprz_pos); stateSetPositionNed_f(<p_pos); struct NedCoor_f ltp_speed; VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel); stateSetSpeedNed_f(<p_speed); struct NedCoor_f ltp_accel; VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel); stateSetAccelNed_f(<p_accel); }
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 nav_init_stage(void) { VECT3_COPY(nav_last_point, *stateGetPositionEnu_i()); stage_time = 0; nav_circle_radians = 0; //horizontal_mode = HORIZONTAL_MODE_WAYPOINT; }
/** * 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_navstik_event(void) { uint32_t now_ts = get_sys_time_usec(); /* MPU-60x0 event taks */ mpu60x0_i2c_event(&imu_navstik.mpu); if (imu_navstik.mpu.data_available) { /* default orientation as should be printed on the pcb, z-down, ICs down */ RATES_COPY(imu.gyro_unscaled, imu_navstik.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_navstik.mpu.data_accel.vect); imu_navstik.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_navstik.hmc); if (imu_navstik.hmc.data_available) { imu.mag_unscaled.x = imu_navstik.hmc.data.vect.y; imu.mag_unscaled.y = -imu_navstik.hmc.data.vect.x; imu.mag_unscaled.z = imu_navstik.hmc.data.vect.z; imu_navstik.hmc.data_available = false; imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); } }
void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct DoubleRMat* body_to_imu) { if (time < gyro->next_update) return; /* transform body rates to IMU frame */ struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_inertial_rotvel); struct DoubleVect3 rate_imu; MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body ); /* compute gyros readings */ MAT33_VECT3_MUL(gyro->value, gyro->sensitivity, rate_imu); VECT3_ADD(gyro->value, gyro->neutral); /* compute gyro error readings */ struct DoubleVect3 gyro_error; VECT3_COPY(gyro_error, gyro->bias_initial); double_vect3_add_gaussian_noise(&gyro_error, &gyro->noise_std_dev); double_vect3_update_random_walk(&gyro->bias_random_walk_value, &gyro->bias_random_walk_std_dev, NPS_GYRO_DT, 5.); VECT3_ADD(gyro_error, gyro->bias_random_walk_value); struct DoubleVect3 gain = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR}; VECT3_EW_MUL(gyro_error, gyro_error, gain); VECT3_ADD(gyro->value, gyro_error); /* round signal to account for adc discretisation */ DOUBLE_VECT3_ROUND(gyro->value); /* saturate */ VECT3_BOUND_CUBE(gyro->value, gyro->min, gyro->max); gyro->next_update += NPS_GYRO_DT; gyro->data_available = TRUE; }
void imu_mpu_hmc_event(void) { uint32_t now_ts = get_sys_time_usec(); mpu60x0_spi_event(&imu_mpu_hmc.mpu); if (imu_mpu_hmc.mpu.data_available) { RATES_COPY(imu.gyro_unscaled, imu_mpu_hmc.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_mpu_hmc.mpu.data_accel.vect); imu_mpu_hmc.mpu.data_available = false; imu_scale_gyro(&imu); imu_scale_accel(&imu); AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel); } /* HMC58XX event task */ hmc58xx_event(&imu_mpu_hmc.hmc); if (imu_mpu_hmc.hmc.data_available) { /* mag rotated by 90deg around z axis relative to MPU */ imu.mag_unscaled.x = imu_mpu_hmc.hmc.data.vect.y; imu.mag_unscaled.y = -imu_mpu_hmc.hmc.data.vect.x; imu.mag_unscaled.z = imu_mpu_hmc.hmc.data.vect.z; imu_mpu_hmc.hmc.data_available = false; imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag); } }
void mag_hmc58xx_module_event(void) { hmc58xx_event(&mag_hmc58xx); if (mag_hmc58xx.data_available) { #if MODULE_HMC58XX_UPDATE_AHRS // current timestamp uint32_t now_ts = get_sys_time_usec(); // set channel order struct Int32Vect3 mag = { (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]), (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]), (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z]) }; // unscaled vector VECT3_COPY(imu.mag_unscaled, mag); // scale vector imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag); #endif #if MODULE_HMC58XX_SYNC_SEND mag_hmc58xx_report(); #endif #if MODULE_HMC58XX_UPDATE_AHRS || MODULE_HMC58XX_SYNC_SEND mag_hmc58xx.data_available = FALSE; #endif } }
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); 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 IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); #endif VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER); VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled); VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1)); VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered); 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 ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) { /* store the origin of the tangeant plane */ VECT3_COPY(def->ecef, *ecef); /* compute the lla representation of the origin */ lla_of_ecef_i(&def->lla, &def->ecef); /* store the rotation matrix */ #if 1 int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC)); int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC)); #else int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC)); int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC)); int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC)); #endif def->ltp_of_ecef.m[0] = -sin_lon; def->ltp_of_ecef.m[1] = cos_lon; def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */ def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[5] = cos_lat; def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC); def->ltp_of_ecef.m[8] = sin_lat; }
void imu_mpu9250_event(void) { uint32_t now_ts = get_sys_time_usec(); // If the MPU9250 I2C transaction has succeeded: convert the data mpu9250_i2c_event(&imu_mpu9250.mpu); if (imu_mpu9250.mpu.data_available) { // set channel order struct Int32Vect3 accel = { IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]), IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]), IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z]) }; struct Int32Rates rates = { IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]), IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]), IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z]) }; // unscaled vector VECT3_COPY(imu.accel_unscaled, accel); RATES_COPY(imu.gyro_unscaled, rates); imu_mpu9250.mpu.data_available = false; imu_scale_gyro(&imu); imu_scale_accel(&imu); AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel); } #if IMU_MPU9250_READ_MAG // Test if mag data are updated if (imu_mpu9250.mpu.akm.data_available) { struct Int32Vect3 mag = { IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]), IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]), -IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z]) }; VECT3_COPY(imu.mag_unscaled, mag); imu_mpu9250.mpu.akm.data_available = false; imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag); } #endif }
void imu_mpu9250_event(void) { uint32_t now_ts = get_sys_time_usec(); // If the MPU9250 SPI transaction has succeeded: convert the data mpu9250_spi_event(&imu_mpu9250.mpu); if (imu_mpu9250.mpu.data_available) { // set channel order struct Int32Vect3 accel = { IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]), IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]), IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z]) }; struct Int32Rates rates = { IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]), IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]), IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z]) }; // unscaled vector VECT3_COPY(imu.accel_unscaled, accel); RATES_COPY(imu.gyro_unscaled, rates); #if IMU_MPU9250_READ_MAG if (!bit_is_set(imu_mpu9250.mpu.data_ext[6], 3)) { //mag valid just HOFL == 0 /** FIXME: assumes that we get new mag data each time instead of reading drdy bit */ struct Int32Vect3 mag; mag.x = (IMU_MPU9250_X_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Y); mag.y = (IMU_MPU9250_Y_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_X); mag.z = -(IMU_MPU9250_Z_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Z); VECT3_COPY(imu.mag_unscaled, mag); imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag); } #endif imu_mpu9250.mpu.data_available = false; imu_scale_gyro(&imu); imu_scale_accel(&imu); AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel); } }
bool_t nav_survey_rectangle_rotorcraft_setup(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so) { rectangle_survey_sweep_num = 0; nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2)); survey_orientation = so; if (survey_orientation == NS) { if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) < fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) { survey_from.x = survey_to.x = nav_survey_west + grid / 4.; } else { survey_from.x = survey_to.x = nav_survey_east - grid / 4.; grid = -grid; } if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) > fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) { survey_to.y = nav_survey_south; survey_from.y = nav_survey_north; } else { survey_from.y = nav_survey_south; survey_to.y = nav_survey_north; } } else { /* survey_orientation == WE */ if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) < fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) { survey_from.y = survey_to.y = nav_survey_south + grid / 4.; } else { survey_from.y = survey_to.y = nav_survey_north - grid / 4.; grid = -grid; } if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) > fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) { survey_to.x = nav_survey_west; survey_from.x = nav_survey_east; } else { survey_from.x = nav_survey_west; survey_to.x = nav_survey_east; } } nav_survey_shift = grid; survey_uturn = FALSE; nav_survey_rectangle_active = FALSE; //go to start position ENU_BFP_OF_REAL(survey_from_i, survey_from); horizontal_mode = HORIZONTAL_MODE_ROUTE; VECT3_COPY(navigation_target, survey_from_i); LINE_STOP_FUNCTION; NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.); if (survey_orientation == NS) { nav_set_heading_deg(0); } else { nav_set_heading_deg(90); } return FALSE; }
void imu_drotek2_event(void) { uint32_t now_ts = get_sys_time_usec(); // If the MPU6050 I2C transaction has succeeded: convert the data mpu60x0_i2c_event(&imu_drotek2.mpu); if (imu_drotek2.mpu.data_available) { #if IMU_DROTEK_2_ORIENTATION_IC_UP /* change orientation, so if ICs face up, z-axis is down */ imu.gyro_unscaled.p = imu_drotek2.mpu.data_rates.rates.p; imu.gyro_unscaled.q = -imu_drotek2.mpu.data_rates.rates.q; imu.gyro_unscaled.r = -imu_drotek2.mpu.data_rates.rates.r; imu.accel_unscaled.x = imu_drotek2.mpu.data_accel.vect.x; imu.accel_unscaled.y = -imu_drotek2.mpu.data_accel.vect.y; imu.accel_unscaled.z = -imu_drotek2.mpu.data_accel.vect.z; #else /* default orientation as should be printed on the pcb, z-down, ICs down */ RATES_COPY(imu.gyro_unscaled, imu_drotek2.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_drotek2.mpu.data_accel.vect); #endif imu_drotek2.mpu.data_available = false; imu_scale_gyro(&imu); imu_scale_accel(&imu); AbiSendMsgIMU_GYRO_INT32(IMU_DROTEK_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_DROTEK_ID, now_ts, &imu.accel); } /* HMC58XX event task */ hmc58xx_event(&imu_drotek2.hmc); if (imu_drotek2.hmc.data_available) { #if IMU_DROTEK_2_ORIENTATION_IC_UP imu.mag_unscaled.x = imu_drotek2.hmc.data.vect.x; imu.mag_unscaled.y = -imu_drotek2.hmc.data.vect.y; imu.mag_unscaled.z = -imu_drotek2.hmc.data.vect.z; #else VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect); #endif imu_drotek2.hmc.data_available = false; imu_scale_mag(&imu); AbiSendMsgIMU_MAG_INT32(IMU_DROTEK_ID, now_ts, &imu.mag); } }
void WEAK imu_scale_accel(struct Imu* _imu) { VECT3_COPY(_imu->accel_prev, _imu->accel); _imu->accel.x = ((_imu->accel_unscaled.x - _imu->accel_neutral.x) * IMU_ACCEL_X_SIGN * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN; _imu->accel.y = ((_imu->accel_unscaled.y - _imu->accel_neutral.y) * IMU_ACCEL_Y_SIGN * IMU_ACCEL_Y_SENS_NUM) / IMU_ACCEL_Y_SENS_DEN; _imu->accel.z = ((_imu->accel_unscaled.z - _imu->accel_neutral.z) * IMU_ACCEL_Z_SIGN * IMU_ACCEL_Z_SENS_NUM) / IMU_ACCEL_Z_SENS_DEN; }
void WEAK imu_scale_accel(struct Imu *_imu) { #ifdef TREF VECT3_COPY(_imu->accel_prev, _imu->accel); _imu->accel.x = ((_imu->accel_unscaled.x + (TREF-_imu->temp_unscaled)*DXA - _imu->accel_neutral.x) * IMU_ACCEL_X_SIGN * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN; _imu->accel.y = ((_imu->accel_unscaled.y + (TREF-_imu->temp_unscaled)*DYA - _imu->accel_neutral.y) * IMU_ACCEL_Y_SIGN * IMU_ACCEL_Y_SENS_NUM) / IMU_ACCEL_Y_SENS_DEN; _imu->accel.z = ((_imu->accel_unscaled.z + (TREF-_imu->temp_unscaled)*DZA - _imu->accel_neutral.z) * IMU_ACCEL_Z_SIGN * IMU_ACCEL_Z_SENS_NUM) / IMU_ACCEL_Z_SENS_DEN; #else VECT3_COPY(_imu->accel_prev, _imu->accel); _imu->accel.x = ((_imu->accel_unscaled.x - _imu->accel_neutral.x) * IMU_ACCEL_X_SIGN * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN; _imu->accel.y = ((_imu->accel_unscaled.y - _imu->accel_neutral.y) * IMU_ACCEL_Y_SIGN * IMU_ACCEL_Y_SENS_NUM) / IMU_ACCEL_Y_SENS_DEN; _imu->accel.z = ((_imu->accel_unscaled.z - _imu->accel_neutral.z) * IMU_ACCEL_Z_SIGN * IMU_ACCEL_Z_SENS_NUM) / IMU_ACCEL_Z_SENS_DEN; #endif // TREF }
void WEAK ins_module_propagate(struct Int32Vect3 *accel, float dt __attribute__((unused))) { /* untilt accels */ struct Int32Vect3 accel_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_module.body_to_imu); int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel); struct Int32Vect3 accel_meas_ltp; int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body); VECT3_COPY(ins_module.ltp_accel, accel_meas_ltp); }
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef) { /* store the origin of the tangent plane */ VECT3_COPY(def->ecef, *ecef); /* compute the lla representation of the origin */ lla_of_ecef_i(&def->lla, &def->ecef); /* store the rotation matrix */ ltp_of_ecef_rmat_from_lla_i(&def->ltp_of_ecef, &def->lla); }
void imu_mpu_spi_event(void) { mpu60x0_spi_event(&imu_mpu_spi.mpu); if (imu_mpu_spi.mpu.data_available) { RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect); imu_mpu_spi.mpu.data_available = FALSE; imu_mpu_spi.gyro_valid = TRUE; imu_mpu_spi.accel_valid = TRUE; } }
inline static void h_ctl_cl_loop(void) { #if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR #if (!defined SITL || defined USE_NPS) struct Int32Vect3 accel_meas_body, accel_ned; struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i(); struct NedCoor_i *accel_tmp = stateGetAccelNed_i(); VECT3_COPY(accel_ned, (*accel_tmp)); accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f); int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned); float nz = -ACCEL_FLOAT_OF_BFP(accel_meas_body.z) / 9.81f; // max load factor to be taken into acount // to prevent negative flap movement du to negative acceleration Bound(nz, 1.f, 2.f); #else float nz = 1.f; #endif #endif // Compute a corrected airspeed corresponding to the current load factor nz // with Cz the lift coef at 1g, Czn the lift coef at n g, both at the same speed V, // the corrected airspeed Vn is so that nz = Czn/Cz = V^2 / Vn^2, // thus Vn = V / sqrt(nz) #if H_CTL_CL_LOOP_USE_AIRSPEED_SETPOINT float corrected_airspeed = v_ctl_auto_airspeed_setpoint; #else float corrected_airspeed = stateGetAirspeed_f(); #endif #if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR corrected_airspeed /= sqrtf(nz); #endif Bound(corrected_airspeed, STALL_AIRSPEED, RACE_AIRSPEED); float cmd = 0.f; // deadband around NOMINAL_AIRSPEED, rest linear if (corrected_airspeed > NOMINAL_AIRSPEED + H_CTL_CL_DEADBAND) { cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_RACE - H_CTL_CL_FLAPS_NOMINAL) / (RACE_AIRSPEED - NOMINAL_AIRSPEED); } else if (corrected_airspeed < NOMINAL_AIRSPEED - H_CTL_CL_DEADBAND) { cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_STALL - H_CTL_CL_FLAPS_NOMINAL) / (STALL_AIRSPEED - NOMINAL_AIRSPEED); } // no control in manual mode if (pprz_mode == PPRZ_MODE_MANUAL) { cmd = 0.f; } // bound max flap angle Bound(cmd, H_CTL_CL_FLAPS_RACE, H_CTL_CL_FLAPS_STALL); // from percent to pprz cmd = cmd * MAX_PPRZ; h_ctl_flaps_setpoint = TRIM_PPRZ(cmd); }
/*************************************************************************************** *decode the gps data packet ***************************************************************************************/ static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer) { /* gps velocity (1e2 m/s to m/s */ struct NedCoor_d vel; vel.x = (double)LongOfBuf(buffer, 3) * 1.0e-2; vel.y = (double)LongOfBuf(buffer, 7) * 1.0e-2; vel.z = (double)LongOfBuf(buffer, 11) * 1.0e-2; fdm->ltp_ecef_vel = vel; ecef_of_ned_vect_d(&fdm->ecef_ecef_vel, <pdef, &vel); /* No airspeed from CRRCSIM? * use ground speed for now, since we also don't know wind */ struct DoubleVect3 ltp_airspeed; VECT3_COPY(ltp_airspeed, vel); fdm.airspeed = double_vect3_norm(<p_airspeed); /* gps position (1e7 deg to rad and 1e3 m to m) */ struct LlaCoor_d pos; pos.lon = (double)LongOfBuf(buffer, 15) * 1.74533e-9; pos.lat = (double)LongOfBuf(buffer, 19) * 1.74533e-9; pos.alt = (double)LongOfBuf(buffer, 23) * 1.0e-3; pos.lat += ltpdef.lla.lat; pos.lon += ltpdef.lla.lon; pos.alt += ltpdef.lla.alt; fdm->lla_pos = pos; ecef_of_lla_d(&fdm->ecef_pos, &pos); fdm->hmsl = pos.alt - NAV_MSL0 / 1000.; fdm->pressure = pprz_isa_pressure_of_altitude(fdm->hmsl); /* gps time */ fdm->time = (double)UShortOfBuf(buffer, 27); /* in LTP pprz */ ned_of_ecef_point_d(&fdm->ltpprz_pos, <pdef, &fdm->ecef_pos); fdm->lla_pos_pprz = pos; ned_of_ecef_vect_d(&fdm->ltpprz_ecef_vel, <pdef, &fdm->ecef_ecef_vel); #if NPS_CRRCSIM_DEBUG printf("decode gps | pos %f %f %f | vel %f %f %f | time %f\n", 57.3 * fdm->lla_pos.lat, 57.3 * fdm->lla_pos.lon, fdm->lla_pos.alt, fdm->ltp_ecef_vel.x, fdm->ltp_ecef_vel.y, fdm->ltp_ecef_vel.z, fdm->time); #endif }
inline static void h_ctl_yaw_loop(void) { #if H_CTL_YAW_TRIM_NY // Actual Acceleration from IMU: #if (!defined SITL || defined USE_NPS) struct Int32Vect3 accel_meas_body, accel_ned; struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i(); struct NedCoor_i *accel_tmp = stateGetAccelNed_i(); VECT3_COPY(accel_ned, (*accel_tmp)); accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f); int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned); float ny = -ACCEL_FLOAT_OF_BFP(accel_meas_body.y) / 9.81f; // Lateral load factor (in g) #else float ny = 0.f; #endif if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { h_ctl_yaw_ny_sum_err = 0.; } else { if (h_ctl_yaw_ny_igain > 0.) { // only update when: phi<60degrees and ny<2g if (fabsf(stateGetNedToBodyEulers_f()->phi) < 1.05 && fabsf(ny) < 2.) { h_ctl_yaw_ny_sum_err += ny * H_CTL_REF_DT; // max half rudder deflection for trim BoundAbs(h_ctl_yaw_ny_sum_err, MAX_PPRZ / (2. * h_ctl_yaw_ny_igain)); } } else { h_ctl_yaw_ny_sum_err = 0.; } } #endif #ifdef USE_AIRSPEED float Vo = stateGetAirspeed_f(); Bound(Vo, STALL_AIRSPEED, RACE_AIRSPEED); #else float Vo = NOMINAL_AIRSPEED; #endif h_ctl_ref.yaw_rate = h_ctl_yaw_rate_setpoint // set by RC + 9.81f / Vo * sinf(h_ctl_roll_setpoint); // for turns float d_err = h_ctl_ref.yaw_rate - stateGetBodyRates_f()->r; float cmd = + h_ctl_yaw_dgain * d_err #if H_CTL_YAW_TRIM_NY + h_ctl_yaw_ny_igain * h_ctl_yaw_ny_sum_err #endif ; cmd /= airspeed_ratio2; h_ctl_rudder_setpoint = TRIM_PPRZ(cmd); }
void nav_init(void) { // convert to const struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU; // init int32 waypoints uint8_t i = 0; for (i = 0; i < nb_waypoint; i++) { waypoints[i].x = POS_BFP_OF_REAL(wp_tmp_float[i].x); waypoints[i].y = POS_BFP_OF_REAL(wp_tmp_float[i].y); waypoints[i].z = POS_BFP_OF_REAL(wp_tmp_float[i].z); } nav_block = 0; nav_stage = 0; nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT); nav_flight_altitude = nav_altitude; flight_altitude = SECURITY_ALT; VECT3_COPY(navigation_target, waypoints[WP_HOME]); VECT3_COPY(navigation_carrot, waypoints[WP_HOME]); horizontal_mode = HORIZONTAL_MODE_WAYPOINT; vertical_mode = VERTICAL_MODE_ALT; nav_roll = 0; nav_pitch = 0; nav_heading = 0; nav_radius = DEFAULT_CIRCLE_RADIUS; nav_throttle = 0; nav_climb = 0; nav_leg_progress = 0; nav_leg_length = 1; too_far_from_home = FALSE; dist2_to_home = 0; dist2_to_wp = 0; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status); register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved); #endif }
static void test_2(void) { struct Int32Vect3 v1 = { 5000, 5000, 5000 }; DISPLAY_INT32_VECT3("v1", v1); struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)}; DISPLAY_FLOAT_EULERS("euler_f", euler_f); struct Int32Eulers euler_i; EULERS_BFP_OF_REAL(euler_i, euler_f); DISPLAY_INT32_EULERS("euler_i", euler_i); struct Int32Quat quat_i; int32_quat_of_eulers(&quat_i, &euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); int32_quat_normalize(&quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; int32_quat_vmult(&v2, &quat_i, &v1); DISPLAY_INT32_VECT3("v2", v2); struct Int32RMat rmat_i; int32_rmat_of_quat(&rmat_i, &quat_i); DISPLAY_INT32_RMAT("rmat_i", rmat_i); struct Int32Vect3 v3; INT32_RMAT_VMULT(v3, rmat_i, v1); DISPLAY_INT32_VECT3("v3", v3); struct Int32RMat rmat_i2; int32_rmat_of_eulers(&rmat_i2, &euler_i); DISPLAY_INT32_RMAT("rmat_i2", rmat_i2); struct Int32Vect3 v4; INT32_RMAT_VMULT(v4, rmat_i2, v1); DISPLAY_INT32_VECT3("v4", v4); struct FloatQuat quat_f; float_quat_of_eulers(&quat_f, &euler_f); DISPLAY_FLOAT_QUAT("quat_f", quat_f); struct FloatVect3 v5; VECT3_COPY(v5, v1); DISPLAY_FLOAT_VECT3("v5", v5); struct FloatVect3 v6; float_quat_vmult(&v6, &quat_f, &v5); DISPLAY_FLOAT_VECT3("v6", v6); }
void imu_mpu9250_event(void) { // If the MPU9250 I2C transaction has succeeded: convert the data mpu9250_i2c_event(&imu_mpu9250.mpu); if (imu_mpu9250.mpu.data_available) { // set channel order struct Int32Vect3 accel = { (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]), (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]), (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z]) }; struct Int32Rates rates = { (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]), (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]), (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z]) }; // unscaled vector VECT3_COPY(imu.accel_unscaled, accel); RATES_COPY(imu.gyro_unscaled, rates); imu_mpu9250.mpu.data_available = FALSE; imu_mpu9250.gyro_valid = TRUE; imu_mpu9250.accel_valid = TRUE; } // Test if mag data are updated if (imu_mpu9250.mpu.akm.data_available) { struct Int32Vect3 mag = { (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]), (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]), -(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z]) }; VECT3_COPY(imu.mag_unscaled, mag); imu_mpu9250.mag_valid = TRUE; imu_mpu9250.mpu.akm.data_available = FALSE; } }
static void test_2(void) { struct Int32Vect3 v1 = { 5000, 5000, 5000 }; DISPLAY_INT32_VECT3("v1", v1); struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)}; DISPLAY_FLOAT_EULERS("euler_f", euler_f); struct Int32Eulers euler_i; EULERS_BFP_OF_REAL(euler_i, euler_f); DISPLAY_INT32_EULERS("euler_i", euler_i); struct Int32Quat quat_i; INT32_QUAT_OF_EULERS(quat_i, euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); INT32_QUAT_NORMALIZE(quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; INT32_QUAT_VMULT(v2, quat_i, v1); DISPLAY_INT32_VECT3("v2", v2); struct Int32RMat rmat_i; INT32_RMAT_OF_QUAT(rmat_i, quat_i); DISPLAY_INT32_RMAT("rmat_i", rmat_i); struct Int32Vect3 v3; INT32_RMAT_VMULT(v3, rmat_i, v1); DISPLAY_INT32_VECT3("v3", v3); struct Int32RMat rmat_i2; INT32_RMAT_OF_EULERS(rmat_i2, euler_i); DISPLAY_INT32_RMAT("rmat_i2", rmat_i2); struct Int32Vect3 v4; INT32_RMAT_VMULT(v4, rmat_i2, v1); DISPLAY_INT32_VECT3("v4", v4); struct FloatQuat quat_f; FLOAT_QUAT_OF_EULERS(quat_f, euler_f); DISPLAY_FLOAT_QUAT("quat_f", quat_f); struct FloatVect3 v5; VECT3_COPY(v5, v1); DISPLAY_FLOAT_VECT3("v5", v5); struct FloatVect3 v6; FLOAT_QUAT_VMULT(v6, quat_f, v5); DISPLAY_FLOAT_VECT3("v6", v6); }
void imu_mpu_spi_event(void) { mpu60x0_spi_event(&imu_mpu_spi.mpu); if (imu_mpu_spi.mpu.data_available) { uint32_t now_ts = get_sys_time_usec(); RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates); VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect); imu_mpu_spi.mpu.data_available = false; imu_scale_gyro(&imu); imu_scale_accel(&imu); AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_ID, now_ts, &imu.accel); } }