diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index 0712bb76..00000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,49 +0,0 @@ -{ - "configurations": [ - { - "name": "Win32", - "includePath": [ - "${workspaceRoot}/Firmware", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/include", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", - "${workspaceRoot}/Firmware/Drivers/DRV8301", - "${workspaceRoot}/Firmware/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", - "${workspaceRoot}/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", - "${workspaceRoot}/Firmware/Drivers/STM32F4xx_HAL_Driver/Inc", - "${workspaceRoot}/Firmware/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", - "${workspaceRoot}/Firmware/Drivers/CMSIS/Device/ST/STM32F4xx/Include", - "${workspaceRoot}/Firmware/Drivers/CMSIS/Include", - "${workspaceRoot}/Firmware/Inc", - "${workspaceRoot}/Firmware/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/lib/gcc/arm-none-eabi/6.3.1/include" - ], - "defines": [ - "_DEBUG", - "UNICODE" - ], - "intelliSenseMode": "msvc-x64", - "browse": { - "path": [ - "${workspaceRoot}/Firmware", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/include", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", - "${workspaceRoot}/Firmware/Drivers/DRV8301", - "${workspaceRoot}/Firmware/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", - "${workspaceRoot}/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", - "${workspaceRoot}/Firmware/Drivers/STM32F4xx_HAL_Driver/Inc", - "${workspaceRoot}/Firmware/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", - "${workspaceRoot}/Firmware/Drivers/CMSIS/Device/ST/STM32F4xx/Include", - "${workspaceRoot}/Firmware/Drivers/CMSIS/Include", - "${workspaceRoot}/Firmware/Inc", - "${workspaceRoot}/Firmware/MotorControl" - ], - "limitSymbolsToIncludedHeaders": true, - "databaseFilename": "" - } - } - ], - "version": 3 -} \ No newline at end of file diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json new file mode 100644 index 00000000..691314ac --- /dev/null +++ b/Firmware/.vscode/c_cpp_properties.json @@ -0,0 +1,141 @@ +{ + "configurations": [ + { + "name": "Win32", + "includePath": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl", + "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", + "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include" + ], + "defines": [ + "_DEBUG", + "UNICODE" + ], + "intelliSenseMode": "clang-x64", + "browse": { + "path": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl" + ], + "limitSymbolsToIncludedHeaders": true, + "databaseFilename": "" + } + }, + { + "name": "Linux", + "includePath": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl", + "/usr/lib/gcc/arm-none-eabi/4.9.3/include", + "/usr/lib/arm-none-eabi/include" + ], + "defines": [ + "_DEBUG", + "UNICODE" + ], + "intelliSenseMode": "clang-x64", + "browse": { + "path": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl" + ], + "limitSymbolsToIncludedHeaders": true, + "databaseFilename": "" + } + }, + { + "name": "MacOS", + "includePath": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl", + "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include", + "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include/c++/6.3.1", + "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include/c++/6.3.1/arm-none-eabi", + "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/lib/gcc/arm-none-eabi/6.3.1/include" + ], + "defines": [ + "_DEBUG", + "UNICODE" + ], + "intelliSenseMode": "clang-x64", + "browse": { + "path": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl" + ], + "limitSymbolsToIncludedHeaders": true, + "databaseFilename": "" + } + } + ], + "version": 3 +} \ No newline at end of file diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json new file mode 100644 index 00000000..747e4f1a --- /dev/null +++ b/Firmware/.vscode/launch.json @@ -0,0 +1,20 @@ +{ + // 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": [ + { + // For the Cortex-Debug extension + "type": "openocd-gdb", + "request": "launch", + "name": "Debug ODrive", + "executable": "${workspaceRoot}/build/ODriveFirmware.elf", + "configFiles": [ + "interface/stlink-v2.cfg", + "target/stm32f4x_stlink.cfg", + ], + "cwd": "${workspaceRoot}" + }, + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/Firmware/.vscode/settings.json similarity index 96% rename from .vscode/settings.json rename to Firmware/.vscode/settings.json index 1089a45c..8f45c82a 100644 --- a/.vscode/settings.json +++ b/Firmware/.vscode/settings.json @@ -1,7 +1,7 @@ { "C_Cpp.clang_format_style": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 0 }", "files.exclude": { - "Firmware/build": true + "build": true }, "files.associations": { "memory": "cpp", diff --git a/.vscode/tasks.json b/Firmware/.vscode/tasks.json similarity index 89% rename from .vscode/tasks.json rename to Firmware/.vscode/tasks.json index 3dfa660a..33956da9 100644 --- a/.vscode/tasks.json +++ b/Firmware/.vscode/tasks.json @@ -6,7 +6,7 @@ { "taskName": "build", "type": "shell", - "command": "(cd Firmware && make -j4)", + "command": "make -j4", "group": { "kind": "build", "isDefault": true @@ -21,7 +21,7 @@ { "taskName": "flash", "type": "shell", - "command": "(cd Firmware && make flash)", + "command": "make flash", "problemMatcher": [] }, { diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 4e0f1196..48bacd7a 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,10 +1,62 @@ +## [0.3.2] - 2018-02-02 -## [0.2.0] - UNRELEASED ### Added -* UART feature +* Gimbal motor mode +* Encoder index pulse support +* `resistance_calib_max_voltage` parameter + +## [0.3.1] - 2018-01-18 + +### Added +* UUID Endpoint +* Reporting of correct ODrive version on USB descriptor +* Getting started instructions for VSCode + +### Changed +* USB Product ID to 0x0D32, as it is the only Pid we were allocated on [pid.codes](http://pid.codes/1209/0D32/) +* Recommended method to debug firmware from VSCode now uses Cortex-Debug extension instead of native-debug. +* Refactor IDE instructions into separate files + +### Fixed +* Bug where the remote function calls from Python to the ODrive were not working properly. + +## [0.3] - 2017-12-18 +### 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 +* Current sense shunt values for HW v3.4 +* Check DRV chip fault line + +### Changed +* Shunt resistance values for v3.3 and earlier to include extra resistance of PCB +* Default HW revision to v3.4 +* Refactoring of control code: + * Lifted top layer of low_level.c into Axis.cpp + +## [0.2.2] - 2017-11-17 +### Fixed +* Incorrect TIM14 interrupt mapping on board v3.2 caused hard-fault + +### Changed +* GPIO communication mode now defaults to NONE + +## [0.2.1] - 2017-11-14 +### Fixed +* USB communication deadlock +* EXTI handler redefiniton in V3.2 + +### Changed +* Resistance/inductance measurement now saved dispite errors, to allow debugging + +## [0.2.0] - 2017-11-12 +### Added +* UART communication * Setting to select UART or Step/dir on GIPIO 1,2 * Basic Anti-cogging -### Changed ## [0.1.0] - 2017-08-26 ### Added diff --git a/Firmware/Inc/freertos_vars.h b/Firmware/Inc/freertos_vars.h index e0853071..c1e122a0 100644 --- a/Firmware/Inc/freertos_vars.h +++ b/Firmware/Inc/freertos_vars.h @@ -3,11 +3,15 @@ #define __FREERTOS_H // List of semaphores -extern osSemaphoreId sem_usb_irq; +osSemaphoreId sem_usb_irq; +osSemaphoreId sem_uart_dma; +osSemaphoreId sem_usb_rx; +osSemaphoreId sem_usb_tx; // List of threads -extern osThreadId thread_motor_0; -extern osThreadId thread_motor_1; -extern osThreadId thread_cmd_parse; +osThreadId thread_motor_0; +osThreadId thread_motor_1; +osThreadId thread_cmd_parse; +osThreadId thread_usb_pump; #endif /* __FREERTOS_H */ \ No newline at end of file diff --git a/Firmware/Inc/gpio.h b/Firmware/Inc/gpio.h index 280128eb..74ce09da 100644 --- a/Firmware/Inc/gpio.h +++ b/Firmware/Inc/gpio.h @@ -72,6 +72,7 @@ void MX_GPIO_Init(void); void SetGPIO12toUART(); void SetGPIO12toStepDir(); +void SetupENCIndexGPIO(); /* USER CODE END Prototypes */ diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index 911d81fe..a9650093 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -53,7 +53,8 @@ /* USER CODE BEGIN Includes */ #define HW_VERSION_MAJOR 3 -#define HW_VERSION_MINOR 3 +#define HW_VERSION_MINOR 4 +// #define HW_VERSION_HIGH_VOLTAGE true #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 @@ -63,7 +64,7 @@ /* Private define ------------------------------------------------------------*/ #define TIM_1_8_CLOCK_HZ 168000000 -#define TIM_1_8_PERIOD_CLOCKS 10192 +#define TIM_1_8_PERIOD_CLOCKS 8192 #define TIM_1_8_DEADTIME_CLOCKS 20 #define TIM_APB1_CLOCK_HZ 84000000 #define TIM_APB1_PERIOD_CLOCKS 4096 @@ -157,6 +158,12 @@ #define CURRENT_MEAS_PERIOD ((float)(2*TIM_1_8_PERIOD_CLOCKS)/(float)TIM_1_8_CLOCK_HZ) #define CURRENT_MEAS_HZ (TIM_1_8_CLOCK_HZ/(2*TIM_1_8_PERIOD_CLOCKS)) +#ifdef HW_VERSION_HIGH_VOLTAGE +#define VBUS_S_DIVIDER_RATIO 19.0f +#else +#define VBUS_S_DIVIDER_RATIO 11.0f +#endif + /* USER CODE END Private defines */ void _Error_Handler(char *, int); diff --git a/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c b/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c index b2ca5f16..345b0b0f 100644 --- a/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c +++ b/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c @@ -62,6 +62,8 @@ #include "usbd_cdc.h" #include "usbd_desc.h" #include "usbd_ctlreq.h" +#include +#include /** @addtogroup STM32_USB_DEVICE_LIBRARY @@ -669,7 +671,7 @@ static uint8_t USBD_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum) { hcdc->TxState = 0; - + osSemaphoreRelease(sem_usb_tx); return USBD_OK; } else 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 index f94ad87f..aaac53b5 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -60,6 +60,10 @@ void Axis::StateMachineLoop() { legacy_motor_ref_->thread_ready = true; bool calibration_ok = false; for (;;) { + // Keep rotor estimation up to date while idling + osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); + update_rotor(legacy_motor_ref_); + if (do_calibration_) { do_calibration_ = false; @@ -86,9 +90,6 @@ void Axis::StateMachineLoop() { 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/commands.cpp b/Firmware/MotorControl/commands.cpp index 582e2b1a..35e5651d 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -3,12 +3,14 @@ // 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 "commands.h" +#include "utils.h" #ifdef ENABLE_LEGACY_PROTOCOL #include "legacy_commands.h" @@ -23,20 +25,33 @@ #define UART_TX_BUFFER_SIZE 64 -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; - /* 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 -// static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir +#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 @@ -58,18 +73,18 @@ void motors_0_set_current_setpoint_func(void) { motors[0].set_current_setpoint_args.current_setpoint); } void motors_1_set_pos_setpoint_func(void) { - set_pos_setpoint(&motors[0], + set_pos_setpoint(&motors[1], 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], + set_vel_setpoint(&motors[1], 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], + set_current_setpoint(&motors[1], motors[1].set_current_setpoint_args.current_setpoint); } @@ -79,6 +94,9 @@ void motors_1_set_current_setpoint_func(void) { 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_property("UUID_0", (const uint32_t*)(ID_UNIQUE_ADDRESS + 0*4)), + Endpoint::make_property("UUID_1", (const uint32_t*)(ID_UNIQUE_ADDRESS + 1*4)), + Endpoint::make_property("UUID_2", (const uint32_t*)(ID_UNIQUE_ADDRESS + 2*4)), Endpoint::make_object("motor0"), Endpoint::make_property("control_mode", reinterpret_cast(&motors[0].control_mode)), Endpoint::make_property("error", reinterpret_cast(&motors[0].error)), @@ -109,6 +127,8 @@ const Endpoint endpoints[] = { 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("Iq_setpoint", &motors[0].current_control.Iq_setpoint), + Endpoint::make_property("Iq_measured", &motors[0].current_control.Iq_measured), Endpoint::make_property("Ibus", const_cast(&motors[0].current_control.Ibus)), Endpoint::close_tree(), Endpoint::make_object("encoder"), @@ -117,8 +137,9 @@ const Endpoint endpoints[] = { 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", reinterpret_cast(&motors[0].encoder.encoder_offset)), - Endpoint::make_property("encoder_state", reinterpret_cast(&motors[0].encoder.encoder_state)), + Endpoint::make_property("encoder_offset", &motors[0].encoder.encoder_offset), + Endpoint::make_property("encoder_state", &motors[0].encoder.encoder_state), + Endpoint::make_property("motor_dir", &motors[0].encoder.motor_dir), 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), @@ -163,6 +184,8 @@ const Endpoint endpoints[] = { 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("Iq_setpoint", &motors[1].current_control.Iq_setpoint), + Endpoint::make_property("Iq_measured", &motors[1].current_control.Iq_measured), Endpoint::make_property("Ibus", const_cast(&motors[1].current_control.Ibus)), Endpoint::close_tree(), Endpoint::make_object("encoder"), @@ -171,8 +194,9 @@ const Endpoint endpoints[] = { 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::make_property("encoder_offset", &motors[1].encoder.encoder_offset), + Endpoint::make_property("encoder_state", &motors[1].encoder.encoder_state), + Endpoint::make_property("motor_dir", &motors[1].encoder.motor_dir), 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), @@ -193,33 +217,43 @@ const Endpoint endpoints[] = { constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]); +#if defined(USB_PROTOCOL_NATIVE) -// The USB channel is natively packet based but on some platforms (specifically -// macOS) it's not possible to directly access the device as a USB device. -// Instead, such platforms expose the device as a serial port, however that -// breaks our packet boundaries. For now we just neglect this. If you happen to -// be limited by such a platform, you should reconsider your life choices -// or as a workaround enable this: +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; -//Oskar: Put switches like this at top of file -//#define STREAM_ON_USB +BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_sender); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - -#ifdef STREAM_ON_USB class USBSender : public StreamSink { public: int process_bytes(const uint8_t* buffer, size_t length) { // Loop to ensure all bytes get sent - // TODO: add timeout while (length) { size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; - while (CDC_Transmit_FS( + // 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) - //Oskar: we made a semaphore sem_usb_tx that guards the USB tx resource, - // that you can wait for to see if busy. Check _write in syscalls.c on devel for example use - osDelay(1); + return -1; buffer += chunk; length -= chunk; } @@ -230,46 +264,29 @@ public: } usb_sender; PacketToStreamConverter usb_packet_sender(usb_sender); -BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_packet_sender); -StreamToPacketConverter usb_stream_sink(usb_connection); +BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender); +StreamToPacketConverter usb_stream_sink(usb_channel); -#else - -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; - while (CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, length) != USBD_OK) - //Oskar: we made a semaphore sem_usb_tx that guards the USB tx resource, - // that you can wait for to see if busy. Check _write in syscalls.c on devel for example use - osDelay(1); - return 0; - } -} usb_sender; - -BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_sender); #endif +#if defined(UART_PROTOCOL_NATIVE) class UART4Sender : public StreamSink { public: int process_bytes(const uint8_t* buffer, size_t length) { - //Check length - if (length > UART_TX_BUFFER_SIZE) - return -1; - // Loop until the UART is ready - // TODO: implement ring buffer to get a more continuous stream of data - while (huart4.gState != HAL_UART_STATE_READY) - //Oskar: we made a semaphore sem_uart_dma that guards the UART tx resource, - // that you can wait for to see if busy. Check _write in syscalls.c on devel for example use - osDelay(1); - // memcpy data into uart_tx_buf - memcpy(tx_buf_, buffer, length); - // Start DMA background trasnfer - HAL_UART_Transmit_DMA(&huart4, tx_buf_, 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; } @@ -279,20 +296,28 @@ private: } uart4_sender; PacketToStreamConverter uart4_packet_sender(uart4_sender); -BidirectionalPacketBasedChannel uart4_connection(endpoints, NUM_ENDPOINTS, uart4_packet_sender); -StreamToPacketConverter UART4_stream_sink(uart4_connection); +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; } } @@ -301,6 +326,8 @@ void init_communication(void) { 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 @@ -312,36 +339,83 @@ void communication_task(void const * argument) { // 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 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; - //Oskar: we don't have to process 1 byte at a time, - // we can process up to MIN(last_rcv_idx, UART_RX_BUFFER_SIZE-1) - UART4_stream_sink.process_bytes(&c, 1); - } + 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 + } - //Oskar: Beware of changes in devel here when merging. - int USB_check_timeout = 1; - int32_t status = osSemaphoreWait(sem_usb_irq, USB_check_timeout); - if (status == osOK) { + // 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. @@ -349,31 +423,5 @@ void communication_task(void const * argument) { } } - // If we get here, then this task is done vTaskDelete(osThreadGetId()); } - -//Oskar: can you also do a ENABLE_LEGACY_PROTOCOL case for UART? -// If this has to be exclusive of the new protocol, that's fine: it -// lets us move on and upgrade the arduino library later. -// Please test that it still works on an arduino. - -void USB_receive_packet(const uint8_t *buffer, size_t length) { - //printf("[USB] got %d bytes, first is %c\r\n", length, buffer[0]); osDelay(5); -#ifdef ENABLE_LEGACY_PROTOCOL - const uint8_t* legacy_commands = (const uint8_t*)"pvcgsmo"; - while (*legacy_commands && length) { - if (buffer[0] == *(legacy_commands++)) { - //printf("[USB] process legacy command %c\r\n", buffer[0]); osDelay(5); - legacy_parse_cmd(buffer, length); - length = 0; - } - } -#endif - -#ifdef STREAM_ON_USB - usb_stream_sink.process_bytes(buffer, length); -#else - usb_connection.process_packet(buffer, length); -#endif -} diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index aa14d50b..0d9d7c87 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -1,3 +1,5 @@ +#ifndef COMMANDS_H +#define COMMANDS_H // TODO: resolve assert #define assert(expr) @@ -8,7 +10,23 @@ #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, GPIO_MODE_UART, GPIO_MODE_STEP_DIR, } GpioMode_t; @@ -18,8 +36,12 @@ extern "C" { 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/legacy_commands.c b/Firmware/MotorControl/legacy_commands.c index be473289..254f9dd7 100644 --- a/Firmware/MotorControl/legacy_commands.c +++ b/Firmware/MotorControl/legacy_commands.c @@ -1,5 +1,6 @@ /* Includes ------------------------------------------------------------------*/ #include "legacy_commands.h" +#include /* Private macros ------------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/ @@ -77,13 +78,13 @@ float* exposed_floats[] = { 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 }; bool* exposed_bools[] = { @@ -112,13 +113,13 @@ static void print_monitoring(int limit); /* Function implementations --------------------------------------------------*/ -void legacy_parse_cmd(const uint8_t* buffer, int len) { +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 = SERIAL_PRINTF_IS_USB; + serial_printf_select = response_interface; // 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 <= 63 ? len : 63] = 0; + ((uint8_t *)buffer)[len < buffer_capacity ? len : (buffer_capacity - 1)] = 0; // check incoming packet type if (buffer[0] == 'p') { @@ -145,6 +146,12 @@ void legacy_parse_cmd(const uint8_t* buffer, int len) { if (numscan == 2 && motor_number < num_motors) { set_current_setpoint(&motors[motor_number], current_feed_forward); } + } else if(buffer[0] == 'i'){ // Dump device info + // Retrieves the device signature, revision, flash size, and UUID + printf("Signature: %#x\n", STM_ID_GetSignature()); + printf("Revision: %#x\n", STM_ID_GetRevision()); + printf("Flash Size: %#x KiB\n", STM_ID_GetFlashSize()); + printf("UUID: 0x%lx%lx%lx\n", STM_ID_GetUUID(2), STM_ID_GetUUID(1), STM_ID_GetUUID(0)); } else if (buffer[0] == 'g') { // GET // g <0:float,1:int,2:bool,3:uint16> index int type = 0; @@ -229,6 +236,40 @@ void legacy_parse_cmd(const uint8_t* buffer, int len) { 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; diff --git a/Firmware/MotorControl/legacy_commands.h b/Firmware/MotorControl/legacy_commands.h index 6ba81239..bb945dab 100644 --- a/Firmware/MotorControl/legacy_commands.h +++ b/Firmware/MotorControl/legacy_commands.h @@ -1,5 +1,5 @@ -#ifndef COMMANDS_H -#define COMMANDS_H +#ifndef LEGACY_COMMANDS_H +#define LEGACY_COMMANDS_H #ifdef __cplusplus extern "C" { @@ -27,10 +27,11 @@ extern uint16_t* exposed_uint16[]; /* Exported functions --------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ -void legacy_parse_cmd(const uint8_t* buffer, int len); +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 /* COMMANDS_H */ +#endif /* LEGACY_COMMANDS_H */ diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 55a556f9..a2f90618 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -3,22 +3,22 @@ // Because of broken cmsis_os.h, we need to include arm_math first, // otherwise chip specific defines are ommited #include -#include // Sets up the correct chip specifc defines required by arm_math +#include // Sets up the correct chip specifc defines required by arm_math #define ARM_MATH_CM4 #include #include +#include +#include #include #include -#include -#include -#include -#include #include -#include +#include +#include #include +#include #include /* Private defines -----------------------------------------------------------*/ @@ -34,39 +34,53 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct -#define ENCODER_CPR (600*4) +#define ENCODER_CPR (2048 * 4) // Default resolution of CUI-AMT102 encoder #define POLE_PAIRS 7 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 +#define SHUNT_RESISTANCE (675e-6f) +#else +#define SHUNT_RESISTANCE (500e-6f) +#endif +#endif + // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public + +// NOTE: for gimbal motors, all units of A are instead V. +// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] +// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. Motor_t motors[] = { - { // M0 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration + { + // M0 + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t + .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] + .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, // .vel_setpoint = 800.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] + .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance + .vel_integrator_current = 0.0f, // [A] + .vel_limit = 20000.0f, // [counts/s] + .current_setpoint = 0.0f, // [A] + .calibration_current = 10.0f, // [A] + .resistance_calib_max_voltage = 1.0f, // [V] + .phase_inductance = 0.0f, // to be set by measure_phase_inductance + .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, .motor_timer = &htim1, - .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2}, + .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, .last_cpu_time = 0, .current_meas = {0.0f, 0.0f}, @@ -82,49 +96,59 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/0.0005f, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .motor_type = MOTOR_TYPE_HIGH_CURRENT, + // .motor_type = MOTOR_TYPE_GIMBAL, + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { - // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + // Read out max_allowed_current to see max supported value for current_lim. + // You can change DRV8301_ShuntAmpGain to get a different range. + // .current_lim = 75.0f, //[A] + .current_lim = 10.0f, //[A] + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, .v_current_control_integral_q = 0.0f, .Ibus = 0.0f, .final_v_alpha = 0.0f, .final_v_beta = 0.0f, - .Iq = 0.0f, + .Iq_setpoint = 0.0f, + .Iq_measured = 0.0f, + .max_allowed_current = 0.0f, }, // .rotor_mode = ROTOR_MODE_SENSORLESS, // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { .encoder_timer = &htim3, + .use_index = false, + .index_found = false, + .calibrated = false, + .idx_search_speed = 10.0f, // [rad/s electrical] .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] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] + .motor_dir = 1, // 1 or -1 + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] }, .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + .observer_gain = 1000.0f, // [rad/s] + .flux_state = {0.0f, 0.0f}, // [Vs] + .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] + .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] + .spin_up_current = 10.0f, // [A] + .spin_up_acceleration = 400.0f, // [rad/s^2] + .spin_up_target_vel = 400.0f, // [rad/s] }, .loop_counter = 0, .timing_log_index = 0, @@ -138,30 +162,31 @@ Motor_t motors[] = { .calib_vel_threshold = 1.0f, }, }, - { // M1 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration + { // M1 + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t + .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] + .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance + .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] + .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] + .vel_integrator_current = 0.0f, // [A] + .vel_limit = 20000.0f, // [counts/s] + .current_setpoint = 0.0f, // [A] + .calibration_current = 10.0f, // [A] + .resistance_calib_max_voltage = 1.0f, // [V] + .phase_inductance = 0.0f, // to be set by measure_phase_inductance + .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, .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, + .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, .last_cpu_time = 0, .current_meas = {0.0f, 0.0f}, .DC_calib = {0.0f, 0.0f}, @@ -176,47 +201,56 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/0.0005f, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .motor_type = MOTOR_TYPE_HIGH_CURRENT, + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { - // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + // Read out max_allowed_current to see max supported value for current_lim. + // You can change DRV8301_ShuntAmpGain to get a different range. + // .current_lim = 75.0f, //[A] + .current_lim = 10.0f, //[A] + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, .v_current_control_integral_q = 0.0f, .Ibus = 0.0f, .final_v_alpha = 0.0f, .final_v_beta = 0.0f, - .Iq = 0.0f, + .Iq_setpoint = 0.0f, + .Iq_measured = 0.0f, + .max_allowed_current = 0.0f, }, .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { .encoder_timer = &htim4, + .use_index = false, + .index_found = false, + .calibrated = false, + .idx_search_speed = 10.0f, // [rad/s electrical] .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] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] + .motor_dir = 1, // 1 or -1 + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] }, .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + .observer_gain = 1000.0f, // [rad/s] + .flux_state = {0.0f, 0.0f}, // [Vs] + .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] + .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] + .spin_up_current = 10.0f, // [A] + .spin_up_acceleration = 400.0f, // [rad/s^2] + .spin_up_target_vel = 400.0f, // [rad/s] }, .loop_counter = 0, .timing_log_index = 0, @@ -240,7 +274,7 @@ static const float current_meas_period = CURRENT_MEAS_PERIOD; static const int current_meas_hz = CURRENT_MEAS_HZ; /* Private variables ---------------------------------------------------------*/ -static float brake_resistance = 0.47f; // [ohm] +static float brake_resistance = 0.47f; // [ohm] /* Function implementations --------------------------------------------------*/ @@ -288,7 +322,7 @@ uint16_t check_timing(Motor_t* motor) { timing = TIM_1_8_PERIOD_CLOCKS + delta; } - if(++(motor->timing_log_index) == TIMING_LOG_SIZE){ + if (++(motor->timing_log_index) == TIMING_LOG_SIZE) { motor->timing_log_index = 0; } motor->timing_log[motor->timing_log_index] = timing; @@ -296,7 +330,7 @@ uint16_t check_timing(Motor_t* motor) { return timing; } -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); @@ -307,18 +341,17 @@ void global_fault(int error){ *(motors[i].axis_legacy.enable_control) = false; } // disable brake resistor - update_brake_current(0.0f); + set_brake_current(0.0f); } 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; + 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; float current = shunt_volt * motor->shunt_conductance; return current; } - //-------------------------------- // Initalisation //-------------------------------- @@ -335,6 +368,10 @@ void init_motor_control() { // Start Encoders HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); + //TODO: Enable index on only one channel + if (motors[0].encoder.use_index || motors[1].encoder.use_index) { + SetupENCIndexGPIO(); + } // Wait for current sense calibration to converge // TODO make timing a function of calibration filter tau @@ -343,42 +380,50 @@ void init_motor_control() { // Set up the gate drivers void DRV8301_setup(Motor_t* motor) { - DRV8301_Obj* gate_driver = &motor->gate_driver; - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; + DRV8301_Obj* gate_driver = &motor->gate_driver; + DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - DRV8301_enable(gate_driver); - DRV8301_setupSpi(gate_driver, local_regs); + DRV8301_enable(gate_driver); + DRV8301_setupSpi(gate_driver, local_regs); - // TODO we can use reporting only if we actually wire up the nOCTW pin - local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; - // Overcurrent set to approximately 150A at 100degC. This may need tweaking. - local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; - // 20V/V on 500uOhm gives a range of +/- 150A - // 40V/V on 500uOhm gives a range of +/- 75A - local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + // TODO we can use reporting only if we actually wire up the nOCTW pin + local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; + // Overcurrent set to approximately 150A at 100degC. This may need tweaking. + local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; + // 20V/V on 500uOhm gives a range of +/- 150A + // 40V/V on 500uOhm gives a range of +/- 75A + // 20V/V on 666uOhm gives a range of +/- 110A + // 40V/V on 666uOhm gives a range of +/- 55A + local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; - switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: - motor->phase_current_rev_gain = 1.0f/10.0f; - break; - case DRV8301_ShuntAmpGain_20VpV: - motor->phase_current_rev_gain = 1.0f/20.0f; - break; - case DRV8301_ShuntAmpGain_40VpV: - motor->phase_current_rev_gain = 1.0f/40.0f; - break; - case DRV8301_ShuntAmpGain_80VpV: - motor->phase_current_rev_gain = 1.0f/80.0f; - break; - } + switch (local_regs->Ctrl_Reg_2.GAIN) { + case DRV8301_ShuntAmpGain_10VpV: + motor->phase_current_rev_gain = 1.0f / 10.0f; + break; + case DRV8301_ShuntAmpGain_20VpV: + motor->phase_current_rev_gain = 1.0f / 20.0f; + break; + case DRV8301_ShuntAmpGain_40VpV: + motor->phase_current_rev_gain = 1.0f / 40.0f; + break; + case DRV8301_ShuntAmpGain_80VpV: + motor->phase_current_rev_gain = 1.0f / 80.0f; + break; + } - local_regs->SndCmd = true; - DRV8301_writeData(gate_driver, local_regs); - local_regs->RcvCmd = true; - DRV8301_readData(gate_driver, local_regs); + float margin = 0.90f; + float max_input = margin * 0.3f * motor->shunt_conductance; + float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; + motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); + + local_regs->SndCmd = true; + DRV8301_writeData(gate_driver, local_regs); + local_regs->RcvCmd = true; + DRV8301_readData(gate_driver, local_regs); } -void start_adc_pwm(){ +void start_adc_pwm() { // Enable ADC and interrupts __HAL_ADC_ENABLE(&hadc1); __HAL_ADC_ENABLE(&hadc2); @@ -398,7 +443,7 @@ void start_adc_pwm(){ start_pwm(&htim1); start_pwm(&htim8); // TODO: explain why this offset - sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS/2 - 1*128); + sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); // Motor output starts in the disabled state __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); @@ -406,14 +451,14 @@ void start_adc_pwm(){ // Start brake resistor PWM in floating output configuration htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS+1; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); } -void start_pwm(TIM_HandleTypeDef* htim){ +void start_pwm(TIM_HandleTypeDef* htim) { // Init PWM - int half_load = TIM_1_8_PERIOD_CLOCKS/2; + int half_load = TIM_1_8_PERIOD_CLOCKS / 2; htim->Instance->CCR1 = half_load; htim->Instance->CCR2 = half_load; htim->Instance->CCR3 = half_load; @@ -431,8 +476,7 @@ 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) { - + 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); @@ -477,7 +521,6 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, htim_b->Instance->BDTR |= MOE_store_b; } - //-------------------------------- // IRQ Callbacks //-------------------------------- @@ -487,30 +530,45 @@ void step_cb(uint16_t GPIO_Pin) { GPIO_PinState dir_pin; float dir; switch (GPIO_Pin) { - case GPIO_1_Pin: - //M0 stepped - if (motors[0].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[0].pos_setpoint += dir * motors[0].counts_per_step; - } - break; - case GPIO_3_Pin: - //M1 stepped - if (motors[1].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[1].pos_setpoint += dir * motors[1].counts_per_step; - } - break; - default: - global_fault(ERROR_UNEXPECTED_STEP_SRC); - break; + case GPIO_1_Pin: + //M0 stepped + if (motors[0].enable_step_dir) { + dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); + dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + motors[0].pos_setpoint += dir * motors[0].counts_per_step; + } + break; + case GPIO_3_Pin: + //M1 stepped + if (motors[1].enable_step_dir) { + dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); + dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + motors[1].pos_setpoint += dir * motors[1].counts_per_step; + } + break; + default: + global_fault(ERROR_UNEXPECTED_STEP_SRC); + break; + } +} + +// Triggered when an encoder passes over the "Index" pin +void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { + Motor_t* motor = &motors[motor_index]; + if (!motor->encoder.index_found) { + setEncoderCount(motor, 0); + motor->encoder.index_found = true; + } + //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default + if(GPIO_Pin == M0_ENC_Z_Pin){ + HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); + } else { + HAL_NVIC_DisableIRQ(EXTI3_IRQn); } } void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * 11.0f / (float)(1<<12); + static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12); // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); vbus_voltage = ADCValue * voltage_scale; @@ -519,11 +577,11 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. // TODO: Document how the phasing is done, link to timing diagram void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - #define calib_tau 0.2f //@TOTO make more easily configurable +#define calib_tau 0.2f //@TOTO make more easily configurable static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; // Ensure ADCs are expected ones to simplify the logic below - if (!(hadc == &hadc2 || hadc == &hadc3)){ + if (!(hadc == &hadc2 || hadc == &hadc3)) { global_fault(ERROR_ADC_FAILED); return; }; @@ -534,7 +592,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // If we are counting down, we just sampled in SVM vector 7, with zero current Motor_t* motor = injected ? &motors[0] : &motors[1]; bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; - + bool current_meas_not_DC_CAL; if (motor == &motors[1] && counting_down) { // We are measuring M1 DC_CAL here @@ -612,19 +670,18 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } } - //-------------------------------- // Measurement and calibration //-------------------------------- // TODO check Ibeta balance to verify good motor connection bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) { - static const float kI = 10.0f; //[(V/s)/A] + 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; for (int i = 0; i < num_test_cycles; ++i) { osEvent evt = osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT); - if (evt.status != osEventSignal){ + if (evt.status != osEventSignal) { motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; return false; } @@ -638,7 +695,7 @@ bool measure_phase_resistance(Motor_t* motor, float test_current, float max_volt // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_PHASE_RESISTANCE_TIMING; return false; } @@ -648,11 +705,11 @@ bool measure_phase_resistance(Motor_t* motor, float test_current, float max_volt queue_voltage_timings(motor, 0.0f, 0.0f); float R = test_voltage / test_current; + motor->phase_resistance = R; if (fabs(test_voltage) == fabs(max_voltage) || R < 0.01f || R > 1.0f) { motor->error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE; return false; } - motor->phase_resistance = R; return true; } @@ -674,7 +731,7 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_PHASE_INDUCTANCE_TIMING; return false; } @@ -689,13 +746,13 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h // However, the discretisation in the current control loop inverts the same discrepancy float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); float L = v_L / dI_by_dt; - + + motor->phase_inductance = L; // TODO arbitrary values set for now if (L < 1e-6f || L > 500e-6f) { motor->error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE; return false; } - motor->phase_inductance = L; return true; } @@ -704,15 +761,15 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h 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; + static const float dt_step = 1.0f / 500.0f; static const float scan_range = 4.0f * M_PI; - const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) + const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; int32_t encvaluesum = 0; - // go to encoder zero phase for start_lock_duration to get ready to scan - for (int i = 0; i < start_lock_duration*current_meas_hz; ++i) { + // go to motor zero phase for start_lock_duration to get ready to scan + for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; @@ -721,13 +778,13 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } // scan forwards for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { - for (int i = 0; i < dt_step*(float)current_meas_hz; ++i) { + for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; @@ -746,13 +803,13 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } // scan backwards for (float ph = scan_range / 2.0f; ph > -scan_range / 2.0f; ph -= step_size) { - for (int i = 0; i < dt_step*(float)current_meas_hz; ++i) { + for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; @@ -760,37 +817,51 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { int offset = encvaluesum / (num_steps * 2); motor->encoder.encoder_offset = offset; + motor->encoder.calibrated = true; return true; } -bool motor_calibration(Motor_t* motor){ +bool motor_calibration(Motor_t* motor) { motor->error = ERROR_NO_ERROR; - // #warning(hardcoded values for SK3-5065-280kv!) - // float R = 0.0332548246f; - // float L = 7.97315806e-06f; - - if (!measure_phase_resistance(motor, motor->calibration_current, 1.0f)) - return false; - if (!measure_phase_inductance(motor, -1.0f, 1.0f)) - return false; - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) + float R_calib_max_voltage = motor->resistance_calib_max_voltage; + float enc_calibration_voltage = 0.0f; + if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (!measure_phase_resistance(motor, motor->calibration_current, R_calib_max_voltage)) return false; + enc_calibration_voltage = motor->calibration_current * motor->phase_resistance; + + if (!measure_phase_inductance(motor, -R_calib_max_voltage, R_calib_max_voltage)) + return false; + } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { + enc_calibration_voltage = motor->calibration_current; + } else { + return false; } - + + if (motor->rotor_mode == ROTOR_MODE_ENCODER || + motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { + if (motor->encoder.use_index && !motor->encoder.index_found) + if (!scan_for_enc_idx(motor, + (float)(motor->encoder.motor_dir) * motor->encoder.idx_search_speed, + enc_calibration_voltage)) + return false; + if (!motor->encoder.calibrated) + if (!calib_enc_offset(motor, enc_calibration_voltage)) + return false; + } + // Calculate current control gains - float current_control_bandwidth = 1000.0f; // [rad/s] + float current_control_bandwidth = 1000.0f; // [rad/s] motor->current_control.p_gain = current_control_bandwidth * motor->phase_inductance; float plant_pole = motor->phase_resistance / motor->phase_inductance; motor->current_control.i_gain = plant_pole * motor->current_control.p_gain; // Calculate encoder pll gains - float encoder_pll_bandwidth = 1000.0f; // [rad/s] + float encoder_pll_bandwidth = 1000.0f; // [rad/s] motor->encoder.pll_kp = 2.0f * encoder_pll_bandwidth; // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)){ + if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)) { motor->error = ERROR_CALIBRATION_TIMING; return false; } @@ -814,8 +885,8 @@ bool motor_calibration(Motor_t* motor){ bool anti_cogging_calibration(Motor_t* motor) { if (motor->anticogging.calib_anticogging && motor->anticogging.cogging_map != NULL) { float pos_err = motor->anticogging.index - motor->encoder.pll_pos; - if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && - fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { + if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && + fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { motor->anticogging.cogging_map[motor->anticogging.index++] = motor->vel_integrator_current; } if (motor->anticogging.index < ENCODER_CPR) { @@ -836,56 +907,33 @@ bool anti_cogging_calibration(Motor_t* motor) { // Test functions //-------------------------------- -__attribute__((unused)) -void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { +bool scan_for_enc_idx(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); + + if (motor->encoder.index_found) + return true; + float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_SCAN_MOTOR_TIMING; - return; + return false; } } } } -//TODO integrate as mode in main control loop -__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); - - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float v_alpha = c*v_d - s*v_q; - float v_beta = c*v_q + s*v_d; - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ - motor->error = ERROR_FOC_VOLTAGE_TIMING; - return; - } - ++(motor->loop_counter); - } -} - - //-------------------------------- // Main motor control //-------------------------------- void update_rotor(Motor_t* motor) { - switch (motor->rotor_mode) { case ROTOR_MODE_ENCODER: case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS: { @@ -914,11 +962,10 @@ void update_rotor(Motor_t* motor) { encoder->pll_pos += current_meas_period * encoder->pll_kp * delta_pos; encoder->pll_vel += current_meas_period * encoder->pll_ki * delta_pos; } - // Drop through to sensorless if also testing - if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - break; + // Drop through to sensorless if also testing + if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) + break; case ROTOR_MODE_SENSORLESS: { - // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf // In particular, equation 8 (and by extension eqn 4 and 6). @@ -933,8 +980,7 @@ void update_rotor(Motor_t* motor) { // Clarke transform float I_alpha_beta[2] = { -motor->current_meas.phB - motor->current_meas.phC, - one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC) - }; + one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC)}; // alpha-beta vector operations float eta[2]; @@ -998,8 +1044,8 @@ void update_rotor(Motor_t* motor) { } break; default: - //TODO error handling - break; + //TODO error handling + break; } } @@ -1019,9 +1065,9 @@ bool using_sensorless(Motor_t* motor) { } float get_rotor_phase(Motor_t* motor) { - if (using_encoder(motor)) + if (using_encoder(motor)) return motor->encoder.phase; - else if (using_sensorless(motor)) + else if (using_sensorless(motor)) return motor->sensorless.phase; else //TODO error handling @@ -1029,15 +1075,26 @@ float get_rotor_phase(Motor_t* motor) { } float get_pll_vel(Motor_t* motor) { - if (using_encoder(motor)) + if (using_encoder(motor)) return motor->encoder.pll_vel; - else if (using_sensorless(motor)) + else if (using_sensorless(motor)) return motor->sensorless.pll_vel; else //TODO error handling return 0.0f; } +// 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->encoder.encoder_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) { @@ -1055,7 +1112,6 @@ bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { } 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; @@ -1068,7 +1124,7 @@ bool spin_up_sensorless(Motor_t* motor) { for (float x = 0.0f; x < 1.0f; x += ramp_step) { phase = wrap_pm_pi(ramp_up_distance * x); I_mag = motor->sensorless.spin_up_current * x; - if(!spin_up_timestep(motor, phase, I_mag)) + if (!spin_up_timestep(motor, phase, I_mag)) return false; } @@ -1076,7 +1132,7 @@ bool spin_up_sensorless(Motor_t* motor) { while (vel < motor->sensorless.spin_up_target_vel) { vel += motor->sensorless.spin_up_acceleration * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - if(!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) + if (!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) return false; } @@ -1092,7 +1148,16 @@ bool spin_up_sensorless(Motor_t* motor) { // TODO: check pll vel (abs ratio, 0.8) } -void update_brake_current(float brake_current) { +void update_brake_current() { + float Ibus_sum = 0.0f; + for (int i = 0; i < num_motors; ++i) { + Ibus_sum += motors[i].current_control.Ibus; + } + // Note: set_brake_current will clip negative values to 0.0f + set_brake_current(-Ibus_sum); +} + +void set_brake_current(float brake_current) { if (brake_current < 0.0f) brake_current = 0.0f; float brake_duty = brake_current * brake_resistance / vbus_voltage; @@ -1106,7 +1171,7 @@ void update_brake_current(float brake_current) { // To avoid race condition, first reset timings to safe state // ch3 is low side, ch4 is high side htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS+1; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; htim2.Instance->CCR3 = low_off; htim2.Instance->CCR4 = high_on; } @@ -1126,9 +1191,28 @@ void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { queue_modulation_timings(motor, mod_alpha, mod_beta); } +bool FOC_voltage(Motor_t* motor, float v_d, float v_q) { + float phase = get_rotor_phase(motor); + float c = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float v_alpha = c*v_d - s*v_q; + float v_beta = c*v_q + s*v_d; + queue_voltage_timings(motor, v_alpha, v_beta); + + // Check we meet deadlines after queueing + if (!(check_timing(motor) < motor->control_deadline)) { + motor->error = ERROR_FOC_VOLTAGE_TIMING; + return false; + } + return true; +} + bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { Current_control_t* ictrl = &motor->current_control; + // For Reporting + ictrl->Iq_setpoint = Iq_des; + // Clarke transform float Ialpha = -motor->current_meas.phB - motor->current_meas.phC; float Ibeta = one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC); @@ -1137,8 +1221,9 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); float s = arm_sin_f32(phase); - float Id = c*Ialpha + s*Ibeta; - float Iq = c*Ibeta - s*Ialpha; + float Id = c * Ialpha + s * Ibeta; + float Iq = c * Ibeta - s * Ialpha; + ictrl->Iq_measured = Iq; // Current error float Ierr_d = Id_des - Id; @@ -1156,9 +1241,8 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Vector modulation saturation, lock integrator if saturated // TODO make maximum modulation configurable - float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f/sqrtf(mod_d*mod_d + mod_q*mod_q); - if (mod_scalefactor < 1.0f) - { + float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); + if (mod_scalefactor < 1.0f) { mod_d *= mod_scalefactor; mod_q *= mod_scalefactor; // TODO make decayfactor configurable @@ -1172,22 +1256,9 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Compute estimated bus current ictrl->Ibus = mod_d * Id + mod_q * Iq; - // If this is last motor, update brake resistor duty - // if (motor == &motors[num_motors-1]) { - // Above check doesn't work if last motor is executing voltage control - // TODO trigger this update in control_motor_loop instead, - // and make voltage control a control mode in it. - float Ibus_sum = 0.0f; - for (int i = 0; i < num_motors; ++i) { - Ibus_sum += motors[i].current_control.Ibus; - } - // Note: function will clip negative values to 0.0f - update_brake_current(-Ibus_sum); - // } - // Inverse park transform - float mod_alpha = c*mod_d - s*mod_q; - float mod_beta = c*mod_q + s*mod_d; + float mod_alpha = c * mod_d - s * mod_q; + float mod_beta = c * mod_q + s * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) ictrl->final_v_alpha = mod_to_V * mod_alpha; @@ -1198,21 +1269,32 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_FOC_TIMING; return false; } return true; } +//Returns true if the fault line is asserted +bool check_DRV_fault(Motor_t* motor) { + //TODO: make this pin configurable per motor ch + GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(nFAULT_GPIO_Port, nFAULT_Pin); + return (nFAULT_state == GPIO_PIN_RESET) ? true : false; +} + 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){ + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; break; } + if (check_DRV_fault(motor)) { + motor->error = ERROR_DRV_FAULT; + break; + } update_rotor(motor); - anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking + anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking // Position control // TODO Decide if we want to use encoder or pll position here @@ -1228,7 +1310,7 @@ void control_motor_loop(Motor_t* motor) { // Velocity limiting float vel_lim = motor->vel_limit; - if (vel_des > vel_lim) vel_des = vel_lim; + if (vel_des > vel_lim) vel_des = vel_lim; if (vel_des < -vel_lim) vel_des = -vel_lim; // Velocity control @@ -1237,12 +1319,12 @@ void control_motor_loop(Motor_t* motor) { // Anti-cogging is enabled after calibration // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == ENCODER_CPR - 1) - if(motor->anticogging.use_anticogging){ + if (motor->anticogging.use_anticogging) { Iq += motor->anticogging.cogging_map[mod(motor->encoder.pll_pos, ENCODER_CPR)]; } float v_err = vel_des - get_pll_vel(motor); - if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { + if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { Iq += motor->vel_gain * v_err; } @@ -1256,7 +1338,7 @@ void control_motor_loop(Motor_t* motor) { } // Current limiting - float Ilim = motor->current_control.current_lim; + float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->current_control.max_allowed_current); bool limited = false; if (Iq > Ilim) { limited = true; @@ -1268,7 +1350,7 @@ void control_motor_loop(Motor_t* motor) { } // Velocity integrator (behaviour dependent on limiting) - if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL ) { + if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL) { // reset integral if not in use motor->vel_integrator_current = 0.0f; } else { @@ -1280,16 +1362,26 @@ void control_motor_loop(Motor_t* motor) { } } - motor->current_control.Iq = Iq; // Execute current command - if(!FOC_current(motor, 0.0f, Iq)){ - break; // in case of error exit loop, motor->error has been set by FOC_current + if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if(!FOC_current(motor, 0.0f, Iq)){ + break; // in case of error exit loop, motor->error has been set by FOC_current + } + } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { + //In gimbal motor mode, current is reinterptreted as voltage. + if(!FOC_voltage(motor, 0.0f, Iq)){ + break; // in case of error exit loop, motor->error has been set by FOC_voltage + } + } else { + motor->error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + break; } + update_brake_current(); ++(motor->loop_counter); } //We are exiting control, reset Ibus, and update brake current - //TODO update brake current from all motors in 1 func - //TODO reset this motor Ibus, then call from here + motor->current_control.Ibus = 0.0f; + update_brake_current(); } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 8ac58b27..9045b1d9 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -48,6 +48,8 @@ typedef enum { ERROR_UNEXPECTED_STEP_SRC, ERROR_POS_CTRL_DURING_SENSORLESS, ERROR_SPIN_UP_TIMEOUT, + ERROR_DRV_FAULT, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, } Error_t; // Note: these should be sorted from lowest level of control to @@ -59,6 +61,12 @@ typedef enum { CTRL_MODE_POSITION_CONTROL } Motor_control_mode_t; +typedef enum { + MOTOR_TYPE_HIGH_CURRENT, + // MOTOR_TYPE_LOW_CURRENT, //Not yet implemented + MOTOR_TYPE_GIMBAL +} Motor_type_t; + typedef struct { float phB; float phC; @@ -74,7 +82,9 @@ typedef struct { // Voltage applied at end of cycle: float final_v_alpha; // [V] float final_v_beta; // [V] - float Iq; + float Iq_setpoint; + float Iq_measured; + float max_allowed_current; } Current_control_t; typedef enum { @@ -101,10 +111,14 @@ typedef struct { typedef struct { TIM_HandleTypeDef* encoder_timer; + bool use_index; + bool index_found; + bool calibrated; + float idx_search_speed; int encoder_cpr; - int encoder_offset; - int encoder_state; - int motor_dir; // 1/-1 for fwd/rev alignment to encoder. + int32_t encoder_offset; + int32_t encoder_state; + int32_t motor_dir; // 1/-1 for fwd/rev alignment to encoder. float phase; float pll_pos; float pll_vel; @@ -122,7 +136,7 @@ typedef struct { 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; @@ -132,6 +146,7 @@ typedef struct { float vel_limit; float current_setpoint; float calibration_current; + float resistance_calib_max_voltage; float phase_inductance; float phase_resistance; osThreadId motor_thread; @@ -147,6 +162,7 @@ typedef struct { Iph_BC_t DC_calib; DRV8301_Obj gate_driver; DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers + Motor_type_t motor_type; float shunt_conductance; float phase_current_rev_gain; //Reverse gain for ADC to Amps Current_control_t current_control; @@ -192,11 +208,15 @@ void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_for void set_current_setpoint(Motor_t* motor, float current_setpoint); void step_cb(uint16_t GPIO_Pin); +void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index); 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); bool motor_calibration(Motor_t* motor); @@ -217,11 +237,11 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, 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 scan_for_enc_idx(Motor_t* motor, float v_d, float v_q); 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); @@ -229,9 +249,11 @@ 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 update_brake_current(); +void set_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_voltage(Motor_t* motor, float v_d, float v_q); bool FOC_current(Motor_t* motor, float Id_des, float Iq_des); void control_motor_loop(Motor_t* motor); diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index 1ab4577b..cde1e2ab 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -1,49 +1,5 @@ /* -* # ODrive Communication Protocol # -* -* Communicating with an ODrive consists of a series of endpoint operations. -* An endpoint can be any data representation that can be serialized. -* 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. -* -* ## Stream format: ## -* (For instance UART) -* -* 1. sync byte -* 2. packet length (0-127, larger values are reserved) -* 3. crc8(sync byte + packet length) -* 4. packet (as per below) -* 5. crc16(packet) -* -* ## Packet format: ## -* (For instance USB) -* -* __Request__ -* -* 1. seq-no, MSB = 0 -* 2. endpoint-id, MSB = "expect ack" -* 3. expected_response_size -* 4. payload (contains offset if required) -* 5. crc16(protocol_version + JSON) or just protocol_version for endpoint 0 -* -* __Response__ -* -* 1. seq-no, MSB = 1 -* 2. payload -* +see protocol.md for the protocol specification */ #ifndef __PROTOCOL_HPP @@ -68,6 +24,8 @@ constexpr uint16_t PROTOCOL_VERSION = 1; 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); @@ -166,8 +124,8 @@ static inline T read_le(const uint8_t** buffer, size_t* length) { 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: add deadline parameter. Currently all implementations block until they can send everything. // 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; }; @@ -175,8 +133,8 @@ public: 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 - // TODO: add deadline parameter. Currently all implementations block until they can send everything. 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. 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 7170b8ca..92436b74 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -2,6 +2,74 @@ #ifndef __UTILS_H #define __UTILS_H +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/** + * @brief Unique ID register address location + */ +#define ID_UNIQUE_ADDRESS (0x1FFF7A10) + +/** + * @brief Flash size register address + */ +#define ID_FLASH_ADDRESS (0x1FFF7A22) + +/** + * @brief Device ID register address + */ +#define ID_DBGMCU_IDCODE (0xE0042000) + +/** + * "Returns" the device signature + * + * Possible returns: + * - 0x0413: STM32F405xx/07xx and STM32F415xx/17xx) + * - 0x0419: STM32F42xxx and STM32F43xxx + * - 0x0423: STM32F401xB/C + * - 0x0433: STM32F401xD/E + * - 0x0431: STM32F411xC/E + * + * Returned data is in 16-bit mode, but only bits 11:0 are valid, bits 15:12 are always 0. + * Defined as macro + */ +#define STM_ID_GetSignature() ((*(uint16_t *)(ID_DBGMCU_IDCODE)) & 0x0FFF) + +/** + * "Returns" the device revision + * + * Revisions possible: + * - 0x1000: Revision A + * - 0x1001: Revision Z + * - 0x1003: Revision Y + * - 0x1007: Revision 1 + * - 0x2001: Revision 3 + * + * Returned data is in 16-bit mode. + */ +#define STM_ID_GetRevision() (*(uint16_t *)(ID_DBGMCU_IDCODE + 2)) + +/** +* "Returns" the Flash size +* +* Returned data is in 16-bit mode, returned value is flash size in kB (kilo bytes). +*/ +#define STM_ID_GetFlashSize() (*(uint16_t *)(ID_FLASH_ADDRESS)) + +/** + * "Returns" the given 32-bit value of the UUID. + * + * Parameters: + * - uint8_t x: + * Value between 0 and 2, corresponding to 4-bytes you want to read from 96bits (12bytes) + * + * Returned data is 32-bit + */ +#define STM_ID_GetUUID(x) ((x >= 0 && x < 3) ? (*(uint32_t *)(ID_UNIQUE_ADDRESS + 4 * (x))) : 0) + #ifndef M_PI #define M_PI 3.14159265358979323846f #endif @@ -20,4 +88,11 @@ float wrap_pm_pi(float theta); float fast_atan2(float y, float x); int mod(int dividend, int divisor); -#endif //__UTILS_H +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 ec9bf71e..f9ca56f1 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -1,14 +1,22 @@ # ODriveFirmware +If you wish to use the latest release, please use the `master` branch (this is the default branch GitHub will present you with). + +If you are a developer, you are encouraged to use the `devel` branch, as it contains the latest features. + +The project is under active development, so make sure to check the [Changelog](CHANGELOG.md) to keep track of updates. + ### Table of contents - [Configuring parameters](#configuring-parameters) - [Compiling and downloading firmware](#compiling-and-downloading-firmware) -- [Communicating over USB](#communicating-over-usb) +- [Setting up an IDE](#setting-up-an-ide) +- [Continuing without an IDE](#no-ide-instructions) +- [Communicating over USB or UART](#communicating-over-usb-or-uart) +- [Encoder Calibration](#encoder-calibration) - [Generating startup code](#generating-startup-code) -- [Setting up Eclipse development environment](#setting-up-eclipse-development-environment) - [Notes for Contributors](#notes-for-contributors) @@ -21,29 +29,66 @@ The first thing to set is your board hardware version, located at the top of [In #define HW_VERSION_MAJOR 3 #define HW_VERSION_MINOR 2 ``` +If you are using the 48V version of ODrive, you should also uncomment this line +```C +#define HW_VERSION_HIGH_VOLTAGE true +``` ### 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 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. +* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). + +### Motor Modes +The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For high-torque gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. + +**Further detail:** + +If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`. +If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (omega * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`. + +If 100's of mA current noise is "large" for you, and you intend to spin the motor fast, then you need to replace the shunt resistors on the ODrive. ### 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. + * Note: The motor current and the current drawn from the power supply is not the same in general. You should not look at the power supply current to see what is going on with the motor current. +* The velocity limit: `.vel_limit = 20000.0f, // [counts/s]`. The motor will be limited to this speed; again the default value is quite slow. +* You can change `.calibration_current` to the largest value you feel comfortable leaving running through the motor continously when the motor is stationary. The motion control gains are currently manually tuned: * `.pos_gain = 20.0f, // [(counts/s) / counts]` @@ -63,8 +108,8 @@ An upcoming feature will enable automatic tuning. Until then, here is a rough tu 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. +

## Compiling and downloading firmware - ### Getting a programmer Get a programmer that supports SWD (Serial Wire Debugging) and is ST-link v2 compatible. You can get them really cheap on [eBay](http://www.ebay.co.uk/itm/ST-Link-V2-Emulator-Downloader-Programming-Mini-Unit-STM8-STM32-with-20CM-Line-/391173940927?hash=item5b13c8a6bf:g:3g8AAOSw~OdVf-Tu) or many other places. @@ -91,6 +136,15 @@ Install the following: * [Make for Windows](http://gnuwin32.sourceforge.net/packages/make.htm). Make is used to script the compilation process. Download and run the complete package setup program. Add the path of the binaries to your PATH environment variable. For me this was at `C:\Program Files (x86)\GnuWin32\bin`. For details on how to set your path envirment in windows see [these instructions.](https://www.java.com/en/download/help/path.xml) * OpenOCD. Follow the instructions at [GNU ARM Eclipse - How to install the OpenOCD binaries](http://gnuarmeclipse.github.io/openocd/install/), including the part about ST-LINK/V2 drivers. Add the path of the binaries to your PATH environment variable. For me this was at `C:\Program Files\GNU ARM Eclipse\OpenOCD\0.10.0-201704182147-dev\bin`. +

+## Setting up an IDE +ODrive is a Makefile project. It does not require an IDE, but the open-source IDE VSCode is recommended. It is also possible to use Eclipse. If you'd like to go that route, please see the respective configuration document: + +* [Configuring VSCode](configuring-vscode.md) +* [Configuring Eclipse](configuring-eclipse.md) + +

+## No IDE Instructions After installing all of the above, open a Git Bash shell. Continue at section [Building the firmware](#building-the-firmware). ### Building the firmware @@ -100,104 +154,84 @@ After installing all of the above, open a Git Bash shell. Continue at section [B ### Flashing the firmware * **Make sure you have [configured the parameters first](#configuring-parameters)** -* Connect `SWD`, `SWC`, and `GND` on connector J2 to the programmer. +* Connect `GND`, `SWD`, and `SWC` on connector J2 to the programmer. Note: Always plug in `GND` first! * 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). +* 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. -## Communicating over USB -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 installed first. +

+## Communicating over USB or UART +Warning: If testing USB or UART communication for the first time it is recommend that your motors are free to spin continuously and are not connected to a drivetrain with limited travel. +### From Linux/Windows/macOS +There are two example python scripts to help you get started with controlling the ODrive using python. One will drop you into an interactive shell to query settings, parameters, and variables, and let you send setpoints manually ([tools/explore_odrive.py](tools/explore_odrive.py)). The other is a demo application to show you how to control the ODrive programmatically ([tools/demo.py](tools/demo.py)). Below follows a step-by-step guide on how to run these. -* Assuming you already have Python, install dependencies: + +* __Windows__: It is recommended to use a Unix style command prompt, such as Git Bash that comes with [Git for windows](https://git-scm.com/download/win). + +1. [Install Python 3](https://www.python.org/downloads/), then install dependencies pyusb and pyserial: ``` -pip install pyusb pyserial prompt_toolkit +pip install pyusb pyserial ``` -* __Linux__: set up USB permissions +* Note: If you have python2 and python3 installed concurrently then you must specifiy that we wish to target python3. This is done as follows: + * __Linux__: Use `pip3` instead of `pip` in the above command. + * __Windows__: Use the full path of the Python3 pip, yeilding something like: + `C:\Users\YOUR_USERNAME\AppData\Local\Programs\Python\Python36-32\Scripts\pip install pyusb pyserial` +* If you have trouble with this step then refer to [this walkthrough.](https://www.youtube.com/watch?v=jnpC_Ib_lbc) + +2. __Linux__: set up USB permissions ``` 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 ``` -* Power the ODrive board (as per the [Flashing the firmware](#flashing-the-firmware) step) -* Plug in a USB cable into the microUSB connector on ODrive, and connect it to your PC -* __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` +3. Power the ODrive board (as per the [Flashing the firmware](#flashing-the-firmware) step). +4. Plug in a USB cable into the microUSB connector on ODrive, and connect it to your PC. +5. __Windows__: Use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb-win32. + * 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. +6. Open the bash prompt in the `ODrive/tools/` folder. +7. Run `python3 demo.py` or `python3 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`. -### 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. +### From Arduino +[See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino) -#### 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! -``` +### Other platforms +See the [protocol specification](protocol.md) or the [legacy protocol specification](legacy-protocol.md). -#### 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. +

+## Encoder Calibration +By default the encoder-to-motor calibration will run on every startup. During encoder calibration the rotor must be allowed to rotate without any biased load during startup. That means mass and weak friction loads are fine, but gravity or spring loads are not okay. -Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. +### Encoder with Index signal +If you have an encoder with an index (Z) signal, you may avoid having to do the calibration on every startup, and instead use the index signal to re-sync the encoder to a stored calibration. Bleow are the steps to do the one-time calibration and configuration. Note that you can follow these steps with one motor at a time, or all motors together, as you wish. -#### 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. +* Since you will only do this once, it is recommended that you mechanically disengage the motor from anything other than the encoder, so it can spin freely. +* All the parameters we will be modifying are in the motor structs at the top of [MotorControl/low_level.c](MotorControl/low_level.c). +* Set `.encoder.use_index = true` and `.encoder.calibrated = false`. +* Flash this configuration, and let the motor scan for the index pulse and then complete the encoder calibration. +* Run `explore_odrive.py`, check [Communicating over USB or UART](#communicating-over-usb-or-uart) for instructions on how to do that. +* Enter the following to print out the calibration parameters (substitute the motor number you are calibrating for ``): + * `my_odrive.motor.encoder.encoder_offset` - This should print a number, like -326 or 1364. + * `my_odrive.motor.encoder.motor_dir` - This should print 1 or -1. +* Copy these numbers to the corresponding entries in low_level.c: `.encoder.encoder_offset` and `.encoder.motor_dir`. + * _Warning_: Please be careful to enter the correct numbers, and not to confuse the motor channels. Incorrect values may cause the motor to spin out of control. +* Set `.encoder.calibrated = true`. +* Flash this configuration and check that the motor scans for the index pulse but skips the encoder calibration. +* Congratulations, you are now done. You may now attach the motor to your mechanical load. +* If you wish to scan for the index pulse in the other direction (if for example your axis usually starts close to a hard-stop), you can set a negative value in `.encoder.idx_search_speed`. +* If your motor has problems reaching the index location due to the mechanical load, you can increase `.calibration_current`. +

## 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. @@ -208,48 +242,7 @@ 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. -### Generate makefile -There is an excellent project called CubeMX2Makefile, originally from baoshi. This project is included as a submodule. -* Initialise and clone the submodules: `git submodule init; git submodule update` -* Generate makefile: `python2 CubeMX2Makefile/CubeMX2Makefile.py .` - - -## Setting up Eclipse development environment - -### Install -* Install [Eclipse IDE for C/C++ Developers](http://www.eclipse.org/downloads/packages/eclipse-ide-cc-developers/neon3) -* Install the [OpenOCD Eclipse plugin](http://gnuarmeclipse.github.io/plugins/install/) - -### Import project -* File -> Import -> C/C++ -> Existing Code as Makefile Project -* Browse for existing code location, find the OdriveFirmware root. -* In the Toolchain options, select `Cross GCC` -* Hit Finish -* Build the project (press ctrl-B) - -![Toolchain options](screenshots/CodeAsMakefile.png "Toolchain options") - -### Load the launch configuration -* File -> Import -> Run/Debug -> Launch Configurations -> Next -* Highlight (don't tick) the OdriveFirmare folder in the left column -* Tick OdriveFirmware.launch in the right column -* Hit Finish - -![Launch Configurations](screenshots/ImportLaunch.png "Launch Configurations") - -### Launch! -* Make sure the programmer is connected to the board as per [Flashing the firmware](#flashing-the-firmware). -* Press the down-arrow of the debug symbol in the toolbar, and hit Debug Configurations - * You can also hit Run -> Debug Configurations -* Highlight the debug configuration you imported, called OdriveFirmware. If you do not see the imported launch configuration rename your project to `ODriveFirmware` or edit the launch configuration to match your project name by unfiltering unavailable projects: - -![Launch Configuration Filters](screenshots/LaunchConfigFilter.png "Launch Configuration Filters") - -* Hit Debug -* Eclipse should flash the board for you and the program should start halted on the first instruction in `Main` -* Set beakpoints, step, hit Resume, etc. -* Make some cool features! ;D - +

## Notes for Contributors In general the project uses the [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html), except that the default indendtation is 4 spaces, and that the 80 character limit is not very strictly enforced, merely encouraged. diff --git a/Firmware/Src/freertos.c b/Firmware/Src/freertos.c index 338bb965..0b3a2e7b 100644 --- a/Firmware/Src/freertos.c +++ b/Firmware/Src/freertos.c @@ -99,6 +99,20 @@ void MX_FREERTOS_Init(void) { osSemaphoreDef(sem_usb_irq); sem_usb_irq = osSemaphoreCreate(osSemaphore(sem_usb_irq), 1); osSemaphoreWait(sem_usb_irq, 0); + + // Create a semaphore for UART DMA and remove a token + osSemaphoreDef(sem_uart_dma); + sem_uart_dma = osSemaphoreCreate(osSemaphore(sem_uart_dma), 1); + + // Create a semaphore for USB RX + osSemaphoreDef(sem_usb_rx); + sem_usb_rx = osSemaphoreCreate(osSemaphore(sem_usb_rx), 1); + osSemaphoreWait(sem_usb_irq, 0); // Remove a token. + + // Create a semaphore for USB RX + osSemaphoreDef(sem_usb_tx); + sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1); + /* USER CODE END RTOS_SEMAPHORES */ /* USER CODE BEGIN RTOS_TIMERS */ @@ -139,10 +153,14 @@ void StartDefaultTask(void const * argument) thread_motor_0 = osThreadCreate(osThread(task_motor_0), &motors[0]); thread_motor_1 = osThreadCreate(osThread(task_motor_1), &motors[1]); - // Start USB/UART command handling thread + // Start command handling thread osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 512); thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); + // Start USB interrupt handler thread + osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); + thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); + //If we get to here, then the default task is done. vTaskDelete(defaultTaskHandle); diff --git a/Firmware/Src/gpio.c b/Firmware/Src/gpio.c index e751fb61..693ab7b1 100644 --- a/Firmware/Src/gpio.c +++ b/Firmware/Src/gpio.c @@ -191,15 +191,46 @@ void SetGPIO12toStepDir() { GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct); + //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0); HAL_NVIC_EnableIRQ(EXTI0_IRQn); } +//TODO: Enable index on only one channel +void SetupENCIndexGPIO(){ + GPIO_InitTypeDef GPIO_InitStruct; + + /*Configure GPIO pins : PAPin PAPin */ + GPIO_InitStruct.Pin = M0_ENC_Z_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(M0_ENC_Z_GPIO_Port, &GPIO_InitStruct); + + //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default + HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(EXTI15_10_IRQn); + + /*Configure GPIO pins : PBPin PBPin */ + GPIO_InitStruct.Pin = M1_ENC_Z_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(M1_ENC_Z_GPIO_Port, &GPIO_InitStruct); + + //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default + HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(EXTI3_IRQn); +} + + //Dispatch processing of external interrupts based on source void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { //Step signals for M0 and M1 if (GPIO_Pin & GPIO_1_Pin || GPIO_Pin & GPIO_3_Pin) { step_cb(GPIO_Pin); + } else if(GPIO_Pin & M0_ENC_Z_Pin){ + enc_index_cb(GPIO_Pin, 0); + } else if(GPIO_Pin & M1_ENC_Z_Pin){ + enc_index_cb(GPIO_Pin, 1); } } diff --git a/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c b/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c deleted file mode 100644 index 70b706bb..00000000 --- a/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c +++ /dev/null @@ -1,189 +0,0 @@ -/* External variables --------------------------------------------------------*/ -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; -extern ADC_HandleTypeDef hadc1; -extern ADC_HandleTypeDef hadc2; -extern ADC_HandleTypeDef hadc3; - -/******************************************************************************/ -/* Cortex-M4 Processor Interruption and Exception Handlers */ -/******************************************************************************/ - -/** -* @brief This function handles Non maskable interrupt. -*/ -void NMI_Handler(void) -{ - /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ - - /* USER CODE END NonMaskableInt_IRQn 0 */ - /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ - - /* USER CODE END NonMaskableInt_IRQn 1 */ -} - -/** -* @brief This function handles Hard fault interrupt. -*/ -void HardFault_Handler(void) -{ - /* USER CODE BEGIN HardFault_IRQn 0 */ - - /* USER CODE END HardFault_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN HardFault_IRQn 1 */ - - /* USER CODE END HardFault_IRQn 1 */ -} - -/** -* @brief This function handles Memory management fault. -*/ -void MemManage_Handler(void) -{ - /* USER CODE BEGIN MemoryManagement_IRQn 0 */ - - /* USER CODE END MemoryManagement_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN MemoryManagement_IRQn 1 */ - - /* USER CODE END MemoryManagement_IRQn 1 */ -} - -/** -* @brief This function handles Pre-fetch fault, memory access fault. -*/ -void BusFault_Handler(void) -{ - /* USER CODE BEGIN BusFault_IRQn 0 */ - - /* USER CODE END BusFault_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN BusFault_IRQn 1 */ - - /* USER CODE END BusFault_IRQn 1 */ -} - -/** -* @brief This function handles Undefined instruction or illegal state. -*/ -void UsageFault_Handler(void) -{ - /* USER CODE BEGIN UsageFault_IRQn 0 */ - - /* USER CODE END UsageFault_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN UsageFault_IRQn 1 */ - - /* USER CODE END UsageFault_IRQn 1 */ -} - -/** -* @brief This function handles Debug monitor. -*/ -void DebugMon_Handler(void) -{ - /* USER CODE BEGIN DebugMonitor_IRQn 0 */ - - /* USER CODE END DebugMonitor_IRQn 0 */ - /* USER CODE BEGIN DebugMonitor_IRQn 1 */ - - /* USER CODE END DebugMonitor_IRQn 1 */ -} - -/** -* @brief This function handles System tick timer. -*/ -void SysTick_Handler(void) -{ - /* USER CODE BEGIN SysTick_IRQn 0 */ - - /* USER CODE END SysTick_IRQn 0 */ - HAL_IncTick(); - osSystickHandler(); - /* USER CODE BEGIN SysTick_IRQn 1 */ - - /* USER CODE END SysTick_IRQn 1 */ -} - -/******************************************************************************/ -/* STM32F4xx Peripheral Interrupt Handlers */ -/* Add here the Interrupt Handlers for the used peripherals. */ -/* For the available peripheral interrupt handler names, */ -/* please refer to the startup file (startup_stm32f4xx.s). */ -/******************************************************************************/ - -/** -* @brief This function handles EXTI line2 interrupt. -*/ -void EXTI2_IRQHandler(void) -{ - /* USER CODE BEGIN EXTI2_IRQn 0 */ - - /* USER CODE END EXTI2_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2); - /* USER CODE BEGIN EXTI2_IRQn 1 */ - - /* USER CODE END EXTI2_IRQn 1 */ -} - -/** -* @brief This function handles EXTI line4 interrupt. -*/ -void EXTI4_IRQHandler(void) -{ - /* USER CODE BEGIN EXTI4_IRQn 0 */ - - /* USER CODE END EXTI4_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4); - /* USER CODE BEGIN EXTI4_IRQn 1 */ - - /* USER CODE END EXTI4_IRQn 1 */ -} - -/** -* @brief This function handles ADC1, ADC2 and ADC3 global interrupts. -*/ -void ADC_IRQHandler(void) -{ - /* USER CODE BEGIN ADC_IRQn 0 */ - - // The HAL's ADC handling mechanism adds many clock cycles of overhead - // So we bypass it and handle the logic ourselves. - //@TODO add vbus meaasurement on adc1 here - ADC_IRQ_Dispatch(&hadc1, &vbus_sense_adc_cb); - ADC_IRQ_Dispatch(&hadc2, &pwm_trig_adc_cb); - ADC_IRQ_Dispatch(&hadc3, &pwm_trig_adc_cb); - - // Bypass HAL - return; - - /* USER CODE END ADC_IRQn 0 */ - HAL_ADC_IRQHandler(&hadc1); - HAL_ADC_IRQHandler(&hadc2); - HAL_ADC_IRQHandler(&hadc3); - /* USER CODE BEGIN ADC_IRQn 1 */ - - /* USER CODE END ADC_IRQn 1 */ -} - -/** -* @brief This function handles USB On The Go FS global interrupt. -*/ -void OTG_FS_IRQHandler(void) -{ - /* USER CODE BEGIN OTG_FS_IRQn 0 */ - - /* USER CODE END OTG_FS_IRQn 0 */ - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - /* USER CODE BEGIN OTG_FS_IRQn 1 */ - - /* USER CODE END OTG_FS_IRQn 1 */ -} \ No newline at end of file diff --git a/Firmware/Src/stm32f4xx_it.c b/Firmware/Src/stm32f4xx_it.c index a740268a..064e1036 100644 --- a/Firmware/Src/stm32f4xx_it.c +++ b/Firmware/Src/stm32f4xx_it.c @@ -43,10 +43,6 @@ typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc, bool injected); void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback); -#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ -|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 -#include "prev_board_ver/stm32f4xx_it_V3_2.c" -#else /* USER CODE END 0 */ /* External variables --------------------------------------------------------*/ @@ -281,7 +277,6 @@ void OTG_FS_IRQHandler(void) } /* USER CODE BEGIN 1 */ -#endif void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback) { @@ -318,5 +313,31 @@ void EXTI2_IRQHandler(void) HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2); } +/** +* @brief This function handles EXTI line3 interrupt. +*/ +void EXTI3_IRQHandler(void) +{ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_3); +} + +/** +* @brief This function handles EXTI line4 interrupt. +*/ +void EXTI4_IRQHandler(void) +{ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4); +} + +/** +* @brief This function handles EXTI lines 10-15 interrupt. +*/ +void EXTI15_10_IRQHandler(void) +{ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15); +} + + + /* USER CODE END 1 */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index f93c33f8..3ddb5701 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -5,10 +5,12 @@ ****************************************************************************** */ -#include -#include +#include +#include +#include #include -#include +#include +#include // TODO: make serial_printf_select constant //int _read(int file, char *data, int len) {} @@ -20,34 +22,43 @@ #define UART_TX_BUFFER_SIZE 64 static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE]; -int _write(int file, char *data, int len) { - //number of bytes written - int written = 0; - switch (serial_printf_select) { +int _write(int file, char* data, int len) { + //number of bytes written + int written = 0; + switch (serial_printf_select) { + case SERIAL_PRINTF_IS_USB: { + // Wait on semaphore for the interface to be available + // Note that the USB driver will release the interface again when the TX completes + const uint32_t usb_tx_timeout = 100; // ms + osStatus sem_stat = osSemaphoreWait(sem_usb_tx, usb_tx_timeout); + if (sem_stat == osOK) { + uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); // transmit over CDC + written = (status == USBD_OK) ? len : 0; + } // If the semaphore times out, we simply leave "written" as 0 + } break; - case SERIAL_PRINTF_IS_USB: { - // transmit over CDC - uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); - written = (status == USBD_OK) ? len : 0; - } break; + case SERIAL_PRINTF_IS_UART: { + //Check length + if (len > UART_TX_BUFFER_SIZE) + return 0; + // Wait on semaphore for the interface to be available + // Note that HAL_UART_TxCpltCallback will release the interface again when the TX completes + const uint32_t uart_tx_timeout = 100; // ms + osStatus sem_stat = osSemaphoreWait(sem_uart_dma, uart_tx_timeout); + if (sem_stat == osOK) { + memcpy(uart_tx_buf, data, len); // memcpy data into uart_tx_buf + HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); // Start DMA background transfer + } // If the semaphore times out, we simply leave "written" as 0 + } break; - case SERIAL_PRINTF_IS_UART: { - //Check length - if (len > UART_TX_BUFFER_SIZE) - return 0; - // Check if transfer is already ongoing - if(huart4.gState != HAL_UART_STATE_READY) - return 0; - // memcpy data into uart_tx_buf - memcpy(uart_tx_buf, data, len); - // Start DMA background trasnfer - HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); - } break; + default: { + written = 0; + } break; + } - default: { - written = 0; - } break; - } - - return written; + return written; +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { + osSemaphoreRelease(sem_uart_dma); } diff --git a/Firmware/Src/usbd_cdc_if.c b/Firmware/Src/usbd_cdc_if.c index f6ef8569..054a912c 100644 --- a/Firmware/Src/usbd_cdc_if.c +++ b/Firmware/Src/usbd_cdc_if.c @@ -53,6 +53,7 @@ #include "freertos_vars.h" #include "utils.h" #include "commands.h" +#include /* USER CODE END INCLUDE */ /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY @@ -267,12 +268,8 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len) { /* USER CODE BEGIN 6 */ - // Process command - USB_receive_packet(Buf, *Len); - - // Allow receiving more bytes - USBD_CDC_SetRxBuffer(&hUsbDeviceFS, UserRxBufferFS); - USBD_CDC_ReceivePacket(&hUsbDeviceFS); + set_cmd_buffer(Buf, *Len); + osSemaphoreRelease(sem_usb_rx); return (USBD_OK); /* USER CODE END 6 */ diff --git a/Firmware/Src/usbd_desc.c b/Firmware/Src/usbd_desc.c index bd00de9a..82703697 100644 --- a/Firmware/Src/usbd_desc.c +++ b/Firmware/Src/usbd_desc.c @@ -70,15 +70,17 @@ /** @defgroup USBD_DESC_Private_Defines * @{ - */ -#define USBD_VID 0x1209 -#define USBD_LANGID_STRING 1033 -#define USBD_MANUFACTURER_STRING "ODrive" -#define USBD_PID_FS 0x0D31 -#define USBD_PRODUCT_STRING_FS "ODrive v3.1" -#define USBD_SERIALNUMBER_STRING_FS "000000000001" -#define USBD_CONFIGURATION_STRING_FS "CDC Config" -#define USBD_INTERFACE_STRING_FS "CDC Interface" + */ +#define USBD_VID 0x1209 +#define USBD_LANGID_STRING 1033 +#define USBD_MANUFACTURER_STRING "ODrive Robotics" +#define USBD_PID_FS 0x0D32 +#define USBD_PRODUCT_XSTR(s) USBD_PRODUCT_STR(s) +#define USBD_PRODUCT_STR(s) #s +#define USBD_PRODUCT_STRING_FS ODrive version HW_VERSION_MAJOR.HW_VERSION_MINOR +#define USBD_SERIALNUMBER_STRING_FS "000000000001" +#define USBD_CONFIGURATION_STRING_FS "CDC Config" +#define USBD_INTERFACE_STRING_FS "CDC Interface" #define USB_SIZ_BOS_DESC 0x0C @@ -253,11 +255,11 @@ uint8_t * USBD_FS_ProductStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *len { if(speed == 0) { - USBD_GetString ((uint8_t *)USBD_PRODUCT_STRING_FS, USBD_StrDesc, length); + USBD_GetString ((uint8_t *)USBD_PRODUCT_XSTR(USBD_PRODUCT_STRING_FS), USBD_StrDesc, length); } else { - USBD_GetString ((uint8_t *)USBD_PRODUCT_STRING_FS, USBD_StrDesc, length); + USBD_GetString ((uint8_t *)USBD_PRODUCT_XSTR(USBD_PRODUCT_STRING_FS), USBD_StrDesc, length); } return USBD_StrDesc; } diff --git a/Firmware/configuring-eclipse.md b/Firmware/configuring-eclipse.md new file mode 100644 index 00000000..e4576255 --- /dev/null +++ b/Firmware/configuring-eclipse.md @@ -0,0 +1,35 @@ +# Setting up Eclipse development environment + +## Install +* Install [Eclipse IDE for C/C++ Developers](http://www.eclipse.org/downloads/packages/eclipse-ide-cc-developers/neon3) +* Install the [OpenOCD Eclipse plugin](http://gnuarmeclipse.github.io/plugins/install/) + +## Import project +* File -> Import -> C/C++ -> Existing Code as Makefile Project +* Browse for existing code location, find the OdriveFirmware root. +* In the Toolchain options, select `Cross GCC` +* Hit Finish +* Build the project (press ctrl-B) + +![Toolchain options](screenshots/CodeAsMakefile.png "Toolchain options") + +## Load the launch configuration +* File -> Import -> Run/Debug -> Launch Configurations -> Next +* Highlight (don't tick) the OdriveFirmare folder in the left column +* Tick OdriveFirmware.launch in the right column +* Hit Finish + +![Launch Configurations](screenshots/ImportLaunch.png "Launch Configurations") + +## Launch! +* Make sure the programmer is connected to the board as per [Flashing the firmware](#flashing-the-firmware). +* Press the down-arrow of the debug symbol in the toolbar, and hit Debug Configurations + * You can also hit Run -> Debug Configurations +* Highlight the debug configuration you imported, called OdriveFirmware. If you do not see the imported launch configuration rename your project to `ODriveFirmware` or edit the launch configuration to match your project name by unfiltering unavailable projects: + +![Launch Configuration Filters](screenshots/LaunchConfigFilter.png "Launch Configuration Filters") + +* Hit Debug +* Eclipse should flash the board for you and the program should start halted on the first instruction in `Main` +* Set beakpoints, step, hit Resume, etc. +* Make some cool features! ;D \ No newline at end of file diff --git a/Firmware/configuring-vscode.md b/Firmware/configuring-vscode.md new file mode 100644 index 00000000..ec9b0adb --- /dev/null +++ b/Firmware/configuring-vscode.md @@ -0,0 +1,54 @@ +# Configuring VSCode + +VSCode is the recommended IDE for working with the ODrive codebase. It is a light-weight text editor with Git integration and GDB debugging functionality. + +Before doing the VSCode setup, make sure you've installed all of your [prerequisites](README.md#installing-prerequisites) + +## Setup Procedure +1. Clone the ODrive repository +1. [Download VSCode](https://code.visualstudio.com/download) +1. Open VSCode +1. Install extensions. This can be done directly from VSCode (Ctrl+Shift+X) + * Required extensions: + * C/C++ + * Recommended Extensions: + * Cortex-Debug + * vscode-icons + * Code Outline + * Include Autocomplete + * Path Autocomplete + * Auto Comment Blocks +1. Restart VSCode +1. Open the VSCode Workspace file, which is located in the root of the ODrive repository. It is called `VSCodeWorkspace.code-workspace`. The first time you open it, VSCode will install some dependencies. If it fails, you may need to [change your proxy settings](https://code.visualstudio.com/docs/getstarted/settings). + +You should now be ready to compile and test the ODrive project. + +## Building the Firmware +* Tasks -> Run Build Task + +A terminal window will open with your native shell. VSCode is configured to run the command `make -j4` in this terminal. + +## Flashing the Firmware +* Tasks -> Run Task -> flash + +A terminal window will open with your native shell. VSCode is configured to run the command `make flash` in this terminal. + +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](README.md#communicating-over-usb-or-uart). + +## Debugging +An extension called Cortex-Debug has recently been released which is designed specifically for debugging ARM Cortex projects. You can read more on Cortex-Debug here: https://github.com/Marus/cortex-debug + +Note: If developing on Windows, you should have `arm-none-eabi-gdb` and `openOCD` on your PATH. + + * 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) + * Debug -> Start Debugging (or press F5) + * 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 (F5) + * Stepping over/in/out, restarting, and changing breakpoints can be done by first pressing the "pause" (F6) button at the top the screen. + * When done debugging, simply stop (Shift+F5) the debugger. It will kill your openOCD process too. + +## Cleaning the Build +This sometimes needs to be done if you change branches. +* Open a terminal (View -> Integrated Terminal) and enter `make clean` \ No newline at end of file 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/README.md b/README.md index 9ed8752c..e2b4349a 100644 --- a/README.md +++ b/README.md @@ -3,10 +3,8 @@ This project is all about accurately driving brushless motors, for cheap. The aim is to make it possible to use inexpensive brushless motors in high performance robotics projects, like [this](https://www.youtube.com/watch?v=WT4E5nb3KtY). ## Getting Started -*References to hardware is with respect to v3.3. Other versions may still apply, but component designators may differ* - It is perfectly fine, and even recommended, to start testing with just a single motor and encoder. -Make sure you have a good mechanical connection between the encdoer and the motor, slip can cause disasterous oscillations. +Make sure you have a good mechanical connection between the encoder and the motor, slip can cause disasterous oscillations. All non-power I/O is 3.3V output and 5V tolerant on input, except: * GPIO 3 and GPIO 4 are NOT 5V tolerant on ODrive v3.2 and earlier. @@ -16,12 +14,13 @@ You need one or two [brushless motors](https://hackaday.io/project/11583-odrive- Wire up the motor phases into the 3-phase screw terminals, and the power resistor to the AUX terminal. Wire up the power source (12-24V) to the DC terminal, make sure to pay attention to the polarity. Do not apply power just yet. -Wire up the encoder(s) to J4. The A,B phases are required, and the Z (index pulse) is optional. The A,B and Z lines have 1k pull up resistors, for use with open-drain encoder outputs. For single ended push-pull signals with weak drive current (\<4mA), you may want to desolder the pull-ups. +Wire up the encoder(s) to J4. The A,B phases are required, and the Z (index pulse) is optional. The A,B and Z lines have 3.3k pull up resistors, for use with open-drain encoder outputs. For single ended push-pull signals with weak drive current (\<4mA), you may want to desolder the pull-ups. ![Image of ODrive all hooked up](https://docs.google.com/drawings/d/e/2PACX-1vTCD0P40Cd-wvD7Fl8UYEaxp3_UL81oI4qUVqrrCJPi6tkJeSs2rsffIXQRpdu6rNZs6-2mRKKYtILG/pub?w=1716&h=1281) -The currently supported command modes are USB and step/direction. +The currently supported command modes are USB, UART and step/direction. * If you are sending commands over USB, you can plug in a cable into the micro-USB port. +* If you are sending commands over UART, please see [Setting up UART](#setting-up-uart) * If you are using step/direction, please see [setting up step/direction](#setting-up-stepdirection) You can now: @@ -34,6 +33,8 @@ The startup procedure is demonstrated [here](https://www.youtube.com/watch?v=VCX Note: the rotor must be allowed to rotate without any biased load during startup. That means mass and weak friction loads are fine, but gravity or spring loads are not okay. Also note that in the video, the motors spin after initalisation, but in the current software the default behaviour is to do position control to position 0 (i.e. the position at startup) +If you have an encoder with an index (Z) signal, you can calibrate once and restore the calibration on startup. Instructions on how to do that are [here](Firmware/README.md#encoder-calibration). + ### Sending commands Sending USB and UART commands is documented [here](Firmware/README.md#communicating-over-usb-and-uart). You can also 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. 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/.vscode/launch.json b/tools/.vscode/launch.json similarity index 59% rename from .vscode/launch.json rename to tools/.vscode/launch.json index babcd14d..a3b07eff 100644 --- a/.vscode/launch.json +++ b/tools/.vscode/launch.json @@ -19,21 +19,6 @@ "WaitOnNormalExit", "RedirectOutput" ] - }, - { - "type": "gdb", - "request": "attach", - "name": "Debug Firmware", - "target": "localhost:3333", - "gdbpath": "arm-none-eabi-gdb", - "remote": true, - "executable": "./build/ODriveFirmware.elf", - "cwd": "${workspaceRoot}/Firmware", - "printCalls": false, - //"preLaunchTask": "openocd", // This isn't working quite right. - "autorun": [ - "monitor reset halt" - ] } ] } \ No newline at end of file diff --git a/tools/demo.py b/tools/demo.py index fa148eba..4a06c260 100755 --- a/tools/demo.py +++ b/tools/demo.py @@ -3,15 +3,14 @@ Example usage of the ODrive python library to monitor and control ODrive devices """ +from __future__ import print_function + import odrive.core import time import math # Find a connected ODrive (this will block until you connect one) -odrives = odrive.core.find_all(printer=print) -odrives = list(odrives) #force eval of generator to test finding functions -my_drive = odrives[0] -# my_drive = odrive.core.find_any(printer=print) +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`. @@ -30,9 +29,9 @@ 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) -# little sine wave to test +# A little sine wave to test t0 = time.monotonic() -while False: +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) @@ -45,4 +44,4 @@ while False: 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 `TypeError: expected value of type float` +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 index bd528b28..bda8b58e 100644 --- a/tools/odrive/core.py +++ b/tools/odrive/core.py @@ -7,11 +7,12 @@ 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 serial import time import os import odrive.protocol @@ -44,15 +45,7 @@ class SimpleDeviceProperty(property): return struct.unpack(self._struct_format, buffer)[0] def fset(self, obj, value): - #Oskar: Pythonic duck typing style means that you should pretend that types are - # compatible, and catch errors. So instead do something like: - # value = self._type(value) - # you could of course wrap this in a try/except block, but when it fails it - # raises a TypeError, just like the one you made below, so I'd just let that fire - # by itself. - - if not isinstance(value, self._type): - raise TypeError("expected value of type {}".format(self._type.__name__)) + 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) @@ -68,17 +61,15 @@ def call_remote_function(channel, trigger_id, arg_properties, *args): arg_properties[i].fset(None, args[i]) channel.remote_endpoint_operation(trigger_id, None, True, 0) -#Oskar: setattr_or_raise_if_undefined -def raise_if_undefined(self, name, value): +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 """ - #Oskar: hasattr internally calls fget to determine if the attribute exists, - # which unnessecarily creates bus traffic. We should try to solve this. - # Step-in on the hasattr line in the debugger to see this. - if hasattr(self, name): + # 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' % ( @@ -128,9 +119,7 @@ def create_property(name, json_data, channel, printer): printer("property {} has no specified ID".format(name)) return None - #Oskar: Bug: json_data calls this "access", but we look for "mode". - # The default should probably be "r" anyway, it's safer I'd say. - access_mode = json_data.get("mode", "rw") + access_mode = json_data.get("access", "r") return SimpleDeviceProperty(channel, id_str, property_type, struct_format, 'r' in access_mode, @@ -162,7 +151,7 @@ def create_object(name, json_data, namespace, channel, printer=noprint): namespace = name # Build attribute list from JSON - attributes = {"__setattr__": raise_if_undefined} + attributes = {"__setattr__": setattr_or_raise_if_undefined} for member in json_data.get("members", []): member_name = member.get("name", None) if member_name is None: @@ -185,7 +174,7 @@ def create_object(name, json_data, namespace, channel, printer=noprint): attributes[member_name] = attribute # Create a type from the property list and instantiate it - jit_type = type(namespace, (object,), attributes) + jit_type = type(str(namespace), (object,), attributes) new_object = jit_type() return new_object @@ -208,11 +197,11 @@ def channel_from_serial_port(port, baud, packet_based, printer=noprint): if packet_based == True: # TODO: implement packet based transport over serial raise NotImplementedError("not supported yet") - serial_device = odrive.serial_transport.SerialStreamTransport(port, 115200) + serial_device = odrive.serial_transport.SerialStreamTransport(port, baud) input_stream = odrive.protocol.PacketFromStreamConverter(serial_device) output_stream = odrive.protocol.PacketToStreamConverter(serial_device) return odrive.protocol.Channel( - "serial port {}@{}".format(port, 115200), + "serial port {}@{}".format(port, baud), input_stream, output_stream) def object_from_channel(channel, printer=noprint): @@ -262,6 +251,9 @@ def find_dev_serial_ports(search_regex): except FileNotFoundError: return [] +def find_pyserial_ports(): + return [x.name for x in serial.tools.list_ports.comports()] + def find_serial_channels(printer=noprint): """ Scans for serial ports. @@ -269,19 +261,15 @@ def find_serial_channels(printer=noprint): Not every returned object necessarily represents a compatible device. """ - #Oskar: Why not just use this tool to find the available ports? - # https://pyserial.readthedocs.io/en/latest/tools.html#module-serial.tools.list_ports - - # Real serial ports or USB-Serial converters - linux_real_serial_ports = find_dev_serial_ports(r'^ttyUSB') - windows_real_serial_ports = [ "COM1", "COM2", "COM3", "COM4" ] + # Real serial ports or USB-Serial converters (tested on Linux and Windows) + real_serial_ports = find_pyserial_ports() # Serial devices that are exposed by the platform # for the device's USB connection linux_usb_serial_ports = find_dev_serial_ports(r'^ttyACM') macos_usb_serial_ports = find_dev_serial_ports(r'^tty\.usbmodem') - for port in linux_real_serial_ports + windows_real_serial_ports + linux_usb_serial_ports + macos_usb_serial_ports: + for port in real_serial_ports + linux_usb_serial_ports + macos_usb_serial_ports: try: yield channel_from_serial_port(port, 115200, False, printer) except serial.serialutil.SerialException: @@ -289,13 +277,16 @@ def find_serial_channels(printer=noprint): continue -def find_all(printer=noprint): +def find_all(consider_usb=True, consider_serial=False, printer=noprint): """ Returns a generator with all the connected devices that speak the ODrive protocol """ - usb_channels = find_usb_channels(printer=printer) - serial_channels = find_serial_channels(printer=printer) - for channel in itertools.chain(usb_channels, serial_channels): + channels = iter(()) + if (consider_usb): + channels = itertools.chain(channels, find_usb_channels(printer=printer)) + if (consider_serial): + channels = itertools.chain(channels, find_serial_channels(printer=printer)) + for channel in channels: # TODO: blacklist known bad channels try: yield object_from_channel(channel, printer) @@ -304,7 +295,7 @@ def find_all(printer=noprint): continue -def find_any(printer=noprint): +def find_any(consider_usb=True, consider_serial=False, printer=noprint): """ Scans for ODrives on all supported interfaces and returns the first device that is found. If no device is connected the function blocks. @@ -314,7 +305,7 @@ def find_any(printer=noprint): # poll for device printer("looking for ODrive...") while True: - dev = next(find_all(printer=printer), None) + dev = next(find_all(consider_usb, consider_serial, printer=printer), None) if dev is not None: return dev printer("no device found") diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index e66dc1f5..9d69aa08 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -2,6 +2,18 @@ import time import struct +import sys + +import abc + +if sys.version_info >= (3, 4): + ABC = abc.ABC +else: + ABC = abc.ABCMeta('ABC', (), {}) + +if sys.version_info <= (3, 3): + from monotonic import monotonic + time.monotonic = monotonic SYNC_BYTE = 0xAA CRC8_INIT = 0x42 @@ -29,6 +41,8 @@ def calc_crc(remainder, value, polynomial, bitwidth): def calc_crc8(remainder, value): if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list): for byte in value: + if not isinstance(byte,int): + byte = ord(byte) remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8) else: remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8) @@ -37,6 +51,8 @@ def calc_crc8(remainder, value): def calc_crc16(remainder, value): if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list): for byte in value: + if not isinstance(byte, int): + byte = ord(byte) remainder = calc_crc(remainder, byte, CRC16_DEFAULT, 16) else: remainder = calc_crc(remainder, value, CRC16_DEFAULT, 16) @@ -56,21 +72,26 @@ class ChannelBrokenException(Exception): class DeviceInitException(Exception): pass -#Oskar: I would just get rid of these "abstract classes", -# I think just looking and seeing that the classes have -# a process_packet or get_packet is enough. -class StreamSource(object): - pass +class StreamSource(ABC): + @abc.abstractmethod + def get_bytes(self, deadline): + pass -class StreamSink(object): - pass +class StreamSink(ABC): + @abc.abstractmethod + def process_bytes(self, bytes): + pass -class PacketSource(object): - pass +class PacketSource(ABC): + @abc.abstractmethod + def get_packet(self, deadline): + pass -class PacketSink(object): - pass +class PacketSink(ABC): + @abc.abstractmethod + def process_packet(self, packet): + pass class StreamToPacketConverter(StreamSink): @@ -174,9 +195,8 @@ class Channel(PacketSink): _interface_definition_crc = 0 _expected_acks = {} - # Chose these parameters to be sensible for a specific transport layer - #Oskar: it's a timeout, not delay. - _resend_delay = 5.0 # [s] + # Choose these parameters to be sensible for a specific transport layer + _resend_timeout = 0.1 # [s] _send_attempts = 5 def __init__(self, name, input, output): @@ -219,7 +239,7 @@ class Channel(PacketSink): attempt = 0 while (attempt < self._send_attempts): self._output.process_packet(packet) - deadline = time.monotonic() + self._resend_delay + deadline = time.monotonic() + self._resend_timeout # Read and process packets until we get an ack or need to resend # TODO: support I/O driven reception (wait on semaphore) while True: @@ -248,7 +268,7 @@ class Channel(PacketSink): buffer = bytes() while True: chunk_length = 64 - chunk = self.remote_endpoint_operation(0, struct.pack(" ", - history=history).strip() - except EOFError: - command = "exit" - - if len(command) == 0: - continue - elif command.startswith("p "): - args = command[2:].split() - try: - motor = motors[int(args[0])] - pos = float(args[1]) - vel = float(args[2]) - cur = float(args[3]) - except (ValueError, IndexError): - print("invalid command format") - continue - motor.set_pos_setpoint(pos, vel, cur) - elif command.startswith("v "): - args = command[2:].split() - try: - motor = motors[int(args[0])] - vel = float(args[1]) - cur = float(args[2]) - except (ValueError, IndexError): - print("invalid command format") - continue - motor.set_vel_setpoint(vel, cur) - elif command.startswith("c "): - args = command[2:].split() - try: - motor = motors[int(args[0])] - cur = float(args[1]) - except (ValueError, IndexError): - print("invalid command format") - continue - motor.set_current_setpoint(cur) - elif command == "h" or command == '?' or command == 'help': - print_usage() - elif command == "q" or command == 'exit': - sys.exit(0) - else: - print("unknown command \"" + command + "\"") - -def main(args): - if (args.verbose): - printer = print - else: - printer = noprint - - history = prompt_toolkit.history.InMemoryHistory() - - print_usage() - - while True: - # 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: - device = 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: - device = odrive.core.open_serial(args.serial, printer=printer) - else: - print("Waiting for device...") - device = odrive.core.find_any(printer=printer) - autoconnected = True - - try: - command_prompt_loop(device, history) - except odrive.protocol.ChannelBrokenException: - print("ODrive disconnected") - if not autoconnected: - sys.exit(1) - - -if __name__ == "__main__": - main(args)