Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-21 07:14:22 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

move USB command handling to separate files, preliminary r, w and j commands

Browse Source
This commit is contained in:
Samuel Sadok
2017-10-01 21:19:58 +02:00
parent 0b53f02b7b
commit 934668fb04
7 changed files with 267 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/tasks.json
Vendored
+3
View File
@@ -11,6 +11,9 @@
"kind": "build",
"isDefault": true
},
"presentation": {
"panel": "new"
},
"problemMatcher": [
"$gcc"
]
Makefile
+2 -1
View File
@@ -78,7 +78,8 @@ C_SOURCES = \
MotorControl/utils.c \
MotorControl/low_level.c
CPP_SOURCES = \
MotorControl/axis.cpp
MotorControl/axis.cpp \
MotorControl/protocol.cpp
ASM_SOURCES = \
startup/startup_stm32f405xx.s
MotorControl/low_level.c
+3 -132
View File
@@ -35,7 +35,7 @@ float vbus_voltage = 12.0f;
// TODO stick parameter into struct
#define ENCODER_CPR (600*4)
#define POLE_PAIRS 7
static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR);
const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR);
// TODO: Migrate to C++, clearly we are actually doing object oriented code here...
// TODO: For nice encapsulation, consider not having the motor objects public
@@ -208,7 +208,7 @@ Motor_t motors[] = {
.timing_log = {0}
}
};
const int num_motors = sizeof(motors)/sizeof(motors[0]);
const size_t num_motors = sizeof(motors) / sizeof(motors[0]);
/* Private constant data -----------------------------------------------------*/
static const float one_by_sqrt3 = 0.57735026919f;
@@ -219,8 +219,6 @@ static const int current_meas_hz = CURRENT_MEAS_HZ;
/* Private variables ---------------------------------------------------------*/
static float brake_resistance = 0.47f; // [ohm]
/* Monitoring */
monitoring_slot monitoring_slots[20] = {0};
/* variables exposed to usb interface via set/get/monitor
* If you change something here, don't forget to regenerate the python interface with generate_api.py
@@ -228,8 +226,6 @@ monitoring_slot monitoring_slots[20] = {0};
* */
float* exposed_floats[] = {
&vbus_voltage, // ro
&elec_rad_per_enc, // ro
&motors[0].pos_setpoint, // rw
&motors[0].pos_gain, // rw
&motors[0].vel_setpoint, // rw
@@ -323,31 +319,8 @@ uint16_t* exposed_uint16[] = {
//--------------------------------
// Command Handling
// TODO move to different file
//--------------------------------
void print_monitoring(int limit) {
for (int i=0;i<limit;i++) {
switch (monitoring_slots[i].type) {
case 0:
printf("%f\t",*exposed_floats[monitoring_slots[i].index]);
break;
case 1:
printf("%d\t",*exposed_ints[monitoring_slots[i].index]);
break;
case 2:
printf("%d\t",*exposed_bools[monitoring_slots[i].index]);
break;
case 3:
printf("%hu\t",*exposed_uint16[monitoring_slots[i].index]);
break;
default:
i=100;
}
}
printf("\n");
}
void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) {
motor->pos_setpoint = pos_setpoint;
motor->vel_setpoint = vel_feed_forward;
@@ -375,108 +348,6 @@ void set_current_setpoint(Motor_t* motor, float current_setpoint) {
#endif
}
void motor_parse_cmd(uint8_t* buffer, int len) {
// TODO very hacky way of terminating sscanf at end of buffer:
// We should do some proper struct packing instead of using sscanf altogether
buffer[len] = 0;
// check incoming packet type
if (buffer[0] == 'p') {
// position control
unsigned motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
if (numscan == 4 && motor_number < num_motors) {
set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'v') {
// velocity control
unsigned motor_number;
float vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
if (numscan == 3 && motor_number < num_motors) {
set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'c') {
// current control
unsigned motor_number;
float current_feed_forward;
int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, &current_feed_forward);
if (numscan == 2 && motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
} else if (buffer[0] == 'g') { // GET
// g <0:float,1:int,2:bool,3:uint16> index
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "g %u %u", &type, &index);
if (numscan == 2) {
switch(type){
case 0: {
printf("%f\n",*exposed_floats[index]);
break;
};
case 1: {
printf("%d\n",*exposed_ints[index]);
break;
};
case 2: {
printf("%d\n",*exposed_bools[index]);
break;
};
case 3: {
printf("%hu\n",*exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 's') { // SET
// s <0:float,1:int,2:bool,3:uint16> index value
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "s %u %u", &type, &index);
if (numscan == 2) {
switch(type) {
case 0: {
sscanf((const char*)buffer, "s %u %u %f", &type, &index, exposed_floats[index]);
break;
};
case 1: {
sscanf((const char*)buffer, "s %u %u %d", &type, &index, exposed_ints[index]);
break;
};
case 2: {
int btmp = 0;
sscanf((const char*)buffer, "s %u %u %d", &type, &index, &btmp);
*exposed_bools[index] = btmp ? true : false;
break;
};
case 3: {
sscanf((const char*)buffer, "s %u %u %hu", &type, &index, exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 'm') { // Setup Monitor
// m <0:float,1:int,2:bool,3:uint16> index monitoring_slot
int type = 0;
int index = 0;
int slot = 0;
int numscan = sscanf((const char*)buffer, "m %u %u %u", &type, &index, &slot);
if (numscan == 3) {
monitoring_slots[slot].type = type;
monitoring_slots[slot].index = index;
}
} else if (buffer[0] == 'o') { // Output Monitor
int limit = 0;
int numscan = sscanf((const char*)buffer, "o %u", &limit);
if (numscan == 1) {
print_monitoring(limit);
}
}
}
//--------------------------------
// Utility
@@ -793,7 +664,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
// and their interrupts should arrive on the same clock cycle.
// We dispatch the callbacks in order, so ADC2 will always be processed before ADC3.
// Therefore we store the value from ADC2 and signal the thread that the
// measurement is ready when we recieve the ADC3 measurement
// measurement is ready when we receive the ADC3 measurement
// return or continue
if (hadc == &hadc2) {
MotorControl/low_level.h
+10 -1
View File
@@ -2,6 +2,10 @@
#ifndef __LOW_LEVEL_H
#define __LOW_LEVEL_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include <cmsis_os.h>
#include "drv8301.h"
@@ -150,7 +154,8 @@ typedef struct{
/* Exported constants --------------------------------------------------------*/
extern float vbus_voltage;
extern Motor_t motors[];
extern const int num_motors;
extern const size_t num_motors;
extern const float elec_rad_per_enc;
/* Exported variables --------------------------------------------------------*/
// Exposed comms table during refactor transition
@@ -213,4 +218,8 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des);
void control_motor_loop(Motor_t* motor);
// Motor thread (is public)
#ifdef __cplusplus
}
#endif
#endif //__LOW_LEVEL_H
MotorControl/protocol.cpp
+113
View File
@@ -0,0 +1,113 @@
#include "low_level.h"
#include "protocol.h"
#include <memory>
Endpoint endpoints[] = {
Endpoint("vbus_voltage", static_cast<const float>(vbus_voltage)),
Endpoint("elec_rad_per_enc", elec_rad_per_enc),
Endpoint("motor0", BEGIN_TREE, nullptr, nullptr, nullptr),
Endpoint("pos_setpoint", motors[0].pos_setpoint),
Endpoint("pos_gain", motors[0].pos_gain),
Endpoint("vel_setpoint", motors[0].vel_setpoint),
Endpoint(nullptr, END_TREE, nullptr, nullptr, nullptr) // motor0
};
constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]);
/* Monitoring */
size_t monitoring_slots[20] = {0};
constexpr size_t NUM_MONITORING_SLOTS = sizeof(monitoring_slots) / sizeof(monitoring_slots[0]);
void Protocol_print_json(void) {
bool need_comma = false;
printf("[");
for (size_t i = 0; i < NUM_ENDPOINTS; ++i) {
endpoints[i].print_json(i, need_comma);
}
printf("]");
}
void Protocol_print_monitoring(size_t limit) {
for (size_t i = 0; i < limit; ++i) {
if (monitoring_slots[i] < NUM_ENDPOINTS) {
endpoints[monitoring_slots[i]].print_value();
}
printf("\t");
}
printf("\n");
}
void Protocol_parse_cmd(uint8_t* buffer, int len) {
// TODO very hacky way of terminating sscanf at end of buffer:
// We should do some proper struct packing instead of using sscanf altogether
buffer[len] = 0;
// check incoming packet type
if (buffer[0] == 'p') {
// position control
unsigned motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
if (numscan == 4 && motor_number < num_motors) {
set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'v') {
// velocity control
unsigned motor_number;
float vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
if (numscan == 3 && motor_number < num_motors) {
set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'c') {
// current control
unsigned motor_number;
float current_feed_forward;
int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, &current_feed_forward);
if (numscan == 2 && motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
} else if (buffer[0] == 'j') {
// Read JSON interface definition
Protocol_print_json();
} else if (buffer[0] == 'w') { // WRITE
// s index value
size_t index = 0;
size_t pos = 0;
int numscan = sscanf((const char*)buffer, "s %u %n", &index, &pos);
if (numscan == 1) {
if (index < NUM_ENDPOINTS) {
endpoints[index].scan_value((const char*)buffer + pos);
}
}
} else if (buffer[0] == 'r') { // READ
// r index
size_t index = 0;
int numscan = sscanf((const char*)buffer, "r %u", &index);
if (numscan == 1) {
if (index < NUM_ENDPOINTS) {
endpoints[index].print_value();
}
}
} else if (buffer[0] == 'm') { // Setup Monitor
// m index monitoring_slot
size_t index = 0;
size_t slot = 0;
int numscan = sscanf((const char*)buffer, "m %u %u", &index, &slot);
if (numscan == 2) {
if (index < NUM_ENDPOINTS && slot < NUM_MONITORING_SLOTS) {
monitoring_slots[slot] = index;
}
}
} else if (buffer[0] == 'o') { // Output Monitor
size_t limit = 0;
int numscan = sscanf((const char*)buffer, "o %u", &limit);
if (numscan == 1) {
Protocol_print_monitoring(limit);
}
}
}
MotorControl/protocol.h
+134
View File
@@ -0,0 +1,134 @@
// TODO: resolve assert
#define assert(expr)
#ifdef __cplusplus
#include <functional>
typedef std::function<void(const void *ctx)> PrintCallback;
typedef std::function<void(const char* buffer, void *ctx)> ScanCallback;
typedef enum {
AS_FLOAT,
AS_INT,
AS_BOOL,
AS_UINT16,
BEGIN_TREE,
END_TREE
} TypeInfo_t;
// The order in this list must correspond to the order in TypeInfo_t
const char *_type_names[] = {
"float",
"int",
"bool",
"uint16",
"tree"
};
// Default getters/setters
PrintCallback print_float = std::bind(printf, "%f", std::placeholders::_1);
ScanCallback scan_float = std::bind(sscanf, std::placeholders::_1, "%f", std::placeholders::_2);
PrintCallback print_int = std::bind(printf, "%d", std::placeholders::_1);
ScanCallback scan_int = std::bind(sscanf, std::placeholders::_1, "%d", std::placeholders::_2);
PrintCallback print_bool = std::bind(printf, "%d", std::placeholders::_1);
ScanCallback scan_bool = std::bind(sscanf, std::placeholders::_1, "%d", std::placeholders::_2);
PrintCallback print_uint16 = std::bind(printf, "%d", std::placeholders::_1);
ScanCallback scan_uint16 = std::bind(sscanf, std::placeholders::_1, "%d", std::placeholders::_2);
class Endpoint {
private:
const TypeInfo_t _type_info;
const PrintCallback _print_callback;
ScanCallback _scan_callback;
const void* const _ctx;
public:
const char* const _name;
Endpoint(const char* name, TypeInfo_t type_info, PrintCallback print_callback, const void *ctx) :
_type_info(type_info),
_print_callback(print_callback),
_scan_callback(nullptr),
_ctx(ctx),
_name(name)
{
}
Endpoint(const char* name, TypeInfo_t type_info, PrintCallback print_callback, ScanCallback scan_callback, void *ctx) :
_type_info(type_info),
_print_callback(print_callback),
_scan_callback(scan_callback),
_ctx(ctx),
_name(name)
{
}
Endpoint(const char* name, const float& ctx) :
Endpoint(name, AS_FLOAT, print_float, &ctx) {}
Endpoint(const char* name, float& ctx) :
Endpoint(name, AS_FLOAT, print_float, scan_float, &ctx) {}
Endpoint(const char* name, const int& ctx) :
Endpoint(name, AS_INT, print_int, &ctx) {}
Endpoint(const char* name, int& ctx) :
Endpoint(name, AS_INT, print_int, scan_int, &ctx) {}
Endpoint(const char* name, const bool& ctx) :
Endpoint(name, AS_BOOL, print_bool, &ctx) {}
Endpoint(const char* name, bool& ctx) :
Endpoint(name, AS_BOOL, print_bool, scan_bool, &ctx) {}
Endpoint(const char* name, const uint16_t& ctx) :
Endpoint(name, AS_UINT16, print_uint16, &ctx) {}
Endpoint(const char* name, uint16_t& ctx) :
Endpoint(name, AS_UINT16, print_uint16, scan_uint16, &ctx) {}
void print_json(size_t id, bool& need_comma) {
if (_type_info == END_TREE) {
printf("]}");
need_comma = true;
return;
} else if (_type_info < END_TREE) {
assert(_name);
assert(_type_names[_type_info]);
if (need_comma)
printf(",");
printf("{\"name\":\"%s\",\"id\":%u,\"type\":\"%s\"", _name, id, _type_names[_type_info]);
if (_type_info == BEGIN_TREE) {
printf(",\"content\":[");
need_comma = false;
} else {
printf("}");
need_comma = true;
}
}
}
void print_value(void) {
if (_print_callback)
_print_callback(_ctx);
}
void scan_value(const char* buffer) {
if (_scan_callback)
_scan_callback(buffer, const_cast<void*>(_ctx));
}
};
extern "C" {
#endif
void Protocol_parse_cmd(uint8_t* buffer, int len);
#ifdef __cplusplus
}
#endif
Src/usbd_cdc_if.c
+2 -2
View File
@@ -50,7 +50,7 @@
#include "usbd_cdc_if.h"
/* USER CODE BEGIN INCLUDE */
#include "utils.h"
#include "low_level.h"
#include "protocol.h"
/* USER CODE END INCLUDE */
/** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
@@ -275,7 +275,7 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
int null_idx = MACRO_MIN(*Len, APP_RX_DATA_SIZE-1);
Buf[null_idx] = 0;
motor_parse_cmd(Buf, *Len);
Protocol_parse_cmd(Buf, *Len);
return (USBD_OK);
/* USER CODE END 6 */