Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-17 03:18:59 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #52 from madcowswe/sam_protocol

Browse Source 
Protocol Upgrade
This commit is contained in:
Oskar Weigl
2017-11-26 17:21:39 -08:00
committed by GitHub
parent ec62253723 19dbbec7d7
commit 685f9c821c
42 changed files with 2896 additions and 661 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/.vscode/c_cpp_properties.json
Vendored
+2
View File
@@ -17,6 +17,8 @@
"${workspaceRoot}/Inc",
"${workspaceRoot}/MotorControl",
"C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include",
"C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1",
"C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1/arm-none-eabi",
"C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/lib/gcc/arm-none-eabi/6.3.1/include"
],
"defines": [
Firmware/.vscode/launch.json
Vendored
+4 -1
View File
@@ -1,10 +1,13 @@
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"type": "gdb",
"request": "attach",
"name": "Launch Program",
"name": "Debug Firmware",
"target": "localhost:3333",
"gdbpath": "arm-none-eabi-gdb",
"remote": true,
Firmware/.vscode/settings.json
Vendored
+36 -1
View File
@@ -1,3 +1,38 @@
{
"C_Cpp.clang_format_style": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 0 }"
"C_Cpp.clang_format_style": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 0 }",
"files.exclude": {
"build": true
},
"files.associations": {
"memory": "cpp",
"utility": "cpp",
"deque": "cpp",
"vector": "cpp",
"array": "cpp",
"*.tcc": "cpp",
"cctype": "cpp",
"clocale": "cpp",
"cstdint": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"exception": "cpp",
"functional": "cpp",
"initializer_list": "cpp",
"iosfwd": "cpp",
"istream": "cpp",
"limits": "cpp",
"new": "cpp",
"ostream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"typeinfo": "cpp",
"algorithm": "cpp"
}
}
Firmware/.vscode/tasks.json
Vendored
+3
View File
@@ -11,6 +11,9 @@
"kind": "build",
"isDefault": true
},
"presentation": {
"panel": "new"
},
"problemMatcher": [
"$gcc"
]
Firmware/CHANGELOG.md
+8 -1
View File
@@ -1,10 +1,17 @@
## [Unreleased]
## [0.3]
### Added
* **New binary communication protocol**
* This is a much richer and more efficient binary protocol than the old human-readable protocol.
* The old protocol is still available (but will be depricated eventually). You must manually chose to fall back on this protocol if you wish to still use it.
* Support for C++
* Demo scripts for getting started with commanding ODrive from python
* Protection from user setting current_lim higher than is measurable
### Changed
* Shunt resistance values for v3.3 and earlier to include extra resistance of PCB
* Refactoring of control code:
* Lifted top layer of low_level.c into Axis.cpp
## [0.2.2] - 2017-11-17
### Fixed
Firmware/Inc/freertos_vars.h
+1 -1
View File
@@ -2,7 +2,7 @@
#ifndef __FREERTOS_H
#define __FREERTOS_H
// List of semaphore
// List of semaphores
osSemaphoreId sem_usb_irq;
osSemaphoreId sem_uart_dma;
osSemaphoreId sem_usb_rx;
Firmware/Inc/usbd_cdc_if.h
+4
View File
@@ -71,6 +71,10 @@
* @{
*/
/* USER CODE BEGIN EXPORTED_DEFINES */
/* Define size for the receive and transmit buffer over CDC */
/* It's up to user to redefine and/or remove those define */
#define USB_RX_DATA_SIZE 64
#define USB_TX_DATA_SIZE 64
/* USER CODE END EXPORTED_DEFINES */
/**
Firmware/Inc/version.h
-10
View File
@@ -1,10 +0,0 @@
/**
******************************************************************************
* @file : version.h
* @brief : This file implements printf functionality
******************************************************************************
*/
#define ODRIVE_FW_VERSION_MAJOR 0
#define ODRIVE_FW_VERSION_MINOR 1
#define ODRIVE_FW_VERSION_PATCH 0
Firmware/Makefile
+8 -1
View File
@@ -80,8 +80,12 @@ C_SOURCES = \
Src/usbd_cdc_if.c \
Src/syscalls.c \
MotorControl/utils.c \
MotorControl/commands.c \
MotorControl/legacy_commands.c \
MotorControl/low_level.c
CPP_SOURCES = \
MotorControl/axis.cpp \
MotorControl/commands.cpp \
MotorControl/protocol.cpp
ASM_SOURCES = \
startup/startup_stm32f405xx.s
@@ -158,6 +162,9 @@ vpath %.s $(sort $(dir $(ASM_SOURCES)))
$(BUILD_DIR)/%.o: %.c Makefile | $(BUILD_DIR)
@$(CC) -c $(CFLAGS) -Wa,-a,-ad,-alms=$(BUILD_DIR)/$(notdir $(<:.c=.lst)) $< -o $@
$(BUILD_DIR)/%.o: %.cpp Makefile | $(BUILD_DIR)
@$(CXX) -c $(CXXFLAGS) -Wa,-a,-ad,-alms=$(BUILD_DIR)/$(notdir $(<:.cpp=.lst)) $< -o $@
$(BUILD_DIR)/%.o: %.s Makefile | $(BUILD_DIR)
@$(AS) -c $(CFLAGS) $< -o $@
Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c
Vendored
+3 -2
View File
@@ -26,7 +26,7 @@
*
*----------------------------------------------------------------------------
*
* Portions Copyright © 2016 STMicroelectronics International N.V. All rights reserved.
* Portions Copyright � 2016 STMicroelectronics International N.V. All rights reserved.
* Portions Copyright (c) 2013 ARM LIMITED
* All rights reserved.
* Redistribution and use in source and binary forms, with or without
@@ -819,7 +819,8 @@ osSemaphoreId osSemaphoreCreate (const osSemaphoreDef_t *semaphore_def, int32_t
* @retval number of available tokens, or -1 in case of incorrect parameters.
* @note MUST REMAIN UNCHANGED: \b osSemaphoreWait shall be consistent in every CMSIS-RTOS.
*/
int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec)
// TODO: submit patch upstream
osStatus osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec)
{
TickType_t ticks;
portBASE_TYPE taskWoken = pdFALSE;
Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
Vendored
+3 -3
View File
@@ -26,7 +26,7 @@
*
*----------------------------------------------------------------------------
*
* Portions Copyright © 2016 STMicroelectronics International N.V. All rights reserved.
* Portions Copyright � 2016 STMicroelectronics International N.V. All rights reserved.
* Portions Copyright (c) 2013 ARM LIMITED
* All rights reserved.
* Redistribution and use in source and binary forms, with or without
@@ -719,9 +719,9 @@ osSemaphoreId osSemaphoreCreate (const osSemaphoreDef_t *semaphore_def, int32_t
/// Wait until a Semaphore token becomes available.
/// \param[in] semaphore_id semaphore object referenced with \ref osSemaphoreCreate.
/// \param[in] millisec timeout value or 0 in case of no time-out.
/// \return number of available tokens, or -1 in case of incorrect parameters.
/// \return osOK if the operation succeded.
/// \note MUST REMAIN UNCHANGED: \b osSemaphoreWait shall be consistent in every CMSIS-RTOS.
int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec);
osStatus osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec);
/// Release a Semaphore token.
/// \param[in] semaphore_id semaphore object referenced with \ref osSemaphoreCreate.
Firmware/MotorControl/axis.cpp
+94
View File
@@ -0,0 +1,94 @@
#include "axis.h"
#include <stdlib.h>
#include "legacy_commands.h"
//TODO: goal of refactor is to kick this out completely
extern "C" {
#include "low_level.h"
}
// C interface
extern "C" {
void axis_thread_entry(void const* temp_motor_ptr) {
Motor_t* motor = (Motor_t*)temp_motor_ptr;
//TODO: explicit axis number assignment
//for now we search for it
uint8_t ax_number = 0;
while (&motors[ax_number] != motor)
++ax_number;
static const AxisConfig default_config;
Axis axis(default_config, ax_number, motor);
axis.StateMachineLoop();
}
} // extern "C"
void Axis::SetupLegacyMappings() {
// Legacy reachability from C
legacy_motor_ref_->axis_legacy.enable_control = &enable_control_;
// override for compatibility with legacy comms paradigm
// TODO next gen comms
exposed_bools[4 * axis_number_ + 1] = &enable_control_;
exposed_bools[4 * axis_number_ + 2] = &do_calibration_;
}
Axis::Axis(const AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref)
: axis_number_(axis_number),
enable_control_(config.enable_control_at_start),
do_calibration_(config.do_calibration_at_start),
legacy_motor_ref_(legacy_motor_ref) {
SetupLegacyMappings();
}
void Axis::StateMachineLoop() {
//TODO: Move this somewhere else
// Allocate the map for anti-cogging algorithm and initialize all values to 0.0f
int encoder_cpr = legacy_motor_ref_->encoder.encoder_cpr;
legacy_motor_ref_->anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float));
if (legacy_motor_ref_->anticogging.cogging_map != NULL) {
for (int i = 0; i < encoder_cpr; i++) {
legacy_motor_ref_->anticogging.cogging_map[i] = 0.0f;
}
}
legacy_motor_ref_->motor_thread = osThreadGetId();
legacy_motor_ref_->thread_ready = true;
bool calibration_ok = false;
for (;;) {
if (do_calibration_) {
do_calibration_ = false;
__HAL_TIM_MOE_ENABLE(legacy_motor_ref_->motor_timer); // enable pwm outputs
calibration_ok = motor_calibration(legacy_motor_ref_);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(legacy_motor_ref_->motor_timer); // disables pwm outputs
}
if (calibration_ok && enable_control_) {
legacy_motor_ref_->enable_step_dir = true;
__HAL_TIM_MOE_ENABLE(legacy_motor_ref_->motor_timer);
bool spin_up_ok = true;
if (legacy_motor_ref_->rotor_mode == ROTOR_MODE_SENSORLESS)
spin_up_ok = spin_up_sensorless(legacy_motor_ref_);
if (spin_up_ok)
control_motor_loop(legacy_motor_ref_);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(legacy_motor_ref_->motor_timer);
legacy_motor_ref_->enable_step_dir = false;
if (enable_control_) { // if control is still enabled, we exited because of error
calibration_ok = false;
enable_control_ = false;
}
}
// give some time to lower priority threads
osDelay(2);
}
legacy_motor_ref_->thread_ready = false;
}
Firmware/MotorControl/axis.h
+60
View File
@@ -0,0 +1,60 @@
#ifndef __AXIS_HPP
#define __AXIS_HPP
//TODO: goal of refactor is to kick this out completely
extern "C" {
#include "low_level.h"
}
//Outside axis:
//command handler
//callback dispatch
// TODO: decide if we want to consolidate all default configs in one file for ease of use?
struct AxisConfig {
bool enable_control_at_start = true;
bool do_calibration_at_start = true;
};
class Axis {
public:
//thread/os/system management
//timing log
//thread id
//etc.
//state machine
//control mode
//control_en/calib_ok
//error state
//motor
//current controller
//contains rotor phase logic
//motor level calibration routines
//low_level (implementation specifics)
//DRV driver
//adc callback handling
//pwm queueing
//rotor estimator
//kick out rotor phase logic
//pos/vel controller
//step/dir handler
// Object operation requires ptr to legacy object for now, TODO: get rid of this dep
Axis(const AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref);
// Infinite loop that does calibration and enters main control loop as appropriate
void StateMachineLoop();
uint8_t axis_number_;
bool enable_control_;
bool do_calibration_;
Motor_t* legacy_motor_ref_;
private:
void SetupLegacyMappings();
};
#endif /* __AXIS_HPP */
Firmware/MotorControl/axis_c_interface.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef __AXIS_C_INTERFACE_H
#define __AXIS_C_INTERFACE_H
#ifdef __cplusplus
extern "C" {
#endif
void axis_thread_entry(void const * temp_motor_ptr);
#ifdef __cplusplus
}
#endif
#endif /* __AXIS_C_INTERFACE_H */
Firmware/MotorControl/commands.cpp
+416
View File
@@ -0,0 +1,416 @@
/* Includes ------------------------------------------------------------------*/
// TODO: remove this option
// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py
// todo: make clean switches for protocol
#define ENABLE_LEGACY_PROTOCOL
#include "commands.h"
#include "low_level.h"
#include "protocol.hpp"
#include "freertos_vars.h"
#include "utils.h"
#ifdef ENABLE_LEGACY_PROTOCOL
#include "legacy_commands.h"
#endif
#include <cmsis_os.h>
#include <memory>
#include <usbd_cdc_if.h>
#include <usb_device.h>
#include <usart.h>
#include <gpio.h>
#define UART_TX_BUFFER_SIZE 64
/* Private defines -----------------------------------------------------------*/
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
extern USBD_HandleTypeDef hUsbDeviceFS;
/* Private constant data -----------------------------------------------------*/
// TODO: make command to switch gpio_mode during run-time
#if defined(USE_GPIO_MODE_STEP_DIR)
static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir
#elif !defined(UART_PROTOCOL_NONE)
static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx
#else
static const GpioMode_t gpio_mode = GPIO_MODE_NONE; //GPIO 1,2 is not configured
#endif
/* Private variables ---------------------------------------------------------*/
static uint8_t* usb_buf;
static uint32_t usb_len;
// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
static thread_local uint32_t deadline_ms = 0;
/* Variables exposed to USB & UART via read/write commands */
// TODO: include range information in JSON description
// TODO: Autogenerate these functions
void motors_0_set_pos_setpoint_func(void) {
set_pos_setpoint(&motors[0],
motors[0].set_pos_setpoint_args.pos_setpoint,
motors[0].set_pos_setpoint_args.vel_feed_forward,
motors[0].set_pos_setpoint_args.current_feed_forward);
}
void motors_0_set_vel_setpoint_func(void) {
set_vel_setpoint(&motors[0],
motors[0].set_vel_setpoint_args.vel_setpoint,
motors[0].set_vel_setpoint_args.current_feed_forward);
}
void motors_0_set_current_setpoint_func(void) {
set_current_setpoint(&motors[0],
motors[0].set_current_setpoint_args.current_setpoint);
}
void motors_1_set_pos_setpoint_func(void) {
set_pos_setpoint(&motors[0],
motors[1].set_pos_setpoint_args.pos_setpoint,
motors[1].set_pos_setpoint_args.vel_feed_forward,
motors[1].set_pos_setpoint_args.current_feed_forward);
}
void motors_1_set_vel_setpoint_func(void) {
set_vel_setpoint(&motors[0],
motors[1].set_vel_setpoint_args.vel_setpoint,
motors[1].set_vel_setpoint_args.current_feed_forward);
}
void motors_1_set_current_setpoint_func(void) {
set_current_setpoint(&motors[0],
motors[1].set_current_setpoint_args.current_setpoint);
}
// This table specifies which fields and functions are exposed on the USB and UART ports.
// TODO: Autogenerate this table. It will come up again very soon in the Arduino library.
// clang-format off
const Endpoint endpoints[] = {
Endpoint::make_property("vbus_voltage", const_cast<const float*>(&vbus_voltage)),
Endpoint::make_property("elec_rad_per_enc", const_cast<const float*>(&elec_rad_per_enc)),
Endpoint::make_object("motor0"),
Endpoint::make_property("control_mode", reinterpret_cast<int32_t*>(&motors[0].control_mode)),
Endpoint::make_property("error", reinterpret_cast<int32_t*>(&motors[0].error)),
Endpoint::make_property("pos_setpoint", &motors[0].pos_setpoint),
Endpoint::make_property("pos_gain", &motors[0].pos_gain),
Endpoint::make_property("vel_setpoint", &motors[0].vel_setpoint),
Endpoint::make_property("vel_gain", &motors[0].vel_gain),
Endpoint::make_property("vel_integrator_gain", &motors[0].vel_integrator_gain),
Endpoint::make_property("vel_integrator_current", &motors[0].vel_integrator_current),
Endpoint::make_property("vel_limit", &motors[0].vel_limit),
Endpoint::make_property("current_setpoint", &motors[0].current_setpoint),
Endpoint::make_property("calibration_current", &motors[0].calibration_current),
Endpoint::make_property("phase_inductance", const_cast<const float*>(&motors[0].phase_inductance)),
Endpoint::make_property("phase_resistance", const_cast<const float*>(&motors[0].phase_resistance)),
Endpoint::make_property("current_meas.phB", const_cast<const float*>(&motors[0].current_meas.phB)),
Endpoint::make_property("current_meas.phC", const_cast<const float*>(&motors[0].current_meas.phC)),
Endpoint::make_property("DC_calib.phB", &motors[0].DC_calib.phB),
Endpoint::make_property("DC_calib.phC", &motors[0].DC_calib.phC),
Endpoint::make_property("shunt_conductance", &motors[0].shunt_conductance),
Endpoint::make_property("phase_current_rev_gain", &motors[0].phase_current_rev_gain),
Endpoint::make_property("thread_ready", reinterpret_cast<uint8_t*>(&motors[0].thread_ready)),
Endpoint::make_property("control_deadline", &motors[0].control_deadline),
Endpoint::make_property("last_cpu_time", &motors[0].last_cpu_time),
Endpoint::make_object("current_control"),
Endpoint::make_property("current_lim", &motors[0].current_control.current_lim),
Endpoint::make_property("p_gain", &motors[0].current_control.p_gain),
Endpoint::make_property("i_gain", &motors[0].current_control.i_gain),
Endpoint::make_property("v_current_control_integral_d", &motors[0].current_control.v_current_control_integral_d),
Endpoint::make_property("v_current_control_integral_q", &motors[0].current_control.v_current_control_integral_q),
Endpoint::make_property("Ibus", const_cast<const float*>(&motors[0].current_control.Ibus)),
Endpoint::close_tree(),
Endpoint::make_object("encoder"),
Endpoint::make_property("phase", const_cast<const float*>(&motors[0].encoder.phase)),
Endpoint::make_property("pll_pos", &motors[0].encoder.pll_pos),
Endpoint::make_property("pll_vel", &motors[0].encoder.pll_vel),
Endpoint::make_property("pll_kp", &motors[0].encoder.pll_kp),
Endpoint::make_property("pll_ki", &motors[0].encoder.pll_ki),
Endpoint::make_property("encoder_offset", &motors[0].encoder.encoder_offset),
Endpoint::make_property("encoder_state", &motors[0].encoder.encoder_state),
Endpoint::close_tree(),
Endpoint::make_function("set_pos_setpoint", &motors_0_set_pos_setpoint_func),
Endpoint::make_property("pos_setpoint", &motors[0].set_pos_setpoint_args.pos_setpoint),
Endpoint::make_property("vel_feed_forward", &motors[0].set_pos_setpoint_args.vel_feed_forward),
Endpoint::make_property("current_feed_forward", &motors[0].set_pos_setpoint_args.current_feed_forward),
Endpoint::close_tree(),
Endpoint::make_function("set_vel_setpoint", &motors_0_set_vel_setpoint_func),
Endpoint::make_property("vel_setpoint", &motors[0].set_vel_setpoint_args.vel_setpoint),
Endpoint::make_property("current_feed_forward", &motors[0].set_vel_setpoint_args.current_feed_forward),
Endpoint::close_tree(),
Endpoint::make_function("set_current_setpoint", &motors_0_set_current_setpoint_func),
Endpoint::make_property("current_setpoint", &motors[0].set_current_setpoint_args.current_setpoint),
Endpoint::close_tree(),
Endpoint::close_tree(), // motor0
Endpoint::make_object("motor1"),
Endpoint::make_property("control_mode", reinterpret_cast<int32_t*>(&motors[1].control_mode)),
Endpoint::make_property("error", reinterpret_cast<int32_t*>(&motors[1].error)),
Endpoint::make_property("pos_setpoint", &motors[1].pos_setpoint),
Endpoint::make_property("pos_gain", &motors[1].pos_gain),
Endpoint::make_property("vel_setpoint", &motors[1].vel_setpoint),
Endpoint::make_property("vel_gain", &motors[1].vel_gain),
Endpoint::make_property("vel_integrator_gain", &motors[1].vel_integrator_gain),
Endpoint::make_property("vel_integrator_current", &motors[1].vel_integrator_current),
Endpoint::make_property("vel_limit", &motors[1].vel_limit),
Endpoint::make_property("current_setpoint", &motors[1].current_setpoint),
Endpoint::make_property("calibration_current", &motors[1].calibration_current),
Endpoint::make_property("phase_inductance", const_cast<const float*>(&motors[1].phase_inductance)),
Endpoint::make_property("phase_resistance", const_cast<const float*>(&motors[1].phase_resistance)),
Endpoint::make_property("current_meas.phB", const_cast<const float*>(&motors[1].current_meas.phB)),
Endpoint::make_property("current_meas.phC", const_cast<const float*>(&motors[1].current_meas.phC)),
Endpoint::make_property("DC_calib.phB", &motors[1].DC_calib.phB),
Endpoint::make_property("DC_calib.phC", &motors[1].DC_calib.phC),
Endpoint::make_property("shunt_conductance", &motors[1].shunt_conductance),
Endpoint::make_property("phase_current_rev_gain", &motors[1].phase_current_rev_gain),
Endpoint::make_property("thread_ready", reinterpret_cast<uint8_t*>(&motors[1].thread_ready)),
Endpoint::make_property("control_deadline", &motors[1].control_deadline),
Endpoint::make_property("last_cpu_time", &motors[1].last_cpu_time),
Endpoint::make_object("current_control"),
Endpoint::make_property("current_lim", &motors[1].current_control.current_lim),
Endpoint::make_property("p_gain", &motors[1].current_control.p_gain),
Endpoint::make_property("i_gain", &motors[1].current_control.i_gain),
Endpoint::make_property("v_current_control_integral_d", &motors[1].current_control.v_current_control_integral_d),
Endpoint::make_property("v_current_control_integral_q", &motors[1].current_control.v_current_control_integral_q),
Endpoint::make_property("Ibus", const_cast<const float*>(&motors[1].current_control.Ibus)),
Endpoint::close_tree(),
Endpoint::make_object("encoder"),
Endpoint::make_property("phase", const_cast<const float*>(&motors[1].encoder.phase)),
Endpoint::make_property("pll_pos", &motors[1].encoder.pll_pos),
Endpoint::make_property("pll_vel", &motors[1].encoder.pll_vel),
Endpoint::make_property("pll_kp", &motors[1].encoder.pll_kp),
Endpoint::make_property("pll_ki", &motors[1].encoder.pll_ki),
Endpoint::make_property("encoder_offset", reinterpret_cast<int32_t*>(&motors[1].encoder.encoder_offset)),
Endpoint::make_property("encoder_state", reinterpret_cast<int32_t*>(&motors[1].encoder.encoder_state)),
Endpoint::close_tree(),
Endpoint::make_function("set_pos_setpoint", &motors_1_set_pos_setpoint_func),
Endpoint::make_property("pos_setpoint", &motors[1].set_pos_setpoint_args.pos_setpoint),
Endpoint::make_property("vel_feed_forward", &motors[1].set_pos_setpoint_args.vel_feed_forward),
Endpoint::make_property("current_feed_forward", &motors[1].set_pos_setpoint_args.current_feed_forward),
Endpoint::close_tree(),
Endpoint::make_function("set_vel_setpoint", &motors_1_set_vel_setpoint_func),
Endpoint::make_property("vel_setpoint", &motors[1].set_vel_setpoint_args.vel_setpoint),
Endpoint::make_property("current_feed_forward", &motors[1].set_vel_setpoint_args.current_feed_forward),
Endpoint::close_tree(),
Endpoint::make_function("set_current_setpoint", &motors_1_set_current_setpoint_func),
Endpoint::make_property("current_setpoint", &motors[1].set_current_setpoint_args.current_setpoint),
Endpoint::close_tree(),
Endpoint::close_tree() // motor1
};
// clang-format on
constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]);
#if defined(USB_PROTOCOL_NATIVE)
class USBSender : public PacketSink {
public:
int process_packet(const uint8_t* buffer, size_t length) {
// cannot send partial packets
if (length > USB_TX_DATA_SIZE)
return -1;
// wait for USB interface to become ready
if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK)
return -1;
// transmit packet
uint8_t status = CDC_Transmit_FS(
const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
well... it's not actually. Stupid STM. */, length);
return (status == USBD_OK) ? 0 : -1;
}
} usb_sender;
BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_sender);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
class USBSender : public StreamSink {
public:
int process_bytes(const uint8_t* buffer, size_t length) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
// wait for USB interface to become ready
if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK)
return -1;
// transmit chunk
if (CDC_Transmit_FS(
const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
well... it's not actually. Stupid STM. */, chunk) != USBD_OK)
return -1;
buffer += chunk;
length -= chunk;
}
return 0;
}
size_t get_free_space() { return SIZE_MAX; }
} usb_sender;
PacketToStreamConverter usb_packet_sender(usb_sender);
BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender);
StreamToPacketConverter usb_stream_sink(usb_channel);
#endif
#if defined(UART_PROTOCOL_NATIVE)
class UART4Sender : public StreamSink {
public:
int process_bytes(const uint8_t* buffer, size_t length) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data
if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
return -1;
// transmit chunk
memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
return -1;
buffer += chunk;
length -= chunk;
}
return 0;
}
size_t get_free_space() { return SIZE_MAX; }
private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart4_sender;
PacketToStreamConverter uart4_packet_sender(uart4_sender);
BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender);
StreamToPacketConverter UART4_stream_sink(uart4_channel);
#endif
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
void init_communication(void) {
switch (gpio_mode) {
case GPIO_MODE_NONE:
break; //do nothing
case GPIO_MODE_UART: {
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
SetGPIO12toUART();
#endif
} break;
case GPIO_MODE_STEP_DIR: {
SetGPIO12toStepDir();
} break;
default:
//TODO: report error unexpected mode
break;
}
}
// Thread to handle deffered processing of USB interrupt, and
// read commands out of the UART DMA circular buffer
void communication_task(void const * argument) {
(void) argument;
#if !defined(UART_PROTOCOL_NONE)
//DMA open loop continous circular buffer
//1ms delay periodic, chase DMA ptr around
#define UART_RX_BUFFER_SIZE 64
static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE];
// DMA is set up to recieve in a circular buffer forever.
// We dont use interrupts to fetch the data, instead we periodically read
// data out of the circular buffer into a parse buffer, controlled by a state machine
HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer));
uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
#endif
// Re-run state-machine forever
for (;;) {
#if !defined(UART_PROTOCOL_NONE)
// Check for UART errors and restart recieve DMA transfer if required
if (huart4.ErrorCode != HAL_UART_ERROR_NONE) {
HAL_UART_AbortReceive(&huart4);
HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer));
}
// Fetch the circular buffer "write pointer", where it would write next
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
#if defined(UART_PROTOCOL_NATIVE)
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < last_rcv_idx) {
UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx,
UART_RX_BUFFER_SIZE - last_rcv_idx);
last_rcv_idx = 0;
}
if (new_rcv_idx > last_rcv_idx) {
UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx,
new_rcv_idx - last_rcv_idx);
last_rcv_idx = new_rcv_idx;
}
#elif defined(UART_PROTOCOL_LEGACY)
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < last_rcv_idx) {
legacy_parse_stream(dma_circ_buffer + last_rcv_idx,
UART_RX_BUFFER_SIZE - last_rcv_idx);
last_rcv_idx = 0;
}
if (new_rcv_idx > last_rcv_idx) {
legacy_parse_stream(dma_circ_buffer + last_rcv_idx,
new_rcv_idx - last_rcv_idx);
last_rcv_idx = new_rcv_idx;
}
#endif
#endif
#if !defined(USB_PROTOCOL_NONE)
// When we reach here, we are out of immediate characters to fetch out of UART buffer
// Now we check if there is any USB processing to do: we wait for up to 1 ms,
// before going back to checking UART again.
const uint32_t usb_check_timeout = 1; // ms
osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout);
if (sem_stat == osOK) {
deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
#if defined(USB_PROTOCOL_NATIVE)
usb_channel.process_packet(usb_buf, usb_len);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
usb_stream_sink.process_bytes(usb_buf, usb_len);
#elif defined(USB_PROTOCOL_LEGACY)
legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB);
#endif
USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet
}
#endif
}
// If we get here, then this task is done
vTaskDelete(osThreadGetId());
}
// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the
// incoming USB data
void set_cmd_buffer(uint8_t *buf, uint32_t len) {
usb_buf = buf;
usb_len = len;
}
void usb_update_thread() {
for (;;) {
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever);
if (semaphore_status == osOK) {
// We have a new incoming USB transmission: handle it
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
// Let the irq (OTG_FS_IRQHandler) fire again.
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
}
}
vTaskDelete(osThreadGetId());
}
Firmware/MotorControl/commands.h
+32 -19
View File
@@ -1,9 +1,29 @@
#ifndef COMMANDS_H
#define COMMANDS_H
/* Includes ------------------------------------------------------------------*/
#include <low_level.h>
/* Exported types ------------------------------------------------------------*/
// TODO: resolve assert
#define assert(expr)
#ifdef __cplusplus
#include <functional>
#include <limits>
#include "crc.hpp"
// Select which protocol to run on USB (see README for more details)
#define USB_PROTOCOL_NATIVE
// #define USB_PROTOCOL_NATIVE_STREAM_BASED
// #define USB_PROTOCOL_LEGACY
// #define USB_PROTOCOL_NONE
// Select which protocol to run on UART (see README for more details)
// #define UART_PROTOCOL_NATIVE
// #define UART_PROTOCOL_LEGACY
#define UART_PROTOCOL_NONE
// Use GPIO 1/2 for step/dir input instead of UART
// #define USE_GPIO_MODE_STEP_DIR
typedef enum {
GPIO_MODE_NONE,
@@ -11,24 +31,17 @@ typedef enum {
GPIO_MODE_STEP_DIR,
} GpioMode_t;
typedef enum {
SERIAL_PRINTF_IS_NONE,
SERIAL_PRINTF_IS_USB,
SERIAL_PRINTF_IS_UART,
} SerialPrintf_t;
/* Exported constants --------------------------------------------------------*/
/* Exported variables --------------------------------------------------------*/
extern SerialPrintf_t serial_printf_select;
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
void init_communication();
void cmd_parse_thread(void const * argument);
void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface);
extern "C" {
#endif
void init_communication(void);
void communication_task(void const * argument);
void set_cmd_buffer(uint8_t *buf, uint32_t len);
void usb_update_thread();
void USB_receive_packet(const uint8_t *buffer, size_t length);
#ifdef __cplusplus
}
#endif
#endif /* COMMANDS_H */
Firmware/MotorControl/crc.hpp
+66
View File
@@ -0,0 +1,66 @@
#ifndef __CRC_HPP
#define __CRC_HPP
#include <limits.h>
// Default CRC-8 Polynomial: x^8 + x^5 + x^4 + x^2 + x + 1
// Can protect a 4 byte payload against toggling of up to 5 bits
// source: https://users.ece.cmu.edu/~koopman/crc/index.html
constexpr uint8_t CRC8_DEFAULT = 0x37;
// Default CRC-16 Polynomial: 0x9eb2 x^16 + x^13 + x^12 + x^11 + x^10 + x^8 + x^6 + x^5 + x^2 + 1
// Can protect a 135 byte payload against toggling of up to 5 bits
// source: https://users.ece.cmu.edu/~koopman/crc/index.html
// Also known as CRC-16-DNP
constexpr uint16_t CRC16_DEFAULT = 0x3d65;
// Calculates an arbitrary CRC for one byte.
// Adapted from https://barrgroup.com/Embedded-Systems/How-To/CRC-Calculation-C-Code
template<typename T, unsigned POLYNOMIAL>
static T calc_crc(T remainder, uint8_t value) {
constexpr T BIT_WIDTH = (CHAR_BIT * sizeof(T));
constexpr T TOPBIT = ((T)1 << (BIT_WIDTH - 1));
// Bring the next byte into the remainder.
remainder ^= (value << (BIT_WIDTH - 8));
// Perform modulo-2 division, a bit at a time.
for (uint8_t bit = 8; bit; --bit) {
if (remainder & TOPBIT) {
remainder = (remainder << 1) ^ POLYNOMIAL;
} else {
remainder = (remainder << 1);
}
}
return remainder;
}
template<typename T, unsigned POLYNOMIAL>
static T calc_crc(T remainder, const uint8_t* buffer, size_t length) {
while (length--)
remainder = calc_crc<T, POLYNOMIAL>(remainder, *(buffer++));
return remainder;
}
template<unsigned POLYNOMIAL = CRC8_DEFAULT>
static uint8_t calc_crc8(uint8_t remainder, uint8_t value) {
return calc_crc<uint8_t, POLYNOMIAL>(remainder, value);
}
template<unsigned POLYNOMIAL = CRC16_DEFAULT>
static uint16_t calc_crc16(uint16_t remainder, uint8_t value) {
return calc_crc<uint16_t, POLYNOMIAL>(remainder, value);
}
template<unsigned POLYNOMIAL = CRC8_DEFAULT>
static uint8_t calc_crc8(uint8_t remainder, const uint8_t* buffer, size_t length) {
return calc_crc<uint8_t, POLYNOMIAL>(remainder, buffer, length);
}
template<unsigned POLYNOMIAL = CRC16_DEFAULT>
static uint16_t calc_crc16(uint16_t remainder, const uint8_t* buffer, size_t length) {
return calc_crc<uint16_t, POLYNOMIAL>(remainder, buffer, length);
}
#endif /* __CRC_HPP */
Firmware/MotorControl/commands.c → Firmware/MotorControl/legacy_commands.c
+63 -156
View File
@@ -1,14 +1,7 @@
/* Includes ------------------------------------------------------------------*/
#include <cmsis_os.h>
#include <commands.h>
#include <usart.h>
#include <gpio.h>
#include <freertos_vars.h>
#include <usbd_cdc.h>
#include "legacy_commands.h"
#include <utils.h>
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
@@ -16,21 +9,13 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
// This automatically updates to the interface that most
// recently recieved a command. In the future we may want to separate
// debug printf and the main serial comms.
SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_NONE;
SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_UART;
/* Private constant data -----------------------------------------------------*/
// TODO: make command to switch gpio_mode during run-time
static const GpioMode_t gpio_mode = GPIO_MODE_NONE; //GPIO 1,2 is not configured
// static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx
// static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir
static uint8_t* usb_buf;
static uint32_t usb_len;
extern USBD_HandleTypeDef hUsbDeviceFS;
// variables exposed to usb/serial interface via set/get/monitor
// Note: this will be depricated soon
static float* const exposed_floats[] = {
float* exposed_floats[] = {
&vbus_voltage, // ro
NULL, //&elec_rad_per_enc, // ro
&motors[0].pos_setpoint, // rw
@@ -91,29 +76,30 @@ static float* const exposed_floats[] = {
&motors[1].encoder.pll_ki, // rw
};
static int* const exposed_ints[] = {
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[0].encoder.encoder_offset, // rw
(int*)&motors[0].encoder.encoder_state, // ro
(int*)&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
(int*)&motors[1].encoder.encoder_offset, // rw
(int*)&motors[1].encoder.encoder_state, // ro
(int*)&motors[1].error, // rw
};
static bool* const exposed_bools[] = {
bool* exposed_bools[] = {
&motors[0].thread_ready, // ro
&motors[0].enable_control, // rw
&motors[0].do_calibration, // rw
&motors[0].calibration_ok, // ro
//For now these are written by Axis::SetupLegacyMappings
NULL, // &motors[0].enable_control, // rw
NULL, // &motors[0].do_calibration, // rw
NULL, // &motors[0].calibration_ok, // ro
&motors[1].thread_ready, // ro
&motors[1].enable_control, // rw
&motors[1].do_calibration, // rw
&motors[1].calibration_ok, // ro
NULL, // &motors[1].enable_control, // rw
NULL, // &motors[1].do_calibration, // rw
NULL, // &motors[1].calibration_ok, // ro
};
static uint16_t* const exposed_uint16[] = {
uint16_t* exposed_uint16[] = {
&motors[0].control_deadline, // rw
&motors[0].last_cpu_time, // ro
&motors[1].control_deadline, // rw
@@ -126,29 +112,14 @@ monitoring_slot monitoring_slots[20] = {0};
static void print_monitoring(int limit);
/* Function implementations --------------------------------------------------*/
void init_communication() {
switch (gpio_mode) {
case GPIO_MODE_NONE:
break; //do nothing
case GPIO_MODE_UART: {
SetGPIO12toUART();
} break;
case GPIO_MODE_STEP_DIR: {
SetGPIO12toStepDir();
} break;
default:
//TODO: report error unexpected mode
break;
}
}
void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface) {
void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_capacity, SerialPrintf_t response_interface) {
// Set response interface
serial_printf_select = response_interface;
// 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-1] = 0;
// Cast away const and write beyond the array bounds. Because we can.
// (TODO: yeah maybe not, but this should be gone once we disable legacy commands)
((uint8_t *)buffer)[len < buffer_capacity ? len : (buffer_capacity - 1)] = 0;
// check incoming packet type
if (buffer[0] == 'p') {
@@ -261,9 +232,47 @@ void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface
}
}
}
serial_printf_select = SERIAL_PRINTF_IS_UART;
}
void legacy_parse_stream(const uint8_t* buffer, size_t len) {
#define PARSE_BUFFER_SIZE 64
static uint8_t parse_buffer[PARSE_BUFFER_SIZE];
static bool read_active = false;
static uint32_t parse_buffer_idx = 0;
while (len--) {
// Fetch the next char
uint8_t c = *(buffer++);
// Look for start character
if (c == '$') {
read_active = true;
continue; // do not record start char
}
// Record into parse buffer when actively reading
if (read_active) {
parse_buffer[parse_buffer_idx++] = c;
if (c == '\r' || c == '\n' || c == '!') {
// End of command string
legacy_parse_cmd(parse_buffer, parse_buffer_idx, PARSE_BUFFER_SIZE, SERIAL_PRINTF_IS_UART);
// Reset receieve state machine
read_active = false;
parse_buffer_idx = 0;
} else if (parse_buffer_idx == PARSE_BUFFER_SIZE - 1) {
// We are not at end of command, and receiving another character after this
// would go into the last slot, which is reserved for terminating null.
// We have effectively overflowed parse buffer: abort.
read_active = false;
parse_buffer_idx = 0;
}
}
}
}
static void print_monitoring(int limit) {
serial_printf_select = SERIAL_PRINTF_IS_USB;
for (int i=0;i<limit;i++) {
switch (monitoring_slots[i].type) {
case 0:
@@ -283,108 +292,6 @@ static void print_monitoring(int limit) {
}
}
printf("\n");
}
// Thread to handle deffered processing of USB interrupt, and
// read commands out of the UART DMA circular buffer
void cmd_parse_thread(void const * argument) {
//DMA open loop continous circular buffer
//1ms delay periodic, chase DMA ptr around, on new data:
// Check for start char
// copy into parse-buffer
// check for end-char
// checksum, etc.
#define UART_RX_BUFFER_SIZE 64
static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE];
static uint8_t parse_buffer[UART_RX_BUFFER_SIZE];
// DMA is set up to recieve in a circular buffer forever.
// We dont use interrupts to fetch the data, instead we periodically read
// data out of the circular buffer into a parse buffer, controlled by a state machine
HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer));
uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
// Re-run state-machine forever
for (;;) {
//Inialize recieve state machine
bool reset_read_state = false;
bool read_active = false;
uint32_t parse_buffer_idx = 0;
//Run state machine until reset
do {
// Check for UART errors and restart recieve DMA transfer if required
if (huart4.ErrorCode != HAL_UART_ERROR_NONE) {
HAL_UART_AbortReceive(&huart4);
HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer));
break; //reset state machine
}
// Fetch the circular buffer "write pointer", where it would write next
uint32_t rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
// During sleeping, we may have fallen several characters behind, so we keep
// going until we are caught up, before we sleep again
while (rcv_idx != last_rcv_idx) {
// Fetch the next char, rotate read ptr
uint8_t c = dma_circ_buffer[last_rcv_idx];
if (++last_rcv_idx == UART_RX_BUFFER_SIZE)
last_rcv_idx = 0;
// Look for start character
if (c == '$') {
read_active = true;
continue; // do not record start char
}
// Record into parse buffer when actively reading
if (read_active) {
parse_buffer[parse_buffer_idx++] = c;
if (c == '\r' || c == '\n' || c == '!') {
// End of command string: exchange end char with terminating null
parse_buffer[parse_buffer_idx-1] = '\0';
motor_parse_cmd(parse_buffer, parse_buffer_idx, SERIAL_PRINTF_IS_UART);
// Reset receieve state machine
reset_read_state = true;
break;
} else if (parse_buffer_idx == UART_RX_BUFFER_SIZE - 1) {
// We are not at end of command, and receiving another character after this
// would go into the last slot, which is reserved for terminating null.
// We have effectively overflowed parse buffer: abort.
reset_read_state = true;
break;
}
}
}
// When we reach here, we are out of immediate characters to fetch out of UART buffer
// Now we check if there is any USB processing to do: we wait for up to 1 ms,
// before going back to checking UART again.
const uint32_t usb_check_timeout = 1; // ms
osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout);
if (sem_stat == osOK) {
motor_parse_cmd(usb_buf, usb_len, SERIAL_PRINTF_IS_USB);
USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet
}
} while (!reset_read_state);
}
// If we get here, then this task is done
vTaskDelete(osThreadGetId());
}
// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the
// incoming USB data
void set_cmd_buffer(uint8_t *buf, uint32_t len) {
usb_buf = buf;
usb_len = len;
}
void usb_update_thread() {
for (;;) {
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever);
if (semaphore_status == osOK) {
// We have a new incoming USB transmission: handle it
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
// Let the irq (OTG_FS_IRQHandler) fire again.
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
}
}
vTaskDelete(osThreadGetId());
serial_printf_select = SERIAL_PRINTF_IS_UART;
}
Firmware/MotorControl/legacy_commands.h
+37
View File
@@ -0,0 +1,37 @@
#ifndef LEGACY_COMMANDS_H
#define LEGACY_COMMANDS_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "low_level.h"
/* Exported types ------------------------------------------------------------*/
typedef enum {
SERIAL_PRINTF_IS_NONE,
SERIAL_PRINTF_IS_USB,
SERIAL_PRINTF_IS_UART,
} SerialPrintf_t;
/* Exported constants --------------------------------------------------------*/
/* Exported variables --------------------------------------------------------*/
extern SerialPrintf_t serial_printf_select;
// Exposed comms table during refactor transition
extern float* exposed_floats[];
extern int* exposed_ints[];
extern bool* exposed_bools[];
extern uint16_t* exposed_uint16[];
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_length, SerialPrintf_t response_interface);
void legacy_parse_stream(const uint8_t* buffer, size_t len);
#ifdef __cplusplus
}
#endif
#endif /* LEGACY_COMMANDS_H */
Firmware/MotorControl/low_level.c
+56 -140
View File
@@ -23,7 +23,6 @@
/* Private defines -----------------------------------------------------------*/
#define STANDALONE_MODE // Drive operates without USB communication
// #define DEBUG_PRINT
/* Private macros ------------------------------------------------------------*/
@@ -37,7 +36,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);
#if HW_VERSION_MAJOR == 3
#if HW_VERSION_MINOR <= 3
@@ -72,9 +71,9 @@ Motor_t motors[] = {
.phase_resistance = 0.0f, // to be set by measure_phase_resistance
.motor_thread = 0,
.thread_ready = false,
.enable_control = true,
.do_calibration = true,
.calibration_ok = false,
// .enable_control = true,
// .do_calibration = true,
// .calibration_ok = false,
.motor_timer = &htim1,
.next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2},
.control_deadline = TIM_1_8_PERIOD_CLOCKS,
@@ -113,7 +112,10 @@ Motor_t motors[] = {
// .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS,
.rotor_mode = ROTOR_MODE_ENCODER,
.encoder = {
.encoder_timer = &htim3, .encoder_offset = 0, .encoder_state = 0,
.encoder_timer = &htim3,
.encoder_cpr = ENCODER_CPR,
.encoder_offset = 0,
.encoder_state = 0,
.motor_dir = 0, // set by calib_enc_offset
.phase = 0.0f, // [rad]
.pll_pos = 0.0f, // [rad]
@@ -165,9 +167,9 @@ Motor_t motors[] = {
.phase_resistance = 0.0f, // to be set by measure_phase_resistance
.motor_thread = 0,
.thread_ready = false,
.enable_control = true,
.do_calibration = true,
.calibration_ok = false,
// .enable_control = true,
// .do_calibration = true,
// .calibration_ok = false,
.motor_timer = &htim8,
.next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2},
.control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2,
@@ -204,7 +206,10 @@ Motor_t motors[] = {
},
.rotor_mode = ROTOR_MODE_ENCODER,
.encoder = {
.encoder_timer = &htim4, .encoder_offset = 0, .encoder_state = 0,
.encoder_timer = &htim4,
.encoder_cpr = ENCODER_CPR,
.encoder_offset = 0,
.encoder_state = 0,
.motor_dir = 0, // set by calib_enc_offset
.phase = 0.0f, // [rad]
.pll_pos = 0.0f, // [rad]
@@ -239,7 +244,7 @@ Motor_t motors[] = {
}
}
};
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;
@@ -250,45 +255,10 @@ static const int current_meas_hz = CURRENT_MEAS_HZ;
/* Private variables ---------------------------------------------------------*/
static float brake_resistance = 0.47f; // [ohm]
/* Private function prototypes -----------------------------------------------*/
// Command Handling
// Utility
static uint16_t check_timing(Motor_t* motor);
static void global_fault(int error);
static float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue);
// Initalisation
static void DRV8301_setup(Motor_t* motor);
static void start_adc_pwm();
static void start_pwm(TIM_HandleTypeDef* htim);
static void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset);
// IRQ Callbacks (are all public)
// Measurement and calibrationa
static bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage);
static bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high);
static bool calib_enc_offset(Motor_t* motor, float voltage_magnitude);
static bool motor_calibration(Motor_t* motor);
// Test functions
static void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude);
static void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q);
// Main motor control
static void update_rotor(Motor_t* motor);
static float get_rotor_phase(Motor_t* motor);
static float get_pll_vel(Motor_t* motor);
static bool spin_up_sensorless(Motor_t* motor);
static void update_brake_current(float brake_current);
static void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta);
static void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta);
static bool FOC_current(Motor_t* motor, float Id_des, float Iq_des);
static void control_motor_loop(Motor_t* motor);
// Motor thread (is public)
/* Function implementations --------------------------------------------------*/
//--------------------------------
// Command Handling
// TODO move to different file
//--------------------------------
void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) {
@@ -322,7 +292,7 @@ void set_current_setpoint(Motor_t* motor, float current_setpoint) {
// Utility
//--------------------------------
static uint16_t check_timing(Motor_t* motor) {
uint16_t check_timing(Motor_t* motor) {
TIM_HandleTypeDef* htim = motor->motor_timer;
uint16_t timing = htim->Instance->CNT;
bool down = htim->Instance->CR1 & TIM_CR1_DIR;
@@ -339,7 +309,7 @@ static uint16_t check_timing(Motor_t* motor) {
return timing;
}
static void global_fault(int error) {
void global_fault(int error) {
// Disable motors NOW!
for (int i = 0; i < num_motors; ++i) {
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motors[i].motor_timer);
@@ -347,14 +317,13 @@ static void global_fault(int error) {
// Set fault codes, etc.
for (int i = 0; i < num_motors; ++i) {
motors[i].error = error;
motors[i].enable_control = false;
motors[i].calibration_ok = false;
*(motors[i].axis_legacy.enable_control) = false;
}
// disable brake resistor
update_brake_current(0.0f);
}
static float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) {
float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) {
int adcval_bal = (int)ADCValue - (1 << 11);
float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal;
float shunt_volt = amp_out_volt * motor->phase_current_rev_gain;
@@ -385,7 +354,7 @@ void init_motor_control() {
}
// Set up the gate drivers
static void DRV8301_setup(Motor_t* motor) {
void DRV8301_setup(Motor_t* motor) {
DRV8301_Obj* gate_driver = &motor->gate_driver;
DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs;
@@ -429,7 +398,7 @@ static void DRV8301_setup(Motor_t* motor) {
DRV8301_readData(gate_driver, local_regs);
}
static void start_adc_pwm() {
void start_adc_pwm() {
// Enable ADC and interrupts
__HAL_ADC_ENABLE(&hadc1);
__HAL_ADC_ENABLE(&hadc2);
@@ -462,7 +431,7 @@ static void start_adc_pwm() {
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
}
static void start_pwm(TIM_HandleTypeDef* htim) {
void start_pwm(TIM_HandleTypeDef* htim) {
// Init PWM
int half_load = TIM_1_8_PERIOD_CLOCKS / 2;
htim->Instance->CCR1 = half_load;
@@ -481,8 +450,8 @@ static void start_pwm(TIM_HandleTypeDef* htim) {
HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4);
}
static void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) {
void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) {
// Store intial timer configs
uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE);
uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE);
@@ -639,7 +608,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) {
@@ -666,7 +635,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
//--------------------------------
// TODO check Ibeta balance to verify good motor connection
static bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) {
bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) {
static const float kI = 10.0f; //[(V/s)/A]
static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s
float test_voltage = 0.0f;
@@ -704,7 +673,7 @@ static bool measure_phase_resistance(Motor_t* motor, float test_current, float m
return true;
}
static bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high) {
bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high) {
float test_voltages[2] = {voltage_low, voltage_high};
float Ialphas[2] = {0.0f};
static const int num_cycles = 5000;
@@ -749,7 +718,7 @@ static bool measure_phase_inductance(Motor_t* motor, float voltage_low, float vo
// TODO: Do the scan with current, not voltage!
// TODO: add check_timing
static bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) {
bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) {
static const float start_lock_duration = 1.0f;
static const int num_steps = 1024;
static const float dt_step = 1.0f / 500.0f;
@@ -811,8 +780,7 @@ static bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) {
return true;
}
static bool motor_calibration(Motor_t* motor) {
motor->calibration_ok = false;
bool motor_calibration(Motor_t* motor) {
motor->error = ERROR_NO_ERROR;
// #warning(hardcoded values for SK3-5065-280kv!)
@@ -850,7 +818,6 @@ static bool motor_calibration(Motor_t* motor) {
motor->sensorless.pll_kp = motor->encoder.pll_kp;
motor->sensorless.pll_ki = motor->encoder.pll_ki;
motor->calibration_ok = true;
return true;
}
@@ -886,7 +853,7 @@ bool anti_cogging_calibration(Motor_t* motor) {
// Test functions
//--------------------------------
__attribute__((unused)) static void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) {
__attribute__((unused)) void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) {
for (;;) {
for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) {
osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever);
@@ -905,7 +872,7 @@ __attribute__((unused)) static void scan_motor_loop(Motor_t* motor, float omega,
}
//TODO integrate as mode in main control loop
__attribute__((unused)) static void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) {
__attribute__((unused)) void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) {
for (;;) {
osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever);
update_rotor(motor);
@@ -930,7 +897,7 @@ __attribute__((unused)) static void FOC_voltage_loop(Motor_t* motor, float v_d,
// Main motor control
//--------------------------------
static void update_rotor(Motor_t* motor) {
void update_rotor(Motor_t* motor) {
switch (motor->rotor_mode) {
case ROTOR_MODE_ENCODER:
case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS: {
@@ -1046,7 +1013,7 @@ static void update_rotor(Motor_t* motor) {
}
}
static bool using_encoder(Motor_t* motor) {
bool using_encoder(Motor_t* motor) {
if (motor->rotor_mode == ROTOR_MODE_ENCODER ||
motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS)
return true;
@@ -1054,14 +1021,14 @@ static bool using_encoder(Motor_t* motor) {
return false;
}
static bool using_sensorless(Motor_t* motor) {
bool using_sensorless(Motor_t* motor) {
if (motor->rotor_mode == ROTOR_MODE_SENSORLESS)
return true;
else
return false;
}
static float get_rotor_phase(Motor_t* motor) {
float get_rotor_phase(Motor_t* motor) {
if (using_encoder(motor))
return motor->encoder.phase;
else if (using_sensorless(motor))
@@ -1071,7 +1038,7 @@ static float get_rotor_phase(Motor_t* motor) {
return 0.0f;
}
static float get_pll_vel(Motor_t* motor) {
float get_pll_vel(Motor_t* motor) {
if (using_encoder(motor))
return motor->encoder.pll_vel;
else if (using_sensorless(motor))
@@ -1081,7 +1048,18 @@ static float get_pll_vel(Motor_t* motor) {
return 0.0f;
}
static bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) {
// Function that sets the current encoder count to a desired 32-bit value.
void setEncoderCount(Motor_t* motor, uint32_t count) {
// Disable interrupts to make a critical section to avoid race condition
uint32_t prim = __get_PRIMASK();
__disable_irq();
motor->encoder.encoder_state = count;
motor->motor_timer->Instance->CNT = count;
motor->encoder.pll_pos = (float)count;
__set_PRIMASK(prim);
}
bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) {
// wait for new timestep
if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) {
motor->error = ERROR_SPIN_UP_TIMEOUT;
@@ -1097,7 +1075,7 @@ static bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) {
return true;
}
static bool spin_up_sensorless(Motor_t* motor) {
bool spin_up_sensorless(Motor_t* motor) {
static const float ramp_up_time = 0.4f;
static const float ramp_up_distance = 4 * M_PI;
float ramp_step = current_meas_period / ramp_up_time;
@@ -1134,7 +1112,7 @@ static bool spin_up_sensorless(Motor_t* motor) {
// TODO: check pll vel (abs ratio, 0.8)
}
static void update_brake_current(float brake_current) {
void update_brake_current(float brake_current) {
if (brake_current < 0.0f) brake_current = 0.0f;
float brake_duty = brake_current * brake_resistance / vbus_voltage;
@@ -1153,7 +1131,7 @@ static void update_brake_current(float brake_current) {
htim2.Instance->CCR4 = high_on;
}
static void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta) {
void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta) {
float tA, tB, tC;
SVM(mod_alpha, mod_beta, &tA, &tB, &tC);
motor->next_timings[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
@@ -1161,14 +1139,14 @@ static void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_
motor->next_timings[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS);
}
static void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) {
void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) {
float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage);
float mod_alpha = vfactor * v_alpha;
float mod_beta = vfactor * v_beta;
queue_modulation_timings(motor, mod_alpha, mod_beta);
}
static bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) {
bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) {
Current_control_t* ictrl = &motor->current_control;
// Clarke transform
@@ -1246,8 +1224,8 @@ static bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) {
return true;
}
static void control_motor_loop(Motor_t* motor) {
while (motor->enable_control) {
void control_motor_loop(Motor_t* motor) {
while (*(motor->axis_legacy.enable_control)) {
if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) {
motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT;
break;
@@ -1332,65 +1310,3 @@ static void control_motor_loop(Motor_t* motor) {
//TODO update brake current from all motors in 1 func
//TODO reset this motor Ibus, then call from here
}
//--------------------------------
// Motor thread
//--------------------------------
void motor_thread(void const* argument) {
Motor_t* motor = (Motor_t*)argument;
// Allocate the map for anti-cogging algorithm and initialize all values to 0.0f
motor->anticogging.cogging_map = (float*)malloc(ENCODER_CPR * sizeof(float));
if (motor->anticogging.cogging_map != NULL) {
for (int i = 0; i < ENCODER_CPR; i++) {
motor->anticogging.cogging_map[i] = 0.0f;
}
}
motor->motor_thread = osThreadGetId();
motor->thread_ready = true;
for (;;) {
if (motor->do_calibration) {
__HAL_TIM_MOE_ENABLE(motor->motor_timer); // enable pwm outputs
motor_calibration(motor);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor->motor_timer); // disables pwm outputs
motor->do_calibration = false;
}
if (motor->calibration_ok && motor->enable_control) {
motor->enable_step_dir = true;
__HAL_TIM_MOE_ENABLE(motor->motor_timer);
bool spin_up_ok = true;
if (motor->rotor_mode == ROTOR_MODE_SENSORLESS)
spin_up_ok = spin_up_sensorless(motor);
if (spin_up_ok)
control_motor_loop(motor);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor->motor_timer);
motor->enable_step_dir = false;
if (motor->enable_control) { // if control is still enabled, we exited because of error
motor->calibration_ok = false;
motor->enable_control = false;
}
}
queue_voltage_timings(motor, 0.0f, 0.0f);
osDelay(100);
}
motor->thread_ready = false;
}
// Function that sets the current encoder count to a desired 32-bit value.
void setEncoderCount(Motor_t* motor, uint32_t count){
// Disable interrupts to make a critical section to avoid race condition
uint32_t prim = __get_PRIMASK();
__disable_irq();
motor->encoder.encoder_state = count;
motor->motor_timer->Instance->CNT = count;
motor->encoder.pll_pos = (float)count;
__set_PRIMASK(prim);
}
Firmware/MotorControl/low_level.h
+76 -11
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"
@@ -98,8 +102,9 @@ typedef struct {
typedef struct {
TIM_HandleTypeDef* encoder_timer;
int encoder_offset;
int encoder_state;
int encoder_cpr;
int32_t encoder_offset;
int32_t encoder_state;
int motor_dir; // 1/-1 for fwd/rev alignment to encoder.
float phase;
float pll_pos;
@@ -108,12 +113,17 @@ typedef struct {
float pll_ki;
} Encoder_t;
typedef struct {
bool* enable_control;
} Axis_legacy_t;
#define TIMING_LOG_SIZE 16
typedef struct {
Axis_legacy_t axis_legacy;
Motor_control_mode_t control_mode;
bool enable_step_dir;
float counts_per_step;
int error;
Error_t error;
float pos_setpoint;
float pos_gain;
float vel_setpoint;
@@ -127,9 +137,9 @@ typedef struct {
float phase_resistance;
osThreadId motor_thread;
bool thread_ready;
bool enable_control; // enable/disable via usb to start motor control. will be set to false again in case of errors.requires calibration_ok=true
bool do_calibration; // trigger motor calibration. will be reset to false after self test
bool calibration_ok;
// bool enable_control; // enable/disable via usb to start motor control. will be set to false again in case of errors.requires calibration_ok=true
// bool do_calibration; // trigger motor calibration. will be reset to false after self test
// bool calibration_ok;
TIM_HandleTypeDef* motor_timer;
uint16_t next_timings[3];
uint16_t control_deadline;
@@ -146,6 +156,19 @@ typedef struct {
Sensorless_t sensorless;
int timing_log_index;
uint16_t timing_log[TIMING_LOG_SIZE];
// Cache for remote procedure calls arguments
struct {
float pos_setpoint;
float vel_feed_forward;
float current_feed_forward;
} set_pos_setpoint_args;
struct {
float vel_setpoint;
float current_feed_forward;
} set_vel_setpoint_args;
struct {
float current_setpoint;
} set_current_setpoint_args;
Anticogging_t anticogging;
} Motor_t;
@@ -155,7 +178,8 @@ typedef struct{
} monitoring_slot;
/* Exported constants --------------------------------------------------------*/
extern const int num_motors;
extern const size_t num_motors;
extern const float elec_rad_per_enc;
/* Exported variables --------------------------------------------------------*/
extern float vbus_voltage;
extern Motor_t motors[];
@@ -167,17 +191,58 @@ void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward
void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward);
void set_current_setpoint(Motor_t* motor, float current_setpoint);
void safe_assert(int arg);
void init_motor_control();
void step_cb(uint16_t GPIO_Pin);
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
void safe_assert(int arg);
void init_motor_control();
void setEncoderCount(Motor_t* motor, uint32_t count);
bool anti_cogging_calibration(Motor_t* motor);
//@TODO move motor thread to high level file
void motor_thread(void const * argument);
bool motor_calibration(Motor_t* motor);
//// Old private:
// Utility
uint16_t check_timing(Motor_t* motor);
void global_fault(int error);
float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue);
// Initalisation
void DRV8301_setup(Motor_t* motor);
void start_adc_pwm();
void start_pwm(TIM_HandleTypeDef* htim);
void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset);
// IRQ Callbacks (are all public)
// Measurement and calibrationa
bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage);
bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high);
bool calib_enc_offset(Motor_t* motor, float voltage_magnitude);
bool anti_cogging_calibration(Motor_t* motor);
// Test functions
void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude);
void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q);
// Main motor control
void update_rotor(Motor_t* motor);
bool using_encoder(Motor_t* motor);
bool using_sensorless(Motor_t* motor);
float get_rotor_phase(Motor_t* motor);
float get_pll_vel(Motor_t* motor);
bool spin_up_sensorless(Motor_t* motor);
void update_brake_current(float brake_current);
void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta);
void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta);
bool FOC_current(Motor_t* motor, float Id_des, float Iq_des);
void control_motor_loop(Motor_t* motor);
//motor thread moved to axis object
//void motor_thread(void const * argument);
#ifdef __cplusplus
}
#endif
#endif //__LOW_LEVEL_H
Firmware/MotorControl/protocol.cpp
+246
View File
@@ -0,0 +1,246 @@
/* Includes ------------------------------------------------------------------*/
#include "low_level.h"
#include "protocol.hpp"
#include <memory>
#include <stdlib.h>
/* Private defines -----------------------------------------------------------*/
// Note that this option cannot be used to debug UART because it prints on UART
//#define DEGUG_PROTOCOL
/* Private macros ------------------------------------------------------------*/
#ifdef DEGUG_PROTOCOL
#define LOG_PROTO(...) do { printf(__VA_ARGS__); osDelay(10); } while (0)
#else
#define LOG_PROTO(...) ((void) 0)
#endif
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
static void hexdump(const uint8_t* buf, size_t len);
static inline int write_string(const char* str, StreamSink* output);
/* Function implementations --------------------------------------------------*/
#if 0
void hexdump(const uint8_t* buf, size_t len) {
for (size_t pos = 0; pos < len; ++pos) {
printf(" %02x", buf[pos]);
if ((((pos + 1) % 16) == 0) || ((pos + 1) == len))
printf("\r\n");
osDelay(2);
}
}
#else
void hexdump(const uint8_t* buf, size_t len) {
(void) buf;
(void) len;
}
#endif
static inline int write_string(const char* str, StreamSink* output) {
return output->process_bytes(reinterpret_cast<const uint8_t*>(str), strlen(str));
}
void Endpoint::write_json(size_t id, bool* need_comma, StreamSink* output) const {
if (type_ == CLOSE_TREE) {
write_string("]}", output);
*need_comma = true;
} else {
if (*need_comma)
write_string(",", output);
// write name
write_string("{\"name\":\"", output);
if (name_)
write_string(name_, output);
// write endpoint ID
write_string("\",\"id\":", output);
char id_buf[10];
snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf
write_string(id_buf, output);
// write additional JSON data
if (json_modifier_ && json_modifier_[0]) {
write_string(",", output);
write_string(json_modifier_, output);
}
if (type_ == BEGIN_OBJECT) {
write_string(",\"members\":[", output);
*need_comma = false;
} else if (type_ == BEGIN_FUNCTION) {
write_string(",\"arguments\":[", output);
*need_comma = false;
} else if (type_ == PROPERTY) {
write_string("}", output);
*need_comma = true;
}
}
}
int StreamToPacketConverter::process_bytes(const uint8_t *buffer, size_t length) {
int result = 0;
while (length--) {
if (header_index_ < sizeof(header_buffer_)) {
// Process header byte
header_buffer_[header_index_++] = *buffer;
if (header_index_ == 1 && header_buffer_[0] != SYNC_BYTE) {
header_index_ = 0;
} else if (header_index_ == 2 && (header_buffer_[1] & 0x80)) {
header_index_ = 0; // TODO: support packets larger than 128 bytes
} else if (header_index_ == 3 && calc_crc8(CRC8_INIT, header_buffer_, 3)) {
header_index_ = 0;
} else if (header_index_ == 3) {
packet_length_ = header_buffer_[1] + 2;
}
} else if (packet_index_ < sizeof(packet_buffer_)) {
// Process payload byte
packet_buffer_[packet_index_++] = *buffer;
}
// If both header and packet are fully received, hand it on to the packet processor
if (header_index_ == 3 && packet_index_ == packet_length_) {
if (calc_crc16(CRC16_INIT, packet_buffer_, packet_length_) == 0) {
result |= output_.process_packet(packet_buffer_, packet_length_ - 2);
}
header_index_ = packet_index_ = packet_length_ = 0;
}
buffer++;
}
return result;
}
int PacketToStreamConverter::process_packet(const uint8_t *buffer, size_t length) {
// TODO: support buffer size >= 128
if (length >= 128)
return -1;
LOG_PROTO("send header\r\n");
uint8_t header[] = {
SYNC_BYTE,
static_cast<uint8_t>(length),
0
};
header[2] = calc_crc8(CRC8_INIT, header, 2);
if (output_.process_bytes(header, sizeof(header)))
return -1;
LOG_PROTO("send payload:\r\n");
hexdump(buffer, length);
if (output_.process_bytes(buffer, length))
return -1;
LOG_PROTO("send crc16\r\n");
uint16_t crc16 = calc_crc16(CRC16_INIT, buffer, length);
uint8_t crc16_buffer[] = {
(uint8_t)((crc16 >> 8) & 0xff),
(uint8_t)((crc16 >> 0) & 0xff)
};
if (output_.process_bytes(crc16_buffer, 2))
return -1;
LOG_PROTO("sent!\r\n");
return 0;
}
// Calculates the CRC16 of the JSON interface descriptor.
// The init value is the protocol version.
uint16_t BidirectionalPacketBasedChannel::calculate_json_crc16(void) {
CRC16Calculator crc16_calculator(PROTOCOL_VERSION);
uint8_t offset[4] = { 0 };
interface_query(offset, sizeof(offset), &crc16_calculator);
return crc16_calculator.get_crc16();
}
// Returns part of the JSON interface definition.
void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size_t input_length, StreamSink* output) {
// The request must contain a 32 bit integer to specify an offset
if (input_length < 4)
return;
uint32_t offset = 0;
read_le<uint32_t>(&offset, input);
NullStreamSink output_with_offset = NullStreamSink(offset, *output);
bool need_comma = false;
write_string("[", &output_with_offset);
for (size_t i = 0; i < n_endpoints_; ++i) {
get_endpoint(i)->write_json(i, &need_comma, &output_with_offset);
if (!output->get_free_space())
return; // return early if the output cannot take more bytes
}
write_string("]", &output_with_offset);
}
int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_t length) {
LOG_PROTO("got packet of length %d: \r\n", length);
hexdump(buffer, length);
if (length < 4)
return -1;
uint16_t seq_no = read_le<uint16_t>(&buffer, &length);
if (seq_no & 0x8000) {
// TODO: ack handling
} else {
// TODO: think about some kind of ordering guarantees
// currently the seq_no is just used to associate a response with a request
uint16_t endpoint_id = read_le<uint16_t>(&buffer, &length);
bool expect_response = endpoint_id & 0x8000;
endpoint_id &= 0x7fff;
const Endpoint* endpoint = get_endpoint(endpoint_id);
if (!endpoint)
return -1;
// Verify packet trailer. The expected trailer value depends on the selected endpoint.
// For endpoint 0 this is just the protocol version, for all other endpoints it's a
// CRC over the entire JSON descriptor tree (this may change in future versions).
uint16_t expected_trailer = endpoint_id ? json_crc_ : PROTOCOL_VERSION;
uint16_t actual_trailer = buffer[length - 2] | (buffer[length - 1] << 8);
if (expected_trailer != actual_trailer) {
LOG_PROTO("trailer mismatch for endpoint %d: expected %04x, got %04x\r\n", endpoint_id, expected_trailer, actual_trailer);
return -1;
}
LOG_PROTO("trailer ok\r\n");
// TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible?
uint16_t expected_response_length = read_le<uint16_t>(&buffer, &length);
// Limit response length according to our local TX buffer size
if (expected_response_length > sizeof(tx_buf_) - 2)
expected_response_length = sizeof(tx_buf_) - 2;
MemoryStreamSink output(tx_buf_ + 2, expected_response_length);
endpoint->handle(buffer, length - 2, &output);
// Send response
if (expect_response) {
size_t actual_response_length = expected_response_length - output.get_free_space() + 2;
write_le<uint16_t>(seq_no | 0x8000, tx_buf_);
LOG_PROTO("send packet:\r\n");
hexdump(tx_buf_, actual_response_length);
output_.process_packet(tx_buf_, actual_response_length);
}
}
return 0;
}
Firmware/MotorControl/protocol.hpp
+472
View File
@@ -0,0 +1,472 @@
/*
see protocol.md for the protocol specification
*/
#ifndef __PROTOCOL_HPP
#define __PROTOCOL_HPP
// TODO: resolve assert
#define assert(expr)
#include <functional>
#include <limits>
#include <cstring>
#include "crc.hpp"
constexpr uint8_t SYNC_BYTE = 0xAA;
constexpr uint8_t CRC8_INIT = 0x42;
constexpr uint16_t CRC16_INIT = 0x1337;
constexpr uint16_t PROTOCOL_VERSION = 1;
// This value must not be larger than USB_TX_DATA_SIZE defined in usbd_cdc_if.h
//Oskar: What's the error? What values work? Does 63 work? Ideally we figure out how to get 64 to work, but if not let's find something better than 32.
constexpr uint16_t TX_BUF_SIZE = 32; // does not work with 64 for some reason
constexpr uint16_t RX_BUF_SIZE = 128; // larger values than 128 have currently no effect because of protocol limitations
// Maximum time we allocate for processing and responding to a request
constexpr uint32_t PROTOCOL_SERVER_TIMEOUT_MS = 10;
template<typename T>
inline size_t write_le(T value, uint8_t* buffer);
template<typename T>
inline size_t read_le(T* value, const uint8_t* buffer);
template<>
inline size_t write_le<uint8_t>(uint8_t value, uint8_t* buffer) {
buffer[0] = value;
return 2;
}
template<>
inline size_t write_le<uint16_t>(uint16_t value, uint8_t* buffer) {
buffer[0] = (value >> 0) & 0xff;
buffer[1] = (value >> 8) & 0xff;
return 2;
}
template<>
inline size_t write_le<uint32_t>(uint32_t value, uint8_t* buffer) {
buffer[0] = (value >> 0) & 0xff;
buffer[1] = (value >> 8) & 0xff;
buffer[2] = (value >> 16) & 0xff;
buffer[3] = (value >> 24) & 0xff;
return 4;
}
template<>
inline size_t write_le<int32_t>(int32_t value, uint8_t* buffer) {
buffer[0] = (value >> 0) & 0xff;
buffer[1] = (value >> 8) & 0xff;
buffer[2] = (value >> 16) & 0xff;
buffer[3] = (value >> 24) & 0xff;
return 4;
}
template<>
inline size_t write_le<float>(float value, uint8_t* buffer) {
static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
return write_le<uint32_t>(*reinterpret_cast<const uint32_t*>(&value), buffer);
}
template<>
inline size_t read_le<uint8_t>(uint8_t* value, const uint8_t* buffer) {
*value = buffer[0];
return 2;
}
template<>
inline size_t read_le<uint16_t>(uint16_t* value, const uint8_t* buffer) {
*value = (static_cast<uint16_t>(buffer[0]) << 0) |
(static_cast<uint16_t>(buffer[1]) << 8);
return 2;
}
template<>
inline size_t read_le<int32_t>(int32_t* value, const uint8_t* buffer) {
*value = (static_cast<int32_t>(buffer[0]) << 0) |
(static_cast<int32_t>(buffer[1]) << 8) |
(static_cast<int32_t>(buffer[2]) << 16) |
(static_cast<int32_t>(buffer[3]) << 24);
return 4;
}
template<>
inline size_t read_le<uint32_t>(uint32_t* value, const uint8_t* buffer) {
*value = (static_cast<uint32_t>(buffer[0]) << 0) |
(static_cast<uint32_t>(buffer[1]) << 8) |
(static_cast<uint32_t>(buffer[2]) << 16) |
(static_cast<uint32_t>(buffer[3]) << 24);
return 4;
}
template<>
inline size_t read_le<float>(float* value, const uint8_t* buffer) {
static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
return read_le(reinterpret_cast<uint32_t*>(value), buffer);
}
// @brief Reads a value of type T from the buffer.
// @param buffer Pointer to the buffer to be read. The pointer is updated by the number of bytes that were read.
// @param length The number of available bytes in buffer. This value is updated to subtract the bytes that were read.
template<typename T>
static inline T read_le(const uint8_t** buffer, size_t* length) {
T result;
size_t cnt = read_le(&result, *buffer);
*buffer += cnt;
*length -= cnt;
return result;
}
class PacketSink {
public:
// @brief Processes a packet.
// The blocking behavior shall depend on the thread-local deadline_ms variable.
// @return: 0 on success, otherwise a non-zero error code
// TODO: define what happens when the packet is larger than what the implementation can handle.
virtual int process_packet(const uint8_t* buffer, size_t length) = 0;
};
class StreamSink {
public:
// @brief Processes a chunk of bytes that is part of a continuous stream.
// The blocking behavior shall depend on the thread-local deadline_ms variable.
// @return: 0 on success, otherwise a non-zero error code
virtual int process_bytes(const uint8_t* buffer, size_t length) = 0;
// @brief Returns the number of bytes that can still be written to the stream.
// Shall return SIZE_MAX if the stream has unlimited lenght.
virtual size_t get_free_space() = 0;
};
class StreamToPacketConverter : public StreamSink {
public:
StreamToPacketConverter(PacketSink& output) :
output_(output)
{
};
int process_bytes(const uint8_t *buffer, size_t length);
size_t get_free_space() { return SIZE_MAX; }
private:
uint8_t header_buffer_[3];
size_t header_index_ = 0;
uint8_t packet_buffer_[RX_BUF_SIZE];
size_t packet_index_ = 0;
size_t packet_length_ = 0;
PacketSink& output_;
};
class PacketToStreamConverter : public PacketSink {
public:
PacketToStreamConverter(StreamSink& output) :
output_(output)
{
};
int process_packet(const uint8_t *buffer, size_t length);
private:
StreamSink& output_;
};
// Implements the StreamSink interface by writing into a fixed size
// memory buffer.
class MemoryStreamSink : public StreamSink {
public:
MemoryStreamSink(uint8_t *buffer, size_t length) :
buffer_(buffer),
buffer_length_(length) {}
// Returns 0 on success and -1 if the buffer could not accept everything because it became full
int process_bytes(const uint8_t* buffer, size_t length) {
int status = 0;
if (length > buffer_length_) {
length = buffer_length_;
status = -1;
}
memcpy(buffer_, buffer, length);
buffer_ += length;
buffer_length_ -= length;
return status;
}
size_t get_free_space() { return buffer_length_; }
private:
uint8_t * buffer_;
size_t buffer_length_;
};
// Implements the StreamSink interface by discarding the first couple of bytes
// and then forwarding the rest to another stream.
class NullStreamSink : public StreamSink {
public:
NullStreamSink(size_t skip, StreamSink& follow_up_stream) :
skip_(skip),
follow_up_stream_(follow_up_stream) {}
// Returns 0 on success and -1 if the buffer could not accept everything because it became full
int process_bytes(const uint8_t* buffer, size_t length) {
if (skip_ < length) {
buffer += skip_;
length -= skip_;
skip_ = 0;
return follow_up_stream_.process_bytes(buffer, length);
} else {
skip_ -= length;
return 0;
}
}
size_t get_free_space() { return skip_ + follow_up_stream_.get_free_space(); }
private:
size_t skip_;
StreamSink& follow_up_stream_;
};
// Implements the StreamSink interface by calculating the CRC16 checksum
// on the data that is sent to it.
class CRC16Calculator : public StreamSink {
public:
CRC16Calculator(uint16_t crc16_init) :
crc16_(crc16_init) {}
int process_bytes(const uint8_t* buffer, size_t length) {
crc16_ = calc_crc16(crc16_, buffer, length);
return 0;
}
size_t get_free_space() { return SIZE_MAX; }
uint16_t get_crc16() { return crc16_; }
private:
uint16_t crc16_;
};
typedef enum {
PROPERTY,
BEGIN_OBJECT,
BEGIN_FUNCTION,
CLOSE_TREE
} EndpointType_t;
// @brief Endpoint request handler
//
// When passed a valid endpoint context, implementing functions shall handle an
// endpoint read/write request by reading the provided input data and filling in
// output data. The exact semantics of this function depends on the corresponding
// endpoint's specification.
//
// @param input: pointer to the input data
// @param input_length: number of available input bytes
// @param output: The stream where to write the output to. Can be null.
// The handler shall abort as soon as the stream returns
// a non-zero error code on write.
typedef std::function<void(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output)> EndpointHandler;
template<typename T>
void default_read_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
const T* value = reinterpret_cast<const T*>(ctx);
// If the old value was requested, call the corresponding little endian serialization function
if (output) {
// TODO: make buffer size dependent on the type
uint8_t buffer[sizeof(T)];
size_t cnt = write_le<T>(*value, buffer);
if (cnt <= output->get_free_space())
output->process_bytes(buffer, cnt);
}
}
template<typename T>
void default_readwrite_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
T* value = reinterpret_cast<T*>(ctx);
// Read the endpoint value into output
default_read_endpoint_handler<T>(ctx, input, input_length, output);
// If a new value was passed, call the corresponding little endian deserialization function
uint8_t buffer[sizeof(T)] = { 0 }; // TODO: make buffer size dependent on the type
if (input_length >= sizeof(buffer))
read_le<T>(value, input);
}
static void trigger_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
(void) input;
(void) input_length;
(void) output;
std::function<void(void)> function = reinterpret_cast<void(*)()>(ctx);
function();
}
template<typename T>
static inline const char* get_default_json_modifier();
template<>
inline const char* get_default_json_modifier<const float>() {
return "\"type\":\"float\",\"access\":\"r\"";
}
template<>
inline const char* get_default_json_modifier<float>() {
return "\"type\":\"float\",\"access\":\"rw\"";
}
template<>
inline const char* get_default_json_modifier<const int32_t>() {
return "\"type\":\"int32\",\"access\":\"r\"";
}
template<>
inline const char* get_default_json_modifier<int32_t>() {
return "\"type\":\"int32\",\"access\":\"rw\"";
}
template<>
inline const char* get_default_json_modifier<const uint16_t>() {
return "\"type\":\"uint16\",\"access\":\"r\"";
}
template<>
inline const char* get_default_json_modifier<uint16_t>() {
return "\"type\":\"uint16\",\"access\":\"rw\"";
}
template<>
inline const char* get_default_json_modifier<const uint8_t>() {
return "\"type\":\"uint8\",\"access\":\"r\"";
}
template<>
inline const char* get_default_json_modifier<uint8_t>() {
return "\"type\":\"uint8\",\"access\":\"rw\"";
}
class Endpoint {
public:
const char* const name_;
Endpoint(const char* name, EndpointType_t type, EndpointHandler handler, const char* json_modifier, void *ctx) :
name_(name),
type_(type),
handler_(handler),
json_modifier_(json_modifier),
ctx_(ctx)
{
}
template<typename T>
static Endpoint make_property(const char* name, const T* ctx) {
return Endpoint(name, PROPERTY,
default_read_endpoint_handler<T>,
get_default_json_modifier<const T>(),
const_cast<T*>(ctx) /* it's safe to cast the const away here because we
know that the default_read_endpoint_handler immediately adds it back */);
}
template<typename T>
static Endpoint make_property(const char* name, T* ctx) {
return Endpoint(name, PROPERTY,
default_readwrite_endpoint_handler<T>,
get_default_json_modifier<T>(), ctx);
}
static Endpoint make_object(const char* name) {
return Endpoint(name, BEGIN_OBJECT, nullptr,
"\"type\":\"object\"", nullptr);
}
static Endpoint make_function(const char* name, void(*function)(void)) {
return Endpoint(name, BEGIN_FUNCTION, trigger_endpoint_handler,
"\"type\":\"function\"", reinterpret_cast<void*>(function));
}
static Endpoint close_tree() {
return Endpoint(nullptr, CLOSE_TREE, nullptr, nullptr, nullptr);
}
void write_json(size_t id, bool* need_comma, StreamSink* output) const;
void handle(const uint8_t* input, size_t input_length, StreamSink* output) const {
if (handler_)
return handler_(ctx_, input, input_length, output);
}
private:
const EndpointType_t type_;
const EndpointHandler handler_;
const char* json_modifier_;
void* const ctx_;
};
/* @brief Handles the communication protocol on one channel.
*
* When instantiated with a list of endpoints and an output packet sink,
* objects of this class will handle packets passed into process_packet,
* pass the relevant data to the corresponding endpoints and dispatch response
* packets on the output.
*/
class BidirectionalPacketBasedChannel : public PacketSink {
public:
BidirectionalPacketBasedChannel(const Endpoint* endpoints, size_t n_endpoints, PacketSink& output) :
global_endpoints_(endpoints),
n_endpoints_(NUM_CHANNEL_SPECIFIC_ENDPOINTS + n_endpoints),
output_(output),
json_crc_(calculate_json_crc16())
{
}
int process_packet(const uint8_t* buffer, size_t length);
private:
uint16_t calculate_json_crc16(void);
void interface_query(const uint8_t* input, size_t input_length, StreamSink* output);
static void interface_query_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
reinterpret_cast<BidirectionalPacketBasedChannel*>(ctx)->interface_query(input, input_length, output);
}
static void subscription_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
reinterpret_cast<BidirectionalPacketBasedChannel*>(ctx)->subscription(input, input_length, output);
}
const Endpoint channel_specific_endpoints_[1] = {
Endpoint("", PROPERTY, BidirectionalPacketBasedChannel::interface_query_handler, "\"type\":\"json\",\"access\":\"rw\"", this),
//Endpoint("subscriptions", PROPERTY, BidirectionalPacketBasedChannel::subscription_handler, nullptr, this)
};
static constexpr size_t NUM_CHANNEL_SPECIFIC_ENDPOINTS = sizeof(channel_specific_endpoints_) / sizeof(channel_specific_endpoints_[0]);
const Endpoint* get_endpoint(size_t index) {
if (index < NUM_CHANNEL_SPECIFIC_ENDPOINTS){
return &channel_specific_endpoints_[index];
} else if (index < n_endpoints_) {
return &global_endpoints_[index - NUM_CHANNEL_SPECIFIC_ENDPOINTS];
} else {
return nullptr;
}
}
void subscription(const uint8_t* input, size_t input_length, StreamSink* output) {
// TODO: handle
return;
}
const Endpoint * const global_endpoints_;
size_t n_endpoints_;
PacketSink& output_;
uint8_t tx_buf_[TX_BUF_SIZE];
const uint16_t json_crc_;
};
#endif
Firmware/MotorControl/utils.c
+17 -1
View File
@@ -1,6 +1,7 @@
#include <utils.h>
#include <math.h>
#include <cmsis_os.h>
static const float one_by_sqrt3 = 0.57735026919f;
static const float two_by_sqrt3 = 1.15470053838f;
@@ -166,4 +167,19 @@ float fast_atan2(float y, float x) {
int mod(int dividend, int divisor){
int r = dividend % divisor;
return (r < 0) ? (r + divisor) : r;
}
}
// @brief: Returns how much time is left until the deadline is reached.
// If the deadline has already passed, the return value is 0 (except if
// the deadline is very far in the past)
uint32_t deadline_to_timeout(uint32_t deadline_ms) {
uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency);
uint32_t timeout_ms = deadline_ms - now_ms;
return (timeout_ms & 0x80000000) ? 0 : timeout_ms;
}
// @brief: Converts a timeout to a deadline based on the current time.
uint32_t timeout_to_deadline(uint32_t timeout_ms) {
uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency);
return now_ms + timeout_ms;
}
Firmware/MotorControl/utils.h
+13
View File
@@ -2,6 +2,12 @@
#ifndef __UTILS_H
#define __UTILS_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
/**
* @brief Unique ID register address location
*/
@@ -82,4 +88,11 @@ float wrap_pm_pi(float theta);
float fast_atan2(float y, float x);
int mod(int dividend, int divisor);
uint32_t deadline_to_timeout(uint32_t deadline_ms);
uint32_t timeout_to_deadline(uint32_t timeout_ms);
#ifdef __cplusplus
}
#endif
#endif //__UTILS_H
Firmware/README.md
+64 -81
View File
@@ -10,7 +10,7 @@ If you are a developer, you are encouraged to use the `devel` branch, as it cont
- [Configuring parameters](#configuring-parameters)
- [Compiling and downloading firmware](#compiling-and-downloading-firmware)
- [Communicating over USB and UART](#communicating-over-usb-and-uart)
- [Communicating over USB or UART](#communicating-over-usb-or-uart)
- [Generating startup code](#generating-startup-code)
- [Setting up Eclipse development environment](#setting-up-eclipse-development-environment)
- [Notes for Contributors](#notes-for-contributors)
@@ -20,7 +20,6 @@ If you are a developer, you are encouraged to use the `devel` branch, as it cont
## Configuring parameters
To correctly operate the ODrive, you need to supply some parameters. Some are mandatory, and if supplied incorrectly will cause the drive to malfunction. To get good performance you must also tune the drive.
### Hardware configuration
The first thing to set is your board hardware version, located at the top of [Inc/main.h](Inc/main.h). If, for example, you are using the hardware: ODrive v3.2, then you should set it like this:
```C
#define HW_VERSION_MAJOR 3
@@ -28,27 +27,47 @@ The first thing to set is your board hardware version, located at the top of [In
```
### Communication configuration
If you are using USB only to communicate with the ODrive, you may skip this step.
If want to use the example python scripts and connect the ODrive via USB, the defaults are fine for you and you can skip this step.
The GPIO 1,2 pins are configurable as either step/direction, or as UART.
In [MotorControl/commands.c](MotorControl/commands.c) please set `gpio_mode` to the corresponding value (`GPIO_MODE_UART` or `GPIO_MODE_STEP_DIR`).
You can select what interface you want to run on USB and GPIO pins. See [Communicating over USB or UART](#communicating-over-usb-or-uart) for more information.
The following options are available in [MotorControl/commands.h](MotorControl/commands.h):
__USB__:
- `USB_PROTOCOL_NATIVE`: Use the native protocol (recommended for new applications).
The python library only understands the native protocol, so this is the way to go
if you use that.
- `USB_PROTOCOL_NATIVE_STREAM_BASED`: Use the native stream based protocol.
On most platforms the device shows up as a serial port when connected over USB.
So instead of using the python tool's direct USB access, you can use this option and then pretend you connected the device over serial.
__On some platforms (specifically macOS), this is required__ because the kernel doesn't allow direct USB access.
- `USB_PROTOCOL_LEGACY`: Use the human-readable legacy protocol
Select this option if you already have an existing application. This option will be removed in the future.
- `USB_PROTOCOL_NONE`: Ignore USB communication
__GPIO 1,2 pins__:
Note that UART is only supported on ODrive v3.3 and higher.
- `UART_PROTOCOL_NATIVE`: Use the native protocol (see notes above).
- `UART_PROTOCOL_LEGACY`: Use the human-readable legacy protocol
Use this option if you control the ODrive with an Arduino. The ODrive Arduino library is not yet updated to the native protocol.
- `UART_PROTOCOL_NONE`: Ignore UART communication
- `USE_GPIO_MODE_STEP_DIR`: Step/direction control mode (use in conjunction with `UART_PROTOCOL_NONE`)
### Motor control parameters
The rest of all the parameters are at the top of the [MotorControl/low_level.c](MotorControl/low_level.c) file. Please note that many parameters occur twice, once for each motor.
In it's current state, the motor structs contain both tuning parameters, meant to be set by the developer, and static variables, meant to be modified by the software. Unfortunatly these are mixed together right now, but cleaning this up is a high priority task.
It may be helpful to know that the entry point of each of the motor threads is `void motor_thread` at the bottom of [MotorControl/low_level.c](MotorControl/low_level.c). This is like `main` for each motor, and is probably where you should start reading the code.
It may be helpful to know that the entry point of each of the motor threads is `void axis_thread_entry` at the top of [MotorControl/axis.cpp](MotorControl/axis.cpp). This is like `main` for each motor, and is probably where you should start reading the code.
#### Mandatory parameters
### Mandatory parameters
You must set:
* `ENCODER_CPR`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value.
* `POLE_PAIRS`: This is the number of magnet poles in the rotor, divided by two. You can simply count the number of magnets in the rotor, if you can see them.
* `brake_resistance`: This is the resistance of the brake resistor. If you are not using it, you may set it to 0.0f.
#### Tuning parameters
### Tuning parameters
The most important parameters are the limits:
* The current limit: `.current_lim = 75.0f, //[A] // Note: consistent with 40v/v gain`. The default current limit, for safety reasons, is set to 10A. This is quite weak, and good for making sure the drive is stable. Once you have tuned the drive, you can increase this to 75A to get some performance. Note that above 75A, you must change the current amplifier gains.
* The velocity limit: `.vel_limit = 20000.0f, // [counts/s]`. Does what it says on the tin.
* The velocity limit: `.vel_limit = 20000.0f, // [counts/s]`. The motor will be limited to this speed; again the default value is quite slow.
The motion control gains are currently manually tuned:
* `.pos_gain = 20.0f, // [(counts/s) / counts]`
@@ -64,7 +83,7 @@ An upcoming feature will enable automatic tuning. Until then, here is a rough tu
* Back down `pos_gain` until you do not have overshoot anymore.
* The integrator is not easily tuned, nor is it strictly required. Tune at your own discression.
#### Optional parameters
### Optional parameters
By default both motors are enabled, and the default control mode is position control.
If you want a different mode, you can change `.control_mode`. To disable a motor, set `.enable_control` and `.do_calibration` to false.
@@ -109,91 +128,57 @@ After installing all of the above, open a Git Bash shell. Continue at section [B
* You need to power the board by only **ONE** of the following: VCC(3.3v), 5V, or the main power connection (the DC bus). The USB port (J1) does not power the board.
* Run `make flash` in the root of this repository.
If the flashing worked, you can start sending commands. If you want to do that now, you can go to [Communicating over USB or UART](#communicating-over-usb-or-uart).
### Debugging the firmware
Run `make gdb`. This will reset and halt at program start. Now you can set breakpoints and run the program. If you know how to use gdb, you are good to go.
If you prefer to debug from eclipse, see [Setting up Eclipse development environment](#setting-up-eclipse-development-environment).
The following options are known to work and supported:
* Command line GDB. Run `make gdb`. This will reset and halt at program start. Now you can set breakpoints and run the program. If you know how to use gdb, you are good to go.
* Eclipse, see [Setting up Eclipse development environment](#setting-up-eclipse-development-environment).
* Visual Studio Code. The solution we have is not the most elegant, and if you know a better way, please do help us.
* Make sure you have the Firmware folder as your active folder
* Flash the board with the newest code (starting debug session doesn't do this)
* Tasks -> Run Task -> openocd
* Debug -> Start Debugging
* The processor will reset and halt.
* Set your breakpoints. Note: you can only set breakpoints when the processor is halted, if you set them during run mode, they won't get applied.
* Run
* When you are done, you must kill the openocd task before you are able to flash the board again: Tasks -> Terminate task -> openocd.
## Communicating over USB and UART
There is currently a very primitive method to read/write configuration, commands and errors from the ODrive over the USB.
Please use the `tools/test_communication.py` python script for this. It is written for [Python 3](https://www.python.org/downloads/) and so should be instlled first.
## Communicating over USB or UART
Setup instructions as follows:
* Install PyUSB (pip install --pre pyusb). This can be done using the [gitbash terminal](https://git-for-windows.github.io/) for windows users.
* Plug in the STLink or another power source to power the ODrive board
* Plug in a separate USB cable into the microUSB connector on ODrive
* On Windows, use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb. If 'Odrive V3.x' is not in the list of devices upon opening Zadig check 'List All Devices' from the options menu. Connecting to the Odrive board directly and not over a usb hub may also help. With the Odrive selected in the device list choose 'libusb-win32' from the target driver list and select the large 'install driver' button.
* Run `tools/test_communication.py`
### From Linux/Windows/macOS
There are two simple python scripts to help you get started with controlling the ODrive using python.
### Command set
The most accurate way to understand the commands is to read [the code](MotorControl/commands.c) that parses the commands. Also you can have a look at the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) that makes it easy to use the UART interface on Arduino. You can also look at it as an implementation example of how to talk to the ODrive over UART.
#### UART framing
USB communicates with packets, so it is easy to frame a command as one command per packet. However, UART doesn't have any packeting, so we need a way to frame the commands. The start-of-packet symbol is `$` and the end-of-packet symbol is `!`, that is, something like this: `$command!`. An example of a valid UART position command:
1. [Install Python 3](https://www.python.org/downloads/), then install dependencies:
```
$p 0 10000 0 0!
pip install pyusb pyserial
```
#### Motor Position command
3. __Linux__: set up USB permissions
```
p motor position velocity_ff current_ff
echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d[0-9][0-9]", MODE="0666"' | sudo tee /etc/udev/rules.d/50-odrive.rules
sudo udevadm control --reload-rules
sudo udevadm trigger # until you reboot you may need to do this everytime you reset the ODrive
```
* `p` for position
* `motor` is the motor number, `0` or `1`.
* `position` is the desired position, in encoder counts.
* `velocity_ff` is the velocity feed-forward term, in counts/s.
* `current_ff` is the current feed-forward term, in A.
4. Power the ODrive board (as per the [Flashing the firmware](#flashing-the-firmware) step)
5. Plug in a USB cable into the microUSB connector on ODrive, and connect it to your PC
6. __Windows__: Use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb.
* If 'Odrive V3.x' is not in the list of devices upon opening Zadig, check 'List All Devices' from the options menu. With the Odrive selected in the device list choose 'libusb-win32' from the target driver list and select the large 'install driver' button.
7. Run `./tools/demo.py` or `./tools/explore_odrive.py`.
- `demo.py` is a very simple script which will make motor 0 turn back and forth. Use this as an example if you want to control the ODrive yourself programatically.
- `explore_odrive.py` drops you into an interactive python shell where you can explore and edit the parameters that are available on your device. For instance `my_odrive.motor0.pos_setpoint = 10000` makes motor0 move to position 10000. To connect over serial instead of USB run `./tools/explore_odrive.py --discover serial`.
Note that if you don't know what feed-forward is or what it's used for, simply set it to 0.
### From Arduino
[See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino)
#### Motor Velocity command
```
v motor velocity current_ff
```
* `v` for velocity
* `motor` is the motor number, `0` or `1`.
* `velocity` is the desired velocity in counts/s.
* `current_ff` is the current feed-forward term, in A.
### Other platforms
See the [protocol specification](https://github.com/madcowswe/ODrive/blob/devel/Firmware/protocol.md) or the [legacy protocol specification](https://github.com/madcowswe/ODrive/blob/devel/Firmware/legacy-protocol.md).
Note that if you don't know what feed-forward is or what it's used for, simply set it to 0.
#### Motor Current command
```
c motor current
```
* `c` for current
* `motor` is the motor number, `0` or `1`.
* `current` is the desired current in A.
#### Variable getting and setting
```
g type index
s type index value
```
* `g` for get, `s` for set
* `type` is the data type as follows:
** `0` is float
** `1` is int
** `2` is bool
* `index` is the index in the corresponding [exposed variable table](MotorControl/commands.c).
For example
* `g 0 12` will return the phase resistance of M0
* `s 0 8 10000.0` will set the velocity limit on M0 to 10000 counts/s
* `g 1 3` will return the error status of M0
* `g 1 7` will return the error status of M1
The error status corresponds to the [Error_t enum in low_level.h](MotorControl/low_level.h).
Note that the links in this section are to a specific commits to make sure that the line numbers are accurate. That is, they don't link to the newest master, but to an old version. Please check the corresponding lines in the code you are using. This is especially important to get the correct indicies in the exposed variable tables, and the error enum values.
#### Continous monitoring of variables
You can set up variables in monitoring slots, and then have them (or a subset of them) repeatedly printed upon request. Please see the code for this.
## Generating startup code
**Note:** You do not need to run this step to program the board. This is only required if you wish to update the auto generated code.
This project uses the STM32CubeMX tool to generate startup code and to ease the configuration of the peripherals.
We also use a tool to generate the Makefile. The steps to do this are as follows.
You will likely want the pinout for this process. It is available [here](https://docs.google.com/spreadsheets/d/1QXDCs1IRtUyG__M_9WruWOheywb-GhOwFtfPcHuN2Fg/edit#gid=404444347)
### Installing prerequisites
* `stm32cubeMX`: Tool from STM to automatically generate setup routines and configure libraries, etc.
@@ -204,8 +189,6 @@ We also use a tool to generate the Makefile. The steps to do this are as follows
* Press `Project -> Generate code`
* You may need to let it download some drivers and such.
You will likely want the pinout for this process. It is available [here](https://docs.google.com/spreadsheets/d/1QXDCs1IRtUyG__M_9WruWOheywb-GhOwFtfPcHuN2Fg/edit#gid=404444347)
## Setting up Eclipse development environment
### Install
Firmware/Src/freertos.c
+10 -3
View File
@@ -54,6 +54,7 @@
/* USER CODE BEGIN Includes */
#include "freertos_vars.h"
#include "low_level.h"
#include "axis_c_interface.h"
#include "commands.h"
/* USER CODE END Includes */
@@ -61,7 +62,13 @@
osThreadId defaultTaskHandle;
/* USER CODE BEGIN Variables */
// List of semaphores
osSemaphoreId sem_usb_irq;
// List of threads
osThreadId thread_motor_0;
osThreadId thread_motor_1;
osThreadId thread_cmd_parse;
/* USER CODE END Variables */
/* Function prototypes -------------------------------------------------------*/
@@ -141,13 +148,13 @@ void StartDefaultTask(void const * argument)
init_motor_control();
// Start motor threads
osThreadDef(task_motor_0, motor_thread, osPriorityHigh+1, 0, 512);
osThreadDef(task_motor_1, motor_thread, osPriorityHigh, 0, 512);
osThreadDef(task_motor_0, axis_thread_entry, osPriorityHigh+1, 0, 512);
osThreadDef(task_motor_1, axis_thread_entry, osPriorityHigh, 0, 512);
thread_motor_0 = osThreadCreate(osThread(task_motor_0), &motors[0]);
thread_motor_1 = osThreadCreate(osThread(task_motor_1), &motors[1]);
// Start command handling thread
osThreadDef(task_cmd_parse, cmd_parse_thread, osPriorityNormal, 0, 512);
osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 512);
thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL);
// Start USB interrupt handler thread
Firmware/Src/syscalls.c
+2 -1
View File
@@ -6,11 +6,12 @@
*/
#include <cmsis_os.h>
#include <commands.h>
#include <freertos_vars.h>
#include <sys/unistd.h>
#include <usart.h>
#include <usbd_cdc_if.h>
#include <legacy_commands.h> // TODO: make serial_printf_select constant
//int _read(int file, char *data, int len) {}
//int _close(int file) {}
Firmware/Src/usbd_cdc_if.c
+6 -11
View File
@@ -49,6 +49,8 @@
/* Includes ------------------------------------------------------------------*/
#include "usbd_cdc_if.h"
/* USER CODE BEGIN INCLUDE */
#include "cmsis_os.h"
#include "freertos_vars.h"
#include "utils.h"
#include "commands.h"
#include <freertos_vars.h>
@@ -76,10 +78,6 @@
* @{
*/
/* USER CODE BEGIN PRIVATE_DEFINES */
/* Define size for the receive and transmit buffer over CDC */
/* It's up to user to redefine and/or remove those define */
#define APP_RX_DATA_SIZE 64
#define APP_TX_DATA_SIZE 64
/* USER CODE END PRIVATE_DEFINES */
/**
* @}
@@ -101,10 +99,10 @@
/* Create buffer for reception and transmission */
/* It's up to user to redefine and/or remove those define */
/* Received Data over USB are stored in this buffer */
uint8_t UserRxBufferFS[APP_RX_DATA_SIZE];
uint8_t UserRxBufferFS[USB_RX_DATA_SIZE];
/* Send Data over USB CDC are stored in this buffer */
uint8_t UserTxBufferFS[APP_TX_DATA_SIZE];
uint8_t UserTxBufferFS[USB_TX_DATA_SIZE];
/* USER CODE BEGIN PRIVATE_VARIABLES */
/* USER CODE END PRIVATE_VARIABLES */
@@ -269,11 +267,8 @@ static int8_t CDC_Control_FS (uint8_t cmd, uint8_t* pbuf, uint16_t length)
static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
{
/* USER CODE BEGIN 6 */
//Append null termination at end of string
int modified_len = MACRO_MIN(*Len+1, APP_RX_DATA_SIZE);
Buf[modified_len-1] = 0;
set_cmd_buffer(Buf, modified_len);
set_cmd_buffer(Buf, *Len);
osSemaphoreRelease(sem_usb_rx);
return (USBD_OK);
@@ -297,7 +292,7 @@ uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len)
/* USER CODE BEGIN 7 */
//Check length
if (Len > APP_TX_DATA_SIZE)
if (Len > USB_TX_DATA_SIZE)
return USBD_FAIL;
// Check for ongoing transmission
USBD_CDC_HandleTypeDef* hcdc = (USBD_CDC_HandleTypeDef*) hUsbDeviceFS.pClassData;
Firmware/legacy-protocol.md
+68
View File
@@ -0,0 +1,68 @@
Warning: this protocol has [been replaced](https://github.com/madcowswe/ODrive/blob/devel/Firmware/protocol.md).
It's still operational but for new applications it's recommended to use the new protocol.
### Command set
The most accurate way to understand the commands is to read [the code](MotorControl/commands.c) that parses the commands. Also you can have a look at the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) that makes it easy to use the UART interface on Arduino. You can also look at it as an implementation example of how to talk to the ODrive over UART.
#### UART framing
USB communicates with packets, so it is easy to frame a command as one command per packet. However, UART doesn't have any packeting, so we need a way to frame the commands. The start-of-packet symbol is `$` and the end-of-packet symbol is `!`, that is, something like this: `$command!`. An example of a valid UART position command:
```
$p 0 10000 0 0!
```
#### Motor Position command
```
p motor position velocity_ff current_ff
```
* `p` for position
* `motor` is the motor number, `0` or `1`.
* `position` is the desired position, in encoder counts.
* `velocity_ff` is the velocity feed-forward term, in counts/s.
* `current_ff` is the current feed-forward term, in A.
Note that if you don't know what feed-forward is or what it's used for, simply set it to 0.
#### Motor Velocity command
```
v motor velocity current_ff
```
* `v` for velocity
* `motor` is the motor number, `0` or `1`.
* `velocity` is the desired velocity in counts/s.
* `current_ff` is the current feed-forward term, in A.
Note that if you don't know what feed-forward is or what it's used for, simply set it to 0.
#### Motor Current command
```
c motor current
```
* `c` for current
* `motor` is the motor number, `0` or `1`.
* `current` is the desired current in A.
#### Variable getting and setting
```
g type index
s type index value
```
* `g` for get, `s` for set
* `type` is the data type as follows:
** `0` is float
** `1` is int
** `2` is bool
* `index` is the index in the corresponding [exposed variable table](MotorControl/commands.c).
For example
* `g 0 12` will return the phase resistance of M0
* `s 0 8 10000.0` will set the velocity limit on M0 to 10000 counts/s
* `g 1 3` will return the error status of M0
* `g 1 7` will return the error status of M1
The error status corresponds to the [Error_t enum in low_level.h](MotorControl/low_level.h).
Note that the links in this section are to a specific commits to make sure that the line numbers are accurate. That is, they don't link to the newest master, but to an old version. Please check the corresponding lines in the code you are using. This is especially important to get the correct indicies in the exposed variable tables, and the error enum values.
#### Continous monitoring of variables
You can set up variables in monitoring slots, and then have them (or a subset of them) repeatedly printed upon request. Please see the code for this.
Firmware/protocol.md
+68
View File
@@ -0,0 +1,68 @@
# ODrive Communication Protocol #
Communicating with an ODrive consists of a series of endpoint operations.
An endpoint can theoretically be any kind data serialized in any way.
There is a default seralization implementation for POD types; for custom types
you must (de)seralize yourself. In the future we may provide a default seralizer
for stucts.
The available endpoints can be enumerated by reading the JSON from endpoint 0
and can theoretically be different for each communication interface (they are not in practice).
Each endpoint operation can send bytes to one endpoint (referenced by it's ID)
and at the same time receive bytes from the same endpoint. The semantics of
these payloads are specific to each endpoint's type, the name of which is
indicated in the JSON.
For instance an int32 endpoint's input and output is a 4 byte little endian
representation. In general the convention for combined read/write requests is
_exchange_, i.e. the returned value is the old value. Custom endpoint handlers
may be non-compliant.
There is a packet based version and a stream based variant of the protocol. Each
variant is employed as appropriate. For instance USB runs the packet based variant
by default while UART runs the stream based variant.
## Packet format ##
We will call the ODrive "server" and the PC "client". A request is a message
from the PC to the ODrive and a response is a message from the PC to the
ODrive.
Each request-response transaction corresponds to a single endpoint operation.
__Request__
- __Bytes 0, 1__ Sequence number, MSB = 0
- Currently the server does not care about ordering and does not filter resent messages.
- __Bytes 2, 3__ Endpoint ID
- The IDs of all endpoints can be obtained from the JSON definition. The JSON definition can be obtained by reading from endpoint 0.
If (and only if) the MSB is set to 1 the client expects a response for this request.
- __Bytes 4, 5__ Expected response size
- The number of bytes that should be returned to the client. If the client doesn't need any response data, it can set this value to 0. The operation will still be acknowledged if the
MSB in EndpointID is set.
- __Bytes 6 to N-3__ Payload
- The length of the payload is determined by the total packet size. The format of the payload depends on the endpoint type. The endpoint type can be obtained from the JSON definition.
- __Bytes N-2, N-1__
- For endpoint 0: Protocol version (currently 1). A server shall ignore packets with other values.
- For all other endpoints: The CRC16 calculated over the JSON definition. The CRC16 init value is the protocol version (currently 1). A server shall ignore packets that set this field incorrectly. See protocol.hpp for CRC details.
__Response__
- __Bytes 0, 1__ Sequence number, MSB = 1
- The sequence number of the request to which this is the response.
- __Bytes 2, 3__ Payload
- The length of the payload tends to be equal to the number of expected bytes as indicated
in the request. The server must not expect the client to accept more bytes than it requested.
## Stream format ##
The stream based format is just a wrapper for the packet format.
- __Byte 0__ Sync byte `0xAA`
- __Bytes 1, 2__ Packet length
- Currently both parties shall only emit and accept values of 0 through 127.
- __Bytes 3__ CRC8 of bytes 0 through 2
- See protocol.hpp for CRC details.
- __Bytes 4 to N-3__ Packet
- __Bytes N-2, N-1__ CRC16
- See protocol.hpp for CRC details.
VSCodeWorkspace.code-workspace
+44
View File
@@ -0,0 +1,44 @@
{
"folders": [
{
"path": "Firmware"
},
{
"path": "tools"
}
],
"settings": {
"files.associations": {
"memory": "cpp",
"utility": "cpp",
"deque": "cpp",
"vector": "cpp",
"array": "cpp",
"*.tcc": "cpp",
"cctype": "cpp",
"clocale": "cpp",
"cstdint": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"exception": "cpp",
"functional": "cpp",
"initializer_list": "cpp",
"iosfwd": "cpp",
"istream": "cpp",
"limits": "cpp",
"new": "cpp",
"ostream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"typeinfo": "cpp",
"algorithm": "cpp"
}
}
}
tools/.vscode/launch.json
Vendored
+24
View File
@@ -0,0 +1,24 @@
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "Python",
"type": "python",
"request": "launch",
"stopOnEntry": true,
"pythonPath": "${config:python.pythonPath}",
"program": "${file}",
"cwd": "${workspaceRoot}",
"env": {},
"envFile": "${workspaceRoot}/.env",
"debugOptions": [
"WaitOnAbnormalExit",
"WaitOnNormalExit",
"RedirectOutput"
]
}
]
}
tools/demo.py
Executable
+45
View File
@@ -0,0 +1,45 @@
#!/usr/bin/env python3
"""
Example usage of the ODrive python library to monitor and control ODrive devices
"""
import odrive.core
import time
import math
# Find a connected ODrive (this will block until you connect one)
my_drive = odrive.core.find_any(consider_usb=True, consider_serial=False, printer=print)
# The above call returns a python object with a dynamically generated type. The
# type hierarchy will correspond to the endpoint list in `MotorControl/protocol.cpp`.
# You can also inspect the object using the dir-function:
#print(dir(my_drive))
#print(dir(my_drive.motor0))
# TODO: maybe provide an introspection method that dumps the whole type hierarchy at once
# To read a value, simply read the property
print("Bus voltage is " + str(my_drive.vbus_voltage) + "V")
# Or to change a value, just assign to the property
my_drive.motor0.pos_setpoint = 3.14
print("Position setpoint is " + str(my_drive.motor0.pos_setpoint))
# And this is how function calls are done:
my_drive.motor0.set_pos_setpoint(0.0, 0.0, 0.0)
# A little sine wave to test
t0 = time.monotonic()
while True:
setpoint = 10000.0 * math.sin((time.monotonic() - t0)*2)
print("goto " + str(int(setpoint)))
my_drive.motor0.set_pos_setpoint(setpoint, 0.0, 0.0)
time.sleep(0.01)
# Some more things you can try:
# Write to a read-only property:
my_drive.vbus_voltage = 11.0 # fails with `AttributeError: can't set attribute`
# Assign an incompatible value:
my_drive.motor0.pos_setpoint = "I like trains" # fails with `ValueError: could not convert string to float`
tools/explore_odrive.py
Executable
+89
View File
@@ -0,0 +1,89 @@
#!/usr/bin/env python3
"""
Load an odrive object to play with in the IPython interactive shell.
"""
import odrive.core
import argparse
import sys
# Parse arguments
parser = argparse.ArgumentParser(description='Load an odrive object to play with in the IPython interactive shell.')
parser.add_argument("-v", "--verbose", action="store_true",
help="print debug information")
group = parser.add_mutually_exclusive_group()
group.add_argument("-d", "--discover", metavar="CHANNELS", action="store",
help="Automatically discover ODrives. Takes a comma-separated list (without spaces) "
"to indicate which connection types should be considered. Possible values are "
"usb and serial. For example \"--discover usb,serial\" indicates "
"that USB and serial ports should be scanned for ODrives. "
"If none of the below options are specified, --discover usb is assumed.")
group.add_argument("-u", "--usb", metavar="BUS:DEVICE", action="store",
help="Specifies the USB port on which the device is connected. "
"For example \"001:014\" means bus 001, device 014. The numbers can be obtained "
"using `lsusb`.")
group.add_argument("-s", "--serial", metavar="PORT", action="store",
help="Specifies the serial port on which the device is connected. "
"For example \"/dev/ttyUSB0\". Use `ls /dev/tty*` to find your port name.")
parser.set_defaults(discover="usb")
args = parser.parse_args()
if (args.verbose):
printer = print
else:
printer = lambda x: None
# Connect to device
if not args.usb is None:
try:
bus = int(args.usb.split(":")[0])
address = int(args.usb.split(":")[1])
except (ValueError, IndexError):
print("the --usb argument must look something like this: \"001:014\"")
sys.exit(1)
try:
my_odrive = odrive.core.open_usb(bus, address, printer=printer)
except odrive.protocol.DeviceInitException as ex:
print(str(ex))
sys.exit(1)
elif not args.serial is None:
my_odrive = odrive.core.open_serial(args.serial, printer=printer)
else:
print("Waiting for device...")
consider_usb = 'usb' in args.discover.split(',')
consider_serial = 'serial' in args.discover.split(',')
my_odrive = odrive.core.find_any(consider_usb, consider_serial, printer=printer)
print("Connected!")
print('')
print('ODRIVE EXPLORER')
print('')
print('You can now type "my_odrive." and press <tab>')
print('This will present you with all the properties that you can reference')
print('')
print('For example: "my_odrive.motor0.encoder.pll_pos"')
print('will print the current encoder position on motor 0')
print('and "my_odrive.motor0.pos_setpoint = 10000"')
print('will send motor0 to 10000')
print('')
try:
# If this assignment works, we are already in interactive mode.
# so just drop out of script to existing shell
interpreter = sys.ps1
except AttributeError:
# We are not in interactive mode, so let's fire one up
# Though let's be real, IPython is the way to go
print('If you want to have an improved interactive console with pretty colors,')
print('you can run this script in interactive mode with IPython with this command:')
print('ipython -i explore_odrive.py')
print('')
# Enter interactive python shell with tab complete enabled
import code
import rlcompleter
import readline
readline.parse_and_bind("tab: complete")
code.interact(local=locals(), banner='')
tools/odrive/core.py
+324
View File
@@ -0,0 +1,324 @@
"""
Provides functions for the discovery of ODrive devices
"""
import sys
import time
import json
import usb.core
import usb.util
import serial
import serial.tools.list_ports
import odrive.util
import odrive.usbbulk_transport
import odrive.serial_transport
import re
import time
import os
import odrive.protocol
import itertools
import struct
import functools
def noprint(x):
pass
class SimpleDeviceProperty(property):
"""
Used internally by dynamically created objects to translate
property assignments and fetches into endpoint operations on the
object's associated channel
"""
def __init__(self, channel, id, type, struct_format, can_read, can_write):
self._channel = channel
self._id = id
self._type = type
self._struct_format = struct_format
property.__init__(self,
self.fget if can_read else None,
self.fset if can_write else None)
def fget(self, obj):
size = struct.calcsize(self._struct_format)
buffer = self._channel.remote_endpoint_operation(self._id, None, True, size)
return struct.unpack(self._struct_format, buffer)[0]
def fset(self, obj, value):
value = self._type(value)
buffer = struct.pack(self._struct_format, value)
# TODO: Currenly we wait for an ack here. Settle on the default guarantee.
self._channel.remote_endpoint_operation(self._id, buffer, True, 0)
def call_remote_function(channel, trigger_id, arg_properties, *args):
"""
Used internally by the dynamically created objects to translate
function calls into endpoint operations on the associated channel
"""
if (len(arg_properties) != len(args)):
raise TypeError("expected {} arguments but have {}".format(len(arg_properties), len(args)))
for i in range(len(args)):
arg_properties[i].fset(None, args[i])
channel.remote_endpoint_operation(trigger_id, None, True, 0)
def setattr_or_raise_if_undefined(self, name, value):
"""
If employed as an object's __setattr__ function, this function
makes sure that an assignment to an undefined attribute doesn't
create a new attribute but instead raises an exception
"""
# We can't use hasattr here because internally it fetches the property
# value, creating unnecessary bus traffic
if name in dir(self):
object.__setattr__(self, name, value)
else:
raise TypeError('Cannot set name %r on object of type %s' % (
name, self.__class__.__name__))
def create_property(name, json_data, channel, printer):
"""
Dynamically creates a property based on a JSON definition
"""
name = name or "[anonymous]"
type_str = json_data.get("type", None)
if type_str is None:
printer("property {} has no specified type".format(name))
return None
if type_str == "float":
property_type = float
struct_format = "<f"
elif type_str == "bool":
property_type = bool
struct_format = "<?"
elif type_str == "int8":
property_type = int
struct_format = "<b"
elif type_str == "uint8":
property_type = int
struct_format = "<B"
elif type_str == "int16":
property_type = int
struct_format = "<h"
elif type_str == "uint16":
property_type = int
struct_format = "<H"
elif type_str == "int32":
property_type = int
struct_format = "<i"
elif type_str == "uint32":
property_type = int
struct_format = "<I"
else:
printer("property {} has unsupported type {}".format(name, type_str))
return None
id_str = json_data.get("id", None)
if id_str is None:
printer("property {} has no specified ID".format(name))
return None
access_mode = json_data.get("access", "r")
return SimpleDeviceProperty(channel, id_str, property_type,
struct_format,
'r' in access_mode,
'w' in access_mode)
def create_function(name, json_data, channel, printer):
"""
Dynamically creates a function based on a JSON definition
"""
id_str = json_data.get("id", None)
if id_str is None:
printer("function {} has no specified ID".format(name))
return None
inputs = []
for param in json_data.get("arguments", []):
param["mode"] = "r"
inputs.append(create_property(json_data["name"], param, channel, printer))
return functools.partial(call_remote_function, channel, id_str, inputs)
def create_object(name, json_data, namespace, channel, printer=noprint):
"""
Creates an object that implements the specified JSON type description by
communicating with the provided device object
"""
if not namespace is None:
namespace = namespace + "." + name
else:
namespace = name
# Build attribute list from JSON
attributes = {"__setattr__": setattr_or_raise_if_undefined}
for member in json_data.get("members", []):
member_name = member.get("name", None)
if member_name is None:
printer("ignoring unnamed attribute in {}".format(namespace))
continue
type_str = member.get("type", None)
if type_str is None:
printer("member {} has no specified type".format(member_name))
continue
if type_str == "object":
attribute = create_object(member_name, member, namespace, channel, printer=printer)
elif type_str == "function":
attribute = create_function(member_name, member, channel, printer)
else:
attribute = create_property(member_name, member, channel, printer)
if not attribute is None:
attributes[member_name] = attribute
# Create a type from the property list and instantiate it
jit_type = type(namespace, (object,), attributes)
new_object = jit_type()
return new_object
def channel_from_usb_device(usb_device, printer=noprint):
"""
Inits an ODrive Protocol channel from a PyUSB device object.
"""
bulk_device = odrive.usbbulk_transport.USBBulkTransport(usb_device, printer)
printer(bulk_device.info())
bulk_device.init(printer)
return odrive.protocol.Channel(
"USB device bus {} device {}".format(usb_device.bus, usb_device.address),
bulk_device, bulk_device)
def channel_from_serial_port(port, baud, packet_based, printer=noprint):
"""
Inits an ODrive Protocol channel from a serial port name and baudrate.
"""
if packet_based == True:
# TODO: implement packet based transport over serial
raise NotImplementedError("not supported yet")
serial_device = odrive.serial_transport.SerialStreamTransport(port, baud)
input_stream = odrive.protocol.PacketFromStreamConverter(serial_device)
output_stream = odrive.protocol.PacketToStreamConverter(serial_device)
return odrive.protocol.Channel(
"serial port {}@{}".format(port, baud),
input_stream, output_stream)
def object_from_channel(channel, printer=noprint):
"""
Inits an object from a given channel.
This queries the endpoint 0 on that channel to gain information
about the interface, which is then used to init the corresponding object.
"""
printer("Connecting to device on " + channel._name)
try:
json_bytes = channel.remote_endpoint_read_buffer(0)
except (odrive.protocol.TimeoutException, odrive.protocol.ChannelBrokenException):
raise odrive.protocol.DeviceInitException("no response - probably incompatible")
json_crc16 = odrive.protocol.calc_crc16(odrive.protocol.PROTOCOL_VERSION, json_bytes)
channel._interface_definition_crc = json_crc16
try:
json_string = json_bytes.decode("ascii")
except UnicodeDecodeError:
raise odrive.protocol.DeviceInitException("device responded on endpoint 0 with something that is not ASCII")
printer("JSON: " + json_string)
try:
json_data = json.loads(json_string)
except json.decoder.JSONDecodeError:
raise odrive.protocol.DeviceInitException("device responded on endpoint 0 with something that is not JSON")
json_data = {"name": "odrive", "members": json_data}
return create_object("odrive", json_data, None, channel, printer=printer)
def find_usb_channels(vid_pid_pairs=odrive.util.USB_VID_PID_PAIRS, printer=noprint):
"""
Scans for compatible USB devices.
Returns a generator of odrive.protocol.Channel objects.
"""
for vid_pid_pair in vid_pid_pairs:
for usb_device in usb.core.find(idVendor=vid_pid_pair[0], idProduct=vid_pid_pair[1], find_all=True):
printer("Found ODrive via PyUSB")
try:
yield channel_from_usb_device(usb_device, printer)
except usb.core.USBError as ex:
if ex.errno == 13:
printer("USB device access denied. Did you set up your udev rules correctly?")
continue
raise
def find_dev_serial_ports(search_regex):
try:
return ['/dev/' + x for x in filter(re.compile(search_regex).search, os.listdir('/dev'))]
except FileNotFoundError:
return []
def find_pyserial_ports():
return [x.name for x in serial.tools.list_ports.comports()]
def find_serial_channels(printer=noprint):
"""
Scans for serial ports.
Returns a generator of odrive.protocol.Channel objects.
Not every returned object necessarily represents a compatible device.
"""
# Real serial ports or USB-Serial converters (tested on Linux and Windows)
real_serial_ports = find_pyserial_ports()
# Serial devices that are exposed by the platform
# for the device's USB connection
linux_usb_serial_ports = find_dev_serial_ports(r'^ttyACM')
macos_usb_serial_ports = find_dev_serial_ports(r'^tty\.usbmodem')
for port in real_serial_ports + linux_usb_serial_ports + macos_usb_serial_ports:
try:
yield channel_from_serial_port(port, 115200, False, printer)
except serial.serialutil.SerialException:
printer("could not open " + port)
continue
def find_all(consider_usb=True, consider_serial=False, printer=noprint):
"""
Returns a generator with all the connected devices that speak the ODrive protocol
"""
channels = iter(())
if (consider_usb):
channels = itertools.chain(channels, find_usb_channels(printer=printer))
if (consider_serial):
channels = itertools.chain(channels, find_serial_channels(printer=printer))
for channel in channels:
# TODO: blacklist known bad channels
try:
yield object_from_channel(channel, printer)
except odrive.protocol.DeviceInitException as ex:
printer(str(ex))
continue
def find_any(consider_usb=True, consider_serial=False, printer=noprint):
"""
Scans for ODrives on all supported interfaces and returns the first device
that is found. If no device is connected the function blocks.
"""
# TODO: do device discovery and instantiation in a separate thread and just wait on a semaphore here
# poll for device
printer("looking for ODrive...")
while True:
dev = next(find_all(consider_usb, consider_serial, printer=printer), None)
if dev is not None:
return dev
printer("no device found")
time.sleep(1)
def open_serial(port_name, printer=noprint):
channel = channel_from_serial_port(port_name, 115200, False, printer)
return object_from_channel(channel, printer)
def open_usb(bus, address, printer=noprint):
usb_device1 = usb.core.find(bus=1, address=16)
usb_device = usb.core.find(bus=bus, address=address)
if usb_device is None:
raise odrive.protocol.DeviceInitException("No USB device found on bus {} device {}".format(bus, address))
channel = channel_from_usb_device(usb_device, printer)
return object_from_channel(channel, printer)
tools/odrive/protocol.py
+278
View File
@@ -0,0 +1,278 @@
# See protocol.hpp for an overview of the protocol
import time
import struct
from abc import ABC, abstractmethod
SYNC_BYTE = 0xAA
CRC8_INIT = 0x42
CRC16_INIT = 0x1337
PROTOCOL_VERSION = 1
CRC8_DEFAULT = 0x37 # this must match the polynomial in the C++ implementation
CRC16_DEFAULT = 0x3d65 # this must match the polynomial in the C++ implementation
MAX_PACKET_SIZE = 128
def calc_crc(remainder, value, polynomial, bitwidth):
topbit = (1 << (bitwidth - 1))
# Bring the next byte into the remainder.
remainder ^= (value << (bitwidth - 8))
for bitnumber in range(0,8):
if (remainder & topbit):
remainder = (remainder << 1) ^ polynomial
else:
remainder = (remainder << 1)
return remainder & ((1 << bitwidth) - 1)
def calc_crc8(remainder, value):
if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list):
for byte in value:
remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8)
else:
remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8)
return remainder
def calc_crc16(remainder, value):
if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list):
for byte in value:
remainder = calc_crc(remainder, byte, CRC16_DEFAULT, 16)
else:
remainder = calc_crc(remainder, value, CRC16_DEFAULT, 16)
return remainder
# Can be verified with http://www.sunshine2k.de/coding/javascript/crc/crc_js.html:
#print(hex(calc_crc8(0x12, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37])))
#print(hex(calc_crc16(0xfeef, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37])))
class TimeoutException(Exception):
pass
class ChannelBrokenException(Exception):
pass
class DeviceInitException(Exception):
pass
class StreamSource(ABC):
@abstractmethod
def get_bytes(self, deadline):
pass
class StreamSink(ABC):
@abstractmethod
def process_bytes(self, bytes):
pass
class PacketSource(ABC):
@abstractmethod
def get_packet(self, deadline):
pass
class PacketSink(ABC):
@abstractmethod
def process_packet(self, packet):
pass
class StreamToPacketConverter(StreamSink):
_header = []
_packet = []
_packet_length = 0
def __init__(self, output):
self._output = output
def process_bytes(self, bytes):
"""
Processes an arbitrary number of bytes. If one or more full packets are
are received, they are sent to this instance's output PacketSink.
Incomplete packets are buffered between subsequent calls to this function.
"""
for byte in bytes:
if (len(self._header) < 3):
# Process header byte
self._header.append(byte)
if (len(self._header) == 1) and (self._header[0] != SYNC_BYTE):
self._header = []
elif (len(self._header) == 2) and (self._header[1] & 0x80):
self._header = [] # TODO: support packets larger than 128 bytes
elif (len(self._header) == 3) and calc_crc8(CRC8_INIT, self._header):
self._header = []
elif (len(self._header) == 3):
self._packet_length = self._header[1] + 2
else:
# Process payload byte
self._packet.append(byte)
# If both header and packet are fully received, hand it on to the packet processor
if (len(self._header) == 3) and (len(self._packet) == self._packet_length):
if calc_crc16(CRC16_INIT, self._packet) == 0:
self._output.process_packet(self._packet[:-2])
self._header = []
self._packet = []
self._packet_length = 0
class PacketToStreamConverter(PacketSink):
def __init__(self, output):
self._output = output
def process_packet(self, packet):
if (len(packet) >= MAX_PACKET_SIZE):
raise NotImplementedError("packet larger than 127 currently not supported")
header = [SYNC_BYTE, len(packet)]
header.append(calc_crc8(CRC8_INIT, header))
self._output.process_bytes(header)
self._output.process_bytes(packet)
# append CRC in big endian
crc16 = calc_crc16(CRC16_INIT, packet)
self._output.process_bytes(struct.pack('>H', crc16))
class PacketFromStreamConverter(PacketSource):
def __init__(self, input):
self._input = input
def get_packet(self, deadline):
"""
Requests bytes from the underlying input stream until a full packet is
received or the deadline is reached, in which case None is returned. A
deadline before the current time corresponds to non-blocking mode.
"""
while True:
header = bytes()
# TODO: sometimes this call hangs, even though the device apparently sent something
header = header + self._input.get_bytes_or_fail(1, deadline)
if (header[0] != SYNC_BYTE):
#print("sync byte mismatch")
continue
header = header + self._input.get_bytes_or_fail(1, deadline)
if (header[1] & 0x80):
#print("packet too large")
continue # TODO: support packets larger than 128 bytes
header = header + self._input.get_bytes_or_fail(1, deadline)
if calc_crc8(CRC8_INIT, header) != 0:
#print("crc8 mismatch")
continue
packet_length = header[1] + 2
#print("wait for {} bytes".format(packet_length))
packet = self._input.get_bytes_or_fail(packet_length, deadline)
if calc_crc16(CRC16_INIT, packet) != 0:
#print("crc16 mismatch")
continue
return packet[:-2]
class Channel(PacketSink):
_outbound_seq_no = 0
_interface_definition_crc = 0
_expected_acks = {}
# Choose these parameters to be sensible for a specific transport layer
_resend_timeout = 5.0 # [s]
_send_attempts = 5
def __init__(self, name, input, output):
"""
Params:
input: A PacketSource where this channel will source packets from on
demand. Alternatively packets can be provided to this channel
directly by calling process_packet on this instance.
output: A PacketSink where this channel will put outgoing packets.
"""
self._name = name
self._input = input
self._output = output
def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length):
if input is None:
input = bytearray(0)
if (len(input) >= 128):
raise Exception("packet larger than 127 currently not supported")
if (expect_ack):
endpoint_id |= 0x8000
self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff)
self._outbound_seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol
seq_no = self._outbound_seq_no
packet = struct.pack('<HHH', seq_no, endpoint_id, output_length)
packet = packet + input
crc16 = calc_crc16(CRC16_INIT, packet)
if (endpoint_id & 0x7fff == 0):
trailer = PROTOCOL_VERSION
else:
trailer = self._interface_definition_crc
#print("append trailer " + trailer)
packet = packet + struct.pack('<H', trailer)
if (expect_ack):
self._expected_acks[seq_no] = None
attempt = 0
while (attempt < self._send_attempts):
self._output.process_packet(packet)
deadline = time.monotonic() + self._resend_timeout
# Read and process packets until we get an ack or need to resend
# TODO: support I/O driven reception (wait on semaphore)
while True:
try:
response = self._input.get_packet(deadline)
except TimeoutException:
break # resend
# process response, which is hopefully our ACK
self.process_packet(response)
if not self._expected_acks[seq_no] is None:
return self._expected_acks.pop(seq_no, None)
break
# TODO: record channel statistics
attempt += 1
raise ChannelBrokenException()
else:
# fire and forget
self._output.process_packet(packet)
return None
def remote_endpoint_read_buffer(self, endpoint_id):
"""
Handles reads from long endpoints
"""
# TODO: handle device that could (maliciously) send infinite stream
buffer = bytes()
while True:
chunk_length = 64
chunk = self.remote_endpoint_operation(endpoint_id, struct.pack("<I", len(buffer)), True, chunk_length)
if (len(chunk) == 0):
break
buffer += chunk
return buffer
def process_packet(self, packet):
#print("process packet")
packet = bytes(packet)
if (len(packet) < 2):
raise Exception("packet too short")
seq_no = struct.unpack('<H', packet[0:2])[0]
if (seq_no & 0x8000):
seq_no &= 0x7fff
self._expected_acks[seq_no] = packet[2:]
else:
#if (calc_crc16(crc16, struct.pack('<HBB', PROTOCOL_VERSION, packet[-2], packet[-1]))):
# raise Exception("CRC16 mismatch")
print("endpoint requested")
# TODO: handle local endpoint operation
tools/odrive/serial_transport.py
+36
View File
@@ -0,0 +1,36 @@
"""
Provides classes that implement the StreamSource/StreamSink and
PacketSource/PacketSink interfaces for serial ports.
"""
import odrive
import serial
import time
class SerialStreamTransport(odrive.protocol.StreamSource, odrive.protocol.StreamSink):
def __init__(self, port, baud):
self._dev = serial.Serial(port, baud, timeout=1)
def process_bytes(self, bytes):
self._dev.write(bytes)
def get_bytes(self, n_bytes, deadline):
"""
Returns n bytes unless the deadline is reached, in which case the bytes
that were read up to that point are returned. If deadline is None the
function blocks forever. A deadline before the current time corresponds
to non-blocking mode.
"""
if deadline is None:
self._dev.timeout = None
else:
self._dev.timeout = max(deadline - time.monotonic(), 0)
return self._dev.read(n_bytes)
def get_bytes_or_fail(self, n_bytes, deadline):
result = self.get_bytes(n_bytes, deadline)
if len(result) < n_bytes:
raise odrive.protocol.TimeoutException("expected {} bytes but got only {}", n_bytes, len(result))
return result
# TODO: provide SerialPacketTransport
tools/odrive/usbbulk.py
-123
View File
@@ -1,123 +0,0 @@
# requires pyusb
# pip install --pre pyusb
import usb.core
import usb.util
import sys
import time
from odrive.util import ODriveNotConnectedError
from odrive.util import USBID_VID_ODRIVE, USBID_PID_ODRIVE
BULK_DEVICE_NOT_FOUND = 'ODrive BulkDevice Not Found'
BULK_DEVICE_FOUND = 'ODrive BulkDevice Found!'
def noprint(x):
pass
def poll_odrive_bulk_device(intv=1, printer=noprint):
# look for device
while 1:
time.sleep(intv)
try:
dev = ODriveBulkDevice()
break
except:
printer(BULK_DEVICE_NOT_FOUND)
# found device
printer(BULK_DEVICE_FOUND)
return dev
class ODriveBulkDevice():
def __init__(self, idVendor=None, idProduct=None):
# ODrive idVendor
if idVendor is None:
idVendor = USBID_VID_ODRIVE
# ODrive idProduct
if idProduct is None:
idProduct = USBID_PID_ODRIVE
# find our device
self.dev = usb.core.find(idVendor=idVendor, idProduct=idProduct)
# was it found?
if self.dev is None:
raise ODriveNotConnectedError()
##
# information about the connected device
##
def info(self):
# loop through configurations
string = ""
for cfg in self.dev:
string += "ConfigurationValue {0}\n".format(cfg.bConfigurationValue)
for intf in cfg:
string += "\tInterfaceNumber {0},{0}\n".format(intf.bInterfaceNumber, intf.bAlternateSetting)
for ep in intf:
string += "\t\tEndpointAddress {0}\n".format(ep.bEndpointAddress)
return string
def init(self):
try:
string = ""
# detach kernel driver
try:
if self.dev.is_kernel_driver_active(1):
self.dev.detach_kernel_driver(1)
string += "Detached Kernel Driver\n"
except NotImplementedError:
pass #is_kernel_driver_active not implemented on Windows
# set the active configuration. With no arguments, the first
# configuration will be the active one
self.dev.set_configuration()
# get an endpoint instance
self.cfg = self.dev.get_active_configuration()
self.intf = self.cfg[(1,0)]
# write endpoint
self.epw = usb.util.find_descriptor(self.intf,
# match the first OUT endpoint
custom_match = \
lambda e: \
usb.util.endpoint_direction(e.bEndpointAddress) == \
usb.util.ENDPOINT_OUT
)
assert self.epw is not None
string += "EndpointAddress for writing {}\n".format(self.epw.bEndpointAddress)
# read endpoint
self.epr = usb.util.find_descriptor(self.intf,
# match the first IN endpoint
custom_match = \
lambda e: \
usb.util.endpoint_direction(e.bEndpointAddress) == \
usb.util.ENDPOINT_IN
)
assert self.epr is not None
string += "EndpointAddress for reading {}\n".format(self.epr.bEndpointAddress)
return string
except usb.core.USBError:
#return -1
raise
def shutdown(self):
return 0
def send(self, usbBuffer):
try:
ret = self.epw.write(usbBuffer, 0)
return ret
except usb.core.USBError:
#return -1
raise
def receive(self, bufferLen):
try:
ret = self.epr.read(bufferLen, 100)
return ret
except usb.core.USBError:
#return -1
raise
def send_max(self):
return 64
def receive_max(self):
return 64
tools/odrive/usbbulk_transport.py
+96
View File
@@ -0,0 +1,96 @@
# requires pyusb
# pip install --pre pyusb
import usb.core
import usb.util
import sys
import odrive.protocol
import time
def noprint(x):
pass
class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink):
def __init__(self, dev, printer=noprint):
self.dev = dev
self._name = "USB device {}:{}".format(dev.idVendor, dev.idProduct)
##
# information about the connected device
##
def info(self):
# loop through configurations
string = ""
for cfg in self.dev:
string += "ConfigurationValue {0}\n".format(cfg.bConfigurationValue)
for intf in cfg:
string += "\tInterfaceNumber {0},{0}\n".format(intf.bInterfaceNumber, intf.bAlternateSetting)
for ep in intf:
string += "\t\tEndpointAddress {0}\n".format(ep.bEndpointAddress)
return string
def init(self, printer=noprint):
# detach kernel driver
try:
if self.dev.is_kernel_driver_active(1):
self.dev.detach_kernel_driver(1)
printer("Detached Kernel Driver\n")
except NotImplementedError:
pass #is_kernel_driver_active not implemented on Windows
# set the active configuration. With no arguments, the first
# configuration will be the active one
self.dev.set_configuration()
# get an endpoint instance
self.cfg = self.dev.get_active_configuration()
self.intf = self.cfg[(1,0)]
# write endpoint
self.epw = usb.util.find_descriptor(self.intf,
# match the first OUT endpoint
custom_match = \
lambda e: \
usb.util.endpoint_direction(e.bEndpointAddress) == \
usb.util.ENDPOINT_OUT
)
assert self.epw is not None
printer("EndpointAddress for writing {}\n".format(self.epw.bEndpointAddress))
# read endpoint
self.epr = usb.util.find_descriptor(self.intf,
# match the first IN endpoint
custom_match = \
lambda e: \
usb.util.endpoint_direction(e.bEndpointAddress) == \
usb.util.ENDPOINT_IN
)
assert self.epr is not None
printer("EndpointAddress for reading {}\n".format(self.epr.bEndpointAddress))
def shutdown(self):
return 0
def process_packet(self, usbBuffer):
try:
ret = self.epw.write(usbBuffer, 0)
return ret
except usb.core.USBError as ex:
if ex.errno == 19: # "no such device"
raise odrive.protocol.ChannelBrokenException()
else:
raise
def get_packet(self, deadline):
try:
bufferLen = self.epr.wMaxPacketSize
timeout = max(int((deadline - time.monotonic()) * 1000), 0)
ret = self.epr.read(bufferLen, timeout)
return ret
except usb.core.USBError as ex:
if ex.errno == 19: # "no such device"
raise odrive.protocol.ChannelBrokenException()
else:
raise
def send_max(self):
return 64
def receive_max(self):
return 64
tools/odrive/util.py
+8 -29
View File
@@ -1,9 +1,6 @@
# requires pyusb
# pip install --pre pyusb
import time
import usb.core
import usb.util
# Exceptions
class ODriveError(Exception):
@@ -12,33 +9,15 @@ class ODriveError(Exception):
class ODriveNotConnectedError(ODriveError):
pass
# ODrive generic 1209:0D31
USBID_VID_ODRIVE = 0x1209
USBID_PID_ODRIVE = 0x0D31
# ODrive rev 3.1 1209:0D31
USBID_VID_ODRIVE_3_1 = USBID_VID_ODRIVE
USBID_PID_ODRIVE_3_1 = USBID_PID_ODRIVE
USB_DEV_ODRIVE_3_1 = (USBID_VID_ODRIVE_3_1, USBID_PID_ODRIVE_3_1)
USB_DEV_ODRIVE_3_1 = (0x1209, 0x0D31)
USB_DEV_ODRIVE_3_2 = (0x1209, 0x0D32)
USB_DEV_ODRIVE_3_3 = (0x1209, 0x0D33)
# all devices
all_devices = [USB_DEV_ODRIVE_3_1]
# device messages
DEVICE_NOT_FOUND = 'ODrive Not Found'
DEVICE_FOUND = 'ODrive Found!'
USB_VID_PID_PAIRS = [
USB_DEV_ODRIVE_3_1,
USB_DEV_ODRIVE_3_2,
USB_DEV_ODRIVE_3_3,
]
def noprint(x):
pass
def poll_odrive_device(devices=all_devices, intv=1, printer=noprint):
while 1:
time.sleep(intv)
for device in devices:
try:
idVendor = device[0]
idProduct = device[1]
dev = usb.core.find(idVendor=idVendor, idProduct=idProduct)
if dev is not None:
printer(DEVICE_FOUND)
return device
except:
printer(DEVICE_NOT_FOUND)
tools/test_communication.py
-66
View File
@@ -1,66 +0,0 @@
#! /usr/bin/env python3
import argparse
def parse_args():
parser = argparse.ArgumentParser(description='Talk to a ODrive board over USB bulk channel.')
return parser.parse_args()
if __name__ == '__main__':
# parse args before other imports
args = parse_args()
import sys
import time
import threading
from odrive import usbbulk
running = True
ready = False
def main(args):
global running
print("ODrive USB Bulk Communications")
print("---------------------------------------------------------------------")
print("USAGE:")
print("\tPOSITION_CONTROL:\n\t\tp MOTOR_NUMBER POSITION VELOCITY CURRENT")
print("\tVELOCITY_CONTROL:\n\t\tv MOTOR_NUMBER VELOCITY CURRENT")
print("\tCURRENT_CONTROL:\n\t\tc MOTOR_NUMBER CURRENT")
print("\tHALT:\n\t\th")
print("\tQuit Python Script:\n\t\tq")
print("---------------------------------------------------------------------")
# query device
dev = usbbulk.poll_odrive_bulk_device(printer=print)
print (dev.info())
print (dev.init())
# thread
thread = threading.Thread(target=receive_thread, args=[dev])
thread.start()
while running:
time.sleep(0.1)
try:
command = input("\r\nEnter ODrive command:\n")
if 'q' in command:
running = False
sys.exit()
else:
dev.send(command)
except:
running = False
def receive_thread(dev):
global ready
while running:
time.sleep(0.001)
try:
message = dev.receive(dev.receive_max())
message_ascii = bytes(message).decode('ascii')
print(message_ascii, end='')
if "ODrive Firmware" in message_ascii:
ready = True
except:
pass
if __name__ == "__main__":
main(args)