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

mc_pos_control: operate in local projection instead of global frame

This commit is contained in:
Anton Babushkin 2014-03-17 22:42:52 +04:00
parent 3d5f52678f
commit c0c54f01cb
1 changed files with 96 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

207
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -62,9 +62,10 @@
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
@ -114,20 +115,21 @@ private:
int _params_sub; /**< notification of parameter updates */ int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */ int _manual_sub; /**< notification of manual control updates */
int _arming_sub; /**< arming status of outputs */ int _arming_sub; /**< arming status of outputs */
int _global_pos_sub; /**< vehicle local position */ int _local_pos_sub; /**< vehicle local position */
int _pos_sp_triplet_sub; /**< position setpoint triplet */ int _pos_sp_triplet_sub; /**< position setpoint triplet */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _pos_sp_triplet_pub; /**< position setpoint triplet publication */ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
struct actuator_armed_s _arming; /**< actuator arming status */ struct actuator_armed_s _arming; /**< actuator arming status */
struct vehicle_global_position_s _global_pos; /**< vehicle global position */ struct vehicle_local_position_s _local_pos; /**< vehicle local position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
struct { struct {
@ -172,14 +174,15 @@ private:
math::Vector<3> sp_offs_max; math::Vector<3> sp_offs_max;
} _params; } _params;
double _lat_sp; struct map_projection_reference_s _ref_pos;
double _lon_sp; float _ref_alt;
float _alt_sp; hrt_abstime _ref_timestamp;
bool _reset_lat_lon_sp; bool _reset_pos_sp;
bool _reset_alt_sp; bool _reset_alt_sp;
bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */
math::Vector<3> _pos;
math::Vector<3> _pos_sp;
math::Vector<3> _vel; math::Vector<3> _vel;
math::Vector<3> _vel_sp; math::Vector<3> _vel_sp;
math::Vector<3> _vel_prev; /**< velocity on previous step */ math::Vector<3> _vel_prev; /**< velocity on previous step */
@ -202,9 +205,13 @@ private:
static float scale_control(float ctl, float end, float dz); static float scale_control(float ctl, float end, float dz);
/** /**
* Reset lat/lon to current position * Update reference for local position projection
*/ */
void reset_lat_lon_sp(); void update_ref();
/**
* Reset position setpoint to current position
*/
void reset_pos_sp();
/** /**
* Reset altitude setpoint to current altitude * Reset altitude setpoint to current altitude
@ -252,31 +259,32 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_sub(-1), _params_sub(-1),
_manual_sub(-1), _manual_sub(-1),
_arming_sub(-1), _arming_sub(-1),
_global_pos_sub(-1), _local_pos_sub(-1),
_pos_sp_triplet_sub(-1), _pos_sp_triplet_sub(-1),
/* publications */ /* publications */
_att_sp_pub(-1), _att_sp_pub(-1),
_pos_sp_triplet_pub(-1), _local_pos_sp_pub(-1),
_global_vel_sp_pub(-1), _global_vel_sp_pub(-1),
_lat_sp(0.0), _ref_alt(0.0f),
_lon_sp(0.0), _ref_timestamp(0),
_alt_sp(0.0f),
_reset_lat_lon_sp(true), _reset_pos_sp(true),
_reset_alt_sp(true), _reset_alt_sp(true)
_use_global_alt(false)
{ {
memset(&_att, 0, sizeof(_att)); memset(&_att, 0, sizeof(_att));
memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual)); memset(&_manual, 0, sizeof(_manual));
memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_control_mode, 0, sizeof(_control_mode));
memset(&_arming, 0, sizeof(_arming)); memset(&_arming, 0, sizeof(_arming));
memset(&_global_pos, 0, sizeof(_global_pos)); memset(&_local_pos, 0, sizeof(_local_pos));
memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
memset(&_ref_pos, 0, sizeof(_ref_pos));
_params.pos_p.zero(); _params.pos_p.zero();
_params.vel_p.zero(); _params.vel_p.zero();
_params.vel_i.zero(); _params.vel_i.zero();
@ -285,6 +293,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params.vel_ff.zero(); _params.vel_ff.zero();
_params.sp_offs_max.zero(); _params.sp_offs_max.zero();
_pos.zero();
_pos_sp.zero();
_vel.zero(); _vel.zero();
_vel_sp.zero(); _vel_sp.zero();
_vel_prev.zero(); _vel_prev.zero();
@ -425,10 +435,10 @@ MulticopterPositionControl::poll_subscriptions()
if (updated) if (updated)
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
orb_check(_global_pos_sub, &updated); orb_check(_local_pos_sub, &updated);
if (updated) if (updated)
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
} }
float float
@ -452,13 +462,25 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
} }
void void
MulticopterPositionControl::reset_lat_lon_sp() MulticopterPositionControl::update_ref()
{ {
if (_reset_lat_lon_sp) { if (_local_pos.ref_timestamp != _ref_timestamp) {
_reset_lat_lon_sp = false; _ref_timestamp = _local_pos.ref_timestamp;
_lat_sp = _global_pos.lat; // TODO mode position setpoint in assisted modes
_lon_sp = _global_pos.lon;
mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp); map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
_ref_alt = _local_pos.ref_alt;
}
}
void
MulticopterPositionControl::reset_pos_sp()
{
if (_reset_pos_sp) {
_reset_pos_sp = 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));
} }
} }
@ -467,25 +489,8 @@ MulticopterPositionControl::reset_alt_sp()
{ {
if (_reset_alt_sp) { if (_reset_alt_sp) {
_reset_alt_sp = false; _reset_alt_sp = false;
_alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt; _pos_sp(2) = _pos(2);
mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
}
}
void
MulticopterPositionControl::select_alt(bool global)
{
if (global != _use_global_alt) {
_use_global_alt = global;
if (global) {
/* switch from barometric to global altitude */
_alt_sp += _global_pos.alt - _global_pos.baro_alt;
} else {
/* switch from global to barometric altitude */
_alt_sp += _global_pos.baro_alt - _global_pos.alt;
}
} }
} }
@ -506,7 +511,7 @@ MulticopterPositionControl::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update)); _params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
parameters_update(true); parameters_update(true);
@ -537,8 +542,7 @@ MulticopterPositionControl::task_main()
/* wakeup source */ /* wakeup source */
struct pollfd fds[1]; struct pollfd fds[1];
/* Setup of loop */ fds[0].fd = _local_pos_sub;
fds[0].fd = _global_pos_sub;
fds[0].events = POLLIN; fds[0].events = POLLIN;
while (!_task_should_exit) { while (!_task_should_exit) {
@ -564,7 +568,7 @@ MulticopterPositionControl::task_main()
if (_control_mode.flag_armed && !was_armed) { if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */ /* reset setpoints and integrals on arming */
_reset_lat_lon_sp = true; _reset_pos_sp = true;
_reset_alt_sp = true; _reset_alt_sp = true;
reset_int_z = true; reset_int_z = true;
reset_int_xy = true; reset_int_xy = true;
@ -577,23 +581,20 @@ MulticopterPositionControl::task_main()
_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) { _control_mode.flag_control_velocity_enabled) {
_vel(0) = _global_pos.vel_n; update_ref();
_vel(1) = _global_pos.vel_e;
_vel(2) = _global_pos.vel_d; _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(); sp_move_rate.zero();
float alt = _global_pos.alt;
/* select control source */ /* select control source */
if (_control_mode.flag_control_manual_enabled) { if (_control_mode.flag_control_manual_enabled) {
/* select altitude source and update setpoint */
select_alt(_global_pos.global_valid);
if (!_use_global_alt) {
alt = _global_pos.baro_alt;
}
/* manual control */ /* manual control */
if (_control_mode.flag_control_altitude_enabled) { if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */ /* reset alt setpoint to current altitude if needed */
@ -604,8 +605,8 @@ MulticopterPositionControl::task_main()
} }
if (_control_mode.flag_control_position_enabled) { if (_control_mode.flag_control_position_enabled) {
/* reset lat/lon setpoint to current position if needed */ /* reset position setpoint to current position if needed */
reset_lat_lon_sp(); reset_pos_sp();
/* move position setpoint with roll/pitch stick */ /* 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(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
@ -625,58 +626,29 @@ MulticopterPositionControl::task_main()
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
/* move position setpoint */ /* move position setpoint */
add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp); _pos_sp += sp_move_rate * dt;
_alt_sp -= sp_move_rate(2) * dt;
/* check if position setpoint is too far from actual position */ /* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs; math::Vector<3> pos_sp_offs;
pos_sp_offs.zero(); pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) { if (_control_mode.flag_control_position_enabled) {
get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]); pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(0) /= _params.sp_offs_max(0); pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
pos_sp_offs(1) /= _params.sp_offs_max(1);
} }
if (_control_mode.flag_control_altitude_enabled) { if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2); pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
} }
float pos_sp_offs_norm = pos_sp_offs.length(); float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) { if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm; pos_sp_offs /= pos_sp_offs_norm;
add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
_alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2);
}
/* fill position setpoint triplet */
_pos_sp_triplet.previous.valid = true;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = true;
_pos_sp_triplet.nav_state = NAV_STATE_NONE;
_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
_pos_sp_triplet.current.lat = _lat_sp;
_pos_sp_triplet.current.lon = _lon_sp;
_pos_sp_triplet.current.alt = _alt_sp;
_pos_sp_triplet.current.yaw = _att_sp.yaw_body;
_pos_sp_triplet.current.loiter_radius = 0.0f;
_pos_sp_triplet.current.loiter_direction = 1.0f;
_pos_sp_triplet.current.pitch_min = 0.0f;
/* publish position setpoint triplet */
if (_pos_sp_triplet_pub > 0) {
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
} else {
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
} }
} else { } else {
/* always use AMSL altitude for AUTO */
select_alt(true);
/* AUTO */ /* AUTO */
bool updated; bool updated;
orb_check(_pos_sp_triplet_sub, &updated); orb_check(_pos_sp_triplet_sub, &updated);
@ -686,13 +658,14 @@ MulticopterPositionControl::task_main()
if (_pos_sp_triplet.current.valid) { if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */ /* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_lat_lon_sp = true; _reset_pos_sp = true;
_reset_alt_sp = true; _reset_alt_sp = true;
/* update position setpoint */ /* project setpoint to local frame */
_lat_sp = _pos_sp_triplet.current.lat; map_projection_project(&_ref_pos,
_lon_sp = _pos_sp_triplet.current.lon; _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
_alt_sp = _pos_sp_triplet.current.alt; &_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* update yaw setpoint if needed */ /* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) { if (isfinite(_pos_sp_triplet.current.yaw)) {
@ -701,11 +674,25 @@ MulticopterPositionControl::task_main()
} else { } else {
/* no waypoint, loiter, reset position setpoint if needed */ /* no waypoint, loiter, reset position setpoint if needed */
reset_lat_lon_sp(); reset_pos_sp();
reset_alt_sp(); reset_alt_sp();
} }
} }
/* fill local position setpoint */
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
/* 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);
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */ /* idle state, don't run controller and set zero thrust */
R.identity(); R.identity();
@ -729,9 +716,7 @@ MulticopterPositionControl::task_main()
} else { } else {
/* run position & altitude controllers, calculate velocity setpoint */ /* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err; math::Vector<3> pos_err = _pos_sp - _pos;
get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
pos_err(2) = -(_alt_sp - alt);
_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
@ -741,7 +726,7 @@ MulticopterPositionControl::task_main()
} }
if (!_control_mode.flag_control_position_enabled) { if (!_control_mode.flag_control_position_enabled) {
_reset_lat_lon_sp = true; _reset_pos_sp = true;
_vel_sp(0) = 0.0f; _vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f; _vel_sp(1) = 0.0f;
} }
@ -1021,7 +1006,7 @@ MulticopterPositionControl::task_main()
} else { } else {
/* position controller disabled, reset setpoints */ /* position controller disabled, reset setpoints */
_reset_alt_sp = true; _reset_alt_sp = true;
_reset_lat_lon_sp = true; _reset_pos_sp = true;
reset_int_z = true; reset_int_z = true;
reset_int_xy = true; reset_int_xy = true;
} }