/* init state and measurements */ static inline void init_invariant_state(void) { // init state float_quat_identity(&ahrs_float_inv.state.quat); FLOAT_RATES_ZERO(ahrs_float_inv.state.bias); ahrs_float_inv.state.as = 1.0f; ahrs_float_inv.state.cs = 1.0f; // init measures FLOAT_VECT3_ZERO(ahrs_float_inv.meas.accel); FLOAT_VECT3_ZERO(ahrs_float_inv.meas.mag); }
void guidance_indi_enter(void) { filt_accelzbody = 0; filt_accelzbodyd = 0; filt_accelzbodydd = 0; roll_filt = 0; roll_filtd = 0; roll_filtdd = 0; pitch_filt = 0; pitch_filtd = 0; pitch_filtdd = 0; FLOAT_VECT3_ZERO(filt_accel_ned); FLOAT_VECT3_ZERO(filt_accel_ned_d); FLOAT_VECT3_ZERO(filt_accel_ned_dd); }
/** * 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 }
/* init state and measurements */ static inline void init_invariant_state(void) { // init state FLOAT_QUAT_ZERO(ins_impl.state.quat); FLOAT_RATES_ZERO(ins_impl.state.bias); FLOAT_VECT3_ZERO(ins_impl.state.pos); FLOAT_VECT3_ZERO(ins_impl.state.speed); ins_impl.state.as = 1.0f; ins_impl.state.hb = 0.0f; // init measures FLOAT_VECT3_ZERO(ins_impl.meas.pos_gps); FLOAT_VECT3_ZERO(ins_impl.meas.speed_gps); ins_impl.meas.baro_alt = 0.0f; // init baro ins_baro_initialized = FALSE; }
void nps_atmosphere_init(void) { nps_atmosphere.qnh = NPS_QNH; FLOAT_VECT3_ZERO(nps_atmosphere.wind); nps_atmosphere_set_wind_speed(NPS_WIND_SPEED); nps_atmosphere_set_wind_dir(NPS_WIND_DIR); nps_atmosphere.turbulence_severity = NPS_TURBULENCE_SEVERITY; nps_atmosphere.last_world_env_req = 0.; }
void nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time) { FLOAT_VECT3_ZERO(gyro->value); gyro->resolution = NPS_GYRO_RESOLUTION; FLOAT_MAT33_DIAG(gyro->sensitivity, NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR); VECT3_ASSIGN(gyro->neutral, NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R); VECT3_ASSIGN(gyro->noise_std_dev, NPS_GYRO_NOISE_STD_DEV_P, NPS_GYRO_NOISE_STD_DEV_Q, NPS_GYRO_NOISE_STD_DEV_R); VECT3_ASSIGN(gyro->bias_initial, NPS_GYRO_BIAS_INITIAL_P, NPS_GYRO_BIAS_INITIAL_Q, NPS_GYRO_BIAS_INITIAL_R); VECT3_ASSIGN(gyro->bias_random_walk_std_dev, NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P, NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q, NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R); FLOAT_VECT3_ZERO(gyro->bias_random_walk_value); gyro->next_update = time; gyro->data_available = FALSE; }
void ArduIMU_init( void ) { FLOAT_EULERS_ZERO(arduimu_eulers); FLOAT_RATES_ZERO(arduimu_rates); FLOAT_VECT3_ZERO(arduimu_accel); ardu_ins_trans.status = I2CTransDone; ardu_gps_trans.status = I2CTransDone; ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; }
void nps_sensor_gps_init(struct NpsSensorGps* gps, double time) { FLOAT_VECT3_ZERO(gps->ecef_pos); FLOAT_VECT3_ZERO(gps->ecef_vel); gps->hmsl = 0.0; gps->pos_latency = NPS_GPS_POS_LATENCY; gps->speed_latency = NPS_GPS_SPEED_LATENCY; VECT3_ASSIGN(gps->pos_noise_std_dev, NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV); VECT3_ASSIGN(gps->speed_noise_std_dev, NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV); VECT3_ASSIGN(gps->pos_bias_initial, NPS_GPS_POS_BIAS_INITIAL_X, NPS_GPS_POS_BIAS_INITIAL_Y, NPS_GPS_POS_BIAS_INITIAL_Z); VECT3_ASSIGN(gps->pos_bias_random_walk_std_dev, NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X, NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y, NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z); FLOAT_VECT3_ZERO(gps->pos_bias_random_walk_value); gps->next_update = time; gps->data_available = FALSE; }
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; }
static void test_enu_of_ecef_int(void) { printf("\n--- enu_of_ecef int ---\n"); struct EcefCoor_f ref_coor_f = { 4624497.0 , 116475.0, 4376563.0}; struct LtpDef_f ltp_def_f; ltp_def_from_ecef_f(<p_def_f, &ref_coor_f); struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_f.x)), rint(CM_OF_M(ref_coor_f.y)), rint(CM_OF_M(ref_coor_f.z))}; printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z); struct LtpDef_i ltp_def_i; ltp_def_from_ecef_i(<p_def_i, &ref_coor_i); printf("lla0 : (%d %d %d) (%f,%f,%f)\n", ltp_def_i.lla.lat, ltp_def_i.lla.lon, ltp_def_i.lla.alt, DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)), DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)), M_OF_CM((double)ltp_def_i.lla.alt)); #define STEP 1000. #define RANGE 100000. double sum_err = 0; struct FloatVect3 max_err; FLOAT_VECT3_ZERO(max_err); struct FloatVect3 offset; for (offset.x=-RANGE; offset.x<=RANGE; offset.x+=STEP) { for (offset.y=-RANGE; offset.y<=RANGE; offset.y+=STEP) { for (offset.z=-RANGE; offset.z<=RANGE; offset.z+=STEP) { struct EcefCoor_f my_ecef_point_f = ref_coor_f; VECT3_ADD(my_ecef_point_f, offset); struct EnuCoor_f my_enu_point_f; enu_of_ecef_point_f(&my_enu_point_f, <p_def_f, &my_ecef_point_f); #if DEBUG printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n", my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z, my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z ); #endif struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)), rint(CM_OF_M(my_ecef_point_f.y)), rint(CM_OF_M(my_ecef_point_f.z))};; struct EnuCoor_i my_enu_point_i; enu_of_ecef_point_i(&my_enu_point_i, <p_def_i, &my_ecef_point_i); #if DEBUG // printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z); printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n", M_OF_CM((double)my_ecef_point_i.x), M_OF_CM((double)my_ecef_point_i.y), M_OF_CM((double)my_ecef_point_i.z), M_OF_CM((double)my_enu_point_i.x), M_OF_CM((double)my_enu_point_i.y), M_OF_CM((double)my_enu_point_i.z)); #endif float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x); if (fabs(ex) > max_err.x) max_err.x = fabs(ex); float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y); if (fabs(ey) > max_err.y) max_err.y = fabs(ey); float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z); if (fabs(ez) > max_err.z) max_err.z = fabs(ez); sum_err += ex*ex + ey*ey + ez*ez; } } } double nb_samples = (2*RANGE / STEP + 1) * (2*RANGE / STEP + 1) * (2*RANGE / STEP + 1); printf("enu_of_ecef int/float comparison:\n"); printf("error max (%f,%f,%f) m\n", max_err.x, max_err.y, max_err.z ); printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples ); printf("\n"); }
void aos_init(int traj_nb) { aos.traj = &traj[traj_nb]; aos.time = 0; aos.dt = 1. / AHRS_PROPAGATE_FREQUENCY; aos.traj->ts = 0; aos.traj->ts = 1.; // default to one second /* default state */ EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler); RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); FLOAT_VECT3_ZERO(aos.ltp_pos); FLOAT_VECT3_ZERO(aos.ltp_vel); FLOAT_VECT3_ZERO(aos.ltp_accel); FLOAT_VECT3_ZERO(aos.ltp_jerk); aos.traj->init_fun(); imu_init(); ahrs_init(); #ifdef PERFECT_SENSORS RATES_ASSIGN(aos.gyro_bias, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.); aos.heading_noise = 0.; #else RATES_ASSIGN(aos.gyro_bias, RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.)); RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.)); VECT3_ASSIGN(aos.accel_noise, .5, .5, .5); aos.heading_noise = RadOfDeg(3.); #endif #ifdef FORCE_ALIGNEMENT // DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat); aos_compute_sensors(); // DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias); // DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates); VECT3_COPY(ahrs_aligner.lp_accel, imu.accel); VECT3_COPY(ahrs_aligner.lp_mag, imu.mag); RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro); // DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro); ahrs_align(); // DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias); #endif #ifdef DISABLE_ALIGNEMENT printf("# DISABLE_ALIGNEMENT\n"); #endif #ifdef DISABLE_PROPAGATE printf("# DISABLE_PROPAGATE\n"); #endif #ifdef DISABLE_ACCEL_UPDATE printf("# DISABLE_ACCEL_UPDATE\n"); #endif #ifdef DISABLE_MAG_UPDATE printf("# DISABLE_MAG_UPDATE\n"); #endif printf("# AHRS_TYPE %s\n", ahrs_type_str[AHRS_TYPE]); printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n"); #endif #if AHRS_MAG_UPDATE_YAW_ONLY printf("# AHRS_MAG_UPDATE_YAW_ONLY\n"); #endif #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n"); #endif #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n"); #endif #ifdef PERFECT_SENSORS printf("# PERFECT_SENSORS\n"); #endif #if AHRS_USE_GPS_HEADING printf("# AHRS_USE_GPS_HEADING\n"); #endif #if USE_AHRS_GPS_ACCELERATIONS printf("# USE_AHRS_GPS_ACCELERATIONS\n"); #endif printf("# tajectory : %s\n", aos.traj->name); };