Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
uavcan_reporting.c
Go to the documentation of this file.
1#include "uavcan.h"
2#include "mcu_periph/can.h"
4#include "uavcan.protocol.NodeStatus.h"
5#include "uavcan.protocol.GetNodeInfo_req.h"
6#include "uavcan.protocol.GetNodeInfo_res.h"
8#include "generated/airframe.h"
9#include "uavcan_reporting.h"
11
12#define AC_CAN_NAME_PREFIX "org.pprz."
13
16
17char ac_can_name[50] = {0};
19
20static void node_info_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer);
21static void node_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer);
22static void get_uavcan_status(struct uavcan_protocol_NodeStatus* status);
23static void get_uavcan_software_version (struct uavcan_protocol_SoftwareVersion* software_version);
24static void get_uavcan_hardware_version(struct uavcan_protocol_HardwareVersion* hardware_version);
25
26
28 size_t prefix_len = Min(strlen(AC_CAN_NAME_PREFIX), sizeof ac_can_name);
29 memcpy(ac_can_name, AC_CAN_NAME_PREFIX, prefix_len);
30 ac_can_name_len = Min(prefix_len + strlen(AIRFRAME_NAME), sizeof ac_can_name);
31 memcpy(ac_can_name+prefix_len, AIRFRAME_NAME, ac_can_name_len-prefix_len);
32
33 uavcan_bind(UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_ID, UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE, &node_info_ev, node_info_cb);
34 uavcan_bind(UAVCAN_PROTOCOL_NODESTATUS_ID, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, &node_status_ev, node_status_cb);
35}
36
41static void node_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer) {
42 if(!uavcan_get_node_id_mapping(transfer->source_node_id)) {
43 request_node_info(iface, transfer->source_node_id);
44 }
45}
46
47void uavcan_reporting(void) {
48 struct uavcan_protocol_NodeStatus report;
49 get_uavcan_status(&report);
50
51 uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE];
52 uint32_t total_size = uavcan_protocol_NodeStatus_encode(&report, buffer);
53
54#if UAVCAN_USE_CAN1
55 uavcan_broadcast(&uavcan1, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, CANARD_TRANSFER_PRIORITY_MEDIUM, buffer, total_size);
56#endif
57#if UAVCAN_USE_CAN2
58 uavcan_broadcast(&uavcan2, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, CANARD_TRANSFER_PRIORITY_MEDIUM, buffer, total_size);
59#endif
60}
61
62
63static void get_uavcan_status(struct uavcan_protocol_NodeStatus* status) {
64 status->uptime_sec = (uint32_t)get_sys_time_float();
65 status->health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
66 status->mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
67 status->sub_mode = 0;
68 status->vendor_specific_status_code = 0;
69}
70
71static void get_uavcan_software_version (struct uavcan_protocol_SoftwareVersion* software_version) {
72 software_version->major = 6;
73 software_version->minor = 0;
74 software_version->optional_field_flags = 0;
75 software_version->vcs_commit = 0;
76 software_version->image_crc = 0;
77}
78
79static void get_uavcan_hardware_version(struct uavcan_protocol_HardwareVersion* hardware_version) {
80 hardware_version->major = 2;
81 hardware_version->minor = 1;
82 memset(hardware_version->unique_id, 0, sizeof(hardware_version->unique_id));
83 hardware_version->certificate_of_authenticity.len = 0;
84}
85
86
87
88static void node_info_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer __attribute__((unused))) {
89 struct uavcan_protocol_GetNodeInfoResponse msg;
90 get_uavcan_status(&msg.status);
91 get_uavcan_software_version(&msg.software_version);
92 get_uavcan_hardware_version(&msg.hardware_version);
93 uint8_t len = Min(sizeof(msg.name.data), ac_can_name_len);
94 strncpy((char*)msg.name.data, ac_can_name, len);
95 msg.name.len = len;
96
97 uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE];
98 uint32_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&msg, buffer);
99
100 CanardTxTransfer resp_transfer;
101 canardInitTxTransfer(&resp_transfer);
102 resp_transfer.data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE;
103 resp_transfer.data_type_id = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_ID;
104 resp_transfer.inout_transfer_id = &transfer->transfer_id;
105 resp_transfer.priority = CANARD_TRANSFER_PRIORITY_MEDIUM;
106 resp_transfer.payload = buffer;
107 resp_transfer.payload_len = total_size;
108
109 uavcan_response(iface, transfer->source_node_id, &resp_transfer);
110}
111
112
static uint8_t status
#define Min(x, y)
Definition esc_dshot.c:110
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:241
void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, const void *payload, uint16_t payload_len)
Legacy function Broadcast an uavcan message to a specific interface.
Definition uavcan.c:345
void uavcan_response(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer *transfer)
Definition uavcan.c:364
void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
Bind to a receiving message from uavcan.
Definition uavcan.c:272
struct uavcan_event_t uavcan_event
Definition uavcan.h:98
static uavcan_event node_info_ev
struct uavcan_node_mapping_t * uavcan_get_node_id_mapping(const uint8_t id)
void request_node_info(struct uavcan_iface_t *iface, uint8_t destination_node_id)
char ac_can_name[50]
static void node_info_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
static void get_uavcan_hardware_version(struct uavcan_protocol_HardwareVersion *hardware_version)
void uavcan_init_reporting()
#define AC_CAN_NAME_PREFIX
static void get_uavcan_status(struct uavcan_protocol_NodeStatus *status)
void uavcan_reporting(void)
uint8_t ac_can_name_len
static void get_uavcan_software_version(struct uavcan_protocol_SoftwareVersion *software_version)
static void node_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
request uniq_id of all nodes on the bus.
static uavcan_event node_status_ev
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
int transfer(const Mat *from, const image_t *to)