From 4049ec2e966461ba36585b504e1560b4df8851b8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 5 Aug 2016 22:38:34 -0400 Subject: [PATCH] mc_pos_control fix and enforce code style --- Tools/check_code_style_all.sh | 1 - .../mc_pos_control/mc_pos_control_main.cpp | 129 ++++++++++-------- 2 files changed, 71 insertions(+), 59 deletions(-) diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh index 56c86eeff3..4530dff518 100755 --- a/Tools/check_code_style_all.sh +++ b/Tools/check_code_style_all.sh @@ -19,7 +19,6 @@ find src/ \ -path src/modules/ekf_att_pos_estimator -prune -o \ -path src/modules/mavlink -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/navigator -prune -o \ -path src/modules/position_estimator_inav -prune -o \ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0f3a453a2f..167b3ac12d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1001,8 +1001,8 @@ void MulticopterPositionControl::control_auto(float dt) //Make sure that the position setpoint is valid if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) || - !PX4_ISFINITE(_pos_sp_triplet.current.lon) || - !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { + !PX4_ISFINITE(_pos_sp_triplet.current.lon) || + !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { _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); if (PX4_ISFINITE(curr_sp(0)) && - PX4_ISFINITE(curr_sp(1)) && - PX4_ISFINITE(curr_sp(2))) { + PX4_ISFINITE(curr_sp(1)) && + PX4_ISFINITE(curr_sp(2))) { current_setpoint_valid = true; } } @@ -1035,8 +1035,8 @@ void MulticopterPositionControl::control_auto(float dt) prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if (PX4_ISFINITE(prev_sp(0)) && - PX4_ISFINITE(prev_sp(1)) && - PX4_ISFINITE(prev_sp(2))) { + PX4_ISFINITE(prev_sp(1)) && + PX4_ISFINITE(prev_sp(2))) { previous_setpoint_valid = true; } } @@ -1047,7 +1047,7 @@ void MulticopterPositionControl::control_auto(float dt) math::Vector<3> cruising_speed = _params.vel_cruise; 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(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 || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) && - previous_setpoint_valid) { + previous_setpoint_valid) { /* follow "previous - current" line */ @@ -1155,8 +1155,10 @@ void MulticopterPositionControl::control_auto(float dt) /* 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; + } else if (PX4_ISFINITE(_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. */ 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.acceptance_radius > 0.0f - /* 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_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) + && _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 */ + && (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) { _reset_pos_sp = false; _reset_alt_sp = false; @@ -1290,26 +1292,30 @@ MulticopterPositionControl::task_main() if (_local_pos.timestamp > 0) { if (PX4_ISFINITE(_local_pos.x) && - PX4_ISFINITE(_local_pos.y) && - PX4_ISFINITE(_local_pos.z)) { + PX4_ISFINITE(_local_pos.y) && + PX4_ISFINITE(_local_pos.z)) { _pos(0) = _local_pos.x; _pos(1) = _local_pos.y; + if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) { _pos(2) = -_local_pos.dist_bottom; + } else { _pos(2) = _local_pos.z; } } if (PX4_ISFINITE(_local_pos.vx) && - PX4_ISFINITE(_local_pos.vy) && - PX4_ISFINITE(_local_pos.vz)) { + PX4_ISFINITE(_local_pos.vy) && + PX4_ISFINITE(_local_pos.vz)) { _vel(0) = _local_pos.vx; _vel(1) = _local_pos.vy; + if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) { _vel(2) = -_local_pos.dist_bottom_rate; + } else { _vel(2) = _local_pos.vz; } @@ -1331,10 +1337,10 @@ MulticopterPositionControl::task_main() } 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 || - _control_mode.flag_control_acceleration_enabled) { + _control_mode.flag_control_position_enabled || + _control_mode.flag_control_climb_rate_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_acceleration_enabled) { _vel_ff.zero(); @@ -1369,7 +1375,7 @@ MulticopterPositionControl::task_main() } 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 */ R.identity(); 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 - && _vehicle_land_detected.landed) { + && _vehicle_land_detected.landed) { /* don't run controller when landed */ _reset_pos_sp = true; _reset_alt_sp = true; @@ -1428,26 +1434,27 @@ MulticopterPositionControl::task_main() // guard against any bad velocity values bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) && - PX4_ISFINITE(_pos_sp_triplet.current.vy) && - _pos_sp_triplet.current.velocity_valid; + PX4_ISFINITE(_pos_sp_triplet.current.vy) && + _pos_sp_triplet.current.velocity_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 && - velocity_valid && - _pos_sp_triplet.current.position_valid) { + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && + velocity_valid && + _pos_sp_triplet.current.position_valid) { 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 // is greater than calculated position set point velocity component - if(cos_ratio > 0) { + if (cos_ratio > 0) { ft_vel *= (cos_ratio); // min speed a little faster than target vel - ft_vel += ft_vel.normalized()*1.5f; + ft_vel += ft_vel.normalized() * 1.5f; + } else { ft_vel.zero(); } @@ -1458,7 +1465,7 @@ MulticopterPositionControl::task_main() // track target using velocity only } 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(1) = _pos_sp_triplet.current.vy; @@ -1479,9 +1486,10 @@ MulticopterPositionControl::task_main() } /* 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; } + if (_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 */ 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; } /* special thrust setpoint generation for takeoff from ground */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid - && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF - && _control_mode.flag_armed) { + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF + && _control_mode.flag_armed) { // check if we are not already in air. // 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 // 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(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(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(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(1) = _vel_sp(1); _vel_sp_prev(2) = _vel_sp(2); @@ -1646,14 +1657,16 @@ MulticopterPositionControl::task_main() /* thrust vector in NED frame */ math::Vector<3> thrust_sp; + 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 { 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 - && !_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 thrust_sp.zero(); thrust_sp(2) = -_takeoff_thrust_sp; @@ -1691,7 +1704,7 @@ MulticopterPositionControl::task_main() /* 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) { + _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; @@ -1701,14 +1714,14 @@ MulticopterPositionControl::task_main() /* descend stabilized, we're landing */ if (!_in_landing && !_lnd_reached_ground - && (float)fabs(_acc_z_lp) < 0.1f - && _vel_z_lp > 0.5f * _params.land_speed) { + && (float)fabs(_acc_z_lp) < 0.1f + && _vel_z_lp > 0.5f * _params.land_speed) { _in_landing = true; } /* assume ground, cut thrust */ if (_in_landing - && _vel_z_lp < 0.1f) { + && _vel_z_lp < 0.1f) { thr_max = 0.0f; _in_landing = false; _lnd_reached_ground = true; @@ -1723,9 +1736,9 @@ MulticopterPositionControl::task_main() /* if we suddenly fall, reset landing logic and remove thrust limit */ if (_lnd_reached_ground - /* XXX: magic value, assuming free fall above 4m/s2 acceleration */ - && (_acc_z_lp > 4.0f - || _vel_z_lp > 2.0f * _params.land_speed)) { + /* XXX: magic value, assuming free fall above 4m/s2 acceleration */ + && (_acc_z_lp > 4.0f + || _vel_z_lp > 2.0f * _params.land_speed)) { thr_max = _params.thr_max; _in_landing = false; _lnd_reached_ground = false; @@ -1968,7 +1981,7 @@ MulticopterPositionControl::task_main() /* do not move yaw while sitting on the ground */ 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 */ 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 // shifting it, only allow if it would make the offset smaller again. 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; } } @@ -2021,7 +2034,7 @@ MulticopterPositionControl::task_main() // compute the vector obtained by rotating a z unit vector by the rotation // given by the roll and pitch commands of the user 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); 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 // by the current yaw error. this vector defines the desired tilt when we look // 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); 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 */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { _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). */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled))) { + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_acceleration_enabled))) { if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);