From 3d9dd86900fcdc146b501efda2d2bcb0d51b3621 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 May 2014 00:12:59 +0200 Subject: [PATCH 01/16] mavlink mission item takeoff: read correct param for minimal pitch --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 28dd97fca1..bb1ad86ef6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - + Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { @@ -886,7 +886,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; + mission_item->pitch_min = mavlink_mission_item->param1; break; default: From 36c938a187e63fa5ec6d72b9ac5628334b23b837 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 May 2014 00:14:28 +0200 Subject: [PATCH 02/16] mtecs: support overriding limit parameters --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 55 ++++++++++++--- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 67 ++++++++++++++++++- .../fw_pos_control_l1/mtecs/mTecs_blocks.h | 3 +- 3 files changed, 113 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 087e1b4765..d370bf9063 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -79,7 +79,8 @@ mTecs::~mTecs() { } -int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode) +int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(altitude) || @@ -105,10 +106,12 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* use flightpath angle setpoint for total energy control */ - return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode); + return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, + airspeedSp, mode, limitOverride); } -int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode) +int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || @@ -135,10 +138,12 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* use longitudinal acceleration setpoint for total energy control */ - return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode); + return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, + accelerationLongitudinalSp, mode, limitOverride); } -int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode) +int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, + float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || @@ -180,8 +185,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } - /* Check airspeed: if below safe value switch to underspeed mode */ - if (airspeed < _airspeedMin.get()) { + /* Check airspeed: if below safe value switch to underspeed mode (if not in takeoff mode) */ + if (!TECS_MODE_LAND && airspeed < _airspeedMin.get()) { mode = TECS_MODE_UNDERSPEED; } @@ -202,6 +207,16 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; } + /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by + * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector + * is running) */ + bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ? + _controlTotalEnergy.getOutputLimiter() : + *outputLimiterThrottle, + outputLimiterPitch == NULL ? + _controlEnergyDistribution.getOutputLimiter() : + *outputLimiterPitch); + /* Write part of the status message */ _status.airspeedDerivativeSp = airspeedDerivativeSp; _status.airspeedDerivative = airspeedDerivative; @@ -280,5 +295,29 @@ void mTecs::debug(const char *fmt, ...) { debug_print(fmt, args); } -} /* namespace fwPosctrl */ +bool mTecs::LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, + BlockOutputLimiter &outputLimiterPitch) +{ + bool ret = false; + if (overrideThrottleMinEnabled) { + outputLimiterThrottle.setMin(overrideThrottleMin); + ret = true; + } + if (overrideThrottleMaxEnabled) { + outputLimiterThrottle.setMax(overrideThrottleMax); + ret = true; + } + if (overridePitchMinEnabled) { + outputLimiterPitch.setMin(overridePitchMin); + ret = true; + } + if (overridePitchMaxEnabled) { + outputLimiterPitch.setMax(overridePitchMax); + ret = true; + } + + return ret; +} + +} /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 376d396986..1a787df728 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -68,20 +68,81 @@ public: TECS_MODE_LAND_THROTTLELIM } tecs_mode; + + /* A small class which provides helper fucntions to override control output limits which are usually set by + * parameters in special cases + */ + class LimitOverride + { + public: + LimitOverride() : + overrideThrottleMinEnabled(false), + overrideThrottleMaxEnabled(false), + overridePitchMinEnabled(false), + overridePitchMaxEnabled(false) + {}; + + ~LimitOverride() {}; + + /* + * Override the limits of the outputlimiter instances given by the arguments with the limits saved in + * this class (if enabled) + * @return true if the limit was applied + */ + bool applyOverride(BlockOutputLimiter &outputLimiterThrottle, + BlockOutputLimiter &outputLimiterPitch); + + /* Functions to enable or disable the override */ + void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled, + &overrideThrottleMin, value); } + void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); } + void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled, + &overrideThrottleMax, value); } + void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); } + void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled, + &overridePitchMin, value); } + void disablePitchMinOverride() { disable(&overridePitchMinEnabled); } + void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled, + &overridePitchMax, value); } + void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); } + + protected: + bool overrideThrottleMinEnabled; + float overrideThrottleMin; + bool overrideThrottleMaxEnabled; + float overrideThrottleMax; + bool overridePitchMinEnabled; + float overridePitchMin; //in degrees (replaces param values) + bool overridePitchMaxEnabled; + float overridePitchMax; //in degrees (replaces param values) + + /* Enable a specific limit override */ + void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; + warnx("value %.3f", value); + }; + /* Disable a specific limit override */ + void disable(bool *flag) { *flag = false; }; + + + }; + /* * Control in altitude setpoint and speed mode */ - int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode); + int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode */ - int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode); + int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ - int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode); + int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, + float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); /* * Reset all integrators diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index a7acd95deb..e4e405227e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -89,6 +89,8 @@ public: bool isAngularLimit() {return _isAngularLimit ;} float getMin() { return _min.get(); } float getMax() { return _max.get(); } + void setMin(float value) { _min.set(value); } + void setMax(float value) { _max.set(value); } protected: //attributes bool _isAngularLimit; @@ -96,7 +98,6 @@ protected: control::BlockParamFloat _max; }; -typedef /* A combination of feed forward, P and I gain using the output limiter*/ class BlockFFPILimited: public SuperBlock From bfb27ff7bd27e3c89eb0eebc923c9ab7bd7c04fe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 May 2014 00:14:56 +0200 Subject: [PATCH 03/16] fw pos control: set takeoff min pitch for mtecs --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) 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 e3b6eb2611..e43469b707 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 @@ -1452,13 +1452,22 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ fwPosctrl::mTecs::tecs_mode mode) { 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); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode); + fwPosctrl::mTecs::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } else { + /* 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, From 33356ed50386ceb086595e5bd13e42d8fd924add Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 19:36:06 +0200 Subject: [PATCH 04/16] mtecs publish state --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++---- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 + src/modules/fw_pos_control_l1/mtecs/mTecs.h | 9 --------- src/modules/uORB/topics/tecs_status.h | 10 ++++++++++ 4 files changed, 15 insertions(+), 13 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 e43469b707..7fd7ed07f8 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 @@ -386,7 +386,7 @@ private: bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL); + tecs_mode mode = TECS_MODE_NORMAL); }; @@ -1018,7 +1018,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 0.0f, throttle_max, throttle_land, false, flare_pitch_angle_rad, _pos_sp_triplet.current.alt + relative_alt, ground_speed, - land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND); + land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -1111,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(10.0f)), _global_pos.alt, ground_speed, - fwPosctrl::mTecs::TECS_MODE_TAKEOFF); + TECS_MODE_TAKEOFF); /* 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)); @@ -1449,7 +1449,7 @@ 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, - fwPosctrl::mTecs::tecs_mode mode) + tecs_mode mode) { if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index d370bf9063..039fc34a85 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -224,6 +224,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight _status.totalEnergyRate = totalEnergyRate; _status.energyDistributionRateSp = energyDistributionRateSp; _status.energyDistributionRate = energyDistributionRate; + _status.mode = mode; /** update control blocks **/ /* update total energy rate control block */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 1a787df728..b7d4af0f90 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -60,15 +60,6 @@ public: mTecs(); virtual ~mTecs(); - typedef enum { - TECS_MODE_NORMAL, - TECS_MODE_UNDERSPEED, - TECS_MODE_TAKEOFF, - TECS_MODE_LAND, - TECS_MODE_LAND_THROTTLELIM - } tecs_mode; - - /* A small class which provides helper fucntions to override control output limits which are usually set by * parameters in special cases */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index f3d33ec206..fc530b2955 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -50,6 +50,14 @@ * @{ */ +typedef enum { + TECS_MODE_NORMAL, + TECS_MODE_UNDERSPEED, + TECS_MODE_TAKEOFF, + TECS_MODE_LAND, + TECS_MODE_LAND_THROTTLELIM +} tecs_mode; + /** * Internal values of the (m)TECS fixed wing speed alnd altitude control system */ @@ -69,6 +77,8 @@ struct tecs_status_s { float totalEnergyRate; float energyDistributionRateSp; float energyDistributionRate; + + tecs_mode mode; }; /** From ca6e309ba05c6793543c55d9507971df6f0721c2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 19:43:58 +0200 Subject: [PATCH 05/16] sdlog2: log mtecs mode --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e1f3490a94..17461f5874 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1508,6 +1508,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 8a0a0ad45a..83832580ca 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -362,6 +362,8 @@ struct log_TECS_s { float totalEnergyRate; float energyDistributionRateSp; float energyDistributionRate; + + uint8_t mode; }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -419,7 +421,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "ffffffffffff", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR"), + LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 7716a1816db9dc469f4c741442d73721469693a6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 09:13:59 +0200 Subject: [PATCH 06/16] mtecs: remove warnx --- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index b7d4af0f90..4643145c16 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -109,7 +109,6 @@ public: /* Enable a specific limit override */ void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; - warnx("value %.3f", value); }; /* Disable a specific limit override */ void disable(bool *flag) { *flag = false; }; From db925db460fc24ae4d02b87e6fc9cebf2b100b7a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 09:38:14 +0200 Subject: [PATCH 07/16] mtecs: do not calculate derivative in first iteration --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 + src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 6 ++++++ src/modules/fw_pos_control_l1/mtecs/mTecs.h | 4 ++++ 3 files changed, 11 insertions(+) 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 7fd7ed07f8..acd623c309 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 @@ -861,6 +861,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); } } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 039fc34a85..a5e680f02d 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -261,6 +261,12 @@ void mTecs::resetIntegrators() _firstIterationAfterReset = true; } +void mTecs::resetDerivatives(float airspeed) +{ + _airspeedDerivative.setU(airspeed); +} + + void mTecs::updateTimeMeasurement() { if (!_dtCalculated) { diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 4643145c16..ddd6583e86 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -139,6 +139,10 @@ public: */ void resetIntegrators(); + /* + * Reset all derivative calculations + */ + void resetDerivatives(float airspeed); /* Accessors */ bool getEnabled() {return _mTecsEnabled.get() > 0;} From bb87cb8c612bb733f0d37226c2157eb080aaf07a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 09:53:04 +0200 Subject: [PATCH 08/16] fw pos control: don't call tecs 50hz update loop if mtecs is enabled --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 acd623c309..954431fa30 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 @@ -845,7 +845,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + if (!_mTecs.getEnabled()) { + _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + } + float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ From c4cf07b1a943b0c4c1ebb553c0322695128c396f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 10:55:50 +0200 Subject: [PATCH 09/16] fix stupid error in underspeed detection --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index a5e680f02d..3de51695e9 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -186,7 +186,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight } /* Check airspeed: if below safe value switch to underspeed mode (if not in takeoff mode) */ - if (!TECS_MODE_LAND && airspeed < _airspeedMin.get()) { + if (mode != TECS_MODE_LAND && airspeed < _airspeedMin.get()) { mode = TECS_MODE_UNDERSPEED; } From 436cefd88c797992ddd066c13ec3a32ea01f1dcf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 12:13:48 +0200 Subject: [PATCH 10/16] make launch detector more flash efficient --- makefiles/config_px4fmu-v1_default.mk | 6 +----- src/lib/launchdetection/module.mk | 2 ++ 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 810e008aeb..e4bc6fecf1 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -52,7 +52,6 @@ MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top -MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/dumpfile @@ -67,12 +66,11 @@ MODULES += modules/mavlink MODULES += modules/gpio_led # -# Estimation modules (EKF/ SO3 / other filters) +# Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav -#MODULES += examples/flow_position_estimator # # Vehicle Control @@ -81,8 +79,6 @@ MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/mc_att_control MODULES += modules/mc_pos_control -#MODULES += examples/flow_position_control -#MODULES += examples/flow_speed_control # # Logging diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk index de0174e372..d92f0bb45a 100644 --- a/src/lib/launchdetection/module.mk +++ b/src/lib/launchdetection/module.mk @@ -38,3 +38,5 @@ SRCS = LaunchDetector.cpp \ CatapultLaunchMethod.cpp \ launchdetection_params.c + +MAXOPTIMIZATION = -Os From ff4aec6588d6452812131b9275069ac2933543ff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:34:27 +0200 Subject: [PATCH 11/16] wind estimate: added uORB topic --- src/modules/uORB/objects_common.cpp | 2 + src/modules/uORB/topics/wind_estimate.h | 68 +++++++++++++++++++++++++ 2 files changed, 70 insertions(+) create mode 100644 src/modules/uORB/topics/wind_estimate.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index bae46e14d7..7c3bb0009e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -203,3 +203,5 @@ ORB_DEFINE(estimator_status, struct estimator_status_report); #include "topics/tecs_status.h" ORB_DEFINE(tecs_status, struct tecs_status_s); +#include "topics/wind_estimate.h" +ORB_DEFINE(wind_estimate, struct wind_estimate_s); diff --git a/src/modules/uORB/topics/wind_estimate.h b/src/modules/uORB/topics/wind_estimate.h new file mode 100644 index 0000000000..58333a64f8 --- /dev/null +++ b/src/modules/uORB/topics/wind_estimate.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file wind_estimate.h + * + * Wind estimate topic topic + * + */ + +#ifndef TOPIC_WIND_ESTIMATE_H +#define TOPIC_WIND_ESTIMATE_H + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** Wind estimate */ +struct wind_estimate_s { + + uint64_t timestamp; /**< Microseconds since system boot */ + float windspeed_north; /**< Wind component in north / X direction */ + float windspeed_east; /**< Wind component in east / Y direction */ + float covariance_north; /**< Uncertainty - set to zero (no uncertainty) if not estimated */ + float covariance_east; /**< Uncertainty - set to zero (no uncertainty) if not estimated */ +}; + +/** + * @} + */ + +ORB_DECLARE(wind_estimate); + +#endif \ No newline at end of file From 50342f96613d6a6d428b8b65a572c93696bd720f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:35:01 +0200 Subject: [PATCH 12/16] att / pos estimator: Publishing wind estimate --- .../ekf_att_pos_estimator_main.cpp | 33 ++++++++++++++++--- .../ekf_att_pos_estimator/estimator.cpp | 4 +-- 2 files changed, 31 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 0a6d3fa8fe..93ed18b8d9 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -158,6 +159,7 @@ private: orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ orb_advert_t _estimator_status_pub; /**< status of the estimator */ + orb_advert_t _wind_pub; /**< wind estimate */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct gyro_report _gyro; @@ -169,6 +171,7 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ + struct wind_estimate_s _wind; /**< Wind estimate */ struct gyro_scale _gyro_offsets; struct accel_scale _accel_offsets; @@ -312,6 +315,7 @@ FixedwingEstimator::FixedwingEstimator() : _global_pos_pub(-1), _local_pos_pub(-1), _estimator_status_pub(-1), + _wind_pub(-1), _att({}), _gyro({}), @@ -323,6 +327,7 @@ FixedwingEstimator::FixedwingEstimator() : _global_pos({}), _local_pos({}), _gps({}), + _wind({}), _gyro_offsets({}), _accel_offsets({}), @@ -1328,13 +1333,31 @@ FixedwingEstimator::task_main() /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { - /* publish the attitude setpoint */ + /* publish the global position */ orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); } else { /* advertise and publish */ _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } + + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = 0.0f; // XXX get form filter + _wind.covariance_east = 0.0f; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + } } } @@ -1396,9 +1419,11 @@ FixedwingEstimator::print_status() printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + printf("states (accel offs) [14]: %8.4f\n", (double)_ekf->states[13]); + printf("states (wind) [15-16]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); + printf("states (earth mag) [17-19]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); + printf("states (body mag) [20-22]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + printf("states (terrain) [23]: %8.4f\n", (double)_ekf->states[22]); printf("states: %s %s %s %s %s %s %s %s %s %s\n", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5db1adbb3c..1320b46689 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2042,10 +2042,10 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) float ret; if (val > max) { ret = max; - ekf_debug("> max: %8.4f, val: %8.4f", max, val); + ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val); } else if (val < min) { ret = min; - ekf_debug("< min: %8.4f, val: %8.4f", min, val); + ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val); } else { ret = val; } From ef6af184aa7858a9124de13c958f02b140b0f712 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:35:26 +0200 Subject: [PATCH 13/16] sdlog2: Logging wind estimate at low rate --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 10 ++++++++++ 2 files changed, 27 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 17461f5874..c19579f0f9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -87,6 +87,7 @@ #include #include #include +#include #include #include @@ -943,6 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct tecs_status_s tecs_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct wind_estimate_s wind_estimate; } buf; memset(&buf, 0, sizeof(buf)); @@ -982,6 +984,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1A_s log_GS1A; struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; + struct log_WIND_s log_WIND; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1016,6 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int tecs_status_sub; int system_power_sub; int servorail_status_sub; + int wind_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1044,6 +1048,9 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); + subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + /* we need to rate-limit wind, as we do not need the full update rate */ + orb_set_interval(subs.wind_sub, 90); thread_running = true; @@ -1512,6 +1519,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(TECS); } + /* --- WIND ESTIMATE --- */ + if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) { + log_msg.msg_type = LOG_WIND_MSG; + log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north; + log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east; + log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north; + log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east; + LOGBUFFER_WRITE_AND_COUNT(WIND); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 83832580ca..a6eb7a5bd3 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -366,6 +366,15 @@ struct log_TECS_s { uint8_t mode; }; +/* --- WIND - WIND ESTIMATE --- */ +#define LOG_WIND_MSG 31 +struct log_WIND_s { + float x; + float y; + float cov_x; + float cov_y; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -422,6 +431,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), + LOG_FORMAT(WIND, "fff", "X,Y,Cov"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 41a763a50e99880147a2e4f5a6988299f8fd6dac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:39:38 +0200 Subject: [PATCH 14/16] Fix log format specifier --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a6eb7a5bd3..a874351b30 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -431,7 +431,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), - LOG_FORMAT(WIND, "fff", "X,Y,Cov"), + LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From eb02c6ce4900a3dfe96592219852b2205c5d691d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 2 Jun 2014 18:28:18 +0200 Subject: [PATCH 15/16] mtecs: disable underspeed mode in takeoff mode (as the comment says) --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 3de51695e9..32f9f19cac 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -185,8 +185,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } - /* Check airspeed: if below safe value switch to underspeed mode (if not in takeoff mode) */ - if (mode != TECS_MODE_LAND && airspeed < _airspeedMin.get()) { + /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ + if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeed < _airspeedMin.get()) { mode = TECS_MODE_UNDERSPEED; } From e03411fc89fcdf36bfca68b5e27e319b56985782 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 4 Jun 2014 18:43:30 +0200 Subject: [PATCH 16/16] mtecs: more comments for params --- .../fw_pos_control_l1/mtecs/mTecs_params.c | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) 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 5912576116..39514b2238 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -58,7 +58,8 @@ PARAM_DEFINE_INT32(MT_ENABLED, 1); /** - * Total Energy Rate Control FF + * Total Energy Rate Control Feedforward + * Maps the total energy rate setpoint to the throttle setpoint * * @min 0.0 * @max 10.0 @@ -68,6 +69,7 @@ PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f); /** * Total Energy Rate Control P + * Maps the total energy rate error to the throttle setpoint * * @min 0.0 * @max 10.0 @@ -77,6 +79,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); /** * Total Energy Rate Control I + * Maps the integrated total energy rate to the throttle setpoint * * @min 0.0 * @max 10.0 @@ -85,7 +88,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f); /** - * Total Energy Rate Control OFF (Cruise throttle) + * Total Energy Rate Control Offset (Cruise throttle sp) * * @min 0.0 * @max 10.0 @@ -94,7 +97,8 @@ PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f); PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); /** - * Energy Distribution Rate Control FF + * Energy Distribution Rate Control Feedforward + * Maps the energy distribution rate setpoint to the pitch setpoint * * @min 0.0 * @max 10.0 @@ -104,6 +108,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f); /** * Energy Distribution Rate Control P + * Maps the energy distribution rate error to the pitch setpoint * * @min 0.0 * @max 10.0 @@ -113,6 +118,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f); /** * Energy Distribution Rate Control I + * Maps the integrated energy distribution rate error to the pitch setpoint * * @min 0.0 * @max 10.0 @@ -122,7 +128,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f); /** - * Total Energy Distribution OFF (Cruise pitch sp) + * Total Energy Distribution Offset (Cruise pitch sp) * * @min 0.0 * @max 10.0 @@ -170,6 +176,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); /** * P gain for the altitude control + * Maps the altitude error to the flight path angle setpoint * * @min 0.0f * @max 10.0f @@ -179,6 +186,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f); /** * D gain for the altitude control + * Maps the change of altitude error to the flight path angle setpoint * * @min 0.0f * @max 10.0f @@ -187,7 +195,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f); PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f); /** - * Lowpass for FPA error derivative (see MT_FPA_D) + * Lowpass for FPA error derivative calculation (see MT_FPA_D) * * @group mTECS */ @@ -217,6 +225,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); /** * P gain for the airspeed control + * Maps the airspeed error to the acceleration setpoint * * @min 0.0f * @max 10.0f @@ -241,7 +250,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); /** - * Airspeed derivative lowpass + * Airspeed derivative calculation lowpass * * @group mTECS */