diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index ad0626be8d..53d12b0401 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -35,13 +35,10 @@ px4_add_module( MAIN fw_pos_control_l1 STACK_MAIN 1200 COMPILE_FLAGS - -Wno-float-equal -Os SRCS fw_pos_control_l1_main.cpp landingslope.cpp - mtecs/mTecs.cpp - mtecs/limitoverride.cpp DEPENDS platforms__common git_ecl 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 fefcc76cea..29ed946778 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 @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 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 @@ -52,53 +52,54 @@ * @author Andreas Antener */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include + +#include "landingslope.h" #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include #include -#include "landingslope.h" -#include "mtecs/mTecs.h" +#include +#include +#include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include static int _control_task = -1; /**< task handle for sensor task */ @@ -249,7 +250,6 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; - fwPosctrl::mTecs _mTecs; enum FW_POSCTRL_MODE { FW_POSCTRL_MODE_AUTO, FW_POSCTRL_MODE_POSITION, @@ -486,8 +486,7 @@ private: void reset_landing_state(); /* - * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) - * XXX need to clean up/remove this function once mtecs fully replaces TECS + * Call TECS : a wrapper function to call the TECS implementation */ void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, float pitch_min_rad, float pitch_max_rad, @@ -495,8 +494,7 @@ private: bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - unsigned mode = tecs_status_s::TECS_MODE_NORMAL, - bool pitch_max_special = false); + unsigned mode = tecs_status_s::TECS_MODE_NORMAL); }; @@ -593,7 +591,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _asp_after_transition(0.0f), _was_in_transition(false), _l1_control(), - _mTecs(), _control_mode_current(FW_POSCTRL_MODE_OTHER) { _nav_capabilities = {}; @@ -780,9 +777,6 @@ FixedwingPositionControl::parameters_update() /* Update Launch Detector Parameters */ launchDetector.updateParams(); - /* Update the mTecs */ - _mTecs.updateParams(); - _runway_takeoff.updateParams(); return OK; @@ -927,18 +921,21 @@ float FixedwingPositionControl::get_demanded_airspeed() { float altctrl_airspeed = 0; + // neutral throttle corresponds to trim airspeed if (_manual.z < 0.5f) { // lower half of throttle is min to trim airspeed altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_trim - _parameters.airspeed_min) * - _manual.z * 2; + (_parameters.airspeed_trim - _parameters.airspeed_min) * + _manual.z * 2; + } else { // upper half of throttle is trim to max airspeed altctrl_airspeed = _parameters.airspeed_trim + - (_parameters.airspeed_max - _parameters.airspeed_trim) * - (_manual.z * 2 - 1); + (_parameters.airspeed_max - _parameters.airspeed_trim) * + (_manual.z * 2 - 1); } + return altctrl_airspeed; } @@ -1232,10 +1229,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _control_mode.flag_control_altitude_enabled)); /* update TECS filters */ - if (!_mTecs.getEnabled()) { - _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, - accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); - } + _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, + accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); @@ -1265,13 +1260,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ - if (_mTecs.getEnabled()) { - _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_ctrl_state.airspeed); - - } else { - _tecs.reset_state(); - } + _tecs.reset_state(); } _control_mode_current = FW_POSCTRL_MODE_AUTO; @@ -1316,8 +1305,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } float mission_airspeed = _parameters.airspeed_trim; + if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) && - _pos_sp_triplet.current.cruising_speed > 0.1f) { + _pos_sp_triplet.current.cruising_speed > 0.1f) { mission_airspeed = _pos_sp_triplet.current.cruising_speed; } @@ -1663,8 +1653,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.pitch_limit_min)), _global_pos.alt, ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); + tecs_status_s::TECS_MODE_TAKEOFF); // assign values _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); @@ -1737,8 +1726,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(10.0f)), _global_pos.alt, ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); + tecs_status_s::TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), @@ -1806,13 +1794,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ - if (_mTecs.getEnabled()) { - _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_ctrl_state.airspeed); - - } else { - _tecs.reset_state(); - } + _tecs.reset_state(); } _control_mode_current = FW_POSCTRL_MODE_POSITION; @@ -1921,13 +1903,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ - if (_mTecs.getEnabled()) { - _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_ctrl_state.airspeed); - - } else { - _tecs.reset_state(); - } + _tecs.reset_state(); } _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; @@ -2002,7 +1978,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* 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 = (launchDetector.launchDetectionEnabled()) ? launchDetector.getThrottlePreTakeoff() : + _parameters.throttle_idle; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && @@ -2050,9 +2027,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } float -FixedwingPositionControl::get_tecs_pitch() { +FixedwingPositionControl::get_tecs_pitch() +{ if (_is_tecs_running) { - return _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + return _tecs.get_pitch_demand(); } else { // return 0 to prevent stale tecs state when it's not running @@ -2061,9 +2039,10 @@ FixedwingPositionControl::get_tecs_pitch() { } float -FixedwingPositionControl::get_tecs_thrust() { +FixedwingPositionControl::get_tecs_thrust() +{ if (_is_tecs_running) { - return _mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(); + return _tecs.get_throttle_demand(); } else { // return 0 to prevent stale tecs state when it's not running @@ -2238,7 +2217,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, - unsigned mode, bool pitch_max_special) + unsigned mode) { bool run_tecs = true; float dt = 0.01f; // prevent division with 0 @@ -2263,7 +2242,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _was_in_transition = true; _asp_after_transition = _ctrl_state.airspeed; - // after transition we ramp up desired airspeed from the speed we had coming out of the transition + // after transition we ramp up desired airspeed from the speed we had coming out of the transition + } else if (_was_in_transition) { _asp_after_transition += dt * 2; // increase 2m/s @@ -2278,6 +2258,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ } _is_tecs_running = run_tecs; + if (!run_tecs) { // next time we run TECS we should reinitialize states _reinitialize_tecs = true; @@ -2289,122 +2270,85 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _reinitialize_tecs = false; } - if (_mTecs.getEnabled()) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); + 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; + } - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2) / ground_speed_length); - } + /* No underspeed protection in landing mode */ + _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND + || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); - fwPosctrl::LimitOverride limitOverride; + /* Using tecs library */ + float pitch_for_tecs = _pitch; - if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { - /* Force the slow downwards spiral */ - limitOverride.enablePitchMinOverride(-1.0f); - limitOverride.enablePitchMaxOverride(5.0f); + // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset + // between multirotor and fixed wing flight + if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + math::Matrix<3, 3> R_offset; + R_offset.from_euler(0, M_PI_2_F, 0); + math::Matrix<3, 3> R_fixed_wing = _R_nb * R_offset; + math::Vector<3> euler = R_fixed_wing.to_euler(); + pitch_for_tecs = euler(1); + } - } else if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + _tecs.update_pitch_throttle(_R_nb, pitch_for_tecs, altitude, alt_sp, v_sp, + _ctrl_state.airspeed, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); - } else { - limitOverride.disablePitchMinOverride(); - } + struct TECS::tecs_state s; + _tecs.get_tecs_state(s); - if (pitch_max_special) { - /* Use the maximum pitch from the argument */ - limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad); + struct tecs_status_s t = {}; - } else { - /* use pitch max set by MT param */ - limitOverride.disablePitchMaxOverride(); - } + t.timestamp = s.timestamp; - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode, - limitOverride); + switch (s.mode) { + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = tecs_status_s::TECS_MODE_NORMAL; + break; + + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; + break; + + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; + break; + + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; + break; + } + + t.altitudeSp = s.altitude_sp; + t.altitude_filtered = s.altitude_filtered; + t.airspeedSp = s.airspeed_sp; + t.airspeed_filtered = s.airspeed_filtered; + + t.flightPathAngleSp = s.altitude_rate_sp; + t.flightPathAngle = s.altitude_rate; + t.flightPathAngleFiltered = s.altitude_rate; + + t.airspeedDerivativeSp = s.airspeed_rate_sp; + t.airspeedDerivative = s.airspeed_rate; + + t.totalEnergyError = s.total_energy_error; + t.totalEnergyRateError = s.total_energy_rate_error; + t.energyDistributionError = s.energy_distribution_error; + t.energyDistributionRateError = s.energy_distribution_rate_error; + + t.throttle_integ = s.throttle_integ; + t.pitch_integ = s.pitch_integ; + + if (_tecs_status_pub != nullptr) { + orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); } else { - 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_status_s::TECS_MODE_LAND - || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); - - /* Using tecs library */ - float pitch_for_tecs = _pitch; - - // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset - // between multirotor and fixed wing flight - if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { - math::Matrix<3,3> R_offset; - R_offset.from_euler(0, M_PI_2_F, 0); - math::Matrix<3,3> R_fixed_wing = _R_nb * R_offset; - math::Vector<3> euler = R_fixed_wing.to_euler(); - pitch_for_tecs = euler(1); - } - - _tecs.update_pitch_throttle(_R_nb, pitch_for_tecs, altitude, alt_sp, v_sp, - _ctrl_state.airspeed, 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_status_s::TECS_MODE_NORMAL; - break; - - case TECS::ECL_TECS_MODE_UNDERSPEED: - t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; - break; - - case TECS::ECL_TECS_MODE_BAD_DESCENT: - t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; - break; - - case TECS::ECL_TECS_MODE_CLIMBOUT: - t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; - break; - } - - t.altitudeSp = s.altitude_sp; - t.altitude_filtered = s.altitude_filtered; - t.airspeedSp = s.airspeed_sp; - t.airspeed_filtered = s.airspeed_filtered; - - t.flightPathAngleSp = s.altitude_rate_sp; - t.flightPathAngle = s.altitude_rate; - t.flightPathAngleFiltered = s.altitude_rate; - - t.airspeedDerivativeSp = s.airspeed_rate_sp; - t.airspeedDerivative = s.airspeed_rate; - - t.totalEnergyError = s.total_energy_error; - t.totalEnergyRateError = s.total_energy_rate_error; - t.energyDistributionError = s.energy_distribution_error; - t.energyDistributionRateError = s.energy_distribution_rate_error; - - t.throttle_integ = s.throttle_integ; - t.pitch_integ = s.pitch_integ; - - if (_tecs_status_pub != nullptr) { - orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); - - } else { - _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); - } + _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); } } 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 e50a898b26..322205b138 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 @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 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 @@ -36,7 +36,7 @@ * * Parameters defined by the L1 position control task * - * @author Lorenz Meier + * @author Lorenz Meier */ /* @@ -223,7 +223,7 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f); * @unit m/s * @min 2.0 * @max 10.0 - * @group FW L1 Control + * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index 744c5509f1..02f6ab47f7 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler + * Copyright (c) 2012-2016 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 @@ -33,8 +32,9 @@ ****************************************************************************/ /* - * @file: landingslope.cpp + * @file landingslope.cpp * + * @author Thomas Gubler */ #include "landingslope.h" @@ -47,9 +47,9 @@ #include void Landingslope::update(float landing_slope_angle_rad_new, - float flare_relative_alt_new, - float motor_lim_relative_alt_new, - float H1_virt_new) + float flare_relative_alt_new, + float motor_lim_relative_alt_new, + float H1_virt_new) { _landing_slope_angle_rad = landing_slope_angle_rad_new; @@ -63,52 +63,65 @@ void Landingslope::update(float landing_slope_angle_rad_new, void Landingslope::calculateSlopeValues() { _H0 = _flare_relative_alt + _H1_virt; - _d1 = _flare_relative_alt/tanf(_landing_slope_angle_rad); - _flare_constant = (_H0 * _d1)/_flare_relative_alt; - _flare_length = - logf(_H1_virt/_H0) * _flare_constant; + _d1 = _flare_relative_alt / tanf(_landing_slope_angle_rad); + _flare_constant = (_H0 * _d1) / _flare_relative_alt; + _flare_length = - logf(_H1_virt / _H0) * _flare_constant; _horizontal_slope_displacement = (_flare_length - _d1); } float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) { - return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad); + return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, + _landing_slope_angle_rad); } -float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp) +float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, + float bearing_airplane_currwp) { /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ - if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) { return getLandingSlopeRelativeAltitude(wp_landing_distance); - else + + } else { return 0.0f; + } } float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude) { - return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); + return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, + _landing_slope_angle_rad); } -float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) +float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, + float bearing_airplane_currwp, float wp_altitude) { /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ - if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) { return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude); - else + + } else { return wp_altitude; + } } -float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp) +float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, + float bearing_airplane_currwp) { /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ - if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) - return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; - else + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) { + return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance) / _flare_constant) - _H1_virt; + + } else { return 0.0f; + } } -float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, + float bearing_airplane_currwp, float wp_landing_altitude) { - return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp); + return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, + bearing_airplane_currwp); } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index a5975ad43e..f7b7d7d960 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler + * Copyright (c) 2012-2016 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 @@ -33,8 +32,9 @@ ****************************************************************************/ /* - * @file: landingslope.h + * @file landingslope.h * + * @author Thomas Gubler */ #ifndef LANDINGSLOPE_H_ @@ -75,7 +75,8 @@ public: * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance * Performs check if aircraft is in front of waypoint to avoid climbout */ - float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); + float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, + float bearing_airplane_currwp); /** @@ -89,44 +90,51 @@ public: * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance * Performs check if aircraft is in front of waypoint to avoid climbout */ - float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude); + float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, + float bearing_airplane_currwp, float wp_landing_altitude); /** * * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ - __EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad) + __EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, + float landing_slope_angle_rad) { - return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative + return (wp_landing_distance - horizontal_slope_displacement) * tanf( + landing_slope_angle_rad); //flare_relative_alt is negative } /** * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ - __EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) + __EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, + float horizontal_slope_displacement, float landing_slope_angle_rad) { - return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude; + return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, + landing_slope_angle_rad) + wp_landing_altitude; } /** * * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude */ - __EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) + __EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, + float horizontal_slope_displacement, float landing_slope_angle_rad) { - return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement; + return (slope_altitude - wp_landing_altitude) / tanf(landing_slope_angle_rad) + horizontal_slope_displacement; } float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); - float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); + float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, + float wp_altitude); void update(float landing_slope_angle_rad_new, - float flare_relative_alt_new, - float motor_lim_relative_alt_new, - float H1_virt_new); + float flare_relative_alt_new, + float motor_lim_relative_alt_new, + float H1_virt_new); inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}