void
MulticopterPositionControl::task_main()
{
warnx("started");
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[mpc] started");
/*
* do subscriptions
*/
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
parameters_update(true);
/* initialize values of critical structs until first regular update */
_arming.armed = false;
/* get an initial update for all sensor and status data */
poll_subscriptions();
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool was_armed = false;
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
math::Vector<3> sp_move_rate;
sp_move_rate.zero();
math::Vector<3> thrust_int;
thrust_int.zero();
math::Matrix<3, 3> R;
R.identity();
/* wakeup source */
struct pollfd fds[1];
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
poll_subscriptions();
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
t_prev = t;
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
_reset_pos_sp = true;
_reset_alt_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
was_armed = _control_mode.flag_armed;
update_ref();
if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) {
_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
_pos(2) = _local_pos.z;
_vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy;
_vel(2) = _local_pos.vz;
sp_move_rate.zero();
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* move altitude setpoint with throttle stick */
sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
}
if (_control_mode.flag_control_position_enabled) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = _manual.x;
sp_move_rate(1) = _manual.y;
}
/* limit setpoint move rate */
float sp_move_norm = sp_move_rate.length();
if (sp_move_norm > 1.0f) {
sp_move_rate /= sp_move_norm;
}
/* scale to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
/* move position setpoint */
_pos_sp += sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
} else {
/* AUTO */
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
} else {
/* no waypoint, loiter, reset position setpoint if needed */
reset_pos_sp();
reset_alt_sp();
}
}
/* fill local position setpoint */
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
/* publish local position setpoint */
if (_local_pos_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _att.yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
/* publish attitude setpoint */
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
} else {
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos;
_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
_vel_sp(2) = 0.0f;
}
if (!_control_mode.flag_control_position_enabled) {
_reset_pos_sp = true;
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
/* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
if (!_control_mode.flag_control_manual_enabled) {
/* limit 3D speed only in non-manual modes */
float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
if (vel_sp_norm > 1.0f) {
_vel_sp /= vel_sp_norm;
}
}
_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);
/* publish velocity setpoint */
if (_global_vel_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
} else {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
}
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
/* reset integrals if needed */
if (_control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = _params.thr_min;
if (reset_int_z_manual) {
i = _manual.z;
if (i < _params.thr_min) {
i = _params.thr_min;
} else if (i > _params.thr_max) {
i = _params.thr_max;
}
}
thrust_int(2) = -i;
}
} else {
reset_int_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
if (reset_int_xy) {
reset_int_xy = false;
thrust_int(0) = 0.0f;
thrust_int(1) = 0.0f;
}
} else {
reset_int_xy = true;
}
/* velocity error */
math::Vector<3> vel_err = _vel_sp - _vel;
/* derivative of velocity error, not includes setpoint acceleration */
math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
_vel_prev = _vel;
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
if (!_control_mode.flag_control_velocity_enabled) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
thrust_sp(2) = 0.0f;
}
/* limit thrust vector and check for saturation */
bool saturation_xy = false;
bool saturation_z = false;
/* limit min lift */
float thr_min = _params.thr_min;
if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
thr_min = 0.0f;
}
float tilt_max = _params.tilt_max_air;
/* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
}
/* limit min lift */
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
if (thrust_sp_xy_len > 0.01f) {
/* max horizontal thrust for given vertical thrust*/
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
if (thrust_sp_xy_len > thrust_xy_max) {
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
}
}
} else {
/* thrust compensation for altitude only control mode */
float att_comp;
if (_att.R[2][2] > TILT_COS_MAX) {
att_comp = 1.0f / _att.R[2][2];
} else if (_att.R[2][2] > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
saturation_z = true;
} else {
att_comp = 1.0f;
saturation_z = true;
}
thrust_sp(2) *= att_comp;
}
/* limit max thrust */
float thrust_abs = thrust_sp.length();
if (thrust_abs > _params.thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > _params.thr_max) {
/* thrust Z component is too large, limit it */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
thrust_sp(2) = -_params.thr_max;
saturation_xy = true;
saturation_z = true;
} else {
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
} else {
/* Z component is negative, going down, simply limit thrust vector */
float k = _params.thr_max / thrust_abs;
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
}
thrust_abs = _params.thr_max;
}
/* update integrals */
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
}
if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f;
}
}
/* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;
} else {
/* no thrust, set Z axis to safe value */
body_z.zero();
body_z(2) = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2 */
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
if (fabsf(body_z(2)) > SIGMA) {
/* desired body_x axis, orthogonal to body_z */
body_x = y_C % body_z;
/* keep nose to front while inverted upside down */
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
body_x.normalize();
} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x.zero();
body_x(2) = 1.0f;
}
/* desired body_y axis */
body_y = body_z % body_x;
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R(i, 0) = body_x(i);
R(i, 1) = body_y(i);
R(i, 2) = body_z(i);
}
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
math::Vector<3> euler = R.to_euler();
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
} else if (!_control_mode.flag_control_manual_enabled) {
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw */
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
}
_att_sp.thrust = thrust_abs;
_att_sp.timestamp = hrt_absolute_time();
/* publish attitude setpoint */
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
} else {
reset_int_z = true;
}
}
} else {
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
}
warnx("stopped");
mavlink_log_info(_mavlink_fd, "[mpc] stopped");
_control_task = -1;
_exit(0);
}
void
MulticopterPositionControl::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
/*
* do subscriptions
*/
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
parameters_update(true);
/* initialize values of critical structs until first regular update */
_arming.armed = false;
/* get an initial update for all sensor and status data */
poll_subscriptions();
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool reset_yaw_sp = true;
bool was_armed = false;
hrt_abstime t_prev = 0;
_hover_time = 0.0; // miao:
_mode_mission = 1;
math::Vector<3> thrust_int;
thrust_int.zero();
math::Matrix<3, 3> R;
R.identity();
/* wakeup source */
struct pollfd fds[1];
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
poll_subscriptions();
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
t_prev = t;
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
_reset_pos_sp = true;
_reset_alt_sp = true;
reset_int_z = true;
reset_int_xy = true;
reset_yaw_sp = true;
_reset_mission = true;//miao:
}
//Update previous arming state
was_armed = _control_mode.flag_armed;
update_ref();
if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) {
_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
_pos(2) = _local_pos.z;
_vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy;
_vel(2) = _local_pos.vz;
_vel_ff.zero();
_sp_move_rate.zero();
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
control_manual(dt);
_mode_auto = false;
} else if (_control_mode.flag_control_offboard_enabled) {
/* offboard control */
control_offboard(dt);
_mode_auto = false;
} else {
/* AUTO */
control_auto(dt);
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _att.yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
/* publish attitude setpoint */
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
} else {
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos;
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
_vel_sp(2) = 0.0f;
}
if (!_control_mode.flag_control_position_enabled) {
_reset_pos_sp = true;
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
/* use constant descend rate when landing, ignore altitude setpoint */
//if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
// miao: for auto landing test with manual mode
if (_mode_mission==3) {
_vel_sp(2) = _params.land_speed;
}
_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);
/* publish velocity setpoint */
if (_global_vel_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
} else {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
}
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
/* reset integrals if needed */
if (_control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = _params.thr_min;
if (reset_int_z_manual) {
i = _manual.z;
if (i < _params.thr_min) {
i = _params.thr_min;
} else if (i > _params.thr_max) {
i = _params.thr_max;
}
}
thrust_int(2) = -i;
}
} else {
reset_int_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
if (reset_int_xy) {
reset_int_xy = false;
thrust_int(0) = 0.0f;
thrust_int(1) = 0.0f;
}
} else {
reset_int_xy = true;
}
/* velocity error */
math::Vector<3> vel_err = _vel_sp - _vel;
/* derivative of velocity error, not includes setpoint acceleration */
math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
_vel_prev = _vel;
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
if (!_control_mode.flag_control_velocity_enabled) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
thrust_sp(2) = 0.0f;
}
/* limit thrust vector and check for saturation */
bool saturation_xy = false;
bool saturation_z = false;
/* limit min lift */
float thr_min = _params.thr_min;
if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
thr_min = 0.0f;
}
float tilt_max = _params.tilt_max_air;
/* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
}
/* limit min lift */
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
if (thrust_sp_xy_len > 0.01f) {
/* max horizontal thrust for given vertical thrust*/
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
if (thrust_sp_xy_len > thrust_xy_max) {
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
}
}
} else {
/* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att.R, 2, 2);
} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
saturation_z = true;
} else {
att_comp = 1.0f;
saturation_z = true;
}
thrust_sp(2) *= att_comp;
}
/* limit max thrust */
float thrust_abs = thrust_sp.length();
if (thrust_abs > _params.thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > _params.thr_max) {
/* thrust Z component is too large, limit it */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
thrust_sp(2) = -_params.thr_max;
saturation_xy = true;
saturation_z = true;
} else {
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
} else {
/* Z component is negative, going down, simply limit thrust vector */
float k = _params.thr_max / thrust_abs;
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
}
thrust_abs = _params.thr_max;
}
/* update integrals */
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
}
if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f;
}
}
/* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;
} else {
/* no thrust, set Z axis to safe value */
body_z.zero();
body_z(2) = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2 */
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
if (fabsf(body_z(2)) > SIGMA) {
/* desired body_x axis, orthogonal to body_z */
body_x = y_C % body_z;
/* keep nose to front while inverted upside down */
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
body_x.normalize();
} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x.zero();
body_x(2) = 1.0f;
}
/* desired body_y axis */
body_y = body_z % body_x;
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R(i, 0) = body_x(i);
R(i, 1) = body_y(i);
R(i, 2) = body_z(i);
}
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
math::Vector<3> euler = R.to_euler();
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
} else if (!_control_mode.flag_control_manual_enabled) {
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw */
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
}
_att_sp.thrust = thrust_abs;
/* save thrust setpoint for logging */
_local_pos_sp.acc_x = thrust_sp(0);
_local_pos_sp.acc_x = thrust_sp(1);
_local_pos_sp.acc_x = thrust_sp(2);
_att_sp.timestamp = hrt_absolute_time();
} else {
reset_int_z = true;
}
}
/* fill local position, velocity and thrust setpoint */
_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
_local_pos_sp.vx = _vel_sp(0);
_local_pos_sp.vy = _vel_sp(1);
_local_pos_sp.vz = _vel_sp(2);
/* publish local position setpoint */
if (_local_pos_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
} else {
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true;
_mode_auto = false;
reset_int_z = true;
reset_int_xy = true;
}
// generate attitude setpoint from manual controls
if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
// reset yaw setpoint to current position if needed
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
}
// do not move yaw while arming
else if (_manual.z > 0.1f)
{
const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw);
if (yaw_offs < - YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX);
} else if (yaw_offs > YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX);
}
}
//Control roll and pitch directly if we no aiding velocity controller is active
if(!_control_mode.flag_control_velocity_enabled) {
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
}
//Control climb rate directly if no aiding altitude controller is active
if(!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = _manual.z;
}
//Construct attitude setpoint rotation matrix
math::Matrix<3,3> R_sp;
R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
_att_sp.timestamp = hrt_absolute_time();
}
else {
reset_yaw_sp = true;
}
/* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity control is disabled,
* in this case the attitude setpoint is published by the mavlink app
*/
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
}
warnx("stopped");
mavlink_log_info(_mavlink_fd, "[mpc] stopped");
_control_task = -1;
_exit(0);
}