aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/lib/ecl/l1/ecl_l1_pos_controller.cpp13
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp40
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c7
-rw-r--r--src/modules/navigator/navigator_main.cpp24
4 files changed, 35 insertions, 49 deletions
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 3b68a0a4e..d1c864d78 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -38,6 +38,8 @@
*
*/
+#include <float.h>
+
#include "ecl_l1_pos_controller.h"
float ECL_L1_Pos_Controller::nav_roll()
@@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
/* calculate the vector from waypoint A to current position */
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
- /* store the normalized vector from waypoint A to current position */
- math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+ math::Vector<2> vector_A_to_airplane_unit;
+
+ /* prevent NaN when normalizing */
+ if (vector_A_to_airplane.length() > FLT_EPSILON) {
+ /* store the normalized vector from waypoint A to current position */
+ vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+ } else {
+ vector_A_to_airplane_unit = vector_A_to_airplane;
+ }
/* calculate eta angle towards the loiter center */
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 1feb539b9..ed6d8792c 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
@@ -233,7 +233,6 @@ private:
float speedrate_p;
float land_slope_angle;
- float land_slope_length;
float land_H1_virt;
float land_flare_alt_relative;
float land_thrust_lim_alt_relative;
@@ -278,7 +277,6 @@ private:
param_t speedrate_p;
param_t land_slope_angle;
- param_t land_slope_length;
param_t land_H1_virt;
param_t land_flare_alt_relative;
param_t land_thrust_lim_alt_relative;
@@ -427,7 +425,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_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_SLLR");
_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
@@ -516,7 +513,6 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
- param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
@@ -699,7 +695,7 @@ void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
{
- if (_global_pos_valid) {
+ if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
/* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -885,8 +881,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 1.3f * _parameters.airspeed_min;
- float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
+ /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
+ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
+
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
@@ -931,38 +929,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
//warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
flare_curve_alt_last = flare_curve_alt;
-
- } else if (wp_distance < L_wp_distance) {
-
- /* 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_pitch_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: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length);
-
- if (!land_onslope) {
-
- mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
- land_onslope = true;
- }
-
} else {
/* intersect glide slope:
- * if current position is higher or within 10m of slope follow the glide slope
- * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
- * */
+ * minimize speed to approach speed
+ * if current position is higher or within 10m of slope follow the glide slope
+ * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
+ * */
float altitude_desired = _global_pos.alt;
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: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement);
+ if (!land_onslope) {
+ mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
+ land_onslope = true;
+ }
} else {
/* continue horizontally */
altitude_desired = math::max(_global_pos.alt, L_altitude);
- //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance);
}
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach),
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 62a340e90..0909135e1 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
@@ -349,13 +349,6 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
/**
- * Landing slope length
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
-
-/**
*
*
* @group L1 Control
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 3abca99f4..5559d7b56 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -853,7 +853,7 @@ Navigator::task_main()
/* notify user about state changes */
if (myState != prevState) {
- mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]);
+ mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]);
prevState = myState;
/* reset time counter on state changes */
@@ -1061,11 +1061,11 @@ Navigator::start_loiter()
/* use current altitude if above min altitude set by parameter */
if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
_pos_sp_triplet.current.alt = min_alt_amsl;
- mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
+ mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
} else {
_pos_sp_triplet.current.alt = _global_pos.alt;
- mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
+ mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
}
}
@@ -1165,14 +1165,14 @@ Navigator::set_mission_item()
}
if (_do_takeoff) {
- mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
+ mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
} else {
if (onboard) {
- mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
+ mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index);
} else {
- mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
+ mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index);
}
}
@@ -1321,7 +1321,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
- mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt));
+ mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt));
break;
}
@@ -1347,7 +1347,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
- mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
+ mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
break;
}
@@ -1374,12 +1374,12 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
- mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
+ mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
break;
}
default: {
- mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
+ mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state);
start_loiter();
break;
}
@@ -1528,7 +1528,7 @@ Navigator::check_mission_item_reached()
_time_first_inside_orbit = now;
if (_mission_item.time_inside > 0.01f) {
- mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", (double)_mission_item.time_inside);
+ mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside);
}
}
@@ -1556,7 +1556,7 @@ Navigator::on_mission_item_reached()
if (_do_takeoff) {
/* takeoff completed */
_do_takeoff = false;
- mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed");
+ mavlink_log_info(_mavlink_fd, "#audio: takeoff completed");
} else {
/* advance by one mission item */
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-09 12:18:55 +0000