Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 16:30:07 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:paparazzi/paparazzi into opticflow

Browse Source
This commit is contained in:
dewagter
2015-01-13 20:29:21 +01:00
parent 98c27f257c a6ce60069e
commit a7f42a603b
13 changed files with 167 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/bebop.xml
+8 -8
View File
@@ -40,10 +40,10 @@
</commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="3000" neutral="3000" max="12000"/>
<servo name="TOP_RIGHT" no="1" min="3000" neutral="3000" max="12000"/>
<servo name="BOTTOM_RIGHT" no="2" min="3000" neutral="3000" max="12000"/>
<servo name="BOTTOM_LEFT" no="3" min="3000" neutral="3000" max="12000"/>
<servo name="TOP_RIGHT" no="0" min="3000" neutral="3000" max="12000"/>
<servo name="TOP_LEFT" no="1" min="3000" neutral="3000" max="12000"/>
<servo name="BOTTOM_LEFT" no="2" min="3000" neutral="3000" max="12000"/>
<servo name="BOTTOM_RIGHT" no="3" min="3000" neutral="3000" max="12000"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
@@ -55,9 +55,9 @@
<define name="MAX_SATURATION_OFFSET" value="3*MAX_PPRZ"/>
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
<define name="ROLL_COEF" value="{ -256, 256, 256, -256 }"/>
<define name="ROLL_COEF" value="{ 256, -256, -256, 256 }"/>
<define name="PITCH_COEF" value="{ 256, 256, -256, -256 }"/>
<define name="YAW_COEF" value="{ 256, -256, 256, -256 }"/>
<define name="YAW_COEF" value="{ -256, 256, -256, 256 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
</section>
@@ -195,8 +195,8 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_ardrone2&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_ardrone2.h&quot;"/>
<define name="JSBSIM_MODEL" value="&quot;simple_x_quad_ccw&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>
<section name="AUTOPILOT">
conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile
+1 -1
View File
@@ -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
conf/firmwares/subsystems/shared/actuators_bebop.makefile
+2 -2
View File
@@ -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
sw/airborne/test/math/compare_utm_enu.py
Executable
+79
View File
@@ -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()
sw/lib/python/math/.gitignore
+3
View File
@@ -0,0 +1,3 @@
build
*.c
pprz_geodetic.py
sw/lib/python/math/Makefile
+7
View File
@@ -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
sw/lib/python/math/__init__.py
View File
sw/lib/python/math/pprz_geodetic.i
+10
View File
@@ -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
%}
sw/lib/python/math/pprz_geodetic_double.i
+7
View File
@@ -0,0 +1,7 @@
/* File : pprz_geodetic_double.i */
%module geodetic_double
%{
#include "math/pprz_geodetic_double.h"
%}
%include "math/pprz_geodetic_double.h"
sw/lib/python/math/pprz_geodetic_float.i
+7
View File
@@ -0,0 +1,7 @@
/* File : pprz_geodetic_float.i */
%module geodetic_float
%{
#include "math/pprz_geodetic_float.h"
%}
%include "math/pprz_geodetic_float.h"
sw/lib/python/math/pprz_geodetic_int.i
+8
View File
@@ -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"
sw/lib/python/math/setup.py
+32
View File
@@ -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"],
)
sw/logalizer/plot.ml
+3 -3
View File
@@ -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.