diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-06-04 13:25:42 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-06-04 13:25:42 +0200 |
commit | de8186e050d51297a2878676b87bb6d1f8b6f24f (patch) | |
tree | d1d1e50b37bdf21fc629b0f6bc711ce61ebbd84e /src/modules | |
parent | abb024c724435937c1a0ae707dccfa28a48ef559 (diff) | |
parent | e3ee6689fe8fe4e6d6c77767d340f7e732ded85e (diff) | |
download | px4-firmware-de8186e050d51297a2878676b87bb6d1f8b6f24f.tar.gz px4-firmware-de8186e050d51297a2878676b87bb6d1f8b6f24f.tar.bz2 px4-firmware-de8186e050d51297a2878676b87bb6d1f8b6f24f.zip |
Merge branch 'master' of github.com:PX4/Firmware into failsafe_io
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 | 5 | ||||
-rw-r--r-- | src/modules/systemlib/geo/geo.h | 16 |
3 files changed, 21 insertions, 4 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..6463e6489 100644 --- a/src/modules/systemlib/geo/geo.c +++ b/src/modules/systemlib/geo/geo.c @@ -185,12 +185,11 @@ __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; + 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); |