diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-12 14:52:33 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-12 14:52:33 +0200 |
commit | 47d21f0c572d3d70e3577cb510d3b305794574ba (patch) | |
tree | bcbf53d1283bb0e64bb44dbf681074721d3a3636 /src | |
parent | c384dc993c94504519e77a9e33dd296d76ba682e (diff) | |
parent | 3074bced5666934abf348c9fb2fc4bd4f4b6ca57 (diff) | |
download | px4-firmware-47d21f0c572d3d70e3577cb510d3b305794574ba.tar.gz px4-firmware-47d21f0c572d3d70e3577cb510d3b305794574ba.tar.bz2 px4-firmware-47d21f0c572d3d70e3577cb510d3b305794574ba.zip |
Merge pull request #744 from thomasgubler/fw_landing_rangefinder
Fw landing rangefinder
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 90 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 11 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/landingslope.cpp | 34 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/landingslope.h | 31 |
4 files changed, 139 insertions, 27 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 569fbbcdb..c62a2d4f3 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 @@ -89,6 +89,7 @@ #include <launchdetection/LaunchDetector.h> #include <ecl/l1/ecl_l1_pos_controller.h> #include <external_lgpl/tecs/tecs.h> +#include <drivers/drv_range_finder.h> #include "landingslope.h" @@ -134,6 +135,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ + int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ @@ -147,6 +149,7 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ + struct range_finder_report _range_finder; /**< range finder report */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -181,7 +184,7 @@ private: /* Landingslope object */ Landingslope landingslope; - float flare_curve_alt_last; + float flare_curve_alt_rel_last; /* heading hold */ float target_bearing; @@ -239,6 +242,7 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; + float range_finder_rel_alt; } _parameters; /**< local copies of interesting parameters */ @@ -283,6 +287,7 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; + param_t range_finder_rel_alt; } _parameter_handles; /**< handles for interesting parameters */ @@ -309,6 +314,12 @@ private: bool vehicle_airspeed_poll(); /** + * Check for range finder updates. + */ + bool range_finder_poll(); + + + /** * Check for position updates. */ void vehicle_attitude_poll(); @@ -329,6 +340,11 @@ private: void navigation_capabilities_publish(); /** + * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder + */ + float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt); + + /** * Control position. */ bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, @@ -384,6 +400,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), @@ -403,7 +420,7 @@ FixedwingPositionControl::FixedwingPositionControl() : launch_detected(false), last_manual(false), usePreTakeoffThrust(false), - flare_curve_alt_last(0.0f), + flare_curve_alt_rel_last(0.0f), launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), @@ -417,7 +434,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode(), _global_pos(), _pos_sp_triplet(), - _sensor_combined() + _sensor_combined(), + _range_finder() { _nav_capabilities.turn_distance = 0.0f; @@ -442,6 +460,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); + _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -531,6 +550,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -626,6 +647,20 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } +bool +FixedwingPositionControl::range_finder_poll() +{ + /* check if there is a range finder measurement */ + bool range_finder_updated; + orb_check(_range_finder_sub, &range_finder_updated); + + if (range_finder_updated) { + orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder); + } + + return range_finder_updated; +} + void FixedwingPositionControl::vehicle_attitude_poll() { @@ -754,6 +789,23 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } +float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt) +{ + float rel_alt_estimated = current_alt - land_setpoint_alt; + + /* only use range finder if: + * parameter (range_finder_use_relative_alt) > 0 + * the measurement is valid + * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt + */ + if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) { + return rel_alt_estimated; + } + + return range_finder.distance; + +} + bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) @@ -896,12 +948,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); + float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); + + float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -911,7 +965,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (relative_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; @@ -920,16 +974,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); + float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* avoid climbout */ - if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) + if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { - flare_curve_alt = pos_sp_triplet.current.alt; + flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land, @@ -941,7 +995,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); - flare_curve_alt_last = flare_curve_alt; + flare_curve_alt_rel_last = flare_curve_alt_rel; } else { /* intersect glide slope: @@ -949,20 +1003,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * if current position is higher or within 10m of slope follow the glide slope * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * */ - float altitude_desired = _global_pos.alt; - if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + float altitude_desired_rel = relative_alt; + if (relative_alt > landing_slope_alt_rel_desired - 10.0f) { /* stay on slope */ - altitude_desired = landing_slope_alt_desired; + altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); land_onslope = true; } } else { /* continue horizontally */ - altitude_desired = math::max(_global_pos.alt, L_altitude); + altitude_desired_rel = math::max(relative_alt, L_altitude_rel); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1185,6 +1239,7 @@ FixedwingPositionControl::task_main() _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); @@ -1264,6 +1319,7 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); + range_finder_poll(); // vehicle_baro_poll(); math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); 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 37f06dbe5..f258f77da 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 @@ -375,3 +375,14 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); + +/** + * Relative altitude threshold for range finder measurements for use during landing + * + * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT + * set to < 0 to disable + * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index e5f7023ae..8ce465fe8 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -69,26 +69,46 @@ void Landingslope::calculateSlopeValues() _horizontal_slope_displacement = (_flare_length - _d1); } -float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude) +float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) { - return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); + return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) +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)) - return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude); + return getLandingSlopeRelativeAltitude(wp_landing_distance); + 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); +} + +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)) + return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude); else return wp_altitude; } -float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +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 wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; else - return wp_landing_altitude; + return 0.0f; +} + +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); } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 76d65a55f..b54fd501c 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -63,11 +63,26 @@ public: Landingslope() {} ~Landingslope() {} + + /** + * + * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + float getLandingSlopeRelativeAltitude(float wp_landing_distance); + + /** + * + * @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); + + /** * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude); /** * @@ -78,11 +93,20 @@ public: /** * + * @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) + { + 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) { - return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative + return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude; } /** @@ -95,8 +119,9 @@ public: } + float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); - float getFlareCurveAltitudeSave(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, float flare_relative_alt, |