Browse Source

Naming consistency improved

sbg
Lorenz Meier 12 years ago
parent
commit
3c59877b50
  1. 2
      src/lib/ecl/attitude_fw/ecl_pitch_controller.h
  2. 30
      src/lib/ecl/l1/ecl_l1_pos_controller.cpp
  3. 10
      src/lib/ecl/l1/ecl_l1_pos_controller.h
  4. 2
      src/lib/ecl/module.mk
  5. 4
      src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

2
src/lib/ecl/attitude_fw/ecl_pitch_controller.h

@ -40,7 +40,7 @@
* Acknowledgements: * Acknowledgements:
* *
* The control design is based on a design * The control design is based on a design
* by Paul Riseborough, 2013. * by Paul Riseborough and Andrew Tridgell, 2013.
*/ */
#ifndef ECL_PITCH_CONTROLLER_H #ifndef ECL_PITCH_CONTROLLER_H

30
src/lib/ecl/l1/ecl_l1_pos_control.cpp → 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. * Implementation of L1 position control.
* Authors and acknowledgements in header. * 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); float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f); ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f);
return ret; return ret;
} }
float ECL_L1_Pos_Control::nav_lateral_acceleration_demand() float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand()
{ {
return _lateral_accel; return _lateral_accel;
} }
float ECL_L1_Pos_Control::nav_bearing() float ECL_L1_Pos_Controller::nav_bearing()
{ {
return _wrap_pi(_nav_bearing); return _wrap_pi(_nav_bearing);
} }
float ECL_L1_Pos_Control::bearing_error() float ECL_L1_Pos_Controller::bearing_error()
{ {
return _bearing_error; return _bearing_error;
} }
float ECL_L1_Pos_Control::target_bearing() float ECL_L1_Pos_Controller::target_bearing()
{ {
return _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 */ /* following [2], switching on L1 distance */
return math::min(wp_radius, _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; return _circle_mode;
} }
float ECL_L1_Pos_Control::crosstrack_error(void) float ECL_L1_Pos_Controller::crosstrack_error(void)
{ {
return _crosstrack_error; 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) const math::Vector2f &ground_speed_vector)
{ {
/* this follows the logic presented in [1] */ /* 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; _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) const math::Vector2f &ground_speed_vector)
{ {
/* the complete guidance logic in this section was proposed by [2] */ /* 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] */ /* 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; _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] */ /* 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] */ /* this is an approximation for small angles, proposed by [2] */

10
src/lib/ecl/l1/ecl_l1_pos_control.h → src/lib/ecl/l1/ecl_l1_pos_controller.h

@ -57,8 +57,8 @@
* *
*/ */
#ifndef ECL_L1_POS_CONTROL_H #ifndef ECL_L1_POS_CONTROLLER_H
#define ECL_L1_POS_CONTROL_H #define ECL_L1_POS_CONTROLLER_H
#include <mathlib/mathlib.h> #include <mathlib/mathlib.h>
#include <geo/geo.h> #include <geo/geo.h>
@ -67,10 +67,10 @@
/** /**
* L1 Nonlinear Guidance Logic * L1 Nonlinear Guidance Logic
*/ */
class __EXPORT ECL_L1_Pos_Control class __EXPORT ECL_L1_Pos_Controller
{ {
public: public:
ECL_L1_Pos_Control() { ECL_L1_Pos_Controller() {
_L1_period = 25; _L1_period = 25;
_L1_damping = 0.75f; _L1_damping = 0.75f;
} }
@ -246,4 +246,4 @@ private:
}; };
#endif /* ECL_L1_POS_CONTROL_H */ #endif /* ECL_L1_POS_CONTROLLER_H */

2
src/lib/ecl/module.mk

@ -38,4 +38,4 @@
SRCS = attitude_fw/ecl_pitch_controller.cpp \ SRCS = attitude_fw/ecl_pitch_controller.cpp \
attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \
attitude_fw/ecl_yaw_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \
l1/ecl_l1_pos_control.cpp l1/ecl_l1_pos_controller.cpp

4
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@ -83,7 +83,7 @@
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <mathlib/mathlib.h> #include <mathlib/mathlib.h>
#include <ecl/l1/ecl_l1_pos_control.h> #include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h> #include <external_lgpl/tecs/tecs.h>
/** /**
@ -163,7 +163,7 @@ private:
bool _global_pos_valid; ///< global position is valid bool _global_pos_valid; ///< global position is valid
math::Dcm _R_nb; ///< current attitude math::Dcm _R_nb; ///< current attitude
ECL_L1_Pos_Control _l1_control; ECL_L1_Pos_Controller _l1_control;
TECS _tecs; TECS _tecs;
struct { struct {

Loading…
Cancel
Save