diff --git a/CHANGELOG.md b/CHANGELOG.md
index 5f7680cae6..a2b5ed5c06 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -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
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/conf/messages.xml b/conf/messages.xml
index 834e395f3c..3613efc8d1 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -356,7 +356,7 @@
-
+ Height above Mean Sea Level (geoid)
@@ -2223,7 +2223,7 @@
-
+ Height above Mean Sea Level (geoid)
diff --git a/paparazzi_version b/paparazzi_version
index d7b59109d3..2276b70cfd 100755
--- a/paparazzi_version
+++ b/paparazzi_version
@@ -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).
diff --git a/sw/airborne/arch/linux/mcu_periph/i2c_arch.c b/sw/airborne/arch/linux/mcu_periph/i2c_arch.c
index a56cd0414d..21e171d742 100644
--- a/sw/airborne/arch/linux/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/linux/mcu_periph/i2c_arch.c
@@ -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;
diff --git a/sw/airborne/arch/sim/jsbsim_transport.c b/sw/airborne/arch/sim/jsbsim_transport.c
index 5f284b8589..07da04a13f 100644
--- a/sw/airborne/arch/sim/jsbsim_transport.c
+++ b/sw/airborne/arch/sim/jsbsim_transport.c
@@ -7,6 +7,7 @@
#include
#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;
diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c
index 456a1bab12..ec87839685 100644
--- a/sw/airborne/firmwares/fixedwing/datalink.c
+++ b/sw/airborne/firmwares/fixedwing/datalink.c
@@ -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;
diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c
index fdf0aeac9a..6fd1bc7b77 100644
--- a/sw/airborne/firmwares/rotorcraft/datalink.c
+++ b/sw/airborne/firmwares/rotorcraft/datalink.c
@@ -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);
}
diff --git a/sw/airborne/math/pprz_algebra_double.c b/sw/airborne/math/pprz_algebra_double.c
index 9e83a314f5..6ef5a05ddf 100644
--- a/sw/airborne/math/pprz_algebra_double.c
+++ b/sw/airborne/math/pprz_algebra_double.c
@@ -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;
+}
diff --git a/sw/airborne/math/pprz_algebra_double.h b/sw/airborne/math/pprz_algebra_double.h
index bb8f1c9b55..98861041a1 100644
--- a/sw/airborne/math/pprz_algebra_double.h
+++ b/sw/airborne/math/pprz_algebra_double.h
@@ -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);
diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h
index 2c62e31d74..f349958cbd 100644
--- a/sw/airborne/math/pprz_geodetic_double.h
+++ b/sw/airborne/math/pprz_geodetic_double.h
@@ -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
};
diff --git a/sw/airborne/math/pprz_geodetic_float.h b/sw/airborne/math/pprz_geodetic_float.h
index 1ebb61214a..14d7a72142 100644
--- a/sw/airborne/math/pprz_geodetic_float.h
+++ b/sw/airborne/math/pprz_geodetic_float.h
@@ -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
};
diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h
index 92f3c0d76f..03dfa301b0 100644
--- a/sw/airborne/math/pprz_geodetic_int.h
+++ b/sw/airborne/math/pprz_geodetic_int.h
@@ -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
};
diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c
index e1b6913dac..d12f0b3c4d 100644
--- a/sw/airborne/subsystems/ins.c
+++ b/sw/airborne/subsystems/ins.c
@@ -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) {}
diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c
index 9d5eb735ff..b081d7f7ca 100644
--- a/sw/airborne/subsystems/ins/ins_ardrone2.c
+++ b/sw/airborne/subsystems/ins/ins_ardrone2.c
@@ -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
}
diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c
index c0dd45d78f..521e8182a7 100644
--- a/sw/airborne/subsystems/ins/ins_float_invariant.c
+++ b/sw/airborne/subsystems/ins/ins_float_invariant.c
@@ -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(<p_def, &lla),
- ltp_def.hmsl = gps.hmsl;
+ ltp_def_from_lla_i(<p_def, &lla);
+ ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(<p_def);
#endif
}
diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c
index 6a42450570..7de8c6dcc2 100644
--- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c
+++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c
@@ -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);
}
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 650ea29abc..35e2637c2b 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -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;
diff --git a/sw/airborne/subsystems/navigation/common_flight_plan.h b/sw/airborne/subsystems/navigation/common_flight_plan.h
index a9fe5329e5..e472987362 100644
--- a/sw/airborne/subsystems/navigation/common_flight_plan.h
+++ b/sw/airborne/subsystems/navigation/common_flight_plan.h
@@ -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);
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..1d98a9cf3a
--- /dev/null
+++ b/sw/airborne/test/math/compare_utm_enu.py
@@ -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()
diff --git a/sw/ground_segment/cockpit/editFP.ml b/sw/ground_segment/cockpit/editFP.ml
index 641ccc8868..e88ead1713 100644
--- a/sw/ground_segment/cockpit/editFP.ml
+++ b/sw/ground_segment/cockpit/editFP.ml
@@ -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 ()
diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml
index d017b37219..0ca6468b29 100644
--- a/sw/ground_segment/cockpit/gcs.ml
+++ b/sw/ground_segment/cockpit/gcs.ml
@@ -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
diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile
index 20dd6d6cba..cdec2a0c29 100644
--- a/sw/ground_segment/misc/Makefile
+++ b/sw/ground_segment/misc/Makefile
@@ -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)
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index 756acea4bb..f9bd8c9b9f 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -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
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index 4a6c092145..d4e0790ba3 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -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
diff --git a/sw/include/pprz_version.h b/sw/include/pprz_version.h
index ec381877a0..dfa7ffa619 100644
--- a/sw/include/pprz_version.h
+++ b/sw/include/pprz_version.h
@@ -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 */
diff --git a/sw/lib/ocaml/env.ml b/sw/lib/ocaml/env.ml
index 736cc056ac..bf6caef262 100644
--- a/sw/lib/ocaml/env.ml
+++ b/sw/lib/ocaml/env.ml
@@ -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
diff --git a/sw/lib/ocaml/latlong.ml b/sw/lib/ocaml/latlong.ml
index a97c68f79f..932240f1b5 100644
--- a/sw/lib/ocaml/latlong.ml
+++ b/sw/lib/ocaml/latlong.ml
@@ -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
diff --git a/sw/lib/ocaml/mapCanvas.ml b/sw/lib/ocaml/mapCanvas.ml
index c532f33850..1fd9eb3c30 100644
--- a/sw/lib/ocaml/mapCanvas.ml
+++ b/sw/lib/ocaml/mapCanvas.ml
@@ -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 ->
diff --git a/sw/lib/ocaml/mapCanvas.mli b/sw/lib/ocaml/mapCanvas.mli
index bf7621a5fe..ddaa73f3e0 100644
--- a/sw/lib/ocaml/mapCanvas.mli
+++ b/sw/lib/ocaml/mapCanvas.mli
@@ -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
diff --git a/sw/lib/ocaml/mapFP.ml b/sw/lib/ocaml/mapFP.ml
index 06424b7a66..367f3ecca4 100644
--- a/sw/lib/ocaml/mapFP.ml
+++ b/sw/lib/ocaml/mapFP.ml
@@ -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
_ -> ()
diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml
index e911a99378..fdad35a988 100644
--- a/sw/lib/ocaml/mapWaypoints.ml
+++ b/sw/lib/ocaml/mapWaypoints.ml
@@ -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
diff --git a/sw/lib/ocaml/mapWaypoints.mli b/sw/lib/ocaml/mapWaypoints.mli
index 09c18514f8..0e10bab1bd 100644
--- a/sw/lib/ocaml/mapWaypoints.mli
+++ b/sw/lib/ocaml/mapWaypoints.mli
@@ -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
diff --git a/sw/lib/ocaml/xmlEdit.ml b/sw/lib/ocaml/xmlEdit.ml
index f7409962bd..3427e8f1b0 100644
--- a/sw/lib/ocaml/xmlEdit.ml
+++ b/sw/lib/ocaml/xmlEdit.ml
@@ -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);
diff --git a/sw/lib/python/pprz_math/.gitignore b/sw/lib/python/pprz_math/.gitignore
new file mode 100644
index 0000000000..94803ce577
--- /dev/null
+++ b/sw/lib/python/pprz_math/.gitignore
@@ -0,0 +1,4 @@
+build
+*.c
+geodetic.py
+algebra.py
diff --git a/sw/lib/python/pprz_math/Makefile b/sw/lib/python/pprz_math/Makefile
new file mode 100644
index 0000000000..968b483503
--- /dev/null
+++ b/sw/lib/python/pprz_math/Makefile
@@ -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
diff --git a/sw/lib/python/pprz_math/__init__.py b/sw/lib/python/pprz_math/__init__.py
new file mode 100644
index 0000000000..83a33e4a8f
--- /dev/null
+++ b/sw/lib/python/pprz_math/__init__.py
@@ -0,0 +1 @@
+__all__ = ["geodetic", "algebra"]
diff --git a/sw/lib/python/pprz_math/algebra.i b/sw/lib/python/pprz_math/algebra.i
new file mode 100644
index 0000000000..7fb5641d22
--- /dev/null
+++ b/sw/lib/python/pprz_math/algebra.i
@@ -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
+%}
diff --git a/sw/lib/python/pprz_math/geodetic.i b/sw/lib/python/pprz_math/geodetic.i
new file mode 100644
index 0000000000..f999b7495f
--- /dev/null
+++ b/sw/lib/python/pprz_math/geodetic.i
@@ -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
+%}
diff --git a/sw/lib/python/pprz_math/pprz_algebra_double.i b/sw/lib/python/pprz_math/pprz_algebra_double.i
new file mode 100644
index 0000000000..05b10675d8
--- /dev/null
+++ b/sw/lib/python/pprz_math/pprz_algebra_double.i
@@ -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;
+ }
+};
diff --git a/sw/lib/python/pprz_math/pprz_algebra_float.i b/sw/lib/python/pprz_math/pprz_algebra_float.i
new file mode 100644
index 0000000000..bb2073928b
--- /dev/null
+++ b/sw/lib/python/pprz_math/pprz_algebra_float.i
@@ -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;
+ }
+};
diff --git a/sw/lib/python/pprz_math/pprz_algebra_int.i b/sw/lib/python/pprz_math/pprz_algebra_int.i
new file mode 100644
index 0000000000..09b3e41230
--- /dev/null
+++ b/sw/lib/python/pprz_math/pprz_algebra_int.i
@@ -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;
+ }
+};
diff --git a/sw/lib/python/pprz_math/pprz_geodetic_double.i b/sw/lib/python/pprz_math/pprz_geodetic_double.i
new file mode 100644
index 0000000000..683826ba41
--- /dev/null
+++ b/sw/lib/python/pprz_math/pprz_geodetic_double.i
@@ -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(<p, $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;
+ }
+};
diff --git a/sw/lib/python/pprz_math/pprz_geodetic_float.i b/sw/lib/python/pprz_math/pprz_geodetic_float.i
new file mode 100644
index 0000000000..db6de25a65
--- /dev/null
+++ b/sw/lib/python/pprz_math/pprz_geodetic_float.i
@@ -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(<p, $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(<p, $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;
+ }
+};
diff --git a/sw/lib/python/pprz_math/pprz_geodetic_int.i b/sw/lib/python/pprz_math/pprz_geodetic_int.i
new file mode 100644
index 0000000000..19cba94a9b
--- /dev/null
+++ b/sw/lib/python/pprz_math/pprz_geodetic_int.i
@@ -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;
+ }
+};
diff --git a/sw/lib/python/pprz_math/setup.py b/sw/lib/python/pprz_math/setup.py
new file mode 100644
index 0000000000..330c81d49c
--- /dev/null
+++ b/sw/lib/python/pprz_math/setup.py
@@ -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"],
+ )
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.
diff --git a/sw/simulator/nps/nps_ivy_fixedwing.c b/sw/simulator/nps/nps_ivy_fixedwing.c
index 0172dbca2f..f94e099a05 100644
--- a/sw/simulator/nps/nps_ivy_fixedwing.c
+++ b/sw/simulator/nps/nps_ivy_fixedwing.c
@@ -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;
diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c
index 90b89eeb3a..9ab8dfc3b0 100644
--- a/sw/simulator/nps/nps_ivy_rotorcraft.c
+++ b/sw/simulator/nps/nps_ivy_rotorcraft.c
@@ -47,11 +47,12 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
struct LlaCoor_i lla;
lla.lat = atoi(argv[3]);
lla.lon = atoi(argv[4]);
- /* 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 = atoi(argv[5]) *10 - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
+ lla.alt = atoi(argv[5]) - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
nav_move_waypoint_lla(wp_id, &lla);
- printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, waypoints[wp_id].x, waypoints[wp_id].y, waypoints[wp_id].z);
+ printf("move wp id=%d x=% .4f, y=% .4f, z=% .4f\n", wp_id,
+ WaypointX(wp_id), WaypointY(wp_id), WaypointAlt(wp_id));
}
}
diff --git a/sw/supervision/paparazzicenter.glade b/sw/supervision/paparazzicenter.glade
index bd3b28cb0f..8de95b8162 100644
--- a/sw/supervision/paparazzicenter.glade
+++ b/sw/supervision/paparazzicenter.glade
@@ -514,6 +514,7 @@
True
False
________________
+ start
True
@@ -594,6 +595,7 @@
True
False
_________________
+ start
True
@@ -816,6 +818,7 @@
True
False
_________________
+ start
True
@@ -894,6 +897,7 @@
True
False
_________________
+ start
True
diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml
index 517780c0cb..a1ce64e7f3 100644
--- a/sw/tools/generators/gen_flight_plan.ml
+++ b/sw/tools/generators/gen_flight_plan.ml
@@ -134,7 +134,15 @@ let print_waypoint_lla = fun utm0 default_alt waypoint ->
and alt = try sof (float_attrib waypoint "height" +. !ground_alt) with _ -> default_alt in
let alt = try Xml.attrib waypoint "alt" with _ -> alt in
let wgs84 = Latlong.of_utm Latlong.WGS84 (Latlong.utm_add utm0 (x, y)) in
- printf " {%Ld, %Ld, %.0f}, /* 1e7deg, 1e7deg, cm (hmsl=%.2fm) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (100. *. float_of_string alt) (Egm96.of_wgs84 wgs84)
+ printf " {.lat=%Ld, .lon=%Ld, .alt=%.0f}, /* 1e7deg, 1e7deg, mm (above NAV_MSL0, local msl=%.2fm) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (1000. *. float_of_string alt) (Egm96.of_wgs84 wgs84)
+
+let print_waypoint_lla_wgs84 = fun utm0 default_alt waypoint ->
+ let (x, y) = (float_attrib waypoint "x", float_attrib waypoint "y")
+ and alt = try sof (float_attrib waypoint "height" +. !ground_alt) with _ -> default_alt in
+ let alt = try Xml.attrib waypoint "alt" with _ -> alt in
+ let wgs84 = Latlong.of_utm Latlong.WGS84 (Latlong.utm_add utm0 (x, y)) in
+ let alt = float_of_string alt +. Egm96.of_wgs84 wgs84 in
+ printf " {.lat=%Ld, .lon=%Ld, .alt=%.0f}, /* 1e7deg, 1e7deg, mm (above WGS84 ref ellipsoid) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (1000. *. alt)
let get_index_block = fun x ->
@@ -837,6 +845,9 @@ let () =
Xml2h.define "WAYPOINTS_LLA" "{ \\";
List.iter (print_waypoint_lla utm0 alt) waypoints;
lprintf "};\n";
+ Xml2h.define "WAYPOINTS_LLA_WGS84" "{ \\";
+ List.iter (print_waypoint_lla_wgs84 utm0 alt) waypoints;
+ lprintf "};\n";
Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints));
Xml2h.define "FP_BLOCKS" "{ \\";
diff --git a/sw/tools/generators/gen_modules.ml b/sw/tools/generators/gen_modules.ml
index 35589412e7..0781512dfc 100644
--- a/sw/tools/generators/gen_modules.ml
+++ b/sw/tools/generators/gen_modules.ml
@@ -175,10 +175,18 @@ let print_periodic_functions = fun modules ->
end
else begin
let status = get_status_name f module_name in
- let start = (ExtXml.attrib_or_default f "start" "") in
- lprintf out_h "if (%s == MODULES_START) { %s; %s = MODULES_RUN; }\n" status start status;
- let stop = (ExtXml.attrib_or_default f "stop" "") in
- lprintf out_h "if (%s == MODULES_STOP) { %s; %s = MODULES_IDLE; }\n" status stop status;
+ lprintf out_h "if (%s == MODULES_START) {\n" status;
+ right ();
+ ignore(try lprintf out_h "%s;\n" (Xml.attrib f "start") with _ -> ());
+ lprintf out_h "%s = MODULES_RUN;\n" status;
+ left ();
+ lprintf out_h "}\n";
+ lprintf out_h "if (%s == MODULES_STOP) {\n" status;
+ right ();
+ ignore(try lprintf out_h "%s;\n" (Xml.attrib f "stop") with _ -> ());
+ lprintf out_h "%s = MODULES_IDLE;\n" status;
+ left ();
+ lprintf out_h "}\n";
end
)
periodic)