/**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file fw_pos_control_l1_main.c * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll * angle, equivalent to a lateral motion (for copters and rovers). * * Original publication for horizontal control class: * S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," * 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) * * More details and acknowledgements in the referenced library headers. * * @author Lorenz Meier */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /** * L1 control app start / stop handling function * * @ingroup apps */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); class FixedwingPositionControl { public: /** * Constructor */ FixedwingPositionControl(); /** * Destructor, also kills the sensors task. */ ~FixedwingPositionControl(); /** * Start the sensors task. * * @return OK on success. */ int start(); private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ int _global_pos_sub; int _global_set_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 _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _accel_sub; /**< body frame accelerations */ 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 */ 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 _loiter_hold_alt; bool _loiter_hold; float _launch_lat; float _launch_lon; float _launch_alt; bool _launch_valid; /* land states */ /* not in non-abort mode for landing yet */ bool land_noreturn; /* heading hold */ float target_bearing; /* 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 ECL_L1_Pos_Controller _l1_control; TECS _tecs; struct { float l1_period; float l1_damping; float time_const; float min_sink_rate; float max_sink_rate; float max_climb_rate; float throttle_damp; float integrator_gain; float vertical_accel_limit; float height_comp_filter_omega; float speed_comp_filter_omega; float roll_throttle_compensation; float speed_weight; float pitch_damping; float airspeed_min; float airspeed_trim; float airspeed_max; float pitch_limit_min; float pitch_limit_max; float roll_limit; float throttle_min; float throttle_max; float throttle_cruise; float throttle_land_max; float loiter_hold_radius; } _parameters; /**< local copies of interesting parameters */ struct { param_t l1_period; param_t l1_damping; param_t time_const; param_t min_sink_rate; param_t max_sink_rate; param_t max_climb_rate; param_t throttle_damp; param_t integrator_gain; param_t vertical_accel_limit; param_t height_comp_filter_omega; param_t speed_comp_filter_omega; param_t roll_throttle_compensation; param_t speed_weight; param_t pitch_damping; param_t airspeed_min; param_t airspeed_trim; param_t airspeed_max; param_t pitch_limit_min; param_t pitch_limit_max; param_t roll_limit; param_t throttle_min; param_t throttle_max; param_t throttle_cruise; param_t throttle_land_max; param_t loiter_hold_radius; } _parameter_handles; /**< handles for interesting parameters */ /** * Update our local parameter cache. */ int parameters_update(); /** * Update control outputs * */ void control_update(); /** * Check for changes in vehicle status. */ void vehicle_control_mode_poll(); /** * Check for airspeed updates. */ bool vehicle_airspeed_poll(); /** * Check for position updates. */ void vehicle_attitude_poll(); /** * Check for accel updates. */ void vehicle_accel_poll(); /** * Check for set triplet updates. */ void vehicle_setpoint_poll(); /** * 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); float calculate_target_airspeed(float airspeed_demand); void calculate_gndspeed_undershoot(); /** * Shim for calling task_main from task_create. */ static void task_main_trampoline(int argc, char *argv[]); /** * Main sensor collection task. */ void task_main() __attribute__((noreturn)); }; namespace l1_control { /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR #endif static const int ERROR = -1; FixedwingPositionControl *g_control; } FixedwingPositionControl::FixedwingPositionControl() : _task_should_exit(false), _control_task(-1), /* subscriptions */ _global_pos_sub(-1), _global_set_triplet_sub(-1), _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), /* publications */ _attitude_sp_pub(-1), _nav_capabilities_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), /* states */ _setpoint_valid(false), _loiter_hold(false), _airspeed_error(0.0f), _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), land_noreturn(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"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); _parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN"); _parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX"); _parameter_handles.roll_limit = param_find("FW_R_LIM"); _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _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"); _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); _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"); /* fetch initial parameter values */ parameters_update(); } FixedwingPositionControl::~FixedwingPositionControl() { if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ _task_should_exit = true; /* wait for a second for the task to quit at our request */ unsigned i = 0; do { /* wait 20ms */ usleep(20000); /* if we have given up, kill it */ if (++i > 50) { task_delete(_control_task); break; } } while (_control_task != -1); } l1_control::g_control = nullptr; } int 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)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min)); param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max)); param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit)); param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); param_get(_parameter_handles.time_const, &(_parameters.time_const)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); _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 || _parameters.airspeed_min > 100.0f || _parameters.airspeed_trim < _parameters.airspeed_min || _parameters.airspeed_trim > _parameters.airspeed_max) { warnx("error: airspeed parameters invalid"); return 1; } return OK; } void FixedwingPositionControl::vehicle_control_mode_poll() { bool vstatus_updated; /* Check HIL state if vehicle status has changed */ orb_check(_control_mode_sub, &vstatus_updated); if (vstatus_updated) { bool was_armed = _control_mode.flag_armed; 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_alt = _global_pos.alt; _launch_valid = true; } } } bool FixedwingPositionControl::vehicle_airspeed_poll() { /* check if there is an airspeed update or if it timed out */ bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); _airspeed_valid = true; _airspeed_last_valid = hrt_absolute_time(); } else { /* no airspeed updates for one second */ if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { _airspeed_valid = false; } } /* update TECS state */ _tecs.enable_airspeed(_airspeed_valid); return airspeed_updated; } void FixedwingPositionControl::vehicle_attitude_poll() { /* check if there is a new position */ bool att_updated; orb_check(_att_sub, &att_updated); if (att_updated) { orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) _R_nb(i, j) = _att.R[i][j]; } } void FixedwingPositionControl::vehicle_accel_poll() { /* check if there is a new position */ bool accel_updated; orb_check(_accel_sub, &accel_updated); if (accel_updated) { orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } } 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); if (global_sp_updated) { orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet); _setpoint_valid = true; } } void FixedwingPositionControl::task_main_trampoline(int argc, char *argv[]) { l1_control::g_control->task_main(); } float FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) { float airspeed; if (_airspeed_valid) { airspeed = _airspeed.true_airspeed_m_s; } else { airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; } /* cruise airspeed for all modes unless modified below */ float target_airspeed = airspeed_demand; /* add minimum ground speed undershoot (only non-zero in presence of sufficient wind) */ target_airspeed += _groundspeed_undershoot; if (0/* throttle nudging enabled */) { //target_airspeed += nudge term. } /* sanity check: limit to range */ target_airspeed = math::constrain(target_airspeed, _parameters.airspeed_min, _parameters.airspeed_max); /* plain airspeed error */ _airspeed_error = target_airspeed - airspeed; return target_airspeed; } void FixedwingPositionControl::calculate_gndspeed_undershoot() { if (_global_pos_valid) { /* get ground speed vector */ math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy); /* rotate with current attitude */ math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); float ground_speed_body = yaw_vector * ground_speed_vector; /* * Ground speed undershoot is the amount of ground velocity not reached * by the plane. Consequently it is zero if airspeed is >= min ground speed * and positive if airspeed < min ground speed. * * This error value ensures that a plane (as long as its throttle capability is * 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); } else { _groundspeed_undershoot = 0; } } bool FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct vehicle_global_position_set_triplet_s &global_triplet) { bool setpoint = true; calculate_gndspeed_undershoot(); 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; _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; /* no throttle limit as default */ float throttle_max = 1.0f; /* AUTONOMOUS FLIGHT */ // 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) { /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ _tecs.set_speed_weight(_parameters.speed_weight); /* execute navigation once we have a setpoint */ if (_setpoint_valid) { /* current waypoint (the one currently heading for) */ math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); /* previous waypoint */ math::Vector2f prev_wp; if (global_triplet.previous_valid) { prev_wp.setX(global_triplet.previous.lat / 1e7f); prev_wp.setY(global_triplet.previous.lon / 1e7f); } 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); } // XXX add RTL switch if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { 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(); _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)); // XXX handle case when having arrived at home (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(); _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)); } 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) { /* 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(); _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)); } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { /* switch to heading hold for the last meters, continue heading hold after */ 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) { /* heading hold, along the line connecting this and the last waypoint */ // 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 { if (!land_noreturn) target_bearing = _att.yaw; //} warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); if (altitude_error > -5.0f) land_noreturn = true; } else { /* normal navigation */ _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); } /* do not go down too early */ if (wp_distance > 50.0f) { altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; } _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); /* 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_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; if (altitude_error > -4.0f) { /* land with minimal speed */ /* 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 = math::min(throttle_max, _parameters.throttle_land_max); /* 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) { /* minimize speed to approach speed */ _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)); } else { /* normal cruise speed */ _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)); } } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { _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(); /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (altitude_error > 10.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)); /* 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)); } } // 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); } _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)); 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)); } } /* reset land state */ if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { land_noreturn = false; } if (was_circle_mode && !_l1_control.circle_mode()) { /* just kicked out of loiter, reset roll integrals */ _att_sp.roll_reset_integral = true; } } else if (0/* easy mode enabled */) { /** EASY FLIGHT **/ if (0/* switched from another mode to easy */) { _seatbelt_hold_heading = _att.yaw; } if (0/* easy on and manual control yaw non-zero */) { _seatbelt_hold_heading = _att.yaw + _manual.yaw; } /* climb out control */ bool climb_out = false; /* user wants to climb out */ if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { climb_out = true; } /* if in seatbelt mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here float seatbelt_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); _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 */) { /** SEATBELT FLIGHT **/ if (0/* switched from another mode to seatbelt */) { _seatbelt_hold_heading = _att.yaw; } if (0/* seatbelt on and manual control yaw non-zero */) { _seatbelt_hold_heading = _att.yaw + _manual.yaw; } /* if in seatbelt mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here float seatbelt_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; /* user switched off throttle */ if (_manual.throttle < 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) { 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); } else { /** MANUAL FLIGHT **/ /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; } _att_sp.pitch_body = _tecs.get_pitch_demand(); _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); return setpoint; } void FixedwingPositionControl::task_main() { /* inform about start */ warnx("Initializing.."); fflush(stdout); /* * 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)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _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)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); /* abort on a nonzero return value from the parameter init */ if (parameters_update()) { /* parameter setup went wrong, abort */ warnx("aborting startup due to errors."); _task_should_exit = true; } /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; while (!_task_should_exit) { /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ vehicle_control_mode_poll(); /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters from storage */ parameters_update(); } /* only run controller if position changed */ if (fds[1].revents & POLLIN) { static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too large deltaT's */ if (deltaT > 1.0f) deltaT = 0.01f; /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); // XXX add timestamp check _global_pos_valid = true; vehicle_attitude_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_airspeed_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); /* * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ if (control_position(current_position, ground_speed, _global_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ if (_attitude_sp_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); } else { /* advertise and publish */ _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); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && 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); } } } } perf_end(_loop_perf); } warnx("exiting.\n"); _control_task = -1; _exit(0); } int FixedwingPositionControl::start() { ASSERT(_control_task == -1); /* start the task */ _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4048, (main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); if (_control_task < 0) { warn("task start failed"); return -errno; } return OK; } int fw_pos_control_l1_main(int argc, char *argv[]) { if (argc < 1) errx(1, "usage: fw_pos_control_l1 {start|stop|status}"); if (!strcmp(argv[1], "start")) { if (l1_control::g_control != nullptr) errx(1, "already running"); l1_control::g_control = new FixedwingPositionControl; if (l1_control::g_control == nullptr) errx(1, "alloc failed"); if (OK != l1_control::g_control->start()) { delete l1_control::g_control; l1_control::g_control = nullptr; err(1, "start failed"); } exit(0); } if (!strcmp(argv[1], "stop")) { if (l1_control::g_control == nullptr) errx(1, "not running"); delete l1_control::g_control; l1_control::g_control = nullptr; exit(0); } if (!strcmp(argv[1], "status")) { if (l1_control::g_control) { errx(0, "running"); } else { errx(1, "not running"); } } warnx("unrecognized command"); return 1; }