def __init__(self, params, control_total_energy_divider, mode): """ :param control_total_energy_divider run total enery controller every xth time """ self.mode = mode # since we run on python2, make sure that all params are floats for p in params: params[p] = float(params[p]) self.params = params # Attitude Control self.c_roll = PyECLRollController() self.c_pitch = PyECLPitchController() self.c_yaw = PyECLYawController() att_tc = params["att_tc"] k_p = params["k_p"] k_ff = params["k_ff"] k_i = params["k_i"] i_max = params["i_max"] self.c_roll.set_time_constant(att_tc) self.c_roll.set_k_p(k_p) self.c_roll.set_k_i(k_i) self.c_roll.set_integrator_max(i_max) self.c_roll.set_k_ff(k_ff) self.c_pitch.set_time_constant(att_tc) self.c_pitch.set_k_p(k_p) self.c_pitch.set_k_i(k_i) self.c_pitch.set_integrator_max(i_max) self.c_pitch.set_k_ff(k_ff) self.c_pitch.set_max_rate_pos(params["pitch_max_rate_pos"]) self.c_pitch.set_max_rate_neg(params["pitch_max_rate_neg"]) self.c_pitch.set_roll_ff(params["pitch_roll_ff"]) self.c_yaw.set_time_constant(att_tc) self.c_yaw.set_k_p(k_p) self.c_yaw.set_k_i(k_i) self.c_yaw.set_integrator_max(i_max) self.c_yaw.set_k_ff(k_ff) self.c_yaw.set_coordinated_min_speed(params["coordinated_min_speed"]) self.c_yaw.set_coordinated_method(params["coordinated_method"]) # Altitude and speed control (total energy control) # XXX load values from params argument where it makes sense c_te_params = { "MT_ENABLED": 1, "MT_THR_FF": params["mtecs_throttle_ff"], "MT_THR_P": params["mtecs_throttle_p"], "MT_THR_I": params["mtecs_throttle_i"], "MT_THR_OFF": params["throttle_default"], "MT_PIT_FF": params["mtecs_pitch_ff"], "MT_PIT_P": params["mtecs_pitch_p"], "MT_PIT_I": params["mtecs_pitch_i"], "MT_PIT_OFF": 0.0, "MT_THR_MIN": 0.0, "MT_THR_MAX": 1.0, "MT_PIT_MIN": -45.0, "MT_PIT_MAX": 20.0, "MT_ALT_LP": params["mtecs_altitude_lowpass_cutoff"], "MT_FPA_LP": params["mtecs_flightpathangle_lowpass_cutoff"], "MT_FPA_P": params["mtecs_fpa_p"], "MT_FPA_D": 0.0, "MT_FPA_D_LP": 1.0, "MT_FPA_MIN": -20.0, "MT_FPA_MAX": 30.0, "MT_A_LP": params["mtecs_airspeed_lowpass_cutoff"], "MT_AD_LP": params["mtecs_airspeed_derivative_lowpass_cutoff"], "MT_ACC_P": params["mtecs_acc_p"], "MT_ACC_D": 0.0, "MT_ACC_D_LP": 0.5, "MT_ACC_MIN": -40.0, "MT_ACC_MAX": 40.0, "MT_TKF_THR_MIN": 1.0, "MT_TKF_THR_MAX": 1.0, "MT_TKF_PIT_MIN": 0.0, "MT_TKF_PIT_MAX": 45.0, "MT_USP_THR_MIN": 1.0, "MT_USP_THR_MAX": 1.0, "MT_USP_PIT_MIN": -45.0, "MT_USP_PIT_MAX": 0.0, "MT_LND_THR_MIN": 0.0, "MT_LND_THR_MAX": 0.0, "MT_LND_PIT_MIN": -5.0, "MT_LND_PIT_MAX": 15.0, "MT_THR_I_MAX": 10.0, "MT_PIT_I_MAX": 10.0, "FW_AIRSPD_MIN": params["airspeed_min"], } self.c_te = PyMTecs(c_te_params) self.control_count = 0 self.control_total_energy_divider = control_total_energy_divider