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; }