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;
/* 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);
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;
}
void
MulticopterAttitudeControl::adjust_params(){
_params_adjust.att_p = _params.att_p;
_params_adjust.rate_p = _params.rate_p;
_params_adjust.rate_i = _params.rate_i;
_params_adjust.rate_d = _params.rate_d;
float thrust_dependent_roll_p = get_gains(_params.pitchroll_rate_p,_thrust_sp);
float thrust_dependent_roll_d = get_gains(_params.pitchroll_rate_d,_thrust_sp);
for(int i = 0; i < 2; i++){
// i==0 is roll, otherwise pitch
if(_params.rate_p(0)<0||_params.rate_p(1)<0)
_params_adjust.rate_p(i) = thrust_dependent_roll_p * (i==0 ? 1 : _params.pitch_scale);
if(_params.rate_d(0)<0||_params.rate_d(1)<0)
_params_adjust.rate_d(i) = thrust_dependent_roll_d * (i==0 ? 1 : _params.pitch_scale);
}
// QGroundControl doesn't allow small enough values for P and D rate gains
// Therefore divide roll and pitch rate p and d gains by 1000 -Patrick
float scale_pd_array[] = {.001, .001, 1.0};
math::Vector<3> scale_pd = math::Vector<3>(scale_pd_array);
_params_adjust.rate_d = _params_adjust.rate_d.emult(scale_pd);
_params_adjust.rate_p = _params_adjust.rate_p.emult(scale_pd);
//Scale if above threshold
if(_thrust_sp> 0.6f&&false){
//If throttle above 60% scale values -Patrick
float scaleFactor = (1.0/6.0)/2.25; //old slope:new slope ratio
float scaleArray[] = {scaleFactor, scaleFactor, 1.0};
math::Vector<3> scale = math::Vector<3>(scaleArray);
_params_adjust.att_p = _params_adjust.att_p.emult(scale);
_params_adjust.rate_p = _params_adjust.rate_p.emult(scale);
_params_adjust.rate_d = _params_adjust.rate_d.emult(scale);
//Print gains whenever throttle crosses threshold -Patrick
if(!throtThreshold){
throtThreshold = true;
warnx_debug("Throttle crossed threshold from low range to high range");
}
}else{
//If throttle below 60% don't change values -Patrick
//Print gains whenever throttle crosses threshold -Patrick
if(throtThreshold){
throtThreshold = false;
warnx_debug("Throttle crossed threshold from high range to low range");
}
}
if(++counter>=500){
counter = 0;
/*warnx_debug("Thrust: %6.3f",(double) _thrust_sp);
warnx_debug(" roll pitch ");
warnx_debug("Rate P: %9.5f %9.5f",(double) _params_adjust.rate_p(0),(double)_params_adjust.rate_p(1));
warnx_debug("Rate D: %9.5f %9.5f",(double) _params_adjust.rate_d(0),(double)_params_adjust.rate_d(1));
for (int i=0; i<11; i++) {
warnx_debug("Rate P[%2d]=%8.3f Rate D[%2d]=%8.3f",
i, (double) _params.pitchroll_rate_p(i),
i, (double) _params.pitchroll_rate_d(i));
}*/
warnx_debug("Channel 6: %9.5f", (double) _rc_channels.channels[5]);
warnx_debug("");
}
}
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;
}
