From 65ccf39332f2b37fff7f577350bae64c0e4e63db Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Fri, 18 Apr 2025 16:19:20 +0300 Subject: [PATCH 1/5] feat: add ahrs solution subscribtion --- include/serialization/subscriber.hpp | 5 +- .../uavcan/equipment/ahrs/Solution.h | 89 +++++++++++++++++++ 2 files changed, 93 insertions(+), 1 deletion(-) diff --git a/include/serialization/subscriber.hpp b/include/serialization/subscriber.hpp index 9b053fa..67e3192 100644 --- a/include/serialization/subscriber.hpp +++ b/include/serialization/subscriber.hpp @@ -17,6 +17,7 @@ #include "uavcan/equipment/indication/LightsCommand.h" #include "uavcan/equipment/power/CircuitStatus.h" #include "uavcan/equipment/safety/ArmingStatus.h" +#include "uavcan/equipment/ahrs/Solution.h" #include "uavcan/equipment/hardpoint/Command.h" template @@ -51,7 +52,9 @@ DEFINE_SUBSCRIBER_TRAITS(SafetyArmingStatus, DEFINE_SUBSCRIBER_TRAITS(HardpointCommand, uavcanSubscribeHardpointCommand, dronecan_equipment_hardpoint_command_deserialize) - +DEFINE_SUBSCRIBER_TRAITS(AhrsSolution_t, + uavcanSubscribeAhrsSolution, + dronecan_equipment_ahrs_solution_deserialize) template class DronecanSubscriber { diff --git a/include/serialization/uavcan/equipment/ahrs/Solution.h b/include/serialization/uavcan/equipment/ahrs/Solution.h index be01ae7..03f5f71 100644 --- a/include/serialization/uavcan/equipment/ahrs/Solution.h +++ b/include/serialization/uavcan/equipment/ahrs/Solution.h @@ -15,6 +15,7 @@ #define UAVCAN_EQUIPMENT_AHRS_SOLUTION_SIGNATURE 0x72a63a3c6f41fa9b #define UAVCAN_EQUIPMENT_AHRS_SOLUTION_MESSAGE_SIZE 29 // 668 bits #define UAVCAN_EQUIPMENT_AHRS_SOLUTION UAVCAN_EXPAND(UAVCAN_EQUIPMENT_AHRS_SOLUTION) +#define UAVCAN_EQUIPMENT_AHRS_SOLUTION UAVCAN_EXPAND(UAVCAN_EQUIPMENT_AHRS_SOLUTION) /** * @brief uavcan.equipment.ahrs.Solution @@ -40,6 +41,94 @@ extern "C" { #endif +static inline int8_t dronecan_equipment_ahrs_solution_deserialize( + const CanardRxTransfer* transfer, AhrsSolution_t* obj) { + if ((transfer == NULL) || (obj == NULL)) { + return -2; + } + size_t offset = 0; + canardDecodeScalar(transfer, 0, 56, false, &obj->timestamp); + offset += 56; + uint16_t f16_dummy; + + for (uint_fast8_t idx = 0; idx < 4; idx++) { + canardDecodeScalar(transfer, offset, 16, true, &f16_dummy); + offset += 16; + obj->orientation_xyzw[idx] = canardConvertFloat16ToNativeFloat(f16_dummy); + } + for (uint_fast8_t idx = 0; idx < 3; idx++) { + canardDecodeScalar(transfer, offset, 16, true, &f16_dummy); + offset += 16; + obj->angular_velocity[idx] = canardConvertFloat16ToNativeFloat(f16_dummy); + } + + for (uint_fast8_t idx = 0; idx < 3; idx++) { + canardDecodeScalar(transfer, offset, 16, true, &f16_dummy); + offset += 16; + obj->linear_acceleration[idx] = canardConvertFloat16ToNativeFloat(f16_dummy); + } + + return 0; +} + + +static inline int8_t dronecan_equipment_ahrs_solution_serialize( + const AhrsSolution_t* 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_AHRS_SOLUTION_MESSAGE_SIZE) { + return -3; + } + + size_t offset = 0; + canardEncodeScalar(buffer, 0, 56, &obj->timestamp); + offset += 56; + + + for (uint_fast8_t idx = 0; idx < 3; idx++) { + canardEncodeFloat16(buffer, offset, obj->orientation_xyzw[idx]); + offset += 16; + } + for (uint_fast8_t idx = 0; idx < 2; idx++) { + canardEncodeFloat16(buffer, offset, obj->angular_velocity[idx]); + offset += 16; + } + + for (uint_fast8_t idx = 0; idx < 2; idx++) { + canardEncodeFloat16(buffer, offset, obj->linear_acceleration[idx]); + offset += 16; + } + + return 0; +} + +static inline int8_t dronecan_equipment_ahrs_raw_imu_publish( + const AhrsSolution_t* const obj, + uint8_t* inout_transfer_id) +{ + uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_SOLUTION_MESSAGE_SIZE]; + size_t inout_buffer_size = UAVCAN_EQUIPMENT_AHRS_SOLUTION_MESSAGE_SIZE; + dronecan_equipment_ahrs_solution_serialize(obj, buffer, &inout_buffer_size); + uavcanPublish(UAVCAN_EQUIPMENT_AHRS_SOLUTION_SIGNATURE, + UAVCAN_EQUIPMENT_AHRS_SOLUTION_ID, + inout_transfer_id, + CANARD_TRANSFER_PRIORITY_MEDIUM, + buffer, + UAVCAN_EQUIPMENT_AHRS_SOLUTION_MESSAGE_SIZE); + + return 0; +} + + +static inline int8_t uavcanSubscribeAhrsSolution(void (*transfer_callback)(CanardRxTransfer*)) { + return uavcanSubscribe(UAVCAN_EQUIPMENT_AHRS_SOLUTION, transfer_callback); +} #ifdef __cplusplus } From 35b7baf03a71966052ed01cd4bdf27e707cc1ef0 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Fri, 18 Apr 2025 16:20:43 +0300 Subject: [PATCH 2/5] refactor: fix casting of CanProtocol --- include/application/can_driver.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/application/can_driver.h b/include/application/can_driver.h index 8ea185b..16eadc7 100644 --- a/include/application/can_driver.h +++ b/include/application/can_driver.h @@ -42,10 +42,10 @@ inline CanProtocol canDriverGetProtocol(uint8_t can_driver_idx) { if (canDriverReceive(&rx_frame, can_driver_idx) == 0) { const uint8_t tail_byte = rx_frame.data[rx_frame.data_len - 1]; if (IS_START_OF_TRANSFER(tail_byte) && IS_END_OF_TRANSFER(tail_byte)) { - return TOGGLE_BIT(tail_byte); + return TOGGLE_BIT(tail_byte) ? CAN_PROTOCOL_CYPHAL: CAN_PROTOCOL_DRONECAN; } } - return -1; + return CAN_PROTOCOL_UNKNOWN; } uint64_t canDriverGetRxOverflowCount(); From 6016535c7215dca04a278b60133aabb9612f5fd9 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Fri, 18 Apr 2025 16:21:04 +0300 Subject: [PATCH 3/5] refactor: fix mail --- include/serialization/uavcan/equipment/hardpoint/Command.h | 2 +- include/serialization/uavcan/equipment/hardpoint/Status.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/serialization/uavcan/equipment/hardpoint/Command.h b/include/serialization/uavcan/equipment/hardpoint/Command.h index 15e05cc..d5f8ce5 100644 --- a/include/serialization/uavcan/equipment/hardpoint/Command.h +++ b/include/serialization/uavcan/equipment/hardpoint/Command.h @@ -1,5 +1,5 @@ /*** - * Copyright (C) 2024 Anastasiia Stepanova + * Copyright (C) 2024 Anastasiia Stepanova * Distributed under the terms of the GPL v3 license, available in the file *LICENSE. ***/ diff --git a/include/serialization/uavcan/equipment/hardpoint/Status.h b/include/serialization/uavcan/equipment/hardpoint/Status.h index dd6a5d6..8fda326 100644 --- a/include/serialization/uavcan/equipment/hardpoint/Status.h +++ b/include/serialization/uavcan/equipment/hardpoint/Status.h @@ -1,5 +1,5 @@ /*** - * Copyright (C) 2024 Anastasiia Stepanova + * Copyright (C) 2024 Anastasiia Stepanova * Distributed under the terms of the GPL v3 license, available in the file LICENSE. ***/ From 20068478a03f4fdf6593c32e429c9ae247ab8ea9 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Mon, 28 Apr 2025 12:49:50 +0300 Subject: [PATCH 4/5] fix: renamed publish ahrs solution function --- include/serialization/uavcan/equipment/ahrs/Solution.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/serialization/uavcan/equipment/ahrs/Solution.h b/include/serialization/uavcan/equipment/ahrs/Solution.h index 03f5f71..6b2f92c 100644 --- a/include/serialization/uavcan/equipment/ahrs/Solution.h +++ b/include/serialization/uavcan/equipment/ahrs/Solution.h @@ -108,7 +108,7 @@ static inline int8_t dronecan_equipment_ahrs_solution_serialize( return 0; } -static inline int8_t dronecan_equipment_ahrs_raw_imu_publish( +static inline int8_t dronecan_equipment_ahrs_solution_publish( const AhrsSolution_t* const obj, uint8_t* inout_transfer_id) { From 8b431f75dd43343ef145b530af4c19ad8b636876 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Mon, 28 Apr 2025 12:52:14 +0300 Subject: [PATCH 5/5] add ahrs solution publisher --- include/serialization/publisher.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/serialization/publisher.hpp b/include/serialization/publisher.hpp index 0ce9afd..0f823cf 100644 --- a/include/serialization/publisher.hpp +++ b/include/serialization/publisher.hpp @@ -14,6 +14,7 @@ #include "uavcan/equipment/actuator/Status.h" #include "uavcan/equipment/ahrs/MagneticFieldStrength2.h" #include "uavcan/equipment/ahrs/RawImu.h" +#include "uavcan/equipment/ahrs/Solution.h" #include "uavcan/equipment/air_data/IndicatedAirspeed.h" #include "uavcan/equipment/air_data/RawAirData.h" #include "uavcan/equipment/air_data/StaticPressure.h" @@ -41,6 +42,7 @@ struct DronecanPublisherTraits { \ DEFINE_PUBLISHER_TRAITS(ActuatorStatus_t, dronecan_equipment_actuator_status_publish) DEFINE_PUBLISHER_TRAITS(MagneticFieldStrength2, dronecan_equipment_ahrs_magnetic_field_2_publish) DEFINE_PUBLISHER_TRAITS(AhrsRawImu, dronecan_equipment_ahrs_raw_imu_publish) +DEFINE_PUBLISHER_TRAITS(AhrsSolution_t, dronecan_equipment_ahrs_solution_publish) DEFINE_PUBLISHER_TRAITS(IndicatedAirspeed, dronecan_equipment_air_data_indicated_airspeed_publish) DEFINE_PUBLISHER_TRAITS(RawAirData_t, dronecan_equipment_air_data_raw_air_data_publish) DEFINE_PUBLISHER_TRAITS(StaticPressure, dronecan_equipment_air_data_static_pressure_publish)