fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-22 00:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw yaw: add enum for method selection

Browse Source
This commit is contained in:
Thomas Gubler
2015-01-04 13:15:23 +01:00
committed by Lorenz Meier
parent 53ffde30c2
commit d294046245
2 changed files with 26 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+15 -2
View File
@@ -61,8 +61,10 @@ ECL_YawController::~ECL_YawController()
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data) float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
{ {
switch (_coordinated_method) { switch (_coordinated_method) {
case 0: case COORD_METHOD_OPEN:
return control_attitude_impl_openloop(ctl_data); return control_attitude_impl_openloop(ctl_data);
case COORD_METHOD_CLOSEACC:
return control_attitude_impl_accclosedloop(ctl_data);
default: default:
static hrt_abstime last_print = 0; static hrt_abstime last_print = 0;
if (hrt_elapsed_time(&last_print) > 5e6) { if (hrt_elapsed_time(&last_print) > 5e6) {
@@ -76,7 +78,8 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data) float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{ {
switch (_coordinated_method) { switch (_coordinated_method) {
case 0: case COORD_METHOD_OPEN:
case COORD_METHOD_CLOSEACC:
return control_bodyrate_impl(ctl_data); return control_bodyrate_impl(ctl_data);
default: default:
static hrt_abstime last_print = 0; static hrt_abstime last_print = 0;
@@ -176,6 +179,11 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
cosf(ctl_data.roll)*cosf(ctl_data.pitch) * _rate_setpoint; cosf(ctl_data.roll)*cosf(ctl_data.pitch) * _rate_setpoint;
/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
if (_coordinated_method == COORD_METHOD_CLOSEACC) {
//XXX
}
/* Transform estimation to body angular rates (jacobian) */ /* Transform estimation to body angular rates (jacobian) */
float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate + float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate +
cosf(ctl_data.roll)*cosf(ctl_data.pitch) * ctl_data.yaw_rate; cosf(ctl_data.roll)*cosf(ctl_data.pitch) * ctl_data.yaw_rate;
@@ -212,3 +220,8 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
return math::constrain(_last_output, -1.0f, 1.0f); return math::constrain(_last_output, -1.0f, 1.0f);
} }
float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data) {
/* dont set a rate setpoint */
return 0.0f;
}
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+11 -2
View File
@@ -76,10 +76,19 @@ public:
protected: protected:
float _coordinated_min_speed; float _coordinated_min_speed;
int32_t _coordinated_method;;
enum {
COORD_METHOD_OPEN = 0,
COORD_METHOD_CLOSEACC = 1,
};
int32_t _coordinated_method;
float control_bodyrate_impl(const struct ECL_ControlData &ctl_data);
float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data); float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data);
float control_bodyrate_impl(const struct ECL_ControlData &ctl_data);
float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);
}; };