Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: Send ardupilot.gnss.Status

Browse Source
This commit is contained in:
Michael du Breuil 2021-02-23 13:17:51 -07:00 committed by Andrew Tridgell
parent a3ddf5264d
commit 06a40da355
1 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
Tools/AP_Periph/can.cpp
View File

@ -42,6 +42,7 @@
#include <ardupilot/indication/SafetyState.h>
#include <ardupilot/indication/Button.h>
#include <ardupilot/equipment/trafficmonitor/TrafficReport.h>
#include <ardupilot/gnss/Status.h>
#include <uavcan/equipment/gnss/RTCMStream.h>
#include <uavcan/equipment/power/BatteryInfo.h>
#include <uavcan/protocol/debug/LogMessage.h>
@ -1708,6 +1709,36 @@ void AP_Periph_FW::can_gps_update(void)
&buffer[0],
total_size);
}
// send the gnss status packet
{
ardupilot_gnss_Status status {};
status.healthy = gps.is_healthy();
if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) {
status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING;
}
uint8_t idx; // unused
if (status.healthy && !gps.first_unconfigured_gps(idx)) {
status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE;
}
uint32_t error_codes;
if (gps.get_error_codes(error_codes)) {
status.error_codes = error_codes;
}
uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE] {};
const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer);
canardBroadcast(&canard,
ARDUPILOT_GNSS_STATUS_SIGNATURE,
ARDUPILOT_GNSS_STATUS_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
#endif // HAL_PERIPH_ENABLE_GPS
}