void Mav2Ivy::cmdThrustCallback(const std_msgs::Float64ConstPtr& cmd_thrust_msg)
{
if (!motors_on_ || engaging_) return;
state_mutex_.lock();
// translate from cmd_thrust [0.0 to 1.0] to ctrl_thrust PPRZ TODO,
ctrl_thrust_ = (int)(cmd_thrust_msg->data * 200);
ROS_DEBUG ("cmd_thrust received: %f (%d)", cmd_thrust_msg->data, ctrl_thrust_);
// limit min-max output
if (ctrl_thrust_ > max_ctrl_thrust_)
{
ROS_WARN("ctrl_thrust of %d too big, clamping to %d!", ctrl_thrust_, max_ctrl_thrust_);
ctrl_thrust_ = max_ctrl_thrust_;
}
else if (ctrl_thrust_ < 0)
{
ROS_WARN("ctrl_thrust of %d too small, clamping to 0!", ctrl_thrust_);
ctrl_thrust_ = 0;
}
publishCtrlInputMsg();
state_mutex_.unlock();
}
void Mav2Ivy::cmdRollCallback(const std_msgs::Float64ConstPtr& cmd_roll_msg)
{
if (!motors_on_ || engaging_) return;
state_mutex_.lock();
// translate from cmd_roll [-1.0 to 1.0] to ctrl_roll PPRZ TODO
ctrl_roll_ = (int)(cmd_roll_msg->data * 100);
ROS_INFO ("cmd_roll received: %f (%d)", cmd_roll_msg->data, ctrl_roll_);
// limit min/max output
if (ctrl_roll_ > max_ctrl_roll_)
{
ROS_WARN("ctrl_roll of %d too big, clamping to %d!", ctrl_roll_, max_ctrl_roll_);
ctrl_roll_ = max_ctrl_roll_;
}
else if (ctrl_roll_ < -max_ctrl_roll_)
{
ROS_WARN("ctrl_roll of %d too small, clamping to -%d!", ctrl_roll_, -max_ctrl_roll_);
ctrl_roll_ = -max_ctrl_roll_;
}
publishCtrlInputMsg();
state_mutex_.unlock();
}
void Mav2Ivy::cmdYawCallback(const std_msgs::Float64ConstPtr& cmd_yaw_rate_msg)
{
if (!motors_on_ || engaging_) return;
state_mutex_.lock();
// translate from cmd_yaw [rad/s] to ctrl_yaw PPRZ TODO,
ctrl_yaw_ = (int)(cmd_yaw_rate_msg->data * 1);
ROS_DEBUG ("cmd_yaw received: %f (%d)", cmd_yaw_rate_msg->data, ctrl_yaw_);
// limit min/max output
if (ctrl_yaw_ > max_ctrl_yaw_)
{
ROS_WARN("ctrl_yaw of %d too big, clamping to %d!", ctrl_yaw_, max_ctrl_yaw_);
ctrl_yaw_ = max_ctrl_yaw_;
}
else if (ctrl_yaw_ < -max_ctrl_yaw_)
{
ROS_WARN("ctrl_yaw of %d too small, clamping to -%d!", ctrl_yaw_, -max_ctrl_yaw_);
ctrl_yaw_ = -max_ctrl_yaw_;
}
publishCtrlInputMsg();
state_mutex_.unlock();
}
void AsctecProc::cmdPitchCallback(const std_msgs::Float64ConstPtr& cmd_pitch_msg)
{
if (!motors_on_ || engaging_) return;
state_mutex_.lock();
// translate from cmd_pitch [-1.0 to 1.0] to ctrl_pitch [-2047 .. 2047],
ctrl_pitch_ = (int)(cmd_pitch_msg->data * asctec::ROS_TO_ASC_PITCH);
ROS_DEBUG ("cmd_pitch received: %f (%d)", cmd_pitch_msg->data, ctrl_pitch_);
// limit min/max output
if (ctrl_pitch_ > max_ctrl_pitch_)
{
ROS_WARN("ctrl_pitch of %d too big, clamping to %d!", ctrl_pitch_, max_ctrl_pitch_);
ctrl_pitch_ = max_ctrl_pitch_;
}
else if (ctrl_pitch_ < -max_ctrl_pitch_)
{
ROS_WARN("ctrl_pitch of %d too small, clamping to -%d!", ctrl_pitch_, -max_ctrl_pitch_);
ctrl_pitch_ = -max_ctrl_pitch_;
}
publishCtrlInputMsg();
state_mutex_.unlock();
}
