Пример #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));
}
Пример #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;
}
Пример #3
0
inline void processKF()
{
  if (g_mav_kf_cfg_pkt_info->updated)
  {
    g_mav_kf_cfg_pkt_info->updated = 0;

    uint8_t kf_reset = 0;

    kf_reset         = g_mav_kf_cfg_pkt.enable_mask & (1<<MAV_KF_RESET_BIT);
    g_kf_x_enabled   = g_mav_kf_cfg_pkt.enable_mask & (1<<MAV_KF_X_BIT);
    g_kf_y_enabled   = g_mav_kf_cfg_pkt.enable_mask & (1<<MAV_KF_Y_BIT);
    g_kf_z_enabled   = g_mav_kf_cfg_pkt.enable_mask & (1<<MAV_KF_Z_BIT);
    g_kf_yaw_enabled = g_mav_kf_cfg_pkt.enable_mask & (1<<MAV_KF_YAW_BIT);

    kal_x.Sigma2Q1 = g_mav_kf_cfg_pkt.Q_x;
    kal_x.Sigma2Q2 = g_mav_kf_cfg_pkt.Q_x;
    kal_x.Sigma2R1 = g_mav_kf_cfg_pkt.R_x;
    kal_x.Sigma2R2 = g_mav_kf_cfg_pkt.R_vx;

    kal_y.Sigma2Q1 = g_mav_kf_cfg_pkt.Q_y;
    kal_y.Sigma2Q2 = g_mav_kf_cfg_pkt.Q_y;
    kal_y.Sigma2R1 = g_mav_kf_cfg_pkt.R_y;
    kal_y.Sigma2R2 = g_mav_kf_cfg_pkt.R_vy;

    kal_z.Sigma2Q1 = g_mav_kf_cfg_pkt.Q_z;
    kal_z.Sigma2Q2 = g_mav_kf_cfg_pkt.Q_z;
    kal_z.Sigma2R1 = g_mav_kf_cfg_pkt.R_z;
    kal_z.Sigma2R2 = g_mav_kf_cfg_pkt.R_vz_p;

    kal_yaw.Sigma2Q = g_mav_kf_cfg_pkt.Q_yaw;
    kal_yaw.Sigma2R = g_mav_kf_cfg_pkt.R_yaw;

    if (kf_reset != 0) resetKalmanFilter();
  }

  KFilter();

  // **** Update pose based on Kalman Filter (or direct CPU updates)

  if (g_kf_yaw_enabled != 0)
  {
    g_pose_pkt.yaw = kal_out.yaw_filtered;
  }
  else
  {
    g_pose_pkt.yaw = g_mav_pose2D_pkt.yaw;
  }

  if (g_kf_x_enabled != 0)
  {
    g_pose_pkt.x  = kal_out.pos_filtered[0];
    g_pose_pkt.vx = kal_out.vel_filtered[0];
  }
  else
  {
    g_pose_pkt.x  = g_mav_pose2D_pkt.x;
    g_pose_pkt.vx = g_mav_pose2D_pkt.vx;
  }

  if (g_kf_y_enabled != 0)
  {
    g_pose_pkt.y  = kal_out.pos_filtered[1];
    g_pose_pkt.vy = kal_out.vel_filtered[1];
  }
  else
  {
    g_pose_pkt.y  = g_mav_pose2D_pkt.y;
    g_pose_pkt.vy = g_mav_pose2D_pkt.vy;
  }

  if (g_kf_z_enabled != 0)
  {
    g_pose_pkt.z  = kal_out.pos_filtered[2];
    g_pose_pkt.vz = kal_out.vel_filtered[2];
  }
  else
  {
    g_pose_pkt.z  = g_mav_height_pkt.z;
    g_pose_pkt.vz = g_mav_height_pkt.vz;
  }

  g_pose_pkt.roll  = LLToSIAngleRoll (LL_1khz_attitude_data.angle_roll);
  g_pose_pkt.pitch = LLToSIAnglePitch(LL_1khz_attitude_data.angle_pitch);
}