mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 07:01:35 +08:00
Launch detection: Fix code style
This commit is contained in:
@@ -58,7 +58,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
CatapultLaunchMethod::~CatapultLaunchMethod() {
|
CatapultLaunchMethod::~CatapultLaunchMethod()
|
||||||
|
{
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -69,34 +70,41 @@ void CatapultLaunchMethod::update(float accel_x)
|
|||||||
|
|
||||||
switch (state) {
|
switch (state) {
|
||||||
case LAUNCHDETECTION_RES_NONE:
|
case LAUNCHDETECTION_RES_NONE:
|
||||||
|
|
||||||
/* Detect a acceleration that is longer and stronger as the minimum given by the params */
|
/* Detect a acceleration that is longer and stronger as the minimum given by the params */
|
||||||
if (accel_x > thresholdAccel.get()) {
|
if (accel_x > thresholdAccel.get()) {
|
||||||
integrator += dt;
|
integrator += dt;
|
||||||
|
|
||||||
if (integrator > thresholdTime.get()) {
|
if (integrator > thresholdTime.get()) {
|
||||||
if (motorDelay.get() > 0.0f) {
|
if (motorDelay.get() > 0.0f) {
|
||||||
state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
|
state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
|
||||||
warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
|
warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
|
||||||
" throttle", (double)motorDelay.get());
|
" throttle", (double)motorDelay.get());
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* No motor delay set: go directly to enablemotors state */
|
/* No motor delay set: go directly to enablemotors state */
|
||||||
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
|
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
|
||||||
warnx("Launch detected: state: enablemotors (delay not activated)");
|
warnx("Launch detected: state: enablemotors (delay not activated)");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* reset */
|
/* reset */
|
||||||
reset();
|
reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
|
case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
|
||||||
/* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is
|
/* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is
|
||||||
* over to allow full throttle */
|
* over to allow full throttle */
|
||||||
motorDelayCounter += dt;
|
motorDelayCounter += dt;
|
||||||
|
|
||||||
if (motorDelayCounter > motorDelay.get()) {
|
if (motorDelayCounter > motorDelay.get()) {
|
||||||
warnx("Launch detected: state enablemotors");
|
warnx("Launch detected: state enablemotors");
|
||||||
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
|
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
@@ -119,10 +127,12 @@ void CatapultLaunchMethod::reset()
|
|||||||
state = LAUNCHDETECTION_RES_NONE;
|
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 motor is turned on do not impose the extra limit on maximum pitch */
|
||||||
if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
|
if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
|
||||||
return pitchMaxDefault;
|
return pitchMaxDefault;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return pitchMaxPreThrottle.get();
|
return pitchMaxPreThrottle.get();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -89,14 +89,15 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected()
|
|||||||
{
|
{
|
||||||
if (launchdetection_on.get() == 1) {
|
if (launchdetection_on.get() == 1) {
|
||||||
if (activeLaunchDetectionMethodIndex < 0) {
|
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++) {
|
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);
|
warnx("selecting launchmethod %d", i);
|
||||||
activeLaunchDetectionMethodIndex = i; // from now on only check this method
|
activeLaunchDetectionMethodIndex = i; // from now on only check this method
|
||||||
return launchMethods[i]->getLaunchDetected();
|
return launchMethods[i]->getLaunchDetected();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
|
return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
|
||||||
}
|
}
|
||||||
@@ -105,7 +106,8 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected()
|
|||||||
return LAUNCHDETECTION_RES_NONE;
|
return LAUNCHDETECTION_RES_NONE;
|
||||||
}
|
}
|
||||||
|
|
||||||
float LaunchDetector::getPitchMax(float pitchMaxDefault) {
|
float LaunchDetector::getPitchMax(float pitchMaxDefault)
|
||||||
|
{
|
||||||
if (!launchdetection_on.get()) {
|
if (!launchdetection_on.get()) {
|
||||||
return pitchMaxDefault;
|
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,
|
/* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method,
|
||||||
* otherwise use the default limit */
|
* otherwise use the default limit */
|
||||||
if (activeLaunchDetectionMethodIndex < 0) {
|
if (activeLaunchDetectionMethodIndex < 0) {
|
||||||
if (sizeof(launchMethods)/sizeof(LaunchMethod *) > 1) {
|
if (sizeof(launchMethods) / sizeof(LaunchMethod *) > 1) {
|
||||||
return pitchMaxDefault;
|
return pitchMaxDefault;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return launchMethods[0]->getPitchMax(pitchMaxDefault);
|
return launchMethods[0]->getPitchMax(pitchMaxDefault);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
|
return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user