mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-03 04:31:36 +08:00
Naming consistency improved
This commit is contained in:
@@ -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
|
||||||
|
|||||||
@@ -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] */
|
||||||
|
|
||||||
@@ -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 */
|
||||||
@@ -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
|
||||||
|
|||||||
@@ -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 {
|
||||||
|
|||||||
Reference in New Issue
Block a user