From 44441ab501be45165d4eeb2e0f138e2153f9f66e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 14:05:17 +0200 Subject: [PATCH] FW pos control: Perform climbout if user requests more than 85% pitch up --- .../fw_pos_control_l1_main.cpp | 45 +++++++++++-------- 1 file changed, 27 insertions(+), 18 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 90cf391536..01d94a8881 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 @@ -100,8 +100,8 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_YAWRATE_THRESH 0.1f // 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 THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes - +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 /** * L1 control app start / stop handling function @@ -370,7 +370,7 @@ private: /** * Publish navigation capabilities */ - void navigation_capabilities_publish(); + void navigation_capabilities_publish(); /** * Get a new waypoint based on heading and distance from current position @@ -386,27 +386,30 @@ private: /** * 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); + float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** * Control position. */ /** - * Do takeoff help when in altitude controlled modes + * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + void do_takeoff_help(); /** - * Update desired altitude base on user pitch stick input + * Update desired altitude base on user pitch stick input + * + * @param dt Time step + * @return true if climbout mode was requested by user (climb with max rate and min airspeed) */ - void update_desired_altitude(float dt); + bool update_desired_altitude(float dt); bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); - float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); + float calculate_target_airspeed(float airspeed_demand); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -421,12 +424,12 @@ private: /* * Reset takeoff state */ - void reset_takeoff_state(); + void reset_takeoff_state(); /* * Reset landing state */ - void reset_landing_state(); + void reset_landing_state(); /* * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) @@ -955,16 +958,20 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } -void FixedwingPositionControl::update_desired_altitude(float dt) +bool FixedwingPositionControl::update_desired_altitude(float dt) { const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; static bool was_in_deadband = false; + bool climbout_mode = false; + + // XXX the sign magic in this function needs to be fixed if (_manual.x > deadBand) { float pitch = (_manual.x - deadBand) / factor; _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; was_in_deadband = false; + climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH); } else if (_manual.x < - deadBand) { float pitch = (_manual.x + deadBand) / factor; _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; @@ -976,6 +983,8 @@ void FixedwingPositionControl::update_desired_altitude(float dt) _hold_alt = _global_pos.alt; was_in_deadband = true; } + + return climbout_mode; } void FixedwingPositionControl::do_takeoff_help() @@ -1275,7 +1284,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Inform user that launchdetection is running */ static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + mavlink_log_critical(_mavlink_fd, "Launchdetection running"); last_sent = hrt_absolute_time(); } @@ -1404,7 +1413,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _manual.z; /* update desired altitude based on user pitch stick input */ - update_desired_altitude(dt); + bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); @@ -1423,7 +1432,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, throttle_max, _parameters.throttle_cruise, - false, + climbout_requested, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, @@ -1497,7 +1506,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _manual.z; /* update desired altitude based on user pitch stick input */ - update_desired_altitude(dt); + bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); @@ -1516,7 +1525,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, throttle_max, _parameters.throttle_cruise, - false, + climbout_requested, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed,