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

wip, fw att ctrl: coordinated turn

Browse Source
This commit is contained in:
Thomas Gubler
2013-10-15 19:05:23 +02:00
parent cb5e5e9143
commit 17e0c5053e
3 changed files with 23 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+18 -6
View File
@@ -47,12 +47,10 @@
ECL_YawController::ECL_YawController() :
_last_run(0),
_last_error(0.0f),
_last_output(0.0f),
_last_rate_hp_out(0.0f),
_last_rate_hp_in(0.0f),
_k_d_last(0.0f),
_integrator(0.0f)
_rate_setpoint(0.0f),
_max_deflection_rad(math::radians(45.0f))
{
}
@@ -66,7 +64,21 @@ float ECL_YawController::control(float roll, float yaw_rate, float accel_y, floa
float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
return 0.0f;
// float psi_dot = 0.0f;
// float denumerator = (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body));
// if(denumerator != 0.0f) {
// psi_dot = (speed_body[2] * phi_dot + 9.81f * sinf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[0] * theta_dot * sinf(att_sp->roll_body))
// / (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body));
// }
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_last_output = 0.0f;
float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
if(denumerator != 0.0f) { //XXX: floating point comparison
_last_output = (speed_body_w * roll_rate_desired + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_desired * sinf(roll)) / denumerator;
}
return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
}
void ECL_YawController::reset_integrator()
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+4 -26
View File
@@ -50,39 +50,17 @@ public:
float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
void reset_integrator();
void set_k_side(float k_a) {
_k_side = k_a;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_roll_ff(float k_roll_ff) {
_k_roll_ff = k_roll_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
float get_desired_rate() {
return _rate_setpoint;
}
private:
uint64_t _last_run;
float _k_side;
float _k_i;
float _k_d;
float _k_roll_ff;
float _integrator_max;
float _last_error;
float _rate_setpoint;
float _last_output;
float _last_rate_hp_out;
float _last_rate_hp_in;
float _k_d_last;
float _integrator;
float _max_deflection_rad;
};
src/modules/fw_att_control/fw_att_control_main.cpp
+1 -1
View File
@@ -661,7 +661,7 @@ FixedwingAttitudeControl::task_main()
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = 0.0f; // XXX not yet implemented
rates_sp.yaw = _yaw_ctrl.get_desired_rate();
rates_sp.timestamp = hrt_absolute_time();