[module] add esc32 driver (#1636)

for testing as only one ESC is currently supported
This commit is contained in:
Gautier Hattenberger
2016-04-25 09:42:07 +02:00
committed by Felix Ruess
parent 0f53c6f270
commit f0f1d307c2
5 changed files with 533 additions and 1 deletions
+37
View File
@@ -0,0 +1,37 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="esc32">
<doc>
<description>
Driver for the ESC32v3 speed controller
Motor is controlled with PWM.
Data (voltage, current, RPM) are received from the serial link.
The driver should work with v2 and v3.
TBD:
- send commands to change ESC settings through serial link.
- support multiple ESC
For more information see:
- http://autoquad.org/esc32/
- https://github.com/svn2github/esc32
</description>
<configure name="ESC32_PORT" value="UARTX" description="UART use to get ESC data"/>
</doc>
<header>
<file name="esc32.h"/>
</header>
<init fun="esc32_init()"/>
<periodic fun="esc32_periodic()" freq="10" autorun="TRUE"/>
<event fun="esc32_event()"/>
<makefile target="ap">
<configure name="ESC32_PORT" case="upper|lower"/>
<define name="USE_$(ESC32_PORT_UPPER)"/>
<define name="ESC32_DEV" value="$(ESC32_PORT_LOWER)"/>
<define name="$(ESC32_PORT_UPPER)_BAUD" value="B230400"/>
<file name="esc32.c"/>
</makefile>
</module>
@@ -34,6 +34,7 @@
<message name="COMMANDS" period="5"/>
<message name="FBW_STATUS" period="2"/>
<message name="AIR_DATA" period="1.3"/>
<message name="ESC" period="0.9"/>
</mode>
<mode name="minimal">
<message name="ALIVE" period="5"/>
@@ -134,6 +135,7 @@
<message name="FBW_STATUS" period="1.0"/>
<message name="NAVIGATION" period="1.0"/>
<message name="DATALINK_REPORT" period="1.0"/>
<message name="ESC" period="0.1"/>
</mode>
</process>
</telemetry>
+345
View File
@@ -0,0 +1,345 @@
/*
* Copyright (C) Murat Bronz and Xavier Paris
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/esc32/esc32.c"
* @author Murat Bronz
* Serial Connection module between esc32v3 and ap
* Mainly from Bill Nesbitt's code parts from AutoQuad project
*/
#include "modules/esc32/esc32.h"
#include "mcu_periph/uart.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/datalink/downlink.h"
#include <stdio.h>
#include <string.h>
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
#endif
struct esc32 esc32;
#define ESC32_BUF_IN_LEN 64
#define ESC32_BUF_OUT_LEN 32
#define ESC32_PARAMS_LEN 10
#define ESC32_TELEMETRY_RATE 100.0
struct esc32_ck {
uint8_t ck_a;
uint8_t ck_b;
};
struct esc32_private {
struct uart_periph *dev;
struct esc32_ck ck_in;
struct esc32_ck ck_out;
uint8_t buf_in[ESC32_BUF_IN_LEN];
uint8_t buf_out[ESC32_BUF_OUT_LEN];
uint8_t in_idx;
uint8_t out_idx;
uint16_t cmd_seq_id;
bool msg_available;
uint8_t state;
uint8_t initialized;
uint8_t params_idx;
uint8_t in_rows;
uint8_t in_cols;
struct esc32_parameter params[ESC32_PARAMS_LEN];
};
static struct esc32_private esc32_priv;
static void esc32_send(struct esc32_private *esc) {
int i;
for (i = 0; i < esc->out_idx; i++) {
uart_put_byte(esc->dev, 0, esc->buf_out[i]);
}
}
static void esc32_compute_ck(struct esc32_ck *ck, uint8_t c) {
ck->ck_a += c;
ck->ck_b += ck->ck_a;
}
static void esc32_init_ck(struct esc32_ck *ck) {
ck->ck_a = 0;
ck->ck_b = 0;
}
static float esc32_get_float(struct esc32_private *esc, int idx) {
float f;
unsigned char *c = (unsigned char *)&f;
unsigned int i;
for (i = 0; i < sizeof(float); i++)
*c++ = esc->buf_in[idx + i];
return f;
}
static void esc32_put_char(struct esc32_private *esc, unsigned char c) {
esc->buf_out[esc->out_idx++] = c;
esc32_compute_ck(&(esc->ck_out), c);
}
static void esc32_put_short(struct esc32_private *esc, unsigned short i) {
unsigned int j;
unsigned char *c = (unsigned char *)&i;
for (j = 0; j < sizeof(short); j++)
esc32_put_char(esc, *c++);
}
static void esc32_put_float(struct esc32_private *esc, float f) {
unsigned int j;
unsigned char *c = (unsigned char *)&f;
for (j = 0; j < sizeof(float); j++)
esc32_put_char(esc, *c++);
}
static uint16_t esc32_send_command(struct esc32_private *esc, enum binaryCommands command, float param1, float param2, int n) {
esc32_init_ck(&(esc->ck_out));
esc->out_idx = 0;
esc->buf_out[esc->out_idx++] = 'A';
esc->buf_out[esc->out_idx++] = 'q';
esc32_put_char(esc, 1 + 2 + n*sizeof(float));
esc32_put_char(esc, command);
esc32_put_short(esc, esc->cmd_seq_id++);
if (n > 0)
esc32_put_float(esc, param1);
if (n > 1)
esc32_put_float(esc, param2);
esc->buf_out[esc->out_idx++] = esc->ck_out.ck_a;
esc->buf_out[esc->out_idx++] = esc->ck_out.ck_b;
esc32_send(esc);
return (esc->cmd_seq_id - 1);
}
#define UINIT 0
#define GOT_SYNC 1
#define SEARCH_COMMAND 2
#define GET_CMD_ROWS 3
#define GET_CMD_COLS 4
#define GET_COMMAND 5
#define CHECK_CK_A 6
#define CHECK_CK_B 7
static void parse_esc32(struct esc32_private *esc, uint8_t c) {
switch (esc->state) {
case UINIT:
if (c == 'A') {
esc->state = GOT_SYNC;
}
break;
case GOT_SYNC:
if (c == 'q') {
esc->state = SEARCH_COMMAND;
}
else {
esc->state = UINIT;
}
break;
case SEARCH_COMMAND:
if (c == 'T') {
esc32_init_ck(&(esc->ck_in));
esc->in_idx = 0;
esc->state = GET_CMD_ROWS;
}
else {
esc->state = UINIT;
}
break;
case GET_CMD_ROWS:
esc32_compute_ck(&(esc->ck_in), c);
esc->in_rows = c;
esc->state = GET_CMD_COLS;
break;
case GET_CMD_COLS:
esc32_compute_ck(&(esc->ck_in), c);
esc->in_cols = c;
esc->state = GET_COMMAND;
break;
case GET_COMMAND:
esc32_compute_ck(&(esc->ck_in), c);
esc->buf_in[esc->in_idx++] = c;
if (esc->in_idx >= esc->in_rows * esc->in_cols * sizeof(float)) {
esc->state = CHECK_CK_A;
}
break;
case CHECK_CK_A:
if (esc->ck_in.ck_a == c) {
esc->state = CHECK_CK_B;
}
else {
esc->state = UINIT;
}
break;
case CHECK_CK_B:
if (esc->ck_in.ck_b == c) {
esc->msg_available = true;
}
esc->state = UINIT;
break;
default:
esc->state = UINIT;
break;
}
}
static void esc32_msg_send(struct transport_tx *trans, struct link_device *dev) {
if (esc32.data_available == true){
pprz_msg_send_ESC(trans, dev, AC_ID,
&esc32.params.amps,
&esc32.params.volts_bat,
&esc32.power,
&esc32.params.rpm,
&esc32.params.volts_motor,
&esc32.energy,
0); // only one motor handled for now
esc32.data_available = false;
};
}
void esc32_init(void) {
memset(&esc32, 0, sizeof(struct esc32));
memset(&esc32_priv, 0, sizeof(struct esc32_private));
// uart device
esc32_priv.dev = &(ESC32_DEV);
// set command sequence id to 1
esc32_priv.cmd_seq_id = 1;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ESC, esc32_msg_send);
#endif
};
#if ESC32_DEBUG
static int debug_start = 0;
#endif
void esc32_periodic(void) {
if (esc32_priv.initialized == false) {
uart_put_byte(esc32_priv.dev, 0, '\r');
// enter binary mode
uint8_t data[] = "binary\r";
int i = 0;
while (i < (int) sizeof(data)) {
uart_put_byte(esc32_priv.dev, 0, data[i]);
i++;
}
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_RATE, 0.0, 0.0, 1);
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_VALUE, 0.0, BINARY_VALUE_AMPS, 2);
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_VALUE, 1.0, BINARY_VALUE_VOLTS_BAT, 2);
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_VALUE, 2.0, BINARY_VALUE_RPM, 2);
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_VALUE, 3.0, BINARY_VALUE_VOLTS_MOTOR, 2);
esc32_send_command(&esc32_priv, BINARY_COMMAND_TELEM_RATE, ESC32_TELEMETRY_RATE, 0.0, 1);
esc32_priv.initialized = true;
}
else {
#if ESC32_DEBUG
if (!debug_start) {
// motor spining for debug
esc32_send_command(&esc32_priv, BINARY_COMMAND_ARM, 0.0, 0.0, 0);
esc32_send_command(&esc32_priv, BINARY_COMMAND_STOP, 0.0, 0.0, 0);
esc32_send_command(&esc32_priv, BINARY_COMMAND_START, 0.0, 0.0, 0);
esc32_send_command(&esc32_priv, BINARY_COMMAND_DUTY, 8.0, 0.0, 1);
debug_start = 1;
}
#endif
}
}
static void esc32_filter_data(struct esc32_parameter *in, struct esc32_parameter *out, struct esc32_parameter *min, struct esc32_parameter *max) {
// amps
out->amps += in->amps;
if (in->amps > max->amps) max->amps = in->amps;
if (in->amps < min->amps) min->amps = in->amps;
// volts_bat
out->volts_bat += in->volts_bat;
if (in->volts_bat > max->volts_bat) max->volts_bat = in->volts_bat;
if (in->volts_bat < min->volts_bat) min->volts_bat = in->volts_bat;
// volts_motor
out->volts_motor += in->volts_motor;
if (in->volts_motor > max->volts_motor) max->volts_motor = in->volts_motor;
if (in->volts_motor < min->volts_motor) min->volts_motor = in->volts_motor;
// rpm
out->rpm += in->rpm;
if (in->rpm > max->rpm) max->rpm = in->rpm;
if (in->rpm < min->rpm) min->rpm = in->rpm;
}
static void esc32_parse_msg(struct esc32_private *esc_priv, struct esc32 *esc) {
if (esc_priv->in_cols >= 4 && esc_priv->in_rows > 0) {
// store new values
esc_priv->params[esc_priv->params_idx].amps = esc32_get_float(esc_priv, 0);
esc_priv->params[esc_priv->params_idx].volts_bat = esc32_get_float(esc_priv, 4);
esc_priv->params[esc_priv->params_idx].rpm = esc32_get_float(esc_priv, 8);
esc_priv->params[esc_priv->params_idx].volts_motor = esc32_get_float(esc_priv, 12);
// increment index and average/filter when input array is full
esc_priv->params_idx++;
if (esc_priv->params_idx == ESC32_PARAMS_LEN) {
int i;
struct esc32_parameter tmp = esc_priv->params[0]; // cumulated values
struct esc32_parameter min = esc_priv->params[0]; // min value
struct esc32_parameter max = esc_priv->params[0]; // max value
for (i = 1; i < ESC32_PARAMS_LEN; i++) {
esc32_filter_data(&(esc_priv->params[i]), &tmp, &min, &max);
}
tmp.amps = (tmp.amps - min.amps - max.amps) / (ESC32_PARAMS_LEN - 2);
tmp.volts_bat = (tmp.volts_bat - min.volts_bat - max.volts_bat) / (ESC32_PARAMS_LEN - 2);
tmp.volts_motor = (tmp.volts_motor - min.volts_motor - max.volts_motor) / (ESC32_PARAMS_LEN - 2);
tmp.rpm = (tmp.rpm - min.rpm - max.rpm) / (ESC32_PARAMS_LEN - 2);
esc->params = tmp;
// compute power
esc->power = tmp.amps * tmp.volts_bat;
// accumulate energy
esc->energy += esc->power * ESC32_PARAMS_LEN / ESC32_TELEMETRY_RATE / 3600.f;
// new data available
esc->data_available = true;
// reset input array
esc_priv->params_idx = 0;
}
}
esc_priv->msg_available = false;
}
void esc32_event(void) {
while (uart_char_available(esc32_priv.dev)) {
parse_esc32(&esc32_priv, uart_getch(esc32_priv.dev));
if (esc32_priv.msg_available) {
esc32_parse_msg(&esc32_priv, &esc32);
}
}
}
+148
View File
@@ -0,0 +1,148 @@
/*
* Copyright (C) Murat Bronz <murat.bronz@enac.fr>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/esc32/esc32.h"
* @author Murat Bronz
* Connection between esc32v3
*/
#ifndef ESC32_H
#define ESC32_H
#include "std.h"
extern void esc32_init(void);
extern void esc32_periodic(void);
extern void esc32_event(void);
struct esc32_parameter {
float amps; ///< current consumption
float volts_bat; ///< input battery voltage
float volts_motor; ///< motor voltage (bat voltage * throttle in % in fact)
float rpm; ///< motor rotation speed
float duty; ///< motor duty cycle (more or less throttle in %)
};
struct esc32 {
struct esc32_parameter params; ///< filtered data from the esc
float energy; ///< accumulated energy
float power; ///< computed battery power
bool data_available; ///< data updated
};
extern struct esc32 esc32;
enum binaryCommands {
BINARY_COMMAND_NOP = 0,
BINARY_COMMAND_ARM,
BINARY_COMMAND_CLI,
BINARY_COMMAND_CONFIG,
BINARY_COMMAND_DISARM,
BINARY_COMMAND_DUTY,
BINARY_COMMAND_PWM,
BINARY_COMMAND_RPM,
BINARY_COMMAND_SET,
BINARY_COMMAND_START,
BINARY_COMMAND_STATUS,
BINARY_COMMAND_STOP,
BINARY_COMMAND_TELEM_RATE,
BINARY_COMMAND_VERSION,
BINARY_COMMAND_TELEM_VALUE,
BINARY_COMMAND_GET_PARAM_ID,
BINARY_COMMAND_ACK = 250,
BINARY_COMMAND_NACK
};
enum binaryValues {
BINARY_VALUE_NONE = 0,
BINARY_VALUE_AMPS,
BINARY_VALUE_VOLTS_BAT,
BINARY_VALUE_VOLTS_MOTOR,
BINARY_VALUE_RPM,
BINARY_VALUE_DUTY,
BINARY_VALUE_COMM_PERIOD,
BINARY_VALUE_BAD_DETECTS,
BINARY_VALUE_ADC_WINDOW,
BINARY_VALUE_IDLE_PERCENT,
BINARY_VALUE_STATE,
BINARY_VALUE_AVGA,
BINARY_VALUE_AVGB,
BINARY_VALUE_AVGC,
BINARY_VALUE_AVGCOMP,
BINARY_VALUE_FETSTEP,
BINARY_VALUE_NUM
};
enum configParameters {
CONFIG_VERSION = 0,
STARTUP_MODE,
BAUD_RATE,
PTERM,
ITERM,
FF1TERM,
FF2TERM,
CL1TERM,
CL2TERM,
CL3TERM,
CL4TERM,
CL5TERM,
SHUNT_RESISTANCE,
MIN_PERIOD,
MAX_PERIOD,
BLANKING_MICROS,
ADVANCE,
START_VOLTAGE,
GOOD_DETECTS_START,
BAD_DETECTS_DISARM,
MAX_CURRENT,
SWITCH_FREQ,
MOTOR_POLES,
PWM_MIN_PERIOD,
PWM_MAX_PERIOD,
PWM_MIN_VALUE,
PWM_LO_VALUE,
PWM_HI_VALUE,
PWM_MAX_VALUE,
PWM_MIN_START,
PWM_RPM_SCALE,
FET_BRAKING,
PNFAC,
INFAC,
THR1TERM,
THR2TERM,
START_ALIGN_TIME,
START_ALIGN_VOLTAGE,
START_STEPS_NUM,
START_STEPS_PERIOD,
START_STEPS_ACCEL,
PWM_LOWPASS,
RPM_MEAS_LP,
SERVO_DUTY,
SERVO_P,
SERVO_D,
SERVO_MAX_RATE,
SERVO_SCALE,
ESC_ID,
DIRECTION,
CONFIG_NUM_PARAMS
};
#endif