diff --git a/conf/abi.xml b/conf/abi.xml
index db4296ab99..e567827741 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -261,6 +261,10 @@
flag to reset REF|VECTICAL_REF|VERTICAL_POS
+
+
+
+
diff --git a/conf/modules/ins_ekf2.xml b/conf/modules/ins_ekf2.xml
index 3fbce0272e..5240faf2da 100644
--- a/conf/modules/ins_ekf2.xml
+++ b/conf/modules/ins_ekf2.xml
@@ -40,7 +40,8 @@
-
+
+
diff --git a/conf/telemetry/GVF/gvf_default_rotorcraft.xml b/conf/telemetry/GVF/gvf_default_rotorcraft.xml
index 4c6e3d89bd..a7c2947ebd 100644
--- a/conf/telemetry/GVF/gvf_default_rotorcraft.xml
+++ b/conf/telemetry/GVF/gvf_default_rotorcraft.xml
@@ -178,9 +178,9 @@
-
-
-
+
+
+
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index bfaf83dc44..8df2636814 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -176,9 +176,9 @@
-
-
-
+
+
+
diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml
index e6e0209038..8cfe818e17 100644
--- a/conf/telemetry/highspeed_rotorcraft.xml
+++ b/conf/telemetry/highspeed_rotorcraft.xml
@@ -47,7 +47,7 @@
-
+
@@ -110,7 +110,7 @@
-
+
@@ -275,7 +275,7 @@
-
+
@@ -298,7 +298,7 @@
-
+
@@ -373,7 +373,7 @@
-
+
diff --git a/conf/telemetry/oneloop_telemetry.xml b/conf/telemetry/oneloop_telemetry.xml
index 4ec6d3422d..a958b46013 100644
--- a/conf/telemetry/oneloop_telemetry.xml
+++ b/conf/telemetry/oneloop_telemetry.xml
@@ -47,7 +47,7 @@
-
+
@@ -109,7 +109,7 @@
-
+
@@ -276,7 +276,7 @@
-
+
@@ -319,7 +319,7 @@
-
+
diff --git a/sw/airborne/modules/gps/gps.c b/sw/airborne/modules/gps/gps.c
index 208471a24f..fce2e98832 100644
--- a/sw/airborne/modules/gps/gps.c
+++ b/sw/airborne/modules/gps/gps.c
@@ -63,12 +63,17 @@ PRINT_CONFIG_VAR(SECONDARY_GPS)
#endif
#endif
+/* Maximum number of relative positions */
+#ifndef GPS_RELPOS_MAX
+#define GPS_RELPOS_MAX 3
+#endif
+
#define MSEC_PER_WEEK (1000*60*60*24*7)
#define TIME_TO_SWITCH 5000 //ten s in ms
struct GpsState gps;
struct GpsTimeSync gps_time_sync;
-struct GpsRelposNED gps_relposned;
+static struct RelPosNED gps_relposned[GPS_RELPOS_MAX] = {0};
#ifdef SECONDARY_GPS
static uint8_t current_gps_id = GpsId(PRIMARY_GPS);
@@ -160,18 +165,26 @@ static void send_gps(struct transport_tx *trans, struct link_device *dev)
send_svinfo_available(trans, dev);
}
-static void send_gps_rtk(struct transport_tx *trans, struct link_device *dev)
+static void send_gps_relpos(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_GPS_RTK(trans, dev, AC_ID,
- &gps_relposned.iTOW,
- &gps_relposned.refStationId,
- &gps_relposned.relPosN, &gps_relposned.relPosE, &gps_relposned.relPosD,
- &gps_relposned.relPosHPN, &gps_relposned.relPosHPE, &gps_relposned.relPosHPD,
- &gps_relposned.accN, &gps_relposned.accE, &gps_relposned.accD,
- &gps_relposned.carrSoln,
- &gps_relposned.relPosValid,
- &gps_relposned.diffSoln,
- &gps_relposned.gnssFixOK);
+ static uint8_t idx = 0;
+ if(gps_relposned[idx].tow == 0)
+ return;
+
+ pprz_msg_send_GPS_RELPOS(trans, dev, AC_ID,
+ &gps_relposned[idx].reference_id,
+ &gps_relposned[idx].tow,
+ &gps_relposned[idx].pos.x, &gps_relposned[idx].pos.y, &gps_relposned[idx].pos.z,
+ &gps_relposned[idx].distance,
+ &gps_relposned[idx].heading,
+ &gps_relposned[idx].pos_acc.x, &gps_relposned[idx].pos_acc.y, &gps_relposned[idx].pos_acc.z,
+ &gps_relposned[idx].distance_acc,
+ &gps_relposned[idx].heading_acc);
+
+ // Send the next index that is set
+ idx++;
+ if(idx >= GPS_RELPOS_MAX || gps_relposned[idx].tow == 0)
+ idx = 0;
}
static void send_gps_int(struct transport_tx *trans, struct link_device *dev)
@@ -317,6 +330,22 @@ static void gps_cb(uint8_t sender_id,
}
}
+static abi_event gps_relpos_ev;
+static void gps_relpos_cb(uint8_t sender_id __attribute__((unused)),
+ uint32_t stamp __attribute__((unused)),
+ struct RelPosNED *relpos)
+{
+ for(uint8_t i = 0; i < GPS_RELPOS_MAX; i++) {
+ // Find our index or a free index
+ if(gps_relposned[i].tow == 0 || gps_relposned[i].reference_id == relpos->reference_id)
+ {
+ // Copy and save result
+ gps_relposned[i] = *relpos;
+ break;
+ }
+ }
+}
+
void gps_init(void)
{
@@ -335,8 +364,6 @@ void gps_init(void)
gps.last_msg_ticks = 0;
gps.last_msg_time = 0;
- gps_relposned.relPosHeading = NAN;
-
#ifdef GPS_POWER_GPIO
gpio_setup_output(GPS_POWER_GPIO);
GPS_POWER_GPIO_ON(GPS_POWER_GPIO);
@@ -346,6 +373,7 @@ void gps_init(void)
#endif
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
+ AbiBindMsgRELPOS(ABI_BROADCAST, &gps_relpos_ev, gps_relpos_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS, send_gps);
@@ -353,7 +381,7 @@ void gps_init(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_LLA, send_gps_lla);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_SOL, send_gps_sol);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SVINFO, send_svinfo);
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_RTK, send_gps_rtk);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_RELPOS, send_gps_relpos);
#endif
/* Register preflight checks */
diff --git a/sw/airborne/modules/gps/gps.h b/sw/airborne/modules/gps/gps.h
index 29f0aaf030..5d39972209 100644
--- a/sw/airborne/modules/gps/gps.h
+++ b/sw/airborne/modules/gps/gps.h
@@ -34,6 +34,7 @@ extern "C" {
#include "std.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_geodetic_float.h"
+#include "math/pprz_geodetic_double.h"
#include "mcu_periph/sys_time.h"
#include "generated/airframe.h"
@@ -124,27 +125,18 @@ struct GpsTimeSync {
uint32_t t0_ticks; ///< hw clock ticks when GPS message is received
};
-/** data structures for GPS with RTK capabilities */
-struct GpsRelposNED {
- uint32_t iTOW;
- uint16_t refStationId;
- int32_t relPosN;
- int32_t relPosE;
- int32_t relPosD;
- int8_t relPosHPN;
- int8_t relPosHPE;
- int8_t relPosHPD;
- float relPosLength;
- float relPosHeading;
- uint32_t accN;
- uint32_t accE;
- uint32_t accD;
- uint8_t carrSoln;
- uint8_t relPosValid;
- uint8_t diffSoln;
- uint8_t gnssFixOK;
+struct RelPosNED {
+ uint16_t reference_id; ///< Reference station identification
+ uint32_t tow; ///< Time of week (GPS) in ms
+
+ struct NedCoor_d pos; ///< Relative postion to the reference station in meters
+ double distance; ///< Relative distance to the reference station in meters
+ float heading; ///< Relative heading to the reference station in radians
+
+ struct NedCoor_f pos_acc; ///< Position accuracy in meters
+ float distance_acc; ///< Distance accuracy in meters
+ float heading_acc; ///< Heading accuracy in radians
};
-extern struct GpsRelposNED gps_relposned;
/** global GPS state */
extern struct GpsState gps;
diff --git a/sw/airborne/modules/gps/gps_ubx.c b/sw/airborne/modules/gps/gps_ubx.c
index ded6041baa..7962802b7b 100644
--- a/sw/airborne/modules/gps/gps_ubx.c
+++ b/sw/airborne/modules/gps/gps_ubx.c
@@ -79,7 +79,6 @@ void gps_ubx_parse(struct GpsUbx *gubx, uint8_t c);
void gps_ubx_msg(struct GpsUbx *gubx);
#if USE_GPS_UBX_RTCM
-extern struct GpsRelposNED gps_relposned;
extern struct RtcmMan rtcm_man;
#ifndef INJECT_BUFF_SIZE
@@ -434,26 +433,22 @@ static void gps_ubx_parse_nav_relposned(struct GpsUbx *gubx)
gubx->state.fix = 0;
}
- gps_relposned.iTOW = UBX_NAV_RELPOSNED_iTOW(gubx->msg_buf);
- gps_relposned.refStationId = UBX_NAV_RELPOSNED_refStationId(gubx->msg_buf);
- gps_relposned.relPosN = UBX_NAV_RELPOSNED_relPosN(gubx->msg_buf);
- gps_relposned.relPosE = UBX_NAV_RELPOSNED_relPosE(gubx->msg_buf);
- gps_relposned.relPosD = UBX_NAV_RELPOSNED_relPosD(gubx->msg_buf) ;
- gps_relposned.relPosHPN = UBX_NAV_RELPOSNED_relPosHPN(gubx->msg_buf);
- gps_relposned.relPosHPE = UBX_NAV_RELPOSNED_relPosHPE(gubx->msg_buf);
- gps_relposned.relPosHPD = UBX_NAV_RELPOSNED_relPosHPD(gubx->msg_buf);
- gps_relposned.relPosLength = UBX_NAV_RELPOSNED_relPosLength(gubx->msg_buf) / 100.f;
- gps_relposned.relPosHeading = UBX_NAV_RELPOSNED_relPosHeading(gubx->msg_buf) * 1e-5;
- gps_relposned.accN = UBX_NAV_RELPOSNED_relPosHeading(gubx->msg_buf) * 1e-4;
- gps_relposned.accE = UBX_NAV_RELPOSNED_relPosLength(gubx->msg_buf);
- gps_relposned.accD = UBX_NAV_RELPOSNED_accD(gubx->msg_buf);
- gps_relposned.carrSoln = carrSoln;
- gps_relposned.relPosValid = relPosValid;
- gps_relposned.diffSoln = diffSoln;
- gps_relposned.gnssFixOK = gnssFixOK;
- } else {
- gps_relposned.relPosHeading = NAN;
- gps_relposned.accN = 999;
+ uint32_t now_ts = get_sys_time_usec();
+ struct RelPosNED relpos = {0};
+ relpos.reference_id = UBX_NAV_RELPOSNED_refStationId(gubx->msg_buf);
+ relpos.tow = UBX_NAV_RELPOSNED_iTOW(gubx->msg_buf);
+ relpos.pos.x = UBX_NAV_RELPOSNED_relPosN(gubx->msg_buf) * 1e-2 + UBX_NAV_RELPOSNED_relPosHPN(gubx->msg_buf) * 1e-4;
+ relpos.pos.y = UBX_NAV_RELPOSNED_relPosE(gubx->msg_buf) * 1e-2 + UBX_NAV_RELPOSNED_relPosHPE(gubx->msg_buf) * 1e-4;
+ relpos.pos.z = UBX_NAV_RELPOSNED_relPosD(gubx->msg_buf) * 1e-2 + UBX_NAV_RELPOSNED_relPosHPD(gubx->msg_buf) * 1e-4;
+ relpos.distance = UBX_NAV_RELPOSNED_relPosLength(gubx->msg_buf) * 1e-2;
+ relpos.heading = RadOfDeg(UBX_NAV_RELPOSNED_relPosHeading(gubx->msg_buf) * 1e-5);
+ relpos.pos_acc.x = UBX_NAV_RELPOSNED_accN(gubx->msg_buf) * 1e-4;
+ relpos.pos_acc.y = UBX_NAV_RELPOSNED_accE(gubx->msg_buf) * 1e-4;
+ relpos.pos_acc.z = UBX_NAV_RELPOSNED_accD(gubx->msg_buf) * 1e-4;
+ relpos.distance_acc = UBX_NAV_RELPOSNED_accLength(gubx->msg_buf) * 1e-4;
+ relpos.heading_acc = RadOfDeg(UBX_NAV_RELPOSNED_accHeading(gubx->msg_buf) * 1e-5);
+
+ AbiSendMsgRELPOS(gubx->state.comp_id, now_ts, &relpos);
}
}
#else
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index 4e7bc593be..5c76a75be5 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -146,6 +146,12 @@ PRINT_CONFIG_VAR(INS_EKF2_MAG_ID)
#endif
PRINT_CONFIG_VAR(INS_EKF2_GPS_ID)
+/* default RELPOS to use for heading in INS */
+#ifndef INS_EKF2_RELPOS_ID
+#define INS_EKF2_RELPOS_ID ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(INS_EKF2_RELPOS_ID)
+
/* default Optical Flow to use in INS */
#ifndef INS_EKF2_OF_ID
#define INS_EKF2_OF_ID ABI_BROADCAST
@@ -278,6 +284,11 @@ PRINT_CONFIG_VAR(INS_EKF2_GPS_P_NOISE)
#endif
PRINT_CONFIG_VAR(INS_EKF2_BARO_NOISE)
+/* Maximum allowed distance error for the RTK relative heading measurement (m) */
+#ifndef INS_EKF2_RELHEADING_ERR
+#define INS_EKF2_RELHEADING_ERR 0.2
+#endif
+
#ifdef INS_EXT_VISION_ROTATION
struct FloatQuat ins_ext_vision_rot;
#endif
@@ -290,6 +301,7 @@ static abi_event gyro_int_ev;
static abi_event accel_int_ev;
static abi_event mag_ev;
static abi_event gps_ev;
+static abi_event relpos_ev;
static abi_event optical_flow_ev;
static abi_event reset_ev;
@@ -301,6 +313,7 @@ static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *de
static void accel_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt);
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
+static void relpos_cb(uint8_t sender_id, uint32_t stamp, struct RelPosNED *relpos);
static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
static void reset_cb(uint8_t sender_id, uint8_t flag);
@@ -633,6 +646,7 @@ void ins_ekf2_init(void)
AbiBindMsgIMU_ACCEL_INT(INS_EKF2_ACCEL_ID, &accel_int_ev, accel_int_cb);
AbiBindMsgIMU_MAG(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
+ AbiBindMsgRELPOS(INS_EKF2_RELPOS_ID, &relpos_ev, relpos_cb);
AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
AbiBindMsgINS_RESET(ABI_BROADCAST, &reset_ev, reset_cb);
}
@@ -983,10 +997,10 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
#if INS_EKF2_GPS_COURSE_YAW
gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
gps_msg.yaw_offset = 0;
-#elif defined(INS_EKF2_GPS_YAW_OFFSET) && defined(INS_EKF2_ANTENNA_DISTANCE)
- if(ISFINITE(gps_relposned.relPosHeading) && gps_relposned.relPosValid && gps_relposned.diffSoln && gps_relposned.carrSoln >= 1
- && fabsf(gps_relposned.relPosLength - INS_EKF2_ANTENNA_DISTANCE) <= INS_EKF2_MAX_REL_LENGTH_ERROR * INS_EKF2_ANTENNA_DISTANCE) {
- gps_msg.yaw = wrap_pi(RadOfDeg(gps_relposned.relPosHeading - INS_EKF2_GPS_YAW_OFFSET));
+#elif defined(INS_EKF2_GPS_YAW_OFFSET)
+ if(ekf2.rel_heading_valid) {
+ gps_msg.yaw = wrap_pi(ekf2.rel_heading - RadOfDeg(INS_EKF2_GPS_YAW_OFFSET));
+ ekf2.rel_heading_valid = false;
} else {
gps_msg.yaw = NAN;
}
@@ -1013,6 +1027,26 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
ekf.setGpsData(gps_msg);
}
+/* Update the local relative position information */
+static void relpos_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct RelPosNED *relpos)
+{
+ // Verify if we received a valid heading
+ if(
+#ifdef INS_EKF2_RELHEADING_REF_ID
+ relpos->reference_id != INS_EKF2_RELHEADING_REF_ID ||
+#endif
+#ifdef INS_EKF2_RELHEADING_DISTANCE
+ fabs(relpos->distance - INS_EKF2_RELHEADING_DISTANCE) > INS_EKF2_RELHEADING_ERR ||
+#endif
+ !ISFINITE(relpos->heading)
+ ) {
+ return;
+ }
+
+ ekf2.rel_heading = relpos->heading;
+ ekf2.rel_heading_valid = true;
+}
+
/* Update INS based on Optical Flow information */
static void optical_flow_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp,
diff --git a/sw/airborne/modules/ins/ins_ekf2.h b/sw/airborne/modules/ins/ins_ekf2.h
index 6f894e2012..fec63df9e4 100644
--- a/sw/airborne/modules/ins/ins_ekf2.h
+++ b/sw/airborne/modules/ins/ins_ekf2.h
@@ -45,6 +45,8 @@ struct ekf2_t {
bool gyro_valid; ///< If we received a gyroscope measurement
bool accel_valid; ///< If we received a acceleration measurement
uint32_t flow_stamp; ///< Optic flow last abi message timestamp
+ float rel_heading; ///< Relative heading from RTK gps (rad)
+ bool rel_heading_valid; ///< If we received a valid relative heading
float temp; ///< Latest temperature measurement in degrees celcius
float qnh; ///< QNH value in hPa
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index b8c9f78d6b..ac28897e79 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit b8c9f78d6b5bf02edd62f8726bac890cf639195d
+Subproject commit ac28897e79ac6f626f5584c42e2e05dac44ec5dc