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

add back g and s commands, add read-only flag to JSON

Browse Source
This commit is contained in:
Samuel Sadok
2017-10-02 16:48:31 +02:00
parent 6822866a6c
commit 0ae0eee8c3
4 changed files with 270 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotorControl/low_level.c
+1 -95
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
const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR);
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
@@ -219,100 +219,6 @@ static const int current_meas_hz = CURRENT_MEAS_HZ;
/* Private variables ---------------------------------------------------------*/
static float brake_resistance = 0.47f; // [ohm]
/* 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
* ro/rw : read only/read write -> ro prevents the code generator from generating setter
* */
float* exposed_floats[] = {
&motors[0].pos_setpoint, // rw
&motors[0].pos_gain, // rw
&motors[0].vel_setpoint, // rw
&motors[0].vel_gain, // rw
&motors[0].vel_integrator_gain, // rw
&motors[0].vel_integrator_current, // rw
&motors[0].vel_limit, // rw
&motors[0].current_setpoint, // rw
&motors[0].calibration_current, // rw
&motors[0].phase_inductance, // ro
&motors[0].phase_resistance, // ro
&motors[0].current_meas.phB, // ro
&motors[0].current_meas.phC, // ro
&motors[0].DC_calib.phB, // rw
&motors[0].DC_calib.phC, // rw
&motors[0].shunt_conductance, // rw
&motors[0].phase_current_rev_gain, // rw
&motors[0].current_control.current_lim, // rw
&motors[0].current_control.p_gain, // rw
&motors[0].current_control.i_gain, // rw
&motors[0].current_control.v_current_control_integral_d, // rw
&motors[0].current_control.v_current_control_integral_q, // rw
&motors[0].current_control.Ibus, // ro
&motors[0].encoder.phase, // ro
&motors[0].encoder.pll_pos, // rw
&motors[0].encoder.pll_vel, // rw
&motors[0].encoder.pll_kp, // rw
&motors[0].encoder.pll_ki, // rw
&motors[1].pos_setpoint, // rw
&motors[1].pos_gain, // rw
&motors[1].vel_setpoint, // rw
&motors[1].vel_gain, // rw
&motors[1].vel_integrator_gain, // rw
&motors[1].vel_integrator_current, // rw
&motors[1].vel_limit, // rw
&motors[1].current_setpoint, // rw
&motors[1].calibration_current, // rw
&motors[1].phase_inductance, // ro
&motors[1].phase_resistance, // ro
&motors[1].current_meas.phB, // ro
&motors[1].current_meas.phC, // ro
&motors[1].DC_calib.phB, // rw
&motors[1].DC_calib.phC, // rw
&motors[1].shunt_conductance, // rw
&motors[1].phase_current_rev_gain, // rw
&motors[1].current_control.current_lim, // rw
&motors[1].current_control.p_gain, // rw
&motors[1].current_control.i_gain, // rw
&motors[1].current_control.v_current_control_integral_d, // rw
&motors[1].current_control.v_current_control_integral_q, // rw
&motors[1].current_control.Ibus, // ro
&motors[1].encoder.phase, // ro
&motors[1].encoder.pll_pos, // rw
&motors[1].encoder.pll_vel, // rw
&motors[1].encoder.pll_kp, // rw
&motors[1].encoder.pll_ki, // rw
};
int* exposed_ints[] = {
(int*)&motors[0].control_mode, // rw
&motors[0].encoder.encoder_offset, // rw
&motors[0].encoder.encoder_state, // ro
&motors[0].error, // rw
(int*)&motors[1].control_mode, // rw
&motors[1].encoder.encoder_offset, // rw
&motors[1].encoder.encoder_state, // ro
&motors[1].error, // rw
};
bool* exposed_bools[] = {
&motors[0].thread_ready, // ro
NULL, // rw
NULL, // rw
NULL, // ro
&motors[1].thread_ready, // ro
NULL, // rw
NULL, // rw
NULL, // ro
};
uint16_t* exposed_uint16[] = {
&motors[0].control_deadline, // rw
&motors[0].last_cpu_time, // ro
&motors[1].control_deadline, // rw
&motors[1].last_cpu_time, // ro
};
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
MotorControl/low_level.h
+1 -1
View File
@@ -155,7 +155,7 @@ typedef struct{
extern float vbus_voltage;
extern Motor_t motors[];
extern const size_t num_motors;
extern const float elec_rad_per_enc;
extern float elec_rad_per_enc; // TODO: make this const (once exposed_floats is gone)
/* Exported variables --------------------------------------------------------*/
// Exposed comms table during refactor transition
MotorControl/protocol.cpp
+231 -6
View File
@@ -1,25 +1,199 @@
/* Includes ------------------------------------------------------------------*/
#include "low_level.h"
#include "protocol.h"
#include <memory>
/* Private defines -----------------------------------------------------------*/
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* 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
* ro/rw : read only/read write -> ro prevents the code generator from generating setter
* */
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
&motors[0].vel_gain, // rw
&motors[0].vel_integrator_gain, // rw
&motors[0].vel_integrator_current, // rw
&motors[0].vel_limit, // rw
&motors[0].current_setpoint, // rw
&motors[0].calibration_current, // rw
&motors[0].phase_inductance, // ro
&motors[0].phase_resistance, // ro
&motors[0].current_meas.phB, // ro
&motors[0].current_meas.phC, // ro
&motors[0].DC_calib.phB, // rw
&motors[0].DC_calib.phC, // rw
&motors[0].shunt_conductance, // rw
&motors[0].phase_current_rev_gain, // rw
&motors[0].current_control.current_lim, // rw
&motors[0].current_control.p_gain, // rw
&motors[0].current_control.i_gain, // rw
&motors[0].current_control.v_current_control_integral_d, // rw
&motors[0].current_control.v_current_control_integral_q, // rw
&motors[0].current_control.Ibus, // ro
&motors[0].encoder.phase, // ro
&motors[0].encoder.pll_pos, // rw
&motors[0].encoder.pll_vel, // rw
&motors[0].encoder.pll_kp, // rw
&motors[0].encoder.pll_ki, // rw
&motors[1].pos_setpoint, // rw
&motors[1].pos_gain, // rw
&motors[1].vel_setpoint, // rw
&motors[1].vel_gain, // rw
&motors[1].vel_integrator_gain, // rw
&motors[1].vel_integrator_current, // rw
&motors[1].vel_limit, // rw
&motors[1].current_setpoint, // rw
&motors[1].calibration_current, // rw
&motors[1].phase_inductance, // ro
&motors[1].phase_resistance, // ro
&motors[1].current_meas.phB, // ro
&motors[1].current_meas.phC, // ro
&motors[1].DC_calib.phB, // rw
&motors[1].DC_calib.phC, // rw
&motors[1].shunt_conductance, // rw
&motors[1].phase_current_rev_gain, // rw
&motors[1].current_control.current_lim, // rw
&motors[1].current_control.p_gain, // rw
&motors[1].current_control.i_gain, // rw
&motors[1].current_control.v_current_control_integral_d, // rw
&motors[1].current_control.v_current_control_integral_q, // rw
&motors[1].current_control.Ibus, // ro
&motors[1].encoder.phase, // ro
&motors[1].encoder.pll_pos, // rw
&motors[1].encoder.pll_vel, // rw
&motors[1].encoder.pll_kp, // rw
&motors[1].encoder.pll_ki, // rw
};
int* exposed_ints[] = {
(int*)&motors[0].control_mode, // rw
&motors[0].encoder.encoder_offset, // rw
&motors[0].encoder.encoder_state, // ro
&motors[0].error, // rw
(int*)&motors[1].control_mode, // rw
&motors[1].encoder.encoder_offset, // rw
&motors[1].encoder.encoder_state, // ro
&motors[1].error, // rw
};
bool* exposed_bools[] = {
&motors[0].thread_ready, // ro
NULL, // rw
NULL, // rw
NULL, // ro
&motors[1].thread_ready, // ro
NULL, // rw
NULL, // rw
NULL, // ro
};
uint16_t* exposed_uint16[] = {
&motors[0].control_deadline, // rw
&motors[0].last_cpu_time, // ro
&motors[1].control_deadline, // rw
&motors[1].last_cpu_time, // ro
};
/* Monitoring */
size_t monitoring_slots[20] = {0};
constexpr size_t NUM_MONITORING_SLOTS = sizeof(monitoring_slots) / sizeof(monitoring_slots[0]);
/* Variables exposed to USB & UART via read/write commands */
// TODO: include range information in JSON description
// clang-format off
Endpoint endpoints[] = {
Endpoint("vbus_voltage", static_cast<const float>(vbus_voltage)),
Endpoint("elec_rad_per_enc", elec_rad_per_enc),
Endpoint("elec_rad_per_enc", static_cast<const float>(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
};
// clang-format on
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]);
/* Private function prototypes -----------------------------------------------*/
int Protocol_print_simple_type(TypeInfo_t type_info, const void *ctx);
int Protocol_scan_simple_type(TypeInfo_t type_info, const char* buffer, void *ctx);
void Protocol_print_json(void);
void Protocol_print_monitoring(size_t limit);
void Protocol_parse_cmd(uint8_t* buffer, int len);
/* Function implementations --------------------------------------------------*/
void Endpoint::print_json(size_t id, bool& need_comma) {
if (type_info_ == END_TREE) {
printf("]}");
need_comma = true;
return;
} else if (type_info_ < END_TREE) {
if (need_comma)
printf(",");
printf("{\"name\":\"%s\",\"id\":%u,\"type\":\"%s\"",
name_ ? name_ : "", id,
type_names_[type_info_] ? type_names_[type_info_] : "");
if (type_info_ == BEGIN_TREE) {
printf(",\"content\":[");
need_comma = false;
} else {
printf(",\"access\":\"");
if (print_callback_)
printf("r");
if (scan_callback_)
printf("w");
printf("\"}");
need_comma = true;
}
}
}
// Returns 0 on success, otherwise a non-zero error code
int Protocol_print_simple_type(TypeInfo_t type_info, const void *ctx) {
switch (type_info) {
case AS_FLOAT:
printf("%f", *reinterpret_cast<const float*>(ctx)); break;
case AS_INT: printf("%d", *reinterpret_cast<const int*>(ctx)); break;
case AS_BOOL: printf("%d", *reinterpret_cast<const bool*>(ctx)); break;
case AS_UINT16: printf("%hu", *reinterpret_cast<const uint16_t*>(ctx)); break;
default:
return -1;
}
return 0;
}
// Returns 0 on success, otherwise a non-zero error code
int Protocol_scan_simple_type(TypeInfo_t type_info, const char* buffer, void *ctx) {
switch (type_info) {
case AS_FLOAT: sscanf(buffer, "%f", reinterpret_cast<float*>(ctx)); break;
case AS_INT: sscanf(buffer, "%d", reinterpret_cast<int*>(ctx)); break;
case AS_BOOL: sscanf(buffer, "%d", reinterpret_cast<int*>(ctx)); break;
case AS_UINT16: sscanf(buffer, "%hu", reinterpret_cast<uint16_t*>(ctx)); break;
default:
return -1;
}
return 0;
}
void Protocol_print_json(void) {
bool need_comma = false;
@@ -78,8 +252,7 @@ void Protocol_parse_cmd(uint8_t* buffer, int len) {
// s index value
size_t index = 0;
size_t pos = 0;
// Oskar: format string start should be w, not s
int numscan = sscanf((const char*)buffer, "s %u %n", &index, &pos);
int numscan = sscanf((const char*)buffer, "w %u %n", &index, &pos);
if (numscan == 1) {
if (index < NUM_ENDPOINTS) {
endpoints[index].scan_value((const char*)buffer + pos);
@@ -94,6 +267,58 @@ void Protocol_parse_cmd(uint8_t* buffer, int len) {
endpoints[index].print_value();
}
}
} 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 index monitoring_slot
size_t index = 0;
MotorControl/protocol.h
+37 -81
View File
@@ -2,14 +2,10 @@
// 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,
@@ -19,8 +15,11 @@ typedef enum {
END_TREE
} TypeInfo_t;
typedef std::function<int(TypeInfo_t type_info, const void *ctx)> PrintCallback;
typedef std::function<int(TypeInfo_t type_info, const char* buffer, void *ctx)> ScanCallback;
// The order in this list must correspond to the order in TypeInfo_t
const char *_type_names[] = {
const char *type_names_[] = {
"float",
"int",
"bool",
@@ -29,113 +28,70 @@ const char *_type_names[] = {
};
int Protocol_print_simple_type(TypeInfo_t type_info, const void *ctx);
int Protocol_scan_simple_type(TypeInfo_t type_info, const char* buffer, void *ctx);
// Default getters/setters
// Oskar: Instead of creating all these functions, couldn't you just create one
// function called "default printer", which takes as an argument the _type_info,
// and has a big switch statement?
// You could extend this, and store only a single parse function pointer, and
// pass in a bool indicating if we are reading or writing, hence save some memory.
// We lose generality of nulling out the scanf part on const, but you could just
// create duplicates in TypeInfo_t (const and non-const). Debatable if it's worth it...
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 {
// Oskar: public before private: https://google.github.io/styleguide/cppguide.html#Declaration_Order
private:
// Oskar: variable naming: underscores at end not beginning of Class Data Members,
// https://google.github.io/styleguide/cppguide.html#Variable_Names
const TypeInfo_t _type_info;
const PrintCallback _print_callback;
ScanCallback _scan_callback;
const void* const _ctx;
public:
const char* const _name;
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)
name_(name),
type_info_(type_info),
print_callback_(print_callback),
scan_callback_(nullptr),
ctx_(ctx)
{
}
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)
name_(name),
type_info_(type_info),
print_callback_(print_callback),
scan_callback_(scan_callback),
ctx_(ctx)
{
}
//Oskar: very nice that you omit scan feature when const.
// It would be cool if you can relay this writable info in the JSON,
// so that something like the GUI can draw a grayed out box.
Endpoint(const char* name, const float& ctx) :
Endpoint(name, AS_FLOAT, print_float, &ctx) {}
Endpoint(name, AS_FLOAT, Protocol_print_simple_type, &ctx) {}
Endpoint(const char* name, float& ctx) :
Endpoint(name, AS_FLOAT, print_float, scan_float, &ctx) {}
Endpoint(name, AS_FLOAT, Protocol_print_simple_type, Protocol_scan_simple_type, &ctx) {}
Endpoint(const char* name, const int& ctx) :
Endpoint(name, AS_INT, print_int, &ctx) {}
Endpoint(name, AS_INT, Protocol_print_simple_type, &ctx) {}
Endpoint(const char* name, int& ctx) :
Endpoint(name, AS_INT, print_int, scan_int, &ctx) {}
Endpoint(name, AS_INT, Protocol_print_simple_type, Protocol_scan_simple_type, &ctx) {}
Endpoint(const char* name, const bool& ctx) :
Endpoint(name, AS_BOOL, print_bool, &ctx) {}
Endpoint(name, AS_BOOL, Protocol_print_simple_type, &ctx) {}
Endpoint(const char* name, bool& ctx) :
Endpoint(name, AS_BOOL, print_bool, scan_bool, &ctx) {}
Endpoint(name, AS_BOOL, Protocol_print_simple_type, Protocol_scan_simple_type, &ctx) {}
Endpoint(const char* name, const uint16_t& ctx) :
Endpoint(name, AS_UINT16, print_uint16, &ctx) {}
Endpoint(name, AS_UINT16, Protocol_print_simple_type, &ctx) {}
Endpoint(const char* name, uint16_t& ctx) :
Endpoint(name, AS_UINT16, print_uint16, scan_uint16, &ctx) {}
Endpoint(name, AS_UINT16, Protocol_print_simple_type, Protocol_scan_simple_type, &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_json(size_t id, bool& need_comma);
void print_value(void) {
if (_print_callback)
_print_callback(_ctx);
if (print_callback_)
print_callback_(type_info_, ctx_);
}
void scan_value(const char* buffer) {
if (_scan_callback)
_scan_callback(buffer, const_cast<void*>(_ctx));
if (scan_callback_)
scan_callback_(type_info_, buffer, const_cast<void*>(ctx_));
}
private:
const TypeInfo_t type_info_;
const PrintCallback print_callback_;
ScanCallback scan_callback_;
const void* const ctx_;
};