Minor cleanups in attitude control

apps/multirotor_att_control/multirotor_att_control_main.c

@@ -65,6 +65,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
@@ -104,6 +105,7 @@ mc_thread_main(int argc, char *argv[])
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int param_sub = orb_subscribe(ORB_ID(parameter_update));
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -118,7 +120,10 @@ mc_thread_main(int argc, char *argv[])
 	 * rate-limit the attitude subscription to 200Hz to pace our loop
 	 * orb_set_interval(att_sub, 5);
 	 */
-	struct pollfd fds = { .fd = att_sub, .events = POLLIN };
+	struct pollfd fds[2] = {
+					{ .fd = att_sub, .events = POLLIN },
+					{ .fd = param_sub, .events = POLLIN }
+				};
 
 	/* publish actuator controls */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
@@ -131,7 +136,9 @@ mc_thread_main(int argc, char *argv[])
 	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");
+	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
+	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
+	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
 
 	/* welcome user */
 	printf("[multirotor_att_control] starting\n");
@@ -140,12 +147,31 @@ mc_thread_main(int argc, char *argv[])
 	bool flag_control_manual_enabled = false;
 	bool flag_control_attitude_enabled = false;
 	bool flag_system_armed = false;
-	bool man_yaw_zero_once = false;
 
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
-		poll(&fds, 1, 500);
+		int ret = poll(fds, 2, 500);
+
+		if (ret < 0) {
+			/* poll error, count it in perf */
+			perf_count(mc_err_perf);
+		} else if (ret == 0) {
+			/* no return value, ignore */
+		} else {
+
+			/* only update parameters if they changed */
+			if (fds[1].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), param_sub, &update);
+
+				/* update parameters */
+				// XXX no params here yet
+			}
+
+			/* only run controller if attitude changed */
+			if (fds[0].revents & POLLIN) {
 
 				perf_begin(mc_loop_perf);
 
@@ -250,8 +276,8 @@ mc_thread_main(int argc, char *argv[])
 				 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
 
-
+				/* measure in what intervals the controller runs */
-		if (state.flag_control_rates_enabled) {
+				perf_count(mc_interval_perf);
 
 				float gyro[3];
 
@@ -269,7 +295,6 @@ mc_thread_main(int argc, char *argv[])
 
 				multirotor_control_rates(&rates_sp, gyro, &actuators);
 				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-		}
 
 				/* update state */
 				flag_control_attitude_enabled = state.flag_control_attitude_enabled;
@@ -277,6 +302,8 @@ mc_thread_main(int argc, char *argv[])
 				flag_system_armed = state.flag_system_armed;
 
 				perf_end(mc_loop_perf);
+			} /* end of poll call for attitude updates */
+		} /* end of poll return value check */
 	}
 
 	printf("[multirotor att control] stopping, disarming motors.\n");

apps/multirotor_att_control/multirotor_attitude_control.c

@@ -199,9 +199,9 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		/* update parameters from storage */
 		parameters_update(&h, &p);
 
-		/* apply parameters */
+		//printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
-		printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
 
+		/* apply parameters */
 		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
 		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
 	}