diff options
Diffstat (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 987 |
1 files changed, 647 insertions, 340 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 a9648b207..08c996ebc 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,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 2014 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 @@ -43,12 +42,13 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Original implementation for total energy control class: - * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) + * Implementation for total energy control class: + * Thomas Gubler * * More details and acknowledgements in the referenced library headers. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> */ #include <nuttx/config.h> @@ -67,7 +67,7 @@ #include <uORB/uORB.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> @@ -75,6 +75,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/navigation_capabilities.h> +#include <uORB/topics/sensor_combined.h> #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/err.h> @@ -83,9 +84,13 @@ #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> - +#include <mavlink/mavlink_log.h> +#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" +#include "mtecs/mTecs.h" + /** * L1 control app start / stop handling function @@ -114,66 +119,94 @@ public: */ int start(); + /** + * Task status + * + * @return true if the mainloop is running + */ + bool task_running() { return _task_running; } + private: + int _mavlink_fd; bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle for sensor task */ int _global_pos_sub; - int _global_set_triplet_sub; + int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _control_mode_sub; /**< vehicle status subscription */ + int _control_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ - int _accel_sub; /**< body frame accelerations */ + 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 */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _control_mode; /**< vehicle status */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ - struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _control_mode; /**< vehicle status */ + 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 */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ - float _loiter_hold_lat; - float _loiter_hold_lon; + float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ + double _loiter_hold_lat; + double _loiter_hold_lon; float _loiter_hold_alt; bool _loiter_hold; - float _launch_lat; - float _launch_lon; + double _launch_lat; + double _launch_lon; float _launch_alt; bool _launch_valid; /* land states */ /* not in non-abort mode for landing yet */ - bool land_noreturn; + bool land_noreturn_horizontal; + bool land_noreturn_vertical; + bool land_stayonground; + bool land_motor_lim; + bool land_onslope; + + /* takeoff/launch states */ + bool launch_detected; + bool usePreTakeoffThrust; + + bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) + + /* Landingslope object */ + Landingslope landingslope; + + float flare_curve_alt_rel_last; /* heading hold */ float target_bearing; + /* Launch detection */ + launchdetection::LaunchDetector launchDetector; + /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s bool _airspeed_valid; ///< flag if a valid airspeed estimate exists uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid - math::Dcm _R_nb; ///< current attitude + math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; - TECS _tecs; + fwPosctrl::mTecs _mTecs; + bool _was_pos_control_mode; struct { float l1_period; @@ -205,7 +238,16 @@ private: float throttle_land_max; - float loiter_hold_radius; + float heightrate_p; + float speedrate_p; + + float land_slope_angle; + float land_H1_virt; + 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 */ struct { @@ -239,7 +281,16 @@ private: param_t throttle_land_max; - param_t loiter_hold_radius; + param_t heightrate_p; + param_t speedrate_p; + + param_t land_slope_angle; + param_t land_H1_virt; + 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 */ @@ -265,6 +316,12 @@ private: bool vehicle_airspeed_poll(); /** + * Check for range finder updates. + */ + bool range_finder_poll(); + + + /** * Check for position updates. */ void vehicle_attitude_poll(); @@ -272,7 +329,7 @@ private: /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_sensor_combined_poll(); /** * Check for set triplet updates. @@ -280,13 +337,23 @@ private: void vehicle_setpoint_poll(); /** + * Publish navigation capabilities + */ + 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::Vector2f &global_pos, const math::Vector2f &ground_speed, - const struct vehicle_global_position_set_triplet_s &global_triplet); + 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(); + 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. @@ -296,7 +363,30 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); + + /* + * Reset takeoff state + */ + void reset_takeoff_state(); + + /* + * 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) + * XXX need to clean up/remove this function once mtecs fully replaces TECS + */ + void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + float altitude, + const math::Vector<3> &ground_speed, + tecs_mode mode = TECS_MODE_NORMAL); + }; namespace l1_control @@ -308,43 +398,67 @@ namespace l1_control #endif static const int ERROR = -1; -FixedwingPositionControl *g_control; +FixedwingPositionControl *g_control = nullptr; } FixedwingPositionControl::FixedwingPositionControl() : + _mavlink_fd(-1), _task_should_exit(false), + _task_running(false), _control_task(-1), /* subscriptions */ _global_pos_sub(-1), - _global_set_triplet_sub(-1), + _pos_sp_triplet_sub(-1), _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), _nav_capabilities_pub(-1), + _att(), + _att_sp(), + _nav_capabilities(), + _manual(), + _airspeed(), + _control_mode(), + _global_pos(), + _pos_sp_triplet(), + _sensor_combined(), + _range_finder(), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), + /* states */ - _setpoint_valid(false), _loiter_hold(false), + land_noreturn_horizontal(false), + land_noreturn_vertical(false), + land_stayonground(false), + land_motor_lim(false), + land_onslope(false), + launch_detected(false), + usePreTakeoffThrust(false), + last_manual(false), + flare_curve_alt_rel_last(0.0f), + launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + _mTecs(), + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); - _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -358,6 +472,13 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); + _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); + _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); + _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"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); @@ -370,6 +491,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); /* fetch initial parameter values */ parameters_update(); @@ -407,7 +530,6 @@ FixedwingPositionControl::parameters_update() /* L1 control parameters */ param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping)); param_get(_parameter_handles.l1_period, &(_parameters.l1_period)); - param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); @@ -435,25 +557,21 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); + + param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); + param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); + param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); + 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)); - _tecs.set_time_const(_parameters.time_const); - _tecs.set_min_sink_rate(_parameters.min_sink_rate); - _tecs.set_max_sink_rate(_parameters.max_sink_rate); - _tecs.set_throttle_damp(_parameters.throttle_damp); - _tecs.set_integrator_gain(_parameters.integrator_gain); - _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); - _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); - _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation)); - _tecs.set_speed_weight(_parameters.speed_weight); - _tecs.set_pitch_damping(_parameters.pitch_damping); - _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); - _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); - _tecs.set_max_climb_rate(_parameters.max_climb_rate); - /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -464,6 +582,21 @@ FixedwingPositionControl::parameters_update() return 1; } + /* Update the landing slope */ + landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); + + /* Update and publish the navigation capabilities */ + _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); + _nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement(); + _nav_capabilities.landing_flare_length = landingslope.flare_length(); + navigation_capabilities_publish(); + + /* Update Launch Detector Parameters */ + launchDetector.updateParams(); + + /* Update the mTecs */ + _mTecs.updateParams(); + return OK; } @@ -482,8 +615,8 @@ FixedwingPositionControl::vehicle_control_mode_poll() orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); if (!was_armed && _control_mode.flag_armed) { - _launch_lat = _global_pos.lat / 1e7f; - _launch_lon = _global_pos.lon / 1e7f; + _launch_lat = _global_pos.lat; + _launch_lon = _global_pos.lon; _launch_alt = _global_pos.alt; _launch_valid = true; } @@ -510,12 +643,23 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - 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() { @@ -533,14 +677,14 @@ FixedwingPositionControl::vehicle_attitude_poll() } void -FixedwingPositionControl::vehicle_accel_poll() +FixedwingPositionControl::vehicle_sensor_combined_poll() { /* check if there is a new position */ - bool accel_updated; - orb_check(_accel_sub, &accel_updated); + bool sensors_updated; + orb_check(_sensor_combined_sub, &sensors_updated); - if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + if (sensors_updated) { + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); } } @@ -548,12 +692,11 @@ void FixedwingPositionControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ - bool global_sp_updated; - orb_check(_global_set_triplet_sub, &global_sp_updated); + bool pos_sp_triplet_updated; + orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated); - if (global_sp_updated) { - orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet); - _setpoint_valid = true; + if (pos_sp_triplet_updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); } } @@ -595,17 +738,29 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot() +FixedwingPositionControl::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) { - if (_global_pos_valid) { - /* get ground speed vector */ - math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy); + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { - /* rotate with current attitude */ - math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); + /* rotate ground speed vector with current attitude */ + math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); - float ground_speed_body = yaw_vector * ground_speed_vector; + float ground_speed_body = yaw_vector * ground_speed_2d; + + /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ + float distance = 0.0f; + float delta_altitude = 0.0f; + if (pos_sp_triplet.previous.valid) { + distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; + } else { + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; + } + + float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); + /* * Ground speed undershoot is the amount of ground velocity not reached @@ -616,32 +771,54 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() * not exceeded) travels towards a waypoint (and is not pushed more and more away * by wind). Not countering this would lead to a fly-away. */ - _groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f); + _groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f); } else { _groundspeed_undershoot = 0; } } +void FixedwingPositionControl::navigation_capabilities_publish() +{ + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } +} + +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::Vector2f ¤t_position, const math::Vector2f &ground_speed, - const struct vehicle_global_position_set_triplet_s &global_triplet) +FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, + const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(); + math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; + calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements - // XXX re-visit - float baro_altitude = _global_pos.alt; - /* filter speed and altitude for controller */ - math::Vector3 accel_body(_accel.x, _accel.y, _accel.z); - math::Vector3 accel_earth = _R_nb.transpose() * accel_body; + math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); - _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; @@ -650,260 +827,311 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // XXX this should only execute if auto AND safety off (actuators active), // else integrators should be constantly reset. - if (_control_mode.flag_control_position_enabled) { + if (pos_sp_triplet.current.valid) { - /* get circle mode */ - bool was_circle_mode = _l1_control.circle_mode(); + if (!_was_pos_control_mode) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } + } - /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ - _tecs.set_speed_weight(_parameters.speed_weight); + _was_pos_control_mode = true; - /* execute navigation once we have a setpoint */ - if (_setpoint_valid) { + /* get circle mode */ + bool was_circle_mode = _l1_control.circle_mode(); - /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + /* current waypoint (the one currently heading for) */ + math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); - /* previous waypoint */ - math::Vector2f prev_wp; + /* current waypoint (the one currently heading for) */ + math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); - if (global_triplet.previous_valid) { - prev_wp.setX(global_triplet.previous.lat / 1e7f); - prev_wp.setY(global_triplet.previous.lon / 1e7f); + /* Initialize attitude controller integrator reset flags to 0 */ + _att_sp.roll_reset_integral = false; + _att_sp.pitch_reset_integral = false; + _att_sp.yaw_reset_integral = false; - } else { - /* - * No valid previous waypoint, go for the current wp. - * This is automatically handled by the L1 library. - */ - prev_wp.setX(global_triplet.current.lat / 1e7f); - prev_wp.setY(global_triplet.current.lon / 1e7f); + /* previous waypoint */ + math::Vector<2> prev_wp; - } + if (pos_sp_triplet.previous.valid) { + prev_wp(0) = (float)pos_sp_triplet.previous.lat; + prev_wp(1) = (float)pos_sp_triplet.previous.lon; - // XXX add RTL switch - if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { + } else { + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp(0) = (float)pos_sp_triplet.current.lat; + prev_wp(1) = (float)pos_sp_triplet.current.lon; - math::Vector2f rtl_pos(_launch_lat, _launch_lon); + } - _l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) { + /* waypoint is a plain navigation waypoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - // XXX handle case when having arrived at home (loiter) + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { - } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { - /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + /* waypoint is a loiter waypoint */ + _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, + pos_sp_triplet.current.loiter_direction, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - } else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { - /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, - global_triplet.current.loiter_direction, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* 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)); + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { - } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { + /* heading hold, along the line connecting this and the last waypoint */ - /* switch to heading hold for the last meters, continue heading hold after */ + if (!land_noreturn_horizontal) {//set target_bearing in first occurrence + if (pos_sp_triplet.previous.valid) { + target_bearing = bearing_lastwp_currwp; + } else { + target_bearing = _att.yaw; + } + mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); + } - float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); - //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < 15.0f || land_noreturn) { +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); - /* heading hold, along the line connecting this and the last waypoint */ - + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); - // if (global_triplet.previous_valid) { - // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); - // } else { + /* 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)); - if (!land_noreturn) - target_bearing = _att.yaw; - //} + land_noreturn_horizontal = true; - warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); + } else { - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + /* normal navigation */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + } - if (altitude_error > -5.0f) - land_noreturn = true; + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - } else { - /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); - } + /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now - /* do not go down too early */ - if (wp_distance > 50.0f) { - altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; - } +// /* do not go down too early */ +// if (wp_distance > 50.0f) { +// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt; +// } + /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ + // XXX this could make a great param + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1) + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = 1.3f * _parameters.airspeed_min; + float airspeed_approach = 1.3f * _parameters.airspeed_min; - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + /* Calculate altitude of last ordinary waypoint L */ + float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; - /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ - // XXX this could make a great param + 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_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); - float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); - float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = _parameters.airspeed_min; - float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - if (altitude_error > -4.0f) { + if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out - /* land with minimal speed */ + /* 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 implicitely controlled now */ +// _tecs.set_speed_weight(2.0f); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - 0.0f, _parameters.throttle_max, throttle_land, - math::radians(-10.0f), math::radians(15.0f)); + /* kill the throttle if param requests it */ + throttle_max = _parameters.throttle_max; - /* kill the throttle if param requests it */ + 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; + mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); + } - /* 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)); + } - } else if (wp_distance < 60.0f && altitude_error > -20.0f) { + float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); - /* minimize speed to approach speed */ + /* avoid climbout */ + 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; + } + + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel, + calculate_target_airspeed(airspeed_land), eas2tas, + flare_pitch_angle_rad, math::radians(15.0f), + 0.0f, throttle_max, throttle_land, + false, flare_pitch_angle_rad, + _pos_sp_triplet.current.alt + relative_alt, ground_speed, + land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); + + if (!land_noreturn_vertical) { + mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); + land_noreturn_vertical = true; + } + //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); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + flare_curve_alt_rel_last = flare_curve_alt_rel; + } else { + /* intersect glide slope: + * minimize speed to approach speed + * if current position is higher than the slope follow the glide slope (sink to the + * glide slope) + * also if the system captures the slope it should stay + * on the slope (bool land_onslope) + * if current position is below the slope continue at previous wp altitude + * until the intersection with slope + * */ + float altitude_desired_rel = relative_alt; + if (relative_alt > landing_slope_alt_rel_desired || land_onslope) { + /* stay on slope */ + 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_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt; + } - /* normal cruise speed */ + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, + calculate_target_airspeed(airspeed_approach), eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _pos_sp_triplet.current.alt + relative_alt, + ground_speed); + } + + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* Perform launch detection */ + if(!launch_detected) { //do not do further checks once a launch was detected + if (launchDetector.launchDetectionEnabled()) { + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { + mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + last_sent = hrt_absolute_time(); + } + + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Detect launch */ + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + if (launchDetector.getLaunchDetected()) { + launch_detected = true; + mavlink_log_info(_mavlink_fd, "#audio: Takeoff"); + } + } else { + /* no takeoff detection --> fly */ + launch_detected = true; + warnx("launchdetection off"); } + } - } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + if (launch_detected) { + usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 10.0f) { + if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, + _parameters.throttle_cruise, + true, + math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + TECS_MODE_TAKEOFF); /* 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)); } else { - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } - } - - // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), - // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), - // (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid"); - - // XXX at this point we always want no loiter hold if a - // mission is active - _loiter_hold = false; - - } else if (_control_mode.flag_armed) { - - /* hold position, but only if armed, climb 20m in case this is engaged on ground level */ - - // XXX rework with smarter state machine - - if (!_loiter_hold) { - _loiter_hold_lat = _global_pos.lat / 1e7f; - _loiter_hold_lon = _global_pos.lon / 1e7f; - _loiter_hold_alt = _global_pos.alt + 25.0f; - _loiter_hold = true; - } - - altitude_error = _loiter_hold_alt - _global_pos.alt; - - math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); - - /* loiter around current position */ - _l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius, - 1, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - /* climb with full throttle if the altitude error is bigger than 5 meters */ - bool climb_out = (altitude_error > 3); - - float min_pitch; - - if (climb_out) { - min_pitch = math::radians(20.0f); } else { - min_pitch = math::radians(_parameters.pitch_limit_min); + usePreTakeoffThrust = true; } + } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - climb_out, min_pitch, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), + // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), + // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); - if (climb_out) { - /* 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)); - } + // XXX at this point we always want no loiter hold if a + // mission is active + _loiter_hold = false; + + /* reset landing state */ + if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { + reset_landing_state(); } - /* reset land state */ - if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { - land_noreturn = false; + /* reset takeoff/launch state */ + if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { + reset_takeoff_state(); } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -911,97 +1139,119 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.roll_reset_integral = true; } - } else if (0/* easy mode enabled */) { + } else if (0/* posctrl mode enabled */) { - /** EASY FLIGHT **/ + _was_pos_control_mode = false; - if (0/* switched from another mode to easy */) { - _seatbelt_hold_heading = _att.yaw; - } + /** POSCTRL FLIGHT **/ - if (0/* easy on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; - } + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; + } - /* climb out control */ - bool climb_out = false; + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.r; + } - /* user wants to climb out */ - if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { - climb_out = true; - } + //XXX not used - /* if in seatbelt mode, set airspeed based on manual control */ + /* climb out control */ +// bool climb_out = false; +// +// /* user wants to climb out */ +// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { +// climb_out = true; +// } - // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + /* if in altctrl mode, set airspeed based on manual control */ - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + // XXX check if ground speed undershoot should be applied here + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; + + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - false, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* seatbelt mode enabled */) { + tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); + + } else if (0/* altctrl mode enabled */) { + + _was_pos_control_mode = false; - /** SEATBELT FLIGHT **/ + /** ALTCTRL FLIGHT **/ - if (0/* switched from another mode to seatbelt */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to altctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* seatbelt on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* altctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.r; } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + _manual.z; /* user switched off throttle */ - if (_manual.throttle < 0.1f) { + if (_manual.z < 0.1f) { throttle_max = 0.0f; - /* switch to pure pitch based altitude control, give up speed */ - _tecs.set_speed_weight(0.0f); } /* climb out control */ bool climb_out = false; /* user wants to climb out */ - if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + if (_manual.x > 0.3f && _manual.z > 0.8f) { climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); - _att_sp.roll_body = _manual.roll; - _att_sp.yaw_body = _manual.yaw; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - climb_out, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); + _att_sp.roll_body = _manual.y; + _att_sp.yaw_body = _manual.r; + tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + climb_out, math::radians(_parameters.pitch_limit_min), + _global_pos.alt, ground_speed); } else { + _was_pos_control_mode = false; + /** MANUAL FLIGHT **/ /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; + + /* reset landing and takeoff state */ + if (!last_manual) { + reset_landing_state(); + reset_takeoff_state(); + } + } + + if (usePreTakeoffThrust) { + _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } + else { + _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); + } + _att_sp.pitch_body = _mTecs.getPitchSetpoint(); + + if (_control_mode.flag_control_position_enabled) { + last_manual = false; + } else { + last_manual = true; } - _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); return setpoint; } @@ -1018,13 +1268,14 @@ FixedwingPositionControl::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _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); @@ -1047,6 +1298,8 @@ FixedwingPositionControl::task_main() fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; + _task_running = true; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -1080,6 +1333,11 @@ FixedwingPositionControl::task_main() /* only run controller if position changed */ if (fds[1].revents & POLLIN) { + /* XXX Hack to get mavlink output going */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -1097,18 +1355,19 @@ FixedwingPositionControl::task_main() vehicle_attitude_poll(); vehicle_setpoint_poll(); - vehicle_accel_poll(); + vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); + range_finder_poll(); // vehicle_baro_poll(); - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); + math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon); /* * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ - if (control_position(current_position, ground_speed, _global_triplet)) { + if (control_position(current_position, ground_speed, _pos_sp_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ @@ -1121,19 +1380,17 @@ FixedwingPositionControl::task_main() _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } - float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy); + /* XXX check if radius makes sense here */ + float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; - if (_nav_capabilities_pub > 0) { - orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); - } else { - _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); - } + navigation_capabilities_publish(); + } } @@ -1143,12 +1400,54 @@ FixedwingPositionControl::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _control_task = -1; _exit(0); } +void FixedwingPositionControl::reset_takeoff_state() +{ + launch_detected = false; + usePreTakeoffThrust = false; + launchDetector.reset(); +} + +void FixedwingPositionControl::reset_landing_state() +{ + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; + land_motor_lim = false; + land_onslope = false; +} + +void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + float altitude, + const math::Vector<3> &ground_speed, + tecs_mode mode) +{ + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); +} + int FixedwingPositionControl::start() { @@ -1158,7 +1457,7 @@ FixedwingPositionControl::start() _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 4048, + 2000, (main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); @@ -1191,6 +1490,14 @@ int fw_pos_control_l1_main(int argc, char *argv[]) err(1, "start failed"); } + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + while (l1_control::g_control == nullptr || !l1_control::g_control->task_running()) { + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + exit(0); } |