Skip to content

populate Fix2 with heading fields used by PX4, to allow MB on PX4 #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
oystub wants to merge 1 commit into
base: release
Choose a base branch
from
Open

populate Fix2 with heading fields used by PX4, to allow MB on PX4 #2

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 17 additions & 0 deletions AP_Periph/sensor_gps.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,23 @@ void AP_Periph_DroneCAN::can_gps_update(void)
float vc3 = sq(sacc);
pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3;
}

#if defined(HAL_PERIPH_ENABLE_GPS) && GPS_MOVING_BASELINE
// PX4 relies on the ecef_position_velocity field for heading, heading offset and heading accuracy,
// since there is no support for RelPosHeading as of 1.14 stable
float yaw;
float yaw_accuracy;
uint32_t curr_timestamp;
static float last_timestamp = 0;

if (gps.gps_yaw_deg(yaw, yaw_accuracy, curr_timestamp) && curr_timestamp != last_timestamp){
pkt.ecef_position_velocity.len = 1;
pkt.ecef_position_velocity.data[0].velocity_xyz[0] = wrap_PI(yaw * DEG_TO_RAD);
pkt.ecef_position_velocity.data[0].velocity_xyz[1] = 0; // Offset already compensated for.
pkt.ecef_position_velocity.data[0].velocity_xyz[2] = yaw_accuracy * DEG_TO_RAD;
}
last_timestamp = curr_timestamp;
#endif

fix2_pub.broadcast(pkt);
}
Expand Down