Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
actuators_esc32.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 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  */
21 
26 #include "subsystems/actuators.h"
28 #include "mcu_periph/sys_time.h"
29 #include "mcu_periph/can.h"
30 #include "autopilot.h"
31 #include <libopencm3/stm32/can.h>
32 #include <limits.h>
33 
35 
36 #define ACTUATORS_ESC32_START_DELAY 500
38  {660, 75},
39  {0, 100},
40  {660, 75},
41  {0, 200},
42  {660, 75},
43  {0, 200},
44  {770, 75},
45  {0, 66},
46  {660, 75},
47  {0, 200},
48  {510, 75},
49  {0, 366},
50  {1120, 75},
51  {0, 500},
52 };
54  sizeof(actuators_esc32_melody[0]);
55 
57 static uint8_t actuators_esc32_send(uint32_t id, uint8_t tid, uint8_t length, uint8_t *data);
58 static inline void actuators_esc32_beep(uint32_t tt, uint8_t tid, uint16_t freq, uint16_t dur);
59 static inline void actuators_esc32_arm(uint32_t tt, uint8_t tid);
60 static inline void actuators_esc32_disarm(uint32_t tt, uint8_t tid);
61 static inline void actuators_esc32_start(uint32_t tt, uint8_t tid);
62 static inline void actuators_esc32_duty(uint32_t tt, uint8_t tid, uint16_t *cmds);
63 static inline void actuators_esc32_dir(uint32_t tt, uint8_t tid);
64 static bool actuators_esc32_play_melody(uint32_t tt, uint8_t tid, uint32_t *status_sub,
65  uint16_t melody[][2], uint8_t length);
66 
68 static void actuators_esc32_can_rx_cb(uint32_t id, uint8_t *data, int len);
69 static inline void actuators_esc32_grant_idx(uint8_t *data);
70 static inline void actuators_esc32_proc_telem(uint8_t *data);
71 
74 {
75 #ifdef ACTUAOTRS_ESC32_RPM
76 #else
77  // In the airframe file the ESC's duty cycle is defined as 10*precentage
78  actuators_esc32.cmds[actuators_esc32.escs_sorted[i]] = v * (65535 / 1000);
79 #endif
80 }
81 
84 {
87  return;
88  }
89 
90  // Switch the command
91  switch (i) {
92  case ESC32_CONFIG_CMD_BEEP: {
94  600, 500);
95  break;
96  }
97 
98  case ESC32_CONFIG_CMD_TURN: {
99  uint16_t cmds[4] = {10000, 10000, 10000, 10000};
102  cmds);
107  break;
108  }
109 
110  case ESC32_CONFIG_CMD_DIR: {
112  //actuators_esc32_beep(ESC32_CAN_TT_NODE, actuators_esc32.escs_sorted[actuators_esc32.config_idx]+1, 300, 200);
113  break;
114  }
115 
116  default:
117  break;
118  }
119 }
120 
123 {
124  int i;
125  // Set default values
128 
129  // Convert the melody
130  for (i = 1; i < actuators_esc32_melody_size; i++) {
132  }
133 
134  // Set everyone to free
135  for (i = 0; i < SERVOS_ESC32_NB; i++) {
137  }
138 
139  // Initialize the the can bus
141 
142  // Reset all devices on the bus
144 }
145 
148 {
149  uint8_t i;
150 
151  // Go trough the groups of ESCs
152  for (i = 1; i <= (SERVOS_ESC32_NB - 1) / 4 + 1; i++) {
153 
154  switch (actuators_esc32.status) {
155  // When the ESCs are running
158 
159  if (!autopilot_get_motors_on()) {
162  }
163  break;
164 
165  // When the ESCs are unarmed
168 
169  if (autopilot_get_motors_on()) {
173  }
174  break;
175 
176  // When the ESC's are playing a melody
177  case ESC32_STATUS_MELODY:
181  }
182  break;
183 
184  default:
185  break;
186  }
187  }
188 }
189 
190 
193 {
195  actuators_esc32.responses[ret_idx].fid = 0;
196 
197  ppz_can_transmit((id >> 3) | ((tid & 0x1f) << 6) | actuators_esc32.can_seq_idx, data, length);
199  return ret_idx;
200 }
201 
203 static inline void actuators_esc32_beep(uint32_t tt, uint8_t tid, uint16_t freq, uint16_t dur)
204 {
205  uint16_t data[2];
206  data[0] = freq;
207  data[1] = dur;
209  tid, 4, (uint8_t *)&data);
210 }
211 
213 static inline void actuators_esc32_arm(uint32_t tt, uint8_t tid)
214 {
216  0, 0);
217 }
218 
220 static inline void actuators_esc32_disarm(uint32_t tt, uint8_t tid)
221 {
223  tid, 0, 0);
224 }
225 
227 static inline void actuators_esc32_start(uint32_t tt, uint8_t tid)
228 {
230  tid, 0, 0);
231 }
232 
234 static inline void actuators_esc32_duty(uint32_t tt, uint8_t tid, uint16_t *cmds)
235 {
237  19), tid, 8, (uint8_t *)cmds);
238 }
239 
241 static inline void actuators_esc32_dir(uint32_t tt, uint8_t tid)
242 {
243  char *name = "DIRECTION";
244 
245  // Request the param id
247  (ESC32_CAN_DATA_PARAM_NAME1 << 19), tid, 0, 0);
249  (ESC32_CAN_DATA_PARAM_NAME2 << 19), tid, 8, (uint8_t *)&name[8]);
250 
251  // Busy waiting really bad (FIXME) TIMEOUT
252  while (actuators_esc32.responses[retId].fid == 0);
253 
254  // Request the parameter value
255  uint16_t *paramId = (uint16_t *) actuators_esc32.responses[retId].data;
257  (ESC32_CAN_DATA_PARAM_ID << 19), tid, 2, actuators_esc32.responses[retId].data);
258 
259  // Busy waiting really bad (FIXME) TIMEOUT
260  while (actuators_esc32.responses[retId].fid == 0);
261 
262  // Invert the direction
263  float *value = ((float *) actuators_esc32.responses[retId].data);
264  *value = *value * -1;
265 
266  // Send the new direction
267  uint32_t data[2];
268  data[0] = *paramId;
269  data[1] = (uint32_t) * value;
271  (ESC32_CAN_DATA_PARAM_ID << 19), tid, 8, (uint8_t *)&data);
272 }
273 
275 static bool actuators_esc32_play_melody(uint32_t tt, uint8_t tid, uint32_t *status_sub,
276  uint16_t melody[][2], uint8_t length)
277 {
278  uint32_t timer = (*status_sub & 0x00FFFFFF) << 8;
279  uint8_t counter = (*status_sub & 0xFF000000) >> 24;
280  uint32_t timeout = counter == 0 ? ACTUATORS_ESC32_START_DELAY * 1000 :
281  (melody[counter - 1][1] + ACTUATORS_ESC32_START_DELAY) * 1000;
282 
283  if (*status_sub == 0) {
284  SysTimeTimerStart(*status_sub);
285  *status_sub = *status_sub >> 8;
286  } else if (counter < length && SysTimeTimer(timer) > timeout) { // Start delay
287  if (melody[counter][0] != 0) {
288  actuators_esc32_beep(tt, tid, melody[counter][0],
289  melody[counter][1] - SysTimeTimer(timer) / 1000 + ACTUATORS_ESC32_START_DELAY);
290  }
291  *status_sub = *status_sub + (1 << 24);
292  } else if (counter == length && SysTimeTimer(timer) > timeout) {
293  return true;
294  }
295 
296  return false;
297 }
298 
300 void actuators_esc32_can_rx_cb(uint32_t id, uint8_t *data, int len __attribute__((unused)))
301 {
302  id = id << 3;
303  //uint8_t doc = (id & ESC32_CAN_DOC_MASK)>>19;
304  //uint8_t sid = (id & ESC32_CAN_SID_MASK)>>14;
305  uint8_t seqId = (id & ESC32_CAN_SEQ_MASK) >> 3;
306 
307  // Switch the different messages
308  switch (id & ESC32_CAN_FID_MASK) {
311  break;
312 
313  case ESC32_CAN_FID_TELEM:
315  break;
316 
317  case ESC32_CAN_FID_ACK:
318  case ESC32_CAN_FID_NACK:
319  case ESC32_CAN_FID_REPLY:
320  actuators_esc32.responses[seqId].fid = (id & ESC32_CAN_FID_MASK) >> 25;
321  memcpy(actuators_esc32.responses[seqId].data, data, 8);
322  break;
323 
324  default:
325  break;
326  }
327 }
328 
330 static inline void actuators_esc32_grant_idx(uint8_t *data)
331 {
332  uint8_t i, j;
333  uint32_t uuid = *((uint32_t *)data);
334 
335  // Look if UUID is already registered
336  for (i = 0; actuators_esc32.escs[i].status != ESC32_STATUS_ESC_FREE; i++)
337  if (actuators_esc32.escs[i].uuid == uuid) {
338  break;
339  }
340 
341  // Store the esc information
342  if (i < (ESC32_CAN_TID_MASK >> 9)) {
344  actuators_esc32.escs[i].network_id = i + 1;
345  actuators_esc32.escs[i].uuid = uuid;
346  actuators_esc32.escs[i].type = data[4];
347  actuators_esc32.escs[i].can_id = data[5];
348 
349  data[4] = (i / 4) + 1; //GroupId
350  data[5] = (i % 4) + 1; //SubGroupId
352  data);
353  }
354 
355  // When this is the last motor assign groups
356  if (i >= SERVOS_ESC32_NB - 1) {
357 
358  // Go trough all servos to sort
359  for (i = 0; i < SERVOS_ESC32_NB; i++) {
360  // Search for next lowest uuid
361  uint8_t lowest = UCHAR_MAX;
362  for (j = 0; j < SERVOS_ESC32_NB; j++) {
363  if ((i == 0 ||
365  && (lowest == UCHAR_MAX || actuators_esc32.escs[j].uuid < actuators_esc32.escs[lowest].uuid)) {
366  lowest = j;
367  }
368  }
369 
371  actuators_esc32.escs_sorted[i] = lowest;
372  }
373 
375  }
376 }
377 
379 static inline void actuators_esc32_proc_telem(uint8_t *data)
380 {
381  data[0] = data[0];
382  // ????
383  if (data[0] == 0) {
384  data[0] = 0;
385  }
386 }
#define ESC32_CAN_LCC_HIGH
unsigned short uint16_t
Definition: types.h:16
struct ESC32_response responses[ESC32_RESPONSE_CNT]
Responses of CAN messages.
static uint8_t actuators_esc32_send(uint32_t id, uint8_t tid, uint8_t length, uint8_t *data)
Transmit a message on the CAN bus.
static void actuators_esc32_proc_telem(uint8_t *data)
When we receive a telemetry message.
uint8_t data[8]
The data of the response.
struct ESC32_com escs[SERVOS_ESC32_NB]
The ESCs connected via CAN.
static void actuators_esc32_start(uint32_t tt, uint8_t tid)
Starts the ESC (let's it turn when armed)
static void actuators_esc32_dir(uint32_t tt, uint8_t tid)
Changes the direction the ESC is turing.
#define ESC32_CAN_FID_NACK
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:207
uint8_t config_idx
Selected ESC.
#define ESC32_CAN_TT_NODE
void ppz_can_init(can_rx_callback_t callback)
Definition: can.c:32
#define ESC32_CAN_FID_TELEM
enum ESC32_status status
The current status of all actuators.
static void actuators_esc32_grant_idx(uint8_t *data)
When we receive a message to grant an id.
void actuators_esc32_commit(void)
Commits the commands and sends them to the ESCs.
uint8_t fid
The FID of the response.
void actuators_esc32_init(void)
Initializes the ESCs.
#define ESC32_CAN_FID_CMD
uint8_t network_id
Index in the array +1.
uint32_t uuid
The UUID of the ESC.
Hardware independent API for actuators (servos, motor controllers).
static uint8_t cmds[6]
Definition: baro_MS5534A.c:82
int ppz_can_transmit(uint32_t id, const uint8_t *buf, uint8_t len)
Definition: can.c:38
#define ESC32_CAN_FID_ACK
#define ESC32_CAN_TID_MASK
Architecture independent timing functions.
static void actuators_esc32_arm(uint32_t tt, uint8_t tid)
Arms the ESC.
unsigned long uint32_t
Definition: types.h:18
int32_t counter
enum ESC32_esc_status status
The current status of the esc.
#define ESC32_CAN_FID_GRANT_ADDR
struct ESC32 actuators_esc32
static uint16_t actuators_esc32_melody[][2]
static void actuators_esc32_can_rx_cb(uint32_t id, uint8_t *data, int len)
When receiving messages on the CAN bus.
#define ESC32_CAN_LCC_EXCEPTION
void actuators_esc32_config_cmd(uint8_t i)
When receiving a configuration setting command.
#define ESC32_CAN_FID_SET
#define ESC32_CAN_TT_GROUP
#define SysTimeTimer(_t)
Definition: sys_time.h:219
uint8_t escs_sorted[SERVOS_ESC32_NB]
The ESCs sorted by uuid.
Core autopilot interface common to all firmwares.
#define ESC32_CAN_SEQ_MASK
#define ESC32_RESPONSE_CNT
unsigned char uint8_t
Definition: types.h:14
uint8_t can_id
The CAN identifier.
uint8_t can_seq_idx
CAN seq ID used for communicating.
static void actuators_esc32_disarm(uint32_t tt, uint8_t tid)
Disarms the ESC.
static void actuators_esc32_beep(uint32_t tt, uint8_t tid, uint16_t freq, uint16_t dur)
Let an ESC beep for a certain amount of time with a specified frequency(frequency doesn't really matc...
void actuators_esc32_set(uint8_t i, uint16_t v)
Set the commands (either RPM or duty cycle)
static uint8_t actuators_esc32_melody_size
#define ESC32_CAN_FID_REQ_ADDR
#define ESC32_CAN_LCC_NORMAL
#define ESC32_CAN_FID_RESET_BUS
uint8_t type
The type of ESC.
#define ESC32_CAN_FID_GET
#define SysTimeTimerStart(_t)
Definition: sys_time.h:218
static void actuators_esc32_duty(uint32_t tt, uint8_t tid, uint16_t *cmds)
Set the duty cycle of an ESC.
#define ESC32_CAN_FID_MASK
#define ACTUATORS_ESC32_START_DELAY
uint16_t cmds[SERVOS_ESC32_NB]
Commands which need to be committed.
#define ESC32_CAN_FID_REPLY
Actuators driver for AutoQuad ESC32 motor controllers.
static bool actuators_esc32_play_melody(uint32_t tt, uint8_t tid, uint32_t *status_sub, uint16_t melody[][2], uint8_t length)
Plays a full melody.
enum ESC32_config_cmd config_cmd
The command.
uint32_t melody_status
The status of the melody.