From 19155372810e2f70d273b7d06069748d1e01071b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 08:04:00 +0100 Subject: initial port of multiplatform version of mc_pos_control --- .../mc_pos_control.cpp | 947 +++++++++++++++++++++ .../mc_pos_control_multiplatform/mc_pos_control.h | 219 +++++ .../mc_pos_control_main.cpp | 63 ++ .../mc_pos_control_params.c | 212 +++++ .../mc_pos_control_params.h | 61 ++ .../mc_pos_control_start_nuttx.cpp | 99 +++ src/modules/mc_pos_control_multiplatform/module.mk | 43 + 7 files changed, 1644 insertions(+) create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control.h create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp create mode 100644 src/modules/mc_pos_control_multiplatform/module.mk (limited to 'src/modules') diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp new file mode 100644 index 000000000..2685a99c3 --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -0,0 +1,947 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control.cpp + * Multicopter position controller. + * + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#include "mc_pos_control.h" +#include "mc_pos_control_params.h" + +#define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f +#define MIN_DIST 0.01f + +MulticopterPositionControl::MulticopterPositionControl() : + + _task_should_exit(false), + _control_task(-1), + _mavlink_fd(-1), + +/* publications */ + _att_sp_pub(nullptr), + _local_pos_sp_pub(nullptr), + _global_vel_sp_pub(nullptr), + + _ref_alt(0.0f), + _ref_timestamp(0), + + _reset_pos_sp(true), + _reset_alt_sp(true), + _mode_auto(false), + _thrust_int(), + _R() +{ + memset(&_ref_pos, 0, sizeof(_ref_pos)); + + /* + * Do subscriptions + */ + _att = _n.subscribe(&MulticopterPositionControl::handle_vehicle_attitude, this, 0); + _v_att_sp = _n.subscribe(0); + _control_mode = _n.subscribe(0); + _parameter_update = _n.subscribe( + &MulticopterPositionControl::handle_parameter_update, this, 1000); + _manual_control_sp = _n.subscribe(0); + _armed = _n.subscribe(0); + _local_pos_sp = _n.subscribe(0); + _global_vel_sp = _n.subscribe(0); + + + + _params.pos_p.zero(); + _params.vel_p.zero(); + _params.vel_i.zero(); + _params.vel_d.zero(); + _params.vel_max.zero(); + _params.vel_ff.zero(); + _params.sp_offs_max.zero(); + + _pos.zero(); + _pos_sp.zero(); + _vel.zero(); + _vel_sp.zero(); + _vel_prev.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); + + _params_handles.thr_min = PX4_PARAM_INIT(MPC_THR_MIN); + _params_handles.thr_max = PX4_PARAM_INIT(MPC_THR_MAX); + _params_handles.z_p = PX4_PARAM_INIT(MPC_Z_P); + _params_handles.z_vel_p = PX4_PARAM_INIT(MPC_Z_VEL_P); + _params_handles.z_vel_i = PX4_PARAM_INIT(MPC_Z_VEL_I); + _params_handles.z_vel_d = PX4_PARAM_INIT(MPC_Z_VEL_D); + _params_handles.z_vel_max = PX4_PARAM_INIT(MPC_Z_VEL_MAX); + _params_handles.z_ff = PX4_PARAM_INIT(MPC_Z_FF); + _params_handles.xy_p = PX4_PARAM_INIT(MPC_XY_P); + _params_handles.xy_vel_p = PX4_PARAM_INIT(MPC_XY_VEL_P); + _params_handles.xy_vel_i = PX4_PARAM_INIT(MPC_XY_VEL_I); + _params_handles.xy_vel_d = PX4_PARAM_INIT(MPC_XY_VEL_D); + _params_handles.xy_vel_max = PX4_PARAM_INIT(MPC_XY_VEL_MAX); + _params_handles.xy_ff = PX4_PARAM_INIT(MPC_XY_FF); + _params_handles.tilt_max_air = PX4_PARAM_INIT(MPC_TILTMAX_AIR); + _params_handles.land_speed = PX4_PARAM_INIT(MPC_LAND_SPEED); + _params_handles.tilt_max_land = PX4_PARAM_INIT(MPC_TILTMAX_LND); + + /* fetch initial parameter values */ + parameters_update(); + + _R.identity(); +} + +MulticopterPositionControl::~MulticopterPositionControl() +{ +} + +int +MulticopterPositionControl::parameters_update() +{ + PX4_PARAM_GET(_params_handles.thr_min, &_params.thr_min); + PX4_PARAM_GET(_params_handles.thr_max, &_params.thr_max); + PX4_PARAM_GET(_params_handles.tilt_max_air, &_params.tilt_max_air); + _params.tilt_max_air = math::radians(_params.tilt_max_air); + PX4_PARAM_GET(_params_handles.land_speed, &_params.land_speed); + PX4_PARAM_GET(_params_handles.tilt_max_land, &_params.tilt_max_land); + _params.tilt_max_land = math::radians(_params.tilt_max_land); + + float v; + PX4_PARAM_GET(_params_handles.xy_p, &v); + _params.pos_p(0) = v; + _params.pos_p(1) = v; + PX4_PARAM_GET(_params_handles.z_p, &v); + _params.pos_p(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_p, &v); + _params.vel_p(0) = v; + _params.vel_p(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_p, &v); + _params.vel_p(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_i, &v); + _params.vel_i(0) = v; + _params.vel_i(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_i, &v); + _params.vel_i(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_d, &v); + _params.vel_d(0) = v; + _params.vel_d(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_d, &v); + _params.vel_d(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_max, &v); + _params.vel_max(0) = v; + _params.vel_max(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_max, &v); + _params.vel_max(2) = v; + PX4_PARAM_GET(_params_handles.xy_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.vel_ff(0) = v; + _params.vel_ff(1) = v; + PX4_PARAM_GET(_params_handles.z_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.vel_ff(2) = v; + + _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; + + return OK; +} + + +float +MulticopterPositionControl::scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +void +MulticopterPositionControl::update_ref() +{ + if (_local_pos->data().ref_timestamp != _ref_timestamp) { + double lat_sp, lon_sp; + float alt_sp = 0.0f; + + if (_ref_timestamp != 0) { + /* calculate current position setpoint in global frame */ + map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + alt_sp = _ref_alt - _pos_sp(2); + } + + /* update local projection reference */ + map_projection_init(&_ref_pos, _local_pos->data().ref_lat, _local_pos->data().ref_lon); + _ref_alt = _local_pos->data().ref_alt; + + if (_ref_timestamp != 0) { + /* reproject position setpoint to new reference */ + map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(alt_sp - _ref_alt); + } + + _ref_timestamp = _local_pos->data().ref_timestamp; + } +} + +void +MulticopterPositionControl::reset_pos_sp() +{ + if (_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_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0) + - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1) + - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); + + //XXX: port this once a mavlink like interface is available + // mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + PX4_INFO("[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + } +} + +void +MulticopterPositionControl::reset_alt_sp() +{ + if (_reset_alt_sp) { + _reset_alt_sp = false; + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); + + //XXX: port this once a mavlink like interface is available + // mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + PX4_INFO("[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + } +} + +void +MulticopterPositionControl::limit_pos_sp_offset() +{ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode->data().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->data().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); + } +} + +void +MulticopterPositionControl::control_manual(float dt) +{ + _sp_move_rate.zero(); + + if (_control_mode->data().flag_control_altitude_enabled) { + /* move altitude setpoint with throttle stick */ + _sp_move_rate(2) = -scale_control(_manual_control_sp->data().z - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode->data().flag_control_position_enabled) { + /* move position setpoint with roll/pitch stick */ + _sp_move_rate(0) = _manual_control_sp->data().x; + _sp_move_rate(1) = _manual_control_sp->data().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; + } + + /* _sp_move_rate 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_msg.data().yaw_body); + _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); + + if (_control_mode->data().flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + } + + if (_control_mode->data().flag_control_position_enabled) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* 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->data().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->data().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); + } +} + +void +MulticopterPositionControl::control_offboard(float dt) +{ + if (_pos_sp_triplet->data().current.valid) { + if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) { + /* control position */ + _pos_sp(0) = _pos_sp_triplet->data().current.x; + _pos_sp(1) = _pos_sp_triplet->data().current.y; + } else if (_control_mode->data().flag_control_velocity_enabled && _pos_sp_triplet->data().current.velocity_valid) { + /* control velocity */ + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + + /* set position setpoint move rate */ + _sp_move_rate(0) = _pos_sp_triplet->data().current.vx; + _sp_move_rate(1) = _pos_sp_triplet->data().current.vy; + } + + if (_pos_sp_triplet->data().current.yaw_valid) { + _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; + } else if (_pos_sp_triplet->data().current.yawspeed_valid) { + _att_sp_msg.data().yaw_body = _att_sp_msg.data().yaw_body + _pos_sp_triplet->data().current.yawspeed * dt; + } + + if (_control_mode->data().flag_control_altitude_enabled && _pos_sp_triplet->data().current.position_valid) { + /* Control altitude */ + _pos_sp(2) = _pos_sp_triplet->data().current.z; + } else if (_control_mode->data().flag_control_climb_rate_enabled && _pos_sp_triplet->data().current.velocity_valid) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* set altitude setpoint move rate */ + _sp_move_rate(2) = _pos_sp_triplet->data().current.vz; + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } +} + +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) +{ + /* project center of sphere on line */ + /* normalized AB */ + math::Vector<3> ab_norm = line_b - line_a; + ab_norm.normalize(); + math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + /* we have triangle CDX with known CD and CX = R, find DX */ + if (sphere_r > cd_len) { + /* have two roots, select one in A->B direction from D */ + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + res = d + ab_norm * dx_len; + return true; + + } else { + /* have no roots, return D */ + res = d; + return false; + } +} + +void +MulticopterPositionControl::control_auto(float dt) +{ + if (!_mode_auto) { + _mode_auto = true; + /* reset position setpoint on AUTO mode activation */ + reset_pos_sp(); + reset_alt_sp(); + } + + if (_pos_sp_triplet->data().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 */ + math::Vector<3> curr_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().current.lat, _pos_sp_triplet->data().current.lon, + &curr_sp.data[0], &curr_sp.data[1]); + curr_sp(2) = -(_pos_sp_triplet->data().current.alt - _ref_alt); + + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here + + /* convert current setpoint to scaled space */ + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + + /* by default use current setpoint as is */ + math::Vector<3> pos_sp_s = curr_sp_s; + + if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION && _pos_sp_triplet->data().previous.valid) { + /* follow "previous - current" line */ + math::Vector<3> prev_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet->data().previous.alt - _ref_alt); + + if ((curr_sp - prev_sp).length() > MIN_DIST) { + + /* find X - cross point of L1 sphere and trajectory */ + math::Vector<3> pos_s = _pos.emult(scale); + math::Vector<3> prev_sp_s = prev_sp.emult(scale); + 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 L1 radius */ + /* check next waypoint and use it to avoid slowing down when passing via waypoint */ + if (_pos_sp_triplet->data().next.valid) { + math::Vector<3> next_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon, + &next_sp.data[0], &next_sp.data[1]); + next_sp(2) = -(_pos_sp_triplet->data().next.alt - _ref_alt); + + if ((next_sp - curr_sp).length() > MIN_DIST) { + math::Vector<3> next_sp_s = next_sp.emult(scale); + + /* calculate angle prev - curr - next */ + math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; + math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized(); + + /* cos(a) * curr_next, a = angle between current and next trajectory segments */ + float cos_a_curr_next = prev_curr_s_norm * curr_next_s; + + /* cos(b), b = angle pos - curr_sp - prev_sp */ + float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + + 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 L1 radius, limit it */ + if (curr_next_s_len > 1.0f) { + cos_a_curr_next /= curr_next_s_len; + } + + /* 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)); + pos_sp_s += pos_ff; + } + } + } + + } else { + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); + if (near) { + /* L1 sphere crosses trajectory */ + + } else { + /* copter is too far from trajectory */ + /* if copter is behind prev waypoint, go directly to prev waypoint */ + if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) { + pos_sp_s = prev_sp_s; + } + + /* if copter is in front of curr waypoint, go directly to curr waypoint */ + if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) { + pos_sp_s = curr_sp_s; + } + + pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized(); + } + } + } + } + + /* move setpoint not faster than max allowed speed */ + math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); + + /* 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); + } + + /* scale result back to normal space */ + _pos_sp = pos_sp_s.edivide(scale); + + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet->data().current.yaw)) { + _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; + } + + } else { + /* no waypoint, do nothing, setpoint was already reset */ + } +} + +void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg) +{ + parameters_update(); +} + +void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) +{ + static bool reset_int_z = true; + static bool reset_int_z_manual = false; + static bool reset_int_xy = true; + static bool was_armed = false; + static uint64_t t_prev = 0; + + uint64_t t = get_time_micros(); + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; + t_prev = t; + + if (_control_mode->data().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->data().flag_armed; + + update_ref(); + + if (_control_mode->data().flag_control_altitude_enabled || + _control_mode->data().flag_control_position_enabled || + _control_mode->data().flag_control_climb_rate_enabled || + _control_mode->data().flag_control_velocity_enabled) { + + _pos(0) = _local_pos->data().x; + _pos(1) = _local_pos->data().y; + _pos(2) = _local_pos->data().z; + + _vel(0) = _local_pos->data().vx; + _vel(1) = _local_pos->data().vy; + _vel(2) = _local_pos->data().vz; + + _vel_ff.zero(); + _sp_move_rate.zero(); + + /* select control source */ + if (_control_mode->data().flag_control_manual_enabled) { + /* manual control */ + control_manual(dt); + _mode_auto = false; + + } else if (_control_mode->data().flag_control_offboard_enabled) { + /* offboard control */ + control_offboard(dt); + _mode_auto = false; + + } else { + /* AUTO */ + control_auto(dt); + } + + /* fill local position setpoint */ + _local_pos_sp_msg.data().timestamp = get_time_micros(); + _local_pos_sp_msg.data().x = _pos_sp(0); + _local_pos_sp_msg.data().y = _pos_sp(1); + _local_pos_sp_msg.data().z = _pos_sp(2); + _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; + + /* publish local position setpoint */ + if (_local_pos_sp_pub != nullptr) { + _local_pos_sp_pub->publish(_local_pos_sp_msg); + + } else { + _local_pos_sp_pub = _n.advertise(); + } + + + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) { + /* idle state, don't run controller and set zero thrust */ + _R.identity(); + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + _att_sp_msg.data().roll_body = 0.0f; + _att_sp_msg.data().pitch_body = 0.0f; + _att_sp_msg.data().yaw_body = _att->data().yaw; + _att_sp_msg.data().thrust = 0.0f; + + _att_sp_msg.data().timestamp = get_time_micros(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise(); + } + + } 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->data().flag_control_altitude_enabled) { + _reset_alt_sp = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode->data().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->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) { + _vel_sp(2) = _params.land_speed; + } + + _global_vel_sp_msg.data().vx = _vel_sp(0); + _global_vel_sp_msg.data().vy = _vel_sp(1); + _global_vel_sp_msg.data().vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub != nullptr) { + _global_vel_sp_pub->publish(_global_vel_sp_msg); + + } else { + _global_vel_sp_pub = _n.advertise(); + } + + if (_control_mode->data().flag_control_climb_rate_enabled || _control_mode->data().flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode->data().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_control_sp->data().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->data().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->data().flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + + if (!_control_mode->data().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->data().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->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && + _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.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->data().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->data().R, 2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / PX4_R(_att->data().R, 2, 2); + + } else if (PX4_R(_att->data().R, 2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().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->data().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->data().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->data().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_msg.data().yaw_body), cosf(_att_sp_msg.data().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_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = _R.to_euler(); + _att_sp_msg.data().roll_body = euler(0); + _att_sp_msg.data().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->data().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_msg.data().yaw_body); + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + _att_sp_msg.data().roll_body = 0.0f; + _att_sp_msg.data().pitch_body = 0.0f; + } + + _att_sp_msg.data().thrust = thrust_abs; + + _att_sp_msg.data().timestamp = get_time_micros(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise(); + } + + } else { + reset_int_z = true; + } + } + + } 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; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled; +} diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h new file mode 100644 index 000000000..05bd1387b --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control.h + * Multicopter position controller. + * + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +// #include +// #include +// #include +// #include +#include +#include +// #include + +using namespace px4; + +class MulticopterPositionControl +{ +public: + /** + * Constructor + */ + MulticopterPositionControl(); + + /** + * Destructor, also kills task. + */ + ~MulticopterPositionControl(); + + /* Callbacks for topics */ + void handle_vehicle_attitude(const px4_vehicle_attitude &msg); + void handle_parameter_update(const px4_parameter_update &msg); + + void spin() { _n.spin(); } + +protected: + const float alt_ctl_dz = 0.2f; + + bool _task_should_exit; /**< if true, task should exit */ + int _control_task; /**< task handle for task */ + int _mavlink_fd; /**< mavlink fd */ + + Publisher *_att_sp_pub; /**< attitude setpoint publication */ + Publisher *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ + Publisher *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + + + Subscriber *_att; /**< vehicle attitude */ + Subscriber *_v_att_sp; /**< vehicle attitude setpoint */ + Subscriber *_control_mode; /**< vehicle control mode */ + Subscriber *_parameter_update; /**< parameter update */ + Subscriber *_manual_control_sp; /**< manual control setpoint */ + Subscriber *_armed; /**< actuator arming status */ + Subscriber *_local_pos; /**< local position */ + Subscriber *_pos_sp_triplet; /**< local position */ + Subscriber *_local_pos_sp; /**< local position */ + Subscriber *_global_vel_sp; /**< local position */ + + px4_vehicle_attitude_setpoint _att_sp_msg; + px4_vehicle_local_position_setpoint _local_pos_sp_msg; + px4_vehicle_global_velocity_setpoint _global_vel_sp_msg; + + px4::NodeHandle _n; + + + struct { + px4_param_t thr_min; + px4_param_t thr_max; + px4_param_t z_p; + px4_param_t z_vel_p; + px4_param_t z_vel_i; + px4_param_t z_vel_d; + px4_param_t z_vel_max; + px4_param_t z_ff; + px4_param_t xy_p; + px4_param_t xy_vel_p; + px4_param_t xy_vel_i; + px4_param_t xy_vel_d; + px4_param_t xy_vel_max; + px4_param_t xy_ff; + px4_param_t tilt_max_air; + px4_param_t land_speed; + px4_param_t tilt_max_land; + } _params_handles; /**< handles for interesting parameters */ + + struct { + float thr_min; + float thr_max; + float tilt_max_air; + float land_speed; + float tilt_max_land; + + math::Vector<3> pos_p; + math::Vector<3> vel_p; + math::Vector<3> vel_i; + math::Vector<3> vel_d; + math::Vector<3> vel_ff; + math::Vector<3> vel_max; + math::Vector<3> sp_offs_max; + } _params; + + struct map_projection_reference_s _ref_pos; + float _ref_alt; + uint64_t _ref_timestamp; + + bool _reset_pos_sp; + bool _reset_alt_sp; + bool _mode_auto; + + math::Vector<3> _pos; + math::Vector<3> _pos_sp; + math::Vector<3> _vel; + math::Vector<3> _vel_sp; + math::Vector<3> _vel_prev; /**< velocity on previous step */ + math::Vector<3> _vel_ff; + math::Vector<3> _sp_move_rate; + + math::Vector<3> _thrust_int; + math::Matrix<3, 3> _R; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + */ + void control_update(); + + static float scale_control(float ctl, float end, float dz); + + /** + * Update reference for local position projection + */ + void update_ref(); + /** + * Reset position setpoint to current position + */ + void reset_pos_sp(); + + /** + * Reset altitude setpoint to current altitude + */ + void reset_alt_sp(); + + /** + * Check if position setpoint is too far from current position and adjust it if needed. + */ + void limit_pos_sp_offset(); + + /** + * Set position setpoint using manual control + */ + void control_manual(float dt); + + /** + * Set position setpoint using offboard control + */ + 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); + + /** + * Set position setpoint for AUTO + */ + void control_auto(float dt); + + /** + * Select between barometric and global (AMSL) altitudes + */ + void select_alt(bool global); +}; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp new file mode 100644 index 000000000..0b5775736 --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_main.cpp + * Multicopter position controller. + * + * The controller has two loops: P loop for position error and PID loop for velocity error. + * Output of velocity controller is thrust vector that splitted to thrust direction + * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#include "mc_pos_control.h" + +bool thread_running = false; /**< Deamon status flag */ + +int main(int argc, char **argv) +{ + px4::init(argc, argv, "mc_pos_control_m"); + + PX4_INFO("starting"); + MulticopterPositionControl posctl; + thread_running = true; + posctl.spin(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c new file mode 100644 index 000000000..c741a7f0a --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_params.c + * Multicopter position controller parameters. + * + * @author Anton Babushkin + */ + +#include +#include "mc_pos_control_params.h" +#include + +/** + * Minimum thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN); + +/** + * Maximum thrust + * + * Limit max allowed thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_P); + +/** + * Proportional gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P); + +/** + * Integral gain for vertical velocity error + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I); + +/** + * Differential gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D); + +/** + * Maximum vertical velocity + * + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL). + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX); + +/** + * Vertical velocity feed forward + * + * Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_P); + +/** + * Proportional gain for horizontal velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P); + +/** + * Integral gain for horizontal velocity error + * + * Non-zero value allows to resist wind. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D); + +/** + * Maximum horizontal velocity + * + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX); + +/** + * Horizontal velocity feed forward + * + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF); + +/** + * Maximum tilt angle in air + * + * Limits maximum tilt in AUTO and POSCTRL modes during flight. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR); + +/** + * Maximum tilt during landing + * + * Limits maximum tilt angle on landing. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); + +/** + * Landing descend rate + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); + diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h new file mode 100644 index 000000000..fec3bcb7c --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_params.h + * Multicopter position controller parameters. + * + * @author Anton Babushkin + */ + +#pragma once + +#define PARAM_MPC_THR_MIN_DEFAULT 0.1f +#define PARAM_MPC_THR_MAX_DEFAULT 1.0f +#define PARAM_MPC_Z_P_DEFAULT 1.0f +#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f +#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f +#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f +#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPC_Z_FF_DEFAULT 0.5f +#define PARAM_MPC_XY_P_DEFAULT 1.0f +#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f +#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f +#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f +#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPC_XY_FF_DEFAULT 0.5f +#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f +#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f +#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f + diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp new file mode 100644 index 000000000..87996d93b --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2013 - 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_m_start_nuttx.cpp + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]); +int mc_pos_control_m_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: mc_pos_control_m {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("mc_pos_control_m", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 3000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/mc_pos_control_multiplatform/module.mk b/src/modules/mc_pos_control_multiplatform/module.mk new file mode 100644 index 000000000..2852ebbec --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build multicopter position controller +# + +MODULE_COMMAND = mc_pos_control_m + +SRCS = mc_pos_control_main.cpp \ + mc_pos_control_start_nuttx.cpp \ + mc_pos_control.cpp \ + mc_pos_control_params.c -- cgit v1.2.3