fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 14:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control fix and enforce code style

Browse Source
This commit is contained in:
Daniel Agar
2016-08-07 10:40:17 +02:00
committed by Lorenz Meier
parent d4196f7f0c
commit 4049ec2e96
2 changed files with 71 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/check_code_style_all.sh
-1
View File
@@ -19,7 +19,6 @@ find src/ \
-path src/modules/ekf_att_pos_estimator -prune -o \ -path src/modules/ekf_att_pos_estimator -prune -o \
-path src/modules/mavlink -prune -o \ -path src/modules/mavlink -prune -o \
-path src/modules/mc_att_control_multiplatform -prune -o \ -path src/modules/mc_att_control_multiplatform -prune -o \
-path src/modules/mc_pos_control -prune -o \
-path src/modules/mc_pos_control_multiplatform -prune -o \ -path src/modules/mc_pos_control_multiplatform -prune -o \
-path src/modules/navigator -prune -o \ -path src/modules/navigator -prune -o \
-path src/modules/position_estimator_inav -prune -o \ -path src/modules/position_estimator_inav -prune -o \
src/modules/mc_pos_control/mc_pos_control_main.cpp
+71 -58
View File
@@ -1001,8 +1001,8 @@ void MulticopterPositionControl::control_auto(float dt)
//Make sure that the position setpoint is valid //Make sure that the position setpoint is valid
if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) || if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||
!PX4_ISFINITE(_pos_sp_triplet.current.lon) || !PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
!PX4_ISFINITE(_pos_sp_triplet.current.alt)) { !PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false; _pos_sp_triplet.current.valid = false;
} }
} }
@@ -1022,8 +1022,8 @@ void MulticopterPositionControl::control_auto(float dt)
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
if (PX4_ISFINITE(curr_sp(0)) && if (PX4_ISFINITE(curr_sp(0)) &&
PX4_ISFINITE(curr_sp(1)) && PX4_ISFINITE(curr_sp(1)) &&
PX4_ISFINITE(curr_sp(2))) { PX4_ISFINITE(curr_sp(2))) {
current_setpoint_valid = true; current_setpoint_valid = true;
} }
} }
@@ -1035,8 +1035,8 @@ void MulticopterPositionControl::control_auto(float dt)
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if (PX4_ISFINITE(prev_sp(0)) && if (PX4_ISFINITE(prev_sp(0)) &&
PX4_ISFINITE(prev_sp(1)) && PX4_ISFINITE(prev_sp(1)) &&
PX4_ISFINITE(prev_sp(2))) { PX4_ISFINITE(prev_sp(2))) {
previous_setpoint_valid = true; previous_setpoint_valid = true;
} }
} }
@@ -1047,7 +1047,7 @@ void MulticopterPositionControl::control_auto(float dt)
math::Vector<3> cruising_speed = _params.vel_cruise; math::Vector<3> cruising_speed = _params.vel_cruise;
if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) && if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
_pos_sp_triplet.current.cruising_speed > 0.1f) { _pos_sp_triplet.current.cruising_speed > 0.1f) {
cruising_speed(0) = _pos_sp_triplet.current.cruising_speed; cruising_speed(0) = _pos_sp_triplet.current.cruising_speed;
cruising_speed(1) = _pos_sp_triplet.current.cruising_speed; cruising_speed(1) = _pos_sp_triplet.current.cruising_speed;
} }
@@ -1062,7 +1062,7 @@ void MulticopterPositionControl::control_auto(float dt)
if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION || if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) &&
previous_setpoint_valid) { previous_setpoint_valid) {
/* follow "previous - current" line */ /* follow "previous - current" line */
@@ -1155,8 +1155,10 @@ void MulticopterPositionControl::control_auto(float dt)
/* update yaw setpoint if needed */ /* update yaw setpoint if needed */
if (_pos_sp_triplet.current.yawspeed_valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) { if (_pos_sp_triplet.current.yawspeed_valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
} else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) { } else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw; _att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} }
@@ -1166,10 +1168,10 @@ void MulticopterPositionControl::control_auto(float dt)
* this makes the takeoff finish smoothly. * this makes the takeoff finish smoothly.
*/ */
if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
|| _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)
&& _pos_sp_triplet.current.acceptance_radius > 0.0f && _pos_sp_triplet.current.acceptance_radius > 0.0f
/* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */ /* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */
&& (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) { && (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) {
_reset_pos_sp = false; _reset_pos_sp = false;
_reset_alt_sp = false; _reset_alt_sp = false;
@@ -1290,26 +1292,30 @@ MulticopterPositionControl::task_main()
if (_local_pos.timestamp > 0) { if (_local_pos.timestamp > 0) {
if (PX4_ISFINITE(_local_pos.x) && if (PX4_ISFINITE(_local_pos.x) &&
PX4_ISFINITE(_local_pos.y) && PX4_ISFINITE(_local_pos.y) &&
PX4_ISFINITE(_local_pos.z)) { PX4_ISFINITE(_local_pos.z)) {
_pos(0) = _local_pos.x; _pos(0) = _local_pos.x;
_pos(1) = _local_pos.y; _pos(1) = _local_pos.y;
if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) { if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) {
_pos(2) = -_local_pos.dist_bottom; _pos(2) = -_local_pos.dist_bottom;
} else { } else {
_pos(2) = _local_pos.z; _pos(2) = _local_pos.z;
} }
} }
if (PX4_ISFINITE(_local_pos.vx) && if (PX4_ISFINITE(_local_pos.vx) &&
PX4_ISFINITE(_local_pos.vy) && PX4_ISFINITE(_local_pos.vy) &&
PX4_ISFINITE(_local_pos.vz)) { PX4_ISFINITE(_local_pos.vz)) {
_vel(0) = _local_pos.vx; _vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy; _vel(1) = _local_pos.vy;
if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) { if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) {
_vel(2) = -_local_pos.dist_bottom_rate; _vel(2) = -_local_pos.dist_bottom_rate;
} else { } else {
_vel(2) = _local_pos.vz; _vel(2) = _local_pos.vz;
} }
@@ -1331,10 +1337,10 @@ MulticopterPositionControl::task_main()
} }
if (_control_mode.flag_control_altitude_enabled || if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled || _control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) { _control_mode.flag_control_acceleration_enabled) {
_vel_ff.zero(); _vel_ff.zero();
@@ -1369,7 +1375,7 @@ MulticopterPositionControl::task_main()
} }
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */ /* idle state, don't run controller and set zero thrust */
R.identity(); R.identity();
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
@@ -1391,7 +1397,7 @@ MulticopterPositionControl::task_main()
} }
} else if (_control_mode.flag_control_manual_enabled } else if (_control_mode.flag_control_manual_enabled
&& _vehicle_land_detected.landed) { && _vehicle_land_detected.landed) {
/* don't run controller when landed */ /* don't run controller when landed */
_reset_pos_sp = true; _reset_pos_sp = true;
_reset_alt_sp = true; _reset_alt_sp = true;
@@ -1428,26 +1434,27 @@ MulticopterPositionControl::task_main()
// guard against any bad velocity values // guard against any bad velocity values
bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) && bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) &&
PX4_ISFINITE(_pos_sp_triplet.current.vy) && PX4_ISFINITE(_pos_sp_triplet.current.vy) &&
_pos_sp_triplet.current.velocity_valid; _pos_sp_triplet.current.velocity_valid;
// do not go slower than the follow target velocity when position tracking is active (set to valid) // do not go slower than the follow target velocity when position tracking is active (set to valid)
if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
velocity_valid && velocity_valid &&
_pos_sp_triplet.current.position_valid) { _pos_sp_triplet.current.position_valid) {
math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0); math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
float cos_ratio = (ft_vel*_vel_sp)/(ft_vel.length()*_vel_sp.length()); float cos_ratio = (ft_vel * _vel_sp) / (ft_vel.length() * _vel_sp.length());
// only override velocity set points when uav is traveling in same direction as target and vector component // only override velocity set points when uav is traveling in same direction as target and vector component
// is greater than calculated position set point velocity component // is greater than calculated position set point velocity component
if(cos_ratio > 0) { if (cos_ratio > 0) {
ft_vel *= (cos_ratio); ft_vel *= (cos_ratio);
// min speed a little faster than target vel // min speed a little faster than target vel
ft_vel += ft_vel.normalized()*1.5f; ft_vel += ft_vel.normalized() * 1.5f;
} else { } else {
ft_vel.zero(); ft_vel.zero();
} }
@@ -1458,7 +1465,7 @@ MulticopterPositionControl::task_main()
// track target using velocity only // track target using velocity only
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
velocity_valid) { velocity_valid) {
_vel_sp(0) = _pos_sp_triplet.current.vx; _vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy; _vel_sp(1) = _pos_sp_triplet.current.vy;
@@ -1479,9 +1486,10 @@ MulticopterPositionControl::task_main()
} }
/* make sure velocity setpoint is saturated in z*/ /* make sure velocity setpoint is saturated in z*/
if (_vel_sp(2) < -1.0f * _params.vel_max_up){ if (_vel_sp(2) < -1.0f * _params.vel_max_up) {
_vel_sp(2) = -1.0f * _params.vel_max_up; _vel_sp(2) = -1.0f * _params.vel_max_up;
} }
if (_vel_sp(2) > _params.vel_max_down) { if (_vel_sp(2) > _params.vel_max_down) {
_vel_sp(2) = _params.vel_max_down; _vel_sp(2) = _params.vel_max_down;
} }
@@ -1508,14 +1516,14 @@ MulticopterPositionControl::task_main()
/* use constant descend rate when landing, ignore altitude setpoint */ /* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed; _vel_sp(2) = _params.land_speed;
} }
/* special thrust setpoint generation for takeoff from ground */ /* special thrust setpoint generation for takeoff from ground */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& _control_mode.flag_armed) { && _control_mode.flag_armed) {
// check if we are not already in air. // check if we are not already in air.
// if yes then we don't need a jumped takeoff anymore // if yes then we don't need a jumped takeoff anymore
@@ -1632,9 +1640,12 @@ MulticopterPositionControl::task_main()
// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction // choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
// given by the last attitude setpoint // given by the last attitude setpoint
_vel_sp(0) = _vel(0) + (-PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0); _vel_sp(0) = _vel(0) + (-PX4_R(_att_sp.R_body, 0,
_vel_sp(1) = _vel(1) + (-PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1); 2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0);
_vel_sp(2) = _vel(2) + (-PX4_R(_att_sp.R_body, 2, 2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2); _vel_sp(1) = _vel(1) + (-PX4_R(_att_sp.R_body, 1,
2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1);
_vel_sp(2) = _vel(2) + (-PX4_R(_att_sp.R_body, 2,
2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2);
_vel_sp_prev(0) = _vel_sp(0); _vel_sp_prev(0) = _vel_sp(0);
_vel_sp_prev(1) = _vel_sp(1); _vel_sp_prev(1) = _vel_sp(1);
_vel_sp_prev(2) = _vel_sp(2); _vel_sp_prev(2) = _vel_sp(2);
@@ -1646,14 +1657,16 @@ MulticopterPositionControl::task_main()
/* thrust vector in NED frame */ /* thrust vector in NED frame */
math::Vector<3> thrust_sp; math::Vector<3> thrust_sp;
if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) { if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) {
thrust_sp = math::Vector<3>(_pos_sp_triplet.current.a_x,_pos_sp_triplet.current.a_y,_pos_sp_triplet.current.a_z); thrust_sp = math::Vector<3>(_pos_sp_triplet.current.a_x, _pos_sp_triplet.current.a_y, _pos_sp_triplet.current.a_z);
} else { } else {
thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
} }
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) { && !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) {
// for jumped takeoffs use special thrust setpoint calculated above // for jumped takeoffs use special thrust setpoint calculated above
thrust_sp.zero(); thrust_sp.zero();
thrust_sp(2) = -_takeoff_thrust_sp; thrust_sp(2) = -_takeoff_thrust_sp;
@@ -1691,7 +1704,7 @@ MulticopterPositionControl::task_main()
/* adjust limits for landing mode */ /* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */ /* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land; tilt_max = _params.tilt_max_land;
@@ -1701,14 +1714,14 @@ MulticopterPositionControl::task_main()
/* descend stabilized, we're landing */ /* descend stabilized, we're landing */
if (!_in_landing && !_lnd_reached_ground if (!_in_landing && !_lnd_reached_ground
&& (float)fabs(_acc_z_lp) < 0.1f && (float)fabs(_acc_z_lp) < 0.1f
&& _vel_z_lp > 0.5f * _params.land_speed) { && _vel_z_lp > 0.5f * _params.land_speed) {
_in_landing = true; _in_landing = true;
} }
/* assume ground, cut thrust */ /* assume ground, cut thrust */
if (_in_landing if (_in_landing
&& _vel_z_lp < 0.1f) { && _vel_z_lp < 0.1f) {
thr_max = 0.0f; thr_max = 0.0f;
_in_landing = false; _in_landing = false;
_lnd_reached_ground = true; _lnd_reached_ground = true;
@@ -1723,9 +1736,9 @@ MulticopterPositionControl::task_main()
/* if we suddenly fall, reset landing logic and remove thrust limit */ /* if we suddenly fall, reset landing logic and remove thrust limit */
if (_lnd_reached_ground if (_lnd_reached_ground
/* XXX: magic value, assuming free fall above 4m/s2 acceleration */ /* XXX: magic value, assuming free fall above 4m/s2 acceleration */
&& (_acc_z_lp > 4.0f && (_acc_z_lp > 4.0f
|| _vel_z_lp > 2.0f * _params.land_speed)) { || _vel_z_lp > 2.0f * _params.land_speed)) {
thr_max = _params.thr_max; thr_max = _params.thr_max;
_in_landing = false; _in_landing = false;
_lnd_reached_ground = false; _lnd_reached_ground = false;
@@ -1968,7 +1981,7 @@ MulticopterPositionControl::task_main()
/* do not move yaw while sitting on the ground */ /* do not move yaw while sitting on the ground */
else if (!_vehicle_land_detected.landed && else if (!_vehicle_land_detected.landed &&
!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) { !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {
/* we want to know the real constraint, and global overrides manual */ /* we want to know the real constraint, and global overrides manual */
const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max : const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
@@ -1982,8 +1995,8 @@ MulticopterPositionControl::task_main()
// If the yaw offset became too big for the system to track stop // If the yaw offset became too big for the system to track stop
// shifting it, only allow if it would make the offset smaller again. // shifting it, only allow if it would make the offset smaller again.
if (fabsf(yaw_offs) < yaw_offset_max || if (fabsf(yaw_offs) < yaw_offset_max ||
(_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) || (_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) ||
(_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) { (_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) {
_att_sp.yaw_body = yaw_target; _att_sp.yaw_body = yaw_target;
} }
} }
@@ -2021,7 +2034,7 @@ MulticopterPositionControl::task_main()
// compute the vector obtained by rotating a z unit vector by the rotation // compute the vector obtained by rotating a z unit vector by the rotation
// given by the roll and pitch commands of the user // given by the roll and pitch commands of the user
math::Vector<3> zB = {0, 0, 1}; math::Vector<3> zB = {0, 0, 1};
math::Matrix<3,3> R_sp_roll_pitch; math::Matrix<3, 3> R_sp_roll_pitch;
R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0); R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB; math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;
@@ -2029,7 +2042,7 @@ MulticopterPositionControl::task_main()
// transform the vector into a new frame which is rotated around the z axis // transform the vector into a new frame which is rotated around the z axis
// by the current yaw error. this vector defines the desired tilt when we look // by the current yaw error. this vector defines the desired tilt when we look
// into the direction of the desired heading // into the direction of the desired heading
math::Matrix<3,3> R_yaw_correction; math::Matrix<3, 3> R_yaw_correction;
R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error); R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp; z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
@@ -2046,8 +2059,8 @@ MulticopterPositionControl::task_main()
/* reset the acceleration set point for all non-attitude flight modes */ /* reset the acceleration set point for all non-attitude flight modes */
if (!(_control_mode.flag_control_offboard_enabled && if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled || !(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) { _control_mode.flag_control_velocity_enabled))) {
_thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust);
} }
@@ -2074,9 +2087,9 @@ MulticopterPositionControl::task_main()
* attitude setpoints for the transition). * attitude setpoints for the transition).
*/ */
if (!(_control_mode.flag_control_offboard_enabled && if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled || !(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled))) { _control_mode.flag_control_acceleration_enabled))) {
if (_att_sp_pub != nullptr) { if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);