dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

initial port of multiplatform version of mc_pos_control

This commit is contained in:
Thomas Gubler 2015-01-28 08:04:00 +01:00
parent 6ba1912309
commit 1915537281
11 changed files with 1676 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
CMakeLists.txt
View File

@ -69,6 +69,11 @@ add_message_files(
actuator_armed.msg
parameter_update.msg
vehicle_status.msg
vehicle_local_position.msg
position_setpoint.msg
position_setpoint_triplet.msg
vehicle_local_position_setpoint.msg
vehicle_global_velocity_setpoint.msg
)
## Generate services in the 'srv' folder
@ -180,6 +185,16 @@ target_link_libraries(mc_att_control
px4
)
## MC Position Control
add_executable(mc_pos_control
src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp)
add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
target_link_libraries(mc_pos_control
${catkin_LIBRARIES}
px4
)
## Attitude Estimator dummy
add_executable(attitude_estimator
src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)

8
ROMFS/px4fmu_common/init.d/rc.mc_apps
View File

@ -14,7 +14,13 @@ else
# try the multiplatform version
mc_att_control_m start
fi
mc_pos_control start
if mc_pos_control start
then
else
# try the multiplatform version
mc_pos_control_m start
fi
#
# Start Land Detector

3
makefiles/config_px4fmu-v2_multiplatform.mk
View File

@ -89,7 +89,8 @@ MODULES += modules/position_estimator_inav
MODULES += modules/mc_att_control_multiplatform
MODULES += examples/subscriber
MODULES += examples/publisher
MODULES += modules/mc_pos_control
# MODULES += modules/mc_pos_control
MODULES += modules/mc_pos_control_multiplatform
MODULES += modules/vtol_att_control
#

947
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp Normal file
View File

@ -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 <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#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<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0);
_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
_parameter_update = _n.subscribe<px4_parameter_update>(
&MulticopterPositionControl::handle_parameter_update, this, 1000);
_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
_armed = _n.subscribe<px4_actuator_armed>(0);
_local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0);
_global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(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<px4_vehicle_local_position_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_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<px4_vehicle_attitude_setpoint>();
}
} 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<px4_vehicle_global_velocity_setpoint>();
}
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<px4_vehicle_attitude_setpoint>();
}
} 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;
}

219
src/modules/mc_pos_control_multiplatform/mc_pos_control.h Normal file
View File

@ -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 <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include <px4.h>
#include <cstdio>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
// #include <poll.h>
// #include <drivers/drv_hrt.h>
// #include <arch/board/board.h>
// #include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
// #include <mavlink/mavlink_log.h>
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<px4_vehicle_attitude_setpoint> *_att_sp_pub; /**< attitude setpoint publication */
Publisher<px4_vehicle_local_position_setpoint> *_local_pos_sp_pub; /**< vehicle local position setpoint publication */
Publisher<px4_vehicle_global_velocity_setpoint> *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
Subscriber<px4_vehicle_attitude> *_att; /**< vehicle attitude */
Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp; /**< vehicle attitude setpoint */
Subscriber<px4_vehicle_control_mode> *_control_mode; /**< vehicle control mode */
Subscriber<px4_parameter_update> *_parameter_update; /**< parameter update */
Subscriber<px4_manual_control_setpoint> *_manual_control_sp; /**< manual control setpoint */
Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */
Subscriber<px4_vehicle_local_position> *_local_pos; /**< local position */
Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet; /**< local position */
Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp; /**< local position */
Subscriber<px4_vehicle_global_velocity_setpoint> *_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);
};

63
src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp Normal file
View File

@ -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 <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#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;
}

212
src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c Normal file
View File

@ -0,0 +1,212 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
*
* 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 <anton.babushkin@me.com>
*/
#include <px4_defines.h>
#include "mc_pos_control_params.h"
#include <systemlib/param/param.h>
/**
* 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);

61
src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h Normal file
View File

@ -0,0 +1,61 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
*
* 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 <anton.babushkin@me.com>
*/
#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

99
src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp Normal file
View File

@ -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 <thomasgubler@gmail.com>
*/
#include <string.h>
#include <cstdlib>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
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;
}

43
src/modules/mc_pos_control_multiplatform/module.mk Normal file
View File

@ -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

8
src/platforms/px4_includes.h
View File

@ -62,6 +62,10 @@
#include <px4_actuator_armed.h>
#include <px4_parameter_update.h>
#include <px4_vehicle_status.h>
#include <px4_vehicle_local_position_setpoint.h>
#include <px4_vehicle_global_velocity_setpoint.h>
#include <px4_vehicle_local_position.h>
#include <px4_position_setpoint_triplet.h>
#endif
#else
@ -85,6 +89,10 @@
#include <platforms/nuttx/px4_messages/px4_actuator_armed.h>
#include <platforms/nuttx/px4_messages/px4_parameter_update.h>
#include <platforms/nuttx/px4_messages/px4_vehicle_status.h>
#include <platforms/nuttx/px4_messages/px4_vehicle_local_position_setpoint.h>
#include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h>
#include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h>
#include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h>
#endif
#include <systemlib/err.h>
#include <systemlib/param/param.h>