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 | 363 |
1 files changed, 179 insertions, 184 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 000c02e3d..350ce6dec 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,8 +42,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Implementation for total energy control class: - * Thomas Gubler + * 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. * @@ -88,10 +87,13 @@ #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" +static int _control_task = -1; /**< task handle for sensor task */ + /** * L1 control app start / stop handling function @@ -116,20 +118,26 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. + */ + static int start(); + + /** + * Task status + * + * @return true if the mainloop is running */ - int start(); + bool task_running() { return _task_running; } private: int _mavlink_fd; bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ + bool _task_running; /**< if true, task is running in its mainloop */ int _global_pos_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 _params_sub; /**< notification of parameter updates */ @@ -138,6 +146,7 @@ private: int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ @@ -153,18 +162,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - /** manual control states */ - 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; - - 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_horizontal; @@ -181,8 +178,8 @@ private: /* Landingslope object */ Landingslope landingslope; - float flare_curve_alt_rel_last; + /* heading hold */ float target_bearing; @@ -198,6 +195,7 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; + TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; @@ -206,9 +204,13 @@ private: float l1_damping; float time_const; + float time_const_throt; float min_sink_rate; float max_sink_rate; float max_climb_rate; + float climbout_diff; + float heightrate_p; + float speedrate_p; float throttle_damp; float integrator_gain; float vertical_accel_limit; @@ -228,12 +230,10 @@ private: float throttle_min; float throttle_max; float throttle_cruise; + float throttle_slew_max; float throttle_land_max; - float heightrate_p; - float speedrate_p; - float land_slope_angle; float land_H1_virt; float land_flare_alt_relative; @@ -249,9 +249,13 @@ private: param_t l1_damping; param_t time_const; + param_t time_const_throt; param_t min_sink_rate; param_t max_sink_rate; param_t max_climb_rate; + param_t climbout_diff; + param_t heightrate_p; + param_t speedrate_p; param_t throttle_damp; param_t integrator_gain; param_t vertical_accel_limit; @@ -271,12 +275,10 @@ private: param_t throttle_min; param_t throttle_max; param_t throttle_cruise; + param_t throttle_slew_max; param_t throttle_land_max; - param_t heightrate_p; - param_t speedrate_p; - param_t land_slope_angle; param_t land_H1_virt; param_t land_flare_alt_relative; @@ -391,14 +393,14 @@ 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), - _control_task(-1), + _task_running(false), /* subscriptions */ _global_pos_sub(-1), @@ -408,17 +410,29 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _sensor_combined_sub(-1), _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), + _tecs_status_pub(-1), _nav_capabilities_pub(-1), +/* states */ + _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 */ - _loiter_hold(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), @@ -427,24 +441,18 @@ FixedwingPositionControl::FixedwingPositionControl() : launch_detected(false), usePreTakeoffThrust(false), last_manual(false), + landingslope(), flare_curve_alt_rel_last(0.0f), + target_bearing(0.0f), launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), + _airspeed_last_valid(0), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - _att(), - _att_sp(), - _nav_capabilities(), - _manual(), - _airspeed(), - _control_mode(), - _global_pos(), - _pos_sp_triplet(), - _sensor_combined(), + _l1_control(), _mTecs(), - _was_pos_control_mode(false), - _range_finder() + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -460,6 +468,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _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_slew_max = param_find("FW_THR_SLEW_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); @@ -471,9 +480,11 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.time_const_throt = param_find("FW_T_THRO_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.climbout_diff = param_find("FW_CLMBOUT_DIFF"); _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"); @@ -532,10 +543,12 @@ FixedwingPositionControl::parameters_update() 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_slew_max, &(_parameters.throttle_slew_max)); param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt)); 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)); @@ -547,6 +560,7 @@ FixedwingPositionControl::parameters_update() 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)); + param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); @@ -563,6 +577,25 @@ FixedwingPositionControl::parameters_update() _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_time_const_throt(_parameters.time_const_throt); + _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_throttle_slewrate(_parameters.throttle_slew_max); + _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(_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); + _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_p); + /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -600,17 +633,7 @@ FixedwingPositionControl::vehicle_control_mode_poll() 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; - _launch_lon = _global_pos.lon; - _launch_alt = _global_pos.alt; - _launch_valid = true; - } } } @@ -634,6 +657,9 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); + return airspeed_updated; } @@ -694,7 +720,17 @@ FixedwingPositionControl::vehicle_setpoint_poll() void FixedwingPositionControl::task_main_trampoline(int argc, char *argv[]) { + l1_control::g_control = new FixedwingPositionControl(); + + if (l1_control::g_control == nullptr) { + warnx("OUT OF MEM"); + return; + } + + /* only returns on exit */ l1_control::g_control->task_main(); + delete l1_control::g_control; + l1_control::g_control = nullptr; } float @@ -806,13 +842,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; + if (!_mTecs.getEnabled()) { + _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + } + + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ @@ -837,6 +875,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* 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); + /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -945,7 +986,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_approach = 1.3f * _parameters.airspeed_min; /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1071,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 15.0f) { + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, @@ -1111,15 +1151,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - // 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"); - - // 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(); @@ -1135,89 +1166,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* posctrl mode enabled */) { - - _was_pos_control_mode = false; - - /** POSCTRL FLIGHT **/ - - if (0/* switched from another mode to posctrl */) { - _altctrl_hold_heading = _att.yaw; - } - - if (0/* posctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.r; - } - - //XXX not used - - /* 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 altctrl mode, set airspeed based on manual control */ - - // 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(_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; - - /** ALTCTRL FLIGHT **/ - - if (0/* switched from another mode to altctrl */) { - _altctrl_hold_heading = _att.yaw; - } - - if (0/* altctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.r; - } - - /* if in altctrl mode, set airspeed based on manual control */ - - // XXX check if ground speed undershoot should be applied here - float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; - - /* user switched off throttle */ - if (_manual.z < 0.1f) { - throttle_max = 0.0f; - } - - /* climb out control */ - bool climb_out = false; - - /* user wants to climb out */ - if (_manual.x > 0.3f && _manual.z > 0.8f) { - climb_out = true; - } - - _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; @@ -1238,9 +1186,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getPitchSetpoint(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1256,10 +1204,6 @@ void FixedwingPositionControl::task_main() { - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); - /* * do subscriptions */ @@ -1294,6 +1238,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 */ @@ -1333,14 +1279,6 @@ FixedwingPositionControl::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } - 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); @@ -1394,6 +1332,8 @@ FixedwingPositionControl::task_main() perf_end(_loop_perf); } + _task_running = false; + warnx("exiting.\n"); _control_task = -1; @@ -1424,20 +1364,74 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ 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); + if (_mTecs.getEnabled()) { + /* 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); } else { - limitOverride.disablePitchMinOverride(); + /* Using tecs library */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); + + struct TECS::tecs_state s; + _tecs.get_tecs_state(s); + + struct tecs_status_s t; + + t.timestamp = s.timestamp; + + switch (s.mode) { + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = TECS_MODE_NORMAL; + break; + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = TECS_MODE_UNDERSPEED; + break; + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = TECS_MODE_BAD_DESCENT; + break; + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = TECS_MODE_CLIMBOUT; + break; + } + + t.altitudeSp = s.hgt_dem; + t.altitude_filtered = s.hgt; + t.airspeedSp = s.spd_dem; + t.airspeed_filtered = s.spd; + + t.flightPathAngleSp = s.dhgt_dem; + t.flightPathAngle = s.dhgt; + t.flightPathAngleFiltered = s.dhgt; + + t.airspeedDerivativeSp = s.dspd_dem; + t.airspeedDerivative = s.dspd; + + t.totalEnergyRateSp = s.thr; + t.totalEnergyRate = s.ithr; + t.energyDistributionRateSp = s.ptch; + t.energyDistributionRate = s.iptch; + + if (_tecs_status_pub > 0) { + orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); + } else { + _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); + } } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); } int @@ -1449,7 +1443,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); @@ -1471,17 +1465,18 @@ int fw_pos_control_l1_main(int argc, char *argv[]) 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; + if (OK != FixedwingPositionControl::start()) { 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); } |