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

LaunchDetection: code style changes and fix info message.

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-11-09 16:21:09 +01:00
parent e6e2c889e0
commit df1cd4f147
2 changed files with 29 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp
View File

@ -46,35 +46,35 @@
namespace launchdetection
{
void LaunchDetector::update(const float dt, float accel_x, orb_advert_t *mavlink_log_pub)
void LaunchDetector::update(const float dt, const float accel_x, orb_advert_t *mavlink_log_pub)
{
switch (_state) {
switch (state_) {
case launch_detection_status_s::STATE_WAITING_FOR_LAUNCH:
_launchDetectionRunningInfoDelay += dt;
_info_delay_counter_s_ += dt;
/* Inform user that launchdetection is running every 4s */
if (_launchDetectionRunningInfoDelay >= 4.f) {
/* Inform user that launchdetection is running every kInfoDelay seconds */
if (_info_delay_counter_s_ >= kInfoDelay) {
mavlink_log_info(mavlink_log_pub, "Launch detection running\t");
events::send(events::ID("launch_detection_running_info"), events::Log::Info, "Launch detection running");
_launchDetectionRunningInfoDelay = 0.f; // reset counter
_info_delay_counter_s_ = 0.f; // reset counter
}
/* Detect a acceleration that is longer and stronger as the minimum given by the params */
if (accel_x > _param_laun_cat_a.get()) {
_launchDetectionDelayCounter += dt;
_info_delay_counter_s_ += dt;
if (_launchDetectionDelayCounter > _param_laun_cat_t.get()) {
if (_info_delay_counter_s_ > _param_laun_cat_t.get()) {
if (_param_laun_cat_mdel.get() > 0.f) {
_state = launch_detection_status_s::STATE_LAUNCH_DETECTED_DISABLED_MOTOR;
mavlink_log_info(mavlink_log_pub, "Launch detected: enable control, waiting %8.1fs until full throttle\t",
state_ = launch_detection_status_s::STATE_LAUNCH_DETECTED_DISABLED_MOTOR;
mavlink_log_info(mavlink_log_pub, "Launch detected: enable control, waiting %8.1fs until throttling up\t",
(double)_param_laun_cat_mdel.get());
events::send<float>(events::ID("launch_detection_wait_for_throttle"), {events::Log::Warning, events::LogInternal::Info},
"Launch detected: enablecontrol, waiting {1:.1}s until full throttle", (double)_param_laun_cat_mdel.get());
} else {
/* No motor delay set: go directly to enablemotors state */
_state = launch_detection_status_s::STATE_FLYING;
state_ = launch_detection_status_s::STATE_FLYING;
mavlink_log_info(mavlink_log_pub, "Launch detected: enable motors (no motor delay)\t");
events::send(events::ID("launch_detection_no_motor_delay"), {events::Log::Warning, events::LogInternal::Info},
"Launch detected: enable motors (no motor delay)");
@ -90,21 +90,21 @@ void LaunchDetector::update(const float dt, float accel_x, orb_advert_t *mavlin
case launch_detection_status_s::STATE_LAUNCH_DETECTED_DISABLED_MOTOR:
/* Vehicle is currently controlling attitude but at idle throttle. Waiting until delay is
* over to allow full throttle */
_motorDelayCounter += dt;
motor_delay_counter_ += dt;
if (_motorDelayCounter > _param_laun_cat_mdel.get()) {
if (motor_delay_counter_ > _param_laun_cat_mdel.get()) {
mavlink_log_info(mavlink_log_pub, "Launch detected: enable motors\t");
events::send(events::ID("launch_detection_enable_motors"), {events::Log::Warning, events::LogInternal::Info},
"Launch detected: enable motors");
_state = launch_detection_status_s::STATE_FLYING;
state_ = launch_detection_status_s::STATE_FLYING;
}
_launchDetectionRunningInfoDelay = 4.f; // reset counter
_info_delay_counter_s_ = kInfoDelay; // reset counter
break;
default:
_launchDetectionRunningInfoDelay = 4.f; // reset counter
_info_delay_counter_s_ = kInfoDelay; // reset counter
break;
}
@ -112,14 +112,14 @@ void LaunchDetector::update(const float dt, float accel_x, orb_advert_t *mavlin
uint LaunchDetector::getLaunchDetected() const
{
return _state;
return state_;
}
void LaunchDetector::reset()
{
_launchDetectionDelayCounter = 0.f;
_motorDelayCounter = 0.f;
_state = launch_detection_status_s::STATE_WAITING_FOR_LAUNCH;
_info_delay_counter_s_ = 0.f;
motor_delay_counter_ = 0.f;
state_ = launch_detection_status_s::STATE_WAITING_FOR_LAUNCH;
}

21
src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h
View File

@ -47,6 +47,9 @@
namespace launchdetection
{
// Info delay threshold (to publish info every kInfoDelay seconds)
static constexpr float kInfoDelay = 4.f;
class __EXPORT LaunchDetector : public ModuleParams
{
public:
@ -68,7 +71,7 @@ public:
* @param accel_x Measured acceleration in body x [m/s/s]
* @param mavlink_log_pub
*/
void update(const float dt, float accel_x, orb_advert_t *mavlink_log_pub);
void update(const float dt, const float accel_x, orb_advert_t *mavlink_log_pub);
/**
* @brief Get the Launch Detected state
@ -80,29 +83,23 @@ public:
/**
* @brief Forces state of launch detection state machine to STATE_FLYING.
*/
void forceSetFlyState() { _state = launch_detection_status_s::STATE_FLYING; }
void forceSetFlyState() { state_ = launch_detection_status_s::STATE_FLYING; }
private:
/**
* Integrator [s]
*/
float _launchDetectionDelayCounter{0.f};
/**
* Motor delay counter [s]
*/
float _motorDelayCounter{0.f};
float motor_delay_counter_{0.f};
/**
* Info delay counter [s]
* Info delay counter (to publish info every kInfoDelay seconds) [s]
*/
float _launchDetectionRunningInfoDelay{4.f};
float _info_delay_counter_s_{kInfoDelay};
/**
* Current state of the launch detection state machine [launch_detection_status_s::launch_detection_state]
*/
uint _state{launch_detection_status_s::STATE_WAITING_FOR_LAUNCH};
uint state_{launch_detection_status_s::STATE_WAITING_FOR_LAUNCH};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::LAUN_CAT_A>) _param_laun_cat_a,