void gps_init(void)
{
gps.fix = GPS_FIX_NONE;
gps.week = 0;
gps.tow = 0;
gps.cacc = 0;
gps.last_3dfix_ticks = 0;
gps.last_3dfix_time = 0;
gps.last_msg_ticks = 0;
gps.last_msg_time = 0;
#ifdef GPS_POWER_GPIO
gpio_setup_output(GPS_POWER_GPIO);
GPS_POWER_GPIO_ON(GPS_POWER_GPIO);
#endif
#ifdef GPS_LED
LED_OFF(GPS_LED);
#endif
#ifdef GPS_TYPE_H
gps_impl_init();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "GPS", send_gps);
register_periodic_telemetry(DefaultPeriodic, "GPS_INT", send_gps_int);
register_periodic_telemetry(DefaultPeriodic, "GPS_LLA", send_gps_lla);
register_periodic_telemetry(DefaultPeriodic, "GPS_SOL", send_gps_sol);
register_periodic_telemetry(DefaultPeriodic, "SVINFO", send_svinfo);
#endif
}
static inline void main_init(void)
{
mcu_init();
sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
stateInit();
actuators_init();
imu_init();
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
ahrs_init();
settings_init();
mcu_int_enable();
downlink_init();
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
void stabilization_attitude_init(void)
{
/* setpoints */
FLOAT_EULERS_ZERO(stab_att_sp_euler);
float_quat_identity(&stab_att_sp_quat);
/* reference */
attitude_ref_quat_float_init(&att_ref_quat_f);
attitude_ref_quat_float_schedule(&att_ref_quat_f, STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT);
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
#ifdef HAS_SURFACE_COMMANDS
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
#endif
}
float_quat_identity(&stabilization_att_sum_err_quat);
FLOAT_RATES_ZERO(last_body_rate);
FLOAT_RATES_ZERO(body_rate_d);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_FLOAT, send_att);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_FLOAT, send_att_ref);
#endif
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
VECT3_ASSIGN(stabilization_gains.p,
STABILIZATION_ATTITUDE_PHI_PGAIN,
STABILIZATION_ATTITUDE_THETA_PGAIN,
STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN(stabilization_gains.d,
STABILIZATION_ATTITUDE_PHI_DGAIN,
STABILIZATION_ATTITUDE_THETA_DGAIN,
STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN(stabilization_gains.i,
STABILIZATION_ATTITUDE_PHI_IGAIN,
STABILIZATION_ATTITUDE_THETA_IGAIN,
STABILIZATION_ATTITUDE_PSI_IGAIN);
VECT3_ASSIGN(stabilization_gains.dd,
STABILIZATION_ATTITUDE_PHI_DDGAIN,
STABILIZATION_ATTITUDE_THETA_DDGAIN,
STABILIZATION_ATTITUDE_PSI_DDGAIN);
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
#endif
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
#ifdef HAS_SURFACE_COMMANDS
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
#endif
}
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
FLOAT_RATES_ZERO( last_body_rate );
FLOAT_RATES_ZERO( body_rate_d );
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
#endif
}
void ins_init(void) {
#if USE_INS_NAV_INIT
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
struct EcefCoor_i ecef_nav0;
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
ins_impl.ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
ins_impl.ltp_initialized = TRUE;
#else
ins_impl.ltp_initialized = FALSE;
#endif
INT32_VECT3_ZERO(ins_impl.ltp_pos);
INT32_VECT3_ZERO(ins_impl.ltp_speed);
INT32_VECT3_ZERO(ins_impl.ltp_accel);
#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);
#endif
}
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 gps_init(void)
{
multi_gps_mode = MULTI_GPS_MODE;
gps.valid_fields = 0;
gps.fix = GPS_FIX_NONE;
gps.week = 0;
gps.tow = 0;
gps.cacc = 0;
gps.last_3dfix_ticks = 0;
gps.last_3dfix_time = 0;
gps.last_msg_ticks = 0;
gps.last_msg_time = 0;
#ifdef GPS_POWER_GPIO
gpio_setup_output(GPS_POWER_GPIO);
GPS_POWER_GPIO_ON(GPS_POWER_GPIO);
#endif
#ifdef GPS_LED
LED_OFF(GPS_LED);
#endif
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS, send_gps);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_INT, send_gps_int);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_LLA, send_gps_lla);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_SOL, send_gps_sol);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SVINFO, send_svinfo);
#endif
}
void guidance_v_init(void)
{
guidance_v_mode = GUIDANCE_V_MODE_KILL;
guidance_v_kp = GUIDANCE_V_HOVER_KP;
guidance_v_kd = GUIDANCE_V_HOVER_KD;
guidance_v_ki = GUIDANCE_V_HOVER_KI;
guidance_v_z_sum_err = 0;
guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE;
guidance_v_adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED;
gv_adapt_init();
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
guidance_v_module_init();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "VERT_LOOP", send_vert_loop);
register_periodic_telemetry(DefaultPeriodic, "TUNE_VERT", send_tune_vert);
#endif
}
void guidance_h_init(void)
{
guidance_h.mode = GUIDANCE_H_MODE_KILL;
guidance_h.use_ref = GUIDANCE_H_USE_REF;
guidance_h.approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST;
INT_VECT2_ZERO(guidance_h.sp.pos);
INT_VECT2_ZERO(guidance_h_trim_att_integrator);
INT_EULERS_ZERO(guidance_h.rc_sp);
guidance_h.sp.heading = 0;
guidance_h.sp.heading_rate = 0;
guidance_h.gains.p = GUIDANCE_H_PGAIN;
guidance_h.gains.i = GUIDANCE_H_IGAIN;
guidance_h.gains.d = GUIDANCE_H_DGAIN;
guidance_h.gains.a = GUIDANCE_H_AGAIN;
guidance_h.gains.v = GUIDANCE_H_VGAIN;
transition_percentage = 0;
transition_theta_offset = 0;
gh_ref_init();
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
guidance_h_module_init();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_INT, send_gh);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HOVER_LOOP, send_hover_loop);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_REF_INT, send_href);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_TUNE_HOVER, send_tune_hover);
#endif
}
void h_ctl_init( void ) {
h_ctl_ref.max_p = H_CTL_REF_MAX_P;
h_ctl_ref.max_p_dot = H_CTL_REF_MAX_P_DOT;
h_ctl_ref.max_q = H_CTL_REF_MAX_Q;
h_ctl_ref.max_q_dot = H_CTL_REF_MAX_Q_DOT;
h_ctl_course_setpoint = 0.;
h_ctl_course_pre_bank = 0.;
h_ctl_course_pre_bank_correction = H_CTL_COURSE_PRE_BANK_CORRECTION;
h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
h_ctl_course_dgain = H_CTL_COURSE_DGAIN;
h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
h_ctl_disabled = FALSE;
h_ctl_roll_setpoint = 0.;
h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN;
h_ctl_roll_igain = H_CTL_ROLL_IGAIN;
h_ctl_roll_sum_err = 0;
h_ctl_roll_Kffa = H_CTL_ROLL_KFFA;
h_ctl_roll_Kffd = H_CTL_ROLL_KFFD;
h_ctl_aileron_setpoint = 0;
#ifdef H_CTL_AILERON_OF_THROTTLE
h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
#endif
h_ctl_pitch_setpoint = 0.;
h_ctl_pitch_loop_setpoint = 0.;
h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN;
h_ctl_pitch_igain = H_CTL_PITCH_IGAIN;
h_ctl_pitch_sum_err = 0.;
h_ctl_pitch_Kffa = H_CTL_PITCH_KFFA;
h_ctl_pitch_Kffd = H_CTL_PITCH_KFFD;
h_ctl_elevator_setpoint = 0;
h_ctl_elevator_of_roll = 0; //H_CTL_ELEVATOR_OF_ROLL;
#ifdef H_CTL_PITCH_OF_ROLL
h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
#endif
use_airspeed_ratio = FALSE;
airspeed_ratio2 = 1.;
#if USE_PITCH_TRIM
v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
#else
v_ctl_pitch_loiter_trim = 0.;
v_ctl_pitch_dash_trim = 0.;
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "CALIBRATION", send_calibration);
register_periodic_telemetry(DefaultPeriodic, "TUNE_ROLL", send_tune_roll);
register_periodic_telemetry(DefaultPeriodic, "H_CTL_A", send_ctl_a);
#endif
}
void cam_init(void)
{
cam_mode = CAM_MODE0;
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CAM, send_cam);
#ifdef SHOW_CAM_COORDINATES
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CAM_POINT, send_cam_point);
#endif
}
/********** INIT *************************************************************/
void init_fbw(void)
{
mcu_init();
#if !(DISABLE_ELECTRICAL)
electrical_init();
#endif
#ifdef ACTUATORS
actuators_init();
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif /* ACTUATORS */
#ifdef RADIO_CONTROL
radio_control_init();
#endif /* RADIO_CONTROL */
#ifdef INTER_MCU
inter_mcu_init();
#endif /* INTER_MCU */
#if defined MCU_SPI_LINK || defined MCU_CAN_LINK
link_mcu_init();
#endif /* MCU_SPI_LINK || MCU_CAN_LINK */
#ifdef MCU_SPI_LINK
link_mcu_restart();
#endif /* MCU_SPI_LINK */
fbw_mode = FBW_MODE_FAILSAFE;
/**** start timers for periodic functions *****/
fbw_periodic_tid = sys_time_register_timer((1. / 60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
#ifndef SINGLE_MCU
mcu_int_enable();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_fbw_status);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
#endif /* ACTUATORS */
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc);
#endif /* RADIO_CONTROL */
#endif /* PERIODIC_TELEMETRY */
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
heading = 0.;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared);
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_status);
#endif
}
void stabilization_indi_init(void)
{
// Initialize filters
indi_init_filters();
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INDI, send_att_indi);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat);
#endif
}
void ahrs_infrared_init(void)
{
heading = 0.;
AbiBindMsgIMU_GYRO_INT32(AHRS_INFRARED_GYRO_ID, &gyro_ev, gyro_cb);
AbiBindMsgGPS(AHRS_INFRARED_GPS_ID, &gps_ev, &gps_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared);
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_status);
#endif
}
void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
register_periodic_telemetry(DefaultPeriodic, "AHRS_REF_QUAT", send_ahrs_ref_quat);
#endif
}
void gvf_init(void)
{
gvf_control.ke = 1;
gvf_control.kn = 1;
gvf_control.s = 1;
gvf_trajectory.type = NONE;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GVF, send_gvf);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CIRCLE, send_circle);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SEGMENT, send_segment);
#endif
}
void stabilization_attitude_init(void)
{
attitude_ref_quat_int_init(&att_ref_quat_i);
int32_quat_identity(&stabilization_att_sum_err_quat);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INT, send_att);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_INT, send_att_ref);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat);
#endif
}
void link_mcu_init( void )
{
intermcu_data.status = LINK_MCU_UNINIT;
intermcu_data.msg_available = FALSE;
intermcu_data.error_cnt = 0;
#ifdef AP
#if PERIODIC_TELEMETRY
// If FBW has not telemetry, then AP can send some of the info
register_periodic_telemetry(DefaultPeriodic, "COMMANDS", send_commands);
register_periodic_telemetry(DefaultPeriodic, "FBW_STATUS", send_fbw_status);
#endif
#endif
}
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);
}
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
{
Rgps_pos = HFF_R_POS;
Rgps_vel = HFF_R_SPEED;
b2_hff_init_x(init_x, init_xdot);
b2_hff_init_y(init_y, init_ydot);
#ifdef GPS_LAG
/* init buffer for past mean accel values */
acc_buf_r = 0;
acc_buf_w = 0;
acc_buf_n = 0;
b2_hff_rb_put = b2_hff_rb;
b2_hff_rb_last = b2_hff_rb;
b2_hff_rb_last->rollback = false;
b2_hff_rb_last->lag_counter = 0;
b2_hff_state.lag_counter = GPS_LAG_N;
#ifdef SITL
printf("GPS_LAG: %f\n", GPS_LAG);
printf("GPS_LAG_N: %d\n", GPS_LAG_N);
printf("GPS_DT_N: %d\n", GPS_DT_N);
printf("DT_HFILTER: %f\n", DT_HFILTER);
printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N);
#endif
#else
b2_hff_rb_last = &b2_hff_state;
b2_hff_state.lag_counter = 0;
#endif
b2_hff_rb_n = 0;
b2_hff_state.rollback = false;
lag_counter_err = 0;
save_counter = -1;
past_save_counter = SAVE_DONE;
b2_hff_ps_counter = 1;
b2_hff_lost_counter = 0;
b2_hff_lost_limit = HFF_LOST_LIMIT;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF, send_hff);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF_DBG, send_hff_debug);
#ifdef GPS_LAG
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF_GPS, send_hff_gps);
#endif
#endif
init_butterworth_2_low_pass_int(&filter_x, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0);
init_butterworth_2_low_pass_int(&filter_y, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0);
init_butterworth_2_low_pass_int(&filter_z, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0);
}
void autopilot_init(void)
{
/* mode is finally set at end of init if MODE_STARTUP is not KILL */
autopilot_mode = AP_MODE_KILL;
autopilot_motors_on = FALSE;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = FALSE;
autopilot_in_flight_counter = 0;
autopilot_mode_auto2 = MODE_AUTO2;
autopilot_ground_detected = FALSE;
autopilot_detect_ground_once = FALSE;
autopilot_flight_time = 0;
autopilot_rc = TRUE;
autopilot_power_switch = FALSE;
#ifdef POWER_SWITCH_GPIO
gpio_setup_output(POWER_SWITCH_GPIO);
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
#endif
autopilot_arming_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
stabilization_none_init();
stabilization_rate_init();
stabilization_attitude_init();
/* set startup mode, propagates through to guidance h/v */
autopilot_set_mode(MODE_STARTUP);
register_periodic_telemetry(DefaultPeriodic, "AUTOPILOT_VERSION", send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_STATUS", send_status);
register_periodic_telemetry(DefaultPeriodic, "ENERGY", send_energy);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_FP", send_fp);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CMD", send_rotorcraft_cmd);
register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, "ACTUATORS", send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, "RC", send_rc);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_RADIO_CONTROL", send_rotorcraft_rc);
#endif
}
void stabilization_rate_init(void) {
INT_RATES_ZERO(stabilization_rate_sp);
RATES_ASSIGN(stabilization_rate_gain,
STABILIZATION_RATE_GAIN_P,
STABILIZATION_RATE_GAIN_Q,
STABILIZATION_RATE_GAIN_R);
RATES_ASSIGN(stabilization_rate_igain,
STABILIZATION_RATE_IGAIN_P,
STABILIZATION_RATE_IGAIN_Q,
STABILIZATION_RATE_IGAIN_R);
RATES_ASSIGN(stabilization_rate_ddgain,
STABILIZATION_RATE_DDGAIN_P,
STABILIZATION_RATE_DDGAIN_Q,
STABILIZATION_RATE_DDGAIN_R);
INT_RATES_ZERO(stabilization_rate_ref);
INT_RATES_ZERO(stabilization_rate_refdot);
INT_RATES_ZERO(stabilization_rate_sum_err);
#if DOWNLINK
register_periodic_telemetry(DefaultPeriodic, "RATE_LOOP", send_rate);
#endif
}
void intermcu_init(void)
{
pprz_transport_init(&intermcu_transport);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_status);
}
void downlink_init(void)
{
downlink.nb_ovrn = 0;
downlink.nb_bytes = 0;
downlink.nb_msgs = 0;
#if defined DATALINK
#if DATALINK == PPRZ || DATALINK == SUPERBITRF || DATALINK == W5100
pprz_transport_init(&pprz_tp);
#endif
#if DATALINK == XBEE
xbee_init();
#endif
#if DATALINK == W5100
w5100_init();
#endif
#endif
#if USE_PPRZLOG
pprzlog_transport_init();
#endif
#if SITL
ivy_transport_init();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "DATALINK_REPORT", send_downlink);
#endif
}
/**
* Initialize Vectornav struct
*/
void ahrs_vectornav_init(void)
{
// Initialize variables
ahrs_vn.vn_status = VNNotTracking;
ahrs_vn.vn_freq = 0;
// Initialize packet
ahrs_vn.vn_packet.status = VNMsgSync;
ahrs_vn.vn_packet.msg_idx = 0;
ahrs_vn.vn_packet.msg_available = false;
ahrs_vn.vn_packet.chksm_error = 0;
ahrs_vn.vn_packet.hdr_error = 0;
ahrs_vn.vn_packet.overrun_error = 0;
ahrs_vn.vn_packet.noise_error = 0;
ahrs_vn.vn_packet.framing_error = 0;
INT32_VECT3_ZERO(ahrs_vn.accel_i);
// initialize data struct
memset(&(ahrs_vn.vn_data), sizeof(struct VNData), 0);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VECTORNAV_INFO, send_vn_info);
#endif
}
void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_offset, float init_obs_height)
{
vff.z = init_z;
vff.zdot = init_zdot;
vff.bias = init_accel_bias;
vff.offset = init_offset;
vff.obs_height = init_obs_height;
int i, j;
for (i = 0; i < VFF_STATE_SIZE; i++) {
for (j = 0; j < VFF_STATE_SIZE; j++) {
vff.P[i][j] = 0.f;
}
vff.P[i][i] = VFF_EXTENDED_INIT_PXX;
}
#ifndef VFF_EXTENDED_NON_FLAT_GROUND
vff.P[4][4] = 0.f;
#endif
vff.accel_noise = VFF_EXTENDED_ACCEL_NOISE;
vff.r_baro = VFF_EXTENDED_R_BARO;
vff.r_alt = R_ALT;
vff.r_obs_height = R_OBS_HEIGHT;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VFF_EXTENDED, send_vffe);
#endif
}
void h_ctl_init( void ) {
h_ctl_course_setpoint = 0.;
h_ctl_course_pre_bank = 0.;
h_ctl_course_pre_bank_correction = H_CTL_COURSE_PRE_BANK_CORRECTION;
h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
h_ctl_course_dgain = H_CTL_COURSE_DGAIN;
h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
#ifdef USE_AOA
h_ctl_pitch_mode = 0;
#endif
h_ctl_disabled = FALSE;
h_ctl_roll_setpoint = 0.;
#ifdef H_CTL_ROLL_PGAIN
h_ctl_roll_pgain = H_CTL_ROLL_PGAIN;
#endif
h_ctl_aileron_setpoint = 0;
#ifdef H_CTL_AILERON_OF_THROTTLE
h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
#endif
h_ctl_pitch_setpoint = 0.;
h_ctl_pitch_loop_setpoint = 0.;
h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN;
h_ctl_elevator_setpoint = 0;
h_ctl_elevator_of_roll = H_CTL_ELEVATOR_OF_ROLL;
#ifdef H_CTL_RATE_LOOP
h_ctl_roll_rate_mode = H_CTL_ROLL_RATE_MODE_DEFAULT;
h_ctl_roll_rate_setpoint_pgain = H_CTL_ROLL_RATE_SETPOINT_PGAIN;
h_ctl_hi_throttle_roll_rate_pgain = H_CTL_HI_THROTTLE_ROLL_RATE_PGAIN;
h_ctl_lo_throttle_roll_rate_pgain = H_CTL_LO_THROTTLE_ROLL_RATE_PGAIN;
h_ctl_roll_rate_igain = H_CTL_ROLL_RATE_IGAIN;
h_ctl_roll_rate_dgain = H_CTL_ROLL_RATE_DGAIN;
#endif
#ifdef H_CTL_ROLL_SLEW
h_ctl_roll_slew = H_CTL_ROLL_SLEW;
#endif
#ifdef H_CTL_COURSE_SLEW_INCREMENT
h_ctl_course_slew_increment = H_CTL_COURSE_SLEW_INCREMENT;
#endif
#ifdef H_CTL_ROLL_ATTITUDE_GAIN
h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN;
#endif
#ifdef AGR_CLIMB
nav_ratio=0;
#endif
#if DOWNLINK
register_periodic_telemetry(DefaultPeriodic, "CALIBRATION", send_calibration);
#endif
}
void stabilization_attitude_init(void)
{
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INDI, send_att_indi);
#endif
}
