diff --git a/include/serialization/publisher.hpp b/include/serialization/publisher.hpp index 51eb8b9..c4962c6 100644 --- a/include/serialization/publisher.hpp +++ b/include/serialization/publisher.hpp @@ -20,6 +20,7 @@ #include "uavcan/equipment/air_data/StaticTemperature.h" #include "uavcan/equipment/air_data/TrueAirspeed.h" #include "uavcan/equipment/esc/Status.h" +#include "uavcan/equipment/gnss/Fix2.h" #include "uavcan/equipment/hardpoint/Status.h" #include "uavcan/equipment/power/CircuitStatus.h" #include "uavcan/equipment/power/BatteryInfo.h" @@ -46,6 +47,7 @@ DEFINE_PUBLISHER_TRAITS(StaticPressure, dronecan_equipment_air_data_static_p DEFINE_PUBLISHER_TRAITS(StaticTemperature, dronecan_equipment_air_data_static_temperature_publish) DEFINE_PUBLISHER_TRAITS(TrueAirspeed, dronecan_equipment_air_data_true_airspeed_publish) DEFINE_PUBLISHER_TRAITS(EscStatus_t, dronecan_equipment_esc_status_publish) +DEFINE_PUBLISHER_TRAITS(GnssFix2, dronecan_equipment_gnss_fix2_publish) DEFINE_PUBLISHER_TRAITS(HardpointStatus, dronecan_equipment_hardpoint_status_publish) DEFINE_PUBLISHER_TRAITS(CircuitStatus_t, dronecan_equipment_circuit_status_publish) DEFINE_PUBLISHER_TRAITS(Temperature_t, dronecan_equipment_temperature_publish) diff --git a/include/serialization/uavcan/equipment/gnss/Fix2.h b/include/serialization/uavcan/equipment/gnss/Fix2.h index 24a510b..227f78a 100644 --- a/include/serialization/uavcan/equipment/gnss/Fix2.h +++ b/include/serialization/uavcan/equipment/gnss/Fix2.h @@ -48,6 +48,95 @@ typedef struct { extern "C" { #endif +static inline int8_t dronecan_equipment_gnss_fix2_serialize( + const GnssFix2* const obj, + uint8_t* const buffer, + size_t* const inout_buffer_size_bytes) +{ + if ((obj == NULL) || (buffer == NULL) || (inout_buffer_size_bytes == NULL)) { + return -2; + } + + const size_t capacity_bytes = *inout_buffer_size_bytes; + if (capacity_bytes < UAVCAN_EQUIPMENT_GNSS_FIX2_MESSAGE_SIZE) { + return -3; + } + + uint32_t offset = 0; + + canardEncodeScalar(buffer, offset, 56, &obj->timestamp); + offset += 56; + canardEncodeScalar(buffer, offset, 56, &obj->gnss_timestamp); + offset += 56; + canardEncodeScalar(buffer, offset, 3, &obj->gnss_time_standard); + offset += 3; + + // void13 # Reserved space + offset += 13; + + canardEncodeScalar(buffer, offset, 8, &obj->num_leap_seconds); + offset += 8; + + canardEncodeScalar(buffer, offset, 37, &obj->longitude_deg_1e8); + offset += 37; + canardEncodeScalar(buffer, offset, 37, &obj->latitude_deg_1e8); + offset += 37; + canardEncodeScalar(buffer, offset, 27, &obj->height_ellipsoid_mm); + offset += 27; + canardEncodeScalar(buffer, offset, 27, &obj->height_msl_mm); + offset += 27; + + canardEncodeFloat32(buffer, offset, obj->ned_velocity[0]); + offset += 32; + canardEncodeFloat32(buffer, offset, obj->ned_velocity[1]); + offset += 32; + canardEncodeFloat32(buffer, offset, obj->ned_velocity[2]); + offset += 32; + + canardEncodeScalar(buffer, offset, 6, &obj->sats_used); + offset += 6; + canardEncodeScalar(buffer, offset, 2, &obj->status); + offset += 2; + canardEncodeScalar(buffer, offset, 4, &obj->mode); + offset += 4; + canardEncodeScalar(buffer, offset, 6, &obj->sub_mode); + offset += 6; + + uint8_t covariance_len = 6; + canardEncodeScalar(buffer, offset, 6, &covariance_len); + offset += 6; + for (uint_fast8_t idx = 0; idx < 6; idx++) { + canardEncodeFloat16(buffer, offset, obj->covariance[idx]); + offset += 16; + } + + canardEncodeFloat16(buffer, offset, obj->pdop); + offset += 16; + + uint8_t ecef_len = 0; + canardEncodeScalar(buffer, offset, 1, &ecef_len); + offset += 1; + // fill ecef here + + return 0; +} + +static inline int8_t dronecan_equipment_gnss_fix2_publish( + const GnssFix2* const obj, + uint8_t* inout_transfer_id) +{ + uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MESSAGE_SIZE]; + size_t inout_buffer_size = UAVCAN_EQUIPMENT_GNSS_FIX2_MESSAGE_SIZE; + dronecan_equipment_gnss_fix2_serialize(obj, buffer, &inout_buffer_size); + uavcanPublish(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, + UAVCAN_EQUIPMENT_GNSS_FIX2_ID, + inout_transfer_id, + CANARD_TRANSFER_PRIORITY_MEDIUM, + buffer, + UAVCAN_EQUIPMENT_GNSS_FIX2_MESSAGE_SIZE); + + return 0; +} #ifdef __cplusplus }