backup changes

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -333,14 +333,21 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind)
     return;
 }
 
+void
+FixedwingPositionINDIControl::_select_trajectory(float initial_energy)
+{
+    
+}
+
 void
 FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(string filename)
 {
     // File pointer
     std::ifstream fin;
   
-    // Open an existing file
+    // Open an existing file for testing
     fin.open("/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"+filename, ios::in);
+    // Open an existing file from SD card
     //fin.open("/fs/microsd/trajectories/"+filename);
   
     // Read the Data from the file
@@ -821,6 +828,49 @@ FixedwingPositionINDIControl::_get_closest_t(Vector3f pos)
     //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;
+    }
+    PX4_INFO("different t: \t%.3f\t%.3f\t%.3f", (double)t, (double)t_multistage, (double)(t-t_multistage));
+    */
+
     return t;
 }
 
@@ -931,7 +981,21 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
     omega_filtered(1) = _lp_filter_omega[1].apply(_omega(1));
     omega_filtered(2) = _lp_filter_omega[2].apply(_omega(2));
     Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-omega_filtered) + alpha_ref;
-    //rot_acc_command = Vector3f{-1.0f,0.f,0.f};
+
+    // ==========================================
+    // input meant for tuning the INDI controller
+    // ==========================================
+    /*
+    if(hrt_absolute_time()%2000000>1000000){
+        rot_acc_command = Vector3f{2.0f,1.f,0.f};
+        //rot_acc_command = Vector3f{0.f,0.f,0.5f};
+    }
+    else{
+        rot_acc_command = Vector3f{-2.0f,-1.f,0.f};
+        //rot_acc_command = Vector3f{0.f,0.f,-0.5f};
+    }
+    */
+
     //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));
 
@@ -942,10 +1006,12 @@ Vector3f
 FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
 {
     // compute the expected actuator efficiencies
-    float c_ail = _param_k_act_roll.get();
+    float k_ail = _param_k_act_roll.get();
-    float c_ele = _param_k_act_pitch.get();
+    float k_ele = _param_k_act_pitch.get();
-    float c_rud = _param_k_act_yaw.get();
+    float k_rud = _param_k_act_yaw.get();
-    float c_damping = 0.0f;
+    float k_d_roll = _param_k_damping_roll.get();
+    float k_d_pitch = _param_k_damping_pitch.get();
+    float k_d_yaw = _param_k_damping_yaw.get();
     // compute velocity in body frame
     Dcmf R_ib(_att);
     Vector3f vel_body = R_ib.transpose()*_vel;
@@ -953,11 +1019,16 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
     //Vector3f vel_body_2 = Dcmf(Quatf(_attitude.q)).transpose()*Vector3f{_local_pos.vx,_local_pos.vy,_local_pos.vz};
     //PX4_INFO("ENU body frame velocity: \t%.2f\t%.2f\t%.2f", (double)vel_body_2(0), (double)vel_body_2(1), (double)vel_body_2(2));
     //PX4_INFO("FRD body frame velocity: \t%.2f\t%.2f\t%.2f", (double)vel_body(0), (double)vel_body(1), (double)vel_body(2));
+    // filter omega at the same rate as the moments
+    Vector3f omega_filtered;
+    omega_filtered(0) = _lp_filter_omega_2[0].apply(_omega(0));
+    omega_filtered(1) = _lp_filter_omega_2[1].apply(_omega(1));
+    omega_filtered(2) = _lp_filter_omega_2[2].apply(_omega(2));
     // compute moments
     Vector3f moment;
-    moment(0) = c_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - c_damping*q*_omega(0);
+    moment(0) = k_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - k_d_roll*q*omega_filtered(0);
-    moment(1) = c_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH];
+    moment(1) = k_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH] - k_d_pitch*q*omega_filtered(1);
-    moment(2) = c_rud*q*_actuators.control[actuator_controls_s::INDEX_YAW];
+    moment(2) = k_rud*q*_actuators.control[actuator_controls_s::INDEX_YAW] - k_d_yaw*q*omega_filtered(2);
     // introduce artificial time delay that is also present in acceleration
     Vector3f moment_filtered;
     moment_filtered(0) = _lp_filter_delay[0].apply(moment(0));
@@ -969,10 +1040,9 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl)
     Vector3f moment_command = _inertia * (ctrl - alpha_filtered) + moment_filtered;
     // perform dynamic inversion
     Vector3f deflection;
-    deflection(0) = (moment_command(0)+c_damping*q*_omega(0))/(c_ail*q);
+    deflection(0) = (moment_command(0) + k_d_roll*q*omega_filtered(0))/(k_ail*q);
-    deflection(1) = moment_command(1)/(c_ele*q);
+    deflection(1) = (moment_command(1) + k_d_pitch*q*omega_filtered(1))/(k_ele*q);
-    deflection(2) = moment_command(2)/(c_rud*q);
+    deflection(2) = (moment_command(2) + k_d_yaw*q*omega_filtered(2))/(k_rud*q);
-    //PX4_INFO("filtered alpha: \t%.2f\t%.2f", (double)(_l_list(0))(1), (double)(_l_lpf_list(0))(1));
 
     return deflection;
 }

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -186,9 +186,13 @@ private:
 		(ParamFloat<px4::params::K_W_ROLL>) _param_k_w_roll,
 		(ParamFloat<px4::params::K_W_PITCH>) _param_k_w_pitch,
 		(ParamFloat<px4::params::K_W_YAW>) _param_k_w_yaw,
+		// low-level controller params
 		(ParamFloat<px4::params::K_ACT_ROLL>) _param_k_act_roll,
 		(ParamFloat<px4::params::K_ACT_PITCH>) _param_k_act_pitch,
 		(ParamFloat<px4::params::K_ACT_YAW>) _param_k_act_yaw,
+		(ParamFloat<px4::params::K_DAMPING_ROLL>) _param_k_damping_roll,
+		(ParamFloat<px4::params::K_DAMPING_PITCH>) _param_k_damping_pitch,
+		(ParamFloat<px4::params::K_DAMPING_YAW>) _param_k_damping_yaw,
 		// location params
 		(ParamFloat<px4::params::ORIGIN_LAT>) _param_origin_lat,
 		(ParamFloat<px4::params::ORIGIN_LON>) _param_origin_lon,
@@ -233,7 +237,7 @@ private:
 	const static size_t _num_basis_funs = 16;			// number of basis functions used for the trajectory approximation
 
 	// controller methods
-	
+	void _select_trajectory(float initial_energy);				// select the correct trajectory based on available energy
 	void _read_trajectory_coeffs_csv(std::string filename);				// read in the correct coefficients of the appropriate trajectory
 	void _set_wind_estimate(Vector3f wind);
 	float _get_closest_t(Vector3f pos);				// get the normalized time, at which the reference path is closest to the current position
@@ -287,13 +291,14 @@ private:
 	// controller frequency
 	const float _sample_frequency = 250.f;
 	// Low-Pass filters stage 1
-	const float _cutoff_frequency_1 = 2.f;
+	const float _cutoff_frequency_1 = 5.f;
 	math::LowPassFilter2p _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// linear acceleration
-	math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, 2}, {_sample_frequency, 2}, {_sample_frequency, 2}};	// force command
+	math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// force command
-	math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, 2}, {_sample_frequency, 2}, {_sample_frequency, 2}};	// body rates
+	math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// body rates
 	// Low-Pass filters stage 2
-	const float _cutoff_frequency_2 = 15.f;
+	const float _cutoff_frequency_2 = 5.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF"
 	math::LowPassFilter2p _lp_filter_delay[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}};	// filter to match acceleration processing delay
+	math::LowPassFilter2p _lp_filter_omega_2[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}};	// body rates
 	uint _counter = 0;
 	hrt_abstime _last_time{0};
 
@@ -334,6 +339,12 @@ private:
 	hrt_abstime _slip_last_valid{0};			///< last time Aoa was received. Used to detect timeouts.
 	float _slip{0.0f};
 
+	// vectors defining the initial velocities, wind speed and shear strength
+	Vector<float, 10> _initial_velocities_trajectory = {};	
+	Vector<float, 10> _wind_speed_trajectory = {};	
+	Vector<float, 10> _shear_param_trajectory = {};	
+
+
 	// helper variables
 	Dcmf _R_ned_to_enu;	// rotation matrix from NED to ENU frame
 	Dcmf _R_enu_to_ned;	// rotation matrix from ENU to NED frame

src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c

@@ -426,6 +426,10 @@ PARAM_DEFINE_FLOAT(K_W_PITCH, 5.0f);
  */
 PARAM_DEFINE_FLOAT(K_W_YAW, 1.0f);
 
+// =============================
+// low level INDI control params
+// =============================
+
 /**
  * roll gain of K_ACT (actuator deflection gain)
  * 
@@ -436,7 +440,7 @@ PARAM_DEFINE_FLOAT(K_W_YAW, 1.0f);
  * @increment 0.01
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.3f);
+PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.1f);
 
 /**
  * pitch gain of K_ACT (actuator deflection gain)
@@ -448,7 +452,7 @@ PARAM_DEFINE_FLOAT(K_ACT_ROLL, 0.3f);
  * @increment 0.01
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.2f);
+PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.03f);
 
 /**
  * yaw gain of K_ACT (actuator deflection gain)
@@ -460,7 +464,43 @@ PARAM_DEFINE_FLOAT(K_ACT_PITCH, 0.2f);
  * @increment 0.01
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(K_ACT_YAW, 1.f);
+PARAM_DEFINE_FLOAT(K_ACT_YAW, 0.02f);
+
+/**
+ * roll gain of K_ACT (actuator deflection gain)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(K_DAMPING_ROLL, 0.02f);
+
+/**
+ * pitch gain of K_ACT (actuator deflection gain)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(K_DAMPING_PITCH, 0.01f);
+
+/**
+ * yaw gain of K_ACT (actuator deflection gain)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(K_DAMPING_YAW, 0.0f);
 
 // ===================================================
 // ==============  trajectory center =================

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

@@ -121,7 +121,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400);
 * @reboot_required true
 * @group Sensors
 */
-PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 15.0f);
+PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 5.0f);
 
 /**
 * IMU gyro dynamic notch filtering