From df1cd4f14775844d4bdfe991ae9ae79e24aba0af Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 9 Nov 2022 16:21:09 +0100 Subject: [PATCH] LaunchDetection: code style changes and fix info message. Signed-off-by: Silvan Fuhrer --- .../launchdetection/LaunchDetector.cpp | 40 +++++++++---------- .../launchdetection/LaunchDetector.h | 21 +++++----- 2 files changed, 29 insertions(+), 32 deletions(-) diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp index 43e055616c..f6e364841b 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp @@ -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(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; } diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h index 717db6afca..d8ab570a08 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h @@ -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) _param_laun_cat_a,