aboutsummaryrefslogtreecommitdiff
path: root/src/modules/fw_pos_control_l1
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/fw_pos_control_l1')
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp559
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c270
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs.cpp6
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs_params.c20
4 files changed, 668 insertions, 187 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 0b111f7bd..6017369aa 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
@@ -42,8 +42,8 @@
* Proceedings of the AIAA Guidance, Navigation and Control
* Conference, Aug 2004. AIAA-2004-4900.
*
- * Implementation for total energy control class:
- * Thomas Gubler
+ * Original implementation for total energy control class:
+ * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
*
* More details and acknowledgements in the referenced library headers.
*
@@ -87,10 +87,12 @@
#include <mavlink/mavlink_log.h>
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
-#include <drivers/drv_range_finder.h>
+#include <external_lgpl/tecs/tecs.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
+static int _control_task = -1; /**< task handle for sensor task */
+
/**
* L1 control app start / stop handling function
@@ -99,6 +101,8 @@
*/
extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
+using namespace launchdetection;
+
class FixedwingPositionControl
{
public:
@@ -115,9 +119,9 @@ public:
/**
* Start the sensors task.
*
- * @return OK on success.
+ * @return OK on success.
*/
- int start();
+ static int start();
/**
* Task status
@@ -131,19 +135,19 @@ private:
bool _task_should_exit; /**< if true, sensor task should exit */
bool _task_running; /**< if true, task is running in its mainloop */
- int _control_task; /**< task handle for sensor task */
int _global_pos_sub;
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _airspeed_sub; /**< airspeed subscription */
- int _control_mode_sub; /**< vehicle status subscription */
+ int _control_mode_sub; /**< control mode subscription */
+ int _vehicle_status_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _sensor_combined_sub; /**< for body frame accelerations */
- int _range_finder_sub; /**< range finder subscription */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
+ orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
@@ -151,25 +155,24 @@ private:
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
- struct vehicle_control_mode_s _control_mode; /**< vehicle status */
+ struct vehicle_control_mode_s _control_mode; /**< control mode */
+ struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
- struct range_finder_report _range_finder; /**< range finder report */
perf_counter_t _loop_perf; /**< loop performance counter */
/* land states */
- /* not in non-abort mode for landing yet */
bool land_noreturn_horizontal;
bool land_noreturn_vertical;
bool land_stayonground;
bool land_motor_lim;
bool land_onslope;
+ bool land_useterrain;
/* takeoff/launch states */
- bool launch_detected;
- bool usePreTakeoffThrust;
+ LaunchDetectionResult launch_detection_state;
bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
@@ -192,6 +195,7 @@ private:
math::Matrix<3, 3> _R_nb; ///< current attitude
ECL_L1_Pos_Controller _l1_control;
+ TECS _tecs;
fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode;
@@ -199,6 +203,24 @@ private:
float l1_period;
float l1_damping;
+ float time_const;
+ float time_const_throt;
+ float min_sink_rate;
+ float max_sink_rate;
+ float max_climb_rate;
+ float climbout_diff;
+ float heightrate_p;
+ float heightrate_ff;
+ float speedrate_p;
+ float throttle_damp;
+ float integrator_gain;
+ float vertical_accel_limit;
+ float height_comp_filter_omega;
+ float speed_comp_filter_omega;
+ float roll_throttle_compensation;
+ float speed_weight;
+ float pitch_damping;
+
float airspeed_min;
float airspeed_trim;
float airspeed_max;
@@ -209,6 +231,7 @@ private:
float throttle_min;
float throttle_max;
float throttle_cruise;
+ float throttle_slew_max;
float throttle_land_max;
@@ -217,7 +240,9 @@ private:
float land_flare_alt_relative;
float land_thrust_lim_alt_relative;
float land_heading_hold_horizontal_distance;
- float range_finder_rel_alt;
+ float land_flare_pitch_min_deg;
+ float land_flare_pitch_max_deg;
+ int land_use_terrain_estimate;
} _parameters; /**< local copies of interesting parameters */
@@ -226,6 +251,24 @@ private:
param_t l1_period;
param_t l1_damping;
+ param_t time_const;
+ param_t time_const_throt;
+ param_t min_sink_rate;
+ param_t max_sink_rate;
+ param_t max_climb_rate;
+ param_t climbout_diff;
+ param_t heightrate_p;
+ param_t heightrate_ff;
+ param_t speedrate_p;
+ param_t throttle_damp;
+ param_t integrator_gain;
+ param_t vertical_accel_limit;
+ param_t height_comp_filter_omega;
+ param_t speed_comp_filter_omega;
+ param_t roll_throttle_compensation;
+ param_t speed_weight;
+ param_t pitch_damping;
+
param_t airspeed_min;
param_t airspeed_trim;
param_t airspeed_max;
@@ -236,6 +279,7 @@ private:
param_t throttle_min;
param_t throttle_max;
param_t throttle_cruise;
+ param_t throttle_slew_max;
param_t throttle_land_max;
@@ -244,7 +288,9 @@ private:
param_t land_flare_alt_relative;
param_t land_thrust_lim_alt_relative;
param_t land_heading_hold_horizontal_distance;
- param_t range_finder_rel_alt;
+ param_t land_flare_pitch_min_deg;
+ param_t land_flare_pitch_max_deg;
+ param_t land_use_terrain_estimate;
} _parameter_handles; /**< handles for interesting parameters */
@@ -261,20 +307,19 @@ private:
void control_update();
/**
- * Check for changes in vehicle status.
+ * Check for changes in control mode
*/
void vehicle_control_mode_poll();
/**
- * Check for airspeed updates.
+ * Check for changes in vehicle status.
*/
- bool vehicle_airspeed_poll();
+ void vehicle_status_poll();
/**
- * Check for range finder updates.
+ * Check for airspeed updates.
*/
- bool range_finder_poll();
-
+ bool vehicle_airspeed_poll();
/**
* Check for position updates.
@@ -297,9 +342,9 @@ private:
void navigation_capabilities_publish();
/**
- * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
+ * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
*/
- float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
+ float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
/**
* Control position.
@@ -340,7 +385,8 @@ private:
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
- tecs_mode mode = TECS_MODE_NORMAL);
+ tecs_mode mode = TECS_MODE_NORMAL,
+ bool pitch_max_special = false);
};
@@ -361,7 +407,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
_task_should_exit(false),
_task_running(false),
- _control_task(-1),
/* subscriptions */
_global_pos_sub(-1),
@@ -369,13 +414,14 @@ FixedwingPositionControl::FixedwingPositionControl() :
_att_sub(-1),
_airspeed_sub(-1),
_control_mode_sub(-1),
+ _vehicle_status_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_sensor_combined_sub(-1),
- _range_finder_sub(-1),
/* publications */
_attitude_sp_pub(-1),
+ _tecs_status_pub(-1),
_nav_capabilities_pub(-1),
/* states */
@@ -385,10 +431,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_manual(),
_airspeed(),
_control_mode(),
+ _vehicle_status(),
_global_pos(),
_pos_sp_triplet(),
_sensor_combined(),
- _range_finder(),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
@@ -398,8 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_stayonground(false),
land_motor_lim(false),
land_onslope(false),
- launch_detected(false),
- usePreTakeoffThrust(false),
+ land_useterrain(false),
+ launch_detection_state(LAUNCHDETECTION_RES_NONE),
last_manual(false),
landingslope(),
flare_curve_alt_rel_last(0.0f),
@@ -428,6 +474,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.roll_limit = param_find("FW_R_LIM");
_parameter_handles.throttle_min = param_find("FW_THR_MIN");
_parameter_handles.throttle_max = param_find("FW_THR_MAX");
+ _parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX");
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
@@ -436,7 +483,27 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
- _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
+ _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN");
+ _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX");
+ _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER");
+
+ _parameter_handles.time_const = param_find("FW_T_TIME_CONST");
+ _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST");
+ _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
+ _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX");
+ _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX");
+ _parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF");
+ _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP");
+ _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN");
+ _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC");
+ _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA");
+ _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA");
+ _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR");
+ _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
+ _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
+ _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
+ _parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF");
+ _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
/* fetch initial parameter values */
parameters_update();
@@ -485,21 +552,68 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
+ param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max));
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
+ param_get(_parameter_handles.time_const, &(_parameters.time_const));
+ param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt));
+ param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
+ param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
+ param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
+ param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain));
+ param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit));
+ param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega));
+ param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega));
+ param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation));
+ param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
+ param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
+ param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
+ param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
+
+ param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
+ param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff));
+ 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_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));
- 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));
+ /* 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.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg));
+ param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
+ param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));
_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));
+ _tecs.set_time_const(_parameters.time_const);
+ _tecs.set_time_const_throt(_parameters.time_const_throt);
+ _tecs.set_min_sink_rate(_parameters.min_sink_rate);
+ _tecs.set_max_sink_rate(_parameters.max_sink_rate);
+ _tecs.set_throttle_damp(_parameters.throttle_damp);
+ _tecs.set_throttle_slewrate(_parameters.throttle_slew_max);
+ _tecs.set_integrator_gain(_parameters.integrator_gain);
+ _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
+ _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
+ _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
+ _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
+ _tecs.set_speed_weight(_parameters.speed_weight);
+ _tecs.set_pitch_damping(_parameters.pitch_damping);
+ _tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
+ _tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
+ _tecs.set_max_climb_rate(_parameters.max_climb_rate);
+ _tecs.set_heightrate_p(_parameters.heightrate_p);
+ _tecs.set_heightrate_ff(_parameters.heightrate_ff);
+ _tecs.set_speedrate_p(_parameters.speedrate_p);
+
/* sanity check parameters */
if (_parameters.airspeed_max < _parameters.airspeed_min ||
_parameters.airspeed_max < 5.0f ||
@@ -531,16 +645,27 @@ FixedwingPositionControl::parameters_update()
void
FixedwingPositionControl::vehicle_control_mode_poll()
{
- bool vstatus_updated;
+ bool updated;
- /* Check HIL state if vehicle status has changed */
- orb_check(_control_mode_sub, &vstatus_updated);
+ orb_check(_control_mode_sub, &updated);
- if (vstatus_updated) {
+ if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
}
+void
+FixedwingPositionControl::vehicle_status_poll()
+{
+ bool updated;
+
+ orb_check(_vehicle_status_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+ }
+}
+
bool
FixedwingPositionControl::vehicle_airspeed_poll()
{
@@ -561,21 +686,10 @@ FixedwingPositionControl::vehicle_airspeed_poll()
}
}
- return airspeed_updated;
-}
+ /* update TECS state */
+ _tecs.enable_airspeed(_airspeed_valid);
-bool
-FixedwingPositionControl::range_finder_poll()
-{
- /* check if there is a range finder measurement */
- bool range_finder_updated;
- orb_check(_range_finder_sub, &range_finder_updated);
-
- if (range_finder_updated) {
- orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
- }
-
- return range_finder_updated;
+ return airspeed_updated;
}
void
@@ -621,7 +735,17 @@ FixedwingPositionControl::vehicle_setpoint_poll()
void
FixedwingPositionControl::task_main_trampoline(int argc, char *argv[])
{
+ l1_control::g_control = new FixedwingPositionControl();
+
+ if (l1_control::g_control == nullptr) {
+ warnx("OUT OF MEM");
+ return;
+ }
+
+ /* only returns on exit */
l1_control::g_control->task_main();
+ delete l1_control::g_control;
+ l1_control::g_control = nullptr;
}
float
@@ -705,21 +829,23 @@ void FixedwingPositionControl::navigation_capabilities_publish()
}
}
-float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
+float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
{
- float rel_alt_estimated = current_alt - land_setpoint_alt;
-
- /* only use range finder if:
- * parameter (range_finder_use_relative_alt) > 0
- * 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 ) {
- return rel_alt_estimated;
+ if (!isfinite(global_pos.terrain_alt)) {
+ return land_setpoint_alt;
}
- return range_finder.distance;
-
+ /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it
+ * for the whole landing */
+ if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) {
+ if(!land_useterrain) {
+ mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate");
+ land_useterrain = true;
+ }
+ return global_pos.terrain_alt;
+ } else {
+ return land_setpoint_alt;
+ }
}
bool
@@ -733,6 +859,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
+ /* filter speed and altitude for controller */
+ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
+ math::Vector<3> accel_earth = _R_nb * accel_body;
+
+ if (!_mTecs.getEnabled()) {
+ _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
+ }
+
/* define altitude error */
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
@@ -758,6 +892,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
+ /* restore speed weight, in case changed intermittently (e.g. in landing handling) */
+ _tecs.set_speed_weight(_parameters.speed_weight);
+
/* current waypoint (the one currently heading for) */
math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
@@ -813,10 +950,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
+ float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+ /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
+ float wp_distance_save = wp_distance;
+ if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) {
+ wp_distance_save = 0.0f;
+ }
+
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
@@ -852,29 +996,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Vertical landing control */
//xxx: using the tecs altitude controller for slope control for now
-
-// /* do not go down too early */
-// if (wp_distance > 50.0f) {
-// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
-// }
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// XXX this could make a great param
- float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 1.3f * _parameters.airspeed_min;
+ /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be
+ * equal to _pos_sp_triplet.current.alt */
+ float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos);
+
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
- float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
+ float L_altitude_rel = _pos_sp_triplet.previous.valid ?
+ _pos_sp_triplet.previous.alt - terrain_alt : 0.0f;
- 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_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
- float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
-
- if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
-
+ /* Check if we should start flaring with a vertical and a
+ * horizontal limit (with some tolerance)
+ * The horizontal limit is only applied when we are in front of the wp
+ */
+ if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) &&
+ (wp_distance_save < landingslope.flare_length() + 5.0f)) ||
+ land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
@@ -883,7 +1028,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* kill the throttle if param requests it */
throttle_max = _parameters.throttle_max;
- if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
+ if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
if (!land_motor_lim) {
land_motor_lim = true;
@@ -901,12 +1046,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
land_stayonground = true;
}
- tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
+ tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land), eas2tas,
- flare_pitch_angle_rad, math::radians(15.0f),
+ math::radians(_parameters.land_flare_pitch_min_deg),
+ math::radians(_parameters.land_flare_pitch_max_deg),
0.0f, throttle_max, throttle_land,
- false, flare_pitch_angle_rad,
- _pos_sp_triplet.current.alt + relative_alt, ground_speed,
+ false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min),
+ _global_pos.alt, ground_speed,
land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
if (!land_noreturn_vertical) {
@@ -927,8 +1073,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
* if current position is below the slope continue at previous wp altitude
* until the intersection with slope
* */
- float altitude_desired_rel = relative_alt;
- if (relative_alt > landing_slope_alt_rel_desired || land_onslope) {
+ float altitude_desired_rel;
+ if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) {
/* stay on slope */
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
@@ -937,10 +1083,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
} else {
/* continue horizontally */
- altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
+ altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel :
+ _global_pos.alt - terrain_alt;
}
- tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel,
+ tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
calculate_target_airspeed(airspeed_approach), eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
@@ -949,67 +1096,74 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
- _pos_sp_triplet.current.alt + relative_alt,
+ _global_pos.alt,
ground_speed);
}
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
- if(!launch_detected) { //do not do further checks once a launch was detected
- if (launchDetector.launchDetectionEnabled()) {
- static hrt_abstime last_sent = 0;
- if(hrt_absolute_time() - last_sent > 4e6) {
- mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
- last_sent = hrt_absolute_time();
- }
-
- /* Tell the attitude controller to stop integrating while we are waiting
- * for the launch */
- _att_sp.roll_reset_integral = true;
- _att_sp.pitch_reset_integral = true;
- _att_sp.yaw_reset_integral = true;
-
- /* Detect launch */
- launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
- if (launchDetector.getLaunchDetected()) {
- launch_detected = true;
- mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
- }
- } else {
- /* no takeoff detection --> fly */
- launch_detected = true;
- warnx("launchdetection off");
+ if (launchDetector.launchDetectionEnabled() &&
+ launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+ /* Inform user that launchdetection is running */
+ static hrt_abstime last_sent = 0;
+ if(hrt_absolute_time() - last_sent > 4e6) {
+ mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
+ last_sent = hrt_absolute_time();
}
+
+ /* Detect launch */
+ launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+
+ /* update our copy of the laucn detection state */
+ launch_detection_state = launchDetector.getLaunchDetected();
+ } else {
+ /* no takeoff detection --> fly */
+ launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
}
- _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
- _att_sp.roll_body = _l1_control.nav_roll();
- _att_sp.yaw_body = _l1_control.nav_bearing();
+ /* Set control values depending on the detection state */
+ if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
+ /* Launch has been detected, hence we have to control the plane. */
+
+ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+ _att_sp.roll_body = _l1_control.nav_roll();
+ _att_sp.yaw_body = _l1_control.nav_bearing();
+
+ /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
+ * full throttle, otherwise we use the preTakeOff Throttle */
+ float takeoff_throttle = launch_detection_state !=
+ LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
+ launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
- if (launch_detected) {
- usePreTakeoffThrust = false;
+ /* select maximum pitch: the launchdetector may impose another limit for the pitch
+ * depending on the state of the launch */
+ float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
+ float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
- /* apply minimum pitch and limit roll if target altitude is not within 10 meters */
- if (altitude_error > 15.0f) {
+ /* apply minimum pitch and limit roll if target altitude is not within climbout_diff
+ * meters */
+ if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
eas2tas,
math::radians(_parameters.pitch_limit_min),
- math::radians(_parameters.pitch_limit_max),
- _parameters.throttle_min, _parameters.throttle_max,
+ takeoff_pitch_max_rad,
+ _parameters.throttle_min, takeoff_throttle,
_parameters.throttle_cruise,
true,
math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f)),
_global_pos.alt,
ground_speed,
- TECS_MODE_TAKEOFF);
+ TECS_MODE_TAKEOFF,
+ takeoff_pitch_max_deg != _parameters.pitch_limit_max);
/* limit roll motion to ensure enough lift */
- _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
+ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
+ math::radians(15.0f));
} else {
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
@@ -1018,17 +1172,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
- _parameters.throttle_max,
+ takeoff_throttle,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
}
-
} else {
- usePreTakeoffThrust = true;
+ /* Tell the attitude controller to stop integrating while we are waiting
+ * for the launch */
+ _att_sp.roll_reset_integral = true;
+ _att_sp.pitch_reset_integral = true;
+ _att_sp.yaw_reset_integral = true;
+
+ /* Set default roll and pitch setpoints during detection phase */
+ _att_sp.roll_body = 0.0f;
+ _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
+ math::radians(10.0f));
}
+
}
/* reset landing state */
@@ -1062,13 +1225,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
- if (usePreTakeoffThrust) {
+ if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
+ /* Set thrust to 0 to minimize damage */
+ _att_sp.thrust = 0.0f;
+ } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+ launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+ /* making sure again that the correct thrust is used,
+ * without depending on library calls for safety reasons */
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
+ } else {
+ /* Copy thrust and pitch values from tecs */
+ _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
+ _tecs.get_throttle_demand(), throttle_max);
}
- else {
- _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
+
+ /* During a takeoff waypoint while waiting for launch the pitch sp is set
+ * already (not by tecs) */
+ if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+ launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
+ _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
}
- _att_sp.pitch_body = _mTecs.getPitchSetpoint();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@@ -1084,10 +1260,6 @@ void
FixedwingPositionControl::task_main()
{
- /* inform about start */
- warnx("Initializing..");
- fflush(stdout);
-
/*
* do subscriptions
*/
@@ -1096,13 +1268,15 @@ FixedwingPositionControl::task_main()
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
- _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
- /* rate limit vehicle status updates to 5Hz */
+ /* rate limit control mode updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
+ /* rate limit vehicle status updates to 5Hz */
+ orb_set_interval(_vehicle_status_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
@@ -1141,9 +1315,12 @@ FixedwingPositionControl::task_main()
perf_begin(_loop_perf);
- /* check vehicle status for changes to publication state */
+ /* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
+ /* check vehicle status for changes to publication state */
+ vehicle_status_poll();
+
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
@@ -1173,7 +1350,6 @@ FixedwingPositionControl::task_main()
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
- range_finder_poll();
// vehicle_baro_poll();
math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
@@ -1226,8 +1402,7 @@ FixedwingPositionControl::task_main()
void FixedwingPositionControl::reset_takeoff_state()
{
- launch_detected = false;
- usePreTakeoffThrust = false;
+ launch_detection_state = LAUNCHDETECTION_RES_NONE;
launchDetector.reset();
}
@@ -1238,6 +1413,7 @@ void FixedwingPositionControl::reset_landing_state()
land_stayonground = false;
land_motor_lim = false;
land_onslope = false;
+ land_useterrain = false;
}
void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
@@ -1246,22 +1422,98 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
- tecs_mode mode)
+ tecs_mode mode, bool pitch_max_special)
{
- /* Using mtecs library: prepare arguments for mtecs call */
- float flightPathAngle = 0.0f;
- float ground_speed_length = ground_speed.length();
- if (ground_speed_length > FLT_EPSILON) {
- flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
- }
- fwPosctrl::LimitOverride limitOverride;
- if (climbout_mode) {
- limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+ if (_mTecs.getEnabled()) {
+ /* Using mtecs library: prepare arguments for mtecs call */
+ float flightPathAngle = 0.0f;
+ float ground_speed_length = ground_speed.length();
+ if (ground_speed_length > FLT_EPSILON) {
+ flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
+ }
+ fwPosctrl::LimitOverride limitOverride;
+ if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
+ /* Force the slow downwards spiral */
+ limitOverride.enablePitchMinOverride(-1.0f);
+ limitOverride.enablePitchMaxOverride(5.0f);
+
+ } else if (climbout_mode) {
+ limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+ } else {
+ limitOverride.disablePitchMinOverride();
+ }
+
+ if (pitch_max_special) {
+ /* Use the maximum pitch from the argument */
+ limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad);
+ } else {
+ /* use pitch max set by MT param */
+ limitOverride.disablePitchMaxOverride();
+ }
+ _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
+ limitOverride);
} else {
- limitOverride.disablePitchMinOverride();
+ if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
+ /* Force the slow downwards spiral */
+ 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,
+ climbout_mode, climbout_pitch_min_rad,
+ throttle_min, throttle_max, throttle_cruise,
+ pitch_min_rad, pitch_max_rad);
+
+ struct TECS::tecs_state s;
+ _tecs.get_tecs_state(s);
+
+ struct tecs_status_s t;
+
+ t.timestamp = s.timestamp;
+
+ switch (s.mode) {
+ case TECS::ECL_TECS_MODE_NORMAL:
+ t.mode = TECS_MODE_NORMAL;
+ break;
+ case TECS::ECL_TECS_MODE_UNDERSPEED:
+ t.mode = TECS_MODE_UNDERSPEED;
+ break;
+ case TECS::ECL_TECS_MODE_BAD_DESCENT:
+ t.mode = TECS_MODE_BAD_DESCENT;
+ break;
+ case TECS::ECL_TECS_MODE_CLIMBOUT:
+ t.mode = TECS_MODE_CLIMBOUT;
+ break;
+ }
+
+ t.altitudeSp = s.hgt_dem;
+ t.altitude_filtered = s.hgt;
+ t.airspeedSp = s.spd_dem;
+ t.airspeed_filtered = s.spd;
+
+ t.flightPathAngleSp = s.dhgt_dem;
+ t.flightPathAngle = s.dhgt;
+ t.flightPathAngleFiltered = s.dhgt;
+
+ t.airspeedDerivativeSp = s.dspd_dem;
+ t.airspeedDerivative = s.dspd;
+
+ t.totalEnergyRateSp = s.thr;
+ t.totalEnergyRate = s.ithr;
+ t.energyDistributionRateSp = s.ptch;
+ t.energyDistributionRate = s.iptch;
+
+ if (_tecs_status_pub > 0) {
+ orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
+ } else {
+ _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
+ }
}
- _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
- limitOverride);
}
int
@@ -1295,14 +1547,7 @@ int fw_pos_control_l1_main(int argc, char *argv[])
if (l1_control::g_control != nullptr)
errx(1, "already running");
- l1_control::g_control = new FixedwingPositionControl;
-
- if (l1_control::g_control == nullptr)
- errx(1, "alloc failed");
-
- if (OK != l1_control::g_control->start()) {
- delete l1_control::g_control;
- l1_control::g_control = nullptr;
+ if (OK != FixedwingPositionControl::start()) {
err(1, "start failed");
}
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 27039ff51..c00d82232 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
@@ -83,6 +83,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
/**
+ * Throttle max slew rate
+ *
+ * Maximum slew rate for the commanded throttle
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
+
+/**
* Negative pitch limit
*
* The minimum negative pitch the controller will output.
@@ -120,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
/**
* Throttle limit max
*
- * This is the maximum throttle % that can be used by the controller.
- * For overpowered aircraft, this should be reduced to a value that
+ * This is the maximum throttle % that can be used by the controller.
+ * For overpowered aircraft, this should be reduced to a value that
* provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
*
* @group L1 Control
@@ -131,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
/**
* Throttle limit min
*
- * This is the minimum throttle % that can be used by the controller.
- * For electric aircraft this will normally be set to zero, but can be set
- * to a small non-zero value if a folding prop is fitted to prevent the
- * prop from folding and unfolding repeatedly in-flight or to provide
+ * This is the minimum throttle % that can be used by the controller.
+ * For electric aircraft this will normally be set to zero, but can be set
+ * to a small non-zero value if a folding prop is fitted to prevent the
+ * prop from folding and unfolding repeatedly in-flight or to provide
* some aerodynamic drag from a turning prop to improve the descent rate.
*
* For aircraft with internal combustion engine this parameter should be set
@@ -147,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
/**
* Throttle limit value before flare
*
- * This throttle value will be set as throttle limit at FW_LND_TLALT,
+ * This throttle value will be set as throttle limit at FW_LND_TLALT,
* before arcraft will flare.
*
* @group L1 Control
@@ -155,6 +166,212 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
/**
+ * Climbout Altitude difference
+ *
+ * If the altitude error exceeds this parameter, the system will climb out
+ * with maximum throttle and minimum airspeed until it is closer than this
+ * distance to the desired altitude. Mostly used for takeoff waypoints / modes.
+ * Set to zero to disable climbout mode (not recommended).
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
+
+/**
+ * Maximum climb rate
+ *
+ * This is the best climb rate that the aircraft can achieve with
+ * the throttle set to THR_MAX and the airspeed set to the
+ * default value. For electric aircraft make sure this number can be
+ * achieved towards the end of flight when the battery voltage has reduced.
+ * The setting of this parameter can be checked by commanding a positive
+ * altitude change of 100m in loiter, RTL or guided mode. If the throttle
+ * required to climb is close to THR_MAX and the aircraft is maintaining
+ * airspeed, then this parameter is set correctly. If the airspeed starts
+ * to reduce, then the parameter is set to high, and if the throttle
+ * demand required to climb and maintain speed is noticeably less than
+ * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
+ * FW_THR_MAX reduced.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
+
+/**
+ * Minimum descent rate
+ *
+ * This is the sink rate of the aircraft with the throttle
+ * set to THR_MIN and flown at the same airspeed as used
+ * to measure FW_T_CLMB_MAX.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
+
+/**
+ * Maximum descent rate
+ *
+ * This sets the maximum descent rate that the controller will use.
+ * If this value is too large, the aircraft can over-speed on descent.
+ * This should be set to a value that can be achieved without
+ * exceeding the lower pitch angle limit and without over-speeding
+ * the aircraft.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
+
+/**
+ * TECS time constant
+ *
+ * This is the time constant of the TECS control algorithm (in seconds).
+ * Smaller values make it faster to respond, larger values make it slower
+ * to respond.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
+
+/**
+ * TECS Throttle time constant
+ *
+ * This is the time constant of the TECS throttle control algorithm (in seconds).
+ * Smaller values make it faster to respond, larger values make it slower
+ * to respond.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
+
+/**
+ * Throttle damping factor
+ *
+ * This is the damping gain for the throttle demand loop.
+ * Increase to add damping to correct for oscillations in speed and height.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
+
+/**
+ * Integrator gain
+ *
+ * This is the integrator gain on the control loop.
+ * Increasing this gain increases the speed at which speed
+ * and height offsets are trimmed out, but reduces damping and
+ * increases overshoot.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
+
+/**
+ * Maximum vertical acceleration
+ *
+ * This is the maximum vertical acceleration (in metres/second square)
+ * either up or down that the controller will use to correct speed
+ * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
+ * allows for reasonably aggressive pitch changes if required to recover
+ * from under-speed conditions.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
+
+/**
+ * Complementary filter "omega" parameter for height
+ *
+ * This is the cross-over frequency (in radians/second) of the complementary
+ * filter used to fuse vertical acceleration and barometric height to obtain
+ * an estimate of height rate and height. Increasing this frequency weights
+ * the solution more towards use of the barometer, whilst reducing it weights
+ * the solution more towards use of the accelerometer data.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
+
+/**
+ * Complementary filter "omega" parameter for speed
+ *
+ * This is the cross-over frequency (in radians/second) of the complementary
+ * filter used to fuse longitudinal acceleration and airspeed to obtain an
+ * improved airspeed estimate. Increasing this frequency weights the solution
+ * more towards use of the arispeed sensor, whilst reducing it weights the
+ * solution more towards use of the accelerometer data.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
+
+/**
+ * Roll -> Throttle feedforward
+ *
+ * Increasing this gain turn increases the amount of throttle that will
+ * be used to compensate for the additional drag created by turning.
+ * Ideally this should be set to approximately 10 x the extra sink rate
+ * in m/s created by a 45 degree bank turn. Increase this gain if
+ * the aircraft initially loses energy in turns and reduce if the
+ * aircraft initially gains energy in turns. Efficient high aspect-ratio
+ * aircraft (eg powered sailplanes) can use a lower value, whereas
+ * inefficient low aspect-ratio models (eg delta wings) can use a higher value.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
+
+/**
+ * Speed <--> Altitude priority
+ *
+ * This parameter adjusts the amount of weighting that the pitch control
+ * applies to speed vs height errors. Setting it to 0.0 will cause the
+ * pitch control to control height and ignore speed errors. This will
+ * normally improve height accuracy but give larger airspeed errors.
+ * Setting it to 2.0 will cause the pitch control loop to control speed
+ * and ignore height errors. This will normally reduce airspeed errors,
+ * but give larger height errors. The default value of 1.0 allows the pitch
+ * control to simultaneously control height and speed.
+ * Note to Glider Pilots - set this parameter to 2.0 (The glider will
+ * adjust its pitch angle to maintain airspeed, ignoring changes in height).
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
+
+/**
+ * Pitch damping factor
+ *
+ * This is the damping gain for the pitch demand loop. Increase to add
+ * damping to correct for oscillations in height. The default value of 0.0
+ * will work well provided the pitch to servo controller has been tuned
+ * properly.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
+
+/**
+ * Height rate P factor
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
+
+/**
+ * Height rate FF factor
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
+
+/**
+ * Speed rate P factor
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
+
+/**
* Landing slope angle
*
* @group L1 Control
@@ -169,18 +386,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
@@ -190,12 +412,28 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
/**
- * Relative altitude threshold for range finder measurements for use during landing
+ * Enable or disable usage of terrain estimate during landing
*
- * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
- * set to < 0 to disable
- * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
+ * 0: disabled, 1: enabled
*
* @group L1 Control
*/
-PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
+PARAM_DEFINE_INT32(FW_LND_USETER, 0);
+
+/**
+ * Flare, minimum pitch
+ *
+ * Minimum pitch during flare, a positive sign means nose up
+ * Applied once FW_LND_TLALT is reached
+ *
+ */
+PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
+
+/**
+ * Flare, maximum pitch
+ *
+ * Maximum pitch during flare, a positive sign means nose up
+ * Applied once FW_LND_TLALT is reached
+ *
+ */
+PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f);
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 749f57a2b..bffeefc1f 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
- _status.altitude = altitude;
- _status.altitudeFiltered = altitudeFiltered;
+ _status.altitude_filtered = altitudeFiltered;
/* use flightpath angle setpoint for total energy control */
@@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
/* Write part of the status message */
_status.airspeedSp = airspeedSp;
- _status.airspeed = airspeed;
- _status.airspeedFiltered = airspeedFiltered;
+ _status.airspeed_filtered = airspeedFiltered;
/* use longitudinal acceleration setpoint for total energy control */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index 4ca31fe20..58a1e9f6b 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -55,7 +55,7 @@
* @max 1
* @group mTECS
*/
-PARAM_DEFINE_INT32(MT_ENABLED, 1);
+PARAM_DEFINE_INT32(MT_ENABLED, 0);
/**
* Total Energy Rate Control Feedforward
@@ -241,7 +241,14 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
*
* @group mTECS
*/
-PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
+PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f);
+
+/**
+ * Airspeed derivative calculation lowpass
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f);
/**
* P gain for the airspeed control
@@ -268,7 +275,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
*
* @group mTECS
*/
-PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
+PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f);
/**
* Minimal acceleration (air)
@@ -287,13 +294,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
/**
- * Airspeed derivative calculation lowpass
- *
- * @group mTECS
- */
-PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
-
-/**
* Minimal throttle during takeoff
*
* @min 0.0f
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-02 08:25:35 +0000