diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index 2ebf2d39..feba750c 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -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": [ diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json index 5dbef258..f6e36562 100644 --- a/Firmware/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -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, diff --git a/Firmware/.vscode/settings.json b/Firmware/.vscode/settings.json index 3c85c084..8f45c82a 100644 --- a/Firmware/.vscode/settings.json +++ b/Firmware/.vscode/settings.json @@ -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" + } } \ No newline at end of file diff --git a/Firmware/.vscode/tasks.json b/Firmware/.vscode/tasks.json index 6bed3fcf..33956da9 100644 --- a/Firmware/.vscode/tasks.json +++ b/Firmware/.vscode/tasks.json @@ -11,6 +11,9 @@ "kind": "build", "isDefault": true }, + "presentation": { + "panel": "new" + }, "problemMatcher": [ "$gcc" ] diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index a4504408..da7b613f 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -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 diff --git a/Firmware/Inc/freertos_vars.h b/Firmware/Inc/freertos_vars.h index 9a08d016..c1e122a0 100644 --- a/Firmware/Inc/freertos_vars.h +++ b/Firmware/Inc/freertos_vars.h @@ -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; diff --git a/Firmware/Inc/usbd_cdc_if.h b/Firmware/Inc/usbd_cdc_if.h index 84d30b6b..3584347d 100644 --- a/Firmware/Inc/usbd_cdc_if.h +++ b/Firmware/Inc/usbd_cdc_if.h @@ -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 */ /** diff --git a/Firmware/Inc/version.h b/Firmware/Inc/version.h deleted file mode 100644 index b5b3e21d..00000000 --- a/Firmware/Inc/version.h +++ /dev/null @@ -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 diff --git a/Firmware/Makefile b/Firmware/Makefile index 43389560..3ad6d167 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -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 $@ diff --git a/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c b/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c index 302fd2fa..32b7b87c 100644 --- a/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c +++ b/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c @@ -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; diff --git a/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h index a3585409..467cb745 100644 --- a/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h +++ b/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h @@ -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. diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp new file mode 100644 index 00000000..f94ad87f --- /dev/null +++ b/Firmware/MotorControl/axis.cpp @@ -0,0 +1,94 @@ + +#include "axis.h" +#include +#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; +} \ No newline at end of file diff --git a/Firmware/MotorControl/axis.h b/Firmware/MotorControl/axis.h new file mode 100644 index 00000000..b3cdf543 --- /dev/null +++ b/Firmware/MotorControl/axis.h @@ -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 */ diff --git a/Firmware/MotorControl/axis_c_interface.h b/Firmware/MotorControl/axis_c_interface.h new file mode 100644 index 00000000..f891e19b --- /dev/null +++ b/Firmware/MotorControl/axis_c_interface.h @@ -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 */ diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp new file mode 100644 index 00000000..77febe2c --- /dev/null +++ b/Firmware/MotorControl/commands.cpp @@ -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 +#include +#include +#include +#include +#include + +#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(&vbus_voltage)), + Endpoint::make_property("elec_rad_per_enc", const_cast(&elec_rad_per_enc)), + Endpoint::make_object("motor0"), + Endpoint::make_property("control_mode", reinterpret_cast(&motors[0].control_mode)), + Endpoint::make_property("error", reinterpret_cast(&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(&motors[0].phase_inductance)), + Endpoint::make_property("phase_resistance", const_cast(&motors[0].phase_resistance)), + Endpoint::make_property("current_meas.phB", const_cast(&motors[0].current_meas.phB)), + Endpoint::make_property("current_meas.phC", const_cast(&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(&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(&motors[0].current_control.Ibus)), + Endpoint::close_tree(), + Endpoint::make_object("encoder"), + Endpoint::make_property("phase", const_cast(&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(&motors[1].control_mode)), + Endpoint::make_property("error", reinterpret_cast(&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(&motors[1].phase_inductance)), + Endpoint::make_property("phase_resistance", const_cast(&motors[1].phase_resistance)), + Endpoint::make_property("current_meas.phB", const_cast(&motors[1].current_meas.phB)), + Endpoint::make_property("current_meas.phC", const_cast(&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(&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(&motors[1].current_control.Ibus)), + Endpoint::close_tree(), + Endpoint::make_object("encoder"), + Endpoint::make_property("phase", const_cast(&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(&motors[1].encoder.encoder_offset)), + Endpoint::make_property("encoder_state", reinterpret_cast(&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(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(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()); +} diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 1c99096f..0d9d7c87 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -1,9 +1,29 @@ #ifndef COMMANDS_H #define COMMANDS_H -/* Includes ------------------------------------------------------------------*/ -#include -/* Exported types ------------------------------------------------------------*/ +// TODO: resolve assert +#define assert(expr) + +#ifdef __cplusplus + +#include +#include +#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 */ diff --git a/Firmware/MotorControl/crc.hpp b/Firmware/MotorControl/crc.hpp new file mode 100644 index 00000000..ce7de238 --- /dev/null +++ b/Firmware/MotorControl/crc.hpp @@ -0,0 +1,66 @@ +#ifndef __CRC_HPP +#define __CRC_HPP + +#include + +// 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 +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 +static T calc_crc(T remainder, const uint8_t* buffer, size_t length) { + while (length--) + remainder = calc_crc(remainder, *(buffer++)); + return remainder; +} + +template +static uint8_t calc_crc8(uint8_t remainder, uint8_t value) { + return calc_crc(remainder, value); +} + +template +static uint16_t calc_crc16(uint16_t remainder, uint8_t value) { + return calc_crc(remainder, value); +} + +template +static uint8_t calc_crc8(uint8_t remainder, const uint8_t* buffer, size_t length) { + return calc_crc(remainder, buffer, length); +} + +template +static uint16_t calc_crc16(uint16_t remainder, const uint8_t* buffer, size_t length) { + return calc_crc(remainder, buffer, length); +} + +#endif /* __CRC_HPP */ diff --git a/Firmware/MotorControl/commands.c b/Firmware/MotorControl/legacy_commands.c similarity index 58% rename from Firmware/MotorControl/commands.c rename to Firmware/MotorControl/legacy_commands.c index 2e0f5e52..254f9dd7 100644 --- a/Firmware/MotorControl/commands.c +++ b/Firmware/MotorControl/legacy_commands.c @@ -1,14 +1,7 @@ /* Includes ------------------------------------------------------------------*/ -#include -#include -#include -#include -#include -#include +#include "legacy_commands.h" #include -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;iInstance->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; } diff --git a/Firmware/MotorControl/legacy_commands.h b/Firmware/MotorControl/legacy_commands.h new file mode 100644 index 00000000..bb945dab --- /dev/null +++ b/Firmware/MotorControl/legacy_commands.h @@ -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 */ diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 30422d16..06e3e1ad 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -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); -} diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 6ea5674a..342c19ec 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -2,6 +2,10 @@ #ifndef __LOW_LEVEL_H #define __LOW_LEVEL_H +#ifdef __cplusplus +extern "C" { +#endif + /* Includes ------------------------------------------------------------------*/ #include #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 diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp new file mode 100644 index 00000000..56f9afb5 --- /dev/null +++ b/Firmware/MotorControl/protocol.cpp @@ -0,0 +1,246 @@ + +/* Includes ------------------------------------------------------------------*/ + +#include "low_level.h" +#include "protocol.hpp" + +#include +#include + +/* 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(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(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(&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(&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(&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(&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(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; +} diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp new file mode 100644 index 00000000..92664d55 --- /dev/null +++ b/Firmware/MotorControl/protocol.hpp @@ -0,0 +1,472 @@ +/* +see protocol.md for the protocol specification +*/ + +#ifndef __PROTOCOL_HPP +#define __PROTOCOL_HPP + +// TODO: resolve assert +#define assert(expr) + +#include +#include +#include +#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 +inline size_t write_le(T value, uint8_t* buffer); + +template +inline size_t read_le(T* value, const uint8_t* buffer); + +template<> +inline size_t write_le(uint8_t value, uint8_t* buffer) { + buffer[0] = value; + return 2; +} + +template<> +inline size_t write_le(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 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 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 value, uint8_t* buffer) { + static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); + static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + return write_le(*reinterpret_cast(&value), buffer); +} + +template<> +inline size_t read_le(uint8_t* value, const uint8_t* buffer) { + *value = buffer[0]; + return 2; +} + +template<> +inline size_t read_le(uint16_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8); + return 2; +} + +template<> +inline size_t read_le(int32_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8) | + (static_cast(buffer[2]) << 16) | + (static_cast(buffer[3]) << 24); + return 4; +} + +template<> +inline size_t read_le(uint32_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8) | + (static_cast(buffer[2]) << 16) | + (static_cast(buffer[3]) << 24); + return 4; +} + +template<> +inline size_t read_le(float* value, const uint8_t* buffer) { + static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); + static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + return read_le(reinterpret_cast(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 +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 EndpointHandler; + + +template +void default_read_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { + const T* value = reinterpret_cast(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(*value, buffer); + if (cnt <= output->get_free_space()) + output->process_bytes(buffer, cnt); + } +} + +template +void default_readwrite_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { + T* value = reinterpret_cast(ctx); + + // Read the endpoint value into output + default_read_endpoint_handler(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(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 function = reinterpret_cast(ctx); + function(); +} + + +template +static inline const char* get_default_json_modifier(); + +template<> +inline const char* get_default_json_modifier() { + return "\"type\":\"float\",\"access\":\"r\""; +} +template<> +inline const char* get_default_json_modifier() { + return "\"type\":\"float\",\"access\":\"rw\""; +} +template<> +inline const char* get_default_json_modifier() { + return "\"type\":\"int32\",\"access\":\"r\""; +} +template<> +inline const char* get_default_json_modifier() { + return "\"type\":\"int32\",\"access\":\"rw\""; +} +template<> +inline const char* get_default_json_modifier() { + return "\"type\":\"uint16\",\"access\":\"r\""; +} +template<> +inline const char* get_default_json_modifier() { + return "\"type\":\"uint16\",\"access\":\"rw\""; +} +template<> +inline const char* get_default_json_modifier() { + return "\"type\":\"uint8\",\"access\":\"r\""; +} +template<> +inline const char* get_default_json_modifier() { + 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 + static Endpoint make_property(const char* name, const T* ctx) { + return Endpoint(name, PROPERTY, + default_read_endpoint_handler, + get_default_json_modifier(), + const_cast(ctx) /* it's safe to cast the const away here because we + know that the default_read_endpoint_handler immediately adds it back */); + } + + template + static Endpoint make_property(const char* name, T* ctx) { + return Endpoint(name, PROPERTY, + default_readwrite_endpoint_handler, + get_default_json_modifier(), 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(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(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(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 diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index e28d70a9..b6fbd0d5 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -1,6 +1,7 @@ #include #include +#include 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; -} \ No newline at end of file +} + +// @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; +} diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 542b7dd7..92436b74 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -2,6 +2,12 @@ #ifndef __UTILS_H #define __UTILS_H +#ifdef __cplusplus +extern "C" { +#endif + +#include + /** * @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 diff --git a/Firmware/README.md b/Firmware/README.md index 10323203..af5fac44 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -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 diff --git a/Firmware/Src/freertos.c b/Firmware/Src/freertos.c index e80163b2..0b3a2e7b 100644 --- a/Firmware/Src/freertos.c +++ b/Firmware/Src/freertos.c @@ -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 diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index 0f850cda..3ddb5701 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -6,11 +6,12 @@ */ #include -#include #include #include #include #include +#include // TODO: make serial_printf_select constant + //int _read(int file, char *data, int len) {} //int _close(int file) {} diff --git a/Firmware/Src/usbd_cdc_if.c b/Firmware/Src/usbd_cdc_if.c index 475c969e..054a912c 100644 --- a/Firmware/Src/usbd_cdc_if.c +++ b/Firmware/Src/usbd_cdc_if.c @@ -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 @@ -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; diff --git a/Firmware/legacy-protocol.md b/Firmware/legacy-protocol.md new file mode 100644 index 00000000..5693a039 --- /dev/null +++ b/Firmware/legacy-protocol.md @@ -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. diff --git a/Firmware/protocol.md b/Firmware/protocol.md new file mode 100644 index 00000000..ce22164e --- /dev/null +++ b/Firmware/protocol.md @@ -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. diff --git a/VSCodeWorkspace.code-workspace b/VSCodeWorkspace.code-workspace new file mode 100644 index 00000000..60d370af --- /dev/null +++ b/VSCodeWorkspace.code-workspace @@ -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" + } + } +} \ No newline at end of file diff --git a/tools/.vscode/launch.json b/tools/.vscode/launch.json new file mode 100644 index 00000000..a3b07eff --- /dev/null +++ b/tools/.vscode/launch.json @@ -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" + ] + } + ] +} \ No newline at end of file diff --git a/tools/demo.py b/tools/demo.py new file mode 100755 index 00000000..ab2fbe2a --- /dev/null +++ b/tools/demo.py @@ -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` diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py new file mode 100755 index 00000000..0991ee81 --- /dev/null +++ b/tools/explore_odrive.py @@ -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 ') +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='') diff --git a/tools/odrive/core.py b/tools/odrive/core.py new file mode 100644 index 00000000..c4888de7 --- /dev/null +++ b/tools/odrive/core.py @@ -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 = "= 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('