fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 18:27:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw: att fix initialization and add parameter to disable coordinated turns at low speed

Browse Source
This commit is contained in:
Thomas Gubler
2013-11-08 21:27:00 +01:00
parent 668f9dc552
commit a1b80ec3f3
4 changed files with 29 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+4 -3
View File
@@ -59,21 +59,22 @@ ECL_YawController::ECL_YawController() :
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f),
_coordinated(0.0f)
_coordinated_min_speed(1.0f)
{
}
float ECL_YawController::control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_w,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint)
{
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
if (_coordinated > 0.1) {
if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
if(denumerator != 0.0f) { //XXX: floating point comparison
_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
}
// if(counter % 20 == 0) {
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+4 -4
View File
@@ -57,7 +57,7 @@ public:
ECL_YawController();
float control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_w,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint);
float control_bodyrate( float roll, float pitch,
@@ -85,8 +85,8 @@ public:
void set_k_roll_ff(float roll_ff) {
_roll_ff = roll_ff;
}
void set_coordinated(float coordinated) {
_coordinated = coordinated;
void set_coordinated_min_speed(float coordinated_min_speed) {
_coordinated_min_speed = coordinated_min_speed;
}
@@ -115,7 +115,7 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
float _coordinated;
float _coordinated_min_speed;
};
src/modules/fw_att_control/fw_att_control_main.cpp
+20 -8
View File
@@ -155,7 +155,7 @@ private:
float y_d;
float y_roll_feedforward;
float y_integrator_max;
float y_coordinated;
float y_coordinated_min_speed;
float y_rmax;
float airspeed_min;
@@ -183,7 +183,7 @@ private:
param_t y_d;
param_t y_roll_feedforward;
param_t y_integrator_max;
param_t y_coordinated;
param_t y_coordinated_min_speed;
param_t y_rmax;
param_t airspeed_min;
@@ -287,6 +287,17 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_setpoint_valid(false),
_airspeed_valid(false)
{
/* safely initialize structs */
vehicle_attitude_s _att = {0};
accel_report _accel = {0};
vehicle_attitude_setpoint_s _att_sp = {0};
manual_control_setpoint_s _manual = {0};
airspeed_s _airspeed = {0};
vehicle_control_mode_s _vcontrol_mode = {0};
actuator_controls_s _actuators = {0};
vehicle_global_position_s _global_pos = {0};
_parameter_handles.tconst = param_find("FW_ATT_TC");
_parameter_handles.p_p = param_find("FW_PR_P");
_parameter_handles.p_d = param_find("FW_PR_D");
@@ -313,7 +324,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
_parameter_handles.y_coordinated = param_find("FW_Y_COORD");
_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
/* fetch initial parameter values */
parameters_update();
@@ -368,7 +379,7 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.y_d, &(_parameters.y_d));
param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward));
param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
param_get(_parameter_handles.y_coordinated, &(_parameters.y_coordinated));
param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
@@ -399,7 +410,7 @@ FixedwingAttitudeControl::parameters_update()
_yaw_ctrl.set_k_d(_parameters.y_d);
_yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward);
_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
_yaw_ctrl.set_coordinated(_parameters.y_coordinated);
_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
return OK;
@@ -672,10 +683,11 @@ FixedwingAttitudeControl::task_main()
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz;
// speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz;
speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz;
speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz;
} else {
warnx("Did not get a valid R\n");
@@ -686,7 +698,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.control_attitude(roll_sp, _att.roll);
_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
speed_body_u,speed_body_w,
speed_body_u, speed_body_v, speed_body_w,
_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
/* Run attitude RATE controllers which need the desired attitudes from above */
@@ -709,7 +721,7 @@ FixedwingAttitudeControl::task_main()
pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
}
float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch,
float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
_att.pitchspeed, _att.yawspeed,
_pitch_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
src/modules/fw_att_control/fw_att_control_params.c
+1 -1
View File
@@ -137,4 +137,4 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);
PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f);
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f);
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60);
PARAM_DEFINE_FLOAT(FW_Y_COORD, 1.0f);
PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f);