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

attitude_fw fix style

Browse Source
This commit is contained in:
Daniel Agar
2017-02-12 11:40:14 +01:00
committed by Lorenz Meier
parent f22ad30ef3
commit 674ab3f81b
3 changed files with 25 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
attitude_fw/ecl_wheel_controller.cpp
+15 -15
View File
@@ -54,12 +54,12 @@ ECL_WheelController::ECL_WheelController() :
float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.groundspeed) &&
PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.groundspeed) &&
PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
@@ -76,8 +76,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
/* input conditioning */
float min_speed = 1.0f;
/* Calculate body angular rate error */
_rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error
/* Calculate body angular rate error */
_rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
@@ -102,9 +102,9 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + integrator_constrained);
/* Apply PI rate controller and store non-limited output */
_last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + integrator_constrained);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -118,10 +118,10 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da
return _rate_setpoint;
}
/* Calculate the error */
float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
/* shortest angle (wrap around) */
yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
/* Calculate the error */
float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
/* shortest angle (wrap around) */
yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = yaw_error / _tc;
attitude_fw/ecl_wheel_controller.h
+3 -3
View File
@@ -61,11 +61,11 @@ public:
ECL_WheelController();
~ECL_WheelController() = default;
float control_attitude(const struct ECL_ControlData &ctl_data);
float control_attitude(const struct ECL_ControlData &ctl_data);
float control_bodyrate(const struct ECL_ControlData &ctl_data);
float control_bodyrate(const struct ECL_ControlData &ctl_data);
float control_euler_rate(const struct ECL_ControlData &ctl_data) {return 0;};
float control_euler_rate(const struct ECL_ControlData &ctl_data) {return 0;};
};
#endif // ECL_HEADING_CONTROLLER_H
attitude_fw/ecl_yaw_controller.cpp
+7 -3
View File
@@ -91,6 +91,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
float constrained_roll;
bool inverted = false;
/* roll is used as feedforward term and inverted flight needs to be considered */
if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
/* not inverted, but numerically still potentially close to infinity */
@@ -98,8 +99,9 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
} else {
inverted = true;
// inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity
//note: the ranges are extended by 10 deg here to avoid numeric resolution effects
// inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity
//note: the ranges are extended by 10 deg here to avoid numeric resolution effects
if (ctl_data.roll > 0.0f) {
/* right hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
@@ -115,10 +117,12 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
if (!inverted) {
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * 9.81f / (ctl_data.airspeed < ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * 9.81f / (ctl_data.airspeed < ctl_data.airspeed_min ?
ctl_data.airspeed_min : ctl_data.airspeed);
}
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;