From 95faba391a8b6d891409679bc6dd94598b47f06b Mon Sep 17 00:00:00 2001
From: Benoit Landry <landry@mit.edu>
Date: Thu, 7 Apr 2016 15:09:21 -0700
Subject: [PATCH] accel control in pos controller
---
src/modules/commander/commander.cpp | 14 ++--
.../mc_att_control/mc_att_control_main.cpp | 83 +------------------
.../mc_pos_control/mc_pos_control_main.cpp | 28 ++++---
3 files changed, 28 insertions(+), 97 deletions(-)
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 84130c0201..d0148d3f19 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -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;
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index d76787be68..3ba1f4769e 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -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));
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 7909286f59..07b29e62b2 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -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);
--
GitLab