Estoy tratando de desarrollar un algoritmo para controlar el timón del barco ... Me perdí en los algoritmos de geo ... La función no funciona correctamente.

direction WhereToMove(double CurrentLatitude, double CurrentLongitude, double TargetLatitude, double TargetLongitude, double azimuth) {
        double azimuthHysteresis = 5; //straight if the deviation is less than 5 degrees
        double pi = 2 * asin(1.0);
        double target = atan2(TargetLatitude - CurrentLatitude, TargetLongitude - CurrentLongitude) * 180 / pi;
        double delta = azimuth - target;
        if (delta > 180) delta -= 360;
        if (delta < -180) delta += 360;
        if (delta < -2) { 
            return right;
        }
        if (delta > 2) {
            return left;
        }
        return straight; // go straight
    }
0
orst 25 jun. 2020 a las 17:24

2 respuestas

La mejor respuesta

Algunos puntos:

Podrías usar la constante M_PI para pi

Me imagino que quieres que tus ángulos se midan en sentido horario desde el norte. atan2 proporciona un ángulo en sentido antihorario desde el eje x. Esto es fácil de arreglar, usar

atan2( dLon, dLat) 

En lugar de

atan2( dLat, dLon) 

La distancia representada por un grado de longitud es, aproximadamente, cos (lat) veces la distancia representada por un grado de latitud. Por lo tanto, debe escalar su dlon en lo anterior por cos (M_PI / 180.0 * lat). (cos, como todas las funciones matemáticas que tratan ángulos, toma radianes como argumento).

Puede simplificar el cálculo de la diferencia de acimut y objetivo utilizando el resto de la función de biblioteca matemática, como en

delta = remainder( azimuth-target, 360.0)

Esto dará un delta entre -180 y 180

No sé si su código se usará alguna vez cerca de 180E. Yo diría que deberías calcular la diferencia en longitudes como si pudiera, es decir, usar

remainder( TargetLongitude - CurrentLongitude, 360.0)

En lugar de

TargetLongitude - CurrentLongitude

Esto puede parecer OTT, pero he descubierto (por el camino difícil) que es mucho más fácil adquirir el hábito de calcular siempre la diferencia de longitudes de esta manera que rastrear en todas partes del código que esas diferencias se toman cuando se usa el código utilizado a través de 180E.

0
dmuir 25 jun. 2020 a las 15:01

Versión definitiva

direction WhereToMove(double CurrentLatitude, double CurrentLongitude, double TargetLatitude, double TargetLongitude, double azimuth) {
            double azimuthHysteresis = 2; //straight if the deviation is less than 2 degrees
            double target = atan2(remainder(TargetLongitude - CurrentLongitude, 360.0), remainder(TargetLatitude - CurrentLatitude, 360.0)) * 180 / M_PI ;
            double delta = remainder(azimuth - target, 360.0);
            double deltaModule = sqrt(delta * delta);
            if (deltaModule <= azimuthHysteresis) //deviation is small go straight
            {
                return straight;
            }
            if (delta <= -azimuthHysteresis) {
                return right;
            }
            if (delta >= azimuthHysteresis) {
                return left;
            }
            return straight; // go straight
        }
0
orst 25 jun. 2020 a las 17:38