Skip to content

Commit

Permalink
Merge pull request #1823 from PX4/multiplatform_1741_1801
Browse files Browse the repository at this point in the history 
Multiplatform 1741 and 1801
  • Loading branch information
@thomasgubler
thomasgubler committed Feb 24, 2015
2 parents e91dabd + a284a98 commit 8794685
Show file tree
Hide file tree
Showing 10 changed files with 137 additions and 200 deletions.
23 changes: 0 additions & 23 deletions src/modules/mc_att_control_multiplatform/mc_att_control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,9 +89,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
.yaw_rate_d = px4::ParameterFloat("MC_YAWRATE_D", PARAM_MC_YAWRATE_D_DEFAULT),
.yaw_ff = px4::ParameterFloat("MC_YAW_FF", PARAM_MC_YAW_FF_DEFAULT),
.yaw_rate_max = px4::ParameterFloat("MC_YAWRATE_MAX", PARAM_MC_YAWRATE_MAX_DEFAULT),
.man_roll_max = px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MC_MAN_R_MAX_DEFAULT),
.man_pitch_max = px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MC_MAN_P_MAX_DEFAULT),
.man_yaw_max = px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MC_MAN_Y_MAX_DEFAULT),
.acro_roll_max = px4::ParameterFloat("MC_ACRO_R_MAX", PARAM_MC_ACRO_R_MAX_DEFAULT),
.acro_pitch_max = px4::ParameterFloat("MC_ACRO_P_MAX", PARAM_MC_ACRO_P_MAX_DEFAULT),
.acro_yaw_max = px4::ParameterFloat("MC_ACRO_Y_MAX", PARAM_MC_ACRO_Y_MAX_DEFAULT)
Expand Down Expand Up @@ -146,11 +143,6 @@ MulticopterAttitudeControl::parameters_update()
_params.yaw_ff = _params_handles.yaw_ff.update();
_params.yaw_rate_max = math::radians(_params_handles.yaw_rate_max.update());

/* manual control scale */
_params.man_roll_max = math::radians(_params_handles.man_roll_max.update());
_params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update());
_params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update());

/* acro control scale */
_params.acro_rate_max(0) = math::radians(_params_handles.acro_roll_max.update());
_params.acro_rate_max(1) = math::radians(_params_handles.acro_pitch_max.update());
Expand Down Expand Up @@ -186,18 +178,6 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti
if (_v_control_mode->data().flag_control_attitude_enabled) {
control_attitude(dt);

/* publish the attitude setpoint if needed */
if (_publish_att_sp && _v_status->data().is_rotary_wing) {
_v_att_sp_mod.data().timestamp = px4::get_time_micros();

if (_att_sp_pub != nullptr) {
_att_sp_pub->publish(_v_att_sp_mod);

} else {
_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
}
}

/* publish attitude rates setpoint */
_v_rates_sp_mod.data().roll = _rates_sp(0);
_v_rates_sp_mod.data().pitch = _rates_sp(1);
Expand All @@ -224,9 +204,6 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti
_manual_control_sp->data().r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp->data().z;

/* reset yaw setpoint after ACRO */
_reset_yaw_sp = true;

/* publish attitude rates setpoint */
_v_rates_sp_mod.data().roll = _rates_sp(0);
_v_rates_sp_mod.data().pitch = _rates_sp(1);
Expand Down
3 changes: 0 additions & 3 deletions src/modules/mc_att_control_multiplatform/mc_att_control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,9 +110,6 @@ class MulticopterAttitudeControl :
px4::ParameterFloat yaw_ff;
px4::ParameterFloat yaw_rate_max;

px4::ParameterFloat man_roll_max;
px4::ParameterFloat man_pitch_max;
px4::ParameterFloat man_yaw_max;
px4::ParameterFloat acro_roll_max;
px4::ParameterFloat acro_pitch_max;
px4::ParameterFloat acro_yaw_max;
Expand Down
119 changes: 12 additions & 107 deletions src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,6 @@ using namespace std;
#endif

MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
_v_att_sp_mod(),
_v_rates_sp_mod(),
_actuators(),
_publish_att_sp(false)
Expand All @@ -67,9 +66,6 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
_params.rate_d.zero();
_params.yaw_ff = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.man_roll_max = 0.0f;
_params.man_pitch_max = 0.0f;
_params.man_yaw_max = 0.0f;
_params.acro_rate_max.zero();

_rates_prev.zero();
Expand All @@ -87,101 +83,11 @@ MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()

void MulticopterAttitudeControlBase::control_attitude(float dt)
{
float yaw_sp_move_rate = 0.0f;
_publish_att_sp = false;


if (_v_control_mode->data().flag_control_manual_enabled) {
/* manual input, set or modify attitude setpoint */

if (_v_control_mode->data().flag_control_velocity_enabled
|| _v_control_mode->data().flag_control_climb_rate_enabled) {
/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
//XXX get rid of memcpy
memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data()));
}

if (!_v_control_mode->data().flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude stabilized mode */
_v_att_sp_mod.data().thrust = _manual_control_sp->data().z;
_publish_att_sp = true;
}

if (!_armed->data().armed) {
/* reset yaw setpoint when disarmed */
_reset_yaw_sp = true;
}

/* reset yaw setpoint to current position if needed */
if (_reset_yaw_sp) {
_reset_yaw_sp = false;
_v_att_sp_mod.data().yaw_body = _v_att->data().yaw;
_v_att_sp_mod.data().R_valid = false;
// _publish_att_sp = true;
}

if (!_v_control_mode->data().flag_control_velocity_enabled) {

if (_v_att_sp_mod.data().thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
_v_att_sp_mod.data().yaw_body = _wrap_pi(
_v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);

if (yaw_offs < -yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);

} else if (yaw_offs > yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
}

_v_att_sp_mod.data().R_valid = false;
// _publish_att_sp = true;
}
/* update attitude setpoint if not in position control mode */
_v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
_v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x
* _params.man_pitch_max;
_v_att_sp_mod.data().R_valid = false;
// _publish_att_sp = true;
}

} else {
/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
//XXX get rid of memcpy
memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data()));

/* reset yaw setpoint after non-manual control mode */
_reset_yaw_sp = true;
}

_thrust_sp = _v_att_sp_mod.data().thrust;
_thrust_sp = _v_att_sp->data().thrust;

/* construct attitude setpoint rotation matrix */
math::Matrix<3, 3> R_sp;

if (_v_att_sp_mod.data().R_valid) {
/* rotation matrix in _att_sp is valid, use it */
R_sp.set(&_v_att_sp_mod.data().R_body[0]);

} else {
/* rotation matrix in _att_sp is not valid, use euler angles instead */
R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body,
_v_att_sp_mod.data().yaw_body);

/* copy rotation matrix back to setpoint struct */
memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0],
sizeof(_v_att_sp_mod.data().R_body));
_v_att_sp_mod.data().R_valid = true;
}
R_sp.set(_v_att_sp->data().R_body);

/* rotation matrix for current state */
math::Matrix<3, 3> R;
Expand All @@ -190,11 +96,11 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
/* all input data is ready, run controller itself */

/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
math::Vector <3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector <3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));

/* axis and sin(angle) of desired rotation */
math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);
math::Vector <3> e_R = R.transposed() * (R_z % R_sp_z);

/* calculate angle error */
float e_R_z_sin = e_R.length();
Expand All @@ -209,7 +115,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;
math::Vector <3> e_R_z_axis = e_R / e_R_z_sin;

e_R = e_R_z_axis * e_R_z_angle;

Expand All @@ -224,26 +130,24 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
e_R_cp(2, 1) = e_R_z_axis(0);

/* rotation matrix for roll/pitch only rotation */
R_rp = R
* (_I + e_R_cp * e_R_z_sin
+ e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));

} else {
/* zero roll/pitch rotation */
R_rp = R;
}

/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
math::Vector <3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector <3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;

if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q;
q.from_dcm(R.transposed() * R_sp);
math::Vector < 3 > e_R_d = q.imag();
math::Vector <3> e_R_d = q.imag();
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));

Expand All @@ -260,7 +164,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
_params.yaw_rate_max);

/* feed forward yaw setpoint rate */
_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
_rates_sp(2) += _v_att_sp->data().yaw_sp_move_rate * yaw_w * _params.yaw_ff;

}

void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
Expand Down
5 changes: 0 additions & 5 deletions src/modules/mc_att_control_multiplatform/mc_att_control_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,8 +97,6 @@ class MulticopterAttitudeControlBase
px4::Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */
px4::Subscriber<px4_vehicle_status> *_v_status; /**< vehicle status */

px4_vehicle_attitude_setpoint _v_att_sp_mod; /**< modified vehicle attitude setpoint
that gets published eventually */
px4_vehicle_rates_setpoint _v_rates_sp_mod; /**< vehicle rates setpoint
that gets published eventually*/
px4_actuator_controls_0 _actuators; /**< actuator controls */
Expand All @@ -121,9 +119,6 @@ class MulticopterAttitudeControlBase
float yaw_ff; /**< yaw control feed-forward */
float yaw_rate_max; /**< max yaw rate */

float man_roll_max;
float man_pitch_max;
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */

int32_t autostart_id;
Expand Down
29 changes: 0 additions & 29 deletions src/modules/mc_att_control_multiplatform/mc_att_control_params.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,35 +189,6 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);

/**
* Max manual roll
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);

/**
* Max manual pitch
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);

/**
* Max manual yaw rate
*
* @unit deg/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);

/**
* Max acro roll rate
*
Expand Down
3 changes: 0 additions & 3 deletions src/modules/mc_att_control_multiplatform/mc_att_control_params.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,9 +57,6 @@
#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f
#define PARAM_MC_YAW_FF_DEFAULT 0.5f
#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f
#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f
#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f
#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f
#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f
#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f
#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f
Loading

0 comments on commit 8794685

Please sign in to comment.