Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
esc32.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Murat Bronz and Xavier Paris
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
27 #include "modules/esc32/esc32.h"
28 #include "mcu_periph/uart.h"
29 #include "mcu_periph/sys_time.h"
31 #include <stdio.h>
32 #include <string.h>
33 
34 #if PERIODIC_TELEMETRY
36 #endif
37 
38 struct esc32 esc32;
39 
40 #define ESC32_BUF_IN_LEN 64
41 #define ESC32_BUF_OUT_LEN 32
42 #define ESC32_PARAMS_LEN 10
43 #define ESC32_TELEMETRY_RATE 100.0
44 
45 struct esc32_ck {
48 };
49 
50 struct esc32_private {
51  struct uart_periph *dev;
52  struct esc32_ck ck_in;
53  struct esc32_ck ck_out;
66 };
67 
68 static struct esc32_private esc32_priv;
69 
70 
71 static void esc32_send(struct esc32_private *esc) {
72  int i;
73  for (i = 0; i < esc->out_idx; i++) {
74  uart_put_byte(esc->dev, 0, esc->buf_out[i]);
75  }
76 }
77 
78 static void esc32_compute_ck(struct esc32_ck *ck, uint8_t c) {
79  ck->ck_a += c;
80  ck->ck_b += ck->ck_a;
81 }
82 
83 static void esc32_init_ck(struct esc32_ck *ck) {
84  ck->ck_a = 0;
85  ck->ck_b = 0;
86 }
87 
88 static float esc32_get_float(struct esc32_private *esc, int idx) {
89  float f;
90  unsigned char *c = (unsigned char *)&f;
91  unsigned int i;
92  for (i = 0; i < sizeof(float); i++)
93  *c++ = esc->buf_in[idx + i];
94 
95  return f;
96 }
97 
98 static void esc32_put_char(struct esc32_private *esc, unsigned char c) {
99  if (esc->out_idx < ESC32_BUF_OUT_LEN) {
100  esc->buf_out[esc->out_idx++] = c;
101  esc32_compute_ck(&(esc->ck_out), c);
102  }
103 }
104 
105 static void esc32_put_short(struct esc32_private *esc, unsigned short i) {
106  unsigned int j;
107  unsigned char *c = (unsigned char *)&i;
108 
109  for (j = 0; j < sizeof(short); j++)
110  esc32_put_char(esc, *c++);
111 }
112 
113 static void esc32_put_float(struct esc32_private *esc, float f) {
114  unsigned int j;
115  unsigned char *c = (unsigned char *)&f;
116 
117  for (j = 0; j < sizeof(float); j++)
118  esc32_put_char(esc, *c++);
119 }
120 
121 static uint16_t esc32_send_command(struct esc32_private *esc, enum binaryCommands command, float param1, float param2, int n) {
122  esc32_init_ck(&(esc->ck_out));
123  esc->out_idx = 0;
124 
125  esc->buf_out[esc->out_idx++] = 'A';
126  esc->buf_out[esc->out_idx++] = 'q';
127  esc32_put_char(esc, 1 + 2 + n*sizeof(float));
128  esc32_put_char(esc, command);
129  esc32_put_short(esc, esc->cmd_seq_id++);
130  if (n > 0)
131  esc32_put_float(esc, param1);
132  if (n > 1)
133  esc32_put_float(esc, param2);
134 
135  if (esc->out_idx < ESC32_BUF_OUT_LEN - 1) {
136  esc->buf_out[esc->out_idx++] = esc->ck_out.ck_a;
137  esc->buf_out[esc->out_idx++] = esc->ck_out.ck_b;
138  esc32_send(esc);
139  }
140 
141  return (esc->cmd_seq_id - 1);
142 }
143 
144 #define UINIT 0
145 #define GOT_SYNC 1
146 #define SEARCH_COMMAND 2
147 #define GET_CMD_ROWS 3
148 #define GET_CMD_COLS 4
149 #define GET_COMMAND 5
150 #define CHECK_CK_A 6
151 #define CHECK_CK_B 7
152 
153 static void parse_esc32(struct esc32_private *esc, uint8_t c) {
154 
155  switch (esc->state) {
156  case UINIT:
157  if (c == 'A') {
158  esc->state = GOT_SYNC;
159  }
160  break;
161  case GOT_SYNC:
162  if (c == 'q') {
163  esc->state = SEARCH_COMMAND;
164  }
165  else {
166  esc->state = UINIT;
167  }
168  break;
169  case SEARCH_COMMAND:
170  if (c == 'T') {
171  esc32_init_ck(&(esc->ck_in));
172  esc->in_idx = 0;
173  esc->state = GET_CMD_ROWS;
174  }
175  else {
176  esc->state = UINIT;
177  }
178  break;
179  case GET_CMD_ROWS:
180  esc32_compute_ck(&(esc->ck_in), c);
181  esc->in_rows = c;
182  esc->state = GET_CMD_COLS;
183  break;
184  case GET_CMD_COLS:
185  esc32_compute_ck(&(esc->ck_in), c);
186  esc->in_cols = c;
187  if ((esc->in_rows * esc->in_cols * sizeof(float)) > ESC32_BUF_IN_LEN) {
188  esc->state = UINIT;
189  } else {
190  esc->state = GET_COMMAND;
191  }
192  break;
193  case GET_COMMAND:
194  if (esc->in_idx >= ESC32_BUF_IN_LEN) {
195  esc->state = UINIT;
196  } else {
197  esc32_compute_ck(&(esc->ck_in), c);
198  esc->buf_in[esc->in_idx++] = c;
199  if (esc->in_idx >= esc->in_rows * esc->in_cols * sizeof(float)) {
200  esc->state = CHECK_CK_A;
201  }
202  }
203  break;
204  case CHECK_CK_A:
205  if (esc->ck_in.ck_a == c) {
206  esc->state = CHECK_CK_B;
207  }
208  else {
209  esc->state = UINIT;
210  }
211  break;
212  case CHECK_CK_B:
213  if (esc->ck_in.ck_b == c) {
214  esc->msg_available = true;
215  }
216  esc->state = UINIT;
217  break;
218  default:
219  esc->state = UINIT;
220  break;
221  }
222 }
223 
224 static void esc32_msg_send(struct transport_tx *trans, struct link_device *dev) {
225  float temp = 0;
226  float temp_dev = 0;
227  uint8_t id0 = 0;
228 
229  pprz_msg_send_ESC(trans, dev, AC_ID,
230  &esc32.params.amps,
232  &esc32.power,
233  &esc32.params.rpm,
235  &esc32.energy,
236  &temp,
237  &temp_dev,
238  &id0,
239  &id0); // only one motor handled for now
240 }
241 
242 void esc32_init(void) {
243  memset(&esc32, 0, sizeof(struct esc32));
244  memset(&esc32_priv, 0, sizeof(struct esc32_private));
245 
246  // uart device
247  esc32_priv.dev = &(ESC32_DEV);
248 
249  // set command sequence id to 1
251 
252 #if PERIODIC_TELEMETRY
254 #endif
255 
256 };
257 
258 #if ESC32_DEBUG
259 static int debug_start = 0;
260 #endif
261 
262 void esc32_periodic(void) {
263  if (esc32_priv.initialized == false) {
264  uart_put_byte(esc32_priv.dev, 0, '\r');
265  // enter binary mode
266  uint8_t data[] = "binary\r";
267  int i = 0;
268  while (i < (int) sizeof(data)) {
269  uart_put_byte(esc32_priv.dev, 0, data[i]);
270  i++;
271  }
278 
279  esc32_priv.initialized = true;
280  }
281  else {
282 #if ESC32_DEBUG
283  if (!debug_start) {
284  // motor spining for debug
289  debug_start = 1;
290  }
291 #endif
292  }
293 
294 }
295 
296 static void esc32_filter_data(struct esc32_parameter *in, struct esc32_parameter *out, struct esc32_parameter *min, struct esc32_parameter *max) {
297  // amps
298  out->amps += in->amps;
299  if (in->amps > max->amps) max->amps = in->amps;
300  if (in->amps < min->amps) min->amps = in->amps;
301  // volts_bat
302  out->volts_bat += in->volts_bat;
303  if (in->volts_bat > max->volts_bat) max->volts_bat = in->volts_bat;
304  if (in->volts_bat < min->volts_bat) min->volts_bat = in->volts_bat;
305  // volts_motor
306  out->volts_motor += in->volts_motor;
307  if (in->volts_motor > max->volts_motor) max->volts_motor = in->volts_motor;
308  if (in->volts_motor < min->volts_motor) min->volts_motor = in->volts_motor;
309  // rpm
310  out->rpm += in->rpm;
311  if (in->rpm > max->rpm) max->rpm = in->rpm;
312  if (in->rpm < min->rpm) min->rpm = in->rpm;
313 }
314 
315 static void esc32_parse_msg(struct esc32_private *esc_priv, struct esc32 *esc) {
316 
317  if (esc_priv->in_cols >= 4 && esc_priv->in_rows > 0) {
318  // store new values
319  esc_priv->params[esc_priv->params_idx].amps = esc32_get_float(esc_priv, 0);
320  esc_priv->params[esc_priv->params_idx].volts_bat = esc32_get_float(esc_priv, 4);
321  esc_priv->params[esc_priv->params_idx].rpm = esc32_get_float(esc_priv, 8);
322  esc_priv->params[esc_priv->params_idx].volts_motor = esc32_get_float(esc_priv, 12);
323  // increment index and average/filter when input array is full
324  esc_priv->params_idx++;
325  if (esc_priv->params_idx == ESC32_PARAMS_LEN) {
326  int i;
327  struct esc32_parameter tmp = esc_priv->params[0]; // cumulated values
328  struct esc32_parameter min = esc_priv->params[0]; // min value
329  struct esc32_parameter max = esc_priv->params[0]; // max value
330  for (i = 1; i < ESC32_PARAMS_LEN; i++) {
331  esc32_filter_data(&(esc_priv->params[i]), &tmp, &min, &max);
332  }
333  tmp.amps = (tmp.amps - min.amps - max.amps) / (ESC32_PARAMS_LEN - 2);
334  tmp.volts_bat = (tmp.volts_bat - min.volts_bat - max.volts_bat) / (ESC32_PARAMS_LEN - 2);
335  tmp.volts_motor = (tmp.volts_motor - min.volts_motor - max.volts_motor) / (ESC32_PARAMS_LEN - 2);
336  tmp.rpm = (tmp.rpm - min.rpm - max.rpm) / (ESC32_PARAMS_LEN - 2);
337  esc->params = tmp;
338  // compute power
339  esc->power = tmp.amps * tmp.volts_bat;
340  // accumulate energy
341  esc->energy += esc->power * ESC32_PARAMS_LEN / ESC32_TELEMETRY_RATE / 3600.f;
342  // new data available
343  esc->data_available = true;
344  // reset input array
345  esc_priv->params_idx = 0;
346  }
347  }
348  esc_priv->msg_available = false;
349 }
350 
351 
352 void esc32_event(void) {
357  }
358  }
359 }
360 
struct uart_periph * dev
Definition: esc32.c:51
uint8_t out_idx
Definition: esc32.c:57
void esc32_periodic(void)
Definition: esc32.c:262
static float esc32_get_float(struct esc32_private *esc, int idx)
Definition: esc32.c:88
#define ESC32_PARAMS_LEN
Definition: esc32.c:42
static void esc32_msg_send(struct transport_tx *trans, struct link_device *dev)
Definition: esc32.c:224
#define GET_CMD_COLS
Definition: esc32.c:148
uint8_t initialized
Definition: esc32.c:61
uint16_t cmd_seq_id
Definition: esc32.c:58
#define ESC32_BUF_IN_LEN
Definition: esc32.c:40
uint8_t buf_in[ESC32_BUF_IN_LEN]
Definition: esc32.c:54
void esc32_init(void)
Definition: esc32.c:242
#define SEARCH_COMMAND
Definition: esc32.c:146
uint8_t params_idx
Definition: esc32.c:62
uint8_t buf_out[ESC32_BUF_OUT_LEN]
Definition: esc32.c:55
static void esc32_init_ck(struct esc32_ck *ck)
Definition: esc32.c:83
#define UINIT
Definition: esc32.c:144
static void esc32_parse_msg(struct esc32_private *esc_priv, struct esc32 *esc)
Definition: esc32.c:315
uint8_t ck_a
Definition: esc32.c:46
#define GET_COMMAND
Definition: esc32.c:149
static void esc32_filter_data(struct esc32_parameter *in, struct esc32_parameter *out, struct esc32_parameter *min, struct esc32_parameter *max)
Definition: esc32.c:296
#define ESC32_BUF_OUT_LEN
Definition: esc32.c:41
void esc32_event(void)
Definition: esc32.c:352
uint8_t in_idx
Definition: esc32.c:56
static void esc32_send(struct esc32_private *esc)
Definition: esc32.c:71
uint8_t ck_b
Definition: esc32.c:47
#define GET_CMD_ROWS
Definition: esc32.c:147
static void esc32_compute_ck(struct esc32_ck *ck, uint8_t c)
Definition: esc32.c:78
#define CHECK_CK_B
Definition: esc32.c:151
#define CHECK_CK_A
Definition: esc32.c:150
#define GOT_SYNC
Definition: esc32.c:145
static struct esc32_private esc32_priv
Definition: esc32.c:68
struct esc32_parameter params[ESC32_PARAMS_LEN]
Definition: esc32.c:65
uint8_t state
Definition: esc32.c:60
static void parse_esc32(struct esc32_private *esc, uint8_t c)
Definition: esc32.c:153
static void esc32_put_float(struct esc32_private *esc, float f)
Definition: esc32.c:113
struct esc32_ck ck_out
Definition: esc32.c:53
struct esc32_ck ck_in
Definition: esc32.c:52
#define ESC32_TELEMETRY_RATE
Definition: esc32.c:43
static uint16_t esc32_send_command(struct esc32_private *esc, enum binaryCommands command, float param1, float param2, int n)
Definition: esc32.c:121
uint8_t in_cols
Definition: esc32.c:64
static void esc32_put_short(struct esc32_private *esc, unsigned short i)
Definition: esc32.c:105
static void esc32_put_char(struct esc32_private *esc, unsigned char c)
Definition: esc32.c:98
bool msg_available
Definition: esc32.c:59
uint8_t in_rows
Definition: esc32.c:63
Definition: esc32.c:45
float rpm
motor rotation speed
Definition: esc32.h:39
binaryCommands
Definition: esc32.h:52
@ BINARY_COMMAND_TELEM_VALUE
Definition: esc32.h:67
@ BINARY_COMMAND_DUTY
Definition: esc32.h:58
@ BINARY_COMMAND_ARM
Definition: esc32.h:54
@ BINARY_COMMAND_STOP
Definition: esc32.h:64
@ BINARY_COMMAND_TELEM_RATE
Definition: esc32.h:65
@ BINARY_COMMAND_START
Definition: esc32.h:62
bool data_available
data updated
Definition: esc32.h:47
float amps
current consumption
Definition: esc32.h:36
float energy
accumulated energy
Definition: esc32.h:45
@ BINARY_VALUE_VOLTS_MOTOR
Definition: esc32.h:77
@ BINARY_VALUE_RPM
Definition: esc32.h:78
@ BINARY_VALUE_VOLTS_BAT
Definition: esc32.h:76
@ BINARY_VALUE_AMPS
Definition: esc32.h:75
float volts_motor
motor voltage (bat voltage * throttle in % in fact)
Definition: esc32.h:38
float volts_bat
input battery voltage
Definition: esc32.h:37
float power
computed battery power
Definition: esc32.h:46
struct esc32_parameter params
filtered data from the esc
Definition: esc32.h:44
Definition: esc32.h:43
void uart_put_byte(struct uart_periph *periph, long fd, uint8_t data)
Definition: uart_arch.c:272
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:314
static uint32_t idx
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
UART peripheral.
Definition: uart.h:72
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204