aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Periph: removed the old DroneCAN Fix message

Browse Source 
only send/process Fix2 message, saving bus bandwidth and flash space
This commit is contained in:
Andrew Tridgell 2022-09-27 14:53:55 +10:00
parent bf89a39621
commit aa757501f4
1 changed files with 0 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
Tools/AP_Periph/can.cpp
View File

@ -1877,82 +1877,6 @@ void AP_Periph_FW::can_gps_update(void)
}
last_gps_update_ms = gps.last_message_time_ms();
{
/*
send Fix packet
*/
uavcan_equipment_gnss_Fix pkt {};
const Location &loc = gps.location();
const Vector3f &vel = gps.velocity();
pkt.timestamp.usec = AP_HAL::native_micros64();
pkt.gnss_timestamp.usec = gps.time_epoch_usec();
if (pkt.gnss_timestamp.usec == 0) {
pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_NONE;
} else {
pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_UTC;
}
pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL;
pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL;
pkt.height_msl_mm = loc.alt * 10;
pkt.height_ellipsoid_mm = loc.alt * 10;
float undulation;
if (gps.get_undulation(undulation)) {
pkt.height_ellipsoid_mm -= undulation*1000;
}
for (uint8_t i=0; i<3; i++) {
// the canard dsdl compiler doesn't understand float16
pkt.ned_velocity[i] = vel[i];
}
pkt.sats_used = gps.num_sats();
switch (gps.status()) {
case AP_GPS::GPS_Status::NO_GPS:
case AP_GPS::GPS_Status::NO_FIX:
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX_STATUS_NO_FIX;
break;
case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX_STATUS_2D_FIX;
break;
case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX_STATUS_3D_FIX;
break;
}
pkt.position_covariance.len = 9;
float vacc;
if (gps.vertical_accuracy(vacc)) {
pkt.position_covariance.data[8] = sq(vacc);
}
float hacc;
if (gps.horizontal_accuracy(hacc)) {
pkt.position_covariance.data[0] = pkt.position_covariance.data[4] = sq(hacc);
}
check_float16_range(pkt.position_covariance.data, pkt.position_covariance.len);
pkt.velocity_covariance.len = 9;
float sacc;
if (gps.speed_accuracy(sacc)) {
float vc3 = sq(sacc);
pkt.velocity_covariance.data[0] = pkt.velocity_covariance.data[4] = pkt.velocity_covariance.data[8] = vc3;
}
check_float16_range(pkt.velocity_covariance.data, pkt.velocity_covariance.len);
uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX_MAX_SIZE] {};
uint16_t total_size = uavcan_equipment_gnss_Fix_encode(&pkt, buffer, !periph.canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE,
UAVCAN_EQUIPMENT_GNSS_FIX_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
{
/*
send Fix2 packet