/* 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);
};
