Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

simple NED acceleration control interface

Browse Source
This commit is contained in:
Benoit Landry
2016-03-09 12:47:08 -08:00
committed by Lorenz Meier
parent 2dac97fe68
commit ce810542e2
3 changed files with 91 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_control_mode.msg
+1
View File
@@ -16,6 +16,7 @@ bool flag_control_rates_enabled # true if rates are stabilized
bool flag_control_attitude_enabled # true if attitude stabilization is mixed in
bool flag_control_rattitude_enabled # true if rate/attitude stabilization is enabled
bool flag_control_force_enabled # true if force control is mixed in
bool flag_control_acceleration_enabled # true if acceleration is controlled
bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled
bool flag_control_position_enabled # true if position is controlled
bool flag_control_altitude_enabled # true if altitude is controlled
src/modules/commander/commander.cpp
+10
View File
@@ -3196,6 +3196,7 @@ set_control_mode()
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
@@ -3209,6 +3210,7 @@ set_control_mode()
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
/* override is not ok in stabilized mode */
control_mode.flag_external_manual_override_ok = false;
@@ -3224,6 +3226,7 @@ set_control_mode()
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
@@ -3237,6 +3240,7 @@ set_control_mode()
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
@@ -3250,6 +3254,7 @@ set_control_mode()
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = !status.in_transition_mode;
control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
@@ -3274,6 +3279,7 @@ set_control_mode()
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = !status.in_transition_mode;
control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
@@ -3287,6 +3293,7 @@ set_control_mode()
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
@@ -3300,6 +3307,7 @@ set_control_mode()
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
@@ -3312,6 +3320,7 @@ set_control_mode()
control_mode.flag_control_rattitude_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_termination_enabled = false;
@@ -3326,6 +3335,7 @@ set_control_mode()
control_mode.flag_control_rattitude_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_termination_enabled = true;
src/modules/mc_att_control/mc_att_control_main.cpp
+80 -3
View File
@@ -103,6 +103,7 @@ 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
{
@@ -130,6 +131,7 @@ 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 */
@@ -149,6 +151,7 @@ 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 */
@@ -249,6 +252,11 @@ private:
*/
void vehicle_manual_poll();
/**
* Check for NED acceleration setpoint updates.
*/
void vehicle_accel_setpoint_poll();
/**
* Check for attitude setpoint updates.
*/
@@ -568,6 +576,17 @@ 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()
{
@@ -650,16 +669,73 @@ 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 */
@@ -808,6 +884,7 @@ 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));