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;
}
/*!
Set the velocity (frame has to be specified) that will be applied to the robot.
\param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to control left
and right wheel velocities and vpRobot::REFERENCE_FRAME to control translational and
rotational velocities are implemented.
\param vel : A two dimension vector that corresponds to the velocities to apply to the robot.
- If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
second value is the velocity of the right wheel in m/s. In that case sets the velocity of the wheels
independently.
- If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
Second value is the rotational velocity in rad/s.
Note that to secure the usage of the robot, velocities are saturated to the maximum allowed
which can be obtained by getMaxTranslationVelocity() and getMaxRotationVelocity(). To change
the default values, use setMaxTranslationVelocity() and setMaxRotationVelocity().
\exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
\exception vpRobotException::wrongStateError : If the specified control frame is not supported.
*/
void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
{
init();
/*
if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
vpERROR_TRACE ("Cannot send a velocity to the robot "
"use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send a velocity to the robot "
"use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
} */
if (vel.size() != 2)
{
throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
}
vpColVector vel_max(2);
vpColVector vel_sat;
if (frame == vpRobot::REFERENCE_FRAME)
{
vel_max[0] = getMaxTranslationVelocity();
vel_max[1] = getMaxRotationVelocity();
vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
this->lock();
this->setVel(vel_sat[0]*1000.); // convert velocity in mm/s
this->setRotVel( vpMath::deg(vel_sat[1]) ); // convert velocity in deg/s
this->unlock();
}
else if (frame == vpRobot::ARTICULAR_FRAME)
{
vel_max[0] = getMaxTranslationVelocity();
vel_max[1] = getMaxTranslationVelocity();
vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
this->lock();
//std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
this->setVel2(vel_sat[0]*1000., vel_sat[1]*1000.); // convert velocity in mm/s
this->unlock();
}
else
{
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send the robot velocity in the specified control frame");
}
}
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;
}
