fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 23:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #294 from sjwilks/hott_v2

Browse Source 
Add GPS Support to the HoTT Telemetry driver
This commit is contained in:
Lorenz Meier
2013-06-04 03:15:40 -07:00
parent 68b884ee6c 82c7e58122
commit e3ee6689fe
6 changed files with 328 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+3 -1
View File
@@ -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;
}
src/modules/systemlib/geo/geo.c
+2 -3
View File
@@ -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)
src/modules/systemlib/geo/geo.h
+16
View File
@@ -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);