void ins_int_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_i_from_flightplan(&ins_int.ltp_def);
ins_int.ltp_initialized = true;
#else
ins_int.ltp_initialized = false;
#endif
/* we haven't had any measurement updates yet, so set the counter to max */
ins_int.propagation_cnt = INS_MAX_PROPAGATION_STEPS;
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_INT_BARO_ID, &baro_ev, baro_cb);
ins_int.baro_initialized = false;
#if USE_SONAR
ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
// Bind to AGL message
AbiBindMsgAGL(INS_INT_SONAR_ID, &sonar_ev, sonar_cb);
#endif
ins_int.vf_reset = false;
ins_int.hf_realign = false;
/* init vertical and horizontal filters */
vff_init_zero();
#if USE_HFF
hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_int.ltp_pos);
INT32_VECT3_ZERO(ins_int.ltp_speed);
INT32_VECT3_ZERO(ins_int.ltp_accel);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS, send_ins);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref);
#endif
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb);
}
static inline void main_init(void)
{
mcu_init();
sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
imu_init();
mcu_int_enable();
downlink_init();
AbiBindMsgIMU_GYRO_INT32(ABI_BROADCAST, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(ABI_BROADCAST, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG_INT32(ABI_BROADCAST, &mag_ev, mag_cb);
}
void ins_module_wrapper_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_i_from_flightplan(&ins_module.ltp_def);
ins_module.ltp_initialized = true;
#else
ins_module.ltp_initialized = false;
#endif
INT32_VECT3_ZERO(ins_module.ltp_pos);
INT32_VECT3_ZERO(ins_module.ltp_speed);
INT32_VECT3_ZERO(ins_module.ltp_accel);
ins_module_init();
// Bind to ABI messages
AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
void v_ctl_init(void)
{
/* mode */
v_ctl_mode = V_CTL_MODE_MANUAL;
v_ctl_speed_mode = V_CTL_SPEED_THROTTLE; //There is only one, added here to be universal in flightplan for different control modes
/* outer loop */
v_ctl_altitude_setpoint = 0.;
v_ctl_altitude_pre_climb = 0.;
v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
#ifdef V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH
v_ctl_auto_throttle_nominal_cruise_pitch = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH;
#else
v_ctl_auto_throttle_nominal_cruise_pitch = 0.;
#endif
v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
v_ctl_auto_airspeed_setpoint_slew = v_ctl_auto_airspeed_setpoint;
v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
v_ctl_max_acceleration = V_CTL_MAX_ACCELERATION;
/* inner loops */
v_ctl_climb_setpoint = 0.;
/* "auto throttle" inner loop parameters */
v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
v_ctl_auto_throttle_of_airspeed_pgain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN;
v_ctl_auto_throttle_of_airspeed_igain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_IGAIN;
v_ctl_auto_pitch_of_airspeed_pgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN;
v_ctl_auto_pitch_of_airspeed_igain = V_CTL_AUTO_PITCH_OF_AIRSPEED_IGAIN;
v_ctl_auto_pitch_of_airspeed_dgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_DGAIN;
#ifdef V_CTL_ENERGY_TOT_PGAIN
v_ctl_energy_total_pgain = V_CTL_ENERGY_TOT_PGAIN;
v_ctl_energy_total_igain = V_CTL_ENERGY_TOT_IGAIN;
v_ctl_energy_diff_pgain = V_CTL_ENERGY_DIFF_PGAIN;
v_ctl_energy_diff_igain = V_CTL_ENERGY_DIFF_IGAIN;
#else
v_ctl_energy_total_pgain = 0.;
v_ctl_energy_total_igain = 0.;
v_ctl_energy_diff_pgain = 0.;
v_ctl_energy_diff_igain = 0.;
#warning "V_CTL_ENERGY_TOT GAINS are not defined and set to 0"
#endif
#ifdef V_CTL_ALTITUDE_MAX_CLIMB
v_ctl_max_climb = V_CTL_ALTITUDE_MAX_CLIMB;
#else
v_ctl_max_climb = 2;
#warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
#endif
#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
v_ctl_auto_groundspeed_sum_err = 0.;
#endif
v_ctl_throttle_setpoint = 0;
float_quat_identity(&imu_to_body_quat);
AbiBindMsgIMU_ACCEL_INT32(V_CTL_ENERGY_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgBODY_TO_IMU_QUAT(V_CTL_ENERGY_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
