fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -61,8 +61,10 @@ ECL_YawController::~ECL_YawController()
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
{
switch (_coordinated_method) {
case 0:
case COORD_METHOD_OPEN:
return control_attitude_impl_openloop(ctl_data);
case COORD_METHOD_CLOSEACC:
return control_attitude_impl_accclosedloop(ctl_data);
default:
static hrt_abstime last_print = 0;
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)
{
switch (_coordinated_method) {
case 0:
case COORD_METHOD_OPEN:
case COORD_METHOD_CLOSEACC:
return control_bodyrate_impl(ctl_data);
default:
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 +
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) */
float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_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);
}
float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data) {
/* dont set a rate setpoint */
return 0.0f;
}

13
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
View File

@ -76,10 +76,19 @@ public:
protected:
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_bodyrate_impl(const struct ECL_ControlData &ctl_data);
float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);
};