fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 03:23:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated to master (solve merge conflicts)

Browse Source
This commit is contained in:
Youssef Demitri
2015-11-06 12:18:31 +01:00
parent 50c6d720f0 9b45daa8f7
commit 170f0032fd
72 changed files with 1418 additions and 830 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+32 -36
View File
@@ -39,7 +39,6 @@
* Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
* Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com>
*
@@ -190,12 +189,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 +209,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 +340,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 +372,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 +456,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 +464,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;
@@ -809,23 +794,34 @@ MulticopterAttitudeControl::task_main()
vehicle_status_poll();
vehicle_motor_limits_poll();
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
* or roll (yaw can rotate 360 in normal att control). If both are true don't
* even bother running the attitude controllers */
if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){
if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
fabsf(_manual_control_sp.x) > _params.rattitude_thres){
_v_control_mode.flag_control_attitude_enabled = false;
}
}
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();
/* 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);
}
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 +972,4 @@ int mc_att_control_main(int argc, char *argv[])
warnx("unrecognized command");
return 1;
}
}