Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 13 Packages Projects Releases Wiki Activity

accel control in pos controller

Browse Source
This commit is contained in:
Benoit Landry
2016-04-07 15:09:21 -07:00
committed by Lorenz Meier
parent ce810542e2
commit 95faba391a
3 changed files with 28 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+8 -6
View File
@@ -3364,15 +3364,17 @@ set_control_mode()
control_mode.flag_control_rattitude_enabled = false;
control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position) && !status.in_transition_mode;
!offboard_control_mode.ignore_position) && !status.in_transition_mode &&
!control_mode.flag_control_acceleration_enabled;
control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position;
control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;
control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode;
control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode &&
!control_mode.flag_control_acceleration_enabled;
control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position;
control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;
break;
src/modules/mc_att_control/mc_att_control_main.cpp
+3 -80
View File
@@ -103,7 +103,6 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
#define RATES_I_LIMIT 0.3f
#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f
#define ATTITUDE_TC_DEFAULT 0.2f
#define SIGMA 0.000001f
class MulticopterAttitudeControl
{
@@ -131,7 +130,6 @@ private:
int _control_task; /**< task handle */
int _ctrl_state_sub; /**< control state subscription */
int _v_accel_sp_sub; /**< vehicle acceleration setpoint subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */
@@ -151,7 +149,6 @@ private:
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct control_state_s _ctrl_state; /**< control state */
struct position_setpoint_triplet_s _v_accel_sp; /**< vehicle acceleration setpoint */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
@@ -252,11 +249,6 @@ private:
*/
void vehicle_manual_poll();
/**
* Check for NED acceleration setpoint updates.
*/
void vehicle_accel_setpoint_poll();
/**
* Check for attitude setpoint updates.
*/
@@ -576,17 +568,6 @@ MulticopterAttitudeControl::vehicle_manual_poll()
}
}
void
MulticopterAttitudeControl::vehicle_accel_setpoint_poll() {
/* check if there is a new setpoint */
bool updated;
orb_check(_v_accel_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _v_accel_sp_sub, &_v_accel_sp);
}
}
void
MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
{
@@ -669,73 +650,16 @@ MulticopterAttitudeControl::control_attitude(float dt)
{
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
/* construct attitude setpoint rotation matrix */
math::Matrix<3, 3> R_sp;
R_sp.set(_v_att_sp.R_body);
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
if (_v_control_mode.flag_control_acceleration_enabled) {
vehicle_accel_setpoint_poll();
/* setpoint is NED */
math::Vector<3> accel_sp(_v_accel_sp.current.a_x, _v_accel_sp.current.a_y, _v_accel_sp.current.a_z);
float accel_abs = accel_sp.length();
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (accel_abs > SIGMA) {
body_z = -accel_sp / accel_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(_v_att_sp.yaw_body), cosf(_v_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;
body_y.normalize();
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R_sp(i, 0) = body_x(i);
R_sp(i, 1) = body_y(i);
R_sp(i, 2) = body_z(i);
}
_thrust_sp = accel_abs;
} else {
R_sp.set(_v_att_sp.R_body);
_thrust_sp = _v_att_sp.thrust;
}
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
@@ -884,7 +808,6 @@ MulticopterAttitudeControl::task_main()
/*
* do subscriptions
*/
_v_accel_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
src/modules/mc_pos_control/mc_pos_control_main.cpp
+17 -11
View File
@@ -1308,7 +1308,8 @@ MulticopterPositionControl::task_main()
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) {
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) {
_vel_ff.zero();
@@ -1517,8 +1518,6 @@ MulticopterPositionControl::task_main()
_takeoff_thrust_sp = 0.0f;
}
// limit total horizontal acceleration
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
@@ -1555,7 +1554,8 @@ MulticopterPositionControl::task_main()
_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) {
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) {
/* reset integrals if needed */
if (_control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
@@ -1613,7 +1613,12 @@ MulticopterPositionControl::task_main()
}
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
math::Vector<3> thrust_sp;
if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) {
thrust_sp = math::Vector<3>(_pos_sp_triplet.current.a_x,_pos_sp_triplet.current.a_y,_pos_sp_triplet.current.a_z);
} else {
thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
}
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) {
@@ -1622,12 +1627,12 @@ MulticopterPositionControl::task_main()
thrust_sp(2) = -_takeoff_thrust_sp;
}
if (!_control_mode.flag_control_velocity_enabled) {
if (!_control_mode.flag_control_velocity_enabled && !_control_mode.flag_control_acceleration_enabled) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
if (!_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_acceleration_enabled) {
thrust_sp(2) = 0.0f;
}
@@ -1705,7 +1710,7 @@ MulticopterPositionControl::task_main()
saturation_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
@@ -1795,7 +1800,7 @@ MulticopterPositionControl::task_main()
}
/* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled) {
if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
math::Vector<3> body_x;
math::Vector<3> body_y;
@@ -1996,14 +2001,15 @@ MulticopterPositionControl::task_main()
_vel_prev = _vel;
/* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity control is disabled,
* Do not publish if offboard is enabled but position/velocity/accel control is disabled,
* in this case the attitude setpoint is published by the mavlink app. Also do not publish
* if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
* attitude setpoints for the transition).
*/
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled))) {
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);