Pr ahrs subscription

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();

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<MessageType> { \
 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)

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 <typename MessageType>
@@ -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 <typename MessageType>
 class DronecanSubscriber {

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_solution_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
 }

include/serialization/uavcan/equipment/hardpoint/Command.h

@@ -1,5 +1,5 @@
 /***
- * Copyright (C) 2024 Anastasiia Stepanova  <asiiapine96@gmail.com>
+ * Copyright (C) 2024 Anastasiia Stepanova  <asiiapine@gmail.com>
  *  Distributed under the terms of the GPL v3 license, available in the file
  *LICENSE.
  ***/

include/serialization/uavcan/equipment/hardpoint/Status.h

@@ -1,5 +1,5 @@
 /***
- * Copyright (C) 2024 Anastasiia Stepanova  <asiiapine96@gmail.com>
+ * Copyright (C) 2024 Anastasiia Stepanova  <asiiapine@gmail.com>
  *  Distributed under the terms of the GPL v3 license, available in the file LICENSE.
 ***/