added dyn soar position setpoint message

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

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
+
+

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;
 }
 

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -63,6 +63,7 @@
 #include <uORB/topics/vehicle_status_flags.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/soaring_controller_heartbeat.h>
+#include <uORB/topics/soaring_controller_position_setpoint.h>
 #include <uORB/topics/soaring_controller_status.h>
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/wind.h>
@@ -128,6 +129,7 @@ private:
 	uORB::Publication<vehicle_angular_acceleration_setpoint_s>		_angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)};
 	uORB::PublicationMulti<rate_ctrl_status_s>						_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
 	uORB::Publication<soaring_controller_heartbeat_s>				_soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)};
+	uORB::Publication<soaring_controller_position_setpoint_s>		_soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)};
 	uORB::Publication<offboard_control_mode_s>						_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<px4::params::ORIGIN_LAT>) _param_origin_lat,
 		(ParamFloat<px4::params::ORIGIN_LON>) _param_origin_lon,
-		(ParamFloat<px4::params::ORIGIN_ALT>) _param_origin_alt
+		(ParamFloat<px4::params::ORIGIN_ALT>) _param_origin_alt,
+		// loiter params
+		(ParamInt<px4::params::LOITER>) _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.

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);

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

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

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

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

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);