dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added switching from attitude control generated rate setpoints to manual rate setpoints when in rattitude mode

This commit is contained in:
Eddy 2015-10-21 17:49:50 -04:00
parent c4a82d78c8
commit d4a19163bb
2 changed files with 59 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -190,12 +190,10 @@ private:
param_t pitch_rate_max;
param_t yaw_rate_max;
param_t man_roll_max;
param_t man_pitch_max;
param_t man_yaw_max;
param_t acro_roll_max;
param_t acro_pitch_max;
param_t acro_yaw_max;
param_t rattitude_thres;
} _params_handles; /**< handles for interesting parameters */
@ -212,11 +210,8 @@ private:
float yaw_rate_max;
math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */
float man_roll_max;
float man_pitch_max;
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
float rattitude_thres;
} _params;
/**
@ -346,11 +341,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.roll_rate_max = 0.0f;
_params.pitch_rate_max = 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.mc_rate_max.zero();
_params.acro_rate_max.zero();
_params.rattitude_thres = 1.0f;
_rates_prev.zero();
_rates_sp.zero();
@ -380,12 +373,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX");
_params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.man_roll_max = param_find("MC_MAN_R_MAX");
_params_handles.man_pitch_max = param_find("MC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX");
_params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.rattitude_thres = param_find("MC_RATT_TH");
/* fetch initial parameter values */
parameters_update();
@ -466,14 +457,6 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
/* manual attitude control scale */
param_get(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
_params.man_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max);
/* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
@ -482,6 +465,9 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
/* stick deflection needed in rattitude mode to control rates not angles */
param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
@ -810,22 +796,45 @@ MulticopterAttitudeControl::task_main()
vehicle_motor_limits_poll();
if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt);
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
/* Check if we want to directly pass through manual rate setpoints*/
if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){
/* Calculate the manual rates for all channels */
math::Vector<3> man_rates = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
/* Check all channels and replace attitude controler rate setpoint if manual input greater than threshold*/
_v_rates_sp.roll = (fabsf(_manual_control_sp.y) > _params.rattitude_thres) ? man_rates(0) : _rates_sp(0);
_v_rates_sp.pitch = (fabsf(_manual_control_sp.x) > _params.rattitude_thres) ? man_rates(1) : _rates_sp(1);
_v_rates_sp.yaw = (fabsf(_manual_control_sp.r) > _params.rattitude_thres) ? man_rates(2) : _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
/* publish attitude rates setpoint */
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
}else{
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {
@ -976,4 +985,4 @@ int mc_att_control_main(int argc, char *argv[])
warnx("unrecognized command");
return 1;
}
}

19
src/modules/mc_att_control/mc_att_control_params.c
View File

@ -251,7 +251,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f);
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 360.0f);
/**
* Max acro pitch rate
@ -261,7 +261,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 360.0f);
/**
* Max acro yaw rate
@ -270,4 +270,17 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f);
/**
* Threshold for Rattitude mode
*
* Manual input needed in order to override attitude control rate setpoints
* and instead pass manual stick inputs as rate setpoints
*
* @unit
* @min 0.0
* @max 1.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f);