From 3c59877b502d2fda0d5a00c0f4ac7d9787e72eaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Sep 2013 09:13:13 +0200 Subject: [PATCH] Naming consistency improved --- .../ecl/attitude_fw/ecl_pitch_controller.h | 2 +- ..._control.cpp => ecl_l1_pos_controller.cpp} | 30 +++++++++---------- ..._pos_control.h => ecl_l1_pos_controller.h} | 10 +++---- src/lib/ecl/module.mk | 2 +- .../fw_pos_control_l1_main.cpp | 4 +-- 5 files changed, 24 insertions(+), 24 deletions(-) rename src/lib/ecl/l1/{ecl_l1_pos_control.cpp => ecl_l1_pos_controller.cpp} (90%) rename src/lib/ecl/l1/{ecl_l1_pos_control.h => ecl_l1_pos_controller.h} (97%) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 6217eb9d0a..3221bd4849 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -40,7 +40,7 @@ * Acknowledgements: * * The control design is based on a design - * by Paul Riseborough, 2013. + * by Paul Riseborough and Andrew Tridgell, 2013. */ #ifndef ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp similarity index 90% rename from src/lib/ecl/l1/ecl_l1_pos_control.cpp rename to src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 4b4e38b0bb..87eb99f16d 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -32,58 +32,58 @@ ****************************************************************************/ /** - * @file ecl_l1_pos_control.h + * @file ecl_l1_pos_controller.h * Implementation of L1 position control. * Authors and acknowledgements in header. * */ -#include "ecl_l1_pos_control.h" +#include "ecl_l1_pos_controller.h" -float ECL_L1_Pos_Control::nav_roll() +float ECL_L1_Pos_Controller::nav_roll() { float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f); return ret; } -float ECL_L1_Pos_Control::nav_lateral_acceleration_demand() +float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand() { return _lateral_accel; } -float ECL_L1_Pos_Control::nav_bearing() +float ECL_L1_Pos_Controller::nav_bearing() { return _wrap_pi(_nav_bearing); } -float ECL_L1_Pos_Control::bearing_error() +float ECL_L1_Pos_Controller::bearing_error() { return _bearing_error; } -float ECL_L1_Pos_Control::target_bearing() +float ECL_L1_Pos_Controller::target_bearing() { return _target_bearing; } -float ECL_L1_Pos_Control::switch_distance(float wp_radius) +float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { /* following [2], switching on L1 distance */ return math::min(wp_radius, _L1_distance); } -bool ECL_L1_Pos_Control::reached_loiter_target(void) +bool ECL_L1_Pos_Controller::reached_loiter_target(void) { return _circle_mode; } -float ECL_L1_Pos_Control::crosstrack_error(void) +float ECL_L1_Pos_Controller::crosstrack_error(void) { return _crosstrack_error; } -void ECL_L1_Pos_Control::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, +void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, const math::Vector2f &ground_speed_vector) { /* this follows the logic presented in [1] */ @@ -175,7 +175,7 @@ void ECL_L1_Pos_Control::navigate_waypoints(const math::Vector2f &vector_A, cons _bearing_error = eta; } -void ECL_L1_Pos_Control::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, +void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, const math::Vector2f &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -265,7 +265,7 @@ void ECL_L1_Pos_Control::navigate_loiter(const math::Vector2f &vector_A, const m } -void ECL_L1_Pos_Control::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -301,7 +301,7 @@ void ECL_L1_Pos_Control::navigate_heading(float navigation_heading, float curren _lateral_accel = 2.0f * sinf(eta) * omega_vel; } -void ECL_L1_Pos_Control::navigate_level_flight(float current_heading) +void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) { /* the logic in this section is trivial, but originally proposed by [2] */ @@ -318,7 +318,7 @@ void ECL_L1_Pos_Control::navigate_level_flight(float current_heading) } -math::Vector2f ECL_L1_Pos_Control::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const { /* this is an approximation for small angles, proposed by [2] */ diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h similarity index 97% rename from src/lib/ecl/l1/ecl_l1_pos_control.h rename to src/lib/ecl/l1/ecl_l1_pos_controller.h index 3ddb691fab..a6a2c01563 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_control.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -57,8 +57,8 @@ * */ -#ifndef ECL_L1_POS_CONTROL_H -#define ECL_L1_POS_CONTROL_H +#ifndef ECL_L1_POS_CONTROLLER_H +#define ECL_L1_POS_CONTROLLER_H #include #include @@ -67,10 +67,10 @@ /** * L1 Nonlinear Guidance Logic */ -class __EXPORT ECL_L1_Pos_Control +class __EXPORT ECL_L1_Pos_Controller { public: - ECL_L1_Pos_Control() { + ECL_L1_Pos_Controller() { _L1_period = 25; _L1_damping = 0.75f; } @@ -246,4 +246,4 @@ private: }; -#endif /* ECL_L1_POS_CONTROL_H */ +#endif /* ECL_L1_POS_CONTROLLER_H */ diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index e8bca3c76a..f2aa3db6a5 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -38,4 +38,4 @@ SRCS = attitude_fw/ecl_pitch_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \ - l1/ecl_l1_pos_control.cpp + l1/ecl_l1_pos_controller.cpp diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3e83f11c8f..d6d135f9f8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -83,7 +83,7 @@ #include #include -#include +#include #include /** @@ -163,7 +163,7 @@ private: bool _global_pos_valid; ///< global position is valid math::Dcm _R_nb; ///< current attitude - ECL_L1_Pos_Control _l1_control; + ECL_L1_Pos_Controller _l1_control; TECS _tecs; struct {