diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-13 15:50:51 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-13 15:50:51 +0200 |
commit | 06ad49a690c66417b509bcb1d4a527ee53dbc992 (patch) | |
tree | 6dddd35c32f9d133d36662d13c0ab9c25d564f1e | |
parent | e8503ab3f90353957136d60dbc3fa7668249119c (diff) | |
parent | dd5627726d55e56dc8ce5674c59f451e66739bf4 (diff) | |
download | px4-firmware-06ad49a690c66417b509bcb1d4a527ee53dbc992.tar.gz px4-firmware-06ad49a690c66417b509bcb1d4a527ee53dbc992.tar.bz2 px4-firmware-06ad49a690c66417b509bcb1d4a527ee53dbc992.zip |
Merge remote-tracking branch 'private_swissfang/stable' into obcfailsafe
Conflicts:
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.cpp | 13 | ||||
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.h | 4 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.cpp | 20 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.h | 3 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchMethod.h | 3 | ||||
-rw-r--r-- | src/lib/launchdetection/launchdetection_params.c | 16 | ||||
-rw-r--r-- | src/modules/bottle_drop/bottle_drop.cpp | 94 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 28 |
8 files changed, 124 insertions, 57 deletions
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 65ae461db..2ea1c414b 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -52,7 +52,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : state(LAUNCHDETECTION_RES_NONE), thresholdAccel(this, "A"), thresholdTime(this, "T"), - motorDelay(this, "MDEL") + motorDelay(this, "MDEL"), + pitchMaxPreThrottle(this, "PMAX") { } @@ -118,4 +119,14 @@ void CatapultLaunchMethod::reset() state = LAUNCHDETECTION_RES_NONE; } +float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) { + /* If motor is turned on do not impose the extra limit on maximum pitch */ + if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + return pitchMaxDefault; + } else { + return pitchMaxPreThrottle.get(); + } + +} + } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index d918c3a76..321dfb1de 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -59,6 +59,7 @@ public: void update(float accel_x); LaunchDetectionResult getLaunchDetected() const; void reset(); + float getPitchMax(float pitchMaxDefault); private: hrt_abstime last_timestamp; @@ -70,6 +71,9 @@ private: control::BlockParamFloat thresholdAccel; control::BlockParamFloat thresholdTime; control::BlockParamFloat motorDelay; + control::BlockParamFloat pitchMaxPreThrottle; /**< Upper pitch limit before throttle is turned on. + Can be used to make sure that the AC does not climb + too much while attached to a bungee */ }; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 2958c0a31..52f5c3257 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -105,4 +105,24 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected() return LAUNCHDETECTION_RES_NONE; } +float LaunchDetector::getPitchMax(float pitchMaxDefault) { + if (!launchdetection_on.get()) { + return pitchMaxDefault; + } + + /* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method, + * otherwise use the default limit */ + if (activeLaunchDetectionMethodIndex < 0) { + if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) { + return pitchMaxDefault; + } else { + return launchMethods[0]->getPitchMax(pitchMaxDefault); + } + } else { + return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault); + } + +} + + } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index b48e724ba..4215b49d2 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -64,6 +64,9 @@ public: float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } + /* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */ + float getPitchMax(float pitchMaxDefault); + // virtual bool getLaunchDetected(); protected: private: diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index d2f091cea..8b5220cb3 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -62,6 +62,9 @@ public: virtual LaunchDetectionResult getLaunchDetected() const = 0; virtual void reset() = 0; + /* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */ + virtual float getPitchMax(float pitchMaxDefault) = 0; + protected: private: }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index d35eb11f6..e3aa7ab2d 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); /** * Motor delay * - * Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller) + * Delay between starting attitude control and powering up the throttle (giving throttle control to the controller) * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate * * @unit seconds @@ -88,6 +88,20 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); * @group Launch detection */ PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); + +/** + * Maximum pitch before the throttle is powered up (during motor delay phase) + * + * This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on. + * This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep). + * + * @unit deg + * @min 0 + * @max 45 + * @group Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f); + /** * Throttle setting while detecting launch. * diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 0bb65252f..53071630d 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -463,10 +463,10 @@ BottleDrop::task_main() continue; } - const unsigned sleeptime_us = 50000; + const unsigned sleeptime_us = 9500; hrt_abstime last_run = hrt_absolute_time(); - float dt_runs = 1e6f / sleeptime_us; + float dt_runs = sleeptime_us / 1e6f; // switch to faster updates during the drop while (_drop_state > DROP_STATE_INIT) { @@ -517,53 +517,49 @@ BottleDrop::task_main() case DROP_STATE_TARGET_VALID: { - // Update drop point at 10 Hz - if (counter % 10 == 0) { - - az = g; // acceleration in z direction[m/s^2] - vz = 0; // velocity in z direction [m/s] - z = 0; // fallen distance [m] - h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] - h = h_0; // height over target [m] - ax = 0; // acceleration in x direction [m/s^2] - vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] - x = 0; // traveled distance in x direction [m] - vw = 0; // wind speed [m/s] - vrx = 0; // relative velocity in x direction [m/s] - v = groundspeed_body; // relative speed vector [m/s] - Fd = 0; // Drag force [N] - Fdx = 0; // Drag force in x direction [N] - Fdz = 0; // Drag force in z direction [N] - - - // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x - while (h > 0.05f) { - // z-direction - vz = vz + az * dt_freefall_prediction; - z = z + vz * dt_freefall_prediction; - h = h_0 - z; - - // x-direction - vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); - vx = vx + ax * dt_freefall_prediction; - x = x + vx * dt_freefall_prediction; - vrx = vx + vw; - - // drag force - v = sqrtf(vz * vz + vrx * vrx); - Fd = 0.5f * rho * A * cd * (v * v); - Fdx = Fd * vrx / v; - Fdz = Fd * vz / v; - - // acceleration - az = g - Fdz / m; - ax = -Fdx / m; - } - - // compute drop vector - x = groundspeed_body * t_signal + x; + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = groundspeed_body; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while (h > 0.05f) { + // z-direction + vz = vz + az * dt_freefall_prediction; + z = z + vz * dt_freefall_prediction; + h = h_0 - z; + + // x-direction + vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); + vx = vx + ax * dt_freefall_prediction; + x = x + vx * dt_freefall_prediction; + vrx = vx + vw; + + // drag force + v = sqrtf(vz * vz + vrx * vrx); + Fd = 0.5f * rho * A * cd * (v * v); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; + + // acceleration + az = g - Fdz / m; + ax = -Fdx / m; } + // compute drop vector + x = groundspeed_body * t_signal + x; + x_t = 0.0f; y_t = 0.0f; @@ -688,10 +684,10 @@ BottleDrop::task_main() // update_actuators(); - // run at roughly 20 Hz + // run at roughly 100 Hz usleep(sleeptime_us); - dt_runs = 1e6f / hrt_elapsed_time(&last_run); + dt_runs = hrt_elapsed_time(&last_run) / 1e6f; last_run = hrt_absolute_time(); } } 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 d177f7552..00b12fffa 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 @@ -390,7 +390,8 @@ private: bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode = TECS_MODE_NORMAL); + tecs_mode mode = TECS_MODE_NORMAL, + bool pitch_max_special = false); }; @@ -1140,7 +1141,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; - /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff + * meters */ if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ @@ -1148,7 +1155,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), + takeoff_pitch_max_rad, _parameters.throttle_min, takeoff_throttle, _parameters.throttle_cruise, true, @@ -1156,7 +1163,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(10.0f)), _global_pos.alt, ground_speed, - TECS_MODE_TAKEOFF); + TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _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), @@ -1228,7 +1236,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, - * without depending on library calls */ + * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { /* Copy thrust and pitch values from tecs */ @@ -1420,7 +1428,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode) + tecs_mode mode, bool pitch_max_special) { if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ @@ -1440,6 +1448,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ } else { limitOverride.disablePitchMinOverride(); } + + if (pitch_max_special) { + /* Use the maximum pitch from the argument */ + limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad); + } else { + /* use pitch max set by MT param */ + limitOverride.disablePitchMaxOverride(); + } _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { |