From 65ccf39332f2b37fff7f577350bae64c0e4e63db Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Fri, 18 Apr 2025 16:19:20 +0300 Subject: [PATCH 1/9] 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/9] 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/9] 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/9] 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/9] 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) From 7abad5e0e2af595a5f42397447693a8365d24d33 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Mon, 12 May 2025 20:45:44 +0300 Subject: [PATCH 6/9] add covariance len usage to fit in the protocol --- .../uavcan/equipment/ahrs/Solution.h | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/include/serialization/uavcan/equipment/ahrs/Solution.h b/include/serialization/uavcan/equipment/ahrs/Solution.h index 6b2f92c..8bdceda 100644 --- a/include/serialization/uavcan/equipment/ahrs/Solution.h +++ b/include/serialization/uavcan/equipment/ahrs/Solution.h @@ -56,12 +56,23 @@ static inline int8_t dronecan_equipment_ahrs_solution_deserialize( offset += 16; obj->orientation_xyzw[idx] = canardConvertFloat16ToNativeFloat(f16_dummy); } + + offset+= 4; // reserved void4 + + uint8_t covariance_len; + canardDecodeScalar(transfer, offset, 4, true, &covariance_len); + offset += 16 * covariance_len; + 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); } + offset+= 4; // reserved void4 + canardDecodeScalar(transfer, offset, 4, true, &covariance_len); + offset += 16 * covariance_len; + for (uint_fast8_t idx = 0; idx < 3; idx++) { canardDecodeScalar(transfer, offset, 16, true, &f16_dummy); offset += 16; @@ -90,16 +101,24 @@ static inline int8_t dronecan_equipment_ahrs_solution_serialize( 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; } + + offset += 4; + canardEncodeScalar(buffer, offset, 4, 0); + offset += 4; // covariance len + for (uint_fast8_t idx = 0; idx < 2; idx++) { canardEncodeFloat16(buffer, offset, obj->angular_velocity[idx]); offset += 16; } + offset += 4; // void4 + canardEncodeScalar(buffer, offset, 4, 0); + offset += 4; // covariance len + for (uint_fast8_t idx = 0; idx < 2; idx++) { canardEncodeFloat16(buffer, offset, obj->linear_acceleration[idx]); offset += 16; From 77081f48415327d1c61b4e979b619e38a105f09f Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Mon, 12 May 2025 20:47:32 +0300 Subject: [PATCH 7/9] fix hardpoint message size --- include/serialization/uavcan/equipment/hardpoint/Status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/serialization/uavcan/equipment/hardpoint/Status.h b/include/serialization/uavcan/equipment/hardpoint/Status.h index 8fda326..b03842d 100644 --- a/include/serialization/uavcan/equipment/hardpoint/Status.h +++ b/include/serialization/uavcan/equipment/hardpoint/Status.h @@ -11,7 +11,7 @@ #define UAVCAN_EQUIPMENT_HARDPOINT_STATUS_ID 1071 #define UAVCAN_EQUIPMENT_HARDPOINT_STATUS_SIGNATURE 0x624a519d42553d82 -#define UAVCAN_EQUIPMENT_HARDPOINT_STATUS_MESSAGE_SIZE 8 +#define UAVCAN_EQUIPMENT_HARDPOINT_STATUS_MESSAGE_SIZE 7 typedef struct { uint8_t hardpoint_id; From db6c51876aebcac0424b4ec8ce51c9b79ee5d383 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Tue, 13 May 2025 09:57:33 +0300 Subject: [PATCH 8/9] add covariance len offset --- include/serialization/uavcan/equipment/ahrs/Solution.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/serialization/uavcan/equipment/ahrs/Solution.h b/include/serialization/uavcan/equipment/ahrs/Solution.h index 8bdceda..f0220fc 100644 --- a/include/serialization/uavcan/equipment/ahrs/Solution.h +++ b/include/serialization/uavcan/equipment/ahrs/Solution.h @@ -61,6 +61,7 @@ static inline int8_t dronecan_equipment_ahrs_solution_deserialize( uint8_t covariance_len; canardDecodeScalar(transfer, offset, 4, true, &covariance_len); + offset += 4; offset += 16 * covariance_len; for (uint_fast8_t idx = 0; idx < 3; idx++) { @@ -71,6 +72,8 @@ static inline int8_t dronecan_equipment_ahrs_solution_deserialize( offset+= 4; // reserved void4 canardDecodeScalar(transfer, offset, 4, true, &covariance_len); + offset += 4; + offset += 16 * covariance_len; for (uint_fast8_t idx = 0; idx < 3; idx++) { From 363da1845b4acb234ff184338631ecfb6715ffa4 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Tue, 13 May 2025 10:28:52 +0300 Subject: [PATCH 9/9] fix serialization --- include/serialization/uavcan/equipment/hardpoint/Status.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/include/serialization/uavcan/equipment/hardpoint/Status.h b/include/serialization/uavcan/equipment/hardpoint/Status.h index b03842d..96c4fd5 100644 --- a/include/serialization/uavcan/equipment/hardpoint/Status.h +++ b/include/serialization/uavcan/equipment/hardpoint/Status.h @@ -53,9 +53,8 @@ static inline int8_t dronecan_equipment_hardpoint_status_serialize( canardEncodeScalar(buffer, 0, 8, &obj->hardpoint_id); canardEncodeFloat16(buffer, 8, obj->payload_weight); - canardEncodeFloat16(buffer, 24, obj->payload_weight); - canardEncodeFloat16(buffer, 40, obj->payload_weight_variance); - canardEncodeScalar(buffer, 56, 16, &obj->status); + canardEncodeFloat16(buffer, 24, obj->payload_weight_variance); + canardEncodeScalar(buffer, 40, 16, &obj->status); return 0; }