fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Launch detection: Fix code style

Browse Source
This commit is contained in:
Lorenz Meier 2015-09-05 22:13:47 +02:00
parent 25f4a9873d
commit b678a554ea
2 changed files with 21 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/lib/launchdetection/CatapultLaunchMethod.cpp
View File

@ -58,7 +58,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
}
CatapultLaunchMethod::~CatapultLaunchMethod() {
CatapultLaunchMethod::~CatapultLaunchMethod()
{
}
@ -69,34 +70,41 @@ void CatapultLaunchMethod::update(float accel_x)
switch (state) {
case LAUNCHDETECTION_RES_NONE:
/* Detect a acceleration that is longer and stronger as the minimum given by the params */
if (accel_x > thresholdAccel.get()) {
integrator += dt;
if (integrator > thresholdTime.get()) {
if (motorDelay.get() > 0.0f) {
state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
" throttle", (double)motorDelay.get());
" throttle", (double)motorDelay.get());
} else {
/* No motor delay set: go directly to enablemotors state */
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
warnx("Launch detected: state: enablemotors (delay not activated)");
}
}
} else {
/* reset */
reset();
}
break;
case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
/* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is
* over to allow full throttle */
motorDelayCounter += dt;
if (motorDelayCounter > motorDelay.get()) {
warnx("Launch detected: state enablemotors");
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
}
break;
default:
@ -119,10 +127,12 @@ void CatapultLaunchMethod::reset()
state = LAUNCHDETECTION_RES_NONE;
}
float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) {
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();
}

12
src/lib/launchdetection/LaunchDetector.cpp
View File

@ -89,14 +89,15 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected()
{
if (launchdetection_on.get() == 1) {
if (activeLaunchDetectionMethodIndex < 0) {
/* None of the active launchmethods has detected a launch, check all launchmethods */
/* None of the active launchmethods has detected a launch, check all launchmethods */
for (unsigned i = 0; i < (sizeof(launchMethods) / sizeof(launchMethods[0])); i++) {
if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
if (launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
warnx("selecting launchmethod %d", i);
activeLaunchDetectionMethodIndex = i; // from now on only check this method
return launchMethods[i]->getLaunchDetected();
}
}
} else {
return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
}
@ -105,7 +106,8 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected()
return LAUNCHDETECTION_RES_NONE;
}
float LaunchDetector::getPitchMax(float pitchMaxDefault) {
float LaunchDetector::getPitchMax(float pitchMaxDefault)
{
if (!launchdetection_on.get()) {
return pitchMaxDefault;
}
@ -113,11 +115,13 @@ float LaunchDetector::getPitchMax(float 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) {
if (sizeof(launchMethods) / sizeof(LaunchMethod *) > 1) {
return pitchMaxDefault;
} else {
return launchMethods[0]->getPitchMax(pitchMaxDefault);
}
} else {
return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
}