Skip to content

Commit

Permalink
AP_GPS: add support for calculating gps message lag over DroneCAN
Browse files Browse the repository at this point in the history
  • Loading branch information
@bugobliterator
bugobliterator committed Dec 9, 2023
1 parent 29dda88 commit 9d3fbf1
Show file tree
Hide file tree
Showing 4 changed files with 76 additions and 8 deletions.
41 changes: 41 additions & 0 deletions libraries/AP_GPS/AP_GPS_DroneCAN.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -340,6 +340,45 @@ void AP_GPS_DroneCAN::handle_velocity(const float vx, const float vy, const floa
}
}

void AP_GPS_DroneCAN::update_fix_lag(uint64_t gnss_timestamp_usec)
{
if (timesync_correction) {
// calculate precise lag
uint64_t current_time = timesync_correction_us + AP_HAL::micros64() + NATIVE_TIME_OFFSET;
_fix_lag = (current_time - gnss_timestamp_usec) / 1000000.0f;
last_gnss_timestamp_usec = gnss_timestamp_usec;
fix_lag_valid = true;
}
}

void AP_GPS_DroneCAN::update_relposned_lag()
{
if (timesync_correction && (last_gnss_timestamp_usec != 0)) {
// calculate precise lag
uint64_t current_time = timesync_correction_us + AP_HAL::micros64() + NATIVE_TIME_OFFSET;
_relposned_lag = (current_time - last_gnss_timestamp_usec) / 1000000.0f;
relposned_lag_valid = true;
}
}

bool AP_GPS_DroneCAN::get_fix_lag(float &lag) const
{
if (fix_lag_valid) {
lag = _fix_lag;
return true;
}
return AP_GPS_Backend::get_lag(lag);
}

bool AP_GPS_DroneCAN::get_relposned_lag(float &lag) const
{
if (relposned_lag_valid) {
lag = _relposned_lag;
return true;
}
return get_fix_lag(lag);
}

void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec)
{
bool process = false;
Expand Down Expand Up @@ -419,6 +458,7 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin
}

interim_state.num_sats = msg.sats_used;
update_fix_lag(msg.gnss_timestamp.usec);
} else {
interim_state.have_vertical_velocity = false;
interim_state.have_vertical_accuracy = false;
Expand Down Expand Up @@ -572,6 +612,7 @@ void AP_GPS_DroneCAN::handle_relposheading_msg(const ardupilot_gnss_RelPosHeadin
interim_state.gps_yaw_configured = true;
seen_relposheading = true;
// push raw heading data to calculate moving baseline heading states
update_relposned_lag();
if (calculate_moving_base_yaw(interim_state,
msg.reported_heading_deg,
msg.relative_distance_m,
Expand Down
17 changes: 17 additions & 0 deletions libraries/AP_GPS/AP_GPS_DroneCAN.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,16 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
// returns true once configuration has finished
bool do_config(void);

void update_fix_lag(uint64_t gnss_timestamp_usec);
void update_relposned_lag();

bool get_lag(float &lag) const override {
return get_fix_lag(lag);
}

bool get_fix_lag(float &lag) const;
bool get_relposned_lag(float &lag) const override;

void handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec);
void handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg);
void handle_heading_msg(const ardupilot_gnss_Heading& msg);
Expand All @@ -105,6 +115,13 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
static void give_registry();
static AP_GPS_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id);

float _fix_lag;
bool fix_lag_valid;
uint64_t last_gnss_timestamp_usec;

float _relposned_lag;
bool relposned_lag_valid;

bool _new_data;
AP_GPS::GPS_State interim_state;

Expand Down
23 changes: 15 additions & 8 deletions libraries/AP_GPS/GPS_Backend.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -328,6 +328,8 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state,

bool selectedOffset = false;
Vector3f offset;
Vector3f antenna_tilt {};
float lag = 0.1;
switch (MovingBase::Type(gps.mb_params[interim_state.instance].type.get())) {
case MovingBase::Type::RelativeToAlternateInstance:
offset = gps._antenna_offset[interim_state.instance^1].get() - gps._antenna_offset[interim_state.instance].get();
Expand Down Expand Up @@ -373,8 +375,7 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state,
#if AP_AHRS_ENABLED
{
// get lag
float lag = 0.1;
get_lag(lag);
get_relposned_lag(lag);

// get vehicle rotation, projected back in time using the gyro
// this is not 100% accurate, but it is good enough for
Expand All @@ -388,7 +389,7 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state,
rot_body_to_ned.rotate(gyro * (-lag));

// apply rotation to the offset to get the Z offset in NED
const Vector3f antenna_tilt = rot_body_to_ned * offset;
antenna_tilt = rot_body_to_ned * offset;
const float alt_error = reported_D + antenna_tilt.z;

if (fabsf(alt_error) > permitted_error_length_pct * min_dist) {
Expand All @@ -398,6 +399,8 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state,
goto bad_yaw;
}
}
#else
(void)lag;
#endif // AP_AHRS_ENABLED

{
Expand All @@ -424,17 +427,21 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state,
// @Field: RHD: reported heading,deg
// @Field: RDist: antenna separation,m
// @Field: RDown: vertical antenna separation,m
// @Field: CRDown: corrected vertical antenna separation,m
// @Field: OK: 1 if have yaw
AP::logger().WriteStreaming("GPYW", "TimeUS,Id,RHD,RDist,RDown,OK",
"s#dmm-",
"F-----",
"QBfffB",
// @Field: lag: lag in actual information,sec
AP::logger().WriteStreaming("GPYW", "TimeUS,Id,RHD,RDist,RDown,CRDown,OK,lag,fixlag",
"s#dmmm-ss",
"F--------",
"QBffffBff",
AP_HAL::micros64(),
state.instance,
reported_heading_deg,
reported_distance,
reported_D,
interim_state.have_gps_yaw);
antenna_tilt.z,
interim_state.have_gps_yaw,
lag);
#endif

return interim_state.have_gps_yaw;
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_GPS/GPS_Backend.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,9 @@ class AP_GPS_Backend
// driver specific lag, returns true if the driver is confident in the provided lag
virtual bool get_lag(float &lag) const { lag = 0.2f; return true; }

// driver specific lag for relposned, returns true if the driver is confident in the provided lag
virtual bool get_relposned_lag(float &lag) const { return get_lag(lag); }

// driver specific health, returns true if the driver is healthy
virtual bool is_healthy(void) const { return true; }
// returns true if the GPS is doing any logging it is expected to
Expand Down

0 comments on commit 9d3fbf1

Please sign in to comment.