HIL fixed, fixedwing control fixes

1 files changed, 56 insertions, 49 deletions

diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
index 8551b5e1c..af7ad7187 100644
--- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
@@ -156,87 +156,96 @@ int attitude_estimator_bm_main(int argc, char *argv[])
 
 	unsigned int loopcounter = 0;
 
+	uint64_t last_checkstate_stamp = 0;
+
 	/* Main loop*/
 	while (true) {
 
+
 		/* wait for sensor update */
-		int ret = poll(fds, 1, 5000);
+		int ret = poll(fds, 1, 1000);
 
 		if (ret < 0) {
 			/* 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("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
-			continue;
-		}
+		} else {
+			orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local);
 
-		orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local);
+			uint64_t now = hrt_absolute_time();
+			unsigned int time_elapsed = now - last_run;
+			last_run = now;
 
-		uint64_t now = hrt_absolute_time();
-		unsigned int time_elapsed = now - last_run;
-		last_run = now;
+			//#if 0
 
-//#if 0
+			if (time_elapsed > loop_interval_alarm) {
+				//TODO: add warning, cpu overload here
+				if (overloadcounter == 20) {
+					printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
+					overloadcounter = 0;
+				}
 
-		if (time_elapsed > loop_interval_alarm) {
-			//TODO: add warning, cpu overload here
-			if (overloadcounter == 20) {
-				printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
-				overloadcounter = 0;
+				overloadcounter++;
 			}
 
-			overloadcounter++;
-		}
+			//#endif
+			//			now = hrt_absolute_time();
+			/* filter values */
+			attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b);
 
-//#endif
-//			now = hrt_absolute_time();
-		/* filter values */
-		attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b);
+			//			time_elapsed = hrt_absolute_time() - now;
+			//			if (blubb == 20)
+			//			{
+			//				printf("Estimator: %lu\n", time_elapsed);
+			//				blubb = 0;
+			//			}
+			//			blubb++;
 
-//			time_elapsed = hrt_absolute_time() - now;
-//			if (blubb == 20)
-//			{
-//				printf("Estimator: %lu\n", time_elapsed);
-//				blubb = 0;
-//			}
-//			blubb++;
+			//		if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data);
 
-//		if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data);
+			//			printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data);
+			// last_data = sensor_combined_s_local.timestamp;
 
-//			printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data);
-		// last_data = sensor_combined_s_local.timestamp;
+			/*correct yaw */
+			//			euler.z = euler.z + M_PI;
 
-		/*correct yaw */
-//			euler.z = euler.z + M_PI;
+			/* send out */
 
-		/* send out */
+			att.timestamp = sensor_combined_s_local.timestamp;
+			att.roll = euler.x;
+			att.pitch = euler.y;
+			att.yaw = euler.z + M_PI;
 
-		att.timestamp = sensor_combined_s_local.timestamp;
-		att.roll = euler.x;
-		att.pitch = euler.y;
-		att.yaw = euler.z + M_PI;
+			if (att.yaw > 2.0f * ((float)M_PI)) {
+				att.yaw -= 2.0f * ((float)M_PI);
+			}
+
+			att.rollspeed = rates.x;
+			att.pitchspeed = rates.y;
+			att.yawspeed = rates.z;
 
-		if (att.yaw > 2.0f * ((float)M_PI)) {
-			att.yaw -= 2.0f * ((float)M_PI);
+			att.R[0][0] = x_n_b.x;
+			att.R[0][1] = x_n_b.y;
+			att.R[0][2] = x_n_b.z;
+
+			// Broadcast
+			if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
 		}
 
-		att.rollspeed = rates.x;
-		att.pitchspeed = rates.y;
-		att.yawspeed = rates.z;
 
-		att.R[0][0] = x_n_b.x;
-		att.R[0][1] = x_n_b.y;
-		att.R[0][2] = x_n_b.z;
 		// XXX add remaining entries
 
-		if (loopcounter % 250 == 0) {
+		if (hrt_absolute_time() - last_checkstate_stamp > 500000) {
 			/* Check HIL state */
 			orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
 			/* switching from non-HIL to HIL mode */
+			//printf("[attitude_estimator_bm] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
 			if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
 				hil_enabled = true;
 				publishing = false;
-				close(pub_att);
+				int ret = close(pub_att);
+				printf("Closing attitude: %i \n", ret);
 
 				/* switching from HIL to non-HIL mode */
 
@@ -246,11 +255,9 @@ int attitude_estimator_bm_main(int argc, char *argv[])
 				hil_enabled = false;
 				publishing = true;
 			}
+			last_checkstate_stamp = hrt_absolute_time();
 		}
 
-		// Broadcast
-		if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
-
 		loopcounter++;
 	}