using jacobians in fw attitude control

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -56,7 +56,7 @@ ECL_PitchController::ECL_PitchController() :
 {
 }
 
-float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
+float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler,
 				   bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
 {
 	/* get the usual dt estimate */
@@ -108,7 +108,8 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 
 	float pitch_error = pitch_setpoint - pitch;
 	/* rate setpoint from current error and time constant */
-	_rate_setpoint = pitch_error / _tc;
+	float theta_dot_setpoint =  pitch_error / _tc;
+	_rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
 
 	/* add turn offset */
 	_rate_setpoint += turn_offset;

src/lib/ecl/attitude_fw/ecl_pitch_controller.h

@@ -56,7 +56,7 @@ class __EXPORT ECL_PitchController
 public:
 	ECL_PitchController();
 
-	float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
+	float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler = 1.0f,
 		      bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
 
 	void reset_integrator();

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -58,7 +58,7 @@ ECL_RollController::ECL_RollController() :
 
 }
 
-float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
+float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, float pitch, float yaw_rate,
 				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
 {
 	/* get the usual dt estimate */
@@ -79,7 +79,8 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	}
 
 	float roll_error = roll_setpoint - roll;
-	_rate_setpoint = roll_error / _tc;
+	float phi_dot_setpoint = roll_error / _tc;
+	_rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian
 
 	/* limit the rate */
 	if (_max_rate > 0.01f) {

src/lib/ecl/attitude_fw/ecl_roll_controller.h

@@ -56,7 +56,7 @@ class __EXPORT ECL_RollController
 public:
 	ECL_RollController();
 
-	float control(float roll_setpoint, float roll, float roll_rate,
+	float control(float roll_setpoint, float roll, float roll_rate,  float pitch, float yaw_rate,
 		      float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
 
 	void reset_integrator();

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -635,11 +635,11 @@ FixedwingAttitudeControl::task_main()
 					}
 				}
 
-				float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
+				float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, _att.pitch, _att.yawspeed,
 								    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
 				_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
 
-				float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
+				float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, _att.yawspeed, airspeed_scaling,
 								      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
 				_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;