inline static void h_ctl_pitch_loop( void ) { static float last_err; /* sanity check */ if (h_ctl_pitch_of_roll <0.) h_ctl_pitch_of_roll = 0.; h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi); #ifdef USE_PITCH_TRIM loiter(); #endif #ifdef USE_ANGLE_REF // Update reference setpoints for pitch h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT; h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT; h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate; // Saturation on references BoundAbs(h_ctl_ref_pitch_accel, H_CTL_REF_MAX_Q_DOT); if (h_ctl_ref_pitch_rate > H_CTL_REF_MAX_Q) { h_ctl_ref_pitch_rate = H_CTL_REF_MAX_Q; if (h_ctl_ref_pitch_accel > 0.) h_ctl_ref_pitch_accel = 0.; } else if (h_ctl_ref_pitch_rate < - H_CTL_REF_MAX_Q) { h_ctl_ref_pitch_rate = - H_CTL_REF_MAX_Q; if (h_ctl_ref_pitch_accel < 0.) h_ctl_ref_pitch_accel = 0.; } #else h_ctl_ref_pitch_angle = h_ctl_pitch_loop_setpoint; h_ctl_ref_pitch_rate = 0.; h_ctl_ref_pitch_accel = 0.; #endif // Compute errors float err = estimator_theta - h_ctl_ref_pitch_angle; float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate; last_err = err; if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { h_ctl_pitch_sum_err = 0.; } else { if (h_ctl_pitch_igain < 0.) { h_ctl_pitch_sum_err += err * H_CTL_REF_DT; BoundAbs(h_ctl_pitch_sum_err, (- H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain)); } else { h_ctl_pitch_sum_err = 0.; } } float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel + h_ctl_pitch_Kffd * h_ctl_ref_pitch_rate + h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err + h_ctl_pitch_igain * h_ctl_pitch_sum_err; cmd /= airspeed_ratio2; h_ctl_elevator_setpoint = TRIM_PPRZ(cmd); }
//光流定点模式 rt_err_t loiter_mode(u8 height) { const u16 basic_thought = 450; if (check_safe(SAFE_ADNS3080)) return RT_EIO; if (pwm.throttle > 0.3f && abs(ahrs.degree_pitch) < 40 && abs(ahrs.degree_roll) < 40) { loiter(pos_X, pos_y, yaw_exp); althold(height); motor_hupdate(basic_thought); } else Motor_Set(60, 60, 60, 60); return RT_EOK; }
inline static void h_ctl_pitch_loop(void) { static float last_err; struct FloatEulers *att = stateGetNedToBodyEulers_f(); /* sanity check */ if (h_ctl_elevator_of_roll < 0.) { h_ctl_elevator_of_roll = 0.; } if (v_ctl_mode == V_CTL_MODE_LANDING) { h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint; } else { h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi); } float err = 0; #ifdef USE_AOA switch (h_ctl_pitch_mode) { case H_CTL_PITCH_MODE_THETA: err = att->theta - h_ctl_pitch_loop_setpoint; break; case H_CTL_PITCH_MODE_AOA: err = stateGetAngleOfAttack_f() - h_ctl_pitch_loop_setpoint; break; default: err = att->theta - h_ctl_pitch_loop_setpoint; break; } #else //NO_AOA err = att->theta - h_ctl_pitch_loop_setpoint; #endif float d_err = err - last_err; last_err = err; float cmd = -h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err); #ifdef LOITER_TRIM cmd += loiter(); #endif h_ctl_elevator_setpoint = TRIM_PPRZ(cmd); }
inline static void h_ctl_pitch_loop( void ) { static float last_err; /* sanity check */ if (h_ctl_elevator_of_roll <0.) h_ctl_elevator_of_roll = 0.; h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint - h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi); float err = estimator_theta - h_ctl_pitch_loop_setpoint; float d_err = err - last_err; last_err = err; float cmd = h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err); #ifdef LOITER_TRIM cmd += loiter(); #endif h_ctl_elevator_setpoint = TRIM_PPRZ(cmd); }
inline static void h_ctl_pitch_loop( void ) { #if !USE_GYRO_PITCH_RATE static float last_err; #endif /* sanity check */ if (h_ctl_pitch_of_roll <0.) h_ctl_pitch_of_roll = 0.; h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi); #if USE_PITCH_TRIM loiter(); #endif #if USE_ANGLE_REF // Update reference setpoints for pitch h_ctl_ref.pitch_angle += h_ctl_ref.pitch_rate * H_CTL_REF_DT; h_ctl_ref.pitch_rate += h_ctl_ref.pitch_accel * H_CTL_REF_DT; h_ctl_ref.pitch_accel = H_CTL_REF_W_Q*H_CTL_REF_W_Q * (h_ctl_pitch_loop_setpoint - h_ctl_ref.pitch_angle) - 2*H_CTL_REF_XI_Q*H_CTL_REF_W_Q * h_ctl_ref.pitch_rate; // Saturation on references BoundAbs(h_ctl_ref.pitch_accel, h_ctl_ref.max_q_dot); if (h_ctl_ref.pitch_rate > h_ctl_ref.max_q) { h_ctl_ref.pitch_rate = h_ctl_ref.max_q; if (h_ctl_ref.pitch_accel > 0.) h_ctl_ref.pitch_accel = 0.; } else if (h_ctl_ref.pitch_rate < - h_ctl_ref.max_q) { h_ctl_ref.pitch_rate = - h_ctl_ref.max_q; if (h_ctl_ref.pitch_accel < 0.) h_ctl_ref.pitch_accel = 0.; } #else h_ctl_ref.pitch_angle = h_ctl_pitch_loop_setpoint; h_ctl_ref.pitch_rate = 0.; h_ctl_ref.pitch_accel = 0.; #endif // Compute errors float err = h_ctl_ref.pitch_angle - stateGetNedToBodyEulers_f()->theta; #if USE_GYRO_PITCH_RATE float d_err = h_ctl_ref.pitch_rate - stateGetBodyRates_f()->q; #else // soft derivation float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref.pitch_rate; last_err = err; #endif if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { h_ctl_pitch_sum_err = 0.; } else { if (h_ctl_pitch_igain > 0.) { h_ctl_pitch_sum_err += err * H_CTL_REF_DT; BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain); } else { h_ctl_pitch_sum_err = 0.; } } float cmd = - h_ctl_pitch_Kffa * h_ctl_ref.pitch_accel - h_ctl_pitch_Kffd * h_ctl_ref.pitch_rate + h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err + h_ctl_pitch_igain * h_ctl_pitch_sum_err; cmd /= airspeed_ratio2; h_ctl_elevator_setpoint = TRIM_PPRZ(cmd); }
void FlyLine::localCallBack(const geometry_msgs::PoseStampedConstPtr &msg) { double roll, pitch, yaw; tf::Quaternion q; tf::quaternionMsgToTF(msg->pose.orientation, q); tf::Matrix3x3(q).getRPY(roll, pitch, yaw); if(fabs(msg->pose.position.x) < AllowError_d && fabs(msg->pose.position.y) < AllowError_d && fabs(msg->pose.position.z - beginpoint_(2)) < AllowError_d && fabs(yaw) < AllowError_rad && !in_line && !in_land) { in_line = true; ROS_INFO("Begin to fly line!!!"); } if(in_line == false && in_land == false) { ROS_INFO("Taking off to beginpoint, waiting line!!!! "); takeoff(beginpoint_); } if(in_line == true) { t_now = ros::Time::now(); double dt = (t_now.toNSec() - t_prev.toNSec())/1e+9; dt = dt < 0.1 ? dt : 0; line_t += dt; if(line_t < loiter_t) { ROS_INFO("Waiting 5 second in begin point!!!"); loiter(beginpoint_); } else if(line_t >= loiter_t && line_t <= allline_t + loiter_t) { ROS_INFO("Fly line!!!"); line(beginpoint_, line_t, endpoint_); } else if(line_t > allline_t + loiter_t && line_t <= allline_t + 2 * loiter_t) { ROS_INFO("line end!!!Waiting endpoint 5 second!!!"); loiter(endpoint_); } else { in_land = true; in_line = false; } } if(in_land == true) { ROS_INFO("it is landing!!!"); t_now = ros::Time::now(); double dt = (t_now.toNSec() - t_prev.toNSec())/1e+9; dt = dt < 0.1 ? dt : 0; land_t += dt; land(endpoint_, land_t); } t_prev = t_now; publish(); }