Clean 250 Hz updates in filter, partial updates enabled

4 files changed, 82 insertions, 21 deletions

diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index 295cc4b54..6368c5157 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -161,7 +161,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		thread_should_exit = false;
 		attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
 							 SCHED_RR,
-							 SCHED_PRIORITY_DEFAULT,
+							 SCHED_PRIORITY_MAX - 5,
 							 20000,
 							 attitude_estimator_ekf_thread_main,
 							 (argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -221,7 +221,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
 	/* rate-limit raw data updates to 200Hz */
-	orb_set_interval(sub_raw, 5);
+	orb_set_interval(sub_raw, 4);
 
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -236,6 +236,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
 	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
 
+	/* keep track of sensor updates */
+	uint32_t sensor_last_count[3] = {0, 0, 0};
+	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
+	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
 
 	/* process noise covariance */
 	float q[12];
@@ -264,17 +268,38 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			/* Calculate data time difference in seconds */
 			dt = (raw.timestamp - last_measurement) / 1000000.0f;
 			last_measurement = raw.timestamp;
+			uint8_t update_vect[3] = {0, 0, 0};
 
 			/* Fill in gyro measurements */
+			if (sensor_last_count[0] != raw.gyro_counter) {
+				update_vect[0] = 1;
+				sensor_last_count[0] = raw.gyro_counter;
+				sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
+				sensor_last_timestamp[0] = raw.timestamp;
+			}
+
 			z_k[0] =  raw.gyro_rad_s[0];
 			z_k[1] =  raw.gyro_rad_s[1];
 			z_k[2] =  raw.gyro_rad_s[2];
 
-			/* scale from 14 bit to m/s2 */
+			/* update accelerometer measurements */
+			if (sensor_last_count[1] != raw.accelerometer_counter) {
+				update_vect[1] = 1;
+				sensor_last_count[1] = raw.accelerometer_counter;
+				sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
+				sensor_last_timestamp[1] = raw.timestamp;
+			}
 			z_k[3] = raw.accelerometer_m_s2[0];
 			z_k[4] = raw.accelerometer_m_s2[1];
 			z_k[5] = raw.accelerometer_m_s2[2];
 
+			/* update magnetometer measurements */
+			if (sensor_last_count[2] != raw.magnetometer_counter) {
+				update_vect[2] = 1;
+				sensor_last_count[2] = raw.magnetometer_counter;
+				sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
+				sensor_last_timestamp[2] = raw.timestamp;
+			}
 			z_k[6] = raw.magnetometer_ga[0];
 			z_k[7] = raw.magnetometer_ga[1];
 			z_k[8] = raw.magnetometer_ga[2];
@@ -293,7 +318,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 				overloadcounter++;
 			}
 
-			uint8_t update_vect[3] = {1, 1, 1};
 			int32_t z_k_sizes = 9;
 			// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
 
@@ -361,21 +385,22 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			uint64_t timing_diff = hrt_absolute_time() - timing_start;
 
 			// /* print rotation matrix every 200th time */
-			// if (printcounter % 200 == 0) {
-			// 	// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
-			// 	// 	x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
-			// 	// 	x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
-			// 	// 	x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
+			if (printcounter % 200 == 0) {
+				// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
+				// 	x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
+				// 	x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
+				// 	x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
 
 
-			// 	// }
+				// }
 
-			// 	printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
-			// 	printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
-			// // 	printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
-			// // 	       (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
-			// // 	       (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
-			// }
+				printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
+				printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
+				printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]);
+			// 	printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
+			// 	       (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
+			// 	       (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
+			}
 
 			// 				int i = printcounter % 9;
 
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 9acdc6fd3..8bd9ebc96 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -110,7 +110,7 @@ static void *rate_control_thread_main(void *arg)
 			/* XXX this is seriously bad - should be an emergency */
 		} else if (ret == 0) {
 			/* XXX this means no sensor data - should be critical or emergency */
-			printf("[mc att control] WARNING: Not getting gyro data, no rate control\n");
+			//printf("[mc att control] WARNING: Not getting gyro data, no rate control\n");
 		} else {
 			/* get data */
 			orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report);
@@ -354,7 +354,7 @@ int multirotor_att_control_main(int argc, char *argv[])
 		mc_task = task_spawn("multirotor_att_control",
 				     SCHED_RR,
 				     SCHED_PRIORITY_MAX - 15,
-				     2048,
+				     4096,
 				     mc_thread_main,
 				     NULL);
 		exit(0);
diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c
index e061256fc..f8390f100 100644
--- a/apps/position_estimator/position_estimator_main.c
+++ b/apps/position_estimator/position_estimator_main.c
@@ -261,6 +261,7 @@ int position_estimator_main(int argc, char *argv[])
 
 	/* subscribe to vehicle status, attitude, gps */
 	struct vehicle_gps_position_s gps;
+	gps.fix_type = 0;
 	struct vehicle_status_s vstatus;
 	struct vehicle_attitude_s att;
 
@@ -283,8 +284,8 @@ int position_estimator_main(int argc, char *argv[])
 			/* Wait for the GPS update to propagate (we have some time) */
 			usleep(5000);
 			/* Read wether the vehicle status changed */
-			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
-			gps_valid = vstatus.gps_valid;
+			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
+			gps_valid = (gps.fix_type > 2);
 		}
 	}
 
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index f4af17597..0426dfb9c 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -60,6 +60,9 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
 
 #include <systemlib/systemlib.h>
 
@@ -273,6 +276,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		struct actuator_outputs_s act_outputs;
 		struct actuator_controls_s act_controls;
 		struct vehicle_command_s cmd;
+		struct vehicle_local_position_s local_pos;
+		struct vehicle_global_position_s global_pos;
+		struct vehicle_gps_position_s gps_pos;
 	} buf;
 	memset(&buf, 0, sizeof(buf));
 
@@ -283,6 +289,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		int spa_sub;
 		int act_0_sub;
 		int controls0_sub;
+		int local_pos_sub;
+		int global_pos_sub;
+		int gps_pos_sub;
 	} subs;
 
 	/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -329,6 +338,27 @@ int sdlog_thread_main(int argc, char *argv[]) {
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- LOCAL POSITION --- */
+	/* subscribe to ORB for local position */
+	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	fds[fdsc_count].fd = subs.local_pos_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
+	/* --- GLOBAL POSITION --- */
+	/* subscribe to ORB for global position */
+	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	fds[fdsc_count].fd = subs.global_pos_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
+	/* --- GPS POSITION --- */
+	/* subscribe to ORB for global position */
+	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	fds[fdsc_count].fd = subs.gps_pos_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -435,6 +465,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		/* copy actuator data into local buffer */
 		orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
 		orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
+		orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
+		orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
+		orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
 
 		#pragma pack(push, 1)
 		struct {
@@ -450,6 +483,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
 			float actuators[8];
 			float vbat;
 			float adc[3];
+			float local_pos[3];
 		} sysvector = {
 			.timestamp = buf.raw.timestamp,
 			.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
@@ -462,7 +496,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
 			.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
 					buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
 			.vbat = buf.raw.battery_voltage_v,
-			.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}
+			.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
+			.local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}
 		};
 		#pragma pack(pop)