Clean 250 Hz updates in filter, partial updates enabled

1 files changed, 41 insertions, 16 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;