int MulticopterAttitudeControl::parameters_update() { float v; /* roll gains */ param_get(_params_handles.roll_p, &v); _params.att_p(0) = v; param_get(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v; param_get(_params_handles.roll_rate_i, &v); _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); _params.att_p(1) = v; param_get(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v; param_get(_params_handles.pitch_rate_i, &v); _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; param_get(_params_handles.yaw_rate_i, &v); _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); _params.yaw_rate_max = math::radians(_params.yaw_rate_max); /* manual control scale */ param_get(_params_handles.man_roll_max, &_params.man_roll_max); param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); /* acro control scale */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); param_get(_params_handles.acro_pitch_max, &v); _params.acro_rate_max(1) = math::radians(v); param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); return OK; }
int MulticopterAttitudeControl::parameters_update() { float v; float roll_tc, pitch_tc; param_get(_params_handles.roll_tc, &roll_tc); param_get(_params_handles.pitch_tc, &pitch_tc); /* roll gains */ param_get(_params_handles.roll_p, &v); _params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_i, &v); _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_ff, &v); _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); _params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_i, &v); _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_ff, &v); _params.rate_ff(1) = v; /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; param_get(_params_handles.yaw_rate_i, &v); _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; param_get(_params_handles.yaw_rate_ff, &v); _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); /* angular rate limits */ param_get(_params_handles.roll_rate_max, &_params.roll_rate_max); _params.mc_rate_max(0) = math::radians(_params.roll_rate_max); param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max); _params.mc_rate_max(1) = math::radians(_params.pitch_rate_max); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); /* manual rate control scale and auto mode roll/pitch rate limits */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); param_get(_params_handles.acro_pitch_max, &v); _params.acro_rate_max(1) = math::radians(v); param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); /* stick deflection needed in rattitude mode to control rates not angles */ param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); param_get(_params_handles.vtol_type, &_params.vtol_type); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); return OK; }
int MulticopterAttitudeControl::parameters_update() { float v; float roll_tc, pitch_tc; param_get(_params_handles.roll_tc, &roll_tc); param_get(_params_handles.pitch_tc, &pitch_tc); /* roll gains */ param_get(_params_handles.roll_p, &v); _params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_i, &v); _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_ff, &v); _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); _params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_i, &v); _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_ff, &v); _params.rate_ff(1) = v; /* pitch/roll P gains, throttle dependent */ param_get(_params_handles.pitchroll_rate_p10, &v); for (int i=0; i<2; i++) _params.pitchroll_rate_p(i) = v; param_get(_params_handles.pitchroll_rate_p20, &v); _params.pitchroll_rate_p(2) = v; param_get(_params_handles.pitchroll_rate_p30, &v); _params.pitchroll_rate_p(3) = v; param_get(_params_handles.pitchroll_rate_p40, &v); _params.pitchroll_rate_p(4) = v; param_get(_params_handles.pitchroll_rate_p50, &v); _params.pitchroll_rate_p(5) = v; param_get(_params_handles.pitchroll_rate_p60, &v); _params.pitchroll_rate_p(6) = v; param_get(_params_handles.pitchroll_rate_p70, &v); _params.pitchroll_rate_p(7) = v; param_get(_params_handles.pitchroll_rate_p80, &v); _params.pitchroll_rate_p(8) = v; param_get(_params_handles.pitchroll_rate_p90, &v); _params.pitchroll_rate_p(9) = v; _params.pitchroll_rate_p(10) = v; /* pitch/roll D gains, throttle dependent */ param_get(_params_handles.pitchroll_rate_d10, &v); for (int i=0; i<2; i++) _params.pitchroll_rate_d(i) = v; param_get(_params_handles.pitchroll_rate_d20, &v); _params.pitchroll_rate_d(2) = v; param_get(_params_handles.pitchroll_rate_d30, &v); _params.pitchroll_rate_d(3) = v; param_get(_params_handles.pitchroll_rate_d40, &v); _params.pitchroll_rate_d(4) = v; param_get(_params_handles.pitchroll_rate_d50, &v); _params.pitchroll_rate_d(5) = v; param_get(_params_handles.pitchroll_rate_d60, &v); _params.pitchroll_rate_d(6) = v; param_get(_params_handles.pitchroll_rate_d70, &v); _params.pitchroll_rate_d(7) = v; param_get(_params_handles.pitchroll_rate_d80, &v); _params.pitchroll_rate_d(8) = v; param_get(_params_handles.pitchroll_rate_d90, &v); _params.pitchroll_rate_d(9) = v; _params.pitchroll_rate_d(10) = v; /* Pulse switch data */ int i; param_get(_params_handles.pulse_channel, &i); _params.pulse_channel = i; param_get(_params_handles.pulse_channel_threshold, &v); _params.pulse_channel_threshold = v; /*Pitch array scale factor*/ param_get(_params_handles.pitch_scale_factor, &v); _params.pitch_scale = v; /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; param_get(_params_handles.yaw_rate_i, &v); _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; param_get(_params_handles.yaw_rate_ff, &v); _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); /* angular rate limits */ param_get(_params_handles.roll_rate_max, &_params.roll_rate_max); _params.mc_rate_max(0) = math::radians(_params.roll_rate_max); param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max); _params.mc_rate_max(1) = math::radians(_params.pitch_rate_max); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); /* manual rate control scale and auto mode roll/pitch rate limits */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); param_get(_params_handles.acro_pitch_max, &v); _params.acro_rate_max(1) = math::radians(v); param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); /* stick deflection needed in rattitude mode to control rates not angles */ param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); param_get(_params_handles.vtol_type, &_params.vtol_type); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); return OK; }
int MulticopterAttitudeControl::parameters_update() { float v; float roll_tc, pitch_tc; param_get(_params_handles.roll_tc, &roll_tc); param_get(_params_handles.pitch_tc, &pitch_tc); /* roll gains */ param_get(_params_handles.roll_p, &v); _params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_i, &v); _params.rate_i(0) = v; param_get(_params_handles.roll_rate_integ_lim, &v); _params.rate_int_lim(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_ff, &v); _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); _params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_i, &v); _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_integ_lim, &v); _params.rate_int_lim(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_ff, &v); _params.rate_ff(1) = v; param_get(_params_handles.tpa_breakpoint_p, &_params.tpa_breakpoint_p); param_get(_params_handles.tpa_breakpoint_i, &_params.tpa_breakpoint_i); param_get(_params_handles.tpa_breakpoint_d, &_params.tpa_breakpoint_d); param_get(_params_handles.tpa_rate_p, &_params.tpa_rate_p); param_get(_params_handles.tpa_rate_i, &_params.tpa_rate_i); param_get(_params_handles.tpa_rate_d, &_params.tpa_rate_d); /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; param_get(_params_handles.yaw_rate_i, &v); _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_integ_lim, &v); _params.rate_int_lim(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; param_get(_params_handles.yaw_rate_ff, &v); _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); /* angular rate limits */ param_get(_params_handles.roll_rate_max, &_params.roll_rate_max); _params.mc_rate_max(0) = math::radians(_params.roll_rate_max); param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max); _params.mc_rate_max(1) = math::radians(_params.pitch_rate_max); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); /* auto angular rate limits */ param_get(_params_handles.roll_rate_max, &_params.roll_rate_max); _params.auto_rate_max(0) = math::radians(_params.roll_rate_max); param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max); _params.auto_rate_max(1) = math::radians(_params.pitch_rate_max); param_get(_params_handles.yaw_auto_max, &_params.yaw_auto_max); _params.auto_rate_max(2) = math::radians(_params.yaw_auto_max); /* manual rate control scale and auto mode roll/pitch rate limits */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); param_get(_params_handles.acro_pitch_max, &v); _params.acro_rate_max(1) = math::radians(v); param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); /* stick deflection needed in rattitude mode to control rates not angles */ param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); param_get(_params_handles.vtol_type, &_params.vtol_type); int tmp; param_get(_params_handles.vtol_opt_recovery_enabled, &tmp); _params.vtol_opt_recovery_enabled = (bool)tmp; param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale); param_get(_params_handles.bat_scale_en, &_params.bat_scale_en); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); /* rotation of the autopilot relative to the body */ param_get(_params_handles.board_rotation, &(_params.board_rotation)); /* fine adjustment of the rotation */ param_get(_params_handles.board_offset[0], &(_params.board_offset[0])); param_get(_params_handles.board_offset[1], &(_params.board_offset[1])); param_get(_params_handles.board_offset[2], &(_params.board_offset[2])); return OK; }