int
MulticopterPositionControl::parameters_update(bool force)
{
bool updated;
struct parameter_update_s param_upd;
orb_check(_params_sub, &updated);
if (updated) {
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd);
}
if (updated || force) {
param_get(_params_handles.thr_min, &_params.thr_min);
param_get(_params_handles.thr_max, &_params.thr_max);
param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
_params.tilt_max_air = math::radians(_params.tilt_max_air);
param_get(_params_handles.land_speed, &_params.land_speed);
param_get(_params_handles.tilt_max_land, &_params.tilt_max_land);
_params.tilt_max_land = math::radians(_params.tilt_max_land);
float v;
param_get(_params_handles.xy_p, &v);
_params.pos_p(0) = v;
_params.pos_p(1) = v;
param_get(_params_handles.z_p, &v);
_params.pos_p(2) = v;
param_get(_params_handles.xy_vel_p, &v);
_params.vel_p(0) = v;
_params.vel_p(1) = v;
param_get(_params_handles.z_vel_p, &v);
_params.vel_p(2) = v;
param_get(_params_handles.xy_vel_i, &v);
_params.vel_i(0) = v;
_params.vel_i(1) = v;
param_get(_params_handles.z_vel_i, &v);
_params.vel_i(2) = v;
param_get(_params_handles.xy_vel_d, &v);
_params.vel_d(0) = v;
_params.vel_d(1) = v;
param_get(_params_handles.z_vel_d, &v);
_params.vel_d(2) = v;
param_get(_params_handles.xy_vel_max, &v);
_params.vel_max(0) = v;
_params.vel_max(1) = v;
param_get(_params_handles.z_vel_max, &v);
_params.vel_max(2) = v;
param_get(_params_handles.xy_ff, &v);
_params.vel_ff(0) = v;
_params.vel_ff(1) = v;
param_get(_params_handles.z_ff, &v);
_params.vel_ff(2) = v;
_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
}
return OK;
}
int
MulticopterPositionControl::parameters_update(bool force)
{
bool updated;
struct parameter_update_s param_upd;
orb_check(_params_sub, &updated);
if (updated) {
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd);
}
if (updated || force) {
param_get(_params_handles.thr_min, &_params.thr_min);
param_get(_params_handles.thr_max, &_params.thr_max);
param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
_params.tilt_max_air = math::radians(_params.tilt_max_air);
param_get(_params_handles.land_speed, &_params.land_speed);
param_get(_params_handles.tilt_max_land, &_params.tilt_max_land);
_params.tilt_max_land = math::radians(_params.tilt_max_land);
float v;
param_get(_params_handles.xy_p, &v);
_params.pos_p(0) = v;
_params.pos_p(1) = v;
param_get(_params_handles.z_p, &v);
_params.pos_p(2) = v;
param_get(_params_handles.xy_vel_p, &v);
_params.vel_p(0) = v;
_params.vel_p(1) = v;
param_get(_params_handles.z_vel_p, &v);
_params.vel_p(2) = v;
param_get(_params_handles.xy_vel_i, &v);
_params.vel_i(0) = v;
_params.vel_i(1) = v;
param_get(_params_handles.z_vel_i, &v);
_params.vel_i(2) = v;
param_get(_params_handles.xy_vel_d, &v);
_params.vel_d(0) = v;
_params.vel_d(1) = v;
param_get(_params_handles.z_vel_d, &v);
_params.vel_d(2) = v;
param_get(_params_handles.xy_vel_max, &v);
_params.vel_max(0) = v;
_params.vel_max(1) = v;
param_get(_params_handles.z_vel_max, &v);
_params.vel_max(2) = v;
param_get(_params_handles.xy_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(0) = v;
_params.vel_ff(1) = v;
param_get(_params_handles.z_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(2) = v;
_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
/* mc attitude control parameters*/
/* 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);
param_get(_params_handles.mc_att_yaw_p,&v);
_params.mc_att_yaw_p = v;
}
return OK;
}
int
MulticopterPositionControl::parameters_update(bool force)
{
bool updated;
struct parameter_update_s param_upd;
orb_check(_params_sub, &updated);
if (updated) {
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd);
}
if (updated || force) {
param_get(_params_handles.thr_min, &_params.thr_min);
param_get(_params_handles.thr_max, &_params.thr_max);
param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
_params.tilt_max_air = math::radians(_params.tilt_max_air);
param_get(_params_handles.land_speed, &_params.land_speed);
param_get(_params_handles.tilt_max_land, &_params.tilt_max_land);
_params.tilt_max_land = math::radians(_params.tilt_max_land);
float v;
param_get(_params_handles.xy_p, &v);
_params.pos_p(0) = v;
_params.pos_p(1) = v;
param_get(_params_handles.z_p, &v);
_params.pos_p(2) = v;
param_get(_params_handles.xy_vel_p, &v);
_params.vel_p(0) = v;
_params.vel_p(1) = v;
param_get(_params_handles.z_vel_p, &v);
_params.vel_p(2) = v;
param_get(_params_handles.xy_vel_i, &v);
_params.vel_i(0) = v;
_params.vel_i(1) = v;
param_get(_params_handles.z_vel_i, &v);
_params.vel_i(2) = v;
param_get(_params_handles.xy_vel_d, &v);
_params.vel_d(0) = v;
_params.vel_d(1) = v;
param_get(_params_handles.z_vel_d, &v);
_params.vel_d(2) = v;
param_get(_params_handles.xy_vel_max, &v);
_params.vel_max(0) = v;
_params.vel_max(1) = v;
param_get(_params_handles.z_vel_max, &v);
_params.vel_max(2) = v;
param_get(_params_handles.xy_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(0) = v;
_params.vel_ff(1) = v;
param_get(_params_handles.z_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(2) = v;
_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
/* Update of sonar parameters */
param_get(_sonar_params_handles._snr_activate_sw_h,&_sonar_params._snr_activate_sw);
param_get(_sonar_params_handles._snr_fwd_min_th_h,&_sonar_params._snr_fwd_min_th);
param_get(_sonar_params_handles._snr_fwd_stab_ratio_h,&_sonar_params._snr_fwd_stab_ratio);
param_get(_sonar_params_handles._snr_fwd_wall_ratio_h,&_sonar_params._snr_fwd_wall_ratio);
param_get(_sonar_params_handles._snr_fwd_max_response_h,&_sonar_params._snr_fwd_max_response);
param_get(_sonar_params_handles._snr_fwd_max_th_h,&_sonar_params._snr_fwd_max_th);
param_get(_sonar_params_handles._snr_fwd_adc_ind_h,&_sonar_params._snr_fwd_adc_ind);
if(_sonar_params._snr_activate_sw != 0)
{
_snr_activated_flag = true;
}
else
{
_snr_activated_flag = false;
}
}
return OK;
}
