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

Conflicts:
	apps/commander/commander.c
	apps/multirotor_att_control/multirotor_att_control_main.c
	apps/multirotor_att_control/multirotor_rate_control.c

6 files changed, 86 insertions, 137 deletions

diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index d2e0efb56..159a70701 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -257,7 +257,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			{ .fd = sub_raw,   .events = POLLIN },
 			{ .fd = sub_params, .events = POLLIN }
 		};
-		int ret = poll(fds, 1, 1000);
+		int ret = poll(fds, 2, 1000);
 
 		if (ret < 0) {
 			/* XXX this is seriously bad - should be an emergency */
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index f29cef165..c569a6aa4 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -293,7 +293,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	struct sensor_combined_s raw;
 
 	/* 30 seconds */
-	const uint64_t calibration_interval_us = 45 * 1000000;
+	int calibration_interval_ms = 30 * 1000;
 	unsigned int calibration_counter = 0;
 
 	float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
@@ -312,10 +312,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		warn("WARNING: failed to set scale / offsets for mag");
 	close(fd);
 
-	mavlink_log_info(mavlink_fd, "[commander] Please rotate around all axes.");
+	mavlink_log_info(mavlink_fd, "[commander] Please rotate around X");
 	
 	uint64_t calibration_start = hrt_absolute_time();
-	while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) {
+	while ((hrt_absolute_time() - calibration_start)/1000 < calibration_interval_ms) {
 
 		/* wait blocking for new data */
 		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
@@ -348,11 +348,13 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 			calibration_counter++;
 		} else {
 			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
+			mavlink_log_info(mavlink_fd, "[commander] mag cal canceled");
 			break;
 		}
 	}
 
+	mavlink_log_info(mavlink_fd, "[commander] mag calibration done");
+
 	/* disable calibration mode */
 	status->flag_preflight_mag_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
@@ -1307,19 +1309,12 @@ int commander_thread_main(int argc, char *argv[])
 				//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
 
 				if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
-					current_status.flag_control_attitude_enabled = false;
-					current_status.flag_control_rates_enabled = true;
 					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
 
 				} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
-					current_status.flag_control_attitude_enabled = false;
-					current_status.flag_control_rates_enabled = true;
-					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
-					//update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
+					update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
 
 				} else {
-					current_status.flag_control_attitude_enabled = false;
-					current_status.flag_control_rates_enabled = true;
 					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
 				}
 
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index e1d24d6a6..1e82bc417 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -501,9 +501,10 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 {
 	int old_mode = current_status->flight_mode;
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-	current_status->flag_control_manual_enabled = true;
+	current_status->flag_control_manual_enabled = true; //XXX
 	/* enable attitude control per default */
-	current_status->flag_control_attitude_enabled = true;
+	current_status->flag_control_attitude_enabled = false;
+	current_status->flag_control_rates_enabled = true;
 	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -516,7 +517,9 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_
 {
 	int old_mode = current_status->flight_mode;
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
-	current_status->flag_control_manual_enabled = true;
+	current_status->flag_control_manual_enabled = true; //XXX
+	current_status->flag_control_attitude_enabled = false;
+	current_status->flag_control_rates_enabled = true;
 	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -529,7 +532,9 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
 {
 	int old_mode = current_status->flight_mode;
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
-	current_status->flag_control_manual_enabled = true;
+	current_status->flag_control_manual_enabled = true; //XXX
+	current_status->flag_control_attitude_enabled = false;
+	current_status->flag_control_rates_enabled = true;
 	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index cec2593c1..f36fb009d 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -1802,7 +1802,9 @@ int mavlink_thread_main(int argc, char *argv[])
 		mavlink_pm_queued_send();
 		/* sleep quarter the time */
 		usleep(25000);
-		mavlink_pm_queued_send();
+		if (baudrate > 57600) {
+			mavlink_pm_queued_send();
+		}
 
 		/* sleep 10 ms */
 		usleep(10000);
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 9961b35f8..40f1bbfde 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -82,68 +82,6 @@ static orb_advert_t actuator_pub;
 
 static struct vehicle_status_s state;
 
-/**
- * Perform rate control right after gyro reading
- */
-static void *rate_control_thread_main(void *arg)
-{
-	prctl(PR_SET_NAME, "mc rate control", getpid());
-
-	struct actuator_controls_s actuators;
-
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-
-	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
-
-//	struct pollfd fds = { .fd = att_sub, .events = POLLIN };
-
-	struct vehicle_attitude_s vehicle_attitude;
-	struct vehicle_rates_setpoint_s rates_sp;
-	memset(&rates_sp, 0, sizeof(rates_sp));
-	float gyro_lp[3] = {0.0f, 0.0f, 0.0f};
-
-	while (true) {
-//		/* rate control at maximum rate */
-//		/* wait for a sensor update, check for exit condition every 1000 ms */
-//		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("[mc att control] WARNING: Not getting gyro data, no rate control\n");
-//		} else {
-			/* get current angular rate */
-			orb_copy(ORB_ID(vehicle_attitude), att_sub, &vehicle_attitude);
-			/* get current rate setpoint */
-			bool rates_sp_valid = false;
-			orb_check(rates_sp_sub, &rates_sp_valid);
-			if (rates_sp_valid) {
-				orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
-			}
-
-			/* perform local lowpass */
-
-			/* apply controller */
-//			if (state.flag_control_rates_enabled) {
-				/* lowpass gyros */
-				// XXX
-				gyro_lp[0] = vehicle_attitude.rollspeed;
-				gyro_lp[1] = vehicle_attitude.pitchspeed;
-				gyro_lp[2] = vehicle_attitude.yawspeed;
-
-				//multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
-				printf(".\n");
-				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-//			}
-//		}
-		usleep(5000);
-	}
-
-	return NULL;
-}
-
 static int
 mc_thread_main(int argc, char *argv[])
 {
@@ -187,9 +125,10 @@ mc_thread_main(int argc, char *argv[])
 		actuators.control[i] = 0.0f;
 	}
 
-	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 	orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
 	/* register the perf counter */
 	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
@@ -197,13 +136,6 @@ mc_thread_main(int argc, char *argv[])
 	/* welcome user */
 	printf("[multirotor_att_control] starting\n");
 
-	/* ready, spawn pthread */
-	pthread_attr_t rate_control_attr;
-	pthread_attr_init(&rate_control_attr);
-	pthread_attr_setstacksize(&rate_control_attr, 4096);
-	pthread_t rate_control_thread;
-	pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL);
-
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
@@ -236,13 +168,24 @@ mc_thread_main(int argc, char *argv[])
 
 		if (state.flag_control_manual_enabled) {
 			/* manual inputs, from RC control or joystick */
-			att_sp.roll_body = manual.roll;
-			att_sp.pitch_body = manual.pitch;
-			att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
-			/* set yaw rate */
-			rates_sp.yaw = manual.yaw;
-			att_sp.thrust = manual.throttle;
-			att_sp.timestamp = hrt_absolute_time();
+
+			if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
+				rates_sp.roll = manual.roll;
+				rates_sp.pitch = manual.pitch;
+				rates_sp.yaw = manual.yaw;
+				rates_sp.thrust = manual.throttle;
+				rates_sp.timestamp = hrt_absolute_time();
+			}
+
+			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, remove, switch to yaw rate controller
+				/* set yaw rate */
+				rates_sp.yaw = manual.yaw;
+				att_sp.thrust = manual.throttle;
+				att_sp.timestamp = hrt_absolute_time();
+			}
 			/* STEP 2: publish the result to the vehicle actuators */
 			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 			
@@ -280,31 +223,36 @@ mc_thread_main(int argc, char *argv[])
 
 		/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
 
-//		/* run attitude controller */
-//		if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
-//			multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
-//			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-//		} else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
-//			multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
-//			orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-//		}
 
-		float gyro_lp[3] = {0.0f, 0.0f, 0.0f};
+		/* run attitude controller */
+		// if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
+		// 	multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
+		// 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+		// } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
+		// 	multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
+		// 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+		// }
 
-		gyro_lp[0] = att.rollspeed;
-		gyro_lp[1] = att.pitchspeed;
-		gyro_lp[2] = att.yawspeed;
 
-		rates_sp.roll = manual.roll;
-		rates_sp.pitch = manual.pitch;
-		rates_sp.yaw = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
-					/* set yaw rate */
-		rates_sp.yaw = manual.yaw;
-		rates_sp.thrust = manual.throttle;
-		rates_sp.timestamp = hrt_absolute_time();
+		if (state.flag_control_rates_enabled) {
 
-		multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
-		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+			float gyro[3] = {0.0f, 0.0f, 0.0f};
+
+			/* get current rate setpoint */
+			bool rates_sp_valid = false;
+			orb_check(rates_sp_sub, &rates_sp_valid);
+			if (rates_sp_valid) {
+				orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+			}
+
+			/* apply controller */
+			gyro[0] = att.rollspeed;
+			gyro[1] = att.pitchspeed;
+			gyro[2] = att.yawspeed;
+
+			multirotor_control_rates(&rates_sp, gyro, &actuators);
+			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+		}
 
 		perf_end(mc_loop_perf);
 	}
@@ -326,8 +274,6 @@ mc_thread_main(int argc, char *argv[])
 	perf_print_counter(mc_loop_perf);
 	perf_free(mc_loop_perf);
 
-	pthread_join(rate_control_thread, NULL);
-
 	fflush(stdout);
 	exit(0);
 }
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index 1d400f51b..372b378d1 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -155,32 +155,33 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	static bool initialized = false;
 
+	/* initialize the pid controllers when the function is called for the first time */
+	if (initialized == false) {
+		parameters_init(&h);
+		parameters_update(&h, &p);
+		initialized = true;
+	}
 
 	/* load new parameters with lower rate */
-
-	if (motor_skip_counter % 500 == 0) {
+	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);
 	}
 
 	/* calculate current control outputs */
-	float setpointXrate;
-	float setpointYrate;
-	float setpointZrate;
-	float setRollRate=rate_sp->roll;
-	float setPitchRate=rate_sp->pitch;
-	float setYawRate=rate_sp->yaw;
-
-	//x-axis
-	setpointXrate=p.attrate_p*(setRollRate-rates[0]);
-	//Y-axis
-	setpointYrate=p.attrate_p*(setPitchRate-rates[1]);
-	//Z-axis
-	setpointZrate=p.yawrate_p*(setYawRate-rates[2]);
-
-	actuators->control[0] = setpointXrate; //roll
-	actuators->control[1] = setpointYrate; //pitch
-	actuators->control[2] = setpointZrate; //yaw
+	
+	/* control pitch (forward) output */
+	float pitch_control = p.attrate_p * deltaT * (rate_sp->pitch-rates[1]);
+	/* control roll (left/right) output */
+	float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0]  );
+
+	/* control yaw rate */
+	float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2]  );
+
+	actuators->control[0] = roll_control;
+	actuators->control[1] = pitch_control;
+	actuators->control[2] = yaw_rate_control;
 	actuators->control[3] = rate_sp->thrust;
 
 	motor_skip_counter++;