aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
authorThomas Gubler <thomasgubler@gmail.com>2014-09-02 15:00:26 +0200
committerThomas Gubler <thomasgubler@gmail.com>2014-09-02 15:00:26 +0200
commit18f0ee3f28763c330dfd0c0998c47c46e5edde04 (patch)
treeaba5b6aea9899928bc8f24cd80098820ea348a3c /src/modules
parentfe0642d5e992304572b6d2eae922bbed1d27406f (diff)
parent2780dc39ce5d47f2d9dfa921062100a1dc86c2be (diff)
downloadpx4-firmware-18f0ee3f28763c330dfd0c0998c47c46e5edde04.tar.gz
px4-firmware-18f0ee3f28763c330dfd0c0998c47c46e5edde04.tar.bz2
px4-firmware-18f0ee3f28763c330dfd0c0998c47c46e5edde04.zip
Merge remote-tracking branch 'upstream/master' into obcfailsafe
Conflicts: src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp12
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c13
2 files changed, 20 insertions, 5 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 0c35ccb2c..5d62c90ab 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
@@ -578,6 +578,12 @@ FixedwingPositionControl::parameters_update()
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));
+
+ /* check if negative value for 2/3 of flare altitude is set for throttle cut */
+ if (_parameters.land_thrust_lim_alt_relative < 0.0f) {
+ _parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative;
+ }
+
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
@@ -843,7 +849,7 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt,
* the measurement is valid
* the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
*/
- if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
+ if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) {
return rel_alt_estimated;
}
@@ -1419,6 +1425,10 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
pitch_min_rad = M_DEG_TO_RAD_F * -1.0f;
pitch_max_rad = M_DEG_TO_RAD_F * 5.0f;
}
+
+/* No underspeed protection in landing mode */
+ _tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM));
+
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
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 41c374407..890ab3bad 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
@@ -379,18 +379,23 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
/**
- * Landing flare altitude (relative)
+ * Landing flare altitude (relative to landing altitude)
*
+ * @unit meter
* @group L1 Control
*/
-PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
+PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f);
/**
- * Landing throttle limit altitude (relative)
+ * Landing throttle limit altitude (relative landing altitude)
*
+ * Default of -1.0f lets the system default to applying throttle
+ * limiting at 2/3 of the flare altitude.
+ *
+ * @unit meter
* @group L1 Control
*/
-PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
+PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
/**
* Landing heading hold horizontal distance