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