From 11e4fbc3745193a61f6f56619318dc1bc0b60307 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 21:49:59 +0200 Subject: Added additional vector functions, fixed seatbelt for global estimators --- src/lib/geo/geo.c | 34 +++++++++++++++++++++++ src/lib/geo/geo.h | 8 +++--- src/modules/commander/state_machine_helper.cpp | 10 ++++--- src/modules/uORB/topics/vehicle_global_position.h | 14 +++++----- 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 __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. */ }; /** -- cgit v1.2.3