From 3cb0418e0b22ecf8b7afcd5dcdb66d7579a309bd Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Mon, 6 Jun 2022 18:29:55 +0200 Subject: [PATCH] added dyn soar position setpoint message --- msg/CMakeLists.txt | 1 + msg/soaring_controller_position_setpoint.msg | 9 +++++ .../FixedwingPositionINDIControl.cpp | 37 ++++++++++++++++--- .../FixedwingPositionINDIControl.hpp | 9 ++++- .../fw_dyn_soar_control_params.c | 23 ++++++++++-- .../trajectories/trajectory0.csv | 6 +-- .../trajectories/trajectory1.csv | 6 +-- .../trajectories/trajectory2.csv | 6 +-- .../trajectories/trajectory3.csv | 6 +-- src/modules/logger/logged_topics.cpp | 1 + 10 files changed, 83 insertions(+), 21 deletions(-) create mode 100644 msg/soaring_controller_position_setpoint.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 86750c9c08..80af177914 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -141,6 +141,7 @@ set(msg_files sensor_selection.msg sensors_status_imu.msg soaring_controller_heartbeat.msg + soaring_controller_position_setpoint.msg soaring_controller_status.msg system_power.msg takeoff_status.msg diff --git a/msg/soaring_controller_position_setpoint.msg b/msg/soaring_controller_position_setpoint.msg new file mode 100644 index 0000000000..464a2727d8 --- /dev/null +++ b/msg/soaring_controller_position_setpoint.msg @@ -0,0 +1,9 @@ +# SOARING CONTROLLER POSITION SETPOINT + +uint64 timestamp # time since system start (microseconds) + +float32[3] pos # position in ENU frame +float32[3] vel # velocity in ENU frame +float32[3] acc # acceleration in ENU frame + + diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 1462e88cbb..f99672a68b 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -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; } diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 2af153e78c..471626b6eb 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -128,6 +129,7 @@ private: uORB::Publication _angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; uORB::Publication _soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)}; + uORB::Publication _soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; // Message structs @@ -147,6 +149,7 @@ private: vehicle_status_s _vehicle_status {}; ///< vehicle status soaring_controller_status_s _soaring_controller_status {}; ///< soaring controller status soaring_controller_heartbeat_s _soaring_controller_heartbeat{}; ///< soaring controller hrt + soaring_controller_position_setpoint_s _soaring_controller_position_setpoint{}; ///< soaring controller pos setpoint // parameter struct DEFINE_PARAMETERS( @@ -196,7 +199,9 @@ private: // location params (ParamFloat) _param_origin_lat, (ParamFloat) _param_origin_lon, - (ParamFloat) _param_origin_alt + (ParamFloat) _param_origin_alt, + // loiter params + (ParamInt) _param_loiter ) @@ -326,6 +331,8 @@ private: float _origin_N; float _origin_E; float _origin_D; + // loiter circle + int _loiter; 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 e75d41d951..dfcf8426aa 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 @@ -467,7 +467,7 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.03f); PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.02f); /** - * roll gain of K_ACT (actuator deflection gain) + * roll gain of K_ACT_DAMPING (actuator damping gain) * * @unit * @min 0 @@ -479,7 +479,7 @@ PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.02f); PARAM_DEFINE_FLOAT(K_DAMPING_ROLL, 0.02f); /** - * pitch gain of K_ACT (actuator deflection gain) + * pitch gain of K_ACT_DAMPING (actuator damping gain) * * @unit * @min 0 @@ -491,7 +491,7 @@ PARAM_DEFINE_FLOAT(K_DAMPING_ROLL, 0.02f); PARAM_DEFINE_FLOAT(K_DAMPING_PITCH, 0.01f); /** - * yaw gain of K_ACT (actuator deflection gain) + * yaw gain of K_ACT_DAMPING (actuator damping gain) * * @unit * @min 0 @@ -541,3 +541,20 @@ PARAM_DEFINE_FLOAT(ORIGIN_LON, 8.54554f); * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(ORIGIN_ALT, 488.0f); + + +// ====================================================== +// ============== loiter circle number ================= +// ====================================================== + +/** + * integer in {0,1,2,3} defining the loiter trajectory + * + * @unit + * @min 0 + * @max 3 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(LOITER, 0); \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv index ce8c8c3937..acfd4d22af 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv @@ -1,3 +1,3 @@ --0.000064,-3020.233571,10609.960177,-17956.458964,15735.479961,-2399.573434,-11421.854705,12388.936542,120.944433,-12530.869640,11346.431128,2643.369342,-15999.009519,18127.094775,-10676.696033,3032.667571 --100.005984,-4686.100637,21963.998713,-50566.542718,71908.811359,-61683.065460,15730.546677,39386.062413,-63952.599923,39525.510553,15526.730604,-61505.752706,71804.582542,-50525.803330,21954.858741,-4685.311429 -100.000000,-0.013254,-0.012083,-0.281296,0.324078,-0.438216,0.063212,0.096122,0.034719,0.060176,0.153558,-0.402818,0.689494,-0.192379,0.050574,-0.011868 +-0.000014,-3125.684664,11089.049565,-18942.770235,16837.269583,-2890.234017,-11837.475189,13153.935749,-141.459060,-12990.951555,11934.676728,2594.466297,-16521.457469,18740.250392,-11010.786326,3111.272558 +-99.995573,-5095.160841,24136.272616,-56105.353355,80560.078564,-70163.335913,19749.848200,41159.562505,-68048.750290,40306.617839,21058.585202,-71404.243825,81395.788686,-56504.078947,24259.929878,-5114.204474 +100.000000,-0.006882,-0.009535,-0.081985,-0.006197,-0.086599,0.025787,0.106096,-0.069921,0.008318,0.030489,-0.376848,0.061624,-0.128956,0.060856,-0.002952 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv index 80248ac32a..eee5c54968 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv @@ -1,3 +1,3 @@ --0.000038,-1812.140143,6365.976106,-10773.875378,9441.287977,-1439.744061,-6853.112823,7433.361925,72.566660,-7518.521784,6807.858677,1586.021605,-9599.405711,10876.256865,-6406.017620,1819.600542 --60.003590,-2811.660382,13178.399228,-30339.925631,43145.286816,-37009.839276,9438.328006,23631.637448,-38371.559954,23715.306332,9316.038362,-36903.451624,43082.749525,-30315.481998,13172.915245,-2811.186857 -100.000000,-0.013254,-0.012083,-0.281296,0.324078,-0.438216,0.063212,0.096122,0.034719,0.060176,0.153558,-0.402818,0.689494,-0.192379,0.050574,-0.011868 +-0.000008,-1875.410798,6653.429739,-11365.662141,10102.361750,-1734.140410,-7102.485113,7892.361450,-84.875436,-7794.570933,7160.806037,1556.679778,-9912.874481,11244.150235,-6606.471795,1866.763535 +-59.997344,-3057.096504,14481.763570,-33663.212013,48336.047139,-42098.001548,11849.908920,24695.737503,-40829.250174,24183.970704,12635.151121,-42842.546295,48837.473212,-33902.447368,14555.957927,-3068.522684 +100.000000,-0.006882,-0.009535,-0.081985,-0.006197,-0.086599,0.025787,0.106096,-0.069921,0.008318,0.030489,-0.376848,0.061624,-0.128956,0.060856,-0.002952 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv index 16ed1b1198..c84f1bb195 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv @@ -1,3 +1,3 @@ --0.000038,-1812.140143,6365.976106,-10773.875378,9441.287977,-1439.744061,-6853.112823,7433.361925,72.566660,-7518.521784,6807.858677,1586.021605,-9599.405711,10876.256865,-6406.017620,1819.600542 --60.003590,-2811.660382,13178.399228,-30339.925631,43145.286816,-37009.839276,9438.328006,23631.637448,-38371.559954,23715.306332,9316.038362,-36903.451624,43082.749525,-30315.481998,13172.915245,-2811.186857 -120.000000,-0.015905,-0.014500,-0.337555,0.388894,-0.525859,0.075855,0.115346,0.041663,0.072211,0.184269,-0.483382,0.827393,-0.230854,0.060689,-0.014241 +-0.000008,-1875.410798,6653.429739,-11365.662141,10102.361750,-1734.140410,-7102.485113,7892.361450,-84.875436,-7794.570933,7160.806037,1556.679778,-9912.874481,11244.150235,-6606.471795,1866.763535 +-59.997344,-3057.096504,14481.763570,-33663.212013,48336.047139,-42098.001548,11849.908920,24695.737503,-40829.250174,24183.970704,12635.151121,-42842.546295,48837.473212,-33902.447368,14555.957927,-3068.522684 +120.000000,-0.008258,-0.011442,-0.098382,-0.007437,-0.103918,0.030944,0.127315,-0.083905,0.009981,0.036587,-0.452217,0.073949,-0.154747,0.073028,-0.003542 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv index 4f8a1fd753..62316cfa89 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv @@ -1,3 +1,3 @@ --0.000064,-3020.233571,10609.960177,-17956.458964,15735.479961,-2399.573434,-11421.854705,12388.936542,120.944433,-12530.869640,11346.431128,2643.369342,-15999.009519,18127.094775,-10676.696033,3032.667571 --100.005984,-4686.100637,21963.998713,-50566.542718,71908.811359,-61683.065460,15730.546677,39386.062413,-63952.599923,39525.510553,15526.730604,-61505.752706,71804.582542,-50525.803330,21954.858741,-4685.311429 -120.000000,-0.015905,-0.014500,-0.337555,0.388894,-0.525859,0.075855,0.115346,0.041663,0.072211,0.184269,-0.483382,0.827393,-0.230854,0.060689,-0.014241 +-0.000014,-3125.684664,11089.049565,-18942.770235,16837.269583,-2890.234017,-11837.475189,13153.935749,-141.459060,-12990.951555,11934.676728,2594.466297,-16521.457469,18740.250392,-11010.786326,3111.272558 +-99.995573,-5095.160841,24136.272616,-56105.353355,80560.078564,-70163.335913,19749.848200,41159.562505,-68048.750290,40306.617839,21058.585202,-71404.243825,81395.788686,-56504.078947,24259.929878,-5114.204474 +120.000000,-0.008258,-0.011442,-0.098382,-0.007437,-0.103918,0.030944,0.127315,-0.083905,0.009981,0.036587,-0.452217,0.073949,-0.154747,0.073028,-0.003542 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3d5652d828..79ec57cabd 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -122,6 +122,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_thrust_setpoint", 20); add_topic("vehicle_torque_setpoint", 20); add_topic("vehicle_actuator_setpoint", 20); + add_topic("soaring_controller_position_setpoint", 50); // multi topics add_topic_multi("actuator_outputs", 100, 3);