Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: fixes for macos CAN SITL build

Browse Source
This commit is contained in:
Andrew Tridgell 2023-08-26 14:12:08 +10:00
parent e6d43811ad
commit 74139423cb
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_GPS/AP_GPS_DroneCAN.cpp
View File

@ -321,7 +321,7 @@ AP_GPS_DroneCAN* AP_GPS_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan,
*/ */
void AP_GPS_DroneCAN::handle_velocity(const float vx, const float vy, const float vz) void AP_GPS_DroneCAN::handle_velocity(const float vx, const float vy, const float vz)
{ {
if (!isnanf(vx)) { if (!isnan(vx)) {
const Vector3f vel(vx, vy, vz); const Vector3f vel(vx, vy, vz);
interim_state.velocity = vel; interim_state.velocity = vel;
velocity_to_speed_course(interim_state); velocity_to_speed_course(interim_state);
@ -391,21 +391,21 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin
handle_velocity(msg.ned_velocity[0], msg.ned_velocity[1], msg.ned_velocity[2]); handle_velocity(msg.ned_velocity[0], msg.ned_velocity[1], msg.ned_velocity[2]);
if (msg.covariance.len == 6) { if (msg.covariance.len == 6) {
if (!isnanf(msg.covariance.data[0])) { if (!isnan(msg.covariance.data[0])) {
interim_state.horizontal_accuracy = sqrtf(msg.covariance.data[0]); interim_state.horizontal_accuracy = sqrtf(msg.covariance.data[0]);
interim_state.have_horizontal_accuracy = true; interim_state.have_horizontal_accuracy = true;
} else { } else {
interim_state.have_horizontal_accuracy = false; interim_state.have_horizontal_accuracy = false;
} }
if (!isnanf(msg.covariance.data[2])) { if (!isnan(msg.covariance.data[2])) {
interim_state.vertical_accuracy = sqrtf(msg.covariance.data[2]); interim_state.vertical_accuracy = sqrtf(msg.covariance.data[2]);
interim_state.have_vertical_accuracy = true; interim_state.have_vertical_accuracy = true;
} else { } else {
interim_state.have_vertical_accuracy = false; interim_state.have_vertical_accuracy = false;
} }
if (!isnanf(msg.covariance.data[3]) && if (!isnan(msg.covariance.data[3]) &&
!isnanf(msg.covariance.data[4]) && !isnan(msg.covariance.data[4]) &&
!isnanf(msg.covariance.data[5])) { !isnan(msg.covariance.data[5])) {
interim_state.speed_accuracy = sqrtf((msg.covariance.data[3] + msg.covariance.data[4] + msg.covariance.data[5])/3); interim_state.speed_accuracy = sqrtf((msg.covariance.data[3] + msg.covariance.data[4] + msg.covariance.data[5])/3);
interim_state.have_speed_accuracy = true; interim_state.have_speed_accuracy = true;
} else { } else {
@ -479,12 +479,12 @@ void AP_GPS_DroneCAN::handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg)
{ {
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
if (!isnanf(msg.hdop)) { if (!isnan(msg.hdop)) {
seen_aux = true; seen_aux = true;
interim_state.hdop = msg.hdop * 100.0; interim_state.hdop = msg.hdop * 100.0;
} }
if (!isnanf(msg.vdop)) { if (!isnan(msg.vdop)) {
seen_aux = true; seen_aux = true;
interim_state.vdop = msg.vdop * 100.0; interim_state.vdop = msg.vdop * 100.0;
} }