add thrust param and mulitstage t_ref computation

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
+float32[3] acc                            # ACCELERATION VECTOR IN SOARING ENU FRAME
+float32[4] att                            # UNIT QUATERNION DESCRIBING BODY FRAME POSE TO ENU

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<float, n> 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<float, n_multistage+1> 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)<min_dist_multistage){
-                min_dist_multistage = distances_multistage(i);
-                t_multistage = 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_multistage<0.f){
-                    t_multistage += 1.f;
-                }
-                else if (t_multistage>1.f){
-                    t_multistage -= 1.f;
-                }
-            }
-        }
-        // next starting point is previous closest point
-        t_ref_next = t_multistage;
+    
+    const uint n_1 = 20;
+    Vector<float, n_1> distances;
+    float t_ref;
+    // =======
+    // STAGE 1
+    // =======
+    // STAGE 1: compute all distances
+    for(uint i=0; i<n_1; i++){
+        t_ref = float(i)/float(n_1);
+        Vector3f pos_ref = _get_position_ref(t_ref);
+        distances(i) = (pos_ref - pos)*(pos_ref - pos);
     }
-    PX4_INFO("different t: \t%.3f\t%.3f\t%.3f", (double)t, (double)t_multistage, (double)(t-t_multistage));
-    */
 
-    return t;
+    // STAGE 1: get index of smallest distance
+    float t_1 = 0.f;
+    float min_dist = distances(0);
+    for(uint i=1; i<n_1; i++){
+        if(distances(i)<min_dist){
+            min_dist = distances(i);
+            t_1 = float(i);
+        }
+    }
+    t_1 = t_1/float(n_1);
+
+    // =======
+    // STAGE 2
+    // =======
+    const uint n_2 = 2*n_1-1;
+    Vector<float, n_2+1> 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)<min_dist){
+            min_dist = distances_2(i);
+            t_2 = float(i);
+        }
+    }
+    t_2 = fmod(t_lower + float(t_2)*2.f/(float(n_2)*float(n_1)),1.0f);
+
+    return t_2;
 }
 
 Quatf
@@ -970,6 +990,7 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
     acc_filtered(0) = _lp_filter_accel[0].apply(_acc(0));
     acc_filtered(1) = _lp_filter_accel[1].apply(_acc(1));
     acc_filtered(2) = _lp_filter_accel[2].apply(_acc(2));
+
     // =========================================
     // apply PD control law on the body position
     // =========================================
@@ -1021,7 +1042,6 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
         }
     }
     
-
     // =========================================
     // apply PD control law on the body attitude
     // =========================================
@@ -1044,7 +1064,7 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
         //rot_acc_command = Vector3f{0.f,0.f,-0.5f};
     }
     */
-
+    //PX4_INFO("force command: \t%.2f", (double)(f_command*f_command));
     //PX4_INFO("force command: \t%.2f\t%.2f\t%.2f", (double)f_command(0), (double)f_command(1), (double)f_command(2));
     //PX4_INFO("FRD body frame rotation vec: \t%.2f\t%.2f\t%.2f", (double)w_err(0), (double)w_err(1), (double)w_err(2));
     if (sqrtf(w_err(0)*w_err(0) + w_err(1)*w_err(1) + w_err(2)*w_err(2))>M_PI_F){

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -204,7 +204,9 @@ private:
 		(ParamFloat<px4::params::ORIGIN_LON>) _param_origin_lon,
 		(ParamFloat<px4::params::ORIGIN_ALT>) _param_origin_alt,
 		// loiter params
-		(ParamInt<px4::params::LOITER>) _param_loiter
+		(ParamInt<px4::params::LOITER>) _param_loiter,
+		// thrust params
+		(ParamFloat<px4::params::THRUST>) _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.

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

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

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

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