コード例 #1
0
ファイル: mav2ivy.cpp プロジェクト: OpenUAS/mav2ivy
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();
}
コード例 #2
0
ファイル: mav2ivy.cpp プロジェクト: OpenUAS/mav2ivy
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();
}
コード例 #3
0
ファイル: mav2ivy.cpp プロジェクト: OpenUAS/mav2ivy
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();
}
コード例 #4
0
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();
}