fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

added parameter for maximal roll angle

Browse Source
This commit is contained in:
Thomas Gubler 2013-10-20 21:51:45 +02:00
parent cc324f2624
commit 1cb73687f7
4 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
View File

@ -43,7 +43,7 @@
float ECL_L1_Pos_Controller::nav_roll()
{
float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f);
ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
return ret;
}

11
src/lib/ecl/l1/ecl_l1_pos_controller.h
View File

@ -222,6 +222,15 @@ public:
_K_L1 = 4.0f * _L1_damping * _L1_damping;
}
/**
* Set the maximum roll angle output in radians
*
*/
void set_l1_roll_limit(float roll_lim_rad) {
_roll_lim_rad = roll_lim_rad;
}
private:
float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2
@ -238,6 +247,8 @@ private:
float _K_L1; ///< L1 control gain for _L1_damping
float _heading_omega; ///< Normalized frequency
float _roll_lim_rad; ///<maximum roll angle
/**
* Convert a 2D vector from WGS84 to planar coordinates.
*

5
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -192,6 +192,7 @@ private:
float pitch_limit_min;
float pitch_limit_max;
float roll_limit;
float throttle_min;
float throttle_max;
float throttle_cruise;
@ -223,6 +224,7 @@ private:
param_t pitch_limit_min;
param_t pitch_limit_max;
param_t roll_limit;
param_t throttle_min;
param_t throttle_max;
param_t throttle_cruise;
@ -339,6 +341,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
_parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX");
_parameter_handles.roll_limit = param_find("FW_R_LIM");
_parameter_handles.throttle_min = param_find("FW_THR_MIN");
_parameter_handles.throttle_max = param_find("FW_THR_MAX");
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
@ -400,6 +403,7 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit));
param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
@ -419,6 +423,7 @@ FixedwingPositionControl::parameters_update()
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_min_sink_rate(_parameters.min_sink_rate);

3
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -67,6 +67,9 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);