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

added turn coordination for rudder deflection

Browse Source
This commit is contained in:
Marvin Harms
2022-06-20 08:46:04 +02:00
parent 02d62a7eba
commit 1e73cdc4d2
4 changed files with 60 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+41 -10
View File
@@ -407,8 +407,8 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
// =======================================================================
bool error = false;
//char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
//char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
//PX4_ERR(home_dir);
strcat(home_dir,filename);
FILE* fp = fopen(home_dir, "r");
@@ -458,8 +458,8 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
}
row++;
}
fclose(fp);
}
fclose(fp);
// =======================================================================
@@ -610,8 +610,8 @@ FixedwingPositionINDIControl::Run()
// =====================
// compute control input
// =====================
Vector3f ctrl = _compute_NDI_stage_1(pos_ref, vel_ref, acc_ref, omega_ref, alpha_ref);
Vector3f ctrl1 = _compute_NDI_stage_2(ctrl);
Vector3f ctrl = _compute_INDI_stage_1(pos_ref, vel_ref, acc_ref, omega_ref, alpha_ref);
Vector3f ctrl1 = _compute_INDI_stage_2(ctrl);
// ============================
// compute actuator deflections
@@ -725,6 +725,17 @@ FixedwingPositionINDIControl::Run()
_soaring_controller_heartbeat.heartbeat = hrt_absolute_time();
_soaring_controller_heartbeat_pub.publish(_soaring_controller_heartbeat);
// ====================
// publish debug values
// ====================
Dcmf R_ib(_att);
Dcmf R_bi(R_ib.transpose());
Vector3f vel_body = R_bi*_vel;
_slip = atan2f(vel_body(1), vel_body(0));
_debug_value.timestamp = hrt_absolute_time();
_debug_value.value = _slip;
_debug_value_pub.publish(_debug_value);
perf_end(_loop_perf);
}
@@ -1023,7 +1034,7 @@ FixedwingPositionINDIControl::_get_attitude(Vector3f vel, Vector3f f)
}
Vector3f
FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref)
FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref)
{
Dcmf R_ib(_att);
Dcmf R_bi(R_ib.transpose());
@@ -1150,11 +1161,31 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
}
// ==============================================================
// overwrite rudder rot_acc_command with turn coordination values
// ==============================================================
Vector3f vel_air = _vel - _wind_estimate;
Vector3f vel_normalized = vel_air.normalized();
Vector3f f = _mass*_acc;
Vector3f f_normalized = f.normalized();
Vector3f omega_turn_ref_normalized = vel_normalized.cross(f_normalized);
Vector3f omega_turn_ref;
if (_airspeed_valid&&_airspeed>5.0f) {
omega_turn_ref = sqrtf(_acc*_acc) / (_airspeed) * R_bi * omega_turn_ref_normalized.normalized();
//PX4_INFO("yaw rate ref, yaw rate: \t%.2f\t%.2f", (double)(omega_turn_ref(2)), (double)(omega_filtered(2)));
}
else {
omega_turn_ref = sqrtf(_acc*_acc) / (5.f) * R_bi * omega_turn_ref_normalized.normalized();
//PX4_ERR("No valid airspeed message detected or airspeed to low");
}
rot_acc_command(2) = _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2));
return rot_acc_command;
}
Vector3f
FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl)
{
// compute velocity in body frame
Dcmf R_ib(_att);
@@ -1188,9 +1219,9 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
deflection(1) = (moment_command(1) + _k_d_pitch*q*omega_filtered(1))/fmaxf((_k_ele*q),0.0001f);
deflection(2) = (moment_command(2) + _k_d_yaw*q*omega_filtered(2))/fmaxf((_k_rud*q),0.0001f);
// TODO: tune feedback turn coordination
float turn_coordination = 0.f*vel_body(1)/(powf(vel_body(0),2)+0.0001f);
deflection(2) += turn_coordination;
// overwrite rudder deflection with NDI turn coordination (no INDI)
Vector3f moment_ref = _inertia*ctrl + _omega.cross(_inertia*_omega);
deflection(2) = (moment_ref(2) + _k_d_yaw*q*_omega(2)) / fmaxf((_k_rud*q),0.0001f);
return deflection;
}
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+8 -2
View File
@@ -67,6 +67,7 @@
#include <uORB/topics/soaring_controller_position.h>
#include <uORB/topics/soaring_controller_status.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/wind.h>
#include <uORB/uORB.h>
@@ -133,6 +134,7 @@ private:
uORB::Publication<soaring_controller_position_setpoint_s> _soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)};
uORB::Publication<soaring_controller_position_s> _soaring_controller_position_pub{ORB_ID(soaring_controller_position)};
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
uORB::Publication<debug_value_s> _debug_value_pub{ORB_ID(debug_value)};
// Message structs
vehicle_angular_acceleration_setpoint_s _angular_accel_sp {};
@@ -153,6 +155,7 @@ private:
soaring_controller_heartbeat_s _soaring_controller_heartbeat{}; ///< soaring controller hrt
soaring_controller_position_setpoint_s _soaring_controller_position_setpoint{}; ///< soaring controller pos setpoint
soaring_controller_position_s _soaring_controller_position{}; ///< soaring controller pos
debug_value_s _debug_value{}; // slip angle
// parameter struct
DEFINE_PARAMETERS(
@@ -263,11 +266,14 @@ private:
Vector3f _get_angular_velocity_ref(float t=0, float T=1); // get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
Vector3f _get_angular_acceleration_ref(float t=0, float T=1); // get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel
Vector3f _compute_NDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref);
Vector3f _compute_NDI_stage_2(Vector3f ctrl);
Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref);
Vector3f _compute_INDI_stage_2(Vector3f ctrl);
Vector3f _apply_LP_filter(Vector3f new_input, Vector<Vector3f, 3> &old_input, Vector<Vector3f, 2> &old_output);
Vector3f _compute_actuator_deflections(Vector3f ctrl);
// yaw controller
ECL_YawController _yaw_ctrl;
// control variables
Vector<float, _num_basis_funs> _basis_coeffs_x = {}; // coefficients of the current path
Vector<float, _num_basis_funs> _basis_coeffs_y = {}; // coefficients of the current path
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+10 -10
View File
@@ -436,8 +436,8 @@ PARAM_DEFINE_FLOAT(DS_K_W_YAW, 1.0f);
* @unit
* @min 0
* @max 100
* @decimal 2
* @increment 0.01
* @decimal 3
* @increment 0.001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.1f);
@@ -460,8 +460,8 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.03f);
* @unit
* @min 0
* @max 100
* @decimal 2
* @increment 0.01
* @decimal 4
* @increment 0.0001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_K_ACT_YAW, 0.1f);
@@ -472,8 +472,8 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_YAW, 0.1f);
* @unit
* @min 0
* @max 100
* @decimal 2
* @increment 0.01
* @decimal 3
* @increment 0.001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.02f);
@@ -484,8 +484,8 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.02f);
* @unit
* @min 0
* @max 100
* @decimal 2
* @increment 0.01
* @decimal 3
* @increment 0.001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.01f);
@@ -496,8 +496,8 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.01f);
* @unit
* @min 0
* @max 100
* @decimal 2
* @increment 0.01
* @decimal 4
* @increment 0.0001
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(DS_K_DAMP_YAW, 0.0f);
src/modules/logger/logged_topics.cpp
+1
View File
@@ -124,6 +124,7 @@ void LoggedTopics::add_default_topics()
add_topic("vehicle_actuator_setpoint", 20);
add_topic("soaring_controller_position", 50);
add_topic("soaring_controller_position_setpoint", 50);
add_topic("debug_value", 50);
// multi topics
add_topic_multi("actuator_outputs", 100, 3);