Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
actuators_uavcan.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
27 #include "actuators_uavcan.h"
29 #include "math/pprz_random.h"
30 
31 /* By default enable the usage of the current sensing in the ESC telemetry */
32 #ifndef UAVCAN_ACTUATORS_USE_CURRENT
33 #define UAVCAN_ACTUATORS_USE_CURRENT TRUE
34 #endif
35 
36 /* uavcan ESC status telemetry structure */
38  float voltage;
39  float current;
40  float temperature;
43 };
44 
45 #ifdef SERVOS_UAVCAN1_NB
46 int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB];
47 static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1_NB];
48 #endif
49 #ifdef SERVOS_UAVCAN2_NB
50 int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB];
51 static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB];
52 #endif
53 
54 /* uavcan EQUIPMENT_ESC_STATUS message definition */
55 #define UAVCAN_EQUIPMENT_ESC_STATUS_ID 1034
56 #define UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE (0xA9AF28AEA2FBB254ULL)
57 #define UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE ((110 + 7)/8)
58 
59 /* uavcan EQUIPMENT_ESC_RAWCOMMAND message definition */
60 #define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID 1030
61 #define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE (0x217F5C87D7EC951DULL)
62 #define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE ((285 + 7)/8)
63 
64 /* private variables */
65 static bool actuators_uavcan_initialized = false;
67 
68 #if PERIODIC_TELEMETRY
70 
71 static void actuators_uavcan_send_esc(struct transport_tx *trans, struct link_device *dev)
72 {
73  static uint8_t esc_idx = 0;
74 
75  // Find the correct telemetry
76  uint8_t max_id = 0;
77  uint8_t offset = 0;
78  struct actuators_uavcan_telem_t *telem = NULL;
79 #ifdef SERVOS_UAVCAN1_NB
80  if (esc_idx >= max_id && esc_idx < max_id + SERVOS_UAVCAN1_NB) {
81  offset = max_id;
82  telem = uavcan1_telem;
83  }
84  max_id += SERVOS_UAVCAN1_NB;
85 #endif
86 #ifdef SERVOS_UAVCAN2_NB
87  if (esc_idx >= max_id && esc_idx < max_id + SERVOS_UAVCAN2_NB) {
88  offset = max_id;
89  telem = uavcan2_telem;
90  }
91  max_id += SERVOS_UAVCAN2_NB;
92 #endif
93 
94  // Safety check
95  if (telem == NULL) {
96  esc_idx = 0;
97  return;
98  }
99 
100  uint8_t i = esc_idx - offset;
101  float power = telem[i].current * telem[i].voltage;
102  float rpm = telem[i].rpm;
103  float energy = telem[i].energy;
104  pprz_msg_send_ESC(trans, dev, AC_ID, &telem[i].current, &electrical.vsupply, &power,
105  &rpm, &telem[i].voltage, &energy, &esc_idx);
106 
107  // Randomness added for multiple transport devices
108  if (rand_uniform() > 0.05) {
109  esc_idx++;
110  }
111 
112  if (esc_idx >= max_id) {
113  esc_idx = 0;
114  }
115 }
116 #endif
117 
121 static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
122 {
123  uint8_t esc_idx;
124  uint16_t tmp_float;
125 
126  struct actuators_uavcan_telem_t *telem = NULL;
127  uint8_t max_id = 0;
128 #ifdef SERVOS_UAVCAN1_NB
129  if (iface == &uavcan1) {
130  telem = uavcan1_telem;
131  max_id = SERVOS_UAVCAN1_NB;
132  }
133 #endif
134 #ifdef SERVOS_UAVCAN2_NB
135  if (iface == &uavcan2) {
136  telem = uavcan2_telem;
137  max_id = SERVOS_UAVCAN2_NB;
138  }
139 #endif
140 
141  canardDecodeScalar(transfer, 105, 5, false, (void *)&esc_idx);
142  //Could not find the right interface
143  if (esc_idx > max_id || telem == NULL || max_id == 0) {
144  return;
145  }
146  canardDecodeScalar(transfer, 0, 32, false, (void *)&telem[esc_idx].energy);
147  canardDecodeScalar(transfer, 32, 16, true, (void *)&tmp_float);
148  telem[esc_idx].voltage = canardConvertFloat16ToNativeFloat(tmp_float);
149  canardDecodeScalar(transfer, 48, 16, true, (void *)&tmp_float);
150  telem[esc_idx].current = canardConvertFloat16ToNativeFloat(tmp_float);
151  canardDecodeScalar(transfer, 64, 16, true, (void *)&tmp_float);
152  telem[esc_idx].temperature = canardConvertFloat16ToNativeFloat(tmp_float);
153  canardDecodeScalar(transfer, 80, 18, true, (void *)&telem[esc_idx].rpm);
154 
155 #ifdef UAVCAN_ACTUATORS_USE_CURRENT
156  // Update total current
157  electrical.current = 0;
158 #ifdef SERVOS_UAVCAN1_NB
159  for (uint8_t i = 0; i < SERVOS_UAVCAN1_NB; ++i) {
160  electrical.current += uavcan1_telem[i].current;
161  }
162 #endif
163 #ifdef SERVOS_UAVCAN2_NB
164  for (uint8_t i = 0; i < SERVOS_UAVCAN2_NB; ++i) {
165  electrical.current += uavcan2_telem[i].current;
166  }
167 #endif
168 #endif
169 }
170 
174 void actuators_uavcan_init(struct uavcan_iface_t *iface __attribute__((unused)))
175 {
176  // Check if not already initialized (for multiple interfaces, needs only 1)
177  if (actuators_uavcan_initialized) { return; }
178 
179  // Bind uavcan ESC_STATUS message from EQUIPMENT
182 
183  // Configure telemetry
184 #if PERIODIC_TELEMETRY
186 #endif
187 
188  // Set initialization
190 
191  // Initialize Random (for telemetry)
192  init_random();
193 }
194 
198 void actuators_uavcan_commit(struct uavcan_iface_t *iface, int16_t *values, uint8_t nb)
199 {
201  uint32_t offset = 0;
202 
203  // Encode the values as 14-bit signed integers
204  for (uint8_t i = 0; i < nb; i++) {
205  canardEncodeScalar(buffer, offset, 14, (void *)&values[i]);
206  offset += 14;
207  }
208 
209  // Broadcast the raw command message on the interface
211  CANARD_TRANSFER_PRIORITY_HIGH, buffer, (offset + 7) / 8);
212 }
actuators_uavcan2_values
int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB]
Stub file needed per interface because of generator.
electrical.h
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
transfer
int transfer(const Mat *from, const image_t *to)
Definition: wedgebug_opencv.cpp:40
rand_uniform
double rand_uniform(void)
Definition: pprz_random.c:45
actuators_uavcan1_values
int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB]
Stub file needed per uavcan interface because of generator.
actuators_uavcan_commit
void actuators_uavcan_commit(struct uavcan_iface_t *iface, int16_t *values, uint8_t nb)
Commit actuator values to the uavcan interface.
Definition: actuators_uavcan.c:198
uavcan_broadcast
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)
Broadcast an uavcan message to a specific interface.
Definition: uavcan.c:427
UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE
#define UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE
Definition: actuators_uavcan.c:56
actuators_uavcan_telem_t::current
float current
Definition: actuators_uavcan.c:39
UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE
#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE
Definition: actuators_uavcan.c:61
actuators_uavcan.h
actuators_uavcan_telem_t
Definition: actuators_uavcan.c:37
uavcan_event_t
Main uavcan event structure for registering/calling callbacks.
Definition: uavcan.h:60
actuators_uavcan_send_esc
static void actuators_uavcan_send_esc(struct transport_tx *trans, struct link_device *dev)
Definition: actuators_uavcan.c:71
Electrical::vsupply
float vsupply
supply voltage in V
Definition: electrical.h:45
rpm
uint16_t rpm
Definition: rpm_sensor.c:33
actuators_uavcan_telem_t::rpm
int32_t rpm
Definition: actuators_uavcan.c:41
telemetry.h
int16_t
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE
#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE
Definition: actuators_uavcan.c:62
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
UAVCAN_EQUIPMENT_ESC_STATUS_ID
#define UAVCAN_EQUIPMENT_ESC_STATUS_ID
Definition: actuators_uavcan.c:55
esc_status_ev
static uavcan_event esc_status_ev
Definition: actuators_uavcan.c:66
uavcan_bind
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:412
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
pprz_random.h
actuators_uavcan_telem_t::voltage
float voltage
Definition: actuators_uavcan.c:38
uavcan_iface_t
uavcan interface structure
Definition: uavcan.h:34
actuators_uavcan_esc_status_cb
static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Whevener an ESC_STATUS message from the EQUIPMENT group is received.
Definition: actuators_uavcan.c:121
actuators_uavcan_init
void actuators_uavcan_init(struct uavcan_iface_t *iface)
Initialize an uavcan interface.
Definition: actuators_uavcan.c:174
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
offset
static const float offset[]
Definition: dw1000_arduino.c:199
UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID
#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID
Definition: actuators_uavcan.c:60
actuators_uavcan_telem_t::temperature
float temperature
Definition: actuators_uavcan.c:40
init_random
void init_random(void)
Definition: pprz_random.c:35
electrical
struct Electrical electrical
Definition: electrical.c:66
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
actuators_uavcan_telem_t::energy
uint32_t energy
Definition: actuators_uavcan.c:42
actuators_uavcan_initialized
static bool actuators_uavcan_initialized
Definition: actuators_uavcan.c:65
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
Electrical::current
float current
current in A
Definition: electrical.h:46