From daf16184202b02119aa1ac83cb82cf85979d43f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Sep 2014 12:42:42 +0200 Subject: additional upper pitch limit during launch The pitch limit can be used by the laucnhdetector to limit pitch during critical phases of a launch. For example this can be used to limit pitch while attached to a bungee differently from the standard pitch limit. --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 13 ++++++++++++- src/lib/launchdetection/CatapultLaunchMethod.h | 4 ++++ src/lib/launchdetection/LaunchDetector.cpp | 20 ++++++++++++++++++++ src/lib/launchdetection/LaunchDetector.h | 3 +++ src/lib/launchdetection/LaunchMethod.h | 3 +++ src/lib/launchdetection/launchdetection_params.c | 16 +++++++++++++++- 6 files changed, 57 insertions(+), 2 deletions(-) (limited to 'src/lib') 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. * -- cgit v1.2.3