diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2012-11-05 23:00:17 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2012-11-05 23:00:17 +0100 |
commit | b842957d8782fb22d2a06c48480ca091adf45f7a (patch) | |
tree | f5b1f0a2c17f9b90fc088b24ab3b1c9268cfd290 | |
parent | 518630268fe176fd8207f2acc442b0bc9f2924d7 (diff) | |
parent | 7d76a8a57b41c82db0712dd0544e67d1ce97d89c (diff) | |
download | px4-firmware-b842957d8782fb22d2a06c48480ca091adf45f7a.tar.gz px4-firmware-b842957d8782fb22d2a06c48480ca091adf45f7a.tar.bz2 px4-firmware-b842957d8782fb22d2a06c48480ca091adf45f7a.zip |
Merge branch 'master' of https://github.com/PX4/Firmware into fw_control
-rw-r--r-- | apps/multirotor_pos_control/multirotor_pos_control.c | 6 | ||||
-rw-r--r-- | apps/systemlib/geo/geo.c | 144 | ||||
-rw-r--r-- | apps/systemlib/geo/geo.h | 8 |
3 files changed, 155 insertions, 3 deletions
diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 474ced731..9821fc7e5 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -196,12 +196,14 @@ multirotor_pos_control_thread_main(int argc, char *argv[]) float x_setpoint = 0.0f; + // XXX enable switching between Vicon and local position estimate /* local pos is the Vicon position */ - att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p * dT; + // XXX just an example, lacks rotation around world-body transformation + att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; att_sp.roll_body = 0.0f; att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.4f; + att_sp.thrust = 0.3f; att_sp.timestamp = hrt_absolute_time(); /* publish new attitude setpoint */ diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index 3709feb15..abe44c69e 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -47,6 +47,150 @@ #include <systemlib/geo/geo.h> #include <math.h> + +/* values for map projection */ +static double phi_1; +static double sin_phi_1; +static double cos_phi_1; +static double lambda_0; +static double scale; + +/** + * Initializes the map transformation. + * + * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + phi_1 = lat_0 / 180.0 * M_PI; + lambda_0 = lon_0 / 180.0 * M_PI; + + sin_phi_1 = sin(phi_1); + cos_phi_1 = cos(phi_1); + + /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale + + /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ + const double r_earth = 6371000; + + double lat1 = phi_1; + double lon1 = lambda_0; + + double lat2 = phi_1 + 0.5 / 180 * M_PI; + double lon2 = lambda_0 + 0.5 / 180 * M_PI; + double sin_lat_2 = sin(lat2); + double cos_lat_2 = cos(lat2); + double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; + + /* 2) calculate distance rho on plane */ + double k_bar = 0; + double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); + + if (0 != c) + k_bar = c / sin(c); + + double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane + double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); + double rho = sqrt(pow(x2, 2) + pow(y2, 2)); + + scale = d / rho; + +} + +/** + * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + double phi = lat / 180.0 * M_PI; + double lambda = lon / 180.0 * M_PI; + + double sin_phi = sin(phi); + double cos_phi = cos(phi); + + double k_bar = 0; + /* using small angle approximation (formula in comment is without aproximation) */ + double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); + + if (0 != c) + k_bar = c / sin(c); + + /* using small angle approximation (formula in comment is without aproximation) */ + *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; + *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; + +// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); +} + +/** + * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + + double x_descaled = x / scale; + double y_descaled = y / scale; + + double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_sphere = 0; + + if (c != 0) + lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); + else + lat_sphere = asin(cos_c * sin_phi_1); + +// printf("lat_sphere = %.10f\n",lat_sphere); + + double lon_sphere = 0; + + if (phi_1 == M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); + + } else if (phi_1 == -M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); + + } else { + + lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); + //using small angle approximation +// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); +// if(denominator != 0) +// { +// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); +// } +// else +// { +// ... +// } + } + +// printf("lon_sphere = %.10f\n",lon_sphere); + + *lat = lat_sphere * 180.0 / M_PI; + *lon = lon_sphere * 180.0 / M_PI; + +} + + __EXPORT 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.0d * M_PI; diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h index c98b4c85d..807e508a2 100644 --- a/apps/systemlib/geo/geo.h +++ b/apps/systemlib/geo/geo.h @@ -55,6 +55,12 @@ typedef struct { float bearing; // Bearing in radians to closest point on line/arc } crosstrack_error_s; +__EXPORT static void map_projection_init(double lat_0, double lon_0); + +__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y); + +__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon); + __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); @@ -69,4 +75,4 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, float _wrap180(float bearing); float _wrap360(float bearing); float _wrapPI(float bearing); -float _wrap2PI(float bearing);
\ No newline at end of file +float _wrap2PI(float bearing); |