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

mc_pos_control: replacement for multirotor_pos_control, rewritten to C++ and new mathlib

This commit is contained in:
Anton Babushkin 2013-12-26 23:03:19 +04:00
parent e103729de3
commit 20c9ce9d6d
9 changed files with 1009 additions and 1313 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/lib/mathlib/math/Vector.hpp
View File

@ -261,6 +261,30 @@ public:
return res;
}
/**
* element by element multiplication
*/
const Vector<N> emult(const Vector<N> &v) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] * v.data[i];
return res;
}
/**
* element by element division
*/
const Vector<N> edivide(const Vector<N> &v) const {
Vector<N> res;
for (unsigned int i = 0; i < N; i++)
res.data[i] = data[i] / v.data[i];
return res;
}
/**
* gets the length of this vector squared
*/

956
src/modules/mc_pos_control/mc_pos_control_main.cpp Normal file
View File

@ -0,0 +1,956 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* 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_main.cpp
*
* Multirotor position controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <mavlink/mavlink_log.h>
#define TILT_COS_MAX 0.7f
#define SIGMA 0.000001f
/**
* Multicopter position control app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
class MulticopterPositionControl
{
public:
/**
* Constructor
*/
MulticopterPositionControl();
/**
* Destructor, also kills task.
*/
~MulticopterPositionControl();
/**
* Start task.
*
* @return OK on success.
*/
int start();
private:
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
int _att_sub; /**< vehicle attitude subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _arming_sub; /**< arming status of outputs */
int _local_pos_sub; /**< vehicle local position */
int _global_pos_sp_sub; /**< vehicle global position setpoint */
orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
struct actuator_armed_s _arming; /**< actuator arming status */
struct vehicle_local_position_s _local_pos; /**< vehicle local position */
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */
struct vehicle_global_position_setpoint_s _global_pos_sp; /**< vehicle global position setpoint */
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
struct {
param_t takeoff_alt;
param_t takeoff_gap;
param_t thr_min;
param_t thr_max;
param_t z_p;
param_t z_vel_p;
param_t z_vel_i;
param_t z_vel_d;
param_t z_vel_max;
param_t z_ff;
param_t xy_p;
param_t xy_vel_p;
param_t xy_vel_i;
param_t xy_vel_d;
param_t xy_vel_max;
param_t xy_ff;
param_t tilt_max;
param_t rc_scale_pitch;
param_t rc_scale_roll;
param_t rc_scale_yaw;
} _params_handles; /**< handles for interesting parameters */
struct {
float takeoff_alt;
float takeoff_gap;
float thr_min;
float thr_max;
float tilt_max;
float rc_scale_pitch;
float rc_scale_roll;
float rc_scale_yaw;
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;
math::Vector<3> _pos;
math::Vector<3> _vel;
math::Vector<3> _pos_sp;
math::Vector<3> _vel_sp;
math::Vector<3> _vel_err_prev; /**< velocity on previous step */
hrt_abstime _time_prev;
bool _was_armed;
/**
* Update our local parameter cache.
*/
int parameters_update(bool force);
/**
* Update control outputs
*/
void control_update();
/**
* Check for changes in subscribed topics.
*/
void poll_subscriptions();
static float scale_control(float ctl, float end, float dz);
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
};
namespace pos_control
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
MulticopterPositionControl *g_control;
}
MulticopterPositionControl::MulticopterPositionControl() :
_task_should_exit(false),
_control_task(-1),
/* subscriptions */
_att_sub(-1),
_att_sp_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
_manual_sub(-1),
_arming_sub(-1),
_local_pos_sub(-1),
_global_pos_sp_sub(-1),
/* publications */
_local_pos_sp_pub(-1),
_att_sp_pub(-1),
_global_vel_sp_pub(-1),
_time_prev(0),
_was_armed(false)
{
memset(&_att, 0, sizeof(_att));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
memset(&_control_mode, 0, sizeof(_control_mode));
memset(&_arming, 0, sizeof(_arming));
memset(&_local_pos, 0, sizeof(_local_pos));
memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
memset(&_global_pos_sp, 0, sizeof(_global_pos_sp));
memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
_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();
_vel.zero();
_pos_sp.zero();
_vel_sp.zero();
_vel_err_prev.zero();
_params_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
_params_handles.takeoff_gap = param_find("NAV_TAKEOFF_GAP");
_params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX");
_params_handles.z_p = param_find("MPC_Z_P");
_params_handles.z_vel_p = param_find("MPC_Z_VEL_P");
_params_handles.z_vel_i = param_find("MPC_Z_VEL_I");
_params_handles.z_vel_d = param_find("MPC_Z_VEL_D");
_params_handles.z_vel_max = param_find("MPC_Z_VEL_MAX");
_params_handles.z_ff = param_find("MPC_Z_FF");
_params_handles.xy_p = param_find("MPC_XY_P");
_params_handles.xy_vel_p = param_find("MPC_XY_VEL_P");
_params_handles.xy_vel_i = param_find("MPC_XY_VEL_I");
_params_handles.xy_vel_d = param_find("MPC_XY_VEL_D");
_params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX");
_params_handles.xy_ff = param_find("MPC_XY_FF");
_params_handles.tilt_max = param_find("MPC_TILT_MAX");
_params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
_params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
_params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
/* fetch initial parameter values */
parameters_update(true);
}
MulticopterPositionControl::~MulticopterPositionControl()
{
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
pos_control::g_control = nullptr;
}
int
MulticopterPositionControl::parameters_update(bool force)
{
bool updated;
struct parameter_update_s param_upd;
orb_check(_params_sub, &updated);
if (updated)
orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd);
if (updated || force) {
param_get(_params_handles.takeoff_alt, &_params.takeoff_alt);
param_get(_params_handles.takeoff_gap, &_params.takeoff_gap);
param_get(_params_handles.thr_min, &_params.thr_min);
param_get(_params_handles.thr_max, &_params.thr_max);
param_get(_params_handles.tilt_max, &_params.tilt_max);
param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch);
param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll);
param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
float v;
param_get(_params_handles.xy_p, &v);
_params.pos_p(0) = v;
_params.pos_p(1) = v;
param_get(_params_handles.z_p, &v);
_params.pos_p(2) = v;
param_get(_params_handles.xy_vel_p, &v);
_params.vel_p(0) = v;
_params.vel_p(1) = v;
param_get(_params_handles.z_vel_p, &v);
_params.vel_p(2) = v;
param_get(_params_handles.xy_vel_i, &v);
_params.vel_i(0) = v;
_params.vel_i(1) = v;
param_get(_params_handles.z_vel_i, &v);
_params.vel_i(2) = v;
param_get(_params_handles.xy_vel_d, &v);
_params.vel_d(0) = v;
_params.vel_d(1) = v;
param_get(_params_handles.z_vel_d, &v);
_params.vel_d(2) = v;
param_get(_params_handles.xy_vel_max, &v);
_params.vel_max(0) = v;
_params.vel_max(1) = v;
param_get(_params_handles.z_vel_max, &v);
_params.vel_max(2) = v;
param_get(_params_handles.xy_ff, &v);
_params.vel_ff(0) = v;
_params.vel_ff(1) = v;
param_get(_params_handles.z_ff, &v);
_params.vel_ff(2) = v;
_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
}
return OK;
}
void
MulticopterPositionControl::poll_subscriptions()
{
bool updated;
orb_check(_att_sub, &updated);
if (updated)
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_check(_att_sp_sub, &updated);
if (updated)
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
orb_check(_control_mode_sub, &updated);
if (updated)
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
orb_check(_manual_sub, &updated);
if (updated)
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
orb_check(_arming_sub, &updated);
if (updated)
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
orb_check(_global_pos_sp_sub, &updated);
if (updated)
orb_copy(ORB_ID(vehicle_global_position_setpoint), _global_pos_sp_sub, &_global_pos_sp);
}
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::task_main_trampoline(int argc, char *argv[])
{
pos_control::g_control->task_main();
}
void
MulticopterPositionControl::task_main()
{
warnx("started");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[mpc] started");
/*
* do subscriptions
*/
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
parameters_update(true);
/* initialize values of critical structs until first regular update */
_arming.armed = false;
/* get an initial update for all sensor and status data */
poll_subscriptions();
bool reset_mission_sp = false;
bool global_pos_sp_valid = false;
bool reset_man_sp_z = true;
bool reset_man_sp_xy = true;
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool was_armed = false;
bool reset_auto_sp_xy = true;
bool reset_auto_sp_z = true;
bool reset_takeoff_sp = true;
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float ref_alt = 0.0f;
hrt_abstime ref_alt_t = 0;
hrt_abstime local_ref_timestamp = 0;
math::Vector<3> sp_move_rate;
sp_move_rate.zero();
math::Vector<3> thrust_int;
thrust_int.zero();
math::Matrix<3, 3> R;
R.identity();
/* wakeup source */
struct pollfd fds[1];
/* Setup of loop */
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* timed out - periodic check for _task_should_exit */
if (pret == 0)
continue;
/* this is undesirable but not much we can do */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
poll_subscriptions();
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
float dt = _time_prev != 0 ? (t - _time_prev) * 0.000001f : 0.0f;
_time_prev = t;
if (_control_mode.flag_armed && !_was_armed) {
/* reset setpoints and integrals on arming */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_auto_sp_z = true;
reset_auto_sp_xy = true;
reset_takeoff_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
_was_armed = _control_mode.flag_armed;
if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) {
_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
_pos(2) = _local_pos.z;
_vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy;
_vel(2) = _local_pos.vz;
sp_move_rate.zero();
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (_local_pos.ref_timestamp != ref_alt_t) {
if (ref_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
_pos_sp(2) += _local_pos.ref_alt - ref_alt;
}
ref_alt_t = _local_pos.ref_timestamp;
ref_alt = _local_pos.ref_alt;
// TODO also correct XY setpoint
}
/* reset setpoints to current position if needed */
if (_control_mode.flag_control_altitude_enabled) {
if (reset_man_sp_z) {
reset_man_sp_z = false;
_pos_sp(2) = _pos(2);
mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2));
}
/* move altitude setpoint with throttle stick */
sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
}
if (_control_mode.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = false;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
}
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz);
}
/* limit setpoint move rate */
float sp_move_norm = sp_move_rate.length();
if (sp_move_norm > 1.0f) {
sp_move_rate /= sp_move_norm;
}
/* scale to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
/* 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 - _pos).edivide(_params.vel_max);
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.vel_max);
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
_local_pos_sp.yaw = _att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
reset_auto_sp_xy = !_control_mode.flag_control_position_enabled;
reset_auto_sp_z = !_control_mode.flag_control_altitude_enabled;
reset_takeoff_sp = true;
/* force reprojection of global setpoint after manual mode */
reset_mission_sp = true;
} else {
// TODO AUTO
_pos_sp = _pos;
}
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
/* publish local position setpoint */
if (_local_pos_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos;
_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
if (!_control_mode.flag_control_altitude_enabled) {
reset_man_sp_z = true;
_vel_sp(2) = 0.0f;
}
if (!_control_mode.flag_control_position_enabled) {
reset_man_sp_xy = true;
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
if (!_control_mode.flag_control_manual_enabled) {
/* limit 3D speed only in AUTO mode */
float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
if (vel_sp_norm > 1.0f) {
_vel_sp /= vel_sp_norm;
}
}
_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);
/* publish velocity setpoint */
if (_global_vel_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
} else {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
}
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
/* reset integrals if needed */
if (_control_mode.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.throttle;
if (i < _params.thr_min) {
i = _params.thr_min;
} else if (i > _params.thr_max) {
i = _params.thr_max;
}
}
thrust_int(2) = -i;
mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
} else {
reset_int_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
if (reset_int_xy) {
reset_int_xy = false;
thrust_int(0) = 0.0f;
thrust_int(1) = 0.0f;
mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral");
}
} else {
reset_int_xy = true;
}
/* calculate desired thrust vector in NED frame */
math::Vector<3> vel_err = _vel_sp - _vel;
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + (vel_err - _vel_err_prev).emult(_params.vel_d) / dt + thrust_int;
_vel_err_prev = vel_err;
if (!_control_mode.flag_control_velocity_enabled) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
if (!_control_mode.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.flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
thr_min = 0.0f;
}
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
/* limit max tilt */
float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
if (tilt > _params.tilt_max && _params.thr_min >= 0.0f) {
/* crop horizontal component */
float k = tanf(_params.tilt_max) / tanf(tilt);
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
/* 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 */
math::Vector<3> m;
m(0) = (_control_mode.flag_control_velocity_enabled && !saturation_xy) ? 1.0f : 0.0f;
m(1) = m(0);
m(2) = (_control_mode.flag_control_climb_rate_enabled && !saturation_z) ? 1.0f : 0.0f;
thrust_int += vel_err.emult(_params.vel_i) * dt;
/* calculate attitude and thrust from thrust vector */
if (_control_mode.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.yaw_body), cosf(_att_sp.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.R_body[0][0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
math::Vector<3> euler = R.to_euler();
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
} else {
/* thrust compensation for altitude only control mode */
float att_comp;
if (_att.R[2][2] > TILT_COS_MAX)
att_comp = 1.0f / _att.R[2][2];
else if (_att.R[2][2] > 0.0f)
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
else
att_comp = 1.0f;
thrust_abs *= att_comp;
}
_att_sp.thrust = thrust_abs;
_att_sp.timestamp = hrt_absolute_time();
/* publish attitude setpoint */
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
} else {
reset_int_z = true;
}
} else {
/* position controller disabled, reset setpoints */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_int_z = true;
reset_int_xy = true;
reset_mission_sp = true;
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
}
warnx("stopped");
mavlink_log_info(mavlink_fd, "[mpc] stopped");
_control_task = -1;
_exit(0);
}
int
MulticopterPositionControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = task_spawn_cmd("mc_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
(main_t)&MulticopterPositionControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int mc_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
errx(1, "usage: mc_pos_control {start|stop|status}");
if (!strcmp(argv[1], "start")) {
if (pos_control::g_control != nullptr)
errx(1, "already running");
pos_control::g_control = new MulticopterPositionControl;
if (pos_control::g_control == nullptr)
errx(1, "alloc failed");
if (OK != pos_control::g_control->start()) {
delete pos_control::g_control;
pos_control::g_control = nullptr;
err(1, "start failed");
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (pos_control::g_control == nullptr)
errx(1, "not running");
delete pos_control::g_control;
pos_control::g_control = nullptr;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (pos_control::g_control) {
errx(0, "running");
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
return 1;
}

25
src/modules/mc_pos_control/mc_pos_control_params.c Normal file
View File

@ -0,0 +1,25 @@
/*
* mc_pos_control_params.c
*
* Created on: 26.12.2013
* Author: ton
*/
#include <systemlib/param/param.h>
/* multicopter position controller parameters */
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f);
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.05f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.01f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);

9
src/modules/multirotor_pos_control/module.mk → src/modules/mc_pos_control/module.mk
View File

@ -32,11 +32,10 @@
############################################################################
#
# Build multirotor position control
# Build multicopter position controller
#
MODULE_COMMAND = multirotor_pos_control
MODULE_COMMAND = mc_pos_control
SRCS = multirotor_pos_control.c \
multirotor_pos_control_params.c \
thrust_pid.c
SRCS = mc_pos_control_main.cpp \
mc_pos_control_params.c

864
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -1,864 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* 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 multirotor_pos_control.c
*
* Multirotor position controller
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <termios.h>
#include <time.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <systemlib/systemlib.h>
#include <systemlib/pid/pid.h>
#include <mavlink/mavlink_log.h>
#include "multirotor_pos_control_params.h"
#include "thrust_pid.h"
#define TILT_COS_MAX 0.7f
#define SIGMA 0.000001f
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
/**
* Mainloop of position controller.
*/
static int multirotor_pos_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static float scale_control(float ctl, float end, float dz);
static float norm(float x, float y);
static void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn().
*/
int multirotor_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
warnx("start");
thread_should_exit = false;
deamon_task = task_spawn_cmd("multirotor_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 60,
4096,
multirotor_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
warnx("stop");
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("app is running");
} else {
warnx("app not started");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
static float 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;
}
}
static float norm(float x, float y)
{
return sqrtf(x * x + y * y);
}
static float norm3(float x, float y, float z)
{
return sqrtf(x * x + y * y + z * z);
}
static void cross3(float a[3], float b[3], float res[3])
{
res[0] = a[1] * b[2] - a[2] * b[1];
res[1] = a[2] * b[0] - a[0] * b[2];
res[2] = a[0] * b[1] - a[1] * b[0];
}
static float normalize3(float x[3])
{
float n = sqrtf(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
if (n > 0.0f) {
x[0] /= n;
x[1] /= n;
x[2] /= n;
}
return n;
}
static float rt_atan2f_snf(float u0, float u1)
{
float y;
int32_t b_u0;
int32_t b_u1;
if (isnanf(u0) || isnanf(u1)) {
y = NAN;
} else if (isinff(u0) && isinff(u1)) {
if (u0 > 0.0f) {
b_u0 = 1;
} else {
b_u0 = -1;
}
if (u1 > 0.0f) {
b_u1 = 1;
} else {
b_u1 = -1;
}
y = atan2f((float)b_u0, (float)b_u1);
} else if (u1 == 0.0f) {
if (u0 > 0.0f) {
y = M_PI_F / 2.0f;
} else if (u0 < 0.0f) {
y = -(M_PI_F / 2.0f);
} else {
y = 0.0F;
}
} else {
y = atan2f(u0, u1);
}
return y;
}
static int multirotor_pos_control_thread_main(int argc, char *argv[])
{
/* welcome user */
warnx("started");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[mpc] started");
/* structures */
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(global_pos_sp));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
/* subscribe to attitude, motor setpoints and system state */
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
/* publish setpoint */
orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
bool reset_mission_sp = false;
bool global_pos_sp_valid = false;
bool reset_man_sp_z = true;
bool reset_man_sp_xy = true;
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool was_armed = false;
bool reset_auto_sp_xy = true;
bool reset_auto_sp_z = true;
bool reset_takeoff_sp = true;
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float ref_alt = 0.0f;
hrt_abstime ref_alt_t = 0;
uint64_t local_ref_timestamp = 0;
float thrust_int[3] = { 0.0f, 0.0f, 0.0f };
thread_running = true;
struct multirotor_position_control_params params;
struct multirotor_position_control_param_handles params_h;
parameters_init(&params_h);
parameters_update(&params_h, &params);
bool param_updated = true;
while (!thread_should_exit) {
if (!param_updated)
orb_check(param_sub, &param_updated);
if (param_updated) {
/* clear updated flag */
struct parameter_update_s ps;
orb_copy(ORB_ID(parameter_update), param_sub, &ps);
/* update params */
parameters_update(&params_h, &params);
}
bool updated;
orb_check(control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
orb_check(global_pos_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
global_pos_sp_valid = true;
reset_mission_sp = true;
}
hrt_abstime t = hrt_absolute_time();
float dt;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
} else {
dt = 0.0f;
}
t_prev = t;
if (control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_auto_sp_z = true;
reset_auto_sp_xy = true;
reset_takeoff_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
was_armed = control_mode.flag_armed;
if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_position_enabled || control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
if (control_mode.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (local_pos.ref_timestamp != ref_alt_t) {
if (ref_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - ref_alt;
}
ref_alt_t = local_pos.ref_timestamp;
ref_alt = local_pos.ref_alt;
// TODO also correct XY setpoint
}
/* reset setpoints to current position if needed */
if (control_mode.flag_control_altitude_enabled) {
if (reset_man_sp_z) {
reset_man_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
}
/* move altitude setpoint with throttle stick */
float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
if (z_sp_ctl != 0.0f) {
sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
local_pos_sp.z += sp_move_rate[2] * dt;
if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
local_pos_sp.z = local_pos.z + z_sp_offs_max;
} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
local_pos_sp.z = local_pos.z - z_sp_offs_max;
}
}
}
if (control_mode.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
/* move position setpoint with roll/pitch stick */
float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
float xy_sp_ctl_dir = att_sp.yaw_body + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
local_pos_sp.x += sp_move_rate[0] * dt;
local_pos_sp.y += sp_move_rate[1] * dt;
/* limit maximum setpoint from position offset and preserve direction
* fail safe, should not happen in normal operation */
float pos_vec_x = local_pos_sp.x - local_pos.x;
float pos_vec_y = local_pos_sp.y - local_pos.y;
float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
if (pos_vec_norm > 1.0f) {
local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
}
}
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
reset_takeoff_sp = true;
/* force reprojection of global setpoint after manual mode */
reset_mission_sp = true;
} else if (control_mode.flag_control_auto_enabled) {
/* AUTO mode, use global setpoint */
if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
if (reset_takeoff_sp) {
reset_takeoff_sp = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
}
reset_auto_sp_xy = false;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
// TODO
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
/* init local projection using local position ref */
if (local_pos.ref_timestamp != local_ref_timestamp) {
reset_mission_sp = true;
local_ref_timestamp = local_pos.ref_timestamp;
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
}
if (reset_mission_sp) {
reset_mission_sp = false;
/* update global setpoint projection */
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.lat * 1e-7;
double sp_lon = global_pos_sp.lon * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
/* update yaw setpoint only if value is valid */
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI_F) {
att_sp.yaw_body = global_pos_sp.yaw;
}
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
} else {
if (reset_auto_sp_xy) {
reset_auto_sp_xy = false;
/* local position setpoint is invalid,
* use current position as setpoint for loiter */
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
}
if (reset_auto_sp_z) {
reset_auto_sp_z = false;
local_pos_sp.z = local_pos.z;
}
mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
}
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
}
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
reset_takeoff_sp = true;
}
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
reset_mission_sp = true;
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* reset setpoints after AUTO mode */
reset_man_sp_xy = true;
reset_man_sp_z = true;
} else {
/* no control (failsafe), loiter or stay on ground */
if (local_pos.landed) {
/* landed: move setpoint down */
/* in air: hold altitude */
if (local_pos_sp.z < 5.0f) {
/* set altitude setpoint to 5m under ground,
* don't set it too deep to avoid unexpected landing in case of false "landed" signal */
local_pos_sp.z = 5.0f;
mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
}
reset_man_sp_z = true;
} else {
/* in air: hold altitude */
if (reset_man_sp_z) {
reset_man_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
}
reset_auto_sp_z = false;
}
if (control_mode.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
reset_auto_sp_xy = false;
}
}
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
/* run position & altitude controllers, calculate velocity setpoint */
if (control_mode.flag_control_altitude_enabled) {
global_vel_sp.vz = (local_pos_sp.z - local_pos.z) * params.z_p + sp_move_rate[2] * params.z_ff;
} else {
reset_man_sp_z = true;
global_vel_sp.vz = 0.0f;
}
if (control_mode.flag_control_position_enabled) {
/* calculate velocity set point in NED frame */
global_vel_sp.vx = (local_pos_sp.x - local_pos.x) * params.xy_p + sp_move_rate[0] * params.xy_ff;
global_vel_sp.vy = (local_pos_sp.y - local_pos.y) * params.xy_p + sp_move_rate[1] * params.xy_ff;
} else {
reset_man_sp_xy = true;
global_vel_sp.vx = 0.0f;
global_vel_sp.vy = 0.0f;
}
if (!control_mode.flag_control_manual_enabled) {
/* limit 3D speed only in AUTO mode */
float vel_sp_norm = norm3(global_vel_sp.vx / params.xy_vel_max, global_vel_sp.vy / params.xy_vel_max, global_vel_sp.vz / params.z_vel_max);
if (vel_sp_norm > 1.0f) {
global_vel_sp.vx /= vel_sp_norm;
global_vel_sp.vy /= vel_sp_norm;
global_vel_sp.vz /= vel_sp_norm;
}
}
/* publish new velocity setpoint */
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
// TODO subscribe to velocity setpoint if altitude/position control disabled
if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
/* calculate desired thrust vector in NED frame */
float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
if (reset_int_z) {
reset_int_z = false;
float i = params.thr_min;
if (reset_int_z_manual) {
i = manual.throttle;
if (i < params.thr_min) {
i = params.thr_min;
} else if (i > params.thr_max) {
i = params.thr_max;
}
}
thrust_int[2] = -i;
mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
thrust_sp[2] = (global_vel_sp.vz - local_pos.vz) * params.z_vel_p + thrust_int[2];
if (control_mode.flag_control_velocity_enabled) {
if (reset_int_xy) {
reset_int_xy = false;
thrust_int[0] = 0.0f;
thrust_int[1] = 0.0f;
mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral");
}
thrust_sp[0] = (global_vel_sp.vx - local_pos.vx) * params.xy_vel_p + thrust_int[0];
thrust_sp[1] = (global_vel_sp.vy - local_pos.vy) * params.xy_vel_p + thrust_int[1];
} else {
reset_int_xy = true;
}
/* 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.flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
thr_min = 0.0f;
}
if (-thrust_sp[2] < thr_min) {
thrust_sp[2] = -thr_min;
saturation_z = true;
}
/* limit max tilt */
float tilt = atan2f(norm(thrust_sp[0], thrust_sp[1]), -thrust_sp[2]);
if (tilt > params.tilt_max && params.thr_min > 0.0f) {
/* crop horizontal component */
float k = tanf(params.tilt_max) / tanf(tilt);
thrust_sp[0] *= k;
thrust_sp[1] *= k;
saturation_xy = true;
}
/* limit max thrust */
float thrust_abs = sqrtf(thrust_sp[0] * thrust_sp[0] + thrust_sp[1] * thrust_sp[1] + thrust_sp[2] * thrust_sp[2]);
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 = norm(thrust_sp[0], thrust_sp[1]);
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[0] *= k;
thrust_sp[1] *= k;
thrust_sp[2] *= k;
saturation_xy = true;
saturation_z = true;
}
thrust_abs = params.thr_max;
}
/* update integrals */
if (control_mode.flag_control_velocity_enabled && !saturation_xy) {
thrust_int[0] += (global_vel_sp.vx - local_pos.vx) * params.xy_vel_i * dt;
thrust_int[1] += (global_vel_sp.vy - local_pos.vy) * params.xy_vel_i * dt;
}
if (control_mode.flag_control_climb_rate_enabled && !saturation_z) {
thrust_int[2] += (global_vel_sp.vz - local_pos.vz) * params.z_vel_i * dt;
}
/* calculate attitude and thrust from thrust vector */
if (control_mode.flag_control_velocity_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
float body_x[3];
float body_y[3];
float body_z[3];
if (thrust_abs > SIGMA) {
body_z[0] = -thrust_sp[0] / thrust_abs;
body_z[1] = -thrust_sp[1] / thrust_abs;
body_z[2] = -thrust_sp[2] / thrust_abs;
} else {
/* no thrust, set Z axis to safe value */
body_z[0] = 0.0f;
body_z[1] = 0.0f;
body_z[2] = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2 */
float y_C[3];
y_C[0] = -sinf(att_sp.yaw_body);
y_C[1] = cosf(att_sp.yaw_body);
y_C[2] = 0;
if (fabsf(body_z[2]) > SIGMA) {
/* desired body_x axis = cross(y_C, body_z) */
cross3(y_C, body_z, body_x);
/* keep nose to front while inverted upside down */
if (body_z[2] < 0.0f) {
body_x[0] = -body_x[0];
body_x[1] = -body_x[1];
body_x[2] = -body_x[2];
}
normalize3(body_x);
} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x[0] = 0.0f;
body_x[1] = 0.0f;
body_x[2] = 1.0f;
}
/* desired body_y axis = cross(body_z, body_x) */
cross3(body_z, body_x, body_y);
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
att_sp.R_body[i][0] = body_x[i];
att_sp.R_body[i][1] = body_y[i];
att_sp.R_body[i][2] = body_z[i];
}
att_sp.R_valid = true;
/* calculate roll, pitch from rotation matrix */
att_sp.roll_body = rt_atan2f_snf(att_sp.R_body[2][1], att_sp.R_body[2][2]);
att_sp.pitch_body = -asinf(att_sp.R_body[2][0]);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
//att_sp.yaw_body = rt_atan2f_snf(att_sp.R_body[1][0], att_sp.R_body[0][0]);
} else {
/* thrust compensation for altitude only control mode */
float att_comp;
if (att.R[2][2] > TILT_COS_MAX)
att_comp = 1.0f / att.R[2][2];
else if (att.R[2][2] > 0.0f)
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * att.R[2][2] + 1.0f;
else
att_comp = 1.0f;
thrust_abs *= att_comp;
}
att_sp.thrust = thrust_abs;
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else {
reset_int_z = true;
}
} else {
/* position controller disabled, reset setpoints */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_int_z = true;
reset_int_xy = true;
reset_mission_sp = true;
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
/* run at approximately 50 Hz */
usleep(20000);
}
warnx("stopped");
mavlink_log_info(mavlink_fd, "[mpc] stopped");
thread_running = false;
fflush(stdout);
return 0;
}

106
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
View File

@ -1,106 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* 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 multirotor_pos_control_params.c
*
* Parameters for multirotor_pos_control
*/
#include "multirotor_pos_control_params.h"
/* controller parameters */
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
int parameters_init(struct multirotor_position_control_param_handles *h)
{
h->takeoff_alt = param_find("NAV_TAKEOFF_ALT");
h->takeoff_gap = param_find("NAV_TAKEOFF_GAP");
h->thr_min = param_find("MPC_THR_MIN");
h->thr_max = param_find("MPC_THR_MAX");
h->z_p = param_find("MPC_Z_P");
h->z_vel_p = param_find("MPC_Z_VEL_P");
h->z_vel_i = param_find("MPC_Z_VEL_I");
h->z_vel_max = param_find("MPC_Z_VEL_MAX");
h->z_ff = param_find("MPC_Z_FF");
h->xy_p = param_find("MPC_XY_P");
h->xy_vel_p = param_find("MPC_XY_VEL_P");
h->xy_vel_i = param_find("MPC_XY_VEL_I");
h->xy_vel_max = param_find("MPC_XY_VEL_MAX");
h->xy_ff = param_find("MPC_XY_FF");
h->tilt_max = param_find("MPC_TILT_MAX");
h->rc_scale_pitch = param_find("RC_SCALE_PITCH");
h->rc_scale_roll = param_find("RC_SCALE_ROLL");
h->rc_scale_yaw = param_find("RC_SCALE_YAW");
return OK;
}
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
{
param_get(h->takeoff_alt, &(p->takeoff_alt));
param_get(h->takeoff_gap, &(p->takeoff_gap));
param_get(h->thr_min, &(p->thr_min));
param_get(h->thr_max, &(p->thr_max));
param_get(h->z_p, &(p->z_p));
param_get(h->z_vel_p, &(p->z_vel_p));
param_get(h->z_vel_i, &(p->z_vel_i));
param_get(h->z_vel_max, &(p->z_vel_max));
param_get(h->z_ff, &(p->z_ff));
param_get(h->xy_p, &(p->xy_p));
param_get(h->xy_vel_p, &(p->xy_vel_p));
param_get(h->xy_vel_i, &(p->xy_vel_i));
param_get(h->xy_vel_max, &(p->xy_vel_max));
param_get(h->xy_ff, &(p->xy_ff));
param_get(h->tilt_max, &(p->tilt_max));
param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
param_get(h->rc_scale_roll, &(p->rc_scale_roll));
param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
return OK;
}

97
src/modules/multirotor_pos_control/multirotor_pos_control_params.h
View File

@ -1,97 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* 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 multirotor_pos_control_params.h
*
* Parameters for multirotor_pos_control
*/
#include <systemlib/param/param.h>
struct multirotor_position_control_params {
float takeoff_alt;
float takeoff_gap;
float thr_min;
float thr_max;
float z_p;
float z_vel_p;
float z_vel_i;
float z_vel_max;
float z_ff;
float xy_p;
float xy_vel_p;
float xy_vel_i;
float xy_vel_max;
float xy_ff;
float tilt_max;
float rc_scale_pitch;
float rc_scale_roll;
float rc_scale_yaw;
};
struct multirotor_position_control_param_handles {
param_t takeoff_alt;
param_t takeoff_gap;
param_t thr_min;
param_t thr_max;
param_t z_p;
param_t z_vel_p;
param_t z_vel_i;
param_t z_vel_max;
param_t z_ff;
param_t xy_p;
param_t xy_vel_p;
param_t xy_vel_i;
param_t xy_vel_max;
param_t xy_ff;
param_t tilt_max;
param_t rc_scale_pitch;
param_t rc_scale_roll;
param_t rc_scale_yaw;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct multirotor_position_control_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);

172
src/modules/multirotor_pos_control/thrust_pid.c
View File

@ -1,172 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* 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 thrust_pid.c
*
* Implementation of thrust control PID.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "thrust_pid.h"
#include <math.h>
#define COS_TILT_MAX 0.7f
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min)
{
pid->dt_min = dt_min;
pid->kp = 0.0f;
pid->ki = 0.0f;
pid->kd = 0.0f;
pid->integral = 0.0f;
pid->output_min = 0.0f;
pid->output_max = 0.0f;
pid->error_previous = 0.0f;
pid->last_output = 0.0f;
}
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max)
{
int ret = 0;
if (isfinite(kp)) {
pid->kp = kp;
} else {
ret = 1;
}
if (isfinite(ki)) {
pid->ki = ki;
} else {
ret = 1;
}
if (isfinite(kd)) {
pid->kd = kd;
} else {
ret = 1;
}
if (isfinite(output_min)) {
pid->output_min = output_min;
} else {
ret = 1;
}
if (isfinite(output_max)) {
pid->output_max = output_max;
} else {
ret = 1;
}
return ret;
}
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
{
if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
return pid->last_output;
}
float i, d;
/* error value */
float error = sp - val;
/* error derivative */
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = -val;
if (!isfinite(d)) {
d = 0.0f;
}
/* calculate the error integral */
i = pid->integral + (pid->ki * error * dt);
/* attitude-thrust compensation
* r22 is (2, 2) component of rotation matrix for current attitude */
float att_comp;
if (r22 > COS_TILT_MAX) {
att_comp = 1.0f / r22;
} else if (r22 > 0.0f) {
att_comp = ((1.0f / COS_TILT_MAX - 1.0f) / COS_TILT_MAX) * r22 + 1.0f;
} else {
att_comp = 1.0f;
}
/* calculate PD output */
float output = ((error * pid->kp) + (d * pid->kd)) * att_comp;
/* check for saturation */
if (isfinite(i)) {
float i_comp = i * att_comp;
if ((output + i_comp) >= pid->output_min || (output + i_comp) <= pid->output_max) {
/* not saturated, use new integral value */
pid->integral = i;
}
}
/* add I component to output */
output += pid->integral * att_comp;
/* limit output */
if (isfinite(output)) {
if (output > pid->output_max) {
output = pid->output_max;
} else if (output < pid->output_min) {
output = pid->output_min;
}
pid->last_output = output;
}
return pid->last_output;
}
__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i)
{
pid->integral = i;
}

69
src/modules/multirotor_pos_control/thrust_pid.h
View File

@ -1,69 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* 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 thrust_pid.h
*
* Definition of thrust control PID interface.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef THRUST_PID_H_
#define THRUST_PID_H_
#include <stdint.h>
__BEGIN_DECLS
typedef struct {
float dt_min;
float kp;
float ki;
float kd;
float integral;
float output_min;
float output_max;
float error_previous;
float last_output;
} thrust_pid_t;
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min);
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max);
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
__END_DECLS
#endif /* THRUST_PID_H_ */