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 2b40b9029e..92081e9c88 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -55,7 +55,6 @@ #include #include -#include #include #include #include @@ -66,23 +65,15 @@ #include #include #include -#include #include #include #include -#include #include #include #include -#define TILT_COS_MAX 0.7f -#define SIGMA 0.000001f -#define MIN_DIST 0.01f -#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f -#define ONE_G 9.8066f - /** * Multicopter position control app start / stop handling function * @@ -122,15 +113,11 @@ private: int _vehicle_status_sub; /**< vehicle status subscription */ int _vehicle_land_detected_sub; /**< vehicle land detected subscription */ int _ctrl_state_sub; /**< control state subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ - int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ - int _local_pos_sp_sub; /**< offboard local position setpoint */ - int _global_vel_sp_sub; /**< offboard global velocity setpoint */ int _home_pos_sub; /**< home position */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ @@ -144,7 +131,6 @@ private: struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct actuator_armed_s _arming; /**< actuator arming status */ struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ @@ -297,23 +283,20 @@ private: */ int parameters_update(bool force); - /** - * Update control outputs - */ - void control_update(); - /** * Check for changes in subscribed topics. */ void poll_subscriptions(); - static float throttle_curve(float ctl, float ctr); - void _slow_land_gradual_velocity_limit(); + float throttle_curve(float ctl, float ctr); + + void slow_land_gradual_velocity_limit(); /** * Update reference for local position projection */ void update_ref(); + /** * Reset position setpoint to current position. * @@ -338,7 +321,7 @@ private: */ void control_manual(float dt); - void control_non_manual(float dt); + void control_non_manual(float dt); /** * Set position setpoint using offboard control @@ -354,11 +337,6 @@ private: void vel_sp_slewrate(float dt); - /** - * Select between barometric and global (AMSL) altitudes - */ - void select_alt(bool global); - void update_velocity_derivative(); void do_control(float dt); @@ -392,7 +370,6 @@ private: namespace pos_control { - MulticopterPositionControl *g_control; } @@ -405,14 +382,11 @@ MulticopterPositionControl::MulticopterPositionControl() : /* subscriptions */ _ctrl_state_sub(-1), - _att_sp_sub(-1), _control_mode_sub(-1), _params_sub(-1), _manual_sub(-1), - _arming_sub(-1), _local_pos_sub(-1), _pos_sp_triplet_sub(-1), - _global_vel_sp_sub(-1), _home_pos_sub(-1), /* publications */ @@ -426,7 +400,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _att_sp{}, _manual{}, _control_mode{}, - _arming{}, _local_pos{}, _pos_sp_triplet{}, _local_pos_sp{}, @@ -473,7 +446,7 @@ MulticopterPositionControl::MulticopterPositionControl() : // Make the quaternion valid for control state _ctrl_state.q[0] = 1.0f; - memset(&_ref_pos, 0, sizeof(_ref_pos)); + _ref_pos = {}; _params.pos_p.zero(); _params.vel_p.zero(); @@ -492,7 +465,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _curr_pos_sp.zero(); _R.identity(); - _R_setpoint.identity(); _thrust_int.zero(); @@ -500,6 +472,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); _params_handles.thr_hover = param_find("MPC_THR_HOVER"); + _params_handles.z_p = param_find("MPC_Z_P"); _params_handles.z_vel_p = param_find("MPC_Z_VEL_P"); _params_handles.z_vel_i = param_find("MPC_Z_VEL_I"); @@ -507,6 +480,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.z_vel_max_up = param_find("MPC_Z_VEL_MAX_UP"); _params_handles.z_vel_max_down = param_find("MPC_Z_VEL_MAX_DN"); _params_handles.z_ff = param_find("MPC_Z_FF"); + _params_handles.xy_p = param_find("MPC_XY_P"); _params_handles.xy_vel_p = param_find("MPC_XY_VEL_P"); _params_handles.xy_vel_i = param_find("MPC_XY_VEL_I"); @@ -514,17 +488,21 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX"); _params_handles.xy_vel_cruise = param_find("MPC_XY_CRUISE"); _params_handles.xy_ff = param_find("MPC_XY_FF"); + _params_handles.slow_land_alt1 = param_find("MPC_LAND_ALT1"); _params_handles.slow_land_alt2 = param_find("MPC_LAND_ALT2"); _params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); _params_handles.tko_speed = param_find("MPC_TKO_SPEED"); _params_handles.tilt_max_land = param_find("MPC_TILTMAX_LND"); + _params_handles.man_roll_max = param_find("MPC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX"); + _params_handles.global_yaw_max = param_find("MC_YAWRATE_MAX"); _params_handles.mc_att_yaw_p = param_find("MC_YAW_P"); + _params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY"); _params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z"); _params_handles.acc_up_max = param_find("MPC_ACC_UP_MAX"); @@ -532,7 +510,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.alt_mode = param_find("MPC_ALT_MODE"); _params_handles.opt_recover = param_find("VT_OPT_RECOV_EN"); - /* fetch initial parameter values */ parameters_update(true); } @@ -675,7 +652,6 @@ MulticopterPositionControl::parameters_update(bool force) void MulticopterPositionControl::poll_subscriptions() { - bool updated; orb_check(_vehicle_status_sub, &updated); @@ -725,12 +701,6 @@ MulticopterPositionControl::poll_subscriptions() } } - orb_check(_att_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); - } - orb_check(_control_mode_sub, &updated); if (updated) { @@ -743,12 +713,6 @@ MulticopterPositionControl::poll_subscriptions() orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); } - orb_check(_arming_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - } - orb_check(_local_pos_sub, &updated); if (updated) { @@ -881,7 +845,6 @@ MulticopterPositionControl::reset_alt_sp() } } - void MulticopterPositionControl::limit_altitude() { @@ -951,7 +914,7 @@ MulticopterPositionControl::get_cruising_speed_xy() } void -MulticopterPositionControl::_slow_land_gradual_velocity_limit() +MulticopterPositionControl::slow_land_gradual_velocity_limit() { /* * Make sure downward velocity (positive Z) is limited close to ground. @@ -1024,13 +987,11 @@ MulticopterPositionControl::control_manual(float dt) /* prepare cruise speed (m/s) vector to scale the velocity setpoint */ float vel_mag = (_velocity_hor_manual.get() < _vel_max_xy) ? _velocity_hor_manual.get() : _vel_max_xy; - matrix::Vector3f vel_cruise_scale(vel_mag, vel_mag, - (man_vel_sp(2) > 0.0f) ? _params.vel_max_down : _params.vel_max_up); + matrix::Vector3f vel_cruise_scale(vel_mag, vel_mag, (man_vel_sp(2) > 0.0f) ? _params.vel_max_down : _params.vel_max_up); /* setpoint in NED frame and scaled to cruise velocity */ man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, yaw_input_fame)) * man_vel_sp.emult(vel_cruise_scale); - /* * assisted velocity mode: user controls velocity, but if velocity is small enough, position * hold is activated for the corresponding axis @@ -1095,10 +1056,8 @@ MulticopterPositionControl::control_manual(float dt) _pos_hold_engaged = true; } - } - /* set requested velocity setpoitns */ if (!_alt_hold_engaged) { _pos_sp(2) = _pos(2); @@ -1134,7 +1093,6 @@ MulticopterPositionControl::control_manual(float dt) } else { control_position(dt); } - } void @@ -1249,9 +1207,8 @@ MulticopterPositionControl::control_non_manual(float dt) /* idle state, don't run controller and set zero thrust */ _R_setpoint.identity(); - matrix::Quatf qd = _R_setpoint; - memcpy(&_att_sp.q_d[0], qd.data(), sizeof(_att_sp.q_d)); + qd.copyTo(_att_sp.q_d); _att_sp.q_d_valid = true; _att_sp.roll_body = 0.0f; @@ -1317,7 +1274,6 @@ MulticopterPositionControl::control_offboard(float dt) _hold_offboard_xy = false; } - } if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.alt_valid) { @@ -1473,7 +1429,6 @@ void MulticopterPositionControl::control_auto(float dt) PX4_ISFINITE(_curr_pos_sp(2))) { current_setpoint_valid = true; } - } if (_pos_sp_triplet.previous.valid) { @@ -1485,6 +1440,7 @@ void MulticopterPositionControl::control_auto(float dt) if (PX4_ISFINITE(prev_sp(0)) && PX4_ISFINITE(prev_sp(1)) && PX4_ISFINITE(prev_sp(2))) { + previous_setpoint_valid = true; } } @@ -1515,9 +1471,7 @@ void MulticopterPositionControl::control_auto(float dt) float cruising_speed_z = (_curr_pos_sp(2) > _pos(2)) ? _params.vel_max_down : _params.vel_max_up; /* scaled space: 1 == position error resulting max allowed speed */ - math::Vector<3> cruising_speed(cruising_speed_xy, - cruising_speed_xy, - cruising_speed_z); + math::Vector<3> cruising_speed(cruising_speed_xy, cruising_speed_xy, cruising_speed_z); /* if previous is valid, we want to follow line */ if (previous_setpoint_valid @@ -1533,7 +1487,9 @@ void MulticopterPositionControl::control_auto(float dt) /* by default use current setpoint as is */ math::Vector<3> pos_sp_s = curr_sp_s; - if ((_curr_pos_sp - prev_sp).length() > MIN_DIST) { + const float minimum_dist = 0.01f; + + if ((_curr_pos_sp - prev_sp).length() > minimum_dist) { /* find X - cross point of unit sphere and trajectory */ math::Vector<3> pos_s = _pos.emult(scale); @@ -1546,7 +1502,7 @@ void MulticopterPositionControl::control_auto(float dt) if (curr_pos_s_len < 1.0f) { /* if next is valid, we want to have smooth transition */ - if (next_setpoint_valid && (next_sp - _curr_pos_sp).length() > MIN_DIST) { + if (next_setpoint_valid && (next_sp - _curr_pos_sp).length() > minimum_dist) { math::Vector<3> next_sp_s = next_sp.emult(scale); @@ -1627,11 +1583,11 @@ void MulticopterPositionControl::control_auto(float dt) && _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) { + _do_reset_alt_pos_flag = false; - /* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */ - } else { + /* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */ _do_reset_alt_pos_flag = true; } @@ -1643,13 +1599,14 @@ void MulticopterPositionControl::control_auto(float dt) _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) && !_vehicle_land_detected.landed && high_enough_for_landing_gear) { - _att_sp.landing_gear = 1.0f; - // During takeoff and landing, we better put it down again. + _att_sp.landing_gear = 1.0f; } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND || !high_enough_for_landing_gear) { + + // During takeoff and landing, we better put it down again. _att_sp.landing_gear = -1.0f; } else { @@ -1664,7 +1621,6 @@ void MulticopterPositionControl::control_auto(float dt) void MulticopterPositionControl::update_velocity_derivative() { - /* Update velocity derivative, * independent of the current flight mode */ @@ -1673,7 +1629,6 @@ MulticopterPositionControl::update_velocity_derivative() } // TODO: this logic should be in the estimator, not the controller! - if (PX4_ISFINITE(_local_pos.x) && PX4_ISFINITE(_local_pos.y) && PX4_ISFINITE(_local_pos.z)) { @@ -1681,7 +1636,6 @@ MulticopterPositionControl::update_velocity_derivative() _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; @@ -1713,7 +1667,6 @@ MulticopterPositionControl::update_velocity_derivative() void MulticopterPositionControl::do_control(float dt) { - _vel_ff.zero(); /* by default, run position/altitude controller. the control_* functions @@ -1737,7 +1690,6 @@ MulticopterPositionControl::do_control(float dt) } else { control_non_manual(dt); } - } void @@ -1785,7 +1737,7 @@ MulticopterPositionControl::control_position(float dt) } - _slow_land_gradual_velocity_limit(); + slow_land_gradual_velocity_limit(); if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = true; @@ -1957,7 +1909,6 @@ MulticopterPositionControl::control_position(float dt) float land_z_threshold = 0.1f; - /* assume ground, cut thrust */ if (_in_landing && _vel_z_lp < land_z_threshold) { @@ -2031,11 +1982,13 @@ MulticopterPositionControl::control_position(float dt) /* thrust compensation when vertical velocity but not horizontal velocity is controlled */ float att_comp; - if (_R(2, 2) > TILT_COS_MAX) { + const float tilt_cos_max = 0.7f; + + if (_R(2, 2) > tilt_cos_max) { att_comp = 1.0f / _R(2, 2); } else if (_R(2, 2) > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f; + att_comp = ((1.0f / tilt_cos_max - 1.0f) / tilt_cos_max) * _R(2, 2) + 1.0f; saturation_z = true; } else { @@ -2107,7 +2060,7 @@ MulticopterPositionControl::control_position(float dt) math::Vector<3> body_y; math::Vector<3> body_z; - if (thrust_sp.length() > SIGMA) { + if (thrust_sp.length() > FLT_EPSILON) { body_z = -thrust_sp.normalized(); } else { @@ -2119,7 +2072,7 @@ MulticopterPositionControl::control_position(float dt) /* 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) { + if (fabsf(body_z(2)) > FLT_EPSILON) { /* desired body_x axis, orthogonal to body_z */ body_x = y_C % body_z; @@ -2149,7 +2102,7 @@ MulticopterPositionControl::control_position(float dt) /* copy quaternion setpoint to attitude setpoint topic */ matrix::Quatf q_sp = _R_setpoint; - memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d)); + q_sp.copyTo(_att_sp.q_d); _att_sp.q_d_valid = true; /* calculate euler angles, for logging only, must not be used for control */ @@ -2165,7 +2118,7 @@ MulticopterPositionControl::control_position(float dt) /* copy quaternion setpoint to attitude setpoint topic */ matrix::Quatf q_sp = _R_setpoint; - memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d)); + q_sp.copyTo(_att_sp.q_d); _att_sp.q_d_valid = true; _att_sp.roll_body = 0.0f; @@ -2173,13 +2126,12 @@ MulticopterPositionControl::control_position(float dt) } /* save thrust setpoint for logging */ - _local_pos_sp.acc_x = thrust_sp(0) * ONE_G; - _local_pos_sp.acc_y = thrust_sp(1) * ONE_G; - _local_pos_sp.acc_z = thrust_sp(2) * ONE_G; + _local_pos_sp.acc_x = thrust_sp(0) * CONSTANTS_ONE_G; + _local_pos_sp.acc_y = thrust_sp(1) * CONSTANTS_ONE_G; + _local_pos_sp.acc_z = thrust_sp(2) * CONSTANTS_ONE_G; _att_sp.timestamp = hrt_absolute_time(); - } else { _reset_int_z = true; } @@ -2192,11 +2144,11 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) if (_reset_yaw_sp) { _reset_yaw_sp = false; _att_sp.yaw_body = _yaw; - } - /* 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)) { + } else if (!_vehicle_land_detected.landed && + !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) { + + /* do not move yaw while sitting on the ground */ /* 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 : @@ -2212,6 +2164,7 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) 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_body = yaw_target; } } @@ -2268,7 +2221,7 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) /* copy quaternion setpoint to attitude setpoint topic */ matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); - memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d)); + q_sp.copyTo(_att_sp.q_d); _att_sp.q_d_valid = true; } @@ -2292,29 +2245,21 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) void MulticopterPositionControl::task_main() { - /* * do subscriptions */ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); - _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)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); 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(); @@ -2328,7 +2273,6 @@ MulticopterPositionControl::task_main() // Let's be safe and have the landing gear down by default _att_sp.landing_gear = -1.0f; - /* wakeup source */ px4_pollfd_struct_t fds[1]; @@ -2391,7 +2335,6 @@ MulticopterPositionControl::task_main() update_ref(); - update_velocity_derivative(); // reset the horizontal and vertical position hold flags for non-manual modes