Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[gps] Move GPS relative position to ABI message (#3410)
Doxygen / build (push) Has been cancelled
Details

Browse Source
This commit is contained in:
Freek van Tienen
2024-12-05 14:01:27 +01:00
committed by GitHub
parent dfd8e93927
commit 31b84454c2
12 changed files with 133 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+4
View File
@@ -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>
conf/modules/ins_ekf2.xml
+2 -1
View File
@@ -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">
conf/telemetry/GVF/gvf_default_rotorcraft.xml
+3 -3
View File
@@ -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>
conf/telemetry/default_rotorcraft.xml
+3 -3
View File
@@ -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>
conf/telemetry/highspeed_rotorcraft.xml
+5 -5
View File
@@ -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"/>
@@ -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"/>
@@ -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">
@@ -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"/>
@@ -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"/>
conf/telemetry/oneloop_telemetry.xml
+4 -4
View File
@@ -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"/>
@@ -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"/>
@@ -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>
@@ -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"/>
sw/airborne/modules/gps/gps.c
+43 -15
View File
@@ -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 */
sw/airborne/modules/gps/gps.h
+12 -20
View File
@@ -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;
sw/airborne/modules/gps/gps_ubx.c
+16 -21
View File
@@ -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
sw/airborne/modules/ins/ins_ekf2.cpp
+38 -4
View File
@@ -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,
sw/airborne/modules/ins/ins_ekf2.h
+2
View File
@@ -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
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: b8c9f78d6b...ac28897e79