Paparazzi UAS  v5.18.0_stable
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  pprz_msg_send_ESC(trans, dev, AC_ID,
226  &esc32.params.amps,
228  &esc32.power,
229  &esc32.params.rpm,
231  &esc32.energy,
232  0); // only one motor handled for now
233 }
234 
235 void esc32_init(void) {
236  memset(&esc32, 0, sizeof(struct esc32));
237  memset(&esc32_priv, 0, sizeof(struct esc32_private));
238 
239  // uart device
240  esc32_priv.dev = &(ESC32_DEV);
241 
242  // set command sequence id to 1
244 
245 #if PERIODIC_TELEMETRY
247 #endif
248 
249 };
250 
251 #if ESC32_DEBUG
252 static int debug_start = 0;
253 #endif
254 
255 void esc32_periodic(void) {
256  if (esc32_priv.initialized == false) {
257  uart_put_byte(esc32_priv.dev, 0, '\r');
258  // enter binary mode
259  uint8_t data[] = "binary\r";
260  int i = 0;
261  while (i < (int) sizeof(data)) {
262  uart_put_byte(esc32_priv.dev, 0, data[i]);
263  i++;
264  }
271 
272  esc32_priv.initialized = true;
273  }
274  else {
275 #if ESC32_DEBUG
276  if (!debug_start) {
277  // motor spining for debug
282  debug_start = 1;
283  }
284 #endif
285  }
286 
287 }
288 
289 static void esc32_filter_data(struct esc32_parameter *in, struct esc32_parameter *out, struct esc32_parameter *min, struct esc32_parameter *max) {
290  // amps
291  out->amps += in->amps;
292  if (in->amps > max->amps) max->amps = in->amps;
293  if (in->amps < min->amps) min->amps = in->amps;
294  // volts_bat
295  out->volts_bat += in->volts_bat;
296  if (in->volts_bat > max->volts_bat) max->volts_bat = in->volts_bat;
297  if (in->volts_bat < min->volts_bat) min->volts_bat = in->volts_bat;
298  // volts_motor
299  out->volts_motor += in->volts_motor;
300  if (in->volts_motor > max->volts_motor) max->volts_motor = in->volts_motor;
301  if (in->volts_motor < min->volts_motor) min->volts_motor = in->volts_motor;
302  // rpm
303  out->rpm += in->rpm;
304  if (in->rpm > max->rpm) max->rpm = in->rpm;
305  if (in->rpm < min->rpm) min->rpm = in->rpm;
306 }
307 
308 static void esc32_parse_msg(struct esc32_private *esc_priv, struct esc32 *esc) {
309 
310  if (esc_priv->in_cols >= 4 && esc_priv->in_rows > 0) {
311  // store new values
312  esc_priv->params[esc_priv->params_idx].amps = esc32_get_float(esc_priv, 0);
313  esc_priv->params[esc_priv->params_idx].volts_bat = esc32_get_float(esc_priv, 4);
314  esc_priv->params[esc_priv->params_idx].rpm = esc32_get_float(esc_priv, 8);
315  esc_priv->params[esc_priv->params_idx].volts_motor = esc32_get_float(esc_priv, 12);
316  // increment index and average/filter when input array is full
317  esc_priv->params_idx++;
318  if (esc_priv->params_idx == ESC32_PARAMS_LEN) {
319  int i;
320  struct esc32_parameter tmp = esc_priv->params[0]; // cumulated values
321  struct esc32_parameter min = esc_priv->params[0]; // min value
322  struct esc32_parameter max = esc_priv->params[0]; // max value
323  for (i = 1; i < ESC32_PARAMS_LEN; i++) {
324  esc32_filter_data(&(esc_priv->params[i]), &tmp, &min, &max);
325  }
326  tmp.amps = (tmp.amps - min.amps - max.amps) / (ESC32_PARAMS_LEN - 2);
327  tmp.volts_bat = (tmp.volts_bat - min.volts_bat - max.volts_bat) / (ESC32_PARAMS_LEN - 2);
328  tmp.volts_motor = (tmp.volts_motor - min.volts_motor - max.volts_motor) / (ESC32_PARAMS_LEN - 2);
329  tmp.rpm = (tmp.rpm - min.rpm - max.rpm) / (ESC32_PARAMS_LEN - 2);
330  esc->params = tmp;
331  // compute power
332  esc->power = tmp.amps * tmp.volts_bat;
333  // accumulate energy
334  esc->energy += esc->power * ESC32_PARAMS_LEN / ESC32_TELEMETRY_RATE / 3600.f;
335  // new data available
336  esc->data_available = true;
337  // reset input array
338  esc_priv->params_idx = 0;
339  }
340  }
341  esc_priv->msg_available = false;
342 }
343 
344 
345 void esc32_event(void) {
350  }
351  }
352 }
353 
esc32_filter_data
static void esc32_filter_data(struct esc32_parameter *in, struct esc32_parameter *out, struct esc32_parameter *min, struct esc32_parameter *max)
Definition: esc32.c:289
c
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
BINARY_COMMAND_STOP
@ BINARY_COMMAND_STOP
Definition: esc32.h:64
uint16_t
unsigned short uint16_t
Definition: types.h:16
BINARY_VALUE_RPM
@ BINARY_VALUE_RPM
Definition: esc32.h:78
ESC32_BUF_OUT_LEN
#define ESC32_BUF_OUT_LEN
Definition: esc32.c:41
GOT_SYNC
#define GOT_SYNC
Definition: esc32.c:145
GET_COMMAND
#define GET_COMMAND
Definition: esc32.c:149
uart_getch
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:968
esc32_compute_ck
static void esc32_compute_ck(struct esc32_ck *ck, uint8_t c)
Definition: esc32.c:78
esc32_ck
Definition: esc32.c:45
binaryCommands
binaryCommands
Definition: esc32.h:52
ESC32_PARAMS_LEN
#define ESC32_PARAMS_LEN
Definition: esc32.c:42
BINARY_VALUE_VOLTS_BAT
@ BINARY_VALUE_VOLTS_BAT
Definition: esc32.h:76
esc32_parameter::volts_motor
float volts_motor
motor voltage (bat voltage * throttle in % in fact)
Definition: esc32.h:38
esc32
Definition: esc32.h:43
CHECK_CK_B
#define CHECK_CK_B
Definition: esc32.c:151
esc32::data_available
bool data_available
data updated
Definition: esc32.h:47
esc32_parameter::amps
float amps
current consumption
Definition: esc32.h:36
esc32_private::params_idx
uint8_t params_idx
Definition: esc32.c:62
esc32_periodic
void esc32_periodic(void)
Definition: esc32.c:255
uart_char_available
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
esc32_private::initialized
uint8_t initialized
Definition: esc32.c:61
BINARY_COMMAND_DUTY
@ BINARY_COMMAND_DUTY
Definition: esc32.h:58
UINIT
#define UINIT
Definition: esc32.c:144
idx
static uint32_t idx
Definition: nps_radio_control_spektrum.c:105
esc32_get_float
static float esc32_get_float(struct esc32_private *esc, int idx)
Definition: esc32.c:88
esc32_private::buf_out
uint8_t buf_out[ESC32_BUF_OUT_LEN]
Definition: esc32.c:55
esc32_private
Definition: esc32.c:50
esc32_put_float
static void esc32_put_float(struct esc32_private *esc, float f)
Definition: esc32.c:113
telemetry.h
esc32_priv
static struct esc32_private esc32_priv
Definition: esc32.c:68
esc32::power
float power
computed battery power
Definition: esc32.h:46
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
esc32_put_short
static void esc32_put_short(struct esc32_private *esc, unsigned short i)
Definition: esc32.c:105
esc32_private::params
struct esc32_parameter params[ESC32_PARAMS_LEN]
Definition: esc32.c:65
ESC32_TELEMETRY_RATE
#define ESC32_TELEMETRY_RATE
Definition: esc32.c:43
esc32_private::dev
struct uart_periph * dev
Definition: esc32.c:51
esc32_event
void esc32_event(void)
Definition: esc32.c:345
esc32_private::ck_in
struct esc32_ck ck_in
Definition: esc32.c:52
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
esc32_ck::ck_b
uint8_t ck_b
Definition: esc32.c:47
esc32_send
static void esc32_send(struct esc32_private *esc)
Definition: esc32.c:71
sys_time.h
Architecture independent timing functions.
uint8_t
unsigned char uint8_t
Definition: types.h:14
esc32_parameter
Definition: esc32.h:35
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:46
uart_put_byte
void uart_put_byte(struct uart_periph *p, long fd, uint8_t data)
Uart transmit implementation.
Definition: uart_arch.c:1095
esc32_msg_send
static void esc32_msg_send(struct transport_tx *trans, struct link_device *dev)
Definition: esc32.c:224
BINARY_VALUE_VOLTS_MOTOR
@ BINARY_VALUE_VOLTS_MOTOR
Definition: esc32.h:77
esc32_private::buf_in
uint8_t buf_in[ESC32_BUF_IN_LEN]
Definition: esc32.c:54
esc32.h
esc32_parse_msg
static void esc32_parse_msg(struct esc32_private *esc_priv, struct esc32 *esc)
Definition: esc32.c:308
esc32_private::state
uint8_t state
Definition: esc32.c:60
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
esc32_private::in_idx
uint8_t in_idx
Definition: esc32.c:56
esc32_send_command
static uint16_t esc32_send_command(struct esc32_private *esc, enum binaryCommands command, float param1, float param2, int n)
Definition: esc32.c:121
esc32_private::ck_out
struct esc32_ck ck_out
Definition: esc32.c:53
BINARY_VALUE_AMPS
@ BINARY_VALUE_AMPS
Definition: esc32.h:75
CHECK_CK_A
#define CHECK_CK_A
Definition: esc32.c:150
BINARY_COMMAND_TELEM_RATE
@ BINARY_COMMAND_TELEM_RATE
Definition: esc32.h:65
esc32_private::msg_available
bool msg_available
Definition: esc32.c:59
GET_CMD_ROWS
#define GET_CMD_ROWS
Definition: esc32.c:147
uart_periph
UART peripheral.
Definition: uart.h:70
esc32_parameter::volts_bat
float volts_bat
input battery voltage
Definition: esc32.h:37
esc32_private::in_cols
uint8_t in_cols
Definition: esc32.c:64
BINARY_COMMAND_START
@ BINARY_COMMAND_START
Definition: esc32.h:62
esc32_private::in_rows
uint8_t in_rows
Definition: esc32.c:63
BINARY_COMMAND_ARM
@ BINARY_COMMAND_ARM
Definition: esc32.h:54
esc32_init
void esc32_init(void)
Definition: esc32.c:235
esc32_ck::ck_a
uint8_t ck_a
Definition: esc32.c:46
esc32_put_char
static void esc32_put_char(struct esc32_private *esc, unsigned char c)
Definition: esc32.c:98
esc32_private::out_idx
uint8_t out_idx
Definition: esc32.c:57
GET_CMD_COLS
#define GET_CMD_COLS
Definition: esc32.c:148
BINARY_COMMAND_TELEM_VALUE
@ BINARY_COMMAND_TELEM_VALUE
Definition: esc32.h:67
esc32_init_ck
static void esc32_init_ck(struct esc32_ck *ck)
Definition: esc32.c:83
esc32::params
struct esc32_parameter params
filtered data from the esc
Definition: esc32.h:44
esc32_parameter::rpm
float rpm
motor rotation speed
Definition: esc32.h:39
esc32::energy
float energy
accumulated energy
Definition: esc32.h:45
parse_esc32
static void parse_esc32(struct esc32_private *esc, uint8_t c)
Definition: esc32.c:153
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
ESC32_BUF_IN_LEN
#define ESC32_BUF_IN_LEN
Definition: esc32.c:40
esc32_private::cmd_seq_id
uint16_t cmd_seq_id
Definition: esc32.c:58
SEARCH_COMMAND
#define SEARCH_COMMAND
Definition: esc32.c:146