commander, multirotor_pos_control, multirotor_att_control: bugfixes

src/modules/commander/commander.cpp

@@ -122,7 +122,7 @@ extern struct system_load_s system_load;
 #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
-#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */
+#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */
 
 #define PRINT_INTERVAL	5000000
 #define PRINT_MODE_REJECT_INTERVAL	2000000
@@ -353,27 +353,42 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			break;
 		}
 
-	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
+	case VEHICLE_CMD_NAV_TAKEOFF: {
+		transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
+		if (nav_res == TRANSITION_CHANGED) {
+			mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
+		}
+
+		if (nav_res != TRANSITION_DENIED) {
+			result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+		} else {
+			result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+		}
 		break;
+	}
 
 	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
-			if (is_safe(status, safety, armed)) {
+		if (is_safe(status, safety, armed)) {
 
-				if (((int)(cmd->param1)) == 1) {
-					/* reboot */
-					up_systemreset();
-				} else if (((int)(cmd->param1)) == 3) {
-					/* reboot to bootloader */
+			if (((int)(cmd->param1)) == 1) {
+				/* reboot */
+				up_systemreset();
 
-					// XXX implement
-					result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
+			} else if (((int)(cmd->param1)) == 3) {
+				/* reboot to bootloader */
+
+				// XXX implement
+				result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 
 			} else {
 				result = VEHICLE_CMD_RESULT_DENIED;
 			}
+
+		} else {
+			result = VEHICLE_CMD_RESULT_DENIED;
+		}
+
 		break;
 
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
@@ -519,6 +534,7 @@ int commander_thread_main(int argc, char *argv[])
 	orb_advert_t status_pub;
 	/* make sure we are in preflight state */
 	memset(&status, 0, sizeof(status));
+	status.condition_landed = true;	// initialize to safe value
 
 	/* armed topic */
 	struct actuator_armed_s armed;
@@ -760,6 +776,14 @@ int commander_thread_main(int argc, char *argv[])
 			last_diff_pres_time = diff_pres.timestamp;
 		}
 
+		/* Check for valid airspeed/differential pressure measurements */
+		if (t - last_diff_pres_time < 2000000 && t > 2000000) {
+			status.condition_airspeed_valid = true;
+
+		} else {
+			status.condition_airspeed_valid = false;
+		}
+
 		orb_check(cmd_sub, &updated);
 
 		if (updated) {
@@ -785,6 +809,20 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
 		}
 
+		/* set the condition to valid if there has recently been a global position estimate */
+		if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) {
+			if (!status.condition_global_position_valid) {
+				status.condition_global_position_valid = true;
+				status_changed = true;
+			}
+
+		} else {
+			if (status.condition_global_position_valid) {
+				status.condition_global_position_valid = false;
+				status_changed = true;
+			}
+		}
+
 		/* update local position estimate */
 		orb_check(local_position_sub, &updated);
 
@@ -794,7 +832,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* set the condition to valid if there has recently been a local position estimate */
-		if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) {
+		if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) {
 			if (!status.condition_local_position_valid) {
 				status.condition_local_position_valid = true;
 				status_changed = true;
@@ -920,36 +958,6 @@ int commander_thread_main(int argc, char *argv[])
 		 * set of position measurements is available.
 		 */
 
-		/* store current state to reason later about a state change */
-		// bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
-		bool global_pos_valid = status.condition_global_position_valid;
-		bool local_pos_valid = status.condition_local_position_valid;
-		bool airspeed_valid = status.condition_airspeed_valid;
-
-
-		/* check for global or local position updates, set a timeout of 2s */
-		if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) {
-			status.condition_global_position_valid = true;
-
-		} else {
-			status.condition_global_position_valid = false;
-		}
-
-		if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) {
-			status.condition_local_position_valid = true;
-
-		} else {
-			status.condition_local_position_valid = false;
-		}
-
-		/* Check for valid airspeed/differential pressure measurements */
-		if (t - last_diff_pres_time < 2000000 && t > 2000000) {
-			status.condition_airspeed_valid = true;
-
-		} else {
-			status.condition_airspeed_valid = false;
-		}
-
 		orb_check(gps_sub, &updated);
 
 		if (updated) {
@@ -1083,6 +1091,7 @@ int commander_thread_main(int argc, char *argv[])
 					} else {
 						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
 					}
+
 				} else if (res == TRANSITION_DENIED) {
 					/* DENIED here indicates safety switch not pressed */
 
@@ -1536,8 +1545,10 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 			break;
 
 		case MAIN_STATE_AUTO:
-			if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
+			if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
 				/* don't act while taking off */
+				res = TRANSITION_NOT_CHANGED;
+			} else {
 				if (current_status->condition_landed) {
 					/* if landed: transitions only to AUTO_READY are allowed */
 					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
@@ -1545,20 +1556,27 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 					break;
 
 				} else {
-					/* if not landed: act depending on switches */
-					if (current_status->return_switch == RETURN_SWITCH_RETURN) {
-						/* RTL */
-						res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
-
-					} else {
-						if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
-							/* MISSION */
-							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+					/* if not landed: */
+					if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
+						/* act depending on switches when manual control enabled */
+						if (current_status->return_switch == RETURN_SWITCH_RETURN) {
+							/* RTL */
+							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
 
 						} else {
-							/* LOITER */
-							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+							if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
+								/* MISSION */
+								res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+
+							} else {
+								/* LOITER */
+								res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+							}
 						}
+
+					} else {
+						/* always switch to loiter in air when no RC control */
+						res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
 					}
 				}
 			}

src/modules/commander/state_machine_helper.cpp

@@ -109,9 +109,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 
 			/* allow arming from STANDBY and IN-AIR-RESTORE */
 			if ((status->arming_state == ARMING_STATE_STANDBY
-			    || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
-			    && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */
-			{
+			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
+			    && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */
 				ret = TRANSITION_CHANGED;
 				armed->armed = true;
 				armed->ready_to_arm = false;
@@ -182,6 +181,7 @@ bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s
 	// 3) Safety switch is present AND engaged -> actuators locked
 	if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
 		return true;
+
 	} else {
 		return false;
 	}
@@ -284,6 +284,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_velocity_enabled = false;
 			control_mode->flag_control_position_enabled = false;
 			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_climb_rate_enabled = false;
 			control_mode->flag_control_manual_enabled = false;
 			break;
 
@@ -294,6 +295,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_velocity_enabled = false;
 			control_mode->flag_control_position_enabled = false;
 			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_climb_rate_enabled = false;
 			control_mode->flag_control_manual_enabled = true;
 			break;
 
@@ -304,6 +306,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_velocity_enabled = false;
 			control_mode->flag_control_position_enabled = false;
 			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_climb_rate_enabled = false;
 			control_mode->flag_control_manual_enabled = true;
 			break;
 
@@ -314,6 +317,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_velocity_enabled = false;
 			control_mode->flag_control_position_enabled = false;
 			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
 			control_mode->flag_control_manual_enabled = true;
 			break;
 
@@ -324,80 +328,68 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_velocity_enabled = true;
 			control_mode->flag_control_position_enabled = true;
 			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
 			control_mode->flag_control_manual_enabled = true;
 			break;
 
 		case NAVIGATION_STATE_AUTO_READY:
 			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_rates_enabled = false;
+			control_mode->flag_control_attitude_enabled = false;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_climb_rate_enabled = false;
 			control_mode->flag_control_manual_enabled = false;
 			break;
 
 		case NAVIGATION_STATE_AUTO_TAKEOFF:
 
 			/* only transitions from AUTO_READY */
-			if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+			if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
 				ret = TRANSITION_CHANGED;
 				control_mode->flag_control_rates_enabled = true;
 				control_mode->flag_control_attitude_enabled = true;
 				control_mode->flag_control_velocity_enabled = true;
 				control_mode->flag_control_position_enabled = true;
 				control_mode->flag_control_altitude_enabled = true;
+				control_mode->flag_control_climb_rate_enabled = true;
 				control_mode->flag_control_manual_enabled = false;
 			}
 
 			break;
 
 		case NAVIGATION_STATE_AUTO_LOITER:
-
-			/* deny transitions from landed states */
-			if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
-			    current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
-				ret = TRANSITION_CHANGED;
-				control_mode->flag_control_rates_enabled = true;
-				control_mode->flag_control_attitude_enabled = true;
-				control_mode->flag_control_velocity_enabled = true;
-				control_mode->flag_control_position_enabled = true;
-				control_mode->flag_control_altitude_enabled = true;
-				control_mode->flag_control_manual_enabled = false;
-			}
-
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
 			break;
 
 		case NAVIGATION_STATE_AUTO_MISSION:
-
-			/* deny transitions from landed states */
-			if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
-			    current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
-				ret = TRANSITION_CHANGED;
-				control_mode->flag_control_rates_enabled = true;
-				control_mode->flag_control_attitude_enabled = true;
-				control_mode->flag_control_velocity_enabled = true;
-				control_mode->flag_control_position_enabled = true;
-				control_mode->flag_control_altitude_enabled = true;
-				control_mode->flag_control_manual_enabled = false;
-			}
-
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
 			break;
 
 		case NAVIGATION_STATE_AUTO_RTL:
-
-			/* deny transitions from landed states */
-			if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
-			    current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
-				ret = TRANSITION_CHANGED;
-				control_mode->flag_control_rates_enabled = true;
-				control_mode->flag_control_attitude_enabled = true;
-				control_mode->flag_control_velocity_enabled = true;
-				control_mode->flag_control_position_enabled = true;
-				control_mode->flag_control_altitude_enabled = true;
-				control_mode->flag_control_manual_enabled = false;
-			}
-
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
 			break;
 
 		case NAVIGATION_STATE_AUTO_LAND:
@@ -411,6 +403,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 				control_mode->flag_control_velocity_enabled = true;
 				control_mode->flag_control_position_enabled = true;
 				control_mode->flag_control_altitude_enabled = true;
+				control_mode->flag_control_climb_rate_enabled = true;
 				control_mode->flag_control_manual_enabled = false;
 			}
 

src/modules/multirotor_att_control/multirotor_att_control_main.c

@@ -250,12 +250,12 @@ mc_thread_main(int argc, char *argv[])
 
 						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
 						if (control_mode.failsave_highlevel) {
-							if (!control_mode.flag_control_position_enabled) {
+							if (!control_mode.flag_control_velocity_enabled) {
 								/* Don't reset attitude setpoint in position control mode, it's handled by position controller. */
 								att_sp.roll_body = 0.0f;
 								att_sp.pitch_body = 0.0f;
 
-								if (!control_mode.flag_control_altitude_enabled) {
+								if (!control_mode.flag_control_climb_rate_enabled) {
 									/* Don't touch throttle in modes with altitude hold, it's handled by position controller.
 									 *
 									 * Only go to failsafe throttle if last known throttle was
@@ -309,12 +309,12 @@ mc_thread_main(int argc, char *argv[])
 								control_yaw_position = true;
 							}
 
-							if (!control_mode.flag_control_position_enabled) {
+							if (!control_mode.flag_control_velocity_enabled) {
 								/* don't update attitude setpoint in position control mode */
 								att_sp.roll_body = manual.roll;
 								att_sp.pitch_body = manual.pitch;
 
-								if (!control_mode.flag_control_position_enabled) {
+								if (!control_mode.flag_control_climb_rate_enabled) {
 									/* don't set throttle in altitude hold modes */
 									att_sp.thrust = manual.throttle;
 								}

src/modules/multirotor_pos_control/multirotor_pos_control.c

@@ -221,10 +221,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	hrt_abstime home_alt_t = 0;
 	uint64_t local_home_timestamp = 0;
 
-	static PID_t xy_pos_pids[2];
-	static PID_t xy_vel_pids[2];
-	static PID_t z_pos_pid;
-	static thrust_pid_t z_vel_pid;
+	PID_t xy_pos_pids[2];
+	PID_t xy_vel_pids[2];
+	PID_t z_pos_pid;
+	thrust_pid_t z_vel_pid;
 
 	thread_running = true;
 
@@ -238,7 +238,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
 	}
 
-	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
+	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
 	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
 
 	int paramcheck_counter = 0;
@@ -247,9 +247,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	while (!thread_should_exit) {
 		orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 
-		/* check parameters at 1 Hz*/
-		if (--paramcheck_counter <= 0) {
-			paramcheck_counter = 50;
+		/* check parameters at 1 Hz */
+		if (++paramcheck_counter >= 50) {
+			paramcheck_counter = 0;
 			bool param_updated;
 			orb_check(param_sub, &param_updated);
 
@@ -441,14 +441,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
 			// TODO subcrive to velocity setpoint if altitude/position control disabled
 
-			if (control_mode.flag_control_velocity_enabled) {
+			if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
 				/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
 				float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
-				if (control_mode.flag_control_altitude_enabled) {
+				if (control_mode.flag_control_climb_rate_enabled) {
 					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
 					att_sp.thrust = -thrust_sp[2];
 				}
-				if (control_mode.flag_control_position_enabled) {
+				if (control_mode.flag_control_velocity_enabled) {
 					/* calculate thrust set point in NED frame */
 					thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
 					thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);

src/modules/uORB/topics/vehicle_control_mode.h

@@ -75,9 +75,10 @@ struct vehicle_control_mode_s
 	//bool flag_auto_enabled;
 	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
 	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
-	bool flag_control_velocity_enabled;		/**< true if speed (implies direction) is controlled */
+	bool flag_control_velocity_enabled;		/**< true if horisontal speed (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
+	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
 
 	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
 	bool failsave_highlevel;			/**< Set to true if high-level failsafe mode is enabled */

src/modules/uORB/topics/vehicle_local_position.h

@@ -75,6 +75,7 @@ struct vehicle_local_position_s
 	float home_alt;			/**< Altitude in meters									LOGME */
 	float home_hdg; 		/**< Compass heading in radians -PI..+PI.					  */
 
+	bool landed;			/**< true if vehicle is landed */
 };
 
 /**