diff options
author | Simon Wilks <sjwilks@gmail.com> | 2013-06-04 00:10:58 +0200 |
---|---|---|
committer | Simon Wilks <sjwilks@gmail.com> | 2013-06-04 00:10:58 +0200 |
commit | f435025d26f49d1d9069282aa72c7e1cb9201773 (patch) | |
tree | eb1e9907db514b5c97140de2392d99409b605e96 /src/modules | |
parent | 6571629dcac252165a210f8a96983fe96be64538 (diff) | |
download | px4-firmware-f435025d26f49d1d9069282aa72c7e1cb9201773.tar.gz px4-firmware-f435025d26f49d1d9069282aa72c7e1cb9201773.tar.bz2 px4-firmware-f435025d26f49d1d9069282aa72c7e1cb9201773.zip |
Completed main implementation and debugging
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 4 | ||||
-rw-r--r-- | src/modules/systemlib/geo/geo.c | 8 | ||||
-rw-r--r-- | src/modules/systemlib/geo/geo.h | 16 |
3 files changed, 23 insertions, 5 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 4ef150da1..c3836bdfa 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -683,7 +683,8 @@ int KalmanNav::correctPos() // fault detetcion float beta = y.dot(S.inverse() * y); - if (beta > _faultPos.get()) { + static int counter = 0; + if (beta > _faultPos.get() && (counter % 10 == 0)) { printf("fault in gps: beta = %8.4f\n", (double)beta); printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", double(y(0) / sqrtf(RPos(0, 0))), @@ -693,6 +694,7 @@ int KalmanNav::correctPos() double(y(4) / sqrtf(RPos(4, 4))), double(y(5) / sqrtf(RPos(5, 5)))); } + counter++; return ret_ok; } diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c index 6746e8ff3..3ac4bd168 100644 --- a/src/modules/systemlib/geo/geo.c +++ b/src/modules/systemlib/geo/geo.c @@ -46,7 +46,7 @@ #include <systemlib/geo/geo.h> #include <nuttx/config.h> -#include <unistd.h> +//#include <unistd.h> #include <pthread.h> #include <stdio.h> #include <math.h> @@ -185,12 +185,12 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; - double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); + double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); const double radius_earth = 6371000.0d; - - return radius_earth * c; + printf("DIST: %.4f\n", radius_earth * c); + return radius_earth * c; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h index 0c0b5c533..84097b49f 100644 --- a/src/modules/systemlib/geo/geo.h +++ b/src/modules/systemlib/geo/geo.h @@ -82,8 +82,24 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y) */ __EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); +/** + * Returns the distance to the next waypoint in meters. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +/** + * Returns the bearing to the next waypoint in radians. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __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); |