Skip to content

Commit

Permalink
[gps] Move GPS relative position to ABI message (#3410)
Browse files Browse the repository at this point in the history
  • Loading branch information
@fvantienen
fvantienen authored Dec 5, 2024
1 parent dfd8e93 commit 31b8445
Show file tree
Hide file tree
Showing 12 changed files with 133 additions and 77 deletions.
4 changes: 4 additions & 0 deletions conf/abi.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -261,6 +261,10 @@
<field name="flag" type="uint8_t">flag to reset REF|VECTICAL_REF|VERTICAL_POS</field>
</message>

<message name="RELPOS" id="39">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="relpos" type="struct RelPosNED *"/>
</message>
</msg_class>

</protocol>
3 changes: 2 additions & 1 deletion conf/modules/ins_ekf2.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,8 @@
<define name="INS_EKF2_GYRO_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for gyro measurements"/>
<define name="INS_EKF2_ACCEL_ID" value="ABI_BROADCAST" description="ABI sensor ID used ad input for acceleration measurements"/>
<define name="INS_EKF2_MAG_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for magnetic measurements"/>
<define name="INS_EKF2_GPS_ID" value="ABI_BROADCAST" description="ABI sensor ID used ad input for GPS measurements"/>
<define name="INS_EKF2_GPS_ID" value="GPS_MULTI_ID" description="ABI sensor ID used ad input for GPS measurements"/>
<define name="INS_EKF2_RELPOS_ID" value="ABI_BROADCAST" description="ABI sensor ID used ad input for Relative Position heading measurements"/>
</doc>
<settings>
<dl_settings NAME="INS">
Expand Down
6 changes: 3 additions & 3 deletions conf/telemetry/GVF/gvf_default_rotorcraft.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,9 +178,9 @@
</mode>

<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RELPOS" period="1"/>
</mode>

</process>
Expand Down
6 changes: 3 additions & 3 deletions conf/telemetry/default_rotorcraft.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,9 +176,9 @@
</mode>

<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RELPOS" period="1"/>
</mode>

</process>
Expand Down
10 changes: 5 additions & 5 deletions conf/telemetry/highspeed_rotorcraft.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@
<message name="ESC" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.1"/>
<message name="INS_FLOW_INFO" period="0.1"/>
<message name="GPS_RTK" period="0.1"/>
<message name="GPS_RELPOS" period="0.1"/>
<message name="AOA" period="0.2"/>
<message name="APPROACH_MOVING_TARGET" period="0.1"/>
<message name="AIRSPEED_RAW" period="0.1"/>
Expand Down Expand Up @@ -110,7 +110,7 @@
<message name="ESC" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.2"/>
<message name="INS_FLOW_INFO" period="0.2"/>
<message name="GPS_RTK" period="0.2"/>
<message name="GPS_RELPOS" period="0.2"/>
<message name="AOA" period="0.2"/>
<message name="APPROACH_MOVING_TARGET" period="0.2"/>
<message name="AIRSPEED_RAW" period="0.2"/>
Expand Down Expand Up @@ -275,7 +275,7 @@
<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
<message name="GPS_RELPOS" period="1"/>
</mode>

<mode name="Reduced">
Expand All @@ -298,7 +298,7 @@
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<message name="ESC" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.07"/>
<message name="GPS_RTK" period="0.9"/>
<message name="GPS_RELPOS" period="0.9"/>
<message name="APPROACH_MOVING_TARGET" period="2.1"/>
<message name="TARGET_POS_INFO" period="2.1"/>
<message name="ROTATING_WING_STATE" period="0.07"/>
Expand Down Expand Up @@ -373,7 +373,7 @@
<message name="ESC" period="0.02"/>
<message name="STAB_ATTITUDE" period="0.002"/>
<message name="ACTUATORS" period="0.002"/>
<message name="GPS_RTK" period="0.1"/>
<message name="GPS_RELPOS" period="0.05"/>
<message name="IMU_HEATER" period="6.2"/>
<message name="EFF_MAT_STAB" period="0.002"/>
<message name="EFF_MAT_GUID" period="0.002"/>
Expand Down
8 changes: 4 additions & 4 deletions conf/telemetry/oneloop_telemetry.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@
<message name="ESC" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.2"/>
<message name="INS_FLOW_INFO" period="0.2"/>
<message name="GPS_RTK" period="0.2"/>
<message name="GPS_RELPOS" period="0.2"/>
<message name="AOA" period="0.2"/>
<message name="APPROACH_MOVING_TARGET" period="0.2"/>
<message name="AIRSPEED_RAW" period="0.2"/>
Expand Down Expand Up @@ -109,7 +109,7 @@
<message name="ESC" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.2"/>
<message name="INS_FLOW_INFO" period="0.2"/>
<message name="GPS_RTK" period="0.2"/>
<message name="GPS_RELPOS" period="0.2"/>
<message name="AOA" period="0.2"/>
<message name="APPROACH_MOVING_TARGET" period="0.2"/>
<message name="AIRSPEED_RAW" period="0.2"/>
Expand Down Expand Up @@ -276,7 +276,7 @@
<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
<message name="GPS_RELPOS" period="1"/>
</mode>

</process>
Expand Down Expand Up @@ -319,7 +319,7 @@
<message name="ESC" period="0.02"/>
<message name="STAB_ATTITUDE" period="0.002"/>
<message name="ACTUATORS" period="0.002"/>
<message name="GPS_RTK" period="0.1"/>
<message name="GPS_RELPOS" period="0.1"/>
<message name="IMU_HEATER" period="1.0"/>
<message name="AIRSPEED_RAW" period="0.01"/>
<message name="EFF_MAT_STAB" period="0.02"/>
Expand Down
58 changes: 43 additions & 15 deletions sw/airborne/modules/gps/gps.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
{
Expand All @@ -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);
Expand All @@ -346,14 +373,15 @@ 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);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_INT, send_gps_int);
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 */
Expand Down
32 changes: 12 additions & 20 deletions sw/airborne/modules/gps/gps.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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;
Expand Down
37 changes: 16 additions & 21 deletions sw/airborne/modules/gps/gps_ubx.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
Loading

0 comments on commit 31b8445

Please sign in to comment.