add experimantal acceleration feedback by @kd0aij

2015-01-04 13:28:15 +01:00 · 2015-01-04 13:28:15 +01:00 · edc5f8a057
parent e14023e98f
commit edc5f8a057
1 changed files with 2 additions and 1 deletions
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@ -181,7 +181,8 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl

 	/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
 	if (_coordinated_method == COORD_METHOD_CLOSEACC) {
-		//XXX
+		//XXX: filtering of acceleration?
+		_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
 	}

 	/* Transform estimation to body angular rates (jacobian) */