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));
}
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;
}
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);
}