Merge branch 'multirotor' of github.com:cvg/Firmware_Private into multirotor

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -564,8 +564,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512
 CONFIG_CDCACM_NWRREQS=4
 CONFIG_CDCACM_NRDREQS=4
 CONFIG_CDCACM_BULKIN_REQLEN=96
-CONFIG_CDCACM_RXBUFSIZE=256
-CONFIG_CDCACM_TXBUFSIZE=256
+CONFIG_CDCACM_RXBUFSIZE=512
+CONFIG_CDCACM_TXBUFSIZE=512
 CONFIG_CDCACM_VENDORID=0x26ac
 CONFIG_CDCACM_PRODUCTID=0x0010
 CONFIG_CDCACM_VENDORSTR="3D Robotics"

src/modules/commander/accelerometer_calibration.cpp

@@ -298,7 +298,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 	/* set accel error threshold to 5m/s^2 */
 	float accel_err_thr = 5.0f;
 	/* still time required in us */
-	int64_t still_time = 2000000;
+	hrt_abstime still_time = 2000000;
 	struct pollfd fds[1];
 	fds[0].fd = sub_sensor_combined;
 	fds[0].events = POLLIN;
@@ -342,7 +342,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 					t_timeout = t + timeout;
 				} else {
 					/* still since t_still */
-					if ((int64_t) t - (int64_t) t_still > still_time) {
+					if (t > t_still + still_time) {
 						/* vehicle is still, exit from the loop to detection of its orientation */
 						break;
 					}

src/modules/commander/commander.cpp

@@ -1592,43 +1592,41 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 	case MAIN_STATE_AUTO:
 		if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
 			if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-				/* check if takeoff completed */
-				if (local_pos->z < -5.0f && !status.condition_landed) {
-					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
-				} else {
+				/* don't switch to other states until takeoff not completed */
+				if (local_pos->z > -5.0f || status.condition_landed) {
 					res = TRANSITION_NOT_CHANGED;
+					break;
 				}
+			}
+			/* check again, state can be changed */
+			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);
+				// TRANSITION_DENIED is not possible here
+				break;
 
 			} 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);
-					// TRANSITION_DENIED is not possible here
-					break;
+				/* 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 {
-					/* 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 {
+						if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
+							/* MISSION */
+							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
 
 						} else {
-							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);
-							}
+							/* 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);
 					}
+
+				} else {
+					/* switch to mission in air when no RC control */
+					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
 				}
 			}
 		} else {

src/modules/multirotor_pos_control/multirotor_pos_control.c

@@ -405,30 +405,38 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					}
 				}
 
-				/* publish local position setpoint */
-				orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+				local_pos_sp.yaw = att_sp.yaw_body;
 
 				/* local position setpoint is valid and can be used for loiter after position controlled mode */
 				local_pos_sp_valid = control_mode.flag_control_position_enabled;
 
+				reset_auto_pos = true;
+
 				/* force reprojection of global setpoint after manual mode */
 				global_pos_sp_reproject = true;
 
 			} else {
 				/* non-manual mode, use global setpoint */
-				if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+				if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
+					reset_auto_pos = true;
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
 					if (reset_auto_pos) {
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
 						local_pos_sp.z = -takeoff_alt_default;
+						local_pos_sp.yaw = att.yaw;
+						local_pos_sp_valid = true;
 						att_sp.yaw_body = att.yaw;
 						reset_auto_pos = false;
+						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z);
 					}
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) {
 					if (reset_auto_pos) {
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
 						local_pos_sp.z = local_pos.z;
+						local_pos_sp.yaw = att.yaw;
+						local_pos_sp_valid = true;
 						att_sp.yaw_body = att.yaw;
 						reset_auto_pos = false;
 					}
@@ -462,6 +470,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							} else {
 								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
 							}
+							local_pos_sp.yaw = global_pos_sp.yaw;
+							att_sp.yaw_body = global_pos_sp.yaw;
 
 							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
 
@@ -472,15 +482,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 								local_pos_sp.x = local_pos.x;
 								local_pos_sp.y = local_pos.y;
 								local_pos_sp.z = local_pos.z;
+								local_pos_sp.yaw = att.yaw;
+								local_pos_sp_valid = true;
 							}
 
 							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
 						}
-
-						/* publish local position setpoint as projection of global position setpoint */
-						orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
 					}
-					att_sp.yaw_body = global_pos_sp.yaw;
 					reset_auto_pos = true;
 				}
 
@@ -493,6 +501,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				reset_sp_z = true;
 			}
 
+			/* publish local position setpoint */
+			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+
 			/* run position & altitude controllers, calculate velocity setpoint */
 			if (control_mode.flag_control_altitude_enabled) {
 				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -75,8 +75,11 @@ static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
 static int position_estimator_inav_task; /**< Handle of deamon task / thread */
 static bool verbose_mode = false;
-static hrt_abstime gps_timeout = 1000000;	// GPS timeout = 1s
-static hrt_abstime flow_timeout = 1000000;	// optical flow timeout = 1s
+
+static const hrt_abstime gps_timeout = 1000000;	// GPS timeout = 1s
+static const hrt_abstime flow_timeout = 1000000;	// optical flow timeout = 1s
+static const uint32_t updates_counter_len = 1000000;
+static const uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
 
 __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
 
@@ -172,6 +175,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float alt_avg = 0.0f;
 	bool landed = true;
 	hrt_abstime landed_time = 0;
+	bool flag_armed = false;
 
 	uint32_t accel_counter = 0;
 	uint32_t baro_counter = 0;
@@ -275,10 +279,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	uint16_t flow_updates = 0;
 
 	hrt_abstime updates_counter_start = hrt_absolute_time();
-	uint32_t updates_counter_len = 1000000;
-
 	hrt_abstime pub_last = hrt_absolute_time();
-	uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
 
 	/* acceleration in NED frame */
 	float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
@@ -407,7 +408,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							} else {
 								// new ground level
 								baro_alt0 += sonar_corr;
-								warnx("new home: alt = %.3f", baro_alt0);
 								mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
 								local_pos.ref_alt = baro_alt0;
 								local_pos.ref_timestamp = hrt_absolute_time();
@@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (fds[6].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 
-				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
+				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) {
 					/* initialize reference position if needed */
 					if (!ref_xy_inited) {
 						if (ref_xy_init_start == 0) {
@@ -486,6 +486,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
 		t_prev = t;
 
+		/* reset ground level on arm */
+		if (armed.armed && !flag_armed) {
+			baro_alt0 -= z_est[0];
+			z_est[0] = 0.0f;
+			local_pos.ref_alt = baro_alt0;
+			local_pos.ref_timestamp = hrt_absolute_time();
+			mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0);
+		}
+
 		/* accel bias correction, now only for Z
 		 * not strictly correct, but stable and works */
 		accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
@@ -629,6 +638,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
 		}
+		flag_armed = armed.armed;
 	}
 
 	warnx("exiting.");