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 #if GUIDANCE_INDI guidance_indi_enter(); #endif #if HYBRID_NAVIGATION guidance_hybrid_init(); #endif }
void guidance_h_mode_changed(uint8_t new_mode) { if (new_mode == guidance_h.mode) { return; } #if HYBRID_NAVIGATION guidance_hybrid_norm_ref_airspeed = 0; #endif switch (new_mode) { case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_enter(); break; #if USE_STABILIZATION_RATE case GUIDANCE_H_MODE_RATE: stabilization_rate_enter(); break; #endif case GUIDANCE_H_MODE_CARE_FREE: stabilization_attitude_reset_care_free_heading(); case GUIDANCE_H_MODE_FORWARD: case GUIDANCE_H_MODE_ATTITUDE: #if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ if (guidance_h.mode == GUIDANCE_H_MODE_KILL || guidance_h.mode == GUIDANCE_H_MODE_RATE || guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT) #endif stabilization_attitude_enter(); break; case GUIDANCE_H_MODE_GUIDED: case GUIDANCE_H_MODE_HOVER: #if GUIDANCE_INDI guidance_indi_enter(); #endif guidance_h_hover_enter(); #if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ if (guidance_h.mode == GUIDANCE_H_MODE_KILL || guidance_h.mode == GUIDANCE_H_MODE_RATE || guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT) #endif stabilization_attitude_enter(); break; #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE case GUIDANCE_H_MODE_MODULE: guidance_h_module_enter(); break; #endif case GUIDANCE_H_MODE_NAV: guidance_h_nav_enter(); #if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ if (guidance_h.mode == GUIDANCE_H_MODE_KILL || guidance_h.mode == GUIDANCE_H_MODE_RATE || guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT) #endif stabilization_attitude_enter(); break; case GUIDANCE_H_MODE_FLIP: guidance_flip_enter(); break; default: break; } guidance_h.mode = new_mode; }