forked from Archive/PX4-Autopilot
Added additional vector functions, fixed seatbelt for global estimators
This commit is contained in:
parent
14828cfda5
commit
11e4fbc374
|
@ -210,6 +210,40 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
|
||||||
return theta;
|
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>
|
// 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)
|
__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)
|
||||||
|
|
|
@ -57,10 +57,6 @@ __BEGIN_DECLS
|
||||||
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */
|
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */
|
||||||
#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */
|
#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
|
// XXX remove
|
||||||
struct crosstrack_error_s {
|
struct crosstrack_error_s {
|
||||||
bool past_end; // Flag indicating we are past the end of the line/arc segment
|
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_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_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,
|
__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
|
||||||
|
|
|
@ -228,8 +228,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
|
||||||
|
|
||||||
case MAIN_STATE_SEATBELT:
|
case MAIN_STATE_SEATBELT:
|
||||||
|
|
||||||
/* need altitude estimate */
|
/* need at minimum altitude estimate */
|
||||||
if (current_state->condition_local_altitude_valid) {
|
if (current_state->condition_local_altitude_valid ||
|
||||||
|
current_state->condition_global_position_valid) {
|
||||||
ret = TRANSITION_CHANGED;
|
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:
|
case MAIN_STATE_EASY:
|
||||||
|
|
||||||
/* need local position estimate */
|
/* need at minimum local position estimate */
|
||||||
if (current_state->condition_local_position_valid) {
|
if (current_state->condition_local_position_valid ||
|
||||||
|
current_state->condition_global_position_valid) {
|
||||||
ret = TRANSITION_CHANGED;
|
ret = TRANSITION_CHANGED;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -62,17 +62,17 @@
|
||||||
struct vehicle_global_position_s
|
struct vehicle_global_position_s
|
||||||
{
|
{
|
||||||
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
|
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
|
||||||
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
|
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
|
||||||
bool valid; /**< true if position satisfies validity criteria of estimator */
|
bool valid; /**< true if position satisfies validity criteria of estimator */
|
||||||
|
|
||||||
int32_t lat; /**< Latitude in 1E7 degrees */
|
int32_t lat; /**< Latitude in 1E7 degrees */
|
||||||
int32_t lon; /**< Longitude 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 relative_alt; /**< Altitude above home position in meters, */
|
||||||
float vx; /**< Ground X velocity, m/s in NED */
|
float vx; /**< Ground X velocity, m/s in NED */
|
||||||
float vy; /**< Ground Y velocity, m/s in NED */
|
float vy; /**< Ground Y velocity, m/s in NED */
|
||||||
float vz; /**< Ground Z velocity, m/s in NED */
|
float vz; /**< Ground Z velocity, m/s in NED */
|
||||||
float yaw; /**< Compass heading in radians -PI..+PI. */
|
float yaw; /**< Compass heading in radians -PI..+PI. */
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
Loading…
Reference in New Issue