From 299c325d20a391fee848c4e43b6b69162e08941a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Nov 2012 15:22:16 +0100 Subject: Defconfig changed to use UART5 --- nuttx/configs/px4fmu/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 91cd0366d..1c0b325b9 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -236,11 +236,11 @@ CONFIG_AT24XX_MTD_BLOCKSIZE=256 CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y -CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART1_SERIAL_CONSOLE=n CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n -CONFIG_UART5_SERIAL_CONSOLE=n +CONFIG_UART5_SERIAL_CONSOLE=y CONFIG_USART6_SERIAL_CONSOLE=n #Mavlink messages can be bigger than 128 -- cgit v1.2.3 From 801cf3af3ec76cc5ff6231e9ac779ea572acee1b Mon Sep 17 00:00:00 2001 From: daregger Date: Mon, 5 Nov 2012 17:17:28 +0100 Subject: add tangent plane mapping to geo.c --- apps/systemlib/geo/geo.c | 144 +++++++++++++++++++++++++++++++++++++++++++++++ apps/systemlib/geo/geo.h | 8 ++- 2 files changed, 151 insertions(+), 1 deletion(-) 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 #include + +/* 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); -- cgit v1.2.3 From 776333a9e54d36e629d1778125775e134a787881 Mon Sep 17 00:00:00 2001 From: daregger Date: Mon, 5 Nov 2012 17:34:04 +0100 Subject: revert console setting --- nuttx/configs/px4fmu/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 1c0b325b9..91cd0366d 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -236,11 +236,11 @@ CONFIG_AT24XX_MTD_BLOCKSIZE=256 CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y -CONFIG_USART1_SERIAL_CONSOLE=n +CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n -CONFIG_UART5_SERIAL_CONSOLE=y +CONFIG_UART5_SERIAL_CONSOLE=n CONFIG_USART6_SERIAL_CONSOLE=n #Mavlink messages can be bigger than 128 -- cgit v1.2.3