From 71004ab8979d68e6b3add8727307c7c14f751abc Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 5 Mar 2017 01:35:05 -0500 Subject: [PATCH] FW launchdetector only run if armed --- .../launchdetection/CatapultLaunchMethod.cpp | 28 ++--- .../launchdetection/CatapultLaunchMethod.h | 20 ++-- src/lib/launchdetection/LaunchDetector.cpp | 34 +++--- src/lib/launchdetection/LaunchDetector.h | 29 ++--- src/lib/launchdetection/LaunchMethod.h | 9 +- .../fw_pos_control_l1_main.cpp | 109 ++++++++++-------- 6 files changed, 105 insertions(+), 124 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 630b6ae3a0..59881440a7 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -40,33 +40,23 @@ */ #include "CatapultLaunchMethod.h" -#include namespace launchdetection { CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : SuperBlock(parent, "CAT"), - last_timestamp(hrt_absolute_time()), - integrator(0.0f), - motorDelayCounter(0.0f), - state(LAUNCHDETECTION_RES_NONE), thresholdAccel(this, "A"), thresholdTime(this, "T"), motorDelay(this, "MDEL"), pitchMaxPreThrottle(this, "PMAX") { - -} - -CatapultLaunchMethod::~CatapultLaunchMethod() -{ - + last_timestamp = hrt_absolute_time(); } void CatapultLaunchMethod::update(float accel_x) { - float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f; + float dt = hrt_elapsed_time(&last_timestamp) * 1e-6f; last_timestamp = hrt_absolute_time(); switch (state) { @@ -79,18 +69,17 @@ void CatapultLaunchMethod::update(float accel_x) if (integrator > thresholdTime.get()) { if (motorDelay.get() > 0.0f) { state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; - warnx("Launch detected: enablecontrol, waiting %8.4fs until full throttle", - double(motorDelay.get())); + PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle", + double(motorDelay.get())); } else { /* No motor delay set: go directly to enablemotors state */ state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - warnx("Launch detected: enablemotors (delay not activated)"); + PX4_WARN("Launch detected: enablemotors (delay not activated)"); } } } else { - /* reset */ reset(); } @@ -102,7 +91,7 @@ void CatapultLaunchMethod::update(float accel_x) motorDelayCounter += dt; if (motorDelayCounter > motorDelay.get()) { - warnx("Launch detected: state enablemotors"); + PX4_WARN("Launch detected: state enablemotors"); state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } @@ -112,7 +101,6 @@ void CatapultLaunchMethod::update(float accel_x) break; } - } LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const @@ -120,7 +108,6 @@ LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const return state; } - void CatapultLaunchMethod::reset() { integrator = 0.0f; @@ -137,7 +124,6 @@ float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) } else { return pitchMaxPreThrottle.get(); } - } -} +} // namespace launchdetection diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index 321dfb1de4..2a7c5612c1 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -54,19 +54,19 @@ class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock { public: CatapultLaunchMethod(SuperBlock *parent); - ~CatapultLaunchMethod(); + ~CatapultLaunchMethod() override = default; - void update(float accel_x); - LaunchDetectionResult getLaunchDetected() const; - void reset(); - float getPitchMax(float pitchMaxDefault); + void update(float accel_x) override; + LaunchDetectionResult getLaunchDetected() const override; + void reset() override; + float getPitchMax(float pitchMaxDefault) override; private: - hrt_abstime last_timestamp; - float integrator; - float motorDelayCounter; + hrt_abstime last_timestamp{0}; + float integrator{0.0f}; + float motorDelayCounter{0.0f}; - LaunchDetectionResult state; + LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE}; control::BlockParamFloat thresholdAccel; control::BlockParamFloat thresholdTime; @@ -79,4 +79,4 @@ private: #endif /* CATAPULTLAUNCHMETHOD_H_ */ -} +} // namespace launchdetection diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 9a7b213ecc..b6673e0e63 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -37,62 +37,56 @@ * @author Thomas Gubler */ -#include "LaunchDetector.h" #include "CatapultLaunchMethod.h" -#include +#include "LaunchDetector.h" namespace launchdetection { LaunchDetector::LaunchDetector() : SuperBlock(nullptr, "LAUN"), - activeLaunchDetectionMethodIndex(-1), - launchdetection_on(this, "ALL_ON"), - throttlePreTakeoff(nullptr, "FW_THR_IDLE") + launchdetection_on(this, "ALL_ON") { /* init all detectors */ launchMethods[0] = new CatapultLaunchMethod(this); - /* update all parameters of all detectors */ updateParams(); } LaunchDetector::~LaunchDetector() { - + delete launchMethods[0]; } void LaunchDetector::reset() { /* Reset all detectors */ - for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { - launchMethods[i]->reset(); + for (const auto launchMethod : launchMethods) { + launchMethod->reset(); } /* Reset active launchdetector */ - activeLaunchDetectionMethodIndex = -1; - - + activeLaunchDetectionMethodIndex = -1; } void LaunchDetector::update(float accel_x) { - if (launchdetection_on.get() == 1) { - for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { - launchMethods[i]->update(accel_x); + if (launchDetectionEnabled()) { + for (const auto launchMethod : launchMethods) { + launchMethod->update(accel_x); } } } LaunchDetectionResult LaunchDetector::getLaunchDetected() { - if (launchdetection_on.get() == 1) { + if (launchDetectionEnabled()) { if (activeLaunchDetectionMethodIndex < 0) { /* None of the active launchmethods has detected a launch, check all launchmethods */ for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) { if (launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { - warnx("selecting launchmethod %d", i); + PX4_WARN("selecting launchmethod %d", i); activeLaunchDetectionMethodIndex = i; // from now on only check this method return launchMethods[i]->getLaunchDetected(); } @@ -108,7 +102,7 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected() float LaunchDetector::getPitchMax(float pitchMaxDefault) { - if (!launchdetection_on.get()) { + if (!launchDetectionEnabled()) { return pitchMaxDefault; } @@ -125,8 +119,6 @@ float LaunchDetector::getPitchMax(float pitchMaxDefault) } else { return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault); } - } - -} +} // namespace launchdetection diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index cc64f8b431..0b3e3e0bee 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -41,9 +41,6 @@ #ifndef LAUNCHDETECTOR_H #define LAUNCHDETECTOR_H -#include -#include - #include "LaunchMethod.h" #include #include @@ -55,36 +52,34 @@ class __EXPORT LaunchDetector : public control::SuperBlock { public: LaunchDetector(); + ~LaunchDetector() override; + LaunchDetector(const LaunchDetector &) = delete; LaunchDetector operator=(const LaunchDetector &) = delete; - virtual ~LaunchDetector(); void reset(); void update(float accel_x); LaunchDetectionResult getLaunchDetected(); - bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; - - float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } + bool launchDetectionEnabled() { return launchdetection_on.get() == 1; }; /* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */ float getPitchMax(float pitchMaxDefault); -// virtual bool getLaunchDetected(); -protected: private: - int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods - which detected a Launch. If no launchMethod has detected a launch yet the - value is -1. Once one launchMetthod has detected a launch only this - method is checked for further adavancing in the state machine (e.g. when - to power up the motors) */ + /* holds an index to the launchMethod in the array launchMethods + * which detected a Launch. If no launchMethod has detected a launch yet the + * value is -1. Once one launchMethod has detected a launch only this + * method is checked for further advancing in the state machine + * (e.g. when to power up the motors) + */ + int activeLaunchDetectionMethodIndex{-1}; LaunchMethod *launchMethods[1]; - control::BlockParamInt launchdetection_on; - control::BlockParamFloat throttlePreTakeoff; + control::BlockParamInt launchdetection_on; }; -} +} // namespace launchdetection #endif // LAUNCHDETECTOR_H diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index 065811aa9c..06eb5af970 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -48,8 +48,7 @@ enum LaunchDetectionResult { LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */ LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should control the attitude. However any motors should not throttle - up and still be set to 'throttlePreTakeoff'. - For instance this is used to have a delay for the motor + up. For instance this is used to have a delay for the motor when launching a fixed wing aircraft from a bungee */ LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, the controller should control attitude and also throttle up the motors. */ @@ -58,7 +57,7 @@ enum LaunchDetectionResult { class LaunchMethod { public: - virtual ~LaunchMethod() {}; + virtual ~LaunchMethod() = default; virtual void update(float accel_x) = 0; virtual LaunchDetectionResult getLaunchDetected() const = 0; @@ -67,10 +66,8 @@ public: /* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */ virtual float getPitchMax(float pitchMaxDefault) = 0; -protected: -private: }; -} +} // namespace launchdetection #endif /* LAUNCHMETHOD_H_ */ 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 fc0dd23880..c9733a5b98 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 @@ -118,21 +118,13 @@ using matrix::Vector3f; extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); using namespace launchdetection; +using namespace runwaytakeoff; class FixedwingPositionControl { public: - /** - * Constructor - */ FixedwingPositionControl(); - - /** - * Destructor, also kills the sensors task. - */ ~FixedwingPositionControl(); - - // prevent copying FixedwingPositionControl(const FixedwingPositionControl &) = delete; FixedwingPositionControl operator=(const FixedwingPositionControl &other) = delete; @@ -222,10 +214,11 @@ private: hrt_abstime _time_went_in_air{0}; ///< time at which the plane went in the air */ /* Takeoff launch detection and runway */ - launchdetection::LaunchDetector _launchDetector; + LaunchDetector _launchDetector; LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE}; + hrt_abstime _launch_detection_notify{0}; - runwaytakeoff::RunwayTakeoff _runway_takeoff; + RunwayTakeoff _runway_takeoff; bool _last_manual{false}; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) @@ -412,11 +405,6 @@ private: void get_waypoint_heading_distance(float heading, struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init); - /** - * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available - */ - float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); - /** * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available */ @@ -1175,9 +1163,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons nav_speed_2d = ground_speed; } - /* define altitude error */ - float altitude_error = pos_sp_curr.alt - _global_pos.alt; - /* no throttle limit as default */ float throttle_max = 1.0f; @@ -1362,7 +1347,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons } _land_noreturn_horizontal = true; - mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold"); + mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold"); } if (_land_noreturn_horizontal) { @@ -1470,7 +1455,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons if (!_land_motor_lim) { _land_motor_lim = true; - mavlink_log_info(&_mavlink_log_pub, "#Landing, limiting throttle"); + mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle"); } } @@ -1500,7 +1485,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons // just started with the flaring phase _att_sp.pitch_body = 0.0f; _flare_height = _global_pos.alt - terrain_alt; - mavlink_log_info(&_mavlink_log_pub, "#Landing, flaring"); + mavlink_log_info(&_mavlink_log_pub, "Landing, flaring"); _land_noreturn_vertical = true; } else { @@ -1508,6 +1493,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _att_sp.pitch_body = radians(_parameters.land_flare_pitch_min_deg) * constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f); } + + // otherwise continue using previous _att_sp.pitch_body } _flare_curve_alt_rel_last = flare_curve_alt_rel; @@ -1530,7 +1517,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons altitude_desired_rel = landing_slope_alt_rel_desired; if (!_land_onslope) { - mavlink_log_info(&_mavlink_log_pub, "#Landing, on slope"); + mavlink_log_info(&_mavlink_log_pub, "Landing, on slope"); _land_onslope = true; } @@ -1558,6 +1545,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + // continuously reset launch detection and runway takeoff until armed + if (!_control_mode.flag_armed) { + _runway_takeoff.reset(); + + _launchDetector.reset(); + _launch_detection_state = LAUNCHDETECTION_RES_NONE; + _launch_detection_notify = 0; + } + if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { Eulerf euler(Quatf(_ctrl_state.q)); @@ -1567,7 +1563,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - mavlink_log_info(&_mavlink_log_pub, "#Takeoff on runway"); + mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway"); } float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); @@ -1614,20 +1610,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons if (_launchDetector.launchDetectionEnabled() && _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - /* Inform user that launchdetection is running */ - static hrt_abstime last_sent = 0; + if (_control_mode.flag_armed) { + /* Perform launch detection */ - if (hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(&_mavlink_log_pub, "#Launch detection running"); - last_sent = hrt_absolute_time(); + /* Inform user that launchdetection is running every 4s */ + if (hrt_absolute_time() - _launch_detection_notify > 4e6) { + mavlink_log_critical(&_mavlink_log_pub, "Launch detection running"); + _launch_detection_notify = hrt_absolute_time(); + } + + /* Detect launch */ + _launchDetector.update(accel_body(0)); + + /* update our copy of the launch detection state */ + _launch_detection_state = _launchDetector.getLaunchDetected(); } - /* Detect launch */ - _launchDetector.update(accel_body(0)); - - /* update our copy of the launch detection state */ - _launch_detection_state = _launchDetector.getLaunchDetected(); - } else { /* no takeoff detection --> fly */ _launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; @@ -1642,11 +1640,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _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 */ + * full throttle, otherwise we use idle throttle */ float takeoff_throttle = _parameters.throttle_max; if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - takeoff_throttle = _launchDetector.getThrottlePreTakeoff(); + takeoff_throttle = _parameters.throttle_idle; } /* select maximum pitch: the launchdetector may impose another limit for the pitch @@ -1654,16 +1652,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = radians(takeoff_pitch_max_deg); - /* 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) { + float altitude_error = pos_sp_curr.alt - _global_pos.alt; + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */ + if (_parameters.climbout_diff > 0.0f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(pos_sp_curr.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), + _parameters.airspeed_trim, eas2tas, radians(_parameters.pitch_limit_min), takeoff_pitch_max_rad, - _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_min, + takeoff_throttle, _parameters.throttle_cruise, true, max(radians(pos_sp_curr.pitch_min), radians(10.0f)), @@ -1910,19 +1910,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && !_runway_takeoff.runwayTakeoffEnabled()) { + /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons. the pre-takeoff throttle and the idle throttle normally map to the same parameter. */ - _att_sp.thrust = (_launchDetector.launchDetectionEnabled()) ? _launchDetector.getThrottlePreTakeoff() : - _parameters.throttle_idle; + _att_sp.thrust = _parameters.throttle_idle; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _runway_takeoff.runwayTakeoffEnabled()) { + _att_sp.thrust = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max)); } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _att_sp.thrust = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { @@ -2000,11 +2002,11 @@ FixedwingPositionControl::handle_command() if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { if (_land_noreturn_vertical) { - mavlink_log_info(&_mavlink_log_pub, "#Landing, can't abort after flare"); + mavlink_log_info(&_mavlink_log_pub, "Landing, can't abort after flare"); } else { _fw_pos_ctrl_status.abort_landing = true; - mavlink_log_info(&_mavlink_log_pub, "#Landing, aborted"); + mavlink_log_info(&_mavlink_log_pub, "Landing, aborted"); } } } @@ -2196,9 +2198,18 @@ FixedwingPositionControl::task_main() void FixedwingPositionControl::reset_takeoff_state() { - _launch_detection_state = LAUNCHDETECTION_RES_NONE; - _launchDetector.reset(); - _runway_takeoff.reset(); + // only reset takeoff if !armed or just landed + if (!_control_mode.flag_armed || (_was_in_air && _vehicle_land_detected.landed)) { + + _runway_takeoff.reset(); + + _launchDetector.reset(); + _launch_detection_state = LAUNCHDETECTION_RES_NONE; + _launch_detection_notify = 0; + + } else { + _launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } } void FixedwingPositionControl::reset_landing_state()