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

added dyn soar position setpoint message

Browse Source
This commit is contained in:
Marvin Harms
2022-06-06 18:29:55 +02:00
parent c44c387b6d
commit 3cb0418e0b
10 changed files with 83 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+32 -5
View File
@@ -77,9 +77,6 @@ FixedwingPositionINDIControl::init()
return false;
}
_read_trajectory_coeffs_csv("trajectory0.csv");
_read_trajectory_coeffs_csv("trajectory1.csv");
_read_trajectory_coeffs_csv("trajectory2.csv");
//_read_trajectory_coeffs_csv("trajectory3.csv");
// initialize transformations
_R_ned_to_enu *= 0.f;
@@ -154,6 +151,9 @@ FixedwingPositionINDIControl::parameters_update()
// TODO: do stuff
}
_loiter = _param_loiter.get();
_select_trajectory(0.0f);
// sanity check parameters
// TODO: include sanity check
@@ -339,7 +339,19 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind)
void
FixedwingPositionINDIControl::_select_trajectory(float initial_energy)
{
// select loiter trajectory for loiter test
if (_loiter==0) {
_read_trajectory_coeffs_csv("trajectory0.csv");
}
else if (_loiter==1) {
_read_trajectory_coeffs_csv("trajectory1.csv");
}
else if (_loiter==2) {
_read_trajectory_coeffs_csv("trajectory2.csv");
}
else{
_read_trajectory_coeffs_csv("trajectory3.csv");
}
}
void
@@ -526,7 +538,7 @@ FixedwingPositionINDIControl::Run()
float t_ref = _get_closest_t(_pos);
// downscale velocity to match current one,
// terminal time is determined such that current velocity is met
Vector3f v_ref_ = _get_velocity_ref(t_ref, 1.f);
Vector3f v_ref_ = _get_velocity_ref(t_ref, 1.0f);
float T = sqrtf((v_ref_*v_ref_)/(_vel*_vel+0.001f));
//PX4_INFO("local velocity:\t%.4f\t%.4f\t%.4f", (double)v_ref_(0),(double)v_ref_(1),(double)v_ref_(2));
//PX4_INFO("T= \t%.1f", (double)T);
@@ -564,6 +576,17 @@ FixedwingPositionINDIControl::Run()
// Publish actuator controls only once in OFFBOARD
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
// ====================================
// publish high level control variables
// ====================================
_soaring_controller_position_setpoint.timestamp = hrt_absolute_time();
for (int i=0; i<3; i++){
_soaring_controller_position_setpoint.pos[i] = pos_ref(i);
_soaring_controller_position_setpoint.vel[i] = vel_ref(i);
_soaring_controller_position_setpoint.acc[i] = acc_ref(i);
}
_soaring_controller_position_setpoint_pub.publish(_soaring_controller_position_setpoint);
// =====================
// publish control input
// =====================
@@ -1047,6 +1070,10 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
deflection(1) = (moment_command(1) + k_d_pitch*q*omega_filtered(1))/(k_ele*q);
deflection(2) = (moment_command(2) + k_d_yaw*q*omega_filtered(2))/(k_rud*q);
// TODO: tune feedback turn coordination
float turn_coordination = 0.f*vel_body(1)/powf(vel_body(0),2);
deflection(2) += turn_coordination;
return deflection;
}