diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-22 18:44:01 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-22 18:46:04 +0200 |
| commit | a9b21886f32d0a8ef7cad8dfe7efbc3276f4fd58 (patch) | |
| tree | 49205188c750a90d46a429f92294a3dbe19f8bde /apps/multirotor_pos_control/position_control.c | |
| parent | c265e07ae0114d3ecea577aaf4d41d17753d955b (diff) | |
| download | px4-firmware-a9b21886f32d0a8ef7cad8dfe7efbc3276f4fd58.tar.gz px4-firmware-a9b21886f32d0a8ef7cad8dfe7efbc3276f4fd58.tar.bz2 px4-firmware-a9b21886f32d0a8ef7cad8dfe7efbc3276f4fd58.zip | |
Halfway-working fixed wing waypoint control, needs more effort
Diffstat (limited to 'apps/multirotor_pos_control/position_control.c')
| -rw-r--r-- | apps/multirotor_pos_control/position_control.c | 40 |
1 files changed, 0 insertions, 40 deletions
diff --git a/apps/multirotor_pos_control/position_control.c b/apps/multirotor_pos_control/position_control.c index fd731a33d..b98f5bede 100644 --- a/apps/multirotor_pos_control/position_control.c +++ b/apps/multirotor_pos_control/position_control.c @@ -51,46 +51,6 @@ #include "multirotor_position_control.h" -float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; - - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - - const double radius_earth = 6371000.0; - - return radius_earth * c; -} - -float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; - - 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 */ - float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - - // XXX wrapping check is incomplete - if (theta < 0.0f) { - theta = theta + 2.0f * M_PI_F; - } - - return theta; -} - void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) |