TAKEOFF implemented for multirotors, added altitude check to waypoint navigation.

src/modules/att_pos_estimator_ekf/KalmanNav.cpp

@@ -325,7 +325,7 @@ void KalmanNav::updatePublications()
 	_pos.vx = vN;
 	_pos.vy = vE;
 	_pos.vz = vD;
-	_pos.hdg = psi;
+	_pos.yaw = psi;
 
 	// attitude publication
 	_att.timestamp = _pubTimeStamp;

src/modules/commander/commander.cpp

@@ -210,7 +210,7 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
-transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
 
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -1214,7 +1214,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* evaluate the navigation state machine */
-		transition_result_t res = check_navigation_state_machine(&status, &control_mode);
+		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
 
 		if (res == TRANSITION_DENIED) {
 			/* DENIED here indicates bug in the commander */
@@ -1572,7 +1572,7 @@ print_reject_arm(const char *msg)
 }
 
 transition_result_t
-check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode)
+check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
 {
 	transition_result_t res = TRANSITION_DENIED;
 
@@ -1592,8 +1592,12 @@ 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) {
-				/* don't act while taking off */
-				res = TRANSITION_NOT_CHANGED;
+				/* 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 {
+					res = TRANSITION_NOT_CHANGED;
+				}
 
 			} else {
 				if (current_status->condition_landed) {

src/modules/commander/state_machine_helper.cpp

@@ -413,6 +413,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 
 		if (ret == TRANSITION_CHANGED) {
 			current_status->navigation_state = new_navigation_state;
+			control_mode->auto_state = current_status->navigation_state;
 			navigation_state_changed = true;
 		}
 	}

src/modules/mavlink/orb_listener.c

@@ -341,7 +341,7 @@ l_global_position(const struct listener *l)
 	int16_t vz = (int16_t)(global_pos.vz * 100.0f);
 
 	/* heading in degrees * 10, from 0 to 36.000) */
-	uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
+	uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
 
 	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
 					     timestamp / 1000,

src/modules/mavlink/waypoints.c

@@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
  */
 float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
 {
-	//TODO: implement for z once altidude contoller is implemented
-
 	static uint16_t counter;
 
-//	if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y);
 	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
+		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
 
-		double current_x_rad = cur->x / 180.0 * M_PI;
-		double current_y_rad = cur->y / 180.0 * M_PI;
+		double current_x_rad = wp->x / 180.0 * M_PI;
+		double current_y_rad = wp->y / 180.0 * M_PI;
 		double x_rad = lat / 180.0 * M_PI;
 		double y_rad = lon / 180.0 * M_PI;
 
@@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
 
 		const double radius_earth = 6371000.0;
 
-		return radius_earth * c;
+		float dxy = radius_earth * c;
+		float dz = alt - wp->z;
+
+		return sqrtf(dxy * dxy + dz * dz);
 
 	} else {
 		return -1.0f;
@@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			// XXX TODO
 		}
 
-		if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
-
+		if (dist >= 0.f && dist <= orbit) {
 			wpm->pos_reached = true;
-
 		}
-
-//		else
-//		{
-//			if(counter % 100 == 0)
-//				printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
-//		}
+		// check if required yaw reached
+		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
+		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
+		if (fabsf(yaw_err) < 0.05f) {
+			wpm->yaw_reached = true;
+		}
 	}
 
 	//check if the current waypoint was reached
-	if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) {
+	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
 		if (wpm->current_active_wp_id < wpm->size) {
 			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
 
@@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 
 			bool time_elapsed = false;
 
-			if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) {
-				if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
-					time_elapsed = true;
-				}
-			} else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
+			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
 				time_elapsed = true;
 			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
 				time_elapsed = true;
@@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 	// 	mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
 	// }
 
-	check_waypoints_reached(now, global_position , local_position);
+	check_waypoints_reached(now, global_position, local_position);
 
 	return OK;
 }

src/modules/multirotor_pos_control/multirotor_pos_control.c

@@ -54,6 +54,7 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -221,11 +222,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	bool reset_int_xy = true;
 	bool was_armed = false;
 	bool reset_integral = true;
+	bool reset_auto_pos = true;
 
 	hrt_abstime t_prev = 0;
-	/* integrate in NED frame to estimate wind but not attitude offset */
 	const float alt_ctl_dz = 0.2f;
 	const float pos_ctl_dz = 0.05f;
+	const float takeoff_alt_default = 10.0f;
 	float ref_alt = 0.0f;
 	hrt_abstime ref_alt_t = 0;
 	uint64_t local_ref_timestamp = 0;
@@ -414,50 +416,76 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 			} else {
 				/* non-manual mode, use global setpoint */
-				/* init local projection using local position ref */
-				if (local_pos.ref_timestamp != local_ref_timestamp) {
-					global_pos_sp_reproject = true;
-					local_ref_timestamp = local_pos.ref_timestamp;
-					double lat_home = local_pos.ref_lat * 1e-7;
-					double lon_home = local_pos.ref_lon * 1e-7;
-					map_projection_init(lat_home, lon_home);
-					mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home);
-				}
-
-				if (global_pos_sp_reproject) {
-					/* update global setpoint projection */
-					global_pos_sp_reproject = false;
+				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;
+						att_sp.yaw_body = att.yaw;
+						reset_auto_pos = false;
+					}
+				} 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;
+						att_sp.yaw_body = att.yaw;
+						reset_auto_pos = false;
+					}
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
+					// TODO
+					reset_auto_pos = true;
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
+					/* init local projection using local position ref */
+					if (local_pos.ref_timestamp != local_ref_timestamp) {
+						global_pos_sp_reproject = true;
+						local_ref_timestamp = local_pos.ref_timestamp;
+						double lat_home = local_pos.ref_lat * 1e-7;
+						double lon_home = local_pos.ref_lon * 1e-7;
+						map_projection_init(lat_home, lon_home);
+						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home);
+					}
 
-					if (global_pos_sp_valid) {
-						/* global position setpoint valid, use it */
-						double sp_lat = global_pos_sp.lat * 1e-7;
-						double sp_lon = global_pos_sp.lon * 1e-7;
-						/* project global setpoint to local setpoint */
-						map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
+					if (global_pos_sp_reproject) {
+						/* update global setpoint projection */
+						global_pos_sp_reproject = false;
+						if (global_pos_sp_valid) {
+							/* global position setpoint valid, use it */
+							double sp_lat = global_pos_sp.lat * 1e-7;
+							double sp_lon = global_pos_sp.lon * 1e-7;
+							/* project global setpoint to local setpoint */
+							map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
+
+							if (global_pos_sp.altitude_is_relative) {
+								local_pos_sp.z = -global_pos_sp.altitude;
+
+							} else {
+								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
+							}
 
-						if (global_pos_sp.altitude_is_relative) {
-							local_pos_sp.z = -global_pos_sp.altitude;
+							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
 
 						} else {
-							local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
-						}
-
-						mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
+							if (!local_pos_sp_valid) {
+								/* local position setpoint is invalid,
+								 * use current position as setpoint for loiter */
+								local_pos_sp.x = local_pos.x;
+								local_pos_sp.y = local_pos.y;
+								local_pos_sp.z = local_pos.z;
+							}
 
-					} else {
-						if (!local_pos_sp_valid) {
-							/* local position setpoint is invalid,
-							 * use current position as setpoint for loiter */
-							local_pos_sp.x = local_pos.x;
-							local_pos_sp.y = local_pos.y;
-							local_pos_sp.z = local_pos.z;
+							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
 						}
 
-						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;
+				}
 
-					/* 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);
+				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
+					global_pos_sp_reproject = true;
 				}
 
 				/* reset setpoints after non-manual modes */
@@ -575,6 +603,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			reset_int_z = true;
 			reset_int_xy = true;
 			global_pos_sp_reproject = true;
+			reset_auto_pos = true;
 		}
 
 		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -591,6 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.z = z_est[0];
 			local_pos.vz = z_est[1];
 			local_pos.landed = landed;
+			local_pos.yaw = att.yaw;
 
 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
 
@@ -609,7 +610,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (local_pos.v_xy_valid) {
 				global_pos.vx = local_pos.vx;
 				global_pos.vy = local_pos.vy;
-				global_pos.hdg = atan2f(local_pos.vy, local_pos.vx);	// TODO is it correct?
 			}
 
 			if (local_pos.z_valid) {
@@ -623,6 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (local_pos.v_z_valid) {
 				global_pos.vz = local_pos.vz;
 			}
+			global_pos.yaw = local_pos.yaw;
 
 			global_pos.timestamp = t;
 

src/modules/uORB/topics/vehicle_control_mode.h

@@ -82,6 +82,8 @@ struct vehicle_control_mode_s
 
 	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 */
+
+	uint8_t auto_state;	// TEMP navigation state for AUTO modes
 };
 
 /**

src/modules/uORB/topics/vehicle_global_position.h

@@ -72,8 +72,7 @@ struct vehicle_global_position_s
 	float vx; 				/**< Ground X velocity, m/s in NED				 			   */
 	float vy;				/**< Ground Y velocity, m/s in NED							   */
 	float vz;				/**< Ground Z velocity, m/s	in NED 							   */
-	float hdg; 				/**< Compass heading in radians -PI..+PI.					   */
-
+	float yaw; 				/**< Compass heading in radians -PI..+PI.					   */
 };
 
 /**

src/modules/uORB/topics/vehicle_local_position.h

@@ -67,6 +67,8 @@ struct vehicle_local_position_s
 	float vx; 				/**< Ground X Speed (Latitude), m/s in NED */
 	float vy;				/**< Ground Y Speed (Longitude), m/s in NED */
 	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED */
+	/* Heading */
+	float yaw;
 	/* Reference position in GPS / WGS84 frame */
 	bool global_xy;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
 	bool global_z;			/**< true if z is valid and has valid global reference (ref_alt) */