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
