mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-17 01:40:34 +08:00
Merge branch 'master' into test_bottle_drop_paul
Conflicts: ROMFS/px4fmu_common/init.d/3035_viper
This commit is contained in:
Julian Oes
@@ -7,4 +7,4 @@
|
||||
|
||||
sh /etc/init.d/rc.fw_defaults
|
||||
|
||||
set MIXER FMU_FX79
|
||||
set MIXER Viper
|
||||
|
||||
@@ -27,12 +27,12 @@ for the elevons.
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 5000 8000 0 -10000 10000
|
||||
S: 0 0 7500 7500 0 -10000 10000
|
||||
S: 0 1 8000 8000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 8000 5000 0 -10000 10000
|
||||
S: 0 0 7500 7500 0 -10000 10000
|
||||
S: 0 1 -8000 -8000 0 -10000 10000
|
||||
|
||||
Output 2
|
||||
|
||||
Executable
+72
@@ -0,0 +1,72 @@
|
||||
Viper Delta-wing mixer
|
||||
=================================
|
||||
|
||||
Designed for Viper.
|
||||
|
||||
TODO (sjwilks): Add mixers for flaps.
|
||||
|
||||
This file defines mixers suitable for controlling a delta wing aircraft using
|
||||
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
|
||||
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
|
||||
assumed to be unused.
|
||||
|
||||
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
|
||||
(roll), 1 (pitch) and 3 (thrust).
|
||||
|
||||
See the README for more information on the scaler format.
|
||||
|
||||
Elevon mixers
|
||||
-------------
|
||||
Three scalers total (output, roll, pitch).
|
||||
|
||||
On the assumption that the two elevon servos are physically reversed, the pitch
|
||||
input is inverted between the two servos.
|
||||
|
||||
The scaling factor for roll inputs is adjusted to implement differential travel
|
||||
for the elevons.
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 7500 7500 0 -10000 10000
|
||||
S: 0 1 8000 8000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 7500 7500 0 -10000 10000
|
||||
S: 0 1 -8000 -8000 0 -10000 10000
|
||||
|
||||
Output 2
|
||||
--------
|
||||
This mixer is empty.
|
||||
|
||||
Z:
|
||||
|
||||
Motor speed mixer
|
||||
-----------------
|
||||
Two scalers total (output, thrust).
|
||||
|
||||
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
|
||||
range. Inputs below zero are treated as zero.
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
Gimbal / flaps / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
@@ -105,6 +105,7 @@ public:
|
||||
int start();
|
||||
|
||||
private:
|
||||
const float alt_ctl_dz = 0.2f;
|
||||
|
||||
bool _task_should_exit; /**< if true, task should exit */
|
||||
int _control_task; /**< task handle for task */
|
||||
@@ -184,6 +185,8 @@ private:
|
||||
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;
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
@@ -216,6 +219,16 @@ private:
|
||||
*/
|
||||
void reset_alt_sp();
|
||||
|
||||
/**
|
||||
* Set position setpoint using manual control
|
||||
*/
|
||||
void control_manual(float dt);
|
||||
|
||||
/**
|
||||
* Set position setpoint using offboard control
|
||||
*/
|
||||
void control_offboard(float dt);
|
||||
|
||||
/**
|
||||
* Select between barometric and global (AMSL) altitudes
|
||||
*/
|
||||
@@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
||||
_vel.zero();
|
||||
_vel_sp.zero();
|
||||
_vel_prev.zero();
|
||||
_vel_ff.zero();
|
||||
_sp_move_rate.zero();
|
||||
|
||||
_params_handles.thr_min = param_find("MPC_THR_MIN");
|
||||
_params_handles.thr_max = param_find("MPC_THR_MAX");
|
||||
@@ -392,9 +407,11 @@ MulticopterPositionControl::parameters_update(bool force)
|
||||
param_get(_params_handles.z_vel_max, &v);
|
||||
_params.vel_max(2) = v;
|
||||
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;
|
||||
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;
|
||||
@@ -497,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp()
|
||||
{
|
||||
if (_reset_pos_sp) {
|
||||
_reset_pos_sp = false;
|
||||
_pos_sp(0) = _pos(0);
|
||||
_pos_sp(1) = _pos(1);
|
||||
/* shift position setpoint to make attitude setpoint continuous */
|
||||
_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
|
||||
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
|
||||
_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
|
||||
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
|
||||
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
|
||||
}
|
||||
}
|
||||
@@ -508,11 +528,125 @@ MulticopterPositionControl::reset_alt_sp()
|
||||
{
|
||||
if (_reset_alt_sp) {
|
||||
_reset_alt_sp = false;
|
||||
_pos_sp(2) = _pos(2);
|
||||
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
|
||||
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::control_manual(float dt)
|
||||
{
|
||||
_sp_move_rate.zero();
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* move altitude setpoint with throttle stick */
|
||||
_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
/* move position setpoint with roll/pitch stick */
|
||||
_sp_move_rate(0) = _manual.x;
|
||||
_sp_move_rate(1) = _manual.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.yaw_body);
|
||||
_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* reset alt setpoint to current altitude if needed */
|
||||
reset_alt_sp();
|
||||
}
|
||||
|
||||
if (_control_mode.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.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.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)
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_pos_sp_triplet_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
|
||||
}
|
||||
|
||||
if (_pos_sp_triplet.current.valid) {
|
||||
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
|
||||
/* control position */
|
||||
_pos_sp(0) = _pos_sp_triplet.current.x;
|
||||
_pos_sp(1) = _pos_sp_triplet.current.y;
|
||||
_pos_sp(2) = _pos_sp_triplet.current.z;
|
||||
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
|
||||
|
||||
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.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.current.vx;
|
||||
_sp_move_rate(1) = _pos_sp_triplet.current.vy;
|
||||
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* reset alt setpoint to current altitude if needed */
|
||||
reset_alt_sp();
|
||||
|
||||
/* set altitude setpoint move rate */
|
||||
_sp_move_rate(2) = _pos_sp_triplet.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();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::task_main()
|
||||
{
|
||||
@@ -551,13 +685,6 @@ MulticopterPositionControl::task_main()
|
||||
|
||||
hrt_abstime t_prev = 0;
|
||||
|
||||
const float alt_ctl_dz = 0.2f;
|
||||
|
||||
math::Vector<3> sp_move_rate;
|
||||
sp_move_rate.zero();
|
||||
|
||||
float yaw_sp_move_rate;
|
||||
|
||||
math::Vector<3> thrust_int;
|
||||
thrust_int.zero();
|
||||
math::Matrix<3, 3> R;
|
||||
@@ -616,138 +743,17 @@ MulticopterPositionControl::task_main()
|
||||
_vel(1) = _local_pos.vy;
|
||||
_vel(2) = _local_pos.vz;
|
||||
|
||||
sp_move_rate.zero();
|
||||
_vel_ff.zero();
|
||||
_sp_move_rate.zero();
|
||||
|
||||
/* select control source */
|
||||
if (_control_mode.flag_control_manual_enabled) {
|
||||
/* manual control */
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* reset alt setpoint to current altitude if needed */
|
||||
reset_alt_sp();
|
||||
|
||||
/* move altitude setpoint with throttle stick */
|
||||
sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
/* reset position setpoint to current position if needed */
|
||||
reset_pos_sp();
|
||||
|
||||
/* move position setpoint with roll/pitch stick */
|
||||
sp_move_rate(0) = _manual.x;
|
||||
sp_move_rate(1) = _manual.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;
|
||||
}
|
||||
|
||||
/* 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_offs.zero();
|
||||
|
||||
if (_control_mode.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.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);
|
||||
}
|
||||
control_manual(dt);
|
||||
|
||||
} else if (_control_mode.flag_control_offboard_enabled) {
|
||||
/* Offboard control */
|
||||
bool updated;
|
||||
orb_check(_pos_sp_triplet_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
|
||||
}
|
||||
|
||||
if (_pos_sp_triplet.current.valid) {
|
||||
|
||||
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
|
||||
|
||||
_pos_sp(0) = _pos_sp_triplet.current.x;
|
||||
_pos_sp(1) = _pos_sp_triplet.current.y;
|
||||
_pos_sp(2) = _pos_sp_triplet.current.z;
|
||||
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
|
||||
|
||||
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
|
||||
/* reset position setpoint to current position if needed */
|
||||
reset_pos_sp();
|
||||
/* move position setpoint with roll/pitch stick */
|
||||
sp_move_rate(0) = _pos_sp_triplet.current.vx;
|
||||
sp_move_rate(1) = _pos_sp_triplet.current.vy;
|
||||
yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed;
|
||||
_att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt;
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* reset alt setpoint to current altitude if needed */
|
||||
reset_alt_sp();
|
||||
|
||||
/* move altitude setpoint with throttle stick */
|
||||
sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_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_offs.zero();
|
||||
|
||||
if (_control_mode.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.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);
|
||||
}
|
||||
|
||||
} else {
|
||||
reset_pos_sp();
|
||||
reset_alt_sp();
|
||||
}
|
||||
/* offboard control */
|
||||
control_offboard(dt);
|
||||
|
||||
} else {
|
||||
/* AUTO */
|
||||
@@ -821,7 +827,7 @@ MulticopterPositionControl::task_main()
|
||||
/* 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);
|
||||
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
|
||||
|
||||
if (!_control_mode.flag_control_altitude_enabled) {
|
||||
_reset_alt_sp = true;
|
||||
@@ -901,7 +907,7 @@ MulticopterPositionControl::task_main()
|
||||
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;
|
||||
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 */
|
||||
|
||||
@@ -60,7 +60,7 @@ enum SETPOINT_TYPE
|
||||
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
|
||||
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */
|
||||
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
|
||||
SETPOINT_TYPE_OFFBOARD, /**< setpoint set by offboard */
|
||||
SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
|
||||
};
|
||||
|
||||
struct position_setpoint_s
|
||||
@@ -71,9 +71,9 @@ struct position_setpoint_s
|
||||
float y; /**< local position setpoint in m in NED */
|
||||
float z; /**< local position setpoint in m in NED */
|
||||
bool position_valid; /**< true if local position setpoint valid */
|
||||
float vx; /**< local velocity setpoint in m in NED */
|
||||
float vy; /**< local velocity setpoint in m in NED */
|
||||
float vz; /**< local velocity setpoint in m in NED */
|
||||
float vx; /**< local velocity setpoint in m/s in NED */
|
||||
float vy; /**< local velocity setpoint in m/s in NED */
|
||||
float vz; /**< local velocity setpoint in m/s in NED */
|
||||
bool velocity_valid; /**< true if local velocity setpoint valid */
|
||||
double lat; /**< latitude, in deg */
|
||||
double lon; /**< longitude, in deg */
|
||||
|
||||
Reference in New Issue
Block a user