From b60a8ab7deaba710e7bf98961f9eb7537412b6d0 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Wed, 8 Jun 2022 13:55:13 +0200 Subject: [PATCH] add thrust param and mulitstage t_ref computation --- msg/soaring_controller_position.msg | 3 +- .../FixedwingPositionINDIControl.cpp | 120 ++++++++++-------- .../FixedwingPositionINDIControl.hpp | 6 +- .../fw_dyn_soar_control_params.c | 21 ++- .../trajectories/trajectory4.csv | 3 + .../trajectories/trajectory5.csv | 3 + .../trajectory_inclined_circle.csv | 3 + 7 files changed, 104 insertions(+), 55 deletions(-) create mode 100644 src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv create mode 100644 src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv create mode 100644 src/modules/fw_dyn_soar_control/trajectories/trajectory_inclined_circle.csv diff --git a/msg/soaring_controller_position.msg b/msg/soaring_controller_position.msg index 33ab052e02..f77f6799b0 100644 --- a/msg/soaring_controller_position.msg +++ b/msg/soaring_controller_position.msg @@ -4,4 +4,5 @@ uint64 timestamp # time since system start (microseconds) float32[3] pos # POSITION VECTOR IN SOARING ENU FRAME float32[3] vel # VELOCITY VECTOR IN SOARING ENU FRAME -float32[3] acc # ACCELERATION VECTOR IN SOARING ENU FRAME \ No newline at end of file +float32[3] acc # ACCELERATION VECTOR IN SOARING ENU FRAME +float32[4] att # UNIT QUATERNION DESCRIBING BODY FRAME POSE TO ENU \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 990a3f898e..510cb3220f 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -154,6 +154,8 @@ FixedwingPositionINDIControl::parameters_update() _loiter = _param_loiter.get(); _select_trajectory(0.0f); + _thrust = _param_thrust.get(); + // sanity check parameters // TODO: include sanity check @@ -349,9 +351,15 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy) else if (_loiter==2) { _read_trajectory_coeffs_csv("trajectory2.csv"); } - else{ + else if (_loiter==3) { _read_trajectory_coeffs_csv("trajectory3.csv"); } + else if (_loiter==4) { + _read_trajectory_coeffs_csv("trajectory4.csv"); + } + else{ + _read_trajectory_coeffs_csv("trajectory5.csv"); + } } void @@ -501,8 +509,11 @@ FixedwingPositionINDIControl::Run() //_last_run = _local_pos.timestamp; // check if local NED reference frame origin has changed: + // || (_local_pos.vxy_reset_counter != _pos_reset_counter if (!map_projection_initialized(&_global_local_proj_ref) - || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) { + || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp) + || (_local_pos.xy_reset_counter != _pos_reset_counter) + || (_local_pos.z_reset_counter != _alt_reset_counter)) { // initialize projection map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon, _local_pos.ref_timestamp); @@ -511,6 +522,9 @@ FixedwingPositionINDIControl::Run() _origin_D = _local_pos.ref_alt - _origin_alt; PX4_INFO("local reference frame updated"); } + // update reset counters + _pos_reset_counter = _local_pos.xy_reset_counter; + _alt_reset_counter = _local_pos.z_reset_counter; // run polls _set_wind_estimate(Vector3f(0.f,0.f,0.f)); @@ -530,7 +544,6 @@ FixedwingPositionINDIControl::Run() actuator_controls_poll(); } - // ============================ // compute reference kinematics // ============================ @@ -540,8 +553,6 @@ FixedwingPositionINDIControl::Run() // terminal time is determined such that current velocity is met 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); Vector3f pos_ref = _get_position_ref(t_ref); // in inertial ENU Vector3f vel_ref = _get_velocity_ref(t_ref,T); // in inertial ENU Vector3f acc_ref = _get_acceleration_ref(t_ref,T); // gravity-corrected acceleration (ENU) @@ -553,7 +564,6 @@ FixedwingPositionINDIControl::Run() //PX4_INFO("vel ref:\t%.4f\t%.4f\t%.4f", (double)vel_ref(0),(double)vel_ref(1),(double)vel_ref(2)); //PX4_INFO("vel:\t%.4f\t%.4f\t%.4f", (double)_vel(0),(double)_vel(1),(double)_vel(2)); - // ===================== // compute control input // ===================== @@ -641,7 +651,7 @@ FixedwingPositionINDIControl::Run() _actuators.control[actuator_controls_s::INDEX_ROLL] = ctrl2(0); _actuators.control[actuator_controls_s::INDEX_PITCH] = ctrl2(1); _actuators.control[actuator_controls_s::INDEX_YAW] = ctrl2(2); - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.8f; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _thrust; _actuators_0_pub.publish(_actuators); //print_message(_actuators); @@ -841,6 +851,7 @@ FixedwingPositionINDIControl::_get_angular_acceleration_ref(float t, float T) float FixedwingPositionINDIControl::_get_closest_t(Vector3f pos) { + /* const uint n = 100; Vector distances; float t_ref; @@ -862,53 +873,62 @@ FixedwingPositionINDIControl::_get_closest_t(Vector3f pos) } } t = t/float(n); + */ + //PX4_INFO("closest point: \t%.2f\t%.2f\t%.2f", (double)_get_position_ref(t)(0), (double)_get_position_ref(t)(1), (double)_get_position_ref(t)(2)); //PX4_INFO("closest t: %.2f", (double)t); //PX4_INFO("closest distance:%.2f", (double)sqrtf(min_dist)); - - /* - const uint n_multistage = 10; - Vector distances_multistage; - float t_multistage = 0; - float t_ref_next = 0; // stage 2 - // compute all distances - for(uint stage=1; stage<=2; stage++){ - for(uint i=0; i<=n_multistage; i++){ - t_ref = t_ref_next + float(i-n_multistage/2.f)/(powf(float(n_multistage),stage)); - // check that t_ref is always in [0,1]: - if(t_ref<0.f){ - t_ref += 1.f; - } - else if (t_ref>1.f){ - t_ref -= 1.f; - } - Vector3f pos_ref = _get_position_ref(t_ref); - //PX4_INFO("trajectory time + point: \t%.2f\t%.2f\t%.2f\t%.2f", (double)t_ref, (double)pos_ref(0), (double)pos_ref(1), (double)pos_ref(2)); - distances_multistage(i) = (pos_ref - pos)*(pos_ref - pos); - } - // get index of smallest distance - float min_dist_multistage = distances_multistage(0); - for(uint i=1; i<=n_multistage; i++){ - if(distances_multistage(i)1.f){ - t_multistage -= 1.f; - } - } - } - // next starting point is previous closest point - t_ref_next = t_multistage; + + const uint n_1 = 20; + Vector distances; + float t_ref; + // ======= + // STAGE 1 + // ======= + // STAGE 1: compute all distances + for(uint i=0; i distances_2; + float t_lower = fmod(t_1 - 1.0f/n_1,1.0f); + // STAGE 2: compute all distances + for(uint i=0; i<=n_2; i++){ + t_ref = fmod(t_lower + float(i)*2.f/float(n_1*n_2),1.0f); + Vector3f pos_ref = _get_position_ref(t_ref); + distances_2(i) = (pos_ref - pos)*(pos_ref - pos); + } + + // STAGE 2: get index of smallest distance + float t_2 = 0.f; + min_dist = distances_2(0); + for(uint i=1; i<=n_2; i++){ + if(distances_2(i)M_PI_F){ diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index fcdb7eb297..4fd6448d80 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -204,7 +204,9 @@ private: (ParamFloat) _param_origin_lon, (ParamFloat) _param_origin_alt, // loiter params - (ParamInt) _param_loiter + (ParamInt) _param_loiter, + // thrust params + (ParamFloat) _param_thrust ) @@ -336,6 +338,8 @@ private: float _origin_D; // loiter circle int _loiter; + // thrust + float _thrust; bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts. diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index dfcf8426aa..50d0223e0e 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c @@ -464,7 +464,7 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.03f); * @increment 0.01 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.02f); +PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.1f); /** * roll gain of K_ACT_DAMPING (actuator damping gain) @@ -552,9 +552,24 @@ PARAM_DEFINE_FLOAT(ORIGIN_ALT, 488.0f); * * @unit * @min 0 - * @max 3 + * @max 5 * @decimal 1 * @increment 1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_INT32(LOITER, 0); \ No newline at end of file +PARAM_DEFINE_INT32(LOITER, 0); + +// ============================================== +// ============== engine thrust ================= +// ============================================== +/** + * float in [0,1] corresponding to the engine thrust + * + * @unit + * @min 0 + * @max 1 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(THRUST, 0); \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv new file mode 100644 index 0000000000..eb78f07b26 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv @@ -0,0 +1,3 @@ +-0.000007,-1562.842332,5544.524783,-9471.385118,8418.634792,-1445.117008,-5918.737594,6576.967875,-70.729530,-6495.475777,5967.338364,1297.233149,-8260.728734,9370.125196,-5505.393163,1555.636279 +-49.997787,-2547.580420,12068.136308,-28052.676677,40280.039282,-35081.667957,9874.924100,20579.781253,-34024.375145,20153.308920,10529.292601,-35702.121913,40697.894343,-28252.039474,12129.964939,-2557.102237 +100.000003,781.414284,-2772.271926,4735.610574,-4209.323593,722.471906,2959.394584,-3288.377841,35.294844,3247.746206,-2983.638693,-648.993422,4130.425992,-4685.191554,2752.757438,-777.821092 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv new file mode 100644 index 0000000000..b87aeed8ef --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv @@ -0,0 +1,3 @@ +-0.000003,-781.421166,2772.262391,-4735.692559,4209.317396,-722.558504,-2959.368797,3288.483937,-35.364765,-3247.737889,2983.669182,648.616574,-4130.364367,4685.062598,-2752.696581,777.818140 +-24.998893,-1273.790210,6034.068154,-14026.338339,20140.019641,-17540.833978,4937.462050,10289.890626,-17012.187572,10076.654460,5264.646301,-17851.060956,20348.947172,-14126.019737,6064.982470,-1278.551118 +100.000002,390.703701,-1386.140730,2367.764295,-2104.664895,361.192653,1479.710185,-1644.135872,17.612462,1623.877262,-1491.804102,-324.685135,2065.243808,-2342.660255,1376.409147,-388.912022 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory_inclined_circle.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory_inclined_circle.csv new file mode 100644 index 0000000000..37abebcd1c --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory_inclined_circle.csv @@ -0,0 +1,3 @@ +-0.000004,-937.705399,3326.714870,-5682.831071,5051.180875,-867.070205,-3551.242557,3946.180725,-42.437718,-3897.285466,3580.403018,778.339889,-4956.437241,5622.075118,-3303.235898,933.381767 +-29.998672,-1528.548252,7240.881785,-16831.606006,24168.023569,-21049.000774,5924.954460,12347.868752,-20414.625087,12091.985352,6317.575561,-21421.273148,24418.736606,-16951.223684,7277.978963,-1534.261342 +100.000002,468.845818,-1663.366969,2841.333551,-2525.596635,433.448504,1775.647065,-1972.984266,21.148938,1948.651051,-1790.171020,-389.546792,2478.280245,-2811.166515,1651.678805,-466.693836