From de14418e93ff9c8758cfd228ae2d2e4cc618db3d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 12 Jul 2016 13:45:32 -0400 Subject: [PATCH] fw_pos_ctrl_l1 var naming consistency and effc++ --- Tools/check_code_style_all.sh | 1 + src/lib/controllib/block/BlockParam.hpp | 3 + src/lib/external_lgpl/tecs/tecs.h | 2 + src/lib/launchdetection/LaunchDetector.h | 6 +- src/lib/launchdetection/LaunchMethod.h | 2 + src/modules/fw_pos_control_l1/CMakeLists.txt | 2 +- .../fw_pos_control_l1_main.cpp | 272 ++++++++++-------- .../fw_pos_control_l1/landingslope.cpp | 15 +- src/modules/fw_pos_control_l1/landingslope.h | 2 +- .../fw_pos_control_l1/mtecs/limitoverride.cpp | 8 +- .../fw_pos_control_l1/mtecs/limitoverride.h | 30 +- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 50 ++-- .../fw_pos_control_l1/mtecs/mTecs_blocks.h | 32 ++- src/modules/vtol_att_control/vtol_type.h | 2 + 14 files changed, 256 insertions(+), 171 deletions(-) diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh index c8224c0a7b..d39e8ea0b9 100755 --- a/Tools/check_code_style_all.sh +++ b/Tools/check_code_style_all.sh @@ -23,6 +23,7 @@ find \ src/modules/controllib_test \ src/modules/dataman \ src/modules/fw_att_control \ + src/modules/fw_pos_control_l1 \ src/modules/gpio_led \ src/modules/land_detector \ src/modules/local_position_estimator \ diff --git a/src/lib/controllib/block/BlockParam.hpp b/src/lib/controllib/block/BlockParam.hpp index 693a8ec3b2..f88a80e6ba 100644 --- a/src/lib/controllib/block/BlockParam.hpp +++ b/src/lib/controllib/block/BlockParam.hpp @@ -78,6 +78,9 @@ class BlockParam : public BlockParamBase public: BlockParam(Block *block, const char *name, bool parent_prefix = true, T *extern_address = NULL); + BlockParam(const BlockParam &) = delete; + BlockParam &operator=(const BlockParam &) = delete; + T get(); void commit(); void set(T val); diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index e892a72f7a..cbcc6ed9ae 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -66,6 +66,8 @@ public: _TASmin(3.0f), _TAS_dem(0.0f), _TAS_dem_last(0.0f), + _EAS_dem(0.0f), + _hgt_dem(0.0f), _hgt_dem_in_old(0.0f), _hgt_dem_adj(0.0f), _hgt_dem_adj_last(0.0f), diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 3b276c5563..cc64f8b431 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -55,7 +55,10 @@ class __EXPORT LaunchDetector : public control::SuperBlock { public: LaunchDetector(); - ~LaunchDetector(); + LaunchDetector(const LaunchDetector &) = delete; + LaunchDetector operator=(const LaunchDetector &) = delete; + virtual ~LaunchDetector(); + void reset(); void update(float accel_x); @@ -80,7 +83,6 @@ private: control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; - }; } diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index 66ee3f2c6e..065811aa9c 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -58,6 +58,8 @@ enum LaunchDetectionResult { class LaunchMethod { public: + virtual ~LaunchMethod() {}; + virtual void update(float accel_x) = 0; virtual LaunchDetectionResult getLaunchDetected() const = 0; virtual void reset() = 0; diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index 53d12b0401..5a4e7fa6f1 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -35,7 +35,7 @@ px4_add_module( MAIN fw_pos_control_l1 STACK_MAIN 1200 COMPILE_FLAGS - -Os + -Os -Weffc++ SRCS fw_pos_control_l1_main.cpp landingslope.cpp 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 ae38b77095..8871ccf551 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 @@ -103,18 +103,16 @@ #include static int _control_task = -1; /**< task handle for sensor task */ + #define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode #define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode #define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane #define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading #define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid - -static constexpr float THROTTLE_THRESH = - 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes -static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = - 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode -static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; +#define THROTTLE_THRESH 0.05f ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +#define MANUAL_THROTTLE_CLIMBOUT_THRESH 0.85f ///< a throttle / pitch input above this value leads to the system switching to climbout mode +#define ALTHOLD_EPV_RESET_THRESH 5.0f /** * L1 control app start / stop handling function @@ -138,6 +136,10 @@ public: */ ~FixedwingPositionControl(); + // prevent copying + FixedwingPositionControl(const FixedwingPositionControl &) = delete; + FixedwingPositionControl operator=(const FixedwingPositionControl &other) = delete; + /** * Start the sensors task. * @@ -164,8 +166,8 @@ private: int _control_mode_sub; /**< control mode subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ int _vehicle_land_detected_sub; /**< vehicle land detected subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_control_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ @@ -198,39 +200,35 @@ private: struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** ¤t_positi float mission_throttle = _parameters.throttle_cruise; if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) && - _pos_sp_triplet.current.cruising_throttle > 0.01f) { + _pos_sp_triplet.current.cruising_throttle > 0.01f) { + mission_throttle = _pos_sp_triplet.current.cruising_throttle; } @@ -1373,8 +1382,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff) && _fw_pos_ctrl_status.abort_landing == true)) { /* 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(-10.0f), + math::radians(10.0f)); } tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(mission_airspeed), eas2tas, @@ -1400,6 +1409,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* 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; @@ -1414,24 +1424,25 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<2> curr_wp_shifted; double lat; double lon; - create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat, - pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); + create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, + pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); curr_wp_shifted(0) = (float)lat; curr_wp_shifted(1) = (float)lon; // we want the plane to keep tracking the desired flight path until we start flaring // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds //if (land_noreturn_vertical) { - if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { + if (wp_distance < _parameters.land_heading_hold_horizontal_distance || _land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ - if (!land_noreturn_horizontal) {//set target_bearing in first occurrence + if (!_land_noreturn_horizontal) { + // set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { - target_bearing = bearing_lastwp_currwp; + _target_bearing = bearing_lastwp_currwp; } else { - target_bearing = _yaw; + _target_bearing = _yaw; } mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold"); @@ -1439,9 +1450,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); - _l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d); + _l1_control.navigate_heading(_target_bearing, _yaw, ground_speed_2d); - land_noreturn_horizontal = true; + _land_noreturn_horizontal = true; } else { @@ -1452,7 +1463,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (land_noreturn_horizontal) { + if (_land_noreturn_horizontal) { /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); } @@ -1491,7 +1502,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) - || land_noreturn_vertical) { + || _land_noreturn_vertical) { // use previous terrain estimate for some time and hope to recover // if we are already flaring (land_noreturn_vertical) then just // go with the old estimate @@ -1513,75 +1524,81 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float L_altitude_rel = pos_sp_triplet.previous.valid ? pos_sp_triplet.previous.alt - terrain_alt : 0.0f; - float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, + float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* 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 + 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 */ -// _tecs.set_speed_weight(2.0f); + /* force TECS to only control speed with pitch, altitude is only implicitly controlled now */ + // _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; /* enable direct yaw control using rudder/wheel */ - _att_sp.yaw_body = target_bearing; + _att_sp.yaw_body = _target_bearing; _att_sp.fw_control_yaw = true; - if (_global_pos.alt < terrain_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; + if (!_land_motor_lim) { + _land_motor_lim = true; mavlink_log_info(&_mavlink_log_pub, "#Landing, limiting throttle"); } } - float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, + float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* avoid climbout */ - if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { + if ((_flare_curve_alt_rel_last < flare_curve_alt_rel && _land_noreturn_vertical) || _land_stayonground) { flare_curve_alt_rel = 0.0f; // stay on ground - land_stayonground = true; + _land_stayonground = true; } tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, - calculate_target_airspeed(airspeed_land), eas2tas, + calculate_target_airspeed(airspeed_land), + eas2tas, math::radians(_parameters.land_flare_pitch_min_deg), math::radians(_parameters.land_flare_pitch_max_deg), - 0.0f, throttle_max, throttle_land, - 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_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); + 0.0f, + throttle_max, + throttle_land, + 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_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); - if (!land_noreturn_vertical) { + if (!_land_noreturn_vertical) { // just started with the flaring phase _att_sp.pitch_body = 0.0f; - height_flare = _global_pos.alt - terrain_alt; + _flare_height = _global_pos.alt - terrain_alt; mavlink_log_info(&_mavlink_log_pub, "#Landing, flaring"); - land_noreturn_vertical = true; + _land_noreturn_vertical = true; } else { if (_global_pos.vel_d > 0.1f) { _att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) * - math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); + math::constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f); } else { _att_sp.pitch_body = _att_sp.pitch_body; } } - flare_curve_alt_rel_last = flare_curve_alt_rel; + _flare_curve_alt_rel_last = flare_curve_alt_rel; } else { @@ -1596,13 +1613,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * */ float altitude_desired_rel; - if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) { + 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) { + if (!_land_onslope) { mavlink_log_info(&_mavlink_log_pub, "#Landing, on slope"); - land_onslope = true; + _land_onslope = true; } } else { @@ -1692,29 +1709,29 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* Perform launch detection */ - if (launchDetector.launchDetectionEnabled() && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + 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_critical(&_mavlink_log_pub, "#Launchdetection running"); + mavlink_log_critical(&_mavlink_log_pub, "#Launch detection running"); last_sent = hrt_absolute_time(); } /* Detect launch */ - launchDetector.update(accel_body(0)); + _launchDetector.update(accel_body(0)); /* update our copy of the launch detection state */ - launch_detection_state = launchDetector.getLaunchDetected(); + _launch_detection_state = _launchDetector.getLaunchDetected(); } else { /* no takeoff detection --> fly */ - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + _launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } /* Set control values depending on the detection state */ - if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + 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); @@ -1723,13 +1740,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* 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 != + float takeoff_throttle = _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? - launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + _launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; /* 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_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 climbout_diff @@ -1970,7 +1987,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { _att_sp.roll_body = (_manual.y * _parameters.man_roll_max_rad) - + _parameters.rollsp_offset_rad; + + _parameters.rollsp_offset_rad; } } else { @@ -1992,7 +2009,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi setpoint = false; /* reset landing and takeoff state */ - if (!last_manual) { + if (!_last_manual) { reset_landing_state(); reset_takeoff_state(); } @@ -2005,12 +2022,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + _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() : + _att_sp.thrust = (_launchDetector.launchDetectionEnabled()) ? _launchDetector.getThrottlePreTakeoff() : _parameters.throttle_idle; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && @@ -2039,19 +2056,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - (launch_detection_state == LAUNCHDETECTION_RES_NONE || + (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())) || (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && - land_noreturn_vertical)) + _land_noreturn_vertical)) )) { _att_sp.pitch_body = get_tecs_pitch(); } if (_control_mode.flag_control_position_enabled) { - last_manual = false; + _last_manual = false; } else { - last_manual = true; + _last_manual = true; } @@ -2216,7 +2233,8 @@ FixedwingPositionControl::task_main() _fw_pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error(); math::Vector<2> curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon); - _fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + _fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), + curr_wp(1)); fw_pos_ctrl_status_publish(); } @@ -2237,19 +2255,21 @@ FixedwingPositionControl::task_main() void FixedwingPositionControl::reset_takeoff_state() { - launch_detection_state = LAUNCHDETECTION_RES_NONE; - launchDetector.reset(); + _launch_detection_state = LAUNCHDETECTION_RES_NONE; + _launchDetector.reset(); _runway_takeoff.reset(); } void FixedwingPositionControl::reset_landing_state() { - land_noreturn_horizontal = false; - land_noreturn_vertical = false; - land_stayonground = false; - land_motor_lim = false; - land_onslope = false; - land_useterrain = false; + _time_started_landing = 0; + + _land_noreturn_horizontal = false; + _land_noreturn_vertical = false; + _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, diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index 02f6ab47f7..56d7cfb434 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -46,6 +46,19 @@ #include #include +Landingslope::Landingslope() : + _landing_slope_angle_rad(0.0f), + _flare_relative_alt(0.0f), + _motor_lim_relative_alt(0.0f), + _H1_virt(0.0f), + _H0(0.0f), + _d1(0.0f), + _flare_constant(0.0f), + _flare_length(0.0f), + _horizontal_slope_displacement(0.0f) +{ +} + void Landingslope::update(float landing_slope_angle_rad_new, float flare_relative_alt_new, float motor_lim_relative_alt_new, @@ -69,7 +82,7 @@ void Landingslope::calculateSlopeValues() _horizontal_slope_displacement = (_flare_length - _d1); } -float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) +float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) { return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad); diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index f7b7d7d960..b801b40f8b 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -60,7 +60,7 @@ private: void calculateSlopeValues(); public: - Landingslope() {} + Landingslope(); ~Landingslope() {} diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp index 58795edb65..66dbf81f70 100644 --- a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp @@ -41,10 +41,11 @@ #include "limitoverride.h" -namespace fwPosctrl { +namespace fwPosctrl +{ bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, - BlockOutputLimiter &outputLimiterPitch) + BlockOutputLimiter &outputLimiterPitch) { bool ret = false; @@ -52,14 +53,17 @@ bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, 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; diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h index 64c2e7bbde..5b2ea9c53a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h @@ -69,20 +69,32 @@ public: * @return true if the limit was applied */ bool applyOverride(BlockOutputLimiter &outputLimiterThrottle, - BlockOutputLimiter &outputLimiterPitch); + BlockOutputLimiter &outputLimiterPitch); /* Functions to enable or disable the override */ - void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled, - &overrideThrottleMin, value); } + void enableThrottleMinOverride(float value) + { + enable(&overrideThrottleMinEnabled, + &overrideThrottleMin, value); + } void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); } - void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled, - &overrideThrottleMax, value); } + void enableThrottleMaxOverride(float value) + { + enable(&overrideThrottleMaxEnabled, + &overrideThrottleMax, value); + } void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); } - void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled, - &overridePitchMin, value); } + void enablePitchMinOverride(float value) + { + enable(&overridePitchMinEnabled, + &overridePitchMin, value); + } void disablePitchMinOverride() { disable(&overridePitchMinEnabled); } - void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled, - &overridePitchMax, value); } + void enablePitchMaxOverride(float value) + { + enable(&overridePitchMaxEnabled, + &overridePitchMax, value); + } void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); } protected: diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 7a045fb1bb..333d8a5b8f 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -44,7 +44,8 @@ #include #include -namespace fwPosctrl { +namespace fwPosctrl +{ mTecs::mTecs() : SuperBlock(NULL, "MT"), @@ -85,11 +86,11 @@ mTecs::~mTecs() } int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, - float airspeedSp, unsigned mode, LimitOverride limitOverride) + float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) || - !PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { + !PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { return -1; } @@ -106,7 +107,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* Debug output */ if (_counter % 10 == 0) { debug("***"); - debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", + (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); } #if defined(__PX4_NUTTX) @@ -118,15 +120,15 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* use flightpath angle setpoint for total energy control */ return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, - airspeedSp, mode, limitOverride); + airspeedSp, mode, limitOverride); } int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, - float airspeedSp, unsigned mode, LimitOverride limitOverride) + float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || - !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { + !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { return -1; } @@ -142,9 +144,9 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* Debug output */ if (_counter % 10 == 0) { debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f," - "accelerationLongitudinalSp%.4f", - (double)airspeedSp, (double)airspeed, - (double)airspeedFiltered, (double)accelerationLongitudinalSp); + "accelerationLongitudinalSp%.4f", + (double)airspeedSp, (double)airspeed, + (double)airspeedFiltered, (double)accelerationLongitudinalSp); } #if defined(__PX4_NUTTX) @@ -164,9 +166,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight { /* check if all input arguments are numbers and abort if not so */ if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || - !PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) { + !PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) { return -1; } + /* time measurement */ updateTimeMeasurement(); @@ -179,9 +182,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* calculate values (energies) */ float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered; float airspeedDerivative = 0.0f; - if(_airspeedDerivative.getDt() > 0.0f) { + + if (_airspeedDerivative.getDt() > 0.0f) { airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); } + float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; float airspeedDerivativeSp = accelerationLongitudinalSp; float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; @@ -200,9 +205,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Debug output */ if (_counter % 10 == 0) { debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", - (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); + (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, + (double)airspeedDerivativeNorm); debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", - (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, + (double)energyDistributionRateError2); } /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ @@ -213,15 +220,19 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Set special output limiters if we are not in MTECS_MODE_NORMAL */ BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); + if (mode == MTECS_MODE_TAKEOFF) { outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; + } else if (mode == MTECS_MODE_LAND) { // only limit pitch but do not limit throttle outputLimiterPitch = &_BlockOutputLimiterLandPitch; + } else if (mode == MTECS_MODE_LAND_THROTTLELIM) { outputLimiterThrottle = &_BlockOutputLimiterLandThrottle; outputLimiterPitch = &_BlockOutputLimiterLandPitch; + } else if (mode == MTECS_MODE_UNDERSPEED) { outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; @@ -256,9 +267,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight if (_counter % 10 == 0) { debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", - (double)_throttleSp, (double)_pitchSp, - (double)flightPathAngleSp, (double)flightPathAngle, - (double)accelerationLongitudinalSp, (double)airspeedDerivative); + (double)_throttleSp, (double)_pitchSp, + (double)flightPathAngleSp, (double)flightPathAngle, + (double)accelerationLongitudinalSp, (double)airspeedDerivative); } #if defined(__PX4_NUTTX) @@ -293,11 +304,13 @@ void mTecs::updateTimeMeasurement() { if (!_dtCalculated) { float deltaTSeconds = 0.0f; + if (!_firstIterationAfterReset) { hrt_abstime timestampNow = hrt_absolute_time(); deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; timestampLastIteration = timestampNow; } + setDt(deltaTSeconds); _dtCalculated = true; @@ -312,7 +325,8 @@ void mTecs::debug_print(const char *fmt, va_list args) fprintf(stderr, "\n"); } -void mTecs::debug(const char *fmt, ...) { +void mTecs::debug(const char *fmt, ...) +{ if (!_debug) { return; 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 bb20654728..1363d121f7 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -68,21 +68,25 @@ public: * * @param value is changed to be on the interval _min to _max * @param difference if the value is changed this corresponds to the change of value * (-1) - * otherwise unchanged + * otherwise unchanged * @return: true if the limit is applied, false otherwise */ - bool limit(float& value, float& difference) { + bool limit(float &value, float &difference) + { float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); + if (value < minimum) { difference = value - minimum; value = minimum; return true; + } else if (value > maximum) { difference = value - maximum; value = maximum; return true; } + return false; } //accessor: @@ -126,15 +130,18 @@ protected: BlockOutputLimiter _outputLimiter; float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);} - float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) { + float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) + { float difference = 0.0f; float integralYPrevious = _integral.getY(); float output = calcUnlimitedOutput(inputValue, inputError); - if(outputLimiter.limit(output, difference) && - (((difference < 0) && (getKI() * getIntegral().getY() < 0)) || - ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) { - getIntegral().setY(integralYPrevious); + + if (outputLimiter.limit(output, difference) && + (((difference < 0) && (getKI() * getIntegral().getY() < 0)) || + ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) { + getIntegral().setY(integralYPrevious); } + return output; } private: @@ -152,9 +159,10 @@ public: // methods BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) : BlockFFPILimited(parent, name, isAngularLimit) - {}; + {}; virtual ~BlockFFPILimitedCustom() {}; - float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) { + float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) + { return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter); } }; @@ -170,7 +178,8 @@ public: _outputLimiter(this, "", isAngularLimit) {}; virtual ~BlockPLimited() {}; - float update(float input) { + float update(float input) + { float difference = 0.0f; float output = getKP() * input; getOutputLimiter().limit(output, difference); @@ -197,7 +206,8 @@ public: _outputLimiter(this, "", isAngularLimit) {}; virtual ~BlockPDLimited() {}; - float update(float input) { + float update(float input) + { float difference = 0.0f; float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f); getOutputLimiter().limit(output, difference); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index d755fb43e4..4570e39d8e 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -82,6 +82,8 @@ class VtolType public: VtolType(VtolAttitudeControl *att_controller); + VtolType(const VtolType &) = delete; + VtolType &operator=(const VtolType &) = delete; virtual ~VtolType();