Merge branch 'daregger_rate_control' of github.com:PX4/Firmware into calibration

12 files changed, 112 insertions, 107 deletions

diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index 8d4a473b6..833425aa6 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -286,7 +286,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 
 	/* close uarts */
 	close(ardrone_write);
-	//ar_multiplexing_deinit(gpios);
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
 	ardrone_write = ardrone_open_uart(device, &uart_config_original);
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index a291a4914..7bb8490e5 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -71,11 +71,10 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
 
 static unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
-static float dt = 1.0f;
+static float dt = 0.005f;
 /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
-static float z_k[9];					/**< Measurement vector */
-static float x_aposteriori_k[12];		/**< */
-static float x_aposteriori[12];
+static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f};					/**< Measurement vector */
+static float x_aposteriori_k[12];		/**< states */
 static float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
 		0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
 		0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
@@ -88,21 +87,10 @@ static float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0
 		0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
 		0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
 		0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
-};
+}; /**< init: diagonal matrix with big values */
 
-static float P_aposteriori[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-		0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-		0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-		0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,
-		0,   0,   0,   0,  100.f,  0,   0,   0,   0,   0,   0,   0,
-		0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
-		0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
-		0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,
-		0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
-		0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
-		0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
-		0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
-};	/**< init: diagonal matrix with big values */
+static float x_aposteriori[12];
+static float P_aposteriori[144];
 
 /* output euler angles */
 static float euler[3] = {0.0f, 0.0f, 0.0f};
@@ -236,7 +224,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 	/* advertise debug value */
 	struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
-	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
+	orb_advert_t pub_dbg = -1;
 
 	/* keep track of sensor updates */
 	uint32_t sensor_last_count[3] = {0, 0, 0};
@@ -293,13 +281,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					gyro_offsets[0] += raw.gyro_rad_s[0];
 					gyro_offsets[1] += raw.gyro_rad_s[1];
 					gyro_offsets[2] += raw.gyro_rad_s[2];
-					offset_count+=1;
+					offset_count++;
+
 					if (hrt_absolute_time() - start_time > 3000000LL) {
 						initialized = true;
 						gyro_offsets[0] /= offset_count;
 						gyro_offsets[1] /= offset_count;
 						gyro_offsets[2] /= offset_count;
-						printf("pipapo %d\n",(int)(gyro_offsets[2]*1000) );
 					}
 				} else {
 
@@ -316,9 +304,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
-					z_k[0] =  raw.gyro_rad_s[0]-gyro_offsets[0];
-					z_k[1] =  raw.gyro_rad_s[1]-gyro_offsets[1];
-					z_k[2] =  raw.gyro_rad_s[2]-gyro_offsets[2];
+					z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
+					z_k[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
+					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
 					if (sensor_last_count[1] != raw.accelerometer_counter) {
@@ -362,7 +350,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					static bool const_initialized = false;
 
 					/* initialize with good values once we have a reasonable dt estimate */
-					if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/)
+					if (!const_initialized && dt < 0.05 && dt > 0.005)
 					{
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
@@ -388,16 +376,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						continue;
 					}
 
-					dt = 0.004f;
-
 					uint64_t timing_start = hrt_absolute_time();
-					// attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
-					// 	Rot_matrix, x_aposteriori, P_aposteriori);
+					
 					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
 							euler, Rot_matrix, x_aposteriori, P_aposteriori);
-					/* swap values for next iteration */
-					memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
-					memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
+					/* swap values for next iteration, check for fatal inputs */
+					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
+						memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
+						memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
+					} else {
+						/* due to inputs or numerical failure the output is invalid, skip it */
+						continue;
+					}
+					
 					uint64_t timing_diff = hrt_absolute_time() - timing_start;
 
 					// /* print rotation matrix every 200th time */
@@ -425,7 +416,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					// 		sprintf(name, "xapo #%d", i);
 					// 		memcpy(dbg.key, name, sizeof(dbg.key));
 					// 		dbg.value = x_aposteriori[i];
+					// if (pub_dbg < 0) {
+							// pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
+					// } else {
 					// 		orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
+					// }
 
 					printcounter++;
 
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index f20d1b204..4fc2e0452 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -61,9 +61,9 @@ PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f);
 PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f);
 
 /* gyro measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f);
 /* accelerometer measurement noise */
 PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f);
 PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f);
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
index 885c01bf3..04f6ea267 100644
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
@@ -50,7 +50,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
       b_u1 = -1;

     }

 

-    y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);

+    y = (real32_T)atan2f((real32_T)b_u0, (real32_T)b_u1);

   } else if (u1 == 0.0F) {

     if (u0 > 0.0F) {

       y = RT_PIF / 2.0F;

@@ -60,7 +60,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
       y = 0.0F;

     }

   } else {

-    y = (real32_T)atan2(u0, u1);

+    y = (real32_T)atan2f(u0, u1);

   }

 

   return y;

@@ -776,12 +776,12 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
     Rot_matrix[6 + i] = z_n_b[i];

   }

 

-  /* 'attitudeKalmanfilter:193' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */

-  /* 'attitudeKalmanfilter:194' theta=-asin(Rot_matrix(1,3)); */

-  /* 'attitudeKalmanfilter:195' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */

+  /* 'attitudeKalmanfilter:193' phi=atan2f(Rot_matrix(2,3),Rot_matrix(3,3)); */

+  /* 'attitudeKalmanfilter:194' theta=-asinf(Rot_matrix(1,3)); */

+  /* 'attitudeKalmanfilter:195' psi=atan2f(Rot_matrix(1,2),Rot_matrix(1,1)); */

   /* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */

   eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);

-  eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);

+  eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);

   eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);

 }

 

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index f11c583d4..545569a65 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -292,10 +292,16 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	struct sensor_combined_s raw;
 
 	/* 30 seconds */
-	uint64_t calibration_interval = 30 * 1000 * 1000;
+	uint64_t calibration_interval = 45 * 1000 * 1000;
 	unsigned int calibration_counter = 0;
 
-	float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN};
+	/*
+	 * FLT_MIN is not the most negative float number,
+	 * but the smallest number by magnitude float can
+	 * represent. Use -FLT_MAX to initialize the most
+	 * negative number
+	 */
+	float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
 	float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
 
 	int fd = open(MAG_DEVICE_PATH, 0);
@@ -322,8 +328,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	uint64_t axis_deadline = hrt_absolute_time();
 	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
 
-	const char axislabels[3] = { 'X', 'Y', 'Z'};
-	int axis_index = 0;
+	const char axislabels[3] = { 'Z', 'X', 'Y'};
+	int axis_index = -1;
 
 	while (hrt_absolute_time() < calibration_deadline) {
 
@@ -331,18 +337,21 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
 
 		/* user guidance */
-		if (hrt_absolute_time() > axis_deadline &&
+		if (hrt_absolute_time() >= axis_deadline &&
 			axis_index < 3) {
+
+			axis_index++;
+
 			char buf[50];
 			sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]);
 			mavlink_log_info(mavlink_fd, buf);
+			ioctl(buzzer, TONE_SET_ALARM, 2);
 		
 			axis_deadline += calibration_interval / 3;
-			axis_index++;
 		}
 
 		if (!(axis_index < 3)) {
-			continue;
+			break;
 		}
 
 		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
@@ -358,26 +367,26 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 			/* get min/max values */
 
 			/* ignore other axes */
-			if (raw.magnetometer_ga[axis_index] < mag_min[axis_index]) {
-				mag_min[axis_index] = raw.magnetometer_ga[axis_index];
+			if (raw.magnetometer_ga[0] < mag_min[0]) {
+				mag_min[0] = raw.magnetometer_ga[0];
 			}
-			else if (raw.magnetometer_ga[axis_index] > mag_max[axis_index]) {
-				mag_max[axis_index] = raw.magnetometer_ga[axis_index];
+			else if (raw.magnetometer_ga[0] > mag_max[0]) {
+				mag_max[0] = raw.magnetometer_ga[0];
 			}
 
-			// if (raw.magnetometer_ga[1] < mag_min[1]) {
-			// 	mag_min[1] = raw.magnetometer_ga[1];
-			// }
-			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
-			// 	mag_max[1] = raw.magnetometer_ga[1];
-			// }
+			if (raw.magnetometer_ga[1] < mag_min[1]) {
+				mag_min[1] = raw.magnetometer_ga[1];
+			}
+			else if (raw.magnetometer_ga[1] > mag_max[1]) {
+				mag_max[1] = raw.magnetometer_ga[1];
+			}
 
-			// if (raw.magnetometer_ga[2] < mag_min[2]) {
-			// 	mag_min[2] = raw.magnetometer_ga[2];
-			// }
-			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
-			// 	mag_max[2] = raw.magnetometer_ga[2];
-			// }
+			if (raw.magnetometer_ga[2] < mag_min[2]) {
+				mag_min[2] = raw.magnetometer_ga[2];
+			}
+			else if (raw.magnetometer_ga[2] > mag_max[2]) {
+				mag_max[2] = raw.magnetometer_ga[2];
+			}
 
 			calibration_counter++;
 		} else {
@@ -840,7 +849,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
 					mavlink_log_info(mavlink_fd, "[commander] CMD starting accel calibration");
+					ioctl(buzzer, TONE_SET_ALARM, 2);
 					do_accel_calibration(status_pub, &current_status);
+					ioctl(buzzer, TONE_SET_ALARM, 2);
 					mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration");
 					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
 					result = MAV_RESULT_ACCEPTED;
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index e893ea951..293bbe00c 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -1,4 +1,4 @@
-/****************************************************************************
+ /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
@@ -576,10 +576,10 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
 	} else if (baudrate >= 57600) {
 		/* 10 Hz / 100 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 100);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200);
 		/* 10 Hz / 100 ms ATTITUDE */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
 		/* 5 Hz / 200 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
 		/* 5 Hz / 200 ms */
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index ad46c3ede..15ed70dbd 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -225,28 +225,20 @@ handle_message(mavlink_message_t *msg)
 			uint8_t ml_mode = 0;
 			bool ml_armed = false;
 
-//			if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) {
-//				ml_armed = true;
-//			}
-
 			switch (quad_motors_setpoint.mode) {
 				case 0:
 					ml_armed = false;
-
 					break;
 				case 1:
-
 					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
 					ml_armed = true;
 
-				break;
+					break;
 				case 2:
-
-
 					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
 					ml_armed = true;
 
-				break;
+					break;
 				case 3:
 					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
 					break;
@@ -259,7 +251,6 @@ handle_message(mavlink_message_t *msg)
 			offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid]  / (float)INT16_MAX;
 			offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid]    / (float)INT16_MAX;
 			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX;
-			//offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid] ;
 
 			if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){
 				ml_armed = false;
@@ -487,6 +478,11 @@ receive_start(int uart)
 {
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
+
+	struct sched_param param;
+  	param.sched_priority = SCHED_PRIORITY_MAX - 40;
+  	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
+
 	pthread_attr_setstacksize(&receiveloop_attr, 2048);
 
 	pthread_t thread;
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index 39358922d..c2428874f 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -141,12 +141,6 @@ l_sensor_combined(struct listener *l)
 
 	last_sensor_timestamp = raw.timestamp;
 
-	/* send raw imu data */
-	// mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, raw.accelerometer_raw[0],
-	// 	raw.accelerometer_raw[1], raw.accelerometer_raw[2], raw.gyro_raw[0],
-	// 	raw.gyro_raw[1], raw.gyro_raw[2], raw.magnetometer_raw[0],
-	// 	raw.magnetometer_raw[1], raw.magnetometer_raw[2]);
-
 	/* mark individual fields as changed */
 	uint16_t fields_updated = 0;
 	static unsigned accel_counter = 0;
@@ -161,19 +155,19 @@ l_sensor_combined(struct listener *l)
 	}
 
 	if (gyro_counter != raw.gyro_counter) {
-		/* mark first three dimensions as changed */
+		/* mark second group dimensions as changed */
 		fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
 		gyro_counter = raw.gyro_counter;
 	}
 
 	if (mag_counter != raw.magnetometer_counter) {
-		/* mark first three dimensions as changed */
+		/* mark third group dimensions as changed */
 		fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
 		mag_counter = raw.magnetometer_counter;
 	}
 
 	if (baro_counter != raw.baro_counter) {
-		/* mark first three dimensions as changed */
+		/* mark last group dimensions as changed */
 		fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
 		baro_counter = raw.baro_counter;
 	}
@@ -187,8 +181,6 @@ l_sensor_combined(struct listener *l)
 		raw.baro_pres_mbar, 0 /* no diff pressure yet */,
 		raw.baro_alt_meter, raw.baro_temp_celcius,
 		fields_updated);
-	/* send pressure */
-	//mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, raw.timestamp / 1000, raw.baro_pres_mbar, raw.baro_alt_meter, raw.baro_temp_celcius * 100);
 
 	sensors_raw_counter++;
 }
@@ -559,12 +551,12 @@ uorb_receive_start(void)
 	/* --- SENSORS RAW VALUE --- */
 	mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	/* rate limit set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.sensor_sub, 100);	/* 10Hz updates */
+	orb_set_interval(mavlink_subs.sensor_sub, 200);	/* 5Hz updates */
 
 	/* --- ATTITUDE VALUE --- */
 	mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	/* rate limit set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.att_sub, 100);	/* 10Hz updates */
+	orb_set_interval(mavlink_subs.att_sub, 200);	/* 5Hz updates */
 
 	/* --- GPS VALUE --- */
 	mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -631,8 +623,7 @@ uorb_receive_start(void)
 	pthread_attr_init(&uorb_attr);
 
 	/* Set stack size, needs more than 8000 bytes */
-	/* XXX verify, should not need anything like this much unless MAVLink really sucks */
-	pthread_attr_setstacksize(&uorb_attr, 8192);
+	pthread_attr_setstacksize(&uorb_attr, 4096);
 
 	pthread_t thread;
 	pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index cb82036fe..dfb778fd0 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -204,9 +204,7 @@ mc_thread_main(int argc, char *argv[])
 			if (state.flag_control_attitude_enabled) {
 				att_sp.roll_body = manual.roll;
 				att_sp.pitch_body = manual.pitch;
-				att_sp.yaw_body = manual.yaw; // XXX Hack, clean up
-				/* set yaw rate */
-				rates_sp.yaw = manual.yaw;
+				att_sp.yaw_body = att.yaw + manual.yaw * -2.0f;
 				att_sp.thrust = manual.throttle;
 				att_sp.timestamp = hrt_absolute_time();
 			}
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 00b759d73..c3da85c5c 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -235,20 +235,21 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		//		pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu);
 		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
 		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
-		printf("delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
+		printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
 	}
 
 	/* calculate current control outputs */
 
 	/* control pitch (forward) output */
 	rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff,
-			att->pitch, att->pitchspeed, deltaT)*1/10.0f;
+			att->pitch, att->pitchspeed, deltaT);
+
 	/* control roll (left/right) output */
 	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
-			att->roll, att->rollspeed, deltaT)*1/10.0f;
+			att->roll, att->rollspeed, deltaT);
+
 	/* control yaw rate */
-	//float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT)*1/10.0f;
-	rates_sp->yaw= deltaT*p.yaw_p*atan2f(sinf(att->yaw-att_sp->yaw_body),cos(att->yaw-att_sp->yaw_body));
+	rates_sp->yaw= deltaT * p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body));
 
 
 
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index c132665d2..7401ef0f6 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -52,6 +52,7 @@
 #include <math.h>
 #include <systemlib/pid/pid.h>
 #include <systemlib/param/param.h>
+#include <systemlib/err.h>
 #include <arch/board/up_hrt.h>
 
 PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */
@@ -153,6 +154,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	static float pitch_control_last = 0;
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+	static uint64_t last_input = 0;
+
+	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
+	if (last_input != rate_sp->timestamp) {
+		last_input = rate_sp->timestamp;
+	}
+
 	last_run = hrt_absolute_time();
 
 	static int motor_skip_counter = 0;
@@ -173,31 +181,36 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	if (motor_skip_counter % 2500 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		printf("p.yawrate_p: %8.4f\n", (double)p.yawrate_p);
+		warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz\n",
+			(double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
 	}
 
 	/* calculate current control outputs */
 	
 	/* control pitch (forward) output */
-	float pitch_control = p.attrate_p * deltaT *((rate_sp->pitch)*p.attrate_lim-rates[1])-p.attrate_d*(pitch_control_last);
+	// XXX fix this line so that deltaT is also applied to the D term
+	float pitch_control = p.attrate_p * deltaT * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last);
 	/* increase resilience to faulty control inputs */
 	if (isfinite(pitch_control)) {
 		pitch_control_last = pitch_control;
 	} else {
 		pitch_control = 0.0f;
+		warnx("rej. NaN ctrl pitch\n");
 	}
 
 	/* control roll (left/right) output */
-	float roll_control = p.attrate_p * deltaT * ((rate_sp->roll)*p.attrate_lim-rates[0])-p.attrate_d*(roll_control_last);
+	// XXX fix this line so that deltaT is also applied to the D term
+	float roll_control = p.attrate_p * deltaT * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last);
 	/* increase resilience to faulty control inputs */
 	if (isfinite(roll_control)) {
 		roll_control_last = roll_control;
 	} else {
 		roll_control = 0.0f;
+		warnx("rej. NaN ctrl roll\n");
 	}
 
 	/* control yaw rate */
-	float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)*p.attrate_lim-rates[2]);
+	float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)-rates[2]);
 
 	actuators->control[0] = roll_control;
 	actuators->control[1] = pitch_control;
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 9a90d7fc5..3e204662a 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -1004,6 +1004,7 @@ Sensors::task_main()
 	 * do advertisements
 	 */
 	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
 	raw.timestamp = hrt_absolute_time();
 	raw.battery_voltage_v = BAT_VOL_INITIAL;
 	raw.adc_voltage_v[0] = 0.9f;