dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added additional vector functions, fixed seatbelt for global estimators

This commit is contained in:
Lorenz Meier 2013-09-08 21:49:59 +02:00
parent 14828cfda5
commit 11e4fbc374
4 changed files with 51 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/lib/geo/geo.c
View File

@ -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)

8
src/lib/geo/geo.h
View File

@ -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,

10
src/modules/commander/state_machine_helper.cpp
View File

@ -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;
}