static void handle_ins_msg(void) { update_state_interface(); if (xsens.new_attitude) { new_ins_attitude = true; xsens.new_attitude = false; } #if USE_GPS_XSENS if (xsens.gps_available) { // Horizontal speed float fspeed = FLOAT_VECT2_NORM(xsens.vel); if (xsens.gps.fix != GPS_FIX_3D) { fspeed = 0; } xsens.gps.gspeed = fspeed * 100.; xsens.gps.speed_3d = float_vect3_norm(&xsens.vel) * 100; float fcourse = atan2f(xsens.vel.y, xsens.vel.x); xsens.gps.course = fcourse * 1e7; SetBit(xsens.gps.valid_fields, GPS_VALID_COURSE_BIT); gps_xsens_publish(); xsens.gps_available = false; } #endif // USE_GPS_XSENS }
void handle_ins_msg(void) { #if USE_INS_MODULE update_state_interface(); #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 // 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 }
void handle_ins_msg(void) { #if USE_INS_MODULE update_state_interface(); #endif #if USE_GPS_XSENS // 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 }