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

Merge branch 'master' into ahrs_wrapper

Browse Source 
* master: (33 commits)
  [ground_segment] use pkg-config for ivy-c if available
  [tests] compare ENU to UTM with north fix
  [python][math] some DoubleRMat additions
  [python] conversion methods for geodetic float/double
  [python] improve algebra int wrapper
  [python] nicer wrappers for float and double algebra
  [python] rename math to pprz_math and add algebra module
  [python][math] improve swig wrapping
  [log] prevent display when no points
  [conf] bebop simple_x_quad_ccw as jsbsim model
  bebop actuators left/right reversed
  [plot] fix loading of logs with strings that are not representing a float
  [conf] fix bebop makefiles for NPS
  [python] simplify pprz_geodetic swig wrapper
  [test] simple python script to compare UTM vs. ENU
  [python] wrap pprz_geodetic with swig
  [arch/linux] GCC diagnostic pragma outside of i2c_submit function
  version v5.5_devel: embarking upon road to next release
  release v5.4.0 stable version
  [navigation] dox fixes
  ...
This commit is contained in:
Felix Ruess
2015-01-16 14:19:29 +01:00
parent 2b94da3284 912e5e5113
commit c2ab7f22a6
54 changed files with 1984 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOG.md
+26 -1
View File
@@ -1,3 +1,28 @@
Paparazzi 5.4.0_stable
======================
Stable version release.
- messages: MOVE_WP and WP_MOVED_LLA with altitude in mm
- ins: immediately effective ins_reset_local_origin
- server: don't fail if settings_modules is missing
- python: messagesapp: growable value column width
- python: add python real time plotter to control panel
- module: mavlink, parse PARAM_SET, send origin and waypoints
- module: FLAPS, HATCH, BRAKE via RC in AUTO1
[#878] (https://github.com/paparazzi/paparazzi/pull/878)
- fixedwing: use min/max_cruise_throttle variables instead of defines
[#1057] (https://github.com/paparazzi/paparazzi/issues/1057)
- GCS: turn off search in flight plan panel to fix hotkeys
[#995] (https://github.com/paparazzi/paparazzi/issues/995)
[#1061] (https://github.com/paparazzi/paparazzi/pull/1061)
- GCS: fixes for flight plan editor and keeping map center on zoom
[#927] (https://github.com/paparazzi/paparazzi/issues/927)
[#1059] (https://github.com/paparazzi/paparazzi/pull/1059)
- stm32/luftboot: update libopencm3 to get usb control call back fix
[#1058] (https://github.com/paparazzi/paparazzi/pull/1058)
Paparazzi 5.3.3_testing
=======================
@@ -30,7 +55,7 @@ Third release candidate for v5.4 stable release.
- fixedwing: airspeed tuning
[#877] (https://github.com/paparazzi/paparazzi/pull/877)
- linux: handle SIGINT for console debug
[#1008] (https://github.com/paparazzi/paparazzi/pull/
[#1008] (https://github.com/paparazzi/paparazzi/pull/1008)
- rotorcraft: always allow to switch to MODE_MANUAL via RC
[#1036] (https://github.com/paparazzi/paparazzi/pull/1036)
- flight plan: use flight plan position in nps for most example 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
conf/messages.xml
+2 -2
View File
@@ -356,7 +356,7 @@
<field name="wp_id" type="uint8"/>
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt" type="int32" unit="cm"/>
<field name="alt" type="int32" unit="mm" alt_unit="m">Height above Mean Sea Level (geoid)</field>
</message>
<message name="CHRONO" id="48">
@@ -2223,7 +2223,7 @@
<field name="ac_id" type="uint8"/>
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt" type="int32" unit="cm"/>
<field name="alt" type="int32" unit="mm" alt_unit="m">Height above Mean Sea Level (geoid)</field>
</message>
<message name="WIND_INFO" id="3" link="forwarded">
paparazzi_version
+1 -1
View File
@@ -1,6 +1,6 @@
#!/bin/sh
DEF_VER=v5.3.3_testing
DEF_VER=v5.5_devel
# First try git describe (if running on a git repo),
# then use default version from above (for release tarballs).
sw/airborne/arch/linux/mcu_periph/i2c_arch.c
+4 -5
View File
@@ -46,6 +46,8 @@ bool_t i2c_idle(struct i2c_periph *p __attribute__((unused)))
return TRUE;
}
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t)
{
int file = (int)p->reg_addr;
@@ -53,9 +55,6 @@ bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t)
// Set the slave address
ioctl(file, I2C_SLAVE, t->slave_addr);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
// Switch the different transaction types
switch (t->type) {
// Just transmitting
@@ -84,12 +83,12 @@ bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t)
break;
}
#pragma GCC diagnostic pop
// Successfull transfer
t->status = I2CTransSuccess;
return TRUE;
}
#pragma GCC diagnostic pop
#if USE_I2C0
struct i2c_errors i2c0_errors;
sw/airborne/arch/sim/jsbsim_transport.c
+2 -1
View File
@@ -7,6 +7,7 @@
#include <stdlib.h>
#define MOfCm(_x) (((float)(_x))/100.)
#define MOfMM(_x) (((float)(_x))/1000.)
void parse_dl_ping(char *argv[] __attribute__((unused)))
{
@@ -52,7 +53,7 @@ void parse_dl_block(char *argv[])
void parse_dl_move_wp(char *argv[])
{
uint8_t wp_id = atoi(argv[1]);
float a = MOfCm(atoi(argv[5]));
float a = MOfMM(atoi(argv[5]));
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla;
sw/airborne/firmwares/fixedwing/datalink.c
+2 -1
View File
@@ -75,6 +75,7 @@ uint8_t joystick_block;
#endif
#define MOfCm(_x) (((float)(_x))/100.)
#define MOfMM(_x) (((float)(_x))/1000.)
#define SenderIdOfMsg(x) (x[0])
#define IdOfMsg(x) (x[1])
@@ -121,7 +122,7 @@ void dl_parse_msg(void)
#ifdef NAV
if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
float a = MOfMM(DL_MOVE_WP_alt(dl_buffer));
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla;
sw/airborne/firmwares/rotorcraft/datalink.c
+2 -2
View File
@@ -103,10 +103,10 @@ void dl_parse_msg(void)
struct LlaCoor_i lla;
lla.lat = DL_MOVE_WP_lat(dl_buffer);
lla.lon = DL_MOVE_WP_lon(dl_buffer);
/* WP_alt from message is alt above MSL in cm
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(dl_buffer) * 10 - state.ned_origin_i.hmsl +
lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
nav_move_waypoint_lla(wp_id, &lla);
}
sw/airborne/math/pprz_algebra_double.c
+64
View File
@@ -110,3 +110,67 @@ void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct D
v_out->y = 2 * (m10 * v_in->x + m11 * v_in->y + m12 * v_in->z);
v_out->z = 2 * (m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
}
void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
{
/*RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);*/
RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
RMAT_ELMT(*m_b2a, 0, 2) = RMAT_ELMT(*m_a2b, 2, 0);
RMAT_ELMT(*m_b2a, 1, 0) = RMAT_ELMT(*m_a2b, 0, 1);
/*RMAT_ELMT(*m_b2a, 1, 1) = RMAT_ELMT(*m_a2b, 1, 1);*/
RMAT_ELMT(*m_b2a, 1, 2) = RMAT_ELMT(*m_a2b, 2, 1);
RMAT_ELMT(*m_b2a, 2, 0) = RMAT_ELMT(*m_a2b, 0, 2);
RMAT_ELMT(*m_b2a, 2, 1) = RMAT_ELMT(*m_a2b, 1, 2);
/*RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);*/
}
/** Composition (multiplication) of two rotation matrices.
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
*/
void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b, struct DoubleRMat *m_b2c)
{
m_a2c->m[0] = m_b2c->m[0] * m_a2b->m[0] + m_b2c->m[1] * m_a2b->m[3] + m_b2c->m[2] * m_a2b->m[6];
m_a2c->m[1] = m_b2c->m[0] * m_a2b->m[1] + m_b2c->m[1] * m_a2b->m[4] + m_b2c->m[2] * m_a2b->m[7];
m_a2c->m[2] = m_b2c->m[0] * m_a2b->m[2] + m_b2c->m[1] * m_a2b->m[5] + m_b2c->m[2] * m_a2b->m[8];
m_a2c->m[3] = m_b2c->m[3] * m_a2b->m[0] + m_b2c->m[4] * m_a2b->m[3] + m_b2c->m[5] * m_a2b->m[6];
m_a2c->m[4] = m_b2c->m[3] * m_a2b->m[1] + m_b2c->m[4] * m_a2b->m[4] + m_b2c->m[5] * m_a2b->m[7];
m_a2c->m[5] = m_b2c->m[3] * m_a2b->m[2] + m_b2c->m[4] * m_a2b->m[5] + m_b2c->m[5] * m_a2b->m[8];
m_a2c->m[6] = m_b2c->m[6] * m_a2b->m[0] + m_b2c->m[7] * m_a2b->m[3] + m_b2c->m[8] * m_a2b->m[6];
m_a2c->m[7] = m_b2c->m[6] * m_a2b->m[1] + m_b2c->m[7] * m_a2b->m[4] + m_b2c->m[8] * m_a2b->m[7];
m_a2c->m[8] = m_b2c->m[6] * m_a2b->m[2] + m_b2c->m[7] * m_a2b->m[5] + m_b2c->m[8] * m_a2b->m[8];
}
/** rotate 3D vector by rotation matrix.
* vb = m_a2b * va
*/
void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b, struct DoubleVect3 *va)
{
vb->x = m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z;
vb->y = m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z;
vb->z = m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z;
}
/* C n->b rotation matrix */
void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q)
{
const double _a = M_SQRT2 * q->qi;
const double _b = M_SQRT2 * q->qx;
const double _c = M_SQRT2 * q->qy;
const double _d = M_SQRT2 * q->qz;
const double a2_1 = _a * _a - 1;
const double ab = _a * _b;
const double ac = _a * _c;
const double ad = _a * _d;
const double bc = _b * _c;
const double bd = _b * _d;
const double cd = _c * _d;
RMAT_ELMT(*rm, 0, 0) = a2_1 + _b * _b;
RMAT_ELMT(*rm, 0, 1) = bc + ad;
RMAT_ELMT(*rm, 0, 2) = bd - ac;
RMAT_ELMT(*rm, 1, 0) = bc - ad;
RMAT_ELMT(*rm, 1, 1) = a2_1 + _c * _c;
RMAT_ELMT(*rm, 1, 2) = cd + ab;
RMAT_ELMT(*rm, 2, 0) = bd + ac;
RMAT_ELMT(*rm, 2, 1) = cd - ab;
RMAT_ELMT(*rm, 2, 2) = a2_1 + _d * _d;
}
sw/airborne/math/pprz_algebra_double.h
+23
View File
@@ -153,6 +153,29 @@ extern void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e);
extern void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q);
extern void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in);
/** initialises a rotation matrix to identity */
static inline void double_rmat_identity(struct DoubleRMat *rm)
{
FLOAT_MAT33_DIAG(*rm, 1., 1., 1.);
}
/** Inverse/transpose of a rotation matrix.
* m_b2a = inv(_m_a2b) = transp(_m_a2b)
*/
extern void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b);
/** Composition (multiplication) of two rotation matrices.
* m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b
*/
extern void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b,
struct DoubleRMat *m_b2c);
/** rotate 3D vector by rotation matrix.
* vb = m_a2b * va
*/
extern void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b,
struct DoubleVect3 *va);
extern void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q);
static inline void double_rmat_of_eulers(struct DoubleRMat *rm, struct DoubleEulers *e)
{
double_rmat_of_eulers_321(rm, e);
sw/airborne/math/pprz_geodetic_double.h
+1 -1
View File
@@ -52,8 +52,8 @@ struct EcefCoor_d {
* @brief vector in Latitude, Longitude and Altitude
*/
struct LlaCoor_d {
double lon; ///< in radians
double lat; ///< in radians
double lon; ///< in radians
double alt; ///< in meters above WGS84 reference ellipsoid
};
sw/airborne/math/pprz_geodetic_float.h
+1 -1
View File
@@ -52,8 +52,8 @@ struct EcefCoor_f {
* @brief vector in Latitude, Longitude and Altitude
*/
struct LlaCoor_f {
float lon; ///< in radians
float lat; ///< in radians
float lon; ///< in radians
float alt; ///< in meters above WGS84 reference ellipsoid
};
sw/airborne/math/pprz_geodetic_int.h
+1 -1
View File
@@ -54,8 +54,8 @@ struct EcefCoor_i {
* @brief vector in Latitude, Longitude and Altitude
*/
struct LlaCoor_i {
int32_t lon; ///< in degrees*1e7
int32_t lat; ///< in degrees*1e7
int32_t lon; ///< in degrees*1e7
int32_t alt; ///< in millimeters above WGS84 reference ellipsoid
};
sw/airborne/subsystems/ins.c
+22 -1
View File
@@ -40,7 +40,28 @@ struct Ins ins;
void WEAK ins_periodic(void) {}
void WEAK ins_reset_local_origin(void) {}
void WEAK ins_reset_local_origin(void)
{
#if USE_GPS
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
#else
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
#endif
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
// reset state UTM ref
stateSetLocalUtmOrigin_f(&utm);
#endif
}
void WEAK ins_reset_altitude_ref(void) {}
sw/airborne/subsystems/ins/ins_ardrone2.c
+18 -5
View File
@@ -78,19 +78,32 @@ void ins_periodic(void)
void ins_reset_local_origin(void)
{
#if USE_GPS
if (gps.fix == GPS_FIX_3D) {
ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
ins_impl.ltp_def.hmsl = gps.hmsl;
ins_impl.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
}
else {
ins_impl.ltp_initialized = FALSE;
}
#else
ins_impl.ltp_initialized = FALSE;
#endif
}
void ins_reset_altitude_ref(void)
{
#if USE_GPS
struct LlaCoor_i lla = {
state.ned_origin_i.lla.lon,
state.ned_origin_i.lla.lat,
gps.lla_pos.alt
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
ltp_def_from_lla_i(&ins_impl.ltp_def, &lla),
ins_impl.ltp_def.hmsl = gps.hmsl;
ltp_def_from_lla_i(&ins_impl.ltp_def, &lla);
ins_impl.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
#endif
}
sw/airborne/subsystems/ins/ins_float_invariant.c
+5 -5
View File
@@ -315,13 +315,13 @@ void ins_reset_altitude_ref(void)
stateSetLocalUtmOrigin_f(&utm);
#else
struct LlaCoor_i lla = {
state.ned_origin_i.lla.lon,
state.ned_origin_i.lla.lat,
gps.lla_pos.alt
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
struct LtpDef_i ltp_def;
ltp_def_from_lla_i(&ltp_def, &lla),
ltp_def.hmsl = gps.hmsl;
ltp_def_from_lla_i(&ltp_def, &lla);
ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ltp_def);
#endif
}
sw/airborne/subsystems/ins/ins_gps_passthrough.c
+5 -5
View File
@@ -139,12 +139,12 @@ void ins_reset_local_origin(void)
void ins_reset_altitude_ref(void)
{
struct LlaCoor_i lla = {
state.ned_origin_i.lla.lon,
state.ned_origin_i.lla.lat,
gps.lla_pos.alt
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
ltp_def_from_lla_i(&ins_impl.ltp_def, &lla),
ins_impl.ltp_def.hmsl = gps.hmsl;
ltp_def_from_lla_i(&ins_impl.ltp_def, &lla);
ins_impl.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
}
sw/airborne/subsystems/ins/ins_int.c
+19 -5
View File
@@ -203,7 +203,21 @@ void ins_periodic(void)
void ins_reset_local_origin(void)
{
#if USE_GPS
if (gps.fix == GPS_FIX_3D) {
ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
ins_impl.ltp_def.hmsl = gps.hmsl;
ins_impl.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
}
else {
ins_impl.ltp_initialized = FALSE;
}
#else
ins_impl.ltp_initialized = FALSE;
#endif
#if USE_HFF
ins_impl.hf_realign = TRUE;
#endif
@@ -214,12 +228,12 @@ void ins_reset_altitude_ref(void)
{
#if USE_GPS
struct LlaCoor_i lla = {
state.ned_origin_i.lla.lon,
state.ned_origin_i.lla.lat,
gps.lla_pos.alt
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
ltp_def_from_lla_i(&ins_impl.ltp_def, &lla),
ins_impl.ltp_def.hmsl = gps.hmsl;
ltp_def_from_lla_i(&ins_impl.ltp_def, &lla);
ins_impl.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
#endif
ins_impl.vf_reset = TRUE;
sw/airborne/subsystems/navigation/common_flight_plan.h
+3 -3
View File
@@ -20,7 +20,7 @@
*/
/**
* @file subsystems/navigation/common_flight_plan.c
* @file subsystems/navigation/common_flight_plan.h
* Common flight_plan functions shared between fixedwing and rotorcraft.
*/
@@ -35,8 +35,8 @@ extern uint16_t stage_time, block_time;
extern uint8_t nav_stage, nav_block;
extern uint8_t last_block, last_stage;
void nav_init_stage(void); /* needs to be implemented by fixedwing and rotorcraft seperately */
/** needs to be implemented by fixedwing and rotorcraft seperately */
void nav_init_stage(void);
void nav_init_block(void);
void nav_goto_block(uint8_t block_id);
sw/airborne/test/math/compare_utm_enu.py
Executable
+90
View File
@@ -0,0 +1,90 @@
#!/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")
from pprz_math.geodetic import *
from pprz_math.algebra import DoubleRMat, DoubleEulers, DoubleVect3
from math import radians, degrees, tan
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(north=UTM_NORTH0, east=UTM_EAST0, alt=ALT0, zone=UTM_ZONE0)
print("origin %s" % utm_origin)
lla_origin = utm_origin.to_lla()
ecef_origin = lla_origin.to_ecef()
ltp_origin = ecef_origin.to_ltp_def()
print(ltp_origin)
# convergence angle to "true north" is approx 1 deg here
earth_radius = 6378137.0
n = 0.9996 * earth_radius
UTM_DELTA_EAST = 500000.
dist_to_meridian = utm_origin.east - UTM_DELTA_EAST
conv = dist_to_meridian / n * tan(lla_origin.lat)
# or (middle meridian of UTM zone 31 is at 3deg)
#conv = atan(tan(lla_origin.lon - radians(3))*sin(lla_origin.lat))
print("approx. convergence angle (north error compared to meridian): %f deg" % degrees(conv))
# Rotation matrix to correct for "true north"
R = DoubleEulers(psi=-conv).to_rmat()
# calculate ENU coordinates for 100 points in 100m distance
nb_points = 100
dist_points = 100
enu_res = np.zeros((nb_points, 2))
enu_res_c = 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_origin.north
utm.east = i * dist_points+ utm_origin.east
utm.alt = utm_origin.alt
utm.zone = utm_origin.zone
#print(utm)
utm_res[i, 0] = utm.east - utm_origin.east
utm_res[i, 1] = utm.north - utm_origin.north
lla = utm.to_lla()
#print(lla)
ecef = lla.to_ecef()
enu = ecef.to_enu(ltp_origin)
enu_res[i, 0] = enu.x
enu_res[i, 1] = enu.y
enu_c = R * DoubleVect3(enu.x, enu.y, enu.z)
enu_res_c[i, 0] = enu_c.x
enu_res_c[i, 1] = enu_c.y
#print(enu)
dist = np.linalg.norm(utm_res, axis=1)
error = np.linalg.norm(utm_res - enu_res, axis=1)
error_c = np.linalg.norm(utm_res - enu_res_c, axis=1)
plt.figure(1)
plt.subplot(311)
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(312)
plt.plot(dist, error, 'r')
plt.xlabel("dist from origin [m]")
plt.ylabel("error [m]")
plt.subplot(313)
plt.plot(dist, error_c, 'r')
plt.xlabel("dist from origin [m]")
plt.ylabel("error with north fix [m]")
plt.show()
sw/ground_segment/cockpit/editFP.ml
+1
View File
@@ -63,6 +63,7 @@ let close_fp = fun geomap ->
| Some (fp, _filename) ->
let close = fun () ->
fp#destroy ();
geomap#clear_georefs ();
current_fp := None in
match GToolbox.question_box ~title:"Closing flight plan" ~buttons:["Close"; "Save&Close"; "Cancel"] "Do you want to save/close ?" with
2 -> save_fp geomap; close ()
sw/ground_segment/cockpit/gcs.ml
+1 -1
View File
@@ -290,7 +290,7 @@ let button_press = fun (geomap:G.widget) ev ->
GToolbox.popup_menu ~entries:([`I ("Load background tile", display_gm)]@m)
~button:3 ~time:(Int32.of_int 0);
true
end else if GdkEvent.Button.button ev = 1 && Gdk.Convert.test_modifier `CONTROL state then
end else if GdkEvent.Button.button ev = 1 && Gdk.Convert.test_modifier `CONTROL state then (* create new wp on Ctrl-click *)
let xc = GdkEvent.Button.x ev in
let yc = GdkEvent.Button.y ev in
let xyw = geomap#canvas#window_to_world xc yc in
sw/ground_segment/misc/Makefile
+5 -1
View File
@@ -39,9 +39,13 @@ CFLAGS += -Wall -fPIC
GTK_CFLAGS = $(shell pkg-config gtk+-2.0 --cflags)
GTK_LDFLAGS = $(shell pkg-config gtk+-2.0 --libs) $(shell pkg-config --libs ivy-glib) $(shell pcre-config --libs) -fPIC
# hack, abuse ivy-glib pkg-config info since there is no ivy.pc
# fallback to ivy-glib pkg-config info if there is no ivy-c.pc
IVY_INC = $(shell pkg-config --cflags-only-I ivy-c 2> /dev/null)
IVY_LDFLAGS = $(shell pkg-config --libs ivy-c 2> /dev/null)
ifeq ($(strip $(IVY_INC)),)
IVY_INC = $(shell pkg-config --cflags-only-I ivy-glib)
IVY_LDFLAGS = $(shell pkg-config --libs-only-L ivy-glib) -livy
endif
# Optitrack specific librarys and includes
NATNET_LIBRARYS = $(shell pkg-config glib-2.0 --libs) -lglibivy -lm $(shell pcre-config --libs)
sw/ground_segment/tmtc/fw_server.ml
+1 -1
View File
@@ -318,7 +318,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
and lon = ivalue "lon"
and alt = ivalue "alt" in
let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
update_waypoint a (ivalue "wp_id") geo (float alt /. 100.)
update_waypoint a (ivalue "wp_id") geo (float alt /. 1000.)
| "GENERIC_COM" ->
let flight_time = ivalue "flight_time" in
if flight_time >= a.flight_time then begin
sw/ground_segment/tmtc/server.ml
+6 -2
View File
@@ -683,8 +683,12 @@ let ivy_server = fun http ->
ignore (Ground_Pprz.message_answerer my_id "AIRCRAFTS" send_aircrafts_msg);
ignore (Ground_Pprz.message_answerer my_id "CONFIG" (send_config http))
(** Convert to cm, with rounding *)
let cm_of_m = fun f -> Pprz.Int (truncate ((100. *. f) +. 0.5))
(** Convert to mm, with rounding *)
let mm_of_m = fun f -> Pprz.Int (truncate ((1000. *. f) +. 0.5))
let cm_of_m = fun f -> Pprz.Int (truncate ((100. *. f) +. 0.5)) (* Convert to cm, with rounding *)
let dl_id = "ground_dl" (* Hack, should be [my_id] *)
(** Got a ground.MOVE_WAYPOINT and send a datalink.MOVE_WP *)
@@ -696,7 +700,7 @@ let move_wp = fun logging _sender vs ->
"ac_id", Pprz.String ac_id;
"lat", deg7 "lat";
"lon", deg7 "long";
"alt", cm_of_m (Pprz.float_assoc "alt" vs) ] in
"alt", mm_of_m (Pprz.float_assoc "alt" vs) ] in
Dl_Pprz.message_send dl_id "MOVE_WP" vs;
log logging ac_id "MOVE_WP" vs
sw/include/pprz_version.h
+8
View File
@@ -41,6 +41,10 @@
#ifndef PPRZ_VERSION_H
#define PPRZ_VERSION_H
#ifdef __cplusplus
extern "C" {
#endif
#define _STRINGIFY(s) #s
#define STRINGIFY(s) _STRINGIFY(s)
@@ -75,4 +79,8 @@ static inline void get_pprz_git_version(uint8_t sha1[8])
}
}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif /* PPRZ_VERSION_H */
sw/lib/ocaml/env.ml
+4 -4
View File
@@ -122,8 +122,8 @@ let expand_ac_xml = fun ?(raise_exception = true) ac_conf ->
(fun filename -> parse_file ~parse_filter a (prefix filename))
(Str.split space_regexp (pre_filter (ExtXml.attrib ac_conf a))) in
let parse_opt = fun a ->
try parse a with ExtXml.Error _ -> [] in
let parse_opt = fun ?(pre_filter=(fun x -> x)) ?(parse_filter=(fun x -> ExtXml.parse_file x)) a ->
try parse ~pre_filter ~parse_filter a with ExtXml.Error _ -> [] in
(* dump expanded version of flight plan before parsing *)
let parse_fp = fun a ->
@@ -148,8 +148,8 @@ let expand_ac_xml = fun ?(raise_exception = true) ac_conf ->
with _ -> []
in
let pervasives = parse "airframe" @ parse "telemetry" @ parse ~pre_filter:filter_settings "settings" @ parse ~pre_filter:filter_settings ~parse_filter:filter_modules_target "settings_modules" in
let optionals = parse_opt "radio" @ parse_fp "flight_plan" @ pervasives in
let pervasives = parse "airframe" @ parse "telemetry" @ parse ~pre_filter:filter_settings "settings" in
let optionals = parse_opt "radio" @ parse_fp "flight_plan" @ parse_opt ~pre_filter:filter_settings ~parse_filter:filter_modules_target "settings_modules" @ pervasives in
let children = Xml.children ac_conf@optionals in
make_element (Xml.tag ac_conf) (Xml.attribs ac_conf) children
sw/lib/ocaml/latlong.ml
+6 -4
View File
@@ -541,10 +541,12 @@ type coordinates_kind =
let l = lambertIIe_of geo in
Printf.sprintf "%d %d" l.lbt_x l.lbt_y
| Bearing georef ->
let (dx, dy) = utm_sub (utm_of WGS84 geo) (utm_of WGS84 georef#pos) in
let d = sqrt (dx*.dx+.dy*.dy) in
let bearing = (int_of_float ((Rad>>Deg)(atan2 dx dy)) + 360) mod 360 in
Printf.sprintf "%4d %4.0f" bearing d
try
let (dx, dy) = utm_sub (utm_of WGS84 geo) (utm_of WGS84 georef#pos) in
let d = sqrt (dx*.dx+.dy*.dy) in
let bearing = (int_of_float ((Rad>>Deg)(atan2 dx dy)) + 360) mod 360 in
Printf.sprintf "%4d %4.0f" bearing d
with _ -> "Dist across diff utm zones unsupported"
let geographic_of_coordinates = fun kind s ->
match kind with
sw/lib/ocaml/mapCanvas.ml
+93 -17
View File
@@ -31,6 +31,7 @@ let (//) = Filename.concat
type world = float * float
let zoom_factor = 1.5 (* Mouse wheel zoom action *)
let max_zoom = 40.
let pan_step = 50 (* Pan keys speed *)
let pan_arrow_size = 40.
@@ -72,6 +73,10 @@ let my_menu_item = fun label ~callback ~packing () ->
let mi = GMenu.menu_item ~label ~packing () in
ignore (mi#connect#activate ~callback)
let my_menu_item_insert = fun label ~menu ~pos ~callback ->
let mi = GMenu.menu_item ~label () in
menu#insert mi ~pos ;
ignore (mi#connect#activate ~callback)
let set_opacity = fun pixbuf opacity ->
let pixbuf = GdkPixbuf.add_alpha pixbuf in
@@ -202,7 +207,7 @@ object (self)
wind_sock#label#set [`TEXT string]
val adj = GData.adjustment
~value:1. ~lower:0.005 ~upper:40.
~value:1. ~lower:0.005 ~upper:max_zoom
~step_incr:0.25 ~page_incr:1.0 ~page_size:0. ()
method info = info
@@ -216,6 +221,7 @@ object (self)
val mutable region = None (* Rectangle selected region *)
val mutable last_mouse_x = 0
val mutable last_mouse_y = 0
val mutable zoom_level = 1.;
val mutable fitted_objects = ([] : geographic list)
@@ -236,7 +242,7 @@ object (self)
fitted_objects
(max_float, -.max_float, max_float, -. max_float) in
(* Over 0° ? *)
(* Over 0° ? *)
let min_long, max_long =
if max_long -. min_long > pi
then (max_long -. 2. *. pi, min_long)
@@ -258,7 +264,7 @@ object (self)
initializer (
spin_button#set_adjustment adj;
spin_button#set_value 1.; (* this should be done by set_adjustment but seems to fail on ubuntu 13.10 (at least) *)
spin_button#set_value zoom_level; (* this should be done by set_adjustment but seems to fail on ubuntu 13.10 (at least) *)
utc_time#hide ();
@@ -273,7 +279,7 @@ object (self)
ignore (canvas#event#connect#after#key_press self#key_press) ;
ignore (canvas#event#connect#enter_notify (fun _ -> self#canvas#misc#grab_focus () ; false));
ignore (canvas#event#connect#any self#any_event);
ignore (adj#connect#value_changed (fun () -> canvas#set_pixels_per_unit adj#value));
ignore (adj#connect#value_changed (fun () -> if abs_float (adj#value -. zoom_level) >= 0.01 then self#zoom_in_center adj#value));
canvas#set_center_scroll_region false ;
canvas#set_scroll_region (-25000000.) (-25000000.) 25000000. 25000000.;
@@ -285,7 +291,7 @@ object (self)
(** methods *)
(** accessors to instance variables *)
method current_zoom = adj#value
method current_zoom = zoom_level
method canvas = canvas
method frame = frame
method factory = factory
@@ -405,13 +411,15 @@ object (self)
pix#affine_relative [| cos_a; sin_a; -. sin_a; cos_a; 0.;0.|];
pix
method fix_bg_coords (xw, yw) = (** FIXME: how to do it properly ? *)
let z = self#current_zoom in
((xw +. 25000000.) *. z, (yw +. 25000000.) *. z)
method fix_bg_coords (xw, yw) = (** FIXME: how to do it properly ? *)
((xw +. 25000000.) *. zoom_level, (yw +. 25000000.) *. zoom_level)
method zoom = fun value ->
adj#set_value value
let value = min max_zoom value in
zoom_level <- value; (* must set this before changing adj so that another zoom is not triggered *)
adj#set_value value;
canvas#set_pixels_per_unit value
(** events *******************************************)
method background_event = fun ev ->
match ev with
@@ -451,8 +459,8 @@ object (self)
| Panning (x0, y0) ->
let xc = GdkEvent.Motion.x ev
and yc = GdkEvent.Motion.y ev in
let dx = self#current_zoom *. (xc -. x0)
and dy = self#current_zoom *. (yc -. y0) in
let dx = zoom_level *. (xc -. x0)
and dy = zoom_level *. (yc -. y0) in
let (x, y) = canvas#get_scroll_offsets in
canvas#scroll_to (x-truncate dx) (y-truncate dy)
| _ -> ()
@@ -514,19 +522,28 @@ object (self)
method connect_view = fun cb ->
Hashtbl.add view_cbs cb ()
(* zoom keeping the center *)
method zoom_in_center = fun z ->
let c = self#get_center () in
self#zoom z;
self#center c
(* zoom keeping the area under the mouse pointer *)
method zoom_in_place = fun z ->
let (x, y) = canvas#get_scroll_offsets in
canvas#scroll_to (x+last_mouse_x) (y+last_mouse_y);
adj#set_value z;
self#zoom z;
let (x, y) = canvas#get_scroll_offsets in
canvas#scroll_to (x-last_mouse_x) (y-last_mouse_y)
method zoom_up () =
self#zoom_in_place (adj#value*.zoom_factor);
self#zoom_in_place (zoom_level*.zoom_factor);
method zoom_down () =
self#zoom_in_place (adj#value/.zoom_factor);
self#zoom_in_place (zoom_level/.zoom_factor);
method any_event =
let rec last_view = ref (0,0,0,0) in
@@ -611,7 +628,7 @@ object (self)
let replace_still = fun _ ->
let (x, y) = canvas#get_scroll_offsets in
let (xc, yc) = canvas#window_to_world (float x) (float y) in
let z = 1./.self#current_zoom in
let z = 1./.zoom_level in
still#affine_absolute [|z;0.;0.;z;xc;yc|]
in
self#connect_view replace_still;
@@ -766,10 +783,69 @@ class widget = fun ?(height=800) ?(srtm=false) ?width ?projection ?georef () ->
method georefs = georefs
method add_info_georef = fun name geo ->
georefs <- (name, geo) :: georefs;
(* add to the end so georefs has same order as waypoint list *)
georefs <- List.append georefs [(name, geo)];
let callback = fun () -> selected_georef <- Bearing geo in
my_menu_item name ~packing:georef_menu#append ~callback ();
(* change wp name *)
method edit_georef_name = fun oldname newname ->
(* get offset between WP list and menu list *)
let extraitems = (List.length georef_menu#children) - (List.length georefs) in
if newname <> oldname then
georefs <- List.fold_left (fun l (label, geo) ->
if label = oldname then
begin
let callback = fun () -> selected_georef <- Bearing geo in
let pos = List.length l + extraitems in
(* remove item and readd with new name *)
georef_menu#remove (List.nth georef_menu#children pos);
(*my_menu_item_insert newname ~menu:georef_menu ~pos:menupos ~callback;*)
my_menu_item newname ~packing:(georef_menu#insert ~pos) ~callback ();
if selected_georef = (Bearing geo) then optmenu#set_history pos;
List.append l [(newname, geo)]
end
else
List.append l [(label, geo)]
) [] georefs
(* Delete item from georefs and from menu *)
method delete_georef = fun name ->
(* get offset between WP list and menu list *)
let extraitems = (List.length georef_menu#children) - (List.length georefs) in
georefs <- List.fold_left (fun l (label, geo) ->
if label = name then
begin
let menupos = List.length l + extraitems in
(* remove item *)
georef_menu#remove (List.nth georef_menu#children menupos);
if selected_georef = (Bearing geo) then
begin
(List.nth georef_menu#children 0)#activate ();
optmenu#set_history 0;
end;
l
end
else
List.append l [(label, geo)]
) [] georefs
(* delete all wp, including fitted objects *)
method clear_georefs = fun () ->
(* get offset between WP list and menu list *)
let extraitems = (List.length georef_menu#children) - (List.length georefs) in
(* delete items from georefs and from menu *)
ignore (List.fold_left (fun i v ->
if i >= extraitems then georef_menu#remove v;
i + 1
) 0 georef_menu#children);
(List.nth georef_menu#children 0)#activate ();
optmenu#set_history 0;
georefs <- [];
(* finally delete all fitted objects *)
fitted_objects <- [];
(** display methods *)
method display_xy = fun s -> lbl_xy#set_text s
method display_geo = fun geo ->
sw/lib/ocaml/mapCanvas.mli
+4
View File
@@ -34,6 +34,9 @@ class widget :
unit ->
object
method add_info_georef : string -> < pos : Latlong.geographic > -> unit
method edit_georef_name : string -> string -> unit
method delete_georef : string -> unit
method clear_georefs : unit -> unit
method altitude : Latlong.geographic -> int
method any_event : GdkEvent.any -> bool
method arc :
@@ -119,5 +122,6 @@ class widget :
method zoom_adj : GData.adjustment
method zoom_down : unit -> unit
method zoom_in_place : float -> unit
method zoom_in_center : float -> unit
method zoom_up : unit -> unit
end
sw/lib/ocaml/mapFP.ml
+2 -1
View File
@@ -62,7 +62,8 @@ XmlEdit.Deleted -> wp#delete ()
let wgs84 = geo_of_xml utm_ref float_attrib in
wp#set wgs84;
wp#geomap#edit_georef_name wp#name (assoc_nocase "name" attribs);
wp#set wgs84;
wp#set_name (assoc_nocase "name" attribs)
with
_ -> ()
sw/lib/ocaml/mapWaypoints.ml
+5
View File
@@ -92,6 +92,7 @@ object (self)
name <- n;
label#set_name name
end
method geomap = geomap
method alt = alt
method label = label
method xy = let a = wpt_group#i2w_affine in (a.(4), a.(5))
@@ -154,7 +155,9 @@ object (self)
ignore(minus10#connect#pressed (fun _ -> change_alt (-10.)));
ignore(plus10#connect#pressed (fun _ -> change_alt (10.)));
(* called when ok button is clicked in WP Edit dialog *)
let callback = fun _ ->
geomap#edit_georef_name name ename#text;
self#set_name ename#text;
alt <- ea#value;
label#set_name name;
@@ -185,6 +188,7 @@ object (self)
let delete_callback = fun () ->
dialog#destroy ();
self#delete ();
geomap#delete_georef name;
updated ()
in
ignore(delete#connect#clicked ~callback:delete_callback)
@@ -286,6 +290,7 @@ object (self)
method set_ground_alt ga = ground_alt <- ga
method delete () =
deleted <- true; (* BOF *)
geomap#delete_georef name;
wpt_group#destroy ()
method zoom (z:float) =
if List.length wpt_group#get_items > 0 then
sw/lib/ocaml/mapWaypoints.mli
+1
View File
@@ -43,6 +43,7 @@ class waypoint :
Latlong.geographic ->
object
method alt : float
method geomap : MapCanvas.widget
method delete : unit -> unit
method edit : unit
method pos : Latlong.geographic
sw/lib/ocaml/xmlEdit.ml
+1 -1
View File
@@ -181,7 +181,7 @@ let set_bg_color = fun renderer (model:GTree.model) iter ->
renderer#set_properties [`CELL_BACKGROUND bg]
let tree_view = fun format_attribs ?(edit=true) (model:GTree.tree_store) window ->
let view = GTree.view ~model ~reorderable:edit ~packing:window#add () in
let view = GTree.view ~model ~enable_search:edit ~reorderable:edit ~packing:window#add () in
let r = GTree.cell_renderer_text [] in
let col = GTree.view_column ~title:"Tag" () ~renderer:(r, ["text",tag_col]) in
col#set_cell_data_func r (set_bg_color r);
sw/lib/python/pprz_math/.gitignore
+4
View File
@@ -0,0 +1,4 @@
build
*.c
geodetic.py
algebra.py
sw/lib/python/pprz_math/Makefile
+8
View File
@@ -0,0 +1,8 @@
all:
swig2.0 -python -keyword -I../../../airborne geodetic.i
swig2.0 -python -keyword -I../../../airborne algebra.i
python setup.py build_ext --inplace
clean:
rm -rf build *.so *.c *.pyc geodetic.py algebra.py
sw/lib/python/pprz_math/__init__.py
+1
View File
@@ -0,0 +1 @@
__all__ = ["geodetic", "algebra"]
sw/lib/python/pprz_math/algebra.i
+10
View File
@@ -0,0 +1,10 @@
/* File : algebra.i */
%module algebra
%feature("autodoc", "3");
%include "typemaps.i"
%include pprz_algebra_int.i
%include pprz_algebra_float.i
%include pprz_algebra_double.i
%{
#define SWIG_FILE_WITH_INIT
%}
sw/lib/python/pprz_math/geodetic.i
+10
View File
@@ -0,0 +1,10 @@
/* File : geodetic.i */
%module 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/pprz_math/pprz_algebra_double.i
+249
View File
@@ -0,0 +1,249 @@
/* File : pprz_algebra_double.i */
%module algebra_double
%{
#include "math/pprz_algebra_double.h"
%}
/* don't wrap everything in header
%include "math/pprz_algebra_double.h"
* instead only wrap the structs and extend them with nicer to use methods
*/
struct DoubleVect2 {
double x;
double y;
};
struct DoubleVect3 {
double x;
double y;
double z;
};
struct DoubleQuat {
double qi;
double qx;
double qy;
double qz;
};
struct DoubleRMat {
double m[3 * 3];
};
struct DoubleEulers {
double phi; ///< in radians
double theta; ///< in radians
double psi; ///< in radians
};
%extend DoubleVect2 {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Vect2(%g, %g)", $self->x ,$self->y);
return tmp;
}
DoubleVect2(double x=0.0, double y=0.0) {
struct DoubleVect2 *v = (struct DoubleVect2 *) malloc(sizeof(struct DoubleVect2));
v->x = x;
v->y = y;
return v;
}
struct DoubleVect2 __add__(struct DoubleVect2 *other) {
struct DoubleVect2 v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
return v;
}
struct DoubleVect2 __sub__(struct DoubleVect2 *other) {
struct DoubleVect2 v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
return v;
}
double norm() {
return sqrt($self->x*$self->x + $self->y*$self->y);
}
void normalize() {
const double n = sqrt($self->x*$self->x + $self->y*$self->y);
if (n > 0) {
$self->x /= n;
$self->y /= n;
}
}
};
%extend DoubleVect3 {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Vect3(%g, %g, %g)", $self->x ,$self->y, $self->z);
return tmp;
}
DoubleVect3(double x=0.0, double y=0.0, double z=0.0) {
struct DoubleVect3 *v = (struct DoubleVect3 *) malloc(sizeof(struct DoubleVect3));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct DoubleVect3 __add__(struct DoubleVect3 *other) {
struct DoubleVect3 v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct DoubleVect3 __sub__(struct DoubleVect3 *other) {
struct DoubleVect3 v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct DoubleVect3 __rmul__(struct DoubleQuat *q) {
struct DoubleVect3 v;
double_quat_vmult(&v, q, $self);
return v;
}
double norm() {
return double_vect3_norm($self);
}
void normalize() {
double_vect3_normalize($self);
}
};
%extend DoubleQuat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Quat(%g, %g, %g, %g)", $self->qi, $self->qx ,$self->qy, $self->qz);
return tmp;
}
DoubleQuat(double qi=1.0, double qx=0.0, double qy=0.0, double qz=0.0) {
struct DoubleQuat *v = (struct DoubleQuat *) malloc(sizeof(struct DoubleQuat));
v->qi = qi;
v->qx = qx;
v->qy = qy;
v->qz = qz;
return v;
}
struct DoubleVect3 __mul__(struct DoubleVect3 *v) {
struct DoubleVect3 vout;
double_quat_vmult(&vout, $self, v);
return vout;
}
double norm() {
return double_quat_norm($self);
}
void normalize() {
double_quat_normalize($self);
}
void set_identity() {
double_quat_identity($self);
}
struct DoubleEulers to_eulers() {
struct DoubleEulers e;
double_eulers_of_quat(&e, $self);
return e;
}
};
%extend DoubleQuat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"RMat[% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]",
$self->m[0], $self->m[1], $self->m[2], $self->m[3], $self->m[4],
$self->m[5], $self->m[6], $self->m[7], $self->m[8]);
return tmp;
}
struct DoubleVect3 __mul__(struct DoubleVect3 *v) {
struct DoubleVect3 vout;
double_quat_vmult(&vout, $self, v);
return vout;
}
double norm() {
return double_quat_norm($self);
}
void normalize() {
double_quat_normalize($self);
}
void set_identity() {
double_quat_identity($self);
}
struct DoubleEulers to_eulers() {
struct DoubleEulers e;
double_eulers_of_quat(&e, $self);
return e;
}
};
%extend DoubleRMat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"RMat[% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]",
$self->m[0], $self->m[1], $self->m[2], $self->m[3], $self->m[4],
$self->m[5], $self->m[6], $self->m[7], $self->m[8]);
return tmp;
}
DoubleRMat() {
struct DoubleRMat *rm = (struct DoubleRMat *) malloc(sizeof(struct DoubleRMat));
double_rmat_identity(rm);
return rm;
}
struct DoubleRMat __mul__(struct DoubleRMat *m_b2c) {
struct DoubleRMat m_a2c;
double_rmat_comp(&m_a2c, $self, m_b2c);
return m_a2c;
}
struct DoubleRMat __mul__(struct DoubleQuat *q) {
struct DoubleRMat m_b2c;
double_rmat_of_quat(&m_b2c, q);
struct DoubleRMat m_a2c;
double_rmat_comp(&m_a2c, $self, &m_b2c);
return m_a2c;
}
struct DoubleVect3 __mul__(struct DoubleVect3 *va) {
struct DoubleVect3 v;
double_rmat_vmult(&v, $self, va);
return v;
}
void set_identity() {
double_rmat_identity($self);
}
struct DoubleRMat inverse() {
struct DoubleRMat inv;
double_rmat_inv(&inv, $self);
return inv;
}
struct DoubleRMat transposed() {
struct DoubleRMat inv;
double_rmat_inv(&inv, $self);
return inv;
}
};
%extend DoubleEulers {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Eulers in deg (phi=%g, theta=%g, psi=%g)", DegOfRad($self->phi),
DegOfRad($self->theta), DegOfRad($self->psi));
return tmp;
}
DoubleEulers(double phi=0.0, double theta=0.0, double psi=0.0) {
struct DoubleEulers *v = (struct DoubleEulers *) malloc(sizeof(struct DoubleEulers));
v->phi = phi;
v->theta = theta;
v->psi = psi;
return v;
}
struct DoubleQuat to_quat() {
struct DoubleQuat q;
double_quat_of_eulers(&q, $self);
return q;
}
struct DoubleRMat to_rmat() {
struct DoubleRMat r;
double_rmat_of_eulers(&r, $self);
return r;
}
};
sw/lib/python/pprz_math/pprz_algebra_float.i
+267
View File
@@ -0,0 +1,267 @@
/* File : pprz_algebra_float.i */
%module algebra_float
%{
#include "math/pprz_algebra_float.h"
%}
/* don't wrap everything in header
%include "math/pprz_algebra_float.h"
* instead only wrap the structs and extend them with nicer to use methods
*/
struct FloatVect2 {
float x;
float y;
};
struct FloatVect3 {
float x;
float y;
float z;
};
struct FloatQuat {
float qi;
float qx;
float qy;
float qz;
};
struct FloatRMat {
float m[3 * 3];
};
struct FloatEulers {
float phi; ///< in radians
float theta; ///< in radians
float psi; ///< in radians
};
%extend FloatVect2 {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Vect2(%g, %g)", $self->x ,$self->y);
return tmp;
}
FloatVect2(float x=0.0, float y=0.0) {
struct FloatVect2 *v = (struct FloatVect2 *) malloc(sizeof(struct FloatVect2));
v->x = x;
v->y = y;
return v;
}
struct FloatVect2 __add__(struct FloatVect2 *other) {
struct FloatVect2 v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
return v;
}
struct FloatVect2 __sub__(struct FloatVect2 *other) {
struct FloatVect2 v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
return v;
}
float norm2() {
return float_vect2_norm2($self);
}
float norm() {
return float_vect2_norm($self);
}
void normalize() {
float_vect2_normalize($self);
}
};
%extend FloatVect3 {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Vect3(%g, %g, %g)", $self->x ,$self->y, $self->z);
return tmp;
}
FloatVect3(float x=0.0, float y=0.0, float z=0.0) {
struct FloatVect3 *v = (struct FloatVect3 *) malloc(sizeof(struct FloatVect3));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct FloatVect3 __add__(struct FloatVect3 *other) {
struct FloatVect3 v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct FloatVect3 __sub__(struct FloatVect3 *other) {
struct FloatVect3 v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct FloatVect3 __rmul__(struct FloatQuat *q) {
struct FloatVect3 v;
float_quat_vmult(&v, q, $self);
return v;
}
float norm2() {
return float_vect3_norm2($self);
}
float norm() {
return float_vect3_norm($self);
}
void normalize() {
float_vect3_normalize($self);
}
};
%extend FloatQuat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Quat(%g, %g, %g, %g)", $self->qi, $self->qx ,$self->qy, $self->qz);
return tmp;
}
FloatQuat(float qi=1.0, float qx=0.0, float qy=0.0, float qz=0.0) {
struct FloatQuat *v = (struct FloatQuat *) malloc(sizeof(struct FloatQuat));
v->qi = qi;
v->qx = qx;
v->qy = qy;
v->qz = qz;
return v;
}
struct FloatQuat __mul__(struct FloatQuat *q) {
struct FloatQuat qout;
float_quat_comp(&qout, $self, q);
return qout;
}
struct FloatQuat __mul__(struct FloatRMat *rm) {
struct FloatQuat q;
float_quat_of_rmat(&q, rm);
struct FloatQuat qout;
float_quat_comp(&qout, $self, &q);
return qout;
}
struct FloatVect3 __mul__(struct FloatVect3 *v) {
struct FloatVect3 vout;
float_quat_vmult(&vout, $self, v);
return vout;
}
float norm() {
return float_quat_norm($self);
}
void normalize() {
float_quat_normalize($self);
}
void set_identity() {
float_quat_identity($self);
}
void wrap_shortest() {
float_quat_wrap_shortest($self);
}
struct FloatEulers to_eulers() {
struct FloatEulers e;
float_eulers_of_quat(&e, $self);
return e;
}
struct FloatRMat to_rmat() {
struct FloatRMat rm;
float_rmat_of_quat(&rm, $self);
return rm;
}
};
%extend FloatRMat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"RMat[% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]",
$self->m[0], $self->m[1], $self->m[2], $self->m[3], $self->m[4],
$self->m[5], $self->m[6], $self->m[7], $self->m[8]);
return tmp;
}
FloatRMat() {
struct FloatRMat *rm = (struct FloatRMat *) malloc(sizeof(struct FloatRMat));
float_rmat_identity(rm);
return rm;
}
struct FloatRMat __mul__(struct FloatRMat *m_b2c) {
struct FloatRMat m_a2c;
float_rmat_comp(&m_a2c, $self, m_b2c);
return m_a2c;
}
struct FloatRMat __mul__(struct FloatQuat *q) {
struct FloatRMat m_b2c;
float_rmat_of_quat(&m_b2c, q);
struct FloatRMat m_a2c;
float_rmat_comp(&m_a2c, $self, &m_b2c);
return m_a2c;
}
struct FloatVect3 __mul__(struct FloatVect3 *va) {
struct FloatVect3 v;
float_rmat_vmult(&v, $self, va);
return v;
}
void set_identity() {
float_rmat_identity($self);
}
struct FloatRMat inverse() {
struct FloatRMat inv;
float_rmat_inv(&inv, $self);
return inv;
}
struct FloatRMat transposed() {
struct FloatRMat inv;
float_rmat_inv(&inv, $self);
return inv;
}
/*
void invert() {
float_rmat_inv($self, $self);
}
void transpose() {
float_rmat_inv($self, $self);
}
*/
float norm() {
return float_rmat_norm($self);
}
struct FloatEulers to_eulers() {
struct FloatEulers e;
float_eulers_of_rmat(&e, $self);
return e;
}
struct FloatQuat to_quat() {
struct FloatQuat q;
float_quat_of_rmat(&q, $self);
return q;
}
double reorthogonalize() {
return float_rmat_reorthogonalize($self);
}
};
%extend FloatEulers {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Eulers in deg (phi=%g, theta=%g, psi=%g)", DegOfRad($self->phi),
DegOfRad($self->theta), DegOfRad($self->psi));
return tmp;
}
FloatEulers(float phi=0.0, float theta=0.0, float psi=0.0) {
struct FloatEulers *v = (struct FloatEulers *) malloc(sizeof(struct FloatEulers));
v->phi = phi;
v->theta = theta;
v->psi = psi;
return v;
}
struct FloatQuat to_quat() {
struct FloatQuat q;
float_quat_of_eulers(&q, $self);
return q;
}
struct FloatRMat to_rmat() {
struct FloatRMat r;
float_rmat_of_eulers(&r, $self);
return r;
}
};
sw/lib/python/pprz_math/pprz_algebra_int.i
+277
View File
@@ -0,0 +1,277 @@
/* File : pprz_algebra_int.i */
%module algebra_int
%include "stdint.i"
%{
#include "math/pprz_algebra_int.h"
%}
/* don't wrap everything in header
%include "math/pprz_algebra_int.h"
* instead only wrap the structs and extend them with nicer to use methods
*/
struct Int32Vect2 {
int32_t x;
int32_t y;
};
struct Int32Vect3 {
int32_t x;
int32_t y;
int32_t z;
};
struct Int32Quat {
int32_t qi;
int32_t qx;
int32_t qy;
int32_t qz;
};
struct Int32RMat {
int32_t m[3 * 3];
};
struct Int32Eulers {
int32_t phi; ///< in rad with #INT32_ANGLE_FRAC
int32_t theta; ///< in rad with #INT32_ANGLE_FRAC
int32_t psi; ///< in rad with #INT32_ANGLE_FRAC
};
extern uint32_t int32_sqrt(uint32_t in);
%extend Int32Vect2 {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Vect2(%d, %d)", $self->x ,$self->y);
return tmp;
}
Int32Vect2(int32_t x=0, int32_t y=0) {
struct Int32Vect2 *v = (struct Int32Vect2 *) malloc(sizeof(struct Int32Vect2));
v->x = x;
v->y = y;
return v;
}
struct Int32Vect2 __add__(struct Int32Vect2 *other) {
struct Int32Vect2 v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
return v;
}
struct Int32Vect2 __sub__(struct Int32Vect2 *other) {
struct Int32Vect2 v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
return v;
}
int32_t norm2() {
return int32_vect2_norm2($self);
}
int32_t norm() {
return int32_vect2_norm($self);
}
void normalize(uint8_t frac) {
int32_vect2_normalize($self, frac);
}
};
%extend Int32Vect3 {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Vect3(%d, %d, %d)", $self->x ,$self->y, $self->z);
return tmp;
}
Int32Vect3(int32_t x=0, int32_t y=0, int32_t z=0) {
struct Int32Vect3 *v = (struct Int32Vect3 *) malloc(sizeof(struct Int32Vect3));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct Int32Vect3 __add__(struct Int32Vect3 *other) {
struct Int32Vect3 v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct Int32Vect3 __sub__(struct Int32Vect3 *other) {
struct Int32Vect3 v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct Int32Vect3 __rmul__(struct Int32Quat *q) {
struct Int32Vect3 v;
int32_quat_vmult(&v, q, $self);
return v;
}
int32_t norm2() {
return VECT3_NORM2(*$self);
}
int32_t norm() {
return int32_sqrt(VECT3_NORM2(*$self));
}
void normalize(uint8_t frac) {
const int32_t n = int32_sqrt(VECT3_NORM2(*$self) >> frac);
if (n > 0) {
const int32_t f = BFP_OF_REAL((1.), frac);
$self->x = $self->x * f / (int32_t)n;
$self->y = $self->y * f / (int32_t)n;
$self->z = $self->z * f / (int32_t)n;
}
}
struct Int32Vect3 normalized(uint8_t frac) {
struct Int32Vect3 v;
const int32_t n = int32_sqrt(VECT3_NORM2(*$self) >> frac);
if (n > 0) {
const int32_t f = BFP_OF_REAL((1.), frac);
v.x = $self->x * f / (int32_t)n;
v.y = $self->y * f / (int32_t)n;
v.z = $self->z * f / (int32_t)n;
}
return v;
}
};
%extend Int32Quat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Quat(%d, %d, %d, %d)", $self->qi, $self->qx ,$self->qy, $self->qz);
return tmp;
}
Int32Quat(int32_t qi=1.0, int32_t qx=0, int32_t qy=0, int32_t qz=0) {
struct Int32Quat *v = (struct Int32Quat *) malloc(sizeof(struct Int32Quat));
v->qi = qi;
v->qx = qx;
v->qy = qy;
v->qz = qz;
return v;
}
struct Int32Quat __mul__(struct Int32Quat *q) {
struct Int32Quat qout;
int32_quat_comp(&qout, $self, q);
return qout;
}
struct Int32Quat __mul__(struct Int32RMat *rm) {
struct Int32Quat q;
int32_quat_of_rmat(&q, rm);
struct Int32Quat qout;
int32_quat_comp(&qout, $self, &q);
return qout;
}
int32_t norm() {
return int32_quat_norm($self);
}
void normalize() {
int32_quat_normalize($self);
}
void set_identity() {
int32_quat_identity($self);
}
void wrap_shortest() {
int32_quat_wrap_shortest($self);
}
struct Int32Eulers to_eulers() {
struct Int32Eulers e;
int32_eulers_of_quat(&e, $self);
return e;
}
struct Int32RMat to_rmat() {
struct Int32RMat rm;
int32_rmat_of_quat(&rm, $self);
return rm;
}
};
%extend Int32RMat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"RMat[% d, % d, % d]\n [% d, % d, % d]\n [% d, % d, % d]",
$self->m[0], $self->m[1], $self->m[2], $self->m[3], $self->m[4],
$self->m[5], $self->m[6], $self->m[7], $self->m[8]);
return tmp;
}
Int32RMat() {
struct Int32RMat *rm = (struct Int32RMat *) malloc(sizeof(struct Int32RMat));
int32_rmat_identity(rm);
return rm;
}
struct Int32RMat __mul__(struct Int32RMat *m_b2c) {
struct Int32RMat m_a2c;
int32_rmat_comp(&m_a2c, $self, m_b2c);
return m_a2c;
}
struct Int32RMat __mul__(struct Int32Quat *q) {
struct Int32RMat m_b2c;
int32_rmat_of_quat(&m_b2c, q);
struct Int32RMat m_a2c;
int32_rmat_comp(&m_a2c, $self, &m_b2c);
return m_a2c;
}
struct Int32Vect3 __mul__(struct Int32Vect3 *va) {
struct Int32Vect3 v;
int32_rmat_vmult(&v, $self, va);
return v;
}
void set_identity() {
int32_rmat_identity($self);
}
/*
struct Int32RMat inverse() {
struct Int32RMat inv;
int32_rmat_inv(&inv, $self);
return inv;
}
struct Int32RMat transposed() {
struct Int32RMat inv;
int32_rmat_inv(&inv, $self);
return inv;
}
void invert() {
int32_rmat_inv($self, $self);
}
void transpose() {
int32_rmat_inv($self, $self);
}
*/
struct Int32Eulers to_eulers() {
struct Int32Eulers e;
int32_eulers_of_rmat(&e, $self);
return e;
}
struct Int32Quat to_quat() {
struct Int32Quat q;
int32_quat_of_rmat(&q, $self);
return q;
}
};
%extend Int32Eulers {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Eulers in deg (phi=%g, theta=%g, psi=%g)",
DegOfRad(DOUBLE_OF_BFP($self->phi, INT32_ANGLE_FRAC)),
DegOfRad(DOUBLE_OF_BFP($self->theta, INT32_ANGLE_FRAC)),
DegOfRad(DOUBLE_OF_BFP($self->psi, INT32_ANGLE_FRAC)));
return tmp;
}
Int32Eulers(int32_t phi=0, int32_t theta=0, int32_t psi=0) {
struct Int32Eulers *v = (struct Int32Eulers *) malloc(sizeof(struct Int32Eulers));
v->phi = phi;
v->theta = theta;
v->psi = psi;
return v;
}
struct Int32Quat to_quat() {
struct Int32Quat q;
int32_quat_of_eulers(&q, $self);
return q;
}
struct Int32RMat to_rmat() {
struct Int32RMat r;
int32_rmat_of_eulers(&r, $self);
return r;
}
};
sw/lib/python/pprz_math/pprz_geodetic_double.i
+248
View File
@@ -0,0 +1,248 @@
/* File : pprz_geodetic_double.i */
%module geodetic_double
%{
#include "math/pprz_geodetic_double.h"
%}
/* don't wrap everything in header
%include "math/pprz_geodetic_double.h"
* instead only wrap the structs and extend them with nicer to use methods
*/
struct EcefCoor_d {
double x; ///< in meters
double y; ///< in meters
double z; ///< in meters
};
struct LlaCoor_d {
double lat; ///< in radians
double lon; ///< in radians
double alt; ///< in meters above WGS84 reference ellipsoid
};
struct NedCoor_d {
double x; ///< in meters
double y; ///< in meters
double z; ///< in meters
};
struct EnuCoor_d {
double x; ///< in meters
double y; ///< in meters
double z; ///< in meters
};
struct UtmCoor_d {
double north; ///< in meters
double east; ///< in meters
double alt; ///< in meters above WGS84 reference ellipsoid
uint8_t zone; ///< UTM zone number
};
struct LtpDef_d {
struct EcefCoor_d ecef; ///< origin of local frame in ECEF
struct LlaCoor_d lla; ///< origin of local frame in LLA
struct DoubleRMat ltp_of_ecef; ///< rotation from ECEF to local frame
double hmsl; ///< height in meters above mean sea level
};
extern double gc_of_gd_lat_d(double gd_lat, double hmsl);
%extend EcefCoor_d {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"ECEF(%.3f, %.3f, %.3f)", $self->x,$self->y,$self->z);
return tmp;
}
EcefCoor_d(double x=0.0, double y=0.0, double z=0.0) {
struct EcefCoor_d *v = (struct EcefCoor_d *) malloc(sizeof(struct EcefCoor_d));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct EcefCoor_d __add__(struct EcefCoor_d *other) {
struct EcefCoor_d v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct EcefCoor_d __sub__(struct EcefCoor_d *other) {
struct EcefCoor_d v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct LlaCoor_d to_lla() {
struct LlaCoor_d lla;
lla_of_ecef_d(&lla, $self);
return lla;
}
struct LtpDef_d to_ltp_def() {
struct LtpDef_d ltp;
ltp_def_from_ecef_d(&ltp, $self);
return ltp;
}
struct NedCoor_d to_ned(struct LtpDef_d *ltp) {
struct NedCoor_d ned;
ned_of_ecef_point_d(&ned, ltp, $self);
return ned;
}
struct EnuCoor_d to_enu(struct LtpDef_d *ltp) {
struct EnuCoor_d enu;
enu_of_ecef_point_d(&enu, ltp, $self);
return enu;
}
struct NedCoor_d to_ned_vect(struct LtpDef_d *ltp) {
struct NedCoor_d ned;
ned_of_ecef_vect_d(&ned, ltp, $self);
return ned;
}
struct EnuCoor_d to_enu_vect(struct LtpDef_d *ltp) {
struct EnuCoor_d enu;
enu_of_ecef_vect_d(&enu, ltp, $self);
return enu;
}
};
%extend NedCoor_d {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"NED(% 3.3f,% 3.3f,% 3.3f)", $self->x,$self->y,$self->z);
return tmp;
}
NedCoor_d(double x=0.0, double y=0.0, double z=0.0) {
struct NedCoor_d *v = (struct NedCoor_d *) malloc(sizeof(struct NedCoor_d));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct NedCoor_d __add__(struct NedCoor_d *other) {
struct NedCoor_d v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct NedCoor_d __sub__(struct NedCoor_d *other) {
struct NedCoor_d v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct EcefCoor_d to_ecef(struct LtpDef_d *ltp) {
struct EcefCoor_d ecef;
ecef_of_ned_point_d(&ecef, ltp, $self);
return ecef;
}
struct EcefCoor_d to_ecef_vect(struct LtpDef_d *ltp) {
struct EcefCoor_d ecef;
ecef_of_ned_vect_d(&ecef, ltp, $self);
return ecef;
}
};
%extend EnuCoor_d {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"ENU(% 3.3f,% 3.3f,% 3.3f)", $self->x,$self->y,$self->z);
return tmp;
}
EnuCoor_d(double x=0.0, double y=0.0, double z=0.0) {
struct EnuCoor_d *v = (struct EnuCoor_d *) malloc(sizeof(struct EnuCoor_d));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct EnuCoor_d __add__(struct EnuCoor_d *other) {
struct EnuCoor_d v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct EnuCoor_d __sub__(struct EnuCoor_d *other) {
struct EnuCoor_d v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct EcefCoor_d to_ecef(struct LtpDef_d *ltp) {
struct EcefCoor_d ecef;
ecef_of_enu_point_d(&ecef, ltp, $self);
return ecef;
}
struct EcefCoor_d to_ecef_vect(struct LtpDef_d *ltp) {
struct EcefCoor_d ecef;
ecef_of_enu_vect_d(&ecef, ltp, $self);
return ecef;
}
};
%extend UtmCoor_d {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"UTM(north=%.3f, east=%.3f, alt=%g, zone=%d)", $self->north, $self->east, $self->alt, $self->zone);
return tmp;
}
UtmCoor_d(double north=0.0, double east=0.0, double alt=0.0, uint8_t zone=0) {
struct UtmCoor_d *v = (struct UtmCoor_d *) malloc(sizeof(struct UtmCoor_d));
v->north = north;
v->east = east;
v->alt = alt;
v->zone = zone;
return v;
}
struct LlaCoor_d to_lla() {
struct LlaCoor_d lla;
lla_of_utm_d(&lla, $self);
return lla;
}
};
%extend LlaCoor_d {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"LLA(lat=%.7f, lon=%.7f, alt=%g)", DegOfRad($self->lat), DegOfRad($self->lon), $self->alt);
return tmp;
}
LlaCoor_d(double lat=0.0, double lon=0.0, double alt=0.0) {
struct LlaCoor_d *v = (struct LlaCoor_d *) malloc(sizeof(struct LlaCoor_d));
v->lat = lat;
v->lon = lon;
v->alt = alt;
return v;
}
struct EcefCoor_d to_ecef() {
struct EcefCoor_d ecef;
ecef_of_lla_d(&ecef, $self);
return ecef;
}
struct NedCoor_d to_ned(struct LtpDef_d *ltp) {
struct NedCoor_d ned;
ned_of_lla_point_d(&ned, ltp, $self);
return ned;
}
struct EnuCoor_d to_enu(struct LtpDef_d *ltp) {
struct EnuCoor_d enu;
enu_of_lla_point_d(&enu, ltp, $self);
return enu;
}
};
%extend LtpDef_d {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"LtpDef: ECEF(%.3f, %.3f, %.3f), LLA(lat=%.7f, lon=%.7f, alt=%g), hmsl=%g",
$self->ecef.x, $self->ecef.y, $self->ecef.z,
DegOfRad($self->lla.lat), DegOfRad($self->lla.lon), $self->lla.alt, $self->hmsl);
return tmp;
}
};
sw/lib/python/pprz_math/pprz_geodetic_float.i
+256
View File
@@ -0,0 +1,256 @@
/* File : pprz_geodetic_float.i */
%module geodetic_float
%{
#include "math/pprz_geodetic_float.h"
%}
/* don't wrap everything in header
%include "math/pprz_geodetic_float.h"
* instead only wrap the structs and extend them with nicer to use methods
*/
struct EcefCoor_f {
float x; ///< in meters
float y; ///< in meters
float z; ///< in meters
};
struct LlaCoor_f {
float lat; ///< in radians
float lon; ///< in radians
float alt; ///< in meters above WGS84 reference ellipsoid
};
struct NedCoor_f {
float x; ///< in meters
float y; ///< in meters
float z; ///< in meters
};
struct EnuCoor_f {
float x; ///< in meters
float y; ///< in meters
float z; ///< in meters
};
struct UtmCoor_f {
float north; ///< in meters
float east; ///< in meters
float alt; ///< in meters above WGS84 reference ellipsoid
uint8_t zone; ///< UTM zone number
};
struct LtpDef_f {
struct EcefCoor_f ecef; ///< origin of local frame in ECEF
struct LlaCoor_f lla; ///< origin of local frame in LLA
struct FloatRMat ltp_of_ecef; ///< rotation from ECEF to local frame
float hmsl; ///< Height above mean sea level in meters
};
%extend EcefCoor_f {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"ECEF(%.3f, %.3f, %.3f)", $self->x,$self->y,$self->z);
return tmp;
}
EcefCoor_f(float x=0.0, float y=0.0, float z=0.0) {
struct EcefCoor_f *v = (struct EcefCoor_f *) malloc(sizeof(struct EcefCoor_f));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct EcefCoor_f __add__(struct EcefCoor_f *other) {
struct EcefCoor_f v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct EcefCoor_f __sub__(struct EcefCoor_f *other) {
struct EcefCoor_f v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct LlaCoor_f to_lla() {
struct LlaCoor_f lla;
lla_of_ecef_f(&lla, $self);
return lla;
}
struct LtpDef_f to_ltp_def() {
struct LtpDef_f ltp;
ltp_def_from_ecef_f(&ltp, $self);
return ltp;
}
struct NedCoor_f to_ned(struct LtpDef_f *ltp) {
struct NedCoor_f ned;
ned_of_ecef_point_f(&ned, ltp, $self);
return ned;
}
struct EnuCoor_f to_enu(struct LtpDef_f *ltp) {
struct EnuCoor_f enu;
enu_of_ecef_point_f(&enu, ltp, $self);
return enu;
}
struct NedCoor_f to_ned_vect(struct LtpDef_f *ltp) {
struct NedCoor_f ned;
ned_of_ecef_vect_f(&ned, ltp, $self);
return ned;
}
struct EnuCoor_f to_enu_vect(struct LtpDef_f *ltp) {
struct EnuCoor_f enu;
enu_of_ecef_vect_f(&enu, ltp, $self);
return enu;
}
};
%extend NedCoor_f {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"NED(% 3.3f,% 3.3f,% 3.3f)", $self->x,$self->y,$self->z);
return tmp;
}
NedCoor_f(float x=0.0, float y=0.0, float z=0.0) {
struct NedCoor_f *v = (struct NedCoor_f *) malloc(sizeof(struct NedCoor_f));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct NedCoor_f __add__(struct NedCoor_f *other) {
struct NedCoor_f v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct NedCoor_f __sub__(struct NedCoor_f *other) {
struct NedCoor_f v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct EcefCoor_f to_ecef(struct LtpDef_f *ltp) {
struct EcefCoor_f ecef;
ecef_of_ned_point_f(&ecef, ltp, $self);
return ecef;
}
struct EcefCoor_f to_ecef_vect(struct LtpDef_f *ltp) {
struct EcefCoor_f ecef;
ecef_of_ned_vect_f(&ecef, ltp, $self);
return ecef;
}
};
%extend EnuCoor_f {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"ENU(% 3.3f,% 3.3f,% 3.3f)", $self->x,$self->y,$self->z);
return tmp;
}
EnuCoor_f(float x=0.0, float y=0.0, float z=0.0) {
struct EnuCoor_f *v = (struct EnuCoor_f *) malloc(sizeof(struct EnuCoor_f));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct EnuCoor_f __add__(struct EnuCoor_f *other) {
struct EnuCoor_f v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct EnuCoor_f __sub__(struct EnuCoor_f *other) {
struct EnuCoor_f v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct EcefCoor_f to_ecef(struct LtpDef_f *ltp) {
struct EcefCoor_f ecef;
ecef_of_enu_point_f(&ecef, ltp, $self);
return ecef;
}
struct EcefCoor_f to_ecef_vect(struct LtpDef_f *ltp) {
struct EcefCoor_f ecef;
ecef_of_enu_vect_f(&ecef, ltp, $self);
return ecef;
}
};
%extend UtmCoor_f {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"UTM(north=%.3f, east=%.3f, alt=%g, zone=%d)", $self->north, $self->east, $self->alt, $self->zone);
return tmp;
}
UtmCoor_f(float north=0.0, float east=0.0, float alt=0.0, uint8_t zone=0) {
struct UtmCoor_f *v = (struct UtmCoor_f *) malloc(sizeof(struct UtmCoor_f));
v->north = north;
v->east = east;
v->alt = alt;
v->zone = zone;
return v;
}
struct LlaCoor_f to_lla() {
struct LlaCoor_f lla;
lla_of_utm_f(&lla, $self);
return lla;
}
};
%extend LlaCoor_f {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"LLA deg (lat=%.7f, lon=%.7f, alt=%g)", DegOfRad($self->lat), DegOfRad($self->lon), $self->alt);
return tmp;
}
LlaCoor_f(float lat=0.0, float lon=0.0, float alt=0.0) {
struct LlaCoor_f *v = (struct LlaCoor_f *) malloc(sizeof(struct LlaCoor_f));
v->lat = lat;
v->lon = lon;
v->alt = alt;
return v;
}
struct EcefCoor_f to_ecef() {
struct EcefCoor_f ecef;
ecef_of_lla_f(&ecef, $self);
return ecef;
}
struct NedCoor_f to_ned(struct LtpDef_f *ltp) {
struct NedCoor_f ned;
ned_of_lla_point_f(&ned, ltp, $self);
return ned;
}
struct EnuCoor_f to_enu(struct LtpDef_f *ltp) {
struct EnuCoor_f enu;
enu_of_lla_point_f(&enu, ltp, $self);
return enu;
}
struct UtmCoor_f to_utm() {
struct UtmCoor_f utm;
utm_of_lla_f(&utm, $self);
return utm;
}
struct LtpDef_f to_ltp_def() {
struct LtpDef_f ltp;
ltp_def_from_lla_f(&ltp, $self);
return ltp;
}
};
%extend LtpDef_f {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"LtpDef: ECEF(%.3f, %.3f, %.3f), LLA(lat=%.7f, lon=%.7f, alt=%g), hmsl=%g",
$self->ecef.x, $self->ecef.y, $self->ecef.z,
DegOfRad($self->lla.lat), DegOfRad($self->lla.lon), $self->lla.alt, $self->hmsl);
return tmp;
}
};
sw/lib/python/pprz_math/pprz_geodetic_int.i
+137
View File
@@ -0,0 +1,137 @@
/* File : pprz_geodetic_int.i */
%module geodetic_int
%include "stdint.i"
%{
#include "math/pprz_geodetic_int.h"
%}
%include "math/pprz_geodetic_int.h"
%extend EcefCoor_i {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"ECEF(%d, %d, %d)", $self->x,$self->y,$self->z);
return tmp;
}
EcefCoor_i(int32_t x=0, int32_t y=0, int32_t z=0) {
struct EcefCoor_i *v = (struct EcefCoor_i *) malloc(sizeof(struct EcefCoor_i));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct EcefCoor_i __add__(struct EcefCoor_i *other) {
struct EcefCoor_i v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct EcefCoor_i __sub__(struct EcefCoor_i *other) {
struct EcefCoor_i v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
};
%extend NedCoor_i {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"NED(%d, %d, %d)", $self->x,$self->y,$self->z);
return tmp;
}
NedCoor_i(int32_t x=0, int32_t y=0, int32_t z=0) {
struct NedCoor_i *v = (struct NedCoor_i *) malloc(sizeof(struct NedCoor_i));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct NedCoor_i __add__(struct NedCoor_i *other) {
struct NedCoor_i v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct NedCoor_i __sub__(struct NedCoor_i *other) {
struct NedCoor_i v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
};
%extend EnuCoor_i {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"ENU(%d, %d, %d)", $self->x,$self->y,$self->z);
return tmp;
}
EnuCoor_i(int32_t x=0, int32_t y=0, int32_t z=0) {
struct EnuCoor_i *v = (struct EnuCoor_i *) malloc(sizeof(struct EnuCoor_i));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct EnuCoor_i __add__(struct EnuCoor_i *other) {
struct EnuCoor_i v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct EnuCoor_i __sub__(struct EnuCoor_i *other) {
struct EnuCoor_i v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
};
%extend UtmCoor_i {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"UTM(north=%d, east=%d, alt=%d, zone=%d)", $self->north, $self->east, $self->alt, $self->zone);
return tmp;
}
UtmCoor_i(int32_t north=0, int32_t east=0, int32_t alt=0, uint8_t zone=0) {
struct UtmCoor_i *v = (struct UtmCoor_i *) malloc(sizeof(struct UtmCoor_i));
v->north = north;
v->east = east;
v->alt = alt;
v->zone = zone;
return v;
}
};
%extend LlaCoor_i {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"LLA(lat=%d, lon=%d, alt=%d)", $self->lat, $self->lon, $self->alt);
return tmp;
}
LlaCoor_i(int32_t lat=0, int32_t lon=0, int32_t alt=0) {
struct LlaCoor_i *v = (struct LlaCoor_i *) malloc(sizeof(struct LlaCoor_i));
v->lat = lat;
v->lon = lon;
v->alt = alt;
return v;
}
};
%extend LtpDef_i {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"LtpDef: ECEF(%d, %d, %d), LLA(lat=%d, lon=%d, alt=%d), hmsl=%d",
$self->ecef.x, $self->ecef.y, $self->ecef.z,
$self->lla.lat, $self->lla.lon, $self->lla.alt, $self->hmsl);
return tmp;
}
};
sw/lib/python/pprz_math/setup.py
+40
View File
@@ -0,0 +1,40 @@
#!/usr/bin/env python
"""
setup.py file for pprz math wrappers
"""
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]
geodetic_module = Extension('_geodetic',
sources=['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)
algebra_module = Extension('_algebra',
sources=['algebra_wrap.c',
path.join(pprz_airborne, 'math/pprz_algebra_int.c'),
path.join(pprz_airborne, 'math/pprz_algebra_double.c'),
path.join(pprz_airborne, 'math/pprz_algebra_float.c'),
path.join(pprz_airborne, 'math/pprz_trig_int.c'),
],
include_dirs=common_inc_dirs)
setup(name='geodetic',
version='0.1',
author="Felix Ruess",
description="""Pprz math wrappers""",
ext_modules=[geodetic_module, algebra_module],
py_modules=["geodetic", "algebra"],
)
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.
sw/simulator/nps/nps_ivy_fixedwing.c
+2 -1
View File
@@ -28,6 +28,7 @@ void nps_ivy_init(char* ivy_bus) {
#include "subsystems/datalink/downlink.h"
#define MOfCm(_x) (((float)(_x))/100.)
#define MOfMM(_x) (((float)(_x))/1000.)
void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
@@ -35,7 +36,7 @@ void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
if (atoi(argv[2]) == AC_ID) {
uint8_t wp_id = atoi(argv[1]);
float a = MOfCm(atoi(argv[5]));
float a = MOfMM(atoi(argv[5]));
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla;

Some files were not shown because too many files have changed in this diff Show More