Added additional vector functions, fixed seatbelt for global estimators

4 files changed, 51 insertions, 15 deletions

diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 63792dda5..e862b1dc0 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -210,6 +210,40 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	return theta;
 }
 
+__EXPORT float get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+{
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+	double lat_next_rad = lat_next * M_DEG_TO_RAD;
+	double lon_next_rad = lon_next * M_DEG_TO_RAD;
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+	/* conscious mix of double and float trig function to maximize speed and efficiency */
+	*vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad)
+	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
+
+	return theta;
+}
+
+__EXPORT float get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+{
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+	double lat_next_rad = lat_next * M_DEG_TO_RAD;
+	double lon_next_rad = lon_next * M_DEG_TO_RAD;
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+	/* conscious mix of double and float trig function to maximize speed and efficiency */
+	*vy = CONSTANTS_RADIUS_OF_EARTH * d_lon;
+	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
+
+	return theta;
+}
+
 // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
 
 __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index dadec51ec..0459909e4 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -57,10 +57,6 @@ __BEGIN_DECLS
 #define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
 #define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
 
-/* compatibility aliases */
-#define RADIUS_OF_EARTH		CONSTANTS_RADIUS_OF_EARTH
-#define GRAVITY_MSS		CONSTANTS_ONE_G
-
 // XXX remove
 struct crosstrack_error_s {
 	bool past_end;		// Flag indicating we are past the end of the line/arc segment
@@ -116,6 +112,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
  */
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
+__EXPORT float get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+
+__EXPORT float get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+
 __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
 
 __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 3cef10185..316b06127 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -228,8 +228,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 
 		case MAIN_STATE_SEATBELT:
 
-			/* need altitude estimate */
-			if (current_state->condition_local_altitude_valid) {
+			/* need at minimum altitude estimate */
+			if (current_state->condition_local_altitude_valid ||
+				current_state->condition_global_position_valid) {
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -237,8 +238,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 
 		case MAIN_STATE_EASY:
 
-			/* need local position estimate */
-			if (current_state->condition_local_position_valid) {
+			/* need at minimum local position estimate */
+			if (current_state->condition_local_position_valid ||
+				current_state->condition_global_position_valid) {
 				ret = TRANSITION_CHANGED;
 			}
 
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 0fc0ed5c9..143734e37 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -62,17 +62,17 @@
 struct vehicle_global_position_s
 {
 	uint64_t timestamp;		/**< time of this estimate, in microseconds since system start */
-	uint64_t time_gps_usec; /**< GPS timestamp in microseconds							   */
-	bool valid;				/**< true if position satisfies validity criteria of estimator */
+	uint64_t time_gps_usec; 	/**< GPS timestamp in microseconds							   */
+	bool valid;			/**< true if position satisfies validity criteria of estimator */
 
 	int32_t lat;			/**< Latitude in 1E7 degrees							 	   */
 	int32_t lon;			/**< Longitude in 1E7 degrees							 	   */
-	float alt;				/**< Altitude in meters									 	   */
+	float alt;			/**< Altitude in meters									 	   */
 	float relative_alt;		/**< Altitude above home position in meters, 				   */
-	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 yaw; 				/**< Compass heading in radians -PI..+PI.					   */
+	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 yaw; 			/**< Compass heading in radians -PI..+PI.					   */
 };
 
 /**