autoland slope parameters, variable rename for bugfix and code clarity, printfs

2 files changed, 33 insertions, 11 deletions

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index a90899528..a4b277888 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -206,6 +206,9 @@ private:
 		float throttle_land_max;
 
 		float loiter_hold_radius;
+
+		float land_slope_angle;
+		float land_slope_length;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -240,6 +243,9 @@ private:
 		param_t throttle_land_max;
 
 		param_t loiter_hold_radius;
+
+		param_t land_slope_angle;
+		param_t land_slope_length;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -363,6 +369,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
+	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
+	_parameter_handles.land_slope_length = param_find("FW_LND_SLL");
+
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
 	_parameter_handles.max_sink_rate =			param_find("FW_T_SINK_MAX");
@@ -440,6 +449,11 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
 	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
 
+
+	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
+	param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
+
+
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
@@ -673,7 +687,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		if (_setpoint_valid) {
 
 			/* current waypoint (the one currently heading for) */
-			math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
+			math::Vector2f curr_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
 
 			/* previous waypoint */
 			math::Vector2f prev_wp;
@@ -711,7 +725,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			} else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
 				/* waypoint is a plain navigation waypoint */
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
 
@@ -726,7 +740,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				   global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
 
 				/* waypoint is a loiter waypoint */
-				_l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius,
+				_l1_control.navigate_loiter(curr_wp, current_position, global_triplet.current.loiter_radius,
 							  global_triplet.current.loiter_direction, ground_speed);
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
@@ -741,7 +755,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 				/* Horizontal landing control */
 				/* switch to heading hold for the last meters, continue heading hold after */
-				float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
+				float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY());
 				//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
 				if (wp_distance < 15.0f || land_noreturn) {
 
@@ -766,7 +780,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				} else {
 
 					/* normal navigation */
-					_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+					_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
 				}
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
@@ -787,9 +801,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				float airspeed_land = _parameters.airspeed_min;
 				float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
 
-				const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param
-				const float landingslope_length = 64.0f;
-				const float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length;
+				float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle);
+				float landingslope_length = _parameters.land_slope_length;
+				float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length;
 				float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad);
 				float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad);
 
@@ -812,15 +826,17 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 					/* limit roll motion to prevent wings from touching the ground first */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
 
+					warnx("Landing:  land with minimal speed");
+
 				} else if (wp_distance < L_wp_distance) {
 
-					/* minimize speed to approach speed, stay on glide slope */
+					/* minimize speed to approach speed, stay on landing slope */
 					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
 								    false, flare_angle_rad,
 								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
-
+					warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance);
 				} else {
 
 					/* normal cruise speed
@@ -832,12 +848,15 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 					if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
 						/* stay on slope */
 						altitude_desired = landing_slope_alt_desired;
+						warnx("Landing: before L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance);
 					} else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) {
 						/* continue horizontally */
 						altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection
+						warnx("Landing: before L,continue horizontal at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude);
 					} else {
 						/* climb to L_altitude */
 						altitude_desired = L_altitude;
+						warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance);
 					}
 
 					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(_parameters.airspeed_trim),
@@ -849,7 +868,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			} else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
 
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 3bb872405..a3140fe48 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -111,3 +111,6 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 
 
 PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
+
+PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
+PARAM_DEFINE_FLOAT(FW_LND_SLL, 64.0f);