From 5303a63da0ab3bf54d7a76f229bf757c47ebbead Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 20 Nov 2015 13:16:42 +0100 Subject: [PATCH] mc_pos_control: code formatting --- .../mc_pos_control/mc_pos_control_main.cpp | 138 ++++++++++-------- 1 file changed, 76 insertions(+), 62 deletions(-) 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 1b261bef81..a26a65d468 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -279,8 +279,8 @@ private: */ void control_offboard(float dt); - bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, - const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + bool cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res); /** * Set position setpoint for AUTO @@ -321,7 +321,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _control_task(-1), _mavlink_fd(-1), -/* subscriptions */ + /* subscriptions */ _ctrl_state_sub(-1), _att_sp_sub(-1), _control_mode_sub(-1), @@ -332,7 +332,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _pos_sp_triplet_sub(-1), _global_vel_sp_sub(-1), -/* publications */ + /* publications */ _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), @@ -517,7 +517,7 @@ MulticopterPositionControl::parameters_update(bool force) _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); - param_get(_params_handles.mc_att_yaw_p,&v); + param_get(_params_handles.mc_att_yaw_p, &v); _params.mc_att_yaw_p = v; } @@ -626,9 +626,9 @@ MulticopterPositionControl::reset_pos_sp() _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - - _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0); + - _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - - _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1); + - _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -703,7 +703,8 @@ MulticopterPositionControl::control_manual(float dt) /* _req_vel_sp scaled to 0..1, scale it 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); - math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_max); // in NED and scaled to actual velocity + math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult( + _params.vel_max); // in NED and scaled to actual velocity /* * assisted velocity mode: user controls velocity, but if velocity is small enough, position @@ -711,20 +712,20 @@ MulticopterPositionControl::control_manual(float dt) */ /* horizontal axes */ - if (_control_mode.flag_control_position_enabled) - { + if (_control_mode.flag_control_position_enabled) { /* check for pos. hold */ - if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) - { + if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { if (!_pos_hold_engaged) { - if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) { + if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy + && fabsf(_vel(1)) < _params.hold_max_xy)) { _pos_hold_engaged = true; + } else { _pos_hold_engaged = false; } } - } - else { + + } else { _pos_hold_engaged = false; } @@ -739,21 +740,19 @@ MulticopterPositionControl::control_manual(float dt) } /* vertical axis */ - if (_control_mode.flag_control_altitude_enabled) - { + if (_control_mode.flag_control_altitude_enabled) { /* check for pos. hold */ - if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) - { - if (!_alt_hold_engaged) - { + if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) { + if (!_alt_hold_engaged) { if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) { _alt_hold_engaged = true; + } else { _alt_hold_engaged = false; } } - } - else { + + } else { _alt_hold_engaged = false; } @@ -781,6 +780,7 @@ MulticopterPositionControl::control_offboard(float dt) /* control position */ _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ /* reset position setpoint to current position if needed */ @@ -795,6 +795,7 @@ MulticopterPositionControl::control_offboard(float dt) if (_pos_sp_triplet.current.yaw_valid) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } else if (_pos_sp_triplet.current.yawspeed_valid) { _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } @@ -802,6 +803,7 @@ MulticopterPositionControl::control_offboard(float dt) if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) { /* Control altitude */ _pos_sp(2) = _pos_sp_triplet.current.z; + } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); @@ -811,6 +813,7 @@ MulticopterPositionControl::control_offboard(float dt) _run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } + } else { reset_pos_sp(); reset_alt_sp(); @@ -818,8 +821,8 @@ MulticopterPositionControl::control_offboard(float dt) } bool -MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, - const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res) { /* project center of sphere on line */ /* normalized AB */ @@ -854,13 +857,14 @@ void MulticopterPositionControl::control_auto(float dt) //Poll position setpoint 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); //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; } } @@ -880,21 +884,21 @@ 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; } } if (_pos_sp_triplet.previous.valid) { map_projection_project(&_ref_pos, - _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, - &prev_sp.data[0], &prev_sp.data[1]); + _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); 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; } } @@ -924,14 +928,15 @@ void MulticopterPositionControl::control_auto(float dt) math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; math::Vector<3> curr_pos_s = pos_s - curr_sp_s; float curr_pos_s_len = curr_pos_s.length(); + if (curr_pos_s_len < 1.0f) { /* copter is closer to waypoint than unit radius */ /* check next waypoint and use it to avoid slowing down when passing via waypoint */ if (_pos_sp_triplet.next.valid) { math::Vector<3> next_sp; map_projection_project(&_ref_pos, - _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, - &next_sp.data[0], &next_sp.data[1]); + _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, + &next_sp.data[0], &next_sp.data[1]); next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); if ((next_sp - curr_sp).length() > MIN_DIST) { @@ -949,6 +954,7 @@ void MulticopterPositionControl::control_auto(float dt) if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than unit radius, limit it */ if (curr_next_s_len > 1.0f) { cos_a_curr_next /= curr_next_s_len; @@ -956,8 +962,8 @@ void MulticopterPositionControl::control_auto(float dt) /* feed forward position setpoint offset */ math::Vector<3> pos_ff = prev_curr_s_norm * - cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * - (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); pos_sp_s += pos_ff; } } @@ -965,6 +971,7 @@ void MulticopterPositionControl::control_auto(float dt) } else { bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); + if (near) { /* unit sphere crosses trajectory */ @@ -992,6 +999,7 @@ void MulticopterPositionControl::control_auto(float dt) /* difference between current and desired position setpoints, 1 = max speed */ math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); } @@ -1149,7 +1157,8 @@ MulticopterPositionControl::task_main() 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) { + 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)); @@ -1182,18 +1191,20 @@ MulticopterPositionControl::task_main() } /* make sure velocity setpoint is saturated in xy*/ - float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + - _vel_sp(1)*_vel_sp(1)); - if (vel_norm_xy > _params.vel_max(0)) { + float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + + _vel_sp(1) * _vel_sp(1)); + + if (vel_norm_xy > _params.vel_max(0)) { /* note assumes vel_max(0) == vel_max(1) */ - _vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy; - _vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy; + _vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy; + _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy; } /* make sure velocity setpoint is saturated in z*/ - float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2)); + float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2)); + if (vel_norm_z > _params.vel_max(2)) { - _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; + _vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z; } if (!_control_mode.flag_control_position_enabled) { @@ -1214,7 +1225,8 @@ 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) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -1304,7 +1316,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; @@ -1514,6 +1526,7 @@ MulticopterPositionControl::task_main() /* publish local position setpoint */ if (_local_pos_sp_pub != nullptr) { 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); } @@ -1528,7 +1541,7 @@ MulticopterPositionControl::task_main() } /* generate attitude setpoint from manual controls */ - if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { + 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) { @@ -1537,8 +1550,7 @@ MulticopterPositionControl::task_main() } /* do not move yaw while arming */ - else if (_manual.z > 0.1f) - { + 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; @@ -1569,8 +1581,8 @@ MulticopterPositionControl::task_main() } /* 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); + 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)); /* copy quaternion setpoint to attitude setpoint topic */ @@ -1578,8 +1590,8 @@ MulticopterPositionControl::task_main() q_sp.from_dcm(R_sp); memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); - } - else { + + } else { reset_yaw_sp = true; } @@ -1591,17 +1603,19 @@ MulticopterPositionControl::task_main() * 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))) { + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + } else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { _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; + reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled + && !_control_mode.flag_control_climb_rate_enabled; } mavlink_log_info(_mavlink_fd, "[mpc] stopped"); @@ -1616,11 +1630,11 @@ MulticopterPositionControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("mc_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (px4_main_t)&MulticopterPositionControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (px4_main_t)&MulticopterPositionControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed");