Merged new EKF version

6 files changed, 19 insertions, 17 deletions

diff --git a/apps/multirotor_att_control/Makefile b/apps/multirotor_att_control/Makefile
index 03cf33e43..03cf33e43 100644..100755
--- a/apps/multirotor_att_control/Makefile
+++ b/apps/multirotor_att_control/Makefile
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 7d5821d3f..63c7d0097 100644..100755
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -172,7 +172,7 @@ mc_thread_main(int argc, char *argv[])
 						rates_sp.pitch = offboard_sp.p2;
 						rates_sp.yaw = offboard_sp.p3;
 						rates_sp.thrust = offboard_sp.p4;
-						printf("thrust_rate=%8.4f\n",offboard_sp.p4);
+						//printf("thrust_rate=%8.4f\n",offboard_sp.p4);
 						rates_sp.timestamp = hrt_absolute_time();
 						orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 					} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
@@ -180,7 +180,7 @@ mc_thread_main(int argc, char *argv[])
 						att_sp.pitch_body = offboard_sp.p2;
 						att_sp.yaw_body = offboard_sp.p3;
 						att_sp.thrust = offboard_sp.p4;
-						printf("thrust_att=%8.4f\n",offboard_sp.p4);
+						//printf("thrust_att=%8.4f\n",offboard_sp.p4);
 						att_sp.timestamp = hrt_absolute_time();
 						/* STEP 2: publish the result to the vehicle actuators */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
@@ -196,6 +196,7 @@ mc_thread_main(int argc, char *argv[])
 				rates_sp.pitch = manual.pitch;
 				rates_sp.yaw = manual.yaw;
 				rates_sp.thrust = manual.throttle;
+				//printf("rates\n");
 				rates_sp.timestamp = hrt_absolute_time();
 			}
 
@@ -212,6 +213,7 @@ mc_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
 			if (motor_test_mode) {
+				printf("testmode");
 				att_sp.roll_body = 0.0f;
 				att_sp.pitch_body = 0.0f;
 				att_sp.yaw_body = 0.0f;
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 2129915d1..458b86057 100644..100755
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -238,12 +238,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	
 	/* control pitch (forward) output */
 	float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff,
-					att->pitch, att->pitchspeed, deltaT);
+					att->pitch, att->pitchspeed, deltaT)*1/10.0f;
 	/* control roll (left/right) output */
 	float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
-					att->roll, att->rollspeed, deltaT);
+					att->roll, att->rollspeed, deltaT)*1/10.0f;
 	/* control yaw rate */
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT);
+	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT)*1/10.0f;
 
 	/*
 	 * compensate the vertical loss of thrust
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h
index d9d3c0444..d9d3c0444 100644..100755
--- a/apps/multirotor_att_control/multirotor_attitude_control.h
+++ b/apps/multirotor_att_control/multirotor_attitude_control.h
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index 7532dffa2..2809c4533 100644..100755
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -50,16 +50,16 @@
 #include <systemlib/param/param.h>
 #include <arch/board/up_hrt.h>
 
-// PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.08f); /* same on Flamewheel */
-// PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f);
-// PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f);
-// PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
+ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */
+ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
+ PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
+ PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.f);
 
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */
-PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 8.0f);	/**< roughly < 500 deg/s limit */
+PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 40.0f); /* 0.15 F405 Flamewheel */
+PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.05f);
+PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
+PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 10.0f);	/**< roughly < 500 deg/s limit */
 
 struct mc_rate_control_params {
 
@@ -175,14 +175,14 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	
 	/* control pitch (forward) output */
 
-	float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last);
+	float pitch_control = p.attrate_p * deltaT *((rate_sp->pitch)*p.attrate_lim-rates[1])-p.attrate_d*(pitch_control_last);
 	pitch_control_last=pitch_control;
 	/* control roll (left/right) output */
 
-	float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last);
+	float roll_control = p.attrate_p * deltaT * ((rate_sp->roll)*p.attrate_lim-rates[0])-p.attrate_d*(roll_control_last);
 	roll_control_last=roll_control;
 	/* control yaw rate */
-	float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2]  );
+	float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)*p.attrate_lim-rates[2]  );
 
 	actuators->control[0] = roll_control;
 	actuators->control[1] = pitch_control;
diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h
index 3c3c50801..3c3c50801 100644..100755
--- a/apps/multirotor_att_control/multirotor_rate_control.h
+++ b/apps/multirotor_att_control/multirotor_rate_control.h