From ebbe4d2235452fd77575bc084bb519987e566ea9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Nov 2013 22:43:07 +0100 Subject: initial wip version of launchdetector --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 61 ++++++++++++++++------ 1 file changed, 44 insertions(+), 17 deletions(-) (limited to 'src/modules/fw_pos_control_l1') 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..99428ea50 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 @@ -86,6 +86,7 @@ #include #include +#include /** * L1 control app start / stop handling function @@ -164,6 +165,9 @@ private: /* heading hold */ float target_bearing; + /* Launch detection */ + 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 @@ -338,7 +342,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + land_noreturn(false), + launchDetector() { _nav_capabilities.turn_distance = 0.0f; @@ -464,6 +469,8 @@ FixedwingPositionControl::parameters_update() return 1; } + launchDetector.updateParams(); + return OK; } @@ -818,30 +825,50 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + /* Perform launch detection */ + bool do_fly_takeoff = false; + warnx("Launch detection running"); + if (launchDetector.launchDetectionEnabled()) { + launchDetector.update(_accel.x); + if (launchDetector.getLaunchDetected()) { + do_fly_takeoff = true; + warnx("Launch detected. Taking off!"); + } + } else { + /* no takeoff detection --> fly */ + do_fly_takeoff = true; + } + + _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) { + if (do_fly_takeoff) { - /* 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)); + /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + if (altitude_error > 10.0f) { - /* 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)); + /* 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)); - } else { + /* 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)); - _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 { + + _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 { + throttle_max = 0.0f; } } -- cgit v1.2.3 From 52960e06c601cacab90a196803e479dce539cd2b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Dec 2013 17:15:38 +0100 Subject: add fw autoland documentation --- Documentation/fw_landing.png | Bin 0 -> 17371 bytes src/modules/fw_pos_control_l1/landingslope.h | 16 ++++++++-------- 2 files changed, 8 insertions(+), 8 deletions(-) create mode 100644 Documentation/fw_landing.png (limited to 'src/modules/fw_pos_control_l1') diff --git a/Documentation/fw_landing.png b/Documentation/fw_landing.png new file mode 100644 index 000000000..c1165f16a Binary files /dev/null and b/Documentation/fw_landing.png differ diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 8ff431509..1a149fc7c 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -46,16 +46,16 @@ class Landingslope { private: - //xxx: documentation of landing pending - float _landing_slope_angle_rad; - float _flare_relative_alt; + /* see Documentation/fw_landing.png for a plot of the landing slope */ + float _landing_slope_angle_rad; /**< phi in the plot */ + float _flare_relative_alt; /**< h_flare,rel in the plot */ float _motor_lim_horizontal_distance; - float _H1_virt; - float _H0; - float _d1; + float _H1_virt; /**< H1 in the plot */ + float _H0; /**< h_flare,rel + H1 in the plot */ + float _d1; /**< d1 in the plot */ float _flare_constant; - float _flare_length; - float _horizontal_slope_displacement; + float _flare_length; /**< d1 + delta d in the plot */ + float _horizontal_slope_displacement; /**< delta d in the plot */ void calculateSlopeValues(); -- cgit v1.2.3 From 9cc1fc1cb59255c1390f058f03918b0d105c2d26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Dec 2013 19:08:09 +0100 Subject: fixed launchdetection logic, catapult tested in HIL --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 5 ++ .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 67 ++++++++++++++-------- 2 files changed, 48 insertions(+), 24 deletions(-) (limited to 'src/modules/fw_pos_control_l1') diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 0a95b46f6..d5c759b17 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -39,6 +39,7 @@ */ #include "CatapultLaunchMethod.h" +#include CatapultLaunchMethod::CatapultLaunchMethod() : last_timestamp(0), @@ -61,11 +62,15 @@ void CatapultLaunchMethod::update(float accel_x) if (accel_x > threshold_accel.get()) { integrator += accel_x * dt; +// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", +// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); if (integrator > threshold_accel.get() * threshold_time.get()) { launchDetected = true; } } else { +// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", +// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); /* reset integrator */ integrator = 0.0f; launchDetected = false; 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 26f6768cc..5056bcdc1 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 @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -132,7 +133,7 @@ private: 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 */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ @@ -145,7 +146,7 @@ private: struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ - struct accel_report _accel; /**< body frame accelerations */ + struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -171,6 +172,10 @@ private: bool land_motor_lim; bool land_onslope; + /* takeoff/launch states */ + bool launch_detected; + bool launch_detection_message_sent; + /* Landingslope object */ Landingslope landingslope; @@ -311,7 +316,7 @@ private: /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_sensor_combined_poll(); /** * Check for set triplet updates. @@ -389,7 +394,9 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), flare_curve_alt_last(0.0f), _mavlink_fd(-1), - launchDetector() + launchDetector(), + launch_detected(false), + launch_detection_message_sent(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -400,7 +407,7 @@ FixedwingPositionControl::FixedwingPositionControl() : vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; mission_item_triplet_s _mission_item_triplet = {0}; - accel_report _accel = {0}; + sensor_combined_s _sensor_combined = {0}; @@ -631,14 +638,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); } } @@ -756,7 +763,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio 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_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]); 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); @@ -973,24 +980,30 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { /* Perform launch detection */ - bool do_fly_takeoff = false; - warnx("Launch detection running"); - if (launchDetector.launchDetectionEnabled()) { - launchDetector.update(_accel.x); - if (launchDetector.getLaunchDetected()) { - do_fly_takeoff = true; - warnx("Launch detected. Taking off!"); +// warnx("Launch detection running"); + if(!launch_detected) { //do not do further checks once a launch was detected + if (launchDetector.launchDetectionEnabled()) { +// warnx("Launch detection enabled"); + if(!launch_detection_message_sent) { + mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + launch_detection_message_sent = true; + } + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + if (launchDetector.getLaunchDetected()) { + launch_detected = true; + warnx("Launch detected. Taking off!"); + } + } else { + /* no takeoff detection --> fly */ + launch_detected = true; } - } else { - /* no takeoff detection --> fly */ - do_fly_takeoff = true; } _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (do_fly_takeoff) { + if (launch_detected) { /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (altitude_error > 15.0f) { @@ -1037,6 +1050,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_onslope = false; } + /* reset takeoff/launch state */ + if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + launch_detected = false; + launch_detection_message_sent = false; + } + if (was_circle_mode && !_l1_control.circle_mode()) { /* just kicked out of loiter, reset roll integrals */ _att_sp.roll_reset_integral = true; @@ -1151,7 +1170,7 @@ FixedwingPositionControl::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_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)); @@ -1233,7 +1252,7 @@ FixedwingPositionControl::task_main() vehicle_attitude_poll(); vehicle_setpoint_poll(); - vehicle_accel_poll(); + vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); // vehicle_baro_poll(); -- cgit v1.2.3 From 080ecf56caac571f905f657875cec9fa8e7e17d0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Dec 2013 22:21:53 +0100 Subject: launchdetection: add mavlink text output --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/fw_pos_control_l1') 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 5056bcdc1..455fe3674 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 @@ -991,7 +991,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { launch_detected = true; - warnx("Launch detected. Taking off!"); + mavlink_log_info(_mavlink_fd, "#audio: Takeoff"); } } else { /* no takeoff detection --> fly */ -- cgit v1.2.3 From ec8bc6c0208a5eb9ad3decf7bec196c5bf113dd4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 1 Jan 2014 16:33:50 +0100 Subject: fw pos ctrl: remove a wrong transpose --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/fw_pos_control_l1') 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 455fe3674..bbb205b2f 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 @@ -764,7 +764,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* filter speed and altitude for controller */ math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]); - math::Vector3 accel_earth = _R_nb.transpose() * accel_body; + math::Vector3 accel_earth = _R_nb * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; -- cgit v1.2.3 From d1e991f1f0183ce6855bf2df15e1fdd311d096d4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Jan 2014 12:20:25 +0100 Subject: launchdetection: add minimal throttle, fix parameter update --- src/lib/launchdetection/LaunchDetector.cpp | 8 +++++--- src/lib/launchdetection/LaunchDetector.h | 3 +++ src/lib/launchdetection/launchdetection_params.c | 11 ++++++++--- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 4 files changed, 17 insertions(+), 7 deletions(-) (limited to 'src/modules/fw_pos_control_l1') diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 2545e0a7e..d894d6a6f 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -43,7 +43,8 @@ #include LaunchDetector::LaunchDetector() : - launchdetection_on(NULL, "LAUN_ALL_ON", false) + launchdetection_on(NULL, "LAUN_ALL_ON", false), + throttle_min(NULL, "LAUN_THR_MIN", false) { /* init all detectors */ launchMethods[0] = new CatapultLaunchMethod(); @@ -83,10 +84,11 @@ bool LaunchDetector::getLaunchDetected() void LaunchDetector::updateParams() { warnx(" LaunchDetector::updateParams()"); - //launchdetection_on.update(); + launchdetection_on.update(); + throttle_min.update(); for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - //launchMethods[i]->updateParams(); + launchMethods[i]->updateParams(); warnx("updating component %d", i); } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 981891143..3b8aa0ced 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -59,11 +59,14 @@ public: void updateParams(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; + float getMinThrottle() {return throttle_min.get(); } + // virtual bool getLaunchDetected(); protected: private: LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; + control::BlockParamFloat throttle_min; }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 536749c88..e07a2b26d 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -33,11 +33,11 @@ ****************************************************************************/ /** - * @file fw_pos_control_l1_params.c + * @file launchdetection_params.c * - * Parameters defined by the L1 position control task + * Parameters for launchdetection * - * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -65,3 +65,8 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); // @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection // @Range > 0, in seconds PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); + +// @DisplayName Throttle setting while detecting the launch +// @Description The throttle is set to this value while the system is waiting for the takeoff +// @Range 0 to 1 +PARAM_DEFINE_FLOAT(LAUN_THR_MIN, 0.0f); 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 3405185e2..826ee0cdb 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 @@ -1028,7 +1028,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } else { - throttle_max = 0.0f; + throttle_max = launchDetector.getMinThrottle(); } } -- cgit v1.2.3 From f387c0ccc3fca38a2989b784847743b752a78de6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Jan 2014 14:19:19 +0100 Subject: launchdetection: rename pre takeoff throttle param and fix usage --- src/lib/launchdetection/LaunchDetector.cpp | 4 ++-- src/lib/launchdetection/LaunchDetector.h | 4 ++-- src/lib/launchdetection/launchdetection_params.c | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 ++++++++++++-- 4 files changed, 17 insertions(+), 7 deletions(-) (limited to 'src/modules/fw_pos_control_l1') diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index d894d6a6f..67b32ad82 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -44,7 +44,7 @@ LaunchDetector::LaunchDetector() : launchdetection_on(NULL, "LAUN_ALL_ON", false), - throttle_min(NULL, "LAUN_THR_MIN", false) + throttlePreTakeoff(NULL, "LAUN_THR_PRE", false) { /* init all detectors */ launchMethods[0] = new CatapultLaunchMethod(); @@ -85,7 +85,7 @@ void LaunchDetector::updateParams() { warnx(" LaunchDetector::updateParams()"); launchdetection_on.update(); - throttle_min.update(); + throttlePreTakeoff.update(); for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { launchMethods[i]->updateParams(); diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 3b8aa0ced..7c2ff826c 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -59,14 +59,14 @@ public: void updateParams(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; - float getMinThrottle() {return throttle_min.get(); } + float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } // virtual bool getLaunchDetected(); protected: private: LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; - control::BlockParamFloat throttle_min; + control::BlockParamFloat throttlePreTakeoff; }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index e07a2b26d..63a8981aa 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -69,4 +69,4 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); // @DisplayName Throttle setting while detecting the launch // @Description The throttle is set to this value while the system is waiting for the takeoff // @Range 0 to 1 -PARAM_DEFINE_FLOAT(LAUN_THR_MIN, 0.0f); +PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f); 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 826ee0cdb..04caf0bbc 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 @@ -174,6 +174,7 @@ private: /* takeoff/launch states */ bool launch_detected; + bool usePreTakeoffThrust; bool launch_detection_message_sent; /* Landingslope object */ @@ -396,6 +397,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), + usePreTakeoffThrust(false), launch_detection_message_sent(false) { /* safely initialize structs */ @@ -1004,6 +1006,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _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 > 15.0f) { @@ -1028,7 +1031,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } else { - throttle_max = launchDetector.getMinThrottle(); + usePreTakeoffThrust = true; } } @@ -1053,6 +1056,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* reset takeoff/launch state */ if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { launch_detected = false; + usePreTakeoffThrust = false; launch_detection_message_sent = false; } @@ -1150,8 +1154,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio setpoint = false; } + if (usePreTakeoffThrust) { + _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } + else { + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + } _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + return setpoint; } -- cgit v1.2.3