Esempio n. 1
0
inline void sendImuData(void)
{
  g_imu_pkt.roll  = LLToSIAngleRoll (LL_1khz_attitude_data.angle_roll);
  g_imu_pkt.pitch = LLToSIAnglePitch(LL_1khz_attitude_data.angle_pitch);
  g_imu_pkt.yaw   = LLToSIAngleYaw  (LL_1khz_attitude_data.angle_yaw);
  g_imu_pkt.roll_rate  = LLToSIAngleRateRoll (LL_1khz_attitude_data.angvel_roll);
  g_imu_pkt.pitch_rate = LLToSIAngleRatePitch(LL_1khz_attitude_data.angvel_pitch);
  g_imu_pkt.yaw_rate   = LLToSIAngleRateYaw  (LL_1khz_attitude_data.angvel_yaw);
  writePacket2Ringbuffer(MAV_IMU_PKT_ID, (unsigned char*)&g_imu_pkt, sizeof(g_imu_pkt));
}
Esempio n. 2
0
void processCtrl(void)
{
  float dt = 0.001;//(g_timestamp - g_latest_ctrl_time) * 0.000001; //dt in sec
  //g_latest_ctrl_time = g_timestamp;
  // **** Check if there's a new packet with PID parameters ***************

  if (g_pid_cfg_pkt_info->updated != 0)
  {
    g_pid_cfg_pkt_info->updated = 0;
    pidParamUpdate();
  }

  if (g_flight_state_pkt.state == MAV_STATE_FLYING)
  {
    float roll     = LLToSIAngleRoll (RO_ALL_Data.angle_roll);//LL_1khz_attitude_data.angle_roll);
    float pitch    = LLToSIAnglePitch(RO_ALL_Data.angle_pitch);//LL_1khz_attitude_data.angle_pitch);

    float a_x = g_accel_x * cos(pitch) + g_accel_y * sin(pitch)*sin(roll) + g_accel_z * sin(pitch)*cos(roll);
    float a_y = g_accel_y * cos(roll)  - g_accel_z * sin(roll);

    float vel_x_bf  = g_pose_pkt.vx * g_cos_psi + g_pose_pkt.vy * g_sin_psi;
    float vel_y_bf  = g_pose_pkt.vy * g_cos_psi - g_pose_pkt.vx * g_sin_psi;
    float vel_z = g_pose_pkt.vz;

    //float dv_x = (vel_x_bf - g_vel_x_bf_last)/dt;
    //float dv_y = (vel_y_bf - g_vel_y_bf_last)/dt;
    //float dv_z = (g_pose_pkt.vz - g_vel_z_last )/dt;

    //g_vel_x_bf_last = vel_x_bf;
    //g_vel_y_bf_last = vel_y_bf;
    //g_vel_z_last = vel_z;

    g_ctrl_debug_pkt.vel_x_bf = vel_x_bf;
    g_ctrl_debug_pkt.vel_y_bf = vel_y_bf;
    g_ctrl_debug_pkt.ax_bf = a_x;
    g_ctrl_debug_pkt.ay_bf = a_y;
    //g_ctrl_debug_pkt.az    = dv_z;

    // *************************** X axis ctrl*********************************

    if (g_ctrl_cfg_pkt.ctrl_mode_pitch == MAV_CTRL_MODE_POSITION)
    {
      float des_x_bf   = (g_des_pose_pkt.x - g_pose_pkt.x) * g_cos_psi + (g_des_pose_pkt.y - g_pose_pkt.y) * g_sin_psi;
      //float vel_x_bf   = g_pose_pkt.vx * g_cos_psi + g_pose_pkt.vy * g_sin_psi;
      //float pitch_rate = LLToSIAngleRatePitch (LL_1khz_attitude_data.angvel_pitch);

      g_ctrl_cmd.cmd_pitch = pidCalc(&pid_x, des_x_bf, -vel_x_bf, pid_x.d_base, dt);
      g_ctrl_debug_pkt.pid_error_x_bf = des_x_bf;
    }

    else if (g_ctrl_cfg_pkt.ctrl_mode_pitch == MAV_CTRL_MODE_VELOCITY)
    {
      //float des_vx_bf   = (g_des_pose_pkt.vx - g_pose_pkt.vx) * g_cos_psi + (g_des_pose_pkt.vy - g_pose_pkt.vy) * g_sin_psi;
      //float des_vx     = (float) g_des_pose_pkt.vx;
      //float current_vx = (float) g_pose_pkt.vx;
      float vx_error = g_des_vel_pkt.vx - vel_x_bf;
      g_ctrl_debug_pkt.pid_error_vx_bf = vx_error;

      g_ctrl_cmd.cmd_pitch = pidCalc(&pid_vx, vx_error, -a_x, 1.0, dt);
    }

    else if (g_ctrl_cfg_pkt.ctrl_mode_pitch == MAV_CTRL_MODE_DIRECT)
      g_ctrl_cmd.cmd_pitch = g_ctrl_input_pkt.cmd_pitch;

    else if (g_ctrl_cfg_pkt.ctrl_mode_pitch == MAV_CTRL_MODE_DISABLED)
      g_ctrl_cmd.cmd_pitch = 0;

    // set debug info
    g_ctrl_debug_pkt.pid_x_i_term = pid_x.sum_error;

    // *************************** Y axis ctrl *********************************

    if (g_ctrl_cfg_pkt.ctrl_mode_roll == MAV_CTRL_MODE_POSITION)
    {
      float des_y_bf  = (g_des_pose_pkt.y - g_pose_pkt.y)* g_cos_psi - (g_des_pose_pkt.x - g_pose_pkt.x) * g_sin_psi;
      //float vel_y_bf  = g_pose_pkt.vy * g_cos_psi - g_pose_pkt.vx * g_sin_psi;
      //float roll_rate = LLToSIAngleRateRoll (LL_1khz_attitude_data.angvel_roll);

      g_ctrl_cmd.cmd_roll = -pidCalc(&pid_y, des_y_bf, -vel_y_bf, pid_y.d_base, dt); // positive roll gives you negative y
      g_ctrl_debug_pkt.pid_error_y_bf = des_y_bf;
    }
    else if (g_ctrl_cfg_pkt.ctrl_mode_roll == MAV_CTRL_MODE_VELOCITY)
    {
      //float des_vy     = (float) g_des_pose_pkt.vy;
      //float current_vy = (float) g_pose_pkt.vy;

      float vy_error = g_des_vel_pkt.vy - vel_y_bf;
      g_ctrl_debug_pkt.pid_error_vy_bf = vy_error;
      g_ctrl_cmd.cmd_roll = -pidCalc(&pid_vy, vy_error, -a_y, 1.0, dt);
    }

    else if (g_ctrl_cfg_pkt.ctrl_mode_roll == MAV_CTRL_MODE_DIRECT)
      g_ctrl_cmd.cmd_roll = g_ctrl_input_pkt.cmd_roll;

    else if (g_ctrl_cfg_pkt.ctrl_mode_roll == MAV_CTRL_MODE_DISABLED)
      g_ctrl_cmd.cmd_roll = 0;

    // set debug info
    g_ctrl_debug_pkt.pid_y_i_term = pid_y.sum_error;

    // **************************** Z axis ctrl *********************************

    if (g_ctrl_cfg_pkt.ctrl_mode_thrust == MAV_CTRL_MODE_DISABLED)
    {
      g_ctrl_cmd.cmd_thrust = 0;
    }
    else
    {
      Thrust new_cmd_thrust = g_ctrl_cmd.cmd_thrust;

      if (g_ctrl_cfg_pkt.ctrl_mode_thrust == MAV_CTRL_MODE_POSITION)
      {
        float des_z     = g_des_pose_pkt.z;
        float current_z = g_pose_pkt.z;

        new_cmd_thrust = pidCalc(&pid_z, des_z - current_z, -vel_z, 1.0, dt);// - pid_z.kd2 * g_accel_z;
      }

      else if (g_ctrl_cfg_pkt.ctrl_mode_thrust == MAV_CTRL_MODE_VELOCITY)
            {
              float des_vz     = g_des_vel_pkt.vz;

              new_cmd_thrust = pidCalc(&pid_vz, des_vz - vel_z, -g_accel_z, 1.0, dt);
            }

      else if (g_ctrl_cfg_pkt.ctrl_mode_thrust == MAV_CTRL_MODE_DIRECT)
      {
        new_cmd_thrust = g_ctrl_input_pkt.cmd_thrust;
      }

      // spike guard
      double delta_cmd_thrust = new_cmd_thrust - g_ctrl_cmd.cmd_thrust;

      if      (delta_cmd_thrust > g_ctrl_cfg_pkt.cmd_thrust_delta_limit)
        g_ctrl_cmd.cmd_thrust += g_ctrl_cfg_pkt.cmd_thrust_delta_limit;
      else if (delta_cmd_thrust < -g_ctrl_cfg_pkt.cmd_thrust_delta_limit)
        g_ctrl_cmd.cmd_thrust -= g_ctrl_cfg_pkt.cmd_thrust_delta_limit;
      else
        g_ctrl_cmd.cmd_thrust = new_cmd_thrust;

      // set debug info
      g_ctrl_debug_pkt.pid_z_i_term = pid_z.sum_error;
    }

    // ****************************** YAW ctrl *********************************

    if (g_ctrl_cfg_pkt.ctrl_mode_yaw_rate == MAV_CTRL_MODE_POSITION)
    {
      float des_yaw     = g_des_pose_pkt.yaw;
      float current_yaw = g_pose_pkt.yaw;
      float yaw_rate    = LLToSIAngleRateYaw(RO_ALL_Data.angvel_yaw);//LL_1khz_attitude_data.angvel_yaw);

      float error =  des_yaw - current_yaw;
      normalizeSIAnglePi(&error);

      g_ctrl_cmd.cmd_yaw_rate = pidCalc(&pid_yaw, error, -yaw_rate, 1, dt);
    }
    if (g_ctrl_cfg_pkt.ctrl_mode_yaw_rate == MAV_CTRL_MODE_VELOCITY)
      g_ctrl_cmd.cmd_yaw_rate = g_des_vel_pkt.yaw_rate;

    else if (g_ctrl_cfg_pkt.ctrl_mode_yaw_rate == MAV_CTRL_MODE_DIRECT)
      g_ctrl_cmd.cmd_yaw_rate = g_ctrl_input_pkt.cmd_yaw_rate;

    else if (g_ctrl_cfg_pkt.ctrl_mode_yaw_rate == MAV_CTRL_MODE_DISABLED)
      g_ctrl_cmd.cmd_yaw_rate = 0;

    // set debug info
    g_ctrl_debug_pkt.pid_yaw_i_term = pid_yaw.sum_error;

  }

  // **************************** CLAMP ********************************

  // Clamp roll command
  if      (g_ctrl_cmd.cmd_roll >  g_ctrl_cfg_pkt.cmd_roll_limit)
    g_ctrl_cmd.cmd_roll =  g_ctrl_cfg_pkt.cmd_roll_limit;
  else if (g_ctrl_cmd.cmd_roll < -g_ctrl_cfg_pkt.cmd_roll_limit)
    g_ctrl_cmd.cmd_roll = -g_ctrl_cfg_pkt.cmd_roll_limit;

  // Clamp pitch command
  if      (g_ctrl_cmd.cmd_pitch >  g_ctrl_cfg_pkt.cmd_pitch_limit)
    g_ctrl_cmd.cmd_pitch =  g_ctrl_cfg_pkt.cmd_pitch_limit;
  else if (g_ctrl_cmd.cmd_pitch < -g_ctrl_cfg_pkt.cmd_pitch_limit)
    g_ctrl_cmd.cmd_pitch = -g_ctrl_cfg_pkt.cmd_pitch_limit;

  // Clamp yaw rate command
  if      (g_ctrl_cmd.cmd_yaw_rate > g_ctrl_cfg_pkt.cmd_yaw_rate_limit)
    g_ctrl_cmd.cmd_yaw_rate =  g_ctrl_cfg_pkt.cmd_yaw_rate_limit;
  else if (g_ctrl_cmd.cmd_yaw_rate < -g_ctrl_cfg_pkt.cmd_yaw_rate_limit)
    g_ctrl_cmd.cmd_yaw_rate = -g_ctrl_cfg_pkt.cmd_yaw_rate_limit;

  // Clamp thrust command
  if      (g_ctrl_cmd.cmd_thrust > g_ctrl_cfg_pkt.cmd_thrust_limit)
    g_ctrl_cmd.cmd_thrust = g_ctrl_cfg_pkt.cmd_thrust_limit;
  else if (g_ctrl_cmd.cmd_thrust < 0)
    g_ctrl_cmd.cmd_thrust = 0;
}