Merge branch 'master' of https://github.com/PX4/Firmware into fw_control

4 files changed, 31 insertions, 94 deletions

diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index 05a6292a2..b25e61229 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -58,6 +58,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -204,6 +205,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	memset(&raw, 0, sizeof(raw));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
+	struct vehicle_status_s state;
+	memset(&state, 0, sizeof(state));
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
@@ -216,6 +219,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
+	/* subscribe to system state*/
+	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
@@ -226,13 +232,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	thread_running = true;
 
 	/* advertise debug value */
-	struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
-	orb_advert_t pub_dbg = -1;
+	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
+	// orb_advert_t pub_dbg = -1;
+	
+	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
+	// XXX write this out to perf regs
 
 	/* 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};
 
 	struct attitude_estimator_ekf_params ekf_params;
 
@@ -259,8 +267,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 		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 ekf] WARNING: Not getting sensor data - sensor app running?\n");
+			/* check if we're in HIL - not getting sensor data is fine then */
+			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+			if (!state.flag_hil_enabled) {
+				fprintf(stderr, 
+					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
+			}
 		} else {
 
 			/* only update parameters if they changed */
@@ -347,9 +359,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						overloadcounter++;
 					}
 
-					int32_t z_k_sizes = 9;
-					// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
-
 					static bool const_initialized = false;
 
 					/* initialize with good values once we have a reasonable dt estimate */
@@ -392,7 +401,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						continue;
 					}
 					
-					uint64_t timing_diff = hrt_absolute_time() - timing_start;
+					// uint64_t timing_diff = hrt_absolute_time() - timing_start;
 
 					// /* print rotation matrix every 200th time */
 					if (printcounter % 200 == 0) {
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index dd5463d4e..bef21848b 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -211,8 +211,9 @@ HIL::init()
 	if (ret != OK)
 		return ret;
 
-	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
-	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+	// XXX already claimed with CDEV
+	///* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	//ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
 	if (ret == OK) {
 		log("default PWM output device");
 		_primary_pwm_device = true;
@@ -221,11 +222,11 @@ HIL::init()
 	/* reset GPIOs */
 	// gpio_reset();
 
-	/* start the IO interface task */
-	_task = task_spawn("fmuservo",
+	/* start the HIL interface task */
+	_task = task_spawn("fmuhil",
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
-			   1024,
+			   2048,
 			   (main_t)&HIL::task_main_trampoline,
 			   nullptr);
 
@@ -821,12 +822,12 @@ hil_main(int argc, char *argv[])
 		if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) {
 			// if (new_mode == PORT1_FULL_PWM || new_mode == PORT1_PWM_AND_GPIO) {
 			// XXX all modes have PWM settings
-				if (argc > i + 1) {
-					pwm_update_rate_in_hz = atoi(argv[i + 1]);
-				} else {
-					fprintf(stderr, "missing argument for pwm update rate (-u)\n");
-					return 1;
-				}
+			if (argc > i + 1) {
+				pwm_update_rate_in_hz = atoi(argv[i + 1]);
+			} else {
+				fprintf(stderr, "missing argument for pwm update rate (-u)\n");
+				return 1;
+			}
 			// } else {
 			// 	fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n");
 			// }
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index ae888323d..3eb4a9ef2 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -224,7 +224,7 @@ PX4FMU::init()
 	_task = task_spawn("fmuservo",
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
-			   1024,
+			   2048,
 			   (main_t)&PX4FMU::task_main_trampoline,
 			   nullptr);
 
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index e1a4e8e8a..3e485274e 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -218,8 +218,6 @@ handle_message(mavlink_message_t *msg)
 	if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
 		mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
 		mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
-		//printf("got message\n");
-		//printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode);
 
 		if (mavlink_system.sysid < 4) {
 
@@ -276,61 +274,6 @@ handle_message(mavlink_message_t *msg)
 				/* Publish */
 				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
 			}
-
-			// /* change armed status if required */
-			// bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED);
-
-			// bool cmd_generated = false;
-
-			// if (v_status.flag_control_offboard_enabled != cmd_armed) {
-			// 	vcmd.param1 = cmd_armed;
-			// 	vcmd.param2 = 0;
-			// 	vcmd.param3 = 0;
-			// 	vcmd.param4 = 0;
-			// 	vcmd.param5 = 0;
-			// 	vcmd.param6 = 0;
-			// 	vcmd.param7 = 0;
-			// 	vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
-			// 	vcmd.target_system = mavlink_system.sysid;
-			// 	vcmd.target_component = MAV_COMP_ID_ALL;
-			// 	vcmd.source_system = msg->sysid;
-			// 	vcmd.source_component = msg->compid;
-			// 	vcmd.confirmation = 1;
-
-			// 	cmd_generated = true;
-			// }
-
-			// /* check if input has to be enabled */
-			// if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) ||
-			// 	(v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) ||
-			// 	(v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) ||
-			// 	(v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) {
-			// 	vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES);
-			// 	vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE);
-			// 	vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY);
-			// 	vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION);
-			// 	vcmd.param5 = 0;
-			// 	vcmd.param6 = 0;
-			// 	vcmd.param7 = 0;
-			// 	vcmd.command = PX4_CMD_CONTROLLER_SELECTION;
-			// 	vcmd.target_system = mavlink_system.sysid;
-			// 	vcmd.target_component = MAV_COMP_ID_ALL;
-			// 	vcmd.source_system = msg->sysid;
-			// 	vcmd.source_component = msg->compid;
-			// 	vcmd.confirmation = 1;
-
-			// 	cmd_generated = true;
-			// }
-
-			// if (cmd_generated) {
-			// 	/* check if topic is advertised */
-			// 	if (cmd_pub <= 0) {
-			// 		cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
-			// 	} else {
-			// 		/* create command */
-			// 		orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-			// 	}
-			// }
 		}
 	}
 
@@ -342,8 +285,6 @@ handle_message(mavlink_message_t *msg)
 	 * COMMAND_LONG message or a SET_MODE message
 	 */
 
-	// printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false");
-
 	if (mavlink_hil_enabled) {
 
 		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
@@ -351,20 +292,6 @@ handle_message(mavlink_message_t *msg)
 			mavlink_hil_state_t hil_state;
 			mavlink_msg_hil_state_decode(msg, &hil_state);
 
-			//	printf("\n HILSTATE : \n LAT: %i \n LON: %i \n ALT: %i \n "
-			//		"ROLL %i \n PITCH %i \n YAW %i \n"
-			//		"ROLLSPEED: %i \n PITCHSPEED: %i \n, YAWSPEED: %i \n",
-			//			hil_state.lat/1000000,	// 1e7
-			//			hil_state.lon/1000000,	// 1e7
-			//			hil_state.alt/1000,  // mm
-			//			hil_state.roll, // float rad
-			//			hil_state.pitch, // float rad
-			//			hil_state.yaw, // float rad
-			//			hil_state.rollspeed, // float rad/s
-			//			hil_state.pitchspeed, // float rad/s
-			//			hil_state.yawspeed); // float rad/s
-
-
 			hil_global_pos.lat = hil_state.lat;
 			hil_global_pos.lon = hil_state.lon;
 			hil_global_pos.alt = hil_state.alt / 1000.0f;