vehicle_control_mode.flag_armed added, reset integrals in multirotor_att_control when disarmed

src/modules/commander/commander.cpp

@@ -1235,8 +1235,9 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* publish control mode */
-		if (navigation_state_changed) {
+		if (navigation_state_changed || arming_state_changed) {
 			/* publish new navigation state */
+			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
 			control_mode.counter++;
 			control_mode.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);

src/modules/multirotor_att_control/multirotor_att_control_main.c

@@ -142,16 +142,13 @@ mc_thread_main(int argc, char *argv[])
 	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
 
 	/* welcome user */
-	printf("[multirotor_att_control] starting\n");
+	warnx("starting");
 
 	/* store last control mode to detect mode switches */
 	bool flag_control_manual_enabled = false;
 	bool flag_control_attitude_enabled = false;
 
-	/* store if yaw position or yaw speed has been changed */
 	bool control_yaw_position = true;
-
-	/* store if we stopped a yaw movement */
 	bool reset_yaw_sp = true;
 
 	/* prepare the handle for the failsafe throttle */
@@ -211,8 +208,7 @@ mc_thread_main(int argc, char *argv[])
 				/* get a local copy of the current sensor values */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
-
+				/* define which input is the dominating control input */
-				/** STEP 1: Define which input is the dominating control input */
 				if (control_mode.flag_control_offboard_enabled) {
 					/* offboard inputs */
 					if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
@@ -220,7 +216,6 @@ mc_thread_main(int argc, char *argv[])
 						rates_sp.pitch = offboard_sp.p2;
 						rates_sp.yaw = offboard_sp.p3;
 						rates_sp.thrust = offboard_sp.p4;
-//						printf("thrust_rate=%8.4f\n",offboard_sp.p4);
 						rates_sp.timestamp = hrt_absolute_time();
 						orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 
@@ -229,13 +224,11 @@ mc_thread_main(int argc, char *argv[])
 						att_sp.pitch_body = offboard_sp.p2;
 						att_sp.yaw_body = offboard_sp.p3;
 						att_sp.thrust = offboard_sp.p4;
-//						printf("thrust_att=%8.4f\n",offboard_sp.p4);
 						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);
 					}
 
-
 				} else if (control_mode.flag_control_manual_enabled) {
 					/* direct manual input */
 					if (control_mode.flag_control_attitude_enabled) {
@@ -265,7 +258,7 @@ mc_thread_main(int argc, char *argv[])
 									 * multicopter (throttle = 0) does not make it jump up in the air
 									 * if shutting down his remote.
 									 */
-									if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
+									if (isfinite(manual.throttle) && manual.throttle > 0.2f) {	// TODO use landed status instead of throttle
 										/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
 										param_get(failsafe_throttle_handle, &failsafe_throttle);
 										att_sp.thrust = failsafe_throttle;
@@ -305,7 +298,6 @@ mc_thread_main(int argc, char *argv[])
 									att_sp.yaw_body = att.yaw;
 									reset_yaw_sp = false;
 								}
-
 								control_yaw_position = true;
 							}
 
@@ -347,22 +339,25 @@ mc_thread_main(int argc, char *argv[])
 					}
 				}
 
-				/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
+				/* check if we should we reset integrals */
-				if (control_mode.flag_control_attitude_enabled) {
+				bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle
-					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
 
+				/* run attitude controller if needed */
+				if (control_mode.flag_control_attitude_enabled) {
+					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
 					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
 
 				/* measure in what intervals the controller runs */
 				perf_count(mc_interval_perf);
 
+				/* run rates controller if needed */
 				if (control_mode.flag_control_rates_enabled) {
 					/* get current rate setpoint */
-					bool rates_sp_valid = false;
+					bool rates_sp_updated = false;
-					orb_check(rates_sp_sub, &rates_sp_valid);
+					orb_check(rates_sp_sub, &rates_sp_updated);
 
-					if (rates_sp_valid) {
+					if (rates_sp_updated) {
 						orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
 					}
 
@@ -371,7 +366,7 @@ mc_thread_main(int argc, char *argv[])
 					rates[0] = att.rollspeed;
 					rates[1] = att.pitchspeed;
 					rates[2] = att.yawspeed;
-					multirotor_control_rates(&rates_sp, rates, &actuators);
+					multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
 				} else {
 					/* rates controller disabled, set actuators to zero for safety */
 					actuators.control[0] = 0.0f;
@@ -391,7 +386,7 @@ mc_thread_main(int argc, char *argv[])
 		} /* end of poll return value check */
 	}
 
-	printf("[multirotor att control] stopping, disarming motors.\n");
+	warnx("stopping, disarming motors");
 
 	/* kill all outputs */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)

src/modules/multirotor_att_control/multirotor_attitude_control.c

@@ -166,7 +166,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral)
 {
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
@@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
 	}
 
-	/* reset integral if on ground */
+	/* reset integrals if needed */
-	if (att_sp->thrust < 0.1f) {
+	if (reset_integral) {
 		pid_reset_integral(&pitch_controller);
 		pid_reset_integral(&roll_controller);
+		//TODO pid_reset_integral(&yaw_controller);
 	}
 
-
 	/* calculate current control outputs */
 
 	/* control pitch (forward) output */
@@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	if (control_yaw_position) {
 		/* control yaw rate */
+		// TODO use pid lib
 
 		/* positive error: rotate to right, negative error, rotate to left (NED frame) */
 		// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);

src/modules/multirotor_att_control/multirotor_attitude_control.h

@@ -60,6 +60,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral);
 
 #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */

src/modules/multirotor_att_control/multirotor_rate_control.c

@@ -152,7 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 }
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators)
+			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
 {
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
 	}
 
-	/* reset integral if on ground */
+	/* reset integrals if needed */
-	if (rate_sp->thrust < 0.01f) {
+	if (reset_integral) {
 		pid_reset_integral(&pitch_rate_controller);
 		pid_reset_integral(&roll_rate_controller);
+		// TODO pid_reset_integral(&yaw_rate_controller);
 	}
 
 	/* control pitch (forward) output */

src/modules/multirotor_att_control/multirotor_rate_control.h

@@ -59,6 +59,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators);
+			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral);
 
 #endif /* MULTIROTOR_RATE_CONTROL_H_ */

src/modules/uORB/topics/vehicle_control_mode.h

@@ -64,6 +64,8 @@ struct vehicle_control_mode_s
 	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
+	bool flag_armed;
+
 	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
 
 	// XXX needs yet to be set by state machine helper