diff --git a/conf/airframes/bebop.xml b/conf/airframes/bebop.xml index 994f08babf..9d16431af6 100644 --- a/conf/airframes/bebop.xml +++ b/conf/airframes/bebop.xml @@ -40,10 +40,10 @@ - - - - + + + +
@@ -55,9 +55,9 @@ - + - +
@@ -195,8 +195,8 @@
- - + +
diff --git a/conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile b/conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile index 7d12d6adb2..49f778847b 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile @@ -19,6 +19,6 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c $(SRC_SUBSYSTEMS)/gps/gps_furuno.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DUSE_GPS -nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/shared/actuators_bebop.makefile b/conf/firmwares/subsystems/shared/actuators_bebop.makefile index b5378a6337..341f84a6e9 100644 --- a/conf/firmwares/subsystems/shared/actuators_bebop.makefile +++ b/conf/firmwares/subsystems/shared/actuators_bebop.makefile @@ -7,5 +7,5 @@ BEBOP_ACTUATORS_I2C_DEV_LOWER=$(shell echo $(BEBOP_ACTUATORS_I2C_DEV) | tr A-Z a BEBOP_ACTUATORS_CFLAGS += -DBEBOP_ACTUATORS_I2C_DEV=$(BEBOP_ACTUATORS_I2C_DEV_LOWER) -DUSE_$(BEBOP_ACTUATORS_I2C_DEV_UPPER)=1 -$(TARGET).CFLAGS += -DACTUATORS $(BEBOP_ACTUATORS_CFLAGS) -$(TARGET).srcs += $(SRC_BOARD)/actuators.c +ap.CFLAGS += -DACTUATORS $(BEBOP_ACTUATORS_CFLAGS) +ap.srcs += $(SRC_BOARD)/actuators.c diff --git a/sw/airborne/test/math/compare_utm_enu.py b/sw/airborne/test/math/compare_utm_enu.py new file mode 100755 index 0000000000..ee5cd4938d --- /dev/null +++ b/sw/airborne/test/math/compare_utm_enu.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python + +from __future__ import division, print_function, absolute_import +import sys +import os + +PPRZ_SRC = os.getenv("PAPARAZZI_SRC", "../../../..") +sys.path.append(PPRZ_SRC + "/sw/lib/python/math") + +from pprz_geodetic import * +from math import degrees +import matplotlib.pyplot as plt +import numpy as np + +# Origin at ENAC +UTM_EAST0 = 377349 # in m +UTM_NORTH0 = 4824583 # in m +UTM_ZONE0 = 31 +ALT0 = 147.000 # in m + +utm_origin = UtmCoor_d() +utm_origin.north = UTM_NORTH0 +utm_origin.east = UTM_EAST0 +utm_origin.zone = UTM_ZONE0 +utm_origin.alt = ALT0 +print("utm origin xyz [%.3f, %.3f, %.3f]" % (utm_origin.east, utm_origin.north, utm_origin.alt)) + +lla_origin = LlaCoor_d() +lla_of_utm_d(lla_origin, utm_origin) +print("lla origin lat/lon/alt [%.7f, %.7f, %.3f]" % (degrees(lla_origin.lat), degrees(lla_origin.lon), lla_origin.alt)) +ecef_origin = EcefCoor_d() +ecef_of_lla_d(ecef_origin, lla_origin) +print("ecef origin xyz [%.3f, %.3f, %.3f]" % (ecef_origin.x, ecef_origin.y, ecef_origin.z)) +ltp_origin = LtpDef_d() +ltp_def_from_ecef_d(ltp_origin, ecef_origin) + +# calculate ENU coordinates for 100 points in 100m distance +nb_points = 100 +dist_points = 100 +enu_res = np.zeros((nb_points, 2)) +utm_res = np.zeros((nb_points, 2)) +for i in range(0, nb_points): + utm = UtmCoor_d() + utm.north = i * dist_points + UTM_NORTH0 + utm.east = i * dist_points + UTM_EAST0 + utm.alt = ALT0 + utm.zone = UTM_ZONE0 + #print("utm x=%.3f, y=%.3f" % (utm.east, utm.north)) + utm_res[i, 0] = utm.east - UTM_EAST0 + utm_res[i, 1] = utm.north - UTM_NORTH0 + lla = LlaCoor_d() + lla_of_utm_d(lla, utm) + #print("lla lat/lon/alt [%.7f, %.7f, %.3f]" % (degrees(lla.lat), degrees(lla.lon), lla.alt)) + ecef = EcefCoor_d() + ecef_of_lla_d(ecef, lla) + enu = EnuCoor_d() + enu_of_ecef_point_d(enu, ltp_origin, ecef) + enu_res[i, 0] = enu.x + enu_res[i, 1] = enu.y + #print("enu x=%.3f, y=%.3f" % (enu.x, enu.y)) + + +dist = np.linalg.norm(utm_res, axis=1) +error = np.linalg.norm(utm_res - enu_res, axis=1) + +plt.figure(1) +plt.subplot(211) +plt.title("utm vs. enu") +plt.plot(enu_res[:, 0], enu_res[:, 1], 'g', label="ENU") +plt.plot(utm_res[:, 0], utm_res[:, 1], 'r', label="UTM") +plt.ylabel("y/north [m]") +plt.xlabel("x/east [m]") +plt.legend(loc='upper left') +plt.subplot(212) +plt.plot(dist, error, 'r') +plt.xlabel("dist from origin [m]") +plt.ylabel("error [m]") + +plt.show() \ No newline at end of file diff --git a/sw/lib/python/math/.gitignore b/sw/lib/python/math/.gitignore new file mode 100644 index 0000000000..391a60a8c1 --- /dev/null +++ b/sw/lib/python/math/.gitignore @@ -0,0 +1,3 @@ +build +*.c +pprz_geodetic.py diff --git a/sw/lib/python/math/Makefile b/sw/lib/python/math/Makefile new file mode 100644 index 0000000000..a0c966a86c --- /dev/null +++ b/sw/lib/python/math/Makefile @@ -0,0 +1,7 @@ + +all: + swig2.0 -python -keyword -I../../../airborne pprz_geodetic.i + python setup.py build_ext --inplace + +clean: + rm -rf build *.so *.c pprz_geodetic.py diff --git a/sw/lib/python/math/__init__.py b/sw/lib/python/math/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sw/lib/python/math/pprz_geodetic.i b/sw/lib/python/math/pprz_geodetic.i new file mode 100644 index 0000000000..af337fc36c --- /dev/null +++ b/sw/lib/python/math/pprz_geodetic.i @@ -0,0 +1,10 @@ +/* File : pprz_geodetic.i */ +%module pprz_geodetic +%feature("autodoc", "3"); +%include "typemaps.i" +%include pprz_geodetic_int.i +%include pprz_geodetic_float.i +%include pprz_geodetic_double.i +%{ +#define SWIG_FILE_WITH_INIT +%} diff --git a/sw/lib/python/math/pprz_geodetic_double.i b/sw/lib/python/math/pprz_geodetic_double.i new file mode 100644 index 0000000000..e6c0223711 --- /dev/null +++ b/sw/lib/python/math/pprz_geodetic_double.i @@ -0,0 +1,7 @@ +/* File : pprz_geodetic_double.i */ +%module geodetic_double +%{ +#include "math/pprz_geodetic_double.h" +%} + +%include "math/pprz_geodetic_double.h" diff --git a/sw/lib/python/math/pprz_geodetic_float.i b/sw/lib/python/math/pprz_geodetic_float.i new file mode 100644 index 0000000000..b252967238 --- /dev/null +++ b/sw/lib/python/math/pprz_geodetic_float.i @@ -0,0 +1,7 @@ +/* File : pprz_geodetic_float.i */ +%module geodetic_float +%{ +#include "math/pprz_geodetic_float.h" +%} + +%include "math/pprz_geodetic_float.h" diff --git a/sw/lib/python/math/pprz_geodetic_int.i b/sw/lib/python/math/pprz_geodetic_int.i new file mode 100644 index 0000000000..210c784d75 --- /dev/null +++ b/sw/lib/python/math/pprz_geodetic_int.i @@ -0,0 +1,8 @@ +/* File : pprz_geodetic_int.i */ +%module geodetic_int +%include "stdint.i" +%{ +#include "math/pprz_geodetic_int.h" +%} + +%include "math/pprz_geodetic_int.h" diff --git a/sw/lib/python/math/setup.py b/sw/lib/python/math/setup.py new file mode 100644 index 0000000000..90a971ff3c --- /dev/null +++ b/sw/lib/python/math/setup.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python + +""" +setup.py file for pprz_geodetic math wrapper +""" + +from distutils.core import setup, Extension + +from os import path, getenv + +# if PAPARAZZI_SRC not set, then assume the tree containing this +# file is a reasonable substitute +pprz_src = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../'))) +pprz_airborne = path.join(pprz_src, "sw/airborne") + +common_inc_dirs = ["./", path.join(pprz_src, "sw/include"), pprz_airborne] + +pprz_geodetic_module = Extension('_pprz_geodetic', + sources=['pprz_geodetic_wrap.c', + path.join(pprz_airborne, 'math/pprz_geodetic_int.c'), + path.join(pprz_airborne, 'math/pprz_geodetic_double.c'), + path.join(pprz_airborne, 'math/pprz_geodetic_float.c') + ], + include_dirs=common_inc_dirs) + +setup(name='geodetic_double', + version='0.1', + author="Felix Ruess", + description="""Pprz geodetic math wrapper""", + ext_modules=[pprz_geodetic_module], + py_modules=["pprz_geodetic"], + ) diff --git a/sw/logalizer/plot.ml b/sw/logalizer/plot.ml index 73e3e002f1..3fad853b71 100644 --- a/sw/logalizer/plot.ml +++ b/sw/logalizer/plot.ml @@ -326,7 +326,7 @@ class plot = fun ~width ~height ~packing () -> let points = ref [] in Array.iteri (fun i (t, v) -> if t > min_x && t <= max_x && (i mod step = 0) then points := List.rev_append [(scale_x t, scale_y v)] !points) curve.values; renderer#set_color curve.color; - renderer#lines !points; + if List.length !points > 0 then renderer#lines !points; (* Title *) let (w, h) = renderer#create_text title in @@ -512,8 +512,8 @@ let pprz_float = function | Pprz.Float f -> f | Pprz.Int32 i -> Int32.to_float i | Pprz.Int64 i -> Int64.to_float i - | Pprz.String s -> float_of_string s - | Pprz.Char c -> float_of_string (String.make 1 c) + | Pprz.String s -> let v = try float_of_string s with _ -> 0. in v + | Pprz.Char c -> let v = try float_of_string (String.make 1 c) with _ -> 0. in v | Pprz.Array _ -> 0.