diff --git a/Firmware/Board/v3/Inc/freertos_vars.h b/Firmware/Board/v3/Inc/freertos_vars.h index c1e122a0..2eb52d7d 100644 --- a/Firmware/Board/v3/Inc/freertos_vars.h +++ b/Firmware/Board/v3/Inc/freertos_vars.h @@ -3,15 +3,9 @@ #define __FREERTOS_H // List of semaphores -osSemaphoreId sem_usb_irq; -osSemaphoreId sem_uart_dma; -osSemaphoreId sem_usb_rx; -osSemaphoreId sem_usb_tx; - -// List of threads -osThreadId thread_motor_0; -osThreadId thread_motor_1; -osThreadId thread_cmd_parse; -osThreadId thread_usb_pump; +extern osSemaphoreId sem_usb_irq; +extern osSemaphoreId sem_uart_dma; +extern osSemaphoreId sem_usb_rx; +extern osSemaphoreId sem_usb_tx; #endif /* __FREERTOS_H */ \ No newline at end of file diff --git a/Firmware/Board/v3/Inc/main.h b/Firmware/Board/v3/Inc/main.h index cef4368a..25d37a98 100644 --- a/Firmware/Board/v3/Inc/main.h +++ b/Firmware/Board/v3/Inc/main.h @@ -172,8 +172,10 @@ #if HW_VERSION_VOLTAGE == 48 #define VBUS_S_DIVIDER_RATIO 19.0f +#define VBUS_OVERVOLTAGE_LEVEL 52.0f #elif HW_VERSION_VOLTAGE == 24 #define VBUS_S_DIVIDER_RATIO 11.0f +#define VBUS_OVERVOLTAGE_LEVEL 26.0f #else #error "unknown board voltage" #endif diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c index b247994a..28ead46d 100644 --- a/Firmware/Board/v3/Src/freertos.c +++ b/Firmware/Board/v3/Src/freertos.c @@ -53,7 +53,8 @@ /* USER CODE BEGIN Includes */ #include "freertos_vars.h" -#include "axis_c_interface.h" +#include "usb_device.h" +extern PCD_HandleTypeDef hpcd_USB_OTG_FS; int odrive_main(void); /* USER CODE END Includes */ @@ -63,11 +64,9 @@ osThreadId defaultTaskHandle; /* USER CODE BEGIN Variables */ // List of semaphores osSemaphoreId sem_usb_irq; - -// List of threads -osThreadId thread_motor_0; -osThreadId thread_motor_1; -osThreadId thread_cmd_parse; +osSemaphoreId sem_uart_dma; +osSemaphoreId sem_usb_rx; +osSemaphoreId sem_usb_tx; // Place FreeRTOS heap in core coupled memory for better performance __attribute__((section(".ccmram"))) @@ -94,6 +93,28 @@ __weak void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTask configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is called if a stack overflow is detected. */ } + +void usb_deferred_interrupt_thread(void * ctx) { + (void) ctx; // unused parameter + + for (;;) { + // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) + osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever); + if (semaphore_status == osOK) { + // We have a new incoming USB transmission: handle it + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); + // Let the irq (OTG_FS_IRQHandler) fire again. + HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + } + } +} + +void init_deferred_interrupts(void) { + // Start USB interrupt handler thread + osThreadDef(task_usb_pump, usb_deferred_interrupt_thread, osPriorityAboveNormal, 0, 512); + osThreadCreate(osThread(task_usb_pump), NULL); +} + /* USER CODE END 4 */ /* Init FreeRTOS */ @@ -126,6 +147,7 @@ void MX_FREERTOS_Init(void) { osSemaphoreDef(sem_usb_tx); sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1); + init_deferred_interrupts(); /* USER CODE END RTOS_SEMAPHORES */ /* USER CODE BEGIN RTOS_TIMERS */ diff --git a/Firmware/Board/v3/Src/main.c b/Firmware/Board/v3/Src/main.c index 64810aac..bafa4d3f 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -59,8 +59,8 @@ #include "gpio.h" /* USER CODE BEGIN Includes */ -#include "utils.h" -#include "communication.h" +#include +#include "freertos_vars.h" /* USER CODE END Includes */ /* Private variables ---------------------------------------------------------*/ @@ -81,12 +81,47 @@ void MX_FREERTOS_Init(void); /* USER CODE BEGIN 0 */ -void jump_to_builtin_bootloader(void) { - __set_MSP(0x20001000); - // http://www.st.com/content/ccc/resource/technical/document/application_note/6a/17/92/02/58/98/45/0c/CD00264379.pdf/files/CD00264379.pdf - void (*builtin_bootloader)(void) = (void (*)(void))(*((uint32_t *)0x1FFF0004)); - builtin_bootloader(); - for (;;); +uint32_t _reboot_cookie __attribute__ ((section (".noinit"))); +extern char _estack; // provided by the linker script + +// Gets called from the startup assembly code +void early_start_checks(void) { + /* We could jump to the bootloader directly on demand without rebooting + but that requires us to reset several peripherals and interrupts for it + to function correctly. Therefore it's easier to just reset the entire chip. */ + if(_reboot_cookie == 0xDEADBEEF) { + _reboot_cookie = 0xCAFEFEED; //Reset bootloader trigger + + /* + * This wait loop solves an obscure timing issue, but we don't exactly understand why. + * When the transition NVIC_SystemReset() => STM bootloader happens very quickly, + * there is a yet unexplained phenomenon where the ODrive would emit an audible click, + * followed by one the following symptoms: + * - Device reboots in normal mode (possibly due to the bootloader exiting immidiately) + * - Device goes into DFU mode and then the power supply turns off + * This manifests in the DFU script detecting the device in DFU mode but then + * losing the device immidiately after. + * There were no motors/encoders/brake resistor connected when testing this. As far as + * we can tell, the only way for the software to cause a short circuit is through the + * brake FETs. + */ + for (size_t i = 0; i < 1000000; ++i) { + __NOP(); + } + + __set_MSP((uintptr_t)&_estack); + // http://www.st.com/content/ccc/resource/technical/document/application_note/6a/17/92/02/58/98/45/0c/CD00264379.pdf/files/CD00264379.pdf + void (*builtin_bootloader)(void) = (void (*)(void))(*((uint32_t *)0x1FFF0004)); + builtin_bootloader(); + } + + /* The bootloader might fail to properly clean up after itself, + so if we're not sure that the system is in a clean state we + just reset it again */ + if(_reboot_cookie != 42) { + _reboot_cookie = 42; + NVIC_SystemReset(); + } } /* USER CODE END 0 */ @@ -100,28 +135,12 @@ int main(void) { /* USER CODE BEGIN 1 */ - /* We could jump to the bootloader directly on demand without rebooting - but that requires us to reset several peripherals and interrupts for it - to function correctly. Therefore it's easier to just reset the entire chip. */ - if(*((unsigned long *)0x2001C000) == 0xDEADBEEF) { - *((unsigned long *)0x2001C000) = 0xCAFEFEED; //Reset bootloader trigger - jump_to_builtin_bootloader(); - } - - /* The bootloader might fail to properly clean up after itself, - so if we're not sure that the system is in a clean state we - just reset it again */ - if(*((unsigned long *)0x2001C000) != 42) { - *((unsigned long *)0x2001C000) = 42; - NVIC_SystemReset(); - } - // This procedure of building a USB serial number should be identical // to the way the STM's built-in USB bootloader does it. This means // that the device will have the same serial number in normal and DFU mode. - uint32_t uuid0 = *(uint32_t *) (ID_UNIQUE_ADDRESS + 0); - uint32_t uuid1 = *(uint32_t *) (ID_UNIQUE_ADDRESS + 4); - uint32_t uuid2 = *(uint32_t *) (ID_UNIQUE_ADDRESS + 8); + uint32_t uuid0 = *(uint32_t *)(UID_BASE + 0); + uint32_t uuid1 = *(uint32_t *)(UID_BASE + 4); + uint32_t uuid2 = *(uint32_t *)(UID_BASE + 8); uint32_t uuid_mixed_part = uuid0 + uuid2; serial_number = ((uint64_t)uuid_mixed_part << 16) | (uint64_t)(uuid1 >> 16); @@ -155,7 +174,6 @@ int main(void) MX_DMA_Init(); MX_ADC1_Init(); MX_ADC2_Init(); - MX_CAN1_Init(); MX_TIM1_Init(); MX_TIM8_Init(); MX_TIM3_Init(); diff --git a/Firmware/Board/v3/Src/usbd_cdc_if.c b/Firmware/Board/v3/Src/usbd_cdc_if.c index b500c6c4..1a9c43c4 100644 --- a/Firmware/Board/v3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3/Src/usbd_cdc_if.c @@ -52,9 +52,7 @@ /* USER CODE BEGIN INCLUDE */ #include "cmsis_os.h" -#include "freertos_vars.h" -#include "utils.h" -#include "communication.h" +#include #include /* USER CODE END INCLUDE */ @@ -292,9 +290,7 @@ static int8_t CDC_Control_FS(uint8_t cmd, uint8_t* pbuf, uint16_t length) static int8_t CDC_Receive_FS(uint8_t* Buf, uint32_t *Len) { /* USER CODE BEGIN 6 */ - - set_cmd_buffer(Buf, *Len); - osSemaphoreRelease(sem_usb_rx); + usb_process_packet(Buf, *Len); return (USBD_OK); /* USER CODE END 6 */ diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index ea8584d8..856d05e2 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c @@ -53,7 +53,7 @@ #include "usbd_conf.h" /* USER CODE BEGIN INCLUDE */ -#include "communication.h" +#include /* USER CODE END INCLUDE */ /* Private typedef -----------------------------------------------------------*/ diff --git a/Firmware/Board/v3/startup_stm32f405xx.s b/Firmware/Board/v3/startup_stm32f405xx.s index ea0e76a9..34c01d1d 100644 --- a/Firmware/Board/v3/startup_stm32f405xx.s +++ b/Firmware/Board/v3/startup_stm32f405xx.s @@ -107,6 +107,8 @@ LoopFillZerobss: /* Call the clock system intitialization function.*/ bl SystemInit + bl early_start_checks + /* Call static constructors */ bl __libc_init_array /* Call the application's entry point.*/ diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index afe587f0..eb7c0769 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -2,24 +2,26 @@ Please add a note of your changes below this heading if you make a Pull Request. ### Added -* Encoder can now go forever in velocity/torque mode due to using circular encoder space. - * `make write_otp` command to burn the board version onto the ODrive's one-time programmable memory. If you have an ODrive v3.4 or older, you should run this once for a better firmware update user experience in the future. Run the command without any options for more details. Once set, the board version is exposed through the `board_version_[...]` properties. + * Encoder can now go forever in velocity/torque mode due to using circular encoder space. + * `make write_otp` command to burn the board version onto the ODrive's one-time programmable memory. If you have an ODrive v3.4 or older, you should run this once for a better firmware update user experience in the future. Run the command without any options for more details. Once set, the board version is exposed through the `hw_version_[...]` properties. * bake Git-derived firmware version into firmware binary. The firmware version is exposed through the `fw_version_[...]` properties. * infrastructure to publish the python tools to PyPi. See `tools/setup.py` for details. ### Changed - * The DFU script now verifies the flash after writing - * Refactor python tools - * The scripts `explore_odrive.py`, `liveplotter.py`, `drv_status.py` and `rate_test.py` have been merged into one single `odrivetool` script. Running this script without any arguments provides the shell that `explore_odrive.py` used to provide. - * The command line options of `odrivetool` have changed compared to the original `explore_odrive.py`. See `odrivetool --help` for more details. - * `odrivetool` (previously `explore_odrive.py`) now supports controlling multiple ODrives concurrently (`odrv0`, `odrv1`, ...) - * No need to restart the `odrivetool` shell when devices get disconnected and reconnected - * ODrive accesses from within python tools are now thread-safe. That means you can read from the same remote property from multiple threads concurrently. - * The liveplotter (`odrivetool liveplotter`, formerly `liveplotter.py`) does no longer steal focus and closes as expected - * (experimental: start liveplotter from `odrivetool` shell by typing `start_liveplotter(lambda: odrv0.motor0.encoder.encoder_state)`) - +* The DFU script now verifies the flash after writing +* Refactor python tools + * The scripts `explore_odrive.py`, `liveplotter.py`, `drv_status.py` and `rate_test.py` have been merged into one single `odrivetool` script. Running this script without any arguments provides the shell that `explore_odrive.py` used to provide. + * The command line options of `odrivetool` have changed compared to the original `explore_odrive.py`. See `odrivetool --help` for more details. + * `odrivetool` (previously `explore_odrive.py`) now supports controlling multiple ODrives concurrently (`odrv0`, `odrv1`, ...) + * No need to restart the `odrivetool` shell when devices get disconnected and reconnected + * ODrive accesses from within python tools are now thread-safe. That means you can read from the same remote property from multiple threads concurrently. + * The liveplotter (`odrivetool liveplotter`, formerly `liveplotter.py`) does no longer steal focus and closes as expected + * (experimental: start liveplotter from `odrivetool` shell by typing `start_liveplotter(lambda: odrv0.motor0.encoder.encoder_state)`) +* `make write_otp` command to burn the board version onto the ODrive's one-time programmable memory. If you have an ODrive v3.4 or older, you can run this once for a better firmware update user experience in the future. Run the command without any options for more details. Once set, the board version is exposed through the `hw_version_[...]` properties. +* bake Git-derived firmware version into firmware binary. The firmware version is exposed through the `fw_version_[...]` properties. * Set thread priority of USB pump thread above protocol thread * GPIO3 not sensitive to edges by default + ### Fixed * Enums now transported with correct underlying type on native protocol @@ -37,6 +39,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * Most of the code from `lowlevel.c` moved to `axis.cpp`, `encoder.cpp`, `controller.cpp`, `sensorless_estimator.cpp`, `motor.cpp` and the corresponding header files * Refactoring of the developer-facing communication protocol interface. See e.g. `axis.hpp` or `controller.hpp` for examples on how to add your own fields and functions * Change of the user-facing field paths. E.g. `my_odrive.motor0.pos_setpoint` is now at `my_odrive.axis0.controller.pos_setpoint`. Names are mostly unchanged. +* Rewrite of the top-level per-axis state-machine * The build is now configured using the `tup.config` file instead of editing source files. Make sure you set your board version correctly. See [here](README.md#configuring-the-build) for details. * The toplevel directory for tup is now `Firmware`. If you used tup before, go to `Firmware` and run `rm -rd ../.tup; rm -rd build/*; make`. * Update CubeMX generated STM platform code to version 1.19.0 diff --git a/Firmware/Makefile b/Firmware/Makefile index 9f239b22..eee7c551 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -48,7 +48,7 @@ erase_config: # - product ID (01: ODrive) # - hardware major version # - hardware minor version -# - hardware variant (00: 24V, 01: 48V) +# - hardware variant (equal to the board nominal voltage) # Bits in the OTP can only ever be set to 0 but never back to 1. # Therefore do not try to run this command on the same board # twice with different data. @@ -59,7 +59,8 @@ erase_config: # FLASH_CR = (1 << FLASH_CR_PG); // unlock flash memory # [write OTP] write_otp: -ifeq ($(ODRV_FACTORY),TRUE) +ifeq ($(OTP_CONFIRM),TRUE) + # Data: $(OPENOCD) \ -c init \ -c 'reset halt' \ @@ -69,9 +70,9 @@ ifeq ($(ODRV_FACTORY),TRUE) -c 'mwb 0x1fff7800 0xFE' -c 'sleep 10' \ -c 'mwb 0x1fff7801 0x01' -c 'sleep 10' \ -c 'mwb 0x1fff7802 0x01' -c 'sleep 10' \ - -c 'mwb 0x1fff7803 0x03' -c 'sleep 10' \ - -c 'mwb 0x1fff7804 0x04' -c 'sleep 10' \ - -c 'mwb 0x1fff7805 0x01' -c 'sleep 10' \ + -c 'mwb 0x1fff7803 3' -c 'sleep 10' \ + -c 'mwb 0x1fff7804 4' -c 'sleep 10' \ + -c 'mwb 0x1fff7805 48' -c 'sleep 10' \ -c 'reset run' \ -c exit @@ -84,7 +85,8 @@ else @echo " 1. open the Makefile and look at the write_otp target" @echo " 2. understand the structure of the OTP" @echo " 3. edit the bytes that are written to match your board version" - @echo "Run this command again, this time with ODRV_FACTORY=TRUE" + @echo "Run this command again, this time with OTP_CONFIRM=TRUE appended" + @echo "to the command in the terminal" endif clean: diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 4486199a..7ddce582 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -4,7 +4,7 @@ #include "gpio.h" #include "utils.h" -#include "odrive_main.hpp" +#include "odrive_main.h" Axis::Axis(const AxisHardwareConfig_t& hw_config, AxisConfig_t& config, diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index e7833202..8e2245d4 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -1,8 +1,8 @@ #ifndef __AXIS_HPP #define __AXIS_HPP -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." +#ifndef __ODRIVE_MAIN_H +#error "This file should not be included directly. Include odrive_main.h instead." #endif // Warning: Do not reorder these enum values. @@ -13,7 +13,7 @@ enum AxisState_t { AXIS_STATE_STARTUP_SEQUENCE = 2, // -#include -#include -#include -#include -#include - -#define UART_TX_BUFFER_SIZE 64 - -/* Private defines -----------------------------------------------------------*/ -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ - -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; -extern USBD_HandleTypeDef hUsbDeviceFS; -uint64_t serial_number; -char serial_number_str[13]; // 12 digits + null termination - -/* Private constant data -----------------------------------------------------*/ - -#if HW_VERSION_MAJOR == 3 -const uint8_t* otp_ptr = - *(uint8_t*)0x1fff7800 == 0xfe ? (uint8_t*)0x1fff7800 : - *(uint8_t*)0x1fff7800 != 0x00 ? NULL : - *(uint8_t*)0x1fff7810 == 0xfe ? (uint8_t*)0x1fff7810 : NULL; - -// Read hardware version from OTP if available, otherwise fall back -// to software defined version. -const uint8_t board_version_major = otp_ptr ? otp_ptr[3] : HW_VERSION_MAJOR; -const uint8_t board_version_minor = otp_ptr ? otp_ptr[4] : HW_VERSION_MINOR; -const uint8_t board_version_variant = otp_ptr ? otp_ptr[5] : (HW_VERSION_VOLTAGE == 24 ? 0 : 1); -#else -#error "not implemented" -#endif - -// the corresponding macros are defined in the autogenerated version.h -const uint8_t fw_version_major = FW_VERSION_MAJOR; -const uint8_t fw_version_minor = FW_VERSION_MINOR; -const uint8_t fw_version_revision = FW_VERSION_REVISION; -const uint8_t fw_version_unreleased = FW_VERSION_UNRELEASED; // 0 for official releases, 1 otherwise - -/* 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; - - -#if !defined(USB_PROTOCOL_NONE) - -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_packet_output; - -#if !defined(USB_PROTOCOL_NATIVE) -class TreatPacketSinkAsStreamSink : public StreamSink { -public: - TreatPacketSinkAsStreamSink(PacketSink& output) : output_(output) {} - int process_bytes(const uint8_t* buffer, size_t length) { - // Loop to ensure all bytes get sent - while (length) { - size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; - if (output_.process_packet(buffer, length) != 0) - return -1; - buffer += chunk; - length -= chunk; - } - return 0; - } - size_t get_free_space() { return SIZE_MAX; } -private: - PacketSink& output_; -} usb_stream_output(usb_packet_output); -#endif - -#if defined(USB_PROTOCOL_NATIVE) -BidirectionalPacketBasedChannel usb_channel(usb_packet_output); -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) -PacketToStreamConverter usb_packetized_output(usb_stream_output); -BidirectionalPacketBasedChannel usb_channel(usb_packetized_output); -#endif - -#if defined(USB_PROTOCOL_NATIVE_STREAM_BASED) -StreamToPacketConverter usb_native_stream_input(usb_channel); -#endif - -#endif // !defined(USB_PROTOCOL_NONE) - - -#if !defined(UART_PROTOCOL_NONE) -class UART4Sender : public StreamSink { -public: - int process_bytes(const uint8_t* buffer, size_t length) { - // Loop to ensure all bytes get sent - while (length) { - size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; - // wait for USB interface to become ready - // TODO: implement ring buffer to get a more continuous stream of data - if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit chunk - memcpy(tx_buf_, buffer, chunk); - if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) - return -1; - buffer += chunk; - length -= chunk; - } - return 0; - } - - size_t get_free_space() { return SIZE_MAX; } -private: - uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; -} uart4_stream_output; - -#if defined(UART_PROTOCOL_NATIVE) -PacketToStreamConverter uart4_packet_sender(uart4_stream_output); -BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender); -StreamToPacketConverter uart4_stream_input(uart4_channel); -#endif - -#endif // !defined(UART_PROTOCOL_NONE) - - -/* Private function prototypes -----------------------------------------------*/ -/* Function implementations --------------------------------------------------*/ - -void enter_dfu_mode() { - *((unsigned long *)0x2001C000) = 0xDEADBEEF; - NVIC_SystemReset(); -} - -void init_communication(void) { - printf("hi!\r\n"); - - // Start command handling thread - osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5000 /* in 32-bit words */); // TODO: fix stack issues - thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); - - // Start USB interrupt handler thread - osThreadDef(task_usb_pump, usb_update_thread, osPriorityAboveNormal, 0, 512); - thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); -} - - -float oscilloscope[OSCILLOSCOPE_SIZE] = { - 0.123f, 0.345f, 0.4576f, 1.543f, -50.0f -}; -size_t oscilloscope_pos = 0; - - -uint32_t comm_stack_info = 0; // for debugging only - -// Helper class because the protocol library doesn't yet -// support non-member functions -// TODO: make this go away -class StaticFunctions { -public: - void save_configuration_helper() { save_configuration(); } - void erase_configuration_helper() { erase_configuration(); } - void NVIC_SystemReset_helper() { NVIC_SystemReset(); } - void enter_dfu_mode_helper() { enter_dfu_mode(); } - float get_oscilloscope_val(uint32_t index) { return oscilloscope[index]; } -} static_functions; - -// When adding new functions/variables to the protocol, be careful not to -// blow the communication stack. You can check comm_stack_info to see -// how much headroom you have. -static inline auto make_obj_tree() { - return make_protocol_member_list( - make_protocol_ro_property("vbus_voltage", &vbus_voltage), - make_protocol_ro_property("comm_stack_info", &comm_stack_info), - make_protocol_ro_property("serial_number", &serial_number), - make_protocol_ro_property("board_version_major", &board_version_major), - make_protocol_ro_property("board_version_minor", &board_version_minor), - make_protocol_ro_property("board_version_variant", &board_version_variant), - make_protocol_ro_property("fw_version_major", &fw_version_major), - make_protocol_ro_property("fw_version_minor", &fw_version_minor), - make_protocol_ro_property("fw_version_revision", &fw_version_revision), - make_protocol_ro_property("fw_version_unreleased", &fw_version_unreleased), - make_protocol_ro_property("brake_resistor_armed", &brake_resistor_armed_), - make_protocol_ro_property("user_config_loaded", &user_config_loaded_), - make_protocol_object("config", - make_protocol_property("brake_resistance", &board_config.brake_resistance), - // TODO: changing this currently requires a reboot - fix this - make_protocol_property("enable_uart", &board_config.enable_uart), - make_protocol_property("dc_bus_undervoltage_trip_level", &board_config.dc_bus_undervoltage_trip_level), - make_protocol_property("dc_bus_overvoltage_trip_level", &board_config.dc_bus_overvoltage_trip_level) - ), - make_protocol_object("axis0", axes[0]->make_protocol_definitions()), - make_protocol_object("axis1", axes[1]->make_protocol_definitions()), - make_protocol_function_with_ret("get_oscilloscope_val", static_functions, &StaticFunctions::get_oscilloscope_val, "index"), - make_protocol_function("save_configuration", static_functions, &StaticFunctions::save_configuration_helper), - make_protocol_function("erase_configuration", static_functions, &StaticFunctions::erase_configuration_helper), - make_protocol_function("reboot", static_functions, &StaticFunctions::NVIC_SystemReset_helper), - make_protocol_function("enter_dfu_mode", static_functions, &StaticFunctions::enter_dfu_mode_helper) - ); -} - -using tree_type = decltype(make_obj_tree()); -uint8_t tree_buffer[sizeof(tree_type)]; - -// the protocol has one additional built-in endpoint -constexpr size_t MAX_ENDPOINTS = decltype(make_obj_tree())::endpoint_count + 1; -Endpoint* endpoints_[MAX_ENDPOINTS] = { 0 }; -const size_t max_endpoints_ = MAX_ENDPOINTS; -size_t n_endpoints_ = 0; - -// Thread to handle deffered processing of USB interrupt, and -// read commands out of the UART DMA circular buffer -void communication_task(void * ctx) { - (void) ctx; // unused parameter - - // TODO: this is supposed to use the move constructor, but currently - // the compiler uses the copy-constructor instead. Thus the make_obj_tree - // ends up with a stupid stack size of around 8000 bytes. Fix this. - auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree()); - auto endpoint_provider = EndpointProvider_from_MemberList(*tree_ptr); - set_application_endpoints(&endpoint_provider); - comm_stack_info = uxTaskGetStackHighWaterMark(nullptr); - -#if !defined(UART_PROTOCOL_NONE) - //DMA open loop continous circular buffer - //1ms delay periodic, chase DMA ptr around - - #define UART_RX_BUFFER_SIZE 64 - static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE]; - - // DMA is set up to recieve in a circular buffer forever. - // We dont use interrupts to fetch the data, instead we periodically read - // data out of the circular buffer into a parse buffer, controlled by a state machine - HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); - uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; -#endif - - // Re-run state-machine forever - for (;;) { -#if !defined(UART_PROTOCOL_NONE) - // Check for UART errors and restart recieve DMA transfer if required - if (huart4.ErrorCode != HAL_UART_ERROR_NONE) { - HAL_UART_AbortReceive(&huart4); - HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); - } - // Fetch the circular buffer "write pointer", where it would write next - uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; - - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { -#if defined(UART_PROTOCOL_NATIVE) - uart4_stream_input.process_bytes(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx); -#endif -#if defined(UART_PROTOCOL_ASCII) - ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx, uart4_stream_output); -#endif - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { -#if defined(UART_PROTOCOL_NATIVE) - uart4_stream_input.process_bytes(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); -#endif -#if defined(UART_PROTOCOL_ASCII) - ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx, uart4_stream_output); -#endif - last_rcv_idx = new_rcv_idx; - } -#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_native_stream_input.process_bytes(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_ASCII) - ASCII_protocol_parse_stream(usb_buf, usb_len, usb_stream_output); -#endif - USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet - } -#endif - -#if defined(USB_PROTOCOL_NONE) && defined(UART_PROTOCOL_NONE) - osDelay(1); // don't starve other threads -#endif - } - - // If we get here, then this task is done - vTaskDelete(osThreadGetId()); -} - -// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the -// incoming USB data -void set_cmd_buffer(uint8_t *buf, uint32_t len) { - usb_buf = buf; - usb_len = len; -} - -void usb_update_thread(void * ctx) { - (void) ctx; // unused parameter - - for (;;) { - // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) - osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever); - if (semaphore_status == osOK) { - // We have a new incoming USB transmission: handle it - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - // Let the irq (OTG_FS_IRQHandler) fire again. - HAL_NVIC_EnableIRQ(OTG_FS_IRQn); - } - } - - vTaskDelete(osThreadGetId()); -} - -extern "C" { -int _write(int file, const char* data, int len); -} - -// @brief This is what printf calls internally -int _write(int file, const char* data, int len) { -#ifdef USB_PROTOCOL_STDOUT - usb_stream_output.process_bytes((const uint8_t *)data, len); -#endif -#ifdef UART_PROTOCOL_STDOUT - uart4_stream_output.process_bytes((const uint8_t *)data, len); -#endif - return len; -} - -void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { - osSemaphoreRelease(sem_uart_dma); -} diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index b433efe7..7cc55894 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -1,5 +1,5 @@ -#include "odrive_main.hpp" +#include "odrive_main.h" Controller::Controller(ControllerConfig_t& config) : diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 3a495d18..c76161ea 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -1,8 +1,8 @@ #ifndef __CONTROLLER_HPP #define __CONTROLLER_HPP -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." +#ifndef __ODRIVE_MAIN_H +#error "This file should not be included directly. Include odrive_main.h instead." #endif // Note: these should be sorted from lowest level of control to diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index f2caa9b2..0e736ae1 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -1,6 +1,5 @@ -//#include "encoder.hpp" -#include "odrive_main.hpp" +#include "odrive_main.h" Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 841127f4..a97b94a1 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -1,8 +1,8 @@ #ifndef __ENCODER_HPP #define __ENCODER_HPP -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." +#ifndef __ODRIVE_MAIN_H +#error "This file should not be included directly. Include odrive_main.h instead." #endif struct EncoderConfig_t { diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 185708db..d7cfcc86 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -19,7 +19,7 @@ #include #include -#include "odrive_main.hpp" +#include "odrive_main.h" /* Private defines -----------------------------------------------------------*/ diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 2bd9fe04..e3784788 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -2,8 +2,8 @@ #ifndef __LOW_LEVEL_H #define __LOW_LEVEL_H -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." +#ifndef __ODRIVE_MAIN_H +#error "This file should not be included directly. Include odrive_main.h instead." #endif #ifdef __cplusplus diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 75cd43d2..56b57bb2 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -1,7 +1,7 @@ -#include "odrive_main.hpp" +#define __MAIN_CPP__ +#include "odrive_main.h" #include "nvm_config.hpp" -#include "communication.h" BoardConfig_t board_config; EncoderConfig_t encoder_configs[AXIS_COUNT]; @@ -10,6 +10,8 @@ MotorConfig_t motor_configs[AXIS_COUNT]; AxisConfig_t axis_configs[AXIS_COUNT]; bool user_config_loaded_; +bool user_config_loaded = false; + Axis *axes[AXIS_COUNT]; typedef Config< @@ -56,6 +58,12 @@ void erase_configuration(void) { NVM_erase(); } +void enter_dfu_mode(void) { + __asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts + _reboot_cookie = 0xDEADBEEF; + NVIC_SystemReset(); +} + extern "C" { int odrive_main(void); void vApplicationStackOverflowHook(void) { for(;;); } diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 9558dc35..3272c746 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -2,8 +2,7 @@ #include #include "drv8301.h" -//#include "motor.hpp" -#include "odrive_main.hpp" +#include "odrive_main.h" Motor::Motor(const MotorHardwareConfig_t& hw_config, diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 7f9e9b08..fb85163d 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -1,8 +1,8 @@ #ifndef __MOTOR_HPP #define __MOTOR_HPP -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." +#ifndef __ODRIVE_MAIN_H +#error "This file should not be included directly. Include odrive_main.h instead." #endif #include "drv8301.h" diff --git a/Firmware/MotorControl/nvm_config.hpp b/Firmware/MotorControl/nvm_config.hpp index 7784322b..bf0f9134 100644 --- a/Firmware/MotorControl/nvm_config.hpp +++ b/Firmware/MotorControl/nvm_config.hpp @@ -12,7 +12,7 @@ #include #include "nvm.h" -#include "crc.hpp" +#include /* Private defines -----------------------------------------------------------*/ diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.h similarity index 87% rename from Firmware/MotorControl/odrive_main.hpp rename to Firmware/MotorControl/odrive_main.h index 3b170736..0b75af48 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.h @@ -1,24 +1,43 @@ -#ifndef __ODRIVE_MAIN_HPP -#define __ODRIVE_MAIN_HPP +#ifndef __ODRIVE_MAIN_H +#define __ODRIVE_MAIN_H -// stdlib includes -#include - -// System includes -#include +#ifdef __cplusplus +extern "C" { +#endif // STM specific includes #include // Sets up the correct chip specifc defines required by arm_math #define ARM_MATH_CM4 // TODO: might change in future board versions #include +// OS includes +#include + // Hardware configuration #if HW_VERSION_MAJOR == 3 -#include +#include "board_config_v3.h" #else #error "unknown board version" #endif +//default timeout waiting for phase measurement signals +#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms] + +static const float current_meas_period = CURRENT_MEAS_PERIOD; +static const int current_meas_hz = CURRENT_MEAS_HZ; +extern float vbus_voltage; +extern bool brake_resistor_armed_; +extern const float elec_rad_per_enc; +extern uint32_t _reboot_cookie; +extern bool user_config_loaded; + +extern uint64_t serial_number; +extern char serial_number_str[13]; + + +#ifdef __cplusplus +} + // @brief general user configurable board configuration struct BoardConfig_t { bool enable_uart = true; @@ -29,21 +48,12 @@ struct BoardConfig_t { //(~static_c // ODrive specific includes -#include +#include #include #include #include @@ -72,9 +82,14 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include #include +#include -// defined in main.cpp +#endif // __cplusplus + + +// general system functions defined in main.cpp void save_configuration(void); void erase_configuration(void); +void enter_dfu_mode(void); -#endif /* __ODRIVE_MAIN_HPP */ +#endif /* __ODRIVE_MAIN_H */ diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 4ae081f4..1098b38c 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -1,6 +1,5 @@ -//#include "sensorless_estimator.hpp" -#include "odrive_main.hpp" +#include "odrive_main.h" SensorlessEstimator::SensorlessEstimator() { diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 52e7f98b..bc95cd27 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -9,11 +9,6 @@ extern "C" { #include #include -/** - * @brief Unique ID register address location - */ -#define ID_UNIQUE_ADDRESS (0x1FFF7A10) - /** * @brief Flash size register address */ @@ -60,17 +55,6 @@ extern "C" { */ #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) - #ifdef M_PI #undef M_PI #endif diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 6aeb6590..feae2670 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -125,7 +125,7 @@ for src in string.gmatch(vars['C_INCLUDES'] or '', "%S+") do end -- TODO: cleaner separation of the platform code and the rest -stm_includes += 'MotorControl' +stm_includes += '.' stm_includes += 'Drivers/DRV8301' stm_sources += boarddir..'/Src/syscalls.c' build{ @@ -150,21 +150,24 @@ build{ sources={ 'Drivers/DRV8301/drv8301.c', 'MotorControl/utils.c', - 'MotorControl/ascii_protocol.cpp', 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', - 'MotorControl/communication.cpp', - 'MotorControl/protocol.cpp', 'MotorControl/motor.cpp', 'MotorControl/encoder.cpp', 'MotorControl/controller.cpp', 'MotorControl/sensorless_estimator.cpp', 'MotorControl/main.cpp', + 'communication/communication.cpp', + 'communication/ascii_protocol.cpp', + 'communication/protocol.cpp', + 'communication/interface_uart.cpp', + 'communication/interface_usb.cpp', 'FreeRTOS-openocd.c' }, includes={ 'Drivers/DRV8301', - 'MotorControl' + 'MotorControl', + '.' } } diff --git a/Firmware/build.lua b/Firmware/build.lua index 1d5ce8d6..8ef67627 100644 --- a/Firmware/build.lua +++ b/Firmware/build.lua @@ -67,7 +67,7 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags) else extra_outputs = {} end - if src == 'MotorControl/communication.cpp' then extra_inputs = 'build/version.h' end -- TODO: fix hack + if src == 'communication/communication.cpp' then extra_inputs = 'build/version.h' end -- TODO: fix hack tup.frule{ inputs= { src, extra_inputs=extra_inputs }, command=compiler..' -c %f '.. diff --git a/Firmware/MotorControl/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp similarity index 79% rename from Firmware/MotorControl/ascii_protocol.cpp rename to Firmware/communication/ascii_protocol.cpp index 08eecc1d..98f1cd02 100644 --- a/Firmware/MotorControl/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -7,7 +7,7 @@ /* Includes ------------------------------------------------------------------*/ -#include "odrive_main.hpp" +#include "odrive_main.h" #include "communication.h" #include "ascii_protocol.h" #include @@ -18,7 +18,9 @@ /* Global variables ----------------------------------------------------------*/ /* Private constant data -----------------------------------------------------*/ -#define MAX_LINE_LENGTH 64 +#define MAX_LINE_LENGTH 256 +#define TO_STR_INNER(s) #s +#define TO_STR(s) TO_STR_INNER(s) /* Private variables ---------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ @@ -131,8 +133,40 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize()); respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); -// } else if (cmd[0] == 'r') { // read property -// } else if (cmd[0] == 'w') { // write property + } else if (cmd[0] == 'r') { // read property + char name[MAX_LINE_LENGTH]; + int numscan = sscanf(cmd, "r %" TO_STR(MAX_LINE_LENGTH) "s", name); + if (numscan < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else { + Endpoint* endpoint = application_endpoints->get_by_name(name, sizeof(name)); + if (!endpoint) { + respond(response_channel, use_checksum, "invalid property"); + } else { + char response[10]; + bool success = endpoint->get_string(response, sizeof(response)); + if (!success) + respond(response_channel, use_checksum, "not implemented"); + else + respond(response_channel, use_checksum, response); + } + } + } else if (cmd[0] == 'w') { // write property + char name[MAX_LINE_LENGTH]; + char value[MAX_LINE_LENGTH]; + int numscan = sscanf(cmd, "w %" TO_STR(MAX_LINE_LENGTH) "s %" TO_STR(MAX_LINE_LENGTH) "s", name, value); + if (numscan < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else { + Endpoint* endpoint = application_endpoints->get_by_name(name, sizeof(name)); + if (!endpoint) { + respond(response_channel, use_checksum, "invalid property"); + } else { + bool success = endpoint->set_string(value, sizeof(value)); + if (!success) + respond(response_channel, use_checksum, "not implemented"); + } + } } else if (cmd[0] == 'h') { // HALT for(size_t i = 0; i < AXIS_COUNT; i++){ diff --git a/Firmware/MotorControl/ascii_protocol.h b/Firmware/communication/ascii_protocol.h similarity index 60% rename from Firmware/MotorControl/ascii_protocol.h rename to Firmware/communication/ascii_protocol.h index 680dd9f3..82830e10 100644 --- a/Firmware/MotorControl/ascii_protocol.h +++ b/Firmware/communication/ascii_protocol.h @@ -1,34 +1,21 @@ -#ifndef ASCII_PROTOCOL_H -#define ASCII_PROTOCOL_H - -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." -#endif +#ifndef __ASCII_PROTOCOL_H +#define __ASCII_PROTOCOL_H #ifdef __cplusplus extern "C" { #endif /* Includes ------------------------------------------------------------------*/ + +#include "protocol.hpp" + #include #include #include + /* Exported types ------------------------------------------------------------*/ - -typedef enum { - SERIAL_PRINTF_IS_NONE, - SERIAL_PRINTF_IS_USB, - SERIAL_PRINTF_IS_UART, -} SerialPrintf_t; - /* Exported constants --------------------------------------------------------*/ /* Exported variables --------------------------------------------------------*/ -extern SerialPrintf_t serial_printf_select; -// Exposed comms table during refactor transition -extern float* exposed_floats[]; -extern int* exposed_ints[]; -extern bool* exposed_bools[]; -extern uint16_t* exposed_uint16[]; /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ @@ -39,4 +26,4 @@ void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& } #endif -#endif /* ASCII_PROTOCOL_H */ +#endif /* __ASCII_PROTOCOL_H */ diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp new file mode 100644 index 00000000..8e4f826a --- /dev/null +++ b/Firmware/communication/communication.cpp @@ -0,0 +1,174 @@ + +/* Includes ------------------------------------------------------------------*/ + +#include "communication.h" + +#include "interface_usb.h" +#include "interface_uart.h" + +#include "odrive_main.h" +#include "protocol.hpp" +#include "freertos_vars.h" +#include "utils.h" + +#include "../build/version.h" // autogenerated based on Git state + +#include +#include +//#include +//#include +//#include +//#include + +#include + +/* Private defines -----------------------------------------------------------*/ +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ + +uint64_t serial_number; +char serial_number_str[13]; // 12 digits + null termination + +/* Private constant data -----------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ + +#if HW_VERSION_MAJOR == 3 +// Determine start address of the OTP struct: +// The OTP is organized into 16-byte blocks. +// If the first block starts with "0xfe" we use the first block. +// If the first block starts with "0x00" and the second block starts with "0xfe", +// we use the second block. This gives the user the chance to screw up once. +// If none of the above is the case, we consider the OTP invalid (otp_ptr will be NULL). +const uint8_t* otp_ptr = + (*(uint8_t*)FLASH_OTP_BASE == 0xfe) ? (uint8_t*)FLASH_OTP_BASE : + (*(uint8_t*)FLASH_OTP_BASE != 0x00) ? NULL : + (*(uint8_t*)(FLASH_OTP_BASE + 0x10) != 0xfe) ? NULL : + (uint8_t*)(FLASH_OTP_BASE + 0x10); + +// Read hardware version from OTP if available, otherwise fall back +// to software defined version. +const uint8_t hw_version_major = otp_ptr ? otp_ptr[3] : HW_VERSION_MAJOR; +const uint8_t hw_version_minor = otp_ptr ? otp_ptr[4] : HW_VERSION_MINOR; +const uint8_t hw_version_variant = otp_ptr ? otp_ptr[5] : HW_VERSION_VOLTAGE; +#else +#error "not implemented" +#endif + +// the corresponding macros are defined in the autogenerated version.h +const uint8_t fw_version_major = FW_VERSION_MAJOR; +const uint8_t fw_version_minor = FW_VERSION_MINOR; +const uint8_t fw_version_revision = FW_VERSION_REVISION; +const uint8_t fw_version_unreleased = FW_VERSION_UNRELEASED; // 0 for official releases, 1 otherwise + +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + +void init_communication(void) { + printf("hi!\r\n"); + + // Start command handling thread + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5000 /* in 32-bit words */); // TODO: fix stack issues + osThreadCreate(osThread(task_cmd_parse), NULL); +} + + +float oscilloscope[OSCILLOSCOPE_SIZE] = { + 0.123f, 0.345f, 0.4576f, 1.543f, -50.0f +}; +size_t oscilloscope_pos = 0; + + +uint32_t comm_stack_info = 0; // for debugging only + +// Helper class because the protocol library doesn't yet +// support non-member functions +// TODO: make this go away +class StaticFunctions { +public: + void save_configuration_helper() { save_configuration(); } + void erase_configuration_helper() { erase_configuration(); } + void NVIC_SystemReset_helper() { NVIC_SystemReset(); } + void enter_dfu_mode_helper() { enter_dfu_mode(); } + float get_oscilloscope_val(uint32_t index) { return oscilloscope[index]; } +} static_functions; + +// When adding new functions/variables to the protocol, be careful not to +// blow the communication stack. You can check comm_stack_info to see +// how much headroom you have. +static inline auto make_obj_tree() { + return make_protocol_member_list( + make_protocol_ro_property("vbus_voltage", &vbus_voltage), + make_protocol_ro_property("comm_stack_info", &comm_stack_info), + make_protocol_ro_property("serial_number", &serial_number), + make_protocol_ro_property("hw_version_major", &hw_version_major), + make_protocol_ro_property("hw_version_minor", &hw_version_minor), + make_protocol_ro_property("hw_version_variant", &hw_version_variant), + make_protocol_ro_property("fw_version_major", &fw_version_major), + make_protocol_ro_property("fw_version_minor", &fw_version_minor), + make_protocol_ro_property("fw_version_revision", &fw_version_revision), + make_protocol_ro_property("fw_version_unreleased", &fw_version_unreleased), + make_protocol_ro_property("user_config_loaded", const_cast(&user_config_loaded)), + make_protocol_ro_property("brake_resistor_armed", &brake_resistor_armed_), + make_protocol_object("config", + make_protocol_property("brake_resistance", &board_config.brake_resistance), + // TODO: changing this currently requires a reboot - fix this + make_protocol_property("enable_uart", &board_config.enable_uart), + make_protocol_property("dc_bus_undervoltage_trip_level", &board_config.dc_bus_undervoltage_trip_level), + make_protocol_property("dc_bus_overvoltage_trip_level", &board_config.dc_bus_overvoltage_trip_level) + ), + make_protocol_object("axis0", axes[0]->make_protocol_definitions()), + make_protocol_object("axis1", axes[1]->make_protocol_definitions()), + make_protocol_function_with_ret("get_oscilloscope_val", static_functions, &StaticFunctions::get_oscilloscope_val, "index"), + make_protocol_function("save_configuration", static_functions, &StaticFunctions::save_configuration_helper), + make_protocol_function("erase_configuration", static_functions, &StaticFunctions::erase_configuration_helper), + make_protocol_function("reboot", static_functions, &StaticFunctions::NVIC_SystemReset_helper), + make_protocol_function("enter_dfu_mode", static_functions, &StaticFunctions::enter_dfu_mode_helper) + ); +} + +using tree_type = decltype(make_obj_tree()); +uint8_t tree_buffer[sizeof(tree_type)]; + +// the protocol has one additional built-in endpoint +constexpr size_t MAX_ENDPOINTS = decltype(make_obj_tree())::endpoint_count + 1; +Endpoint* endpoints_[MAX_ENDPOINTS] = { 0 }; +const size_t max_endpoints_ = MAX_ENDPOINTS; +size_t n_endpoints_ = 0; + +// Thread to handle deffered processing of USB interrupt, and +// read commands out of the UART DMA circular buffer +void communication_task(void * ctx) { + (void) ctx; // unused parameter + + // TODO: this is supposed to use the move constructor, but currently + // the compiler uses the copy-constructor instead. Thus the make_obj_tree + // ends up with a stupid stack size of around 8000 bytes. Fix this. + auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree()); + auto endpoint_provider = EndpointProvider_from_MemberList(*tree_ptr); + set_application_endpoints(&endpoint_provider); + comm_stack_info = uxTaskGetStackHighWaterMark(nullptr); + + serve_on_uart(); + serve_on_usb(); + + for (;;) { + osDelay(1000); // nothing to do + } +} + +extern "C" { +int _write(int file, const char* data, int len); +} + +// @brief This is what printf calls internally +int _write(int file, const char* data, int len) { +#ifdef USB_PROTOCOL_STDOUT + usb_stream_output.process_bytes((const uint8_t *)data, len); +#endif +#ifdef UART_PROTOCOL_STDOUT + uart4_stream_output.process_bytes((const uint8_t *)data, len); +#endif + return len; +} diff --git a/Firmware/MotorControl/communication.h b/Firmware/communication/communication.h similarity index 58% rename from Firmware/MotorControl/communication.h rename to Firmware/communication/communication.h index 88e37921..9d1dff2e 100644 --- a/Firmware/MotorControl/communication.h +++ b/Firmware/communication/communication.h @@ -15,12 +15,6 @@ extern "C" { void init_communication(void); void communication_task(void * ctx); -void set_cmd_buffer(uint8_t *buf, uint32_t len); -void usb_update_thread(void * ctx); -void USB_receive_packet(const uint8_t *buffer, size_t length); - -extern uint64_t serial_number; -extern char serial_number_str[13]; #ifdef __cplusplus } diff --git a/Firmware/MotorControl/crc.hpp b/Firmware/communication/crc.hpp similarity index 100% rename from Firmware/MotorControl/crc.hpp rename to Firmware/communication/crc.hpp diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp new file mode 100644 index 00000000..d83442af --- /dev/null +++ b/Firmware/communication/interface_uart.cpp @@ -0,0 +1,101 @@ + +#include "interface_uart.h" +#include "protocol.hpp" + +#include "ascii_protocol.h" + +#include + +#include +#include +#include + +#define UART_TX_BUFFER_SIZE 64 +#define UART_RX_BUFFER_SIZE 64 + +// DMA open loop continous circular buffer +// 1ms delay periodic, chase DMA ptr around +static uint8_t dma_rx_buffer[UART_RX_BUFFER_SIZE]; +static uint32_t dma_last_rcv_idx; + +// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable +static thread_local uint32_t deadline_ms = 0; + + +class UART4Sender : public StreamSink { +public: + int process_bytes(const uint8_t* buffer, size_t length) { + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; + // wait for USB interface to become ready + // TODO: implement ring buffer to get a more continuous stream of data + if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit chunk + memcpy(tx_buf_, buffer, chunk); + if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) + return -1; + buffer += chunk; + length -= chunk; + } + return 0; + } + + size_t get_free_space() { return SIZE_MAX; } +private: + uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; +} uart4_stream_output; + +PacketToStreamConverter uart4_packet_output(uart4_stream_output); +BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output); +StreamToPacketConverter uart4_stream_input(uart4_channel); + +static void uart_server_thread(void * ctx) { + (void) ctx; + + for (;;) { + // 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_rx_buffer, sizeof(dma_rx_buffer)); + } + // Fetch the circular buffer "write pointer", where it would write next + uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < dma_last_rcv_idx) { + uart4_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx, + UART_RX_BUFFER_SIZE - dma_last_rcv_idx); + ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx, + UART_RX_BUFFER_SIZE - dma_last_rcv_idx, uart4_stream_output); + dma_last_rcv_idx = 0; + } + if (new_rcv_idx > dma_last_rcv_idx) { + uart4_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx, + new_rcv_idx - dma_last_rcv_idx); + ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx, + new_rcv_idx - dma_last_rcv_idx, uart4_stream_output); + dma_last_rcv_idx = new_rcv_idx; + } + + osDelay(1); + }; +} + +void serve_on_uart() { + // DMA is set up to recieve in a circular buffer forever. + // We dont use interrupts to fetch the data, instead we periodically read + // data out of the circular buffer into a parse buffer, controlled by a state machine + HAL_UART_Receive_DMA(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer)); + dma_last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + + // Start UART communication thread + osThreadDef(uart_server_thread_def, uart_server_thread, osPriorityNormal, 0, 512); + osThreadCreate(osThread(uart_server_thread_def), NULL); +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { + osSemaphoreRelease(sem_uart_dma); +} diff --git a/Firmware/communication/interface_uart.h b/Firmware/communication/interface_uart.h new file mode 100644 index 00000000..02c47331 --- /dev/null +++ b/Firmware/communication/interface_uart.h @@ -0,0 +1,14 @@ +#ifndef __INTERFACE_UART_HPP +#define __INTERFACE_UART_HPP + +#ifdef __cplusplus +extern "C" { +#endif + +void serve_on_uart(void); + +#ifdef __cplusplus +} +#endif + +#endif // __INTERFACE_UART_HPP diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp new file mode 100644 index 00000000..0bca55c1 --- /dev/null +++ b/Firmware/communication/interface_usb.cpp @@ -0,0 +1,103 @@ + +#include "interface_usb.h" +#include "protocol.hpp" + +#include + +#include +#include +#include +#include +#include + +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; + + + +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_packet_output; + +#if !defined(USB_PROTOCOL_NATIVE) +class TreatPacketSinkAsStreamSink : public StreamSink { +public: + TreatPacketSinkAsStreamSink(PacketSink& output) : output_(output) {} + int process_bytes(const uint8_t* buffer, size_t length) { + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; + if (output_.process_packet(buffer, length) != 0) + return -1; + buffer += chunk; + length -= chunk; + } + return 0; + } + size_t get_free_space() { return SIZE_MAX; } +private: + PacketSink& output_; +} usb_stream_output(usb_packet_output); +#endif + +#if defined(USB_PROTOCOL_NATIVE) +BidirectionalPacketBasedChannel usb_channel(usb_packet_output); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) +PacketToStreamConverter usb_packetized_output(usb_stream_output); +BidirectionalPacketBasedChannel usb_channel(usb_packetized_output); +#endif + +#if defined(USB_PROTOCOL_NATIVE_STREAM_BASED) +StreamToPacketConverter usb_native_stream_input(usb_channel); +#endif + + +static void usb_server_thread(void * ctx) { + (void) ctx; + + for (;;) { + 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_native_stream_input.process_bytes(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_ASCII) + ASCII_protocol_parse_stream(usb_buf, usb_len, usb_stream_output); +#endif + USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet + } + } +} + +// Called from CDC_Receive_FS callback function, this allows the communication +// thread to handle the incoming data +void usb_process_packet(uint8_t *buf, uint32_t len) { + usb_buf = buf; + usb_len = len; + osSemaphoreRelease(sem_usb_rx); +} + +void serve_on_usb() { + // Start USB communication thread + osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 512); + osThreadCreate(osThread(usb_server_thread_def), NULL); +} diff --git a/Firmware/communication/interface_usb.h b/Firmware/communication/interface_usb.h new file mode 100644 index 00000000..3602843f --- /dev/null +++ b/Firmware/communication/interface_usb.h @@ -0,0 +1,17 @@ +#ifndef __INTERFACE_USB_HPP +#define __INTERFACE_USB_HPP + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +void usb_process_packet(uint8_t *buf, uint32_t len); +void serve_on_usb(void); + +#ifdef __cplusplus +} +#endif + +#endif // __INTERFACE_USB_HPP diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/communication/protocol.cpp similarity index 100% rename from Firmware/MotorControl/protocol.cpp rename to Firmware/communication/protocol.cpp diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/communication/protocol.hpp similarity index 89% rename from Firmware/MotorControl/protocol.hpp rename to Firmware/communication/protocol.hpp index 7c2f9d38..827b226a 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/communication/protocol.hpp @@ -407,12 +407,15 @@ class Endpoint { public: //const char* const name_; virtual void handle(const uint8_t* input, size_t input_length, StreamSink* output) = 0; + virtual bool get_string(char * output, size_t length) { return false; }; + virtual bool set_string(char * buffer, size_t length) { return false; } }; class EndpointProvider { public: virtual size_t get_endpoint_count() = 0; virtual void write_json(size_t id, StreamSink* output) = 0; + virtual Endpoint* get_by_name(char * name, size_t length) = 0; virtual void register_endpoints(Endpoint** list, size_t id, size_t length) = 0; }; @@ -456,6 +459,9 @@ public: void register_endpoints(Endpoint** list, size_t id, size_t length) { // no action } + Endpoint* get_by_name(const char * name, size_t length) { + return nullptr; + } std::tuple<> get_names_as_tuple() const { return std::tuple<>(); } }; @@ -485,6 +491,12 @@ public: subsequent_members_.write_json(id + TMember::endpoint_count, output); } + Endpoint* get_by_name(const char * name, size_t length) { + Endpoint* result = this_member_.get_by_name(name, length); + if (result) return result; + else return subsequent_members_.get_by_name(name, length); + } + void register_endpoints(Endpoint** list, size_t id, size_t length) /*final*/ { this_member_.register_endpoints(list, id, length); subsequent_members_.register_endpoints(list, id + TMember::endpoint_count, length); @@ -516,6 +528,14 @@ public: write_string("]}", output); } + Endpoint* get_by_name(const char * name, size_t length) { + size_t segment_length = strlen(name); + if (!strncmp(name, name_, length)) + return member_list_.get_by_name(name + segment_length + 1, length - segment_length - 1); + else + return nullptr; + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { member_list_.register_endpoints(list, id, length); } @@ -529,6 +549,11 @@ ProtocolObject make_protocol_object(const char * name, TMembers&&.. return ProtocolObject(name, std::forward(member_list)...); } + +// TODO: move to cpp_utils +#define ENABLE_IF_SAME(a, b, type) \ + template typename std::enable_if_t::value, bool> + template class ProtocolProperty : public Endpoint { public: @@ -585,6 +610,71 @@ public: write_string("}", output); } + Endpoint* get_by_name(const char * name, size_t length) { + if (!strncmp(name, name_, length)) + return this; + else + return nullptr; + } + + + // *** ASCII protocol handlers *** + + ENABLE_IF_SAME(std::decay_t, float, bool) + get_string_ex(char * buffer, size_t length, int) { + snprintf(buffer, length, "%f", *property_); + return true; + } + ENABLE_IF_SAME(std::decay_t, int32_t, bool) + get_string_ex(char * buffer, size_t length, int) { + snprintf(buffer, length, "%ld", *property_); + return true; + } + ENABLE_IF_SAME(std::decay_t, uint32_t, bool) + get_string_ex(char * buffer, size_t length, int) { + snprintf(buffer, length, "%lu", *property_); + return true; + } + ENABLE_IF_SAME(std::decay_t, bool, bool) + get_string_ex(char * buffer, size_t length, int) { + buffer[0] = (*property_) ? '1' : '0'; + buffer[1] = 0; + return true; + } + bool get_string_ex(char * buffer, size_t length, ...) { + return false; + } + bool get_string(char * buffer, size_t length) final { + return get_string_ex(buffer, length, 0); + } + ENABLE_IF_SAME(TProperty, float, bool) + set_string_ex(char * buffer, size_t length, int) { + return sscanf(buffer, "%f", property_) == 1; + } + ENABLE_IF_SAME(TProperty, int32_t, bool) + set_string_ex(char * buffer, size_t length, int) { + return sscanf(buffer, "%ld", property_) == 1; + } + ENABLE_IF_SAME(TProperty, uint32_t, bool) + set_string_ex(char * buffer, size_t length, int) { + return sscanf(buffer, "%lu", property_) == 1; + } + ENABLE_IF_SAME(TProperty, bool, bool) + set_string_ex(char * buffer, size_t length, int) { + int val; + if (sscanf(buffer, "%d", &val) != 1) + return false; + *property_ = val; + return true; + } + bool set_string_ex(char * buffer, size_t length, ...) { + return false; + } + bool set_string(char * buffer, size_t length) final { + //__asm ("bkpt"); + return set_string_ex(buffer, length, 0); + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { if (id < length) list[id] = this; @@ -686,7 +776,7 @@ struct PropertyListFactory { template -class ProtocolFunction : Endpoint { +class ProtocolFunction : public Endpoint { public: static constexpr size_t endpoint_count = 1 + MemberList...>::endpoint_count; template @@ -722,6 +812,10 @@ public: write_string("]}", output); } + Endpoint* get_by_name(const char * name, size_t length) { + return nullptr; // can't address functions by name + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { if (id < length) list[id] = this; @@ -845,6 +939,14 @@ public: void register_endpoints(Endpoint** list, size_t id, size_t length) final { return member_list_.register_endpoints(list, id, length); } + Endpoint* get_by_name(char * name, size_t length) final { + for (size_t i = 0; i < length; i++) { + if (name[i] == '.') + name[i] = 0; + } + name[length-1] = 0; + return member_list_.get_by_name(name, length); + } T& member_list_; }; @@ -855,5 +957,6 @@ void set_application_endpoints(EndpointProvider* endpoints); extern Endpoint* endpoints_[]; extern size_t n_endpoints_; extern const size_t max_endpoints_; +extern EndpointProvider* application_endpoints; #endif diff --git a/README.md b/README.md index 192a3c6b..16613f9a 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ All non-power I/O is 3.3V output and 5V tolerant on input, except: You need one or two [brushless motors](https://hackaday.io/project/11583-odrive-high-performance-motor-control/log/37666-hobby-motors-in-your-robots), [quadrature incremental encoder(s)](https://discourse.odriverobotics.com/t/which-encoders-to-choose/63/2), and a power resistor. -*The power resistor values you need depends on your motor setup, and peak/average decelleration power. A good starting point would be a [0.47 ohm, 50W resistor](https://www.digikey.com/product-detail/en/te-connectivity-passive-product/HSA50R47J/A102181-ND/2056131).* +The power resistor values you need depends on your motor setup, and peak/average decelleration power. A good starting point would be a [0.47 ohm, 50W resistor](https://www.digikey.com/product-detail/en/te-connectivity-passive-product/HSA50R47J/A102181-ND/2056131). **Warning! Failure to use a break resistor may result in damage to your ODrive and/or power supply!** 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. diff --git a/tools/odrive/dfu.py b/tools/odrive/dfu.py index aad9a3d8..d1b59f82 100755 --- a/tools/odrive/dfu.py +++ b/tools/odrive/dfu.py @@ -13,6 +13,7 @@ import array import fractions import usb.core import odrive.discovery +from odrive.utils import Event from odrive.dfuse import * try: @@ -244,14 +245,13 @@ def launch_dfu(args, app_shutdown_token): serial_number = args.serial_number - find_odrive_cancellation_token = threading.Event() - app_shutdown_token.subscribe(lambda: find_odrive_cancellation_token.set()) + find_odrive_cancellation_token = Event(app_shutdown_token) print("Waiting for ODrive...") # Scan for ODrives not in DFU mode and put them into DFU mode once they appear # We only scan on USB because DFU is only possible over USB - odrive.discovery.find_all(args.path, serial_number, put_odrive_into_dfu_mode, find_odrive_cancellation_token) + odrive.discovery.find_all(args.path, serial_number, put_odrive_into_dfu_mode, find_odrive_cancellation_token, app_shutdown_token) # Poll libUSB until a device in DFU mode is found while not app_shutdown_token.is_set(): diff --git a/tools/odrive/discovery.py b/tools/odrive/discovery.py index 18b5107a..a6536638 100644 --- a/tools/odrive/discovery.py +++ b/tools/odrive/discovery.py @@ -24,7 +24,8 @@ def noprint(text): def find_all(path, serial_number, did_discover_object_callback, - cancellation_token, printer=noprint): + search_cancellation_token, + channel_termination_token, printer=noprint): """ Starts scanning for ODrives that match the specified path spec and calls the callback for each ODrive that is found. @@ -53,6 +54,7 @@ def find_all(path, serial_number, printer("device responded on endpoint 0 with something that is not ASCII") return printer("JSON: " + json_string) + printer("JSON checksum: 0x{:02X} 0x{:02X}".format(json_crc16 & 0xff, (json_crc16 >> 8) & 0xff)) try: json_data = json.loads(json_string) except json.decoder.JSONDecodeError as error: @@ -74,21 +76,23 @@ def find_all(path, serial_number, the_rest = ':'.join(search_spec.split(':')[1:]) if prefix in channel_types: threading.Thread(target=channel_types[prefix], - args=(the_rest, serial_number, did_discover_channel, cancellation_token, printer)).start() + args=(the_rest, serial_number, did_discover_channel, search_cancellation_token, channel_termination_token, printer)).start() else: raise Exception("Invalid path spec \"{}\"".format(search_spec)) -def find_any(path="usb", serial_number=None, cancellation_token=None, timeout=None, printer=noprint): +def find_any(path="usb", serial_number=None, + search_cancellation_token=None, channel_termination_token=None, + timeout=None, printer=noprint): """ Blocks until the first matching ODrive is connected and then returns that device """ result = [ None ] - done_signal = Event(cancellation_token) + done_signal = Event(search_cancellation_token) def did_discover_object(obj): result[0] = obj done_signal.set() - find_all(path, serial_number, did_discover_object, done_signal, printer) + find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, printer) try: done_signal.wait(timeout=timeout) finally: diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index a99bc0b1..4c11971c 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -11,14 +11,14 @@ from odrive.utils import Event import abc -# if sys.version_info >= (3, 4): -ABC = abc.ABC -# else: -# ABC = abc.ABCMeta('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 +if sys.version_info < (3, 3): + from monotonic import monotonic + time.monotonic = monotonic SYNC_BYTE = 0xAA CRC8_INIT = 0x42 @@ -206,7 +206,7 @@ class Channel(PacketSink): _resend_timeout = 0.1 # [s] _send_attempts = 5 - def __init__(self, name, input, output, printer): + def __init__(self, name, input, output, cancellation_token, printer): """ Params: input: A PacketSource where this channel will source packets from on @@ -223,8 +223,8 @@ class Channel(PacketSink): self._expected_acks = {} self._responses = {} self._my_lock = threading.Lock() - self._channel_broken = Event() - self.start_receiver_thread(Event()) # TODO: use app_shutdown_token + self._channel_broken = Event(cancellation_token) + self.start_receiver_thread(Event(self._channel_broken)) def start_receiver_thread(self, cancellation_token): """ @@ -251,7 +251,7 @@ class Channel(PacketSink): # Process response # This should not throw an exception, otherwise the channel breaks self.process_packet(response) - print("receiver thread is exiting") + #print("receiver thread is exiting") except Exception: self._printer("receiver thread is exiting: " + traceback.format_exc()) finally: @@ -301,7 +301,7 @@ class Channel(PacketSink): self._my_lock.release() # Wait for ACK until the resend timeout is exceeded try: - if wait_any(ack_event, self._channel_broken, timeout=self._resend_timeout) != 0: + if wait_any(self._resend_timeout, ack_event, self._channel_broken) != 0: raise ChannelBrokenException() except odrive.utils.TimeoutException: attempt += 1 @@ -324,7 +324,7 @@ class Channel(PacketSink): # TODO: handle device that could (maliciously) send infinite stream buffer = bytes() while True: - chunk_length = 64 + chunk_length = 512 chunk = self.remote_endpoint_operation(endpoint_id, struct.pack(" 0: return self._outputs[0].get_value() + def dump(self): + return "{}({})".format(self._name, ", ".join("{}: {}".format(x._name, x._property_type.__name__) for x in self._inputs)) + class RemoteObject(object): """ Object with functions and properties that map to remote endpoints @@ -163,8 +181,21 @@ class RemoteObject(object): self.__sealed__ = True channel._channel_broken.subscribe(self._tear_down) + def dump(self, indent, depth): + if depth <= 0: + return "..." + lines = [] + for key, val in self._remote_attributes.items(): + if isinstance(val, RemoteObject): + val_str = indent + key + (": " if depth == 1 else ":\n") + val.dump(indent + " ", depth - 1) + else: + val_str = indent + val.dump() + lines.append(val_str) + return "\n".join(lines) + def __str__(self): - return str(dir(self)) # TODO: improve print output + return self.dump("", depth=2) + def __repr__(self): return self.__str__() diff --git a/tools/odrive/serial_transport.py b/tools/odrive/serial_transport.py index dd595c6d..8a8b350a 100644 --- a/tools/odrive/serial_transport.py +++ b/tools/odrive/serial_transport.py @@ -6,6 +6,7 @@ PacketSource/PacketSink interfaces for serial ports. import os import re import time +import traceback import serial import serial.tools.list_ports import odrive.protocol @@ -53,10 +54,11 @@ def find_pyserial_ports(): return [x.device for x in serial.tools.list_ports.comports()] -def discover_channels(path, serial_number, callback, cancellation_token, printer): +def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, printer): """ Scans for serial ports that match the path spec. This function blocks until cancellation_token is set. + Channels spawned by this function run until channel_termination_token is set. """ if path == None: # This regex should match all desired port names on macOS, @@ -86,7 +88,7 @@ def discover_channels(path, serial_number, callback, cancellation_token, printer output_stream = odrive.protocol.PacketToStreamConverter(serial_device) channel = odrive.protocol.Channel( "serial port {}@{}".format(port_name, ODRIVE_BAUDRATE), - input_stream, output_stream, printer) + input_stream, output_stream, channel_termination_token, printer) channel.serial_device = serial_device except serial.serialutil.SerialException: printer("Serial device init failed. Ignoring this port. More info: " + traceback.format_exc()) diff --git a/tools/odrive/shell.py b/tools/odrive/shell.py index 19fee9dd..ef072010 100644 --- a/tools/odrive/shell.py +++ b/tools/odrive/shell.py @@ -8,7 +8,7 @@ from odrive.enums import * # pylint: disable=W0614 def print_banner(): print('Please connect your ODrive.') - print('Type help() for help.') + print('You can also type help() or quit().') def print_help(args): print('') @@ -78,6 +78,7 @@ def launch_shell(args, logger, printer, app_shutdown_token): odrive.discovery.find_all(args.path, args.serial_number, lambda dev: did_discover_device(dev, logger, app_shutdown_token), app_shutdown_token, + app_shutdown_token, printer=printer) # Check if IPython is installed diff --git a/tools/odrive/usbbulk_transport.py b/tools/odrive/usbbulk_transport.py index dc36f334..eefc33e8 100644 --- a/tools/odrive/usbbulk_transport.py +++ b/tools/odrive/usbbulk_transport.py @@ -132,17 +132,12 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) self._was_damaged = True raise odrive.protocol.ChannelDamagedException() - def send_max(self): - return 64 - def receive_max(self): - return 64 - - -def discover_channels(path, serial_number, callback, cancellation_token, printer): +def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, printer): """ Scans for USB devices that match the path spec. This function blocks until cancellation_token is set. + Channels spawned by this function run until channel_termination_token is set. """ if path == None or path == "": bus = None @@ -181,7 +176,7 @@ def discover_channels(path, serial_number, callback, cancellation_token, printer bulk_device.init() channel = odrive.protocol.Channel( "USB device bus {} device {}".format(usb_device.bus, usb_device.address), - bulk_device, bulk_device, printer) + bulk_device, bulk_device, channel_termination_token, printer) channel.usb_device = usb_device # for debugging only except usb.core.USBError as ex: if ex.errno == 13: diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index af54cf3c..eb47227d 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -7,6 +7,8 @@ import sys import time import threading import platform +import subprocess +import os try: if platform.system() == 'Windows': @@ -31,7 +33,7 @@ def start_liveplotter(get_var_callback): import matplotlib.pyplot as plt - cancellation_token = threading.Event() + cancellation_token = Event() global vals vals = [] @@ -142,6 +144,17 @@ def usb_burn_in_test(get_var_callback, cancellation_token): print("read {} values".format(i)) threading.Thread(target=fetch_data).start() +def setup_udev_rules(logger): + if platform.system() != 'Linux': + logger.error("This command only makes sense on Linux") + if os.getuid() != 0: + logger.warn("you should run this as root, otherwise it will probably not work") + with open('/etc/udev/rules.d/50-odrive.rules', 'w') as file: + file.write('SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d3[0-9]", MODE="0666"\n') + subprocess.run(["udevadm", "control", "--reload-rules"], check=True) + subprocess.run(["udevadm", "trigger"], check=True) + logger.info('udev rules configured successfully') + ## Exceptions ## @@ -162,7 +175,7 @@ class Event(): self._subscribers = [] self._mutex = threading.Lock() if not trigger is None: - trigger.subscribe(self.set()) + trigger.subscribe(lambda: self.set()) def is_set(self): return self._evt.is_set() @@ -188,6 +201,8 @@ class Event(): handler is invoked immediately. Returns a function that can be invoked to unsubscribe. """ + if handler is None: + raise TypeError self._mutex.acquire() try: self._subscribers.append(handler) @@ -195,7 +210,7 @@ class Event(): handler() finally: self._mutex.release() - return lambda: self.unsubscribe(handler) + return handler def unsubscribe(self, handler): self._mutex.acquire() @@ -218,19 +233,20 @@ class Event(): self.set() threading.Thread(target=delayed_trigger, daemon=True).start() -def wait_any(*events, timeout=None): +def wait_any(timeout=None, *events): """ Blocks until any of the specified events are triggered. - Returns the number of the event that was triggerd or raises + Returns the index of the event that was triggerd or raises a TimeoutException + Param timeout: A timeout in seconds """ or_event = threading.Event() - unsubscribe_functions = [] + subscriptions = [] for event in events: - unsubscribe_functions.append(event.subscribe(lambda: or_event.set())) + subscriptions.append((event, event.subscribe(lambda: or_event.set()))) or_event.wait(timeout=timeout) - for unsubscribe_function in unsubscribe_functions: - unsubscribe_function() + for event, sub in subscriptions: + event.unsubscribe(sub) for i in range(len(events)): if events[i].is_set(): return i diff --git a/tools/odrivetool b/tools/odrivetool index add4c0c6..a6e80acf 100755 --- a/tools/odrivetool +++ b/tools/odrivetool @@ -3,13 +3,21 @@ ODrive command line utility """ +from __future__ import print_function +import sys import argparse import odrive.discovery from odrive.utils import Logger, Event -# We are interactively printing status messages, so flush by default -import functools -print = functools.partial(print, flush=True) +# Flush stdout by default +# Source: +# https://stackoverflow.com/questions/230751/how-to-flush-output-of-python-print +old_print = print +def print(*args, **kwargs): + kwargs.pop('flush', False) + old_print(*args, **kwargs) + file = kwargs.get('file', sys.stdout) + file.flush() if file is not None else sys.stdout.flush() ## Parse arguments ## @@ -31,6 +39,7 @@ dfu_parser.add_argument('file', metavar='HEX', help='The .hex file to be flashed subparsers.add_parser('liveplotter', help="Upgrade the ODrive's Firmware") subparsers.add_parser('drv-status', help="Show status of the on-board DRV8301 chips (for debugging only)") subparsers.add_parser('rate-test', help="Estimate the average transmission bandwidth over USB") +subparsers.add_parser('udev-setup', help="Linux only: Gives users on your system permission to access the ODrive by installing udev rules") # General arguments parser.add_argument("-p", "--path", metavar="PATH", action="store", @@ -75,21 +84,30 @@ else: printer = lambda x: None logger = Logger(verbose=args.verbose) -logger.debug(str(args)) -print("ODrive control utility v" + odrive.__version__) +def print_version(): + print("ODrive control utility v" + odrive.__version__) app_shutdown_token = Event() try: if args.version == True: - pass + print_version() elif args.command == 'shell': + print_version() + if ".dev" in odrive.__version__: + print("") + logger.warn("Developer Preview") + print(" If you find issues, please report them") + print(" on https://github.com/madcowswe/ODrive/issues") + print(" or better yet, submit a pull request to fix it.") + print("") import odrive.shell odrive.shell.launch_shell(args, logger, printer, app_shutdown_token) elif args.command == 'dfu': + print_version() import odrive.dfu odrive.dfu.launch_dfu(args, app_shutdown_token) @@ -116,6 +134,10 @@ try: my_odrive = odrive.discovery.find_any(path=args.path, serial_number=args.serial_number) rate_test(my_odrive) + elif args.command == 'udev-setup': + from odrive.utils import setup_udev_rules + setup_udev_rules(logger) + else: raise Exception("unknown command: " + args.command) diff --git a/tools/odrivetool.bat b/tools/odrivetool.bat new file mode 100644 index 00000000..765e31cb --- /dev/null +++ b/tools/odrivetool.bat @@ -0,0 +1,2 @@ +@echo off +python %~dp0\odrivetool \ No newline at end of file diff --git a/tools/setup.py b/tools/setup.py index 1c43b0a1..946e6900 100644 --- a/tools/setup.py +++ b/tools/setup.py @@ -10,7 +10,7 @@ To build and package the python tools into a tar archive: python setup.py sdist Warning: Before you proceed, be aware that you can upload a -specific version only ever once. After that you need to increment +specific version only once ever. After that you need to increment the hotfix number. Deleting the release manually on the PyPi website does not help. @@ -19,10 +19,10 @@ Use TestPyPi while developing. To build, package and upload the python tools to TestPyPi, run: python setup.py sdist upload -r pypitest To make a real release ensure you're at the release commit -and then run the above command without the "test". +and then run the above command without the "test" (so just "pypi"). To install a prerelease version from test index: - sudo pip install --index-url https://test.pypi.org/simple/ --no-cache-dir odrive + sudo pip install --pre --index-url https://test.pypi.org/simple/ --no-cache-dir odrive PyPi access requires that you have set up ~/.pypirc with your @@ -32,7 +32,7 @@ to publish packages with the name odrive. # TODO: add additional y/n prompt to prevent from erroneous upload -from distutils.core import setup +from setuptools import setup import os import sys @@ -58,11 +58,17 @@ if creating_package: with open(version_file_path, mode='w') as version_file: version_file.write(version) +# TODO: find a better place for this +if not creating_package: + import platform + if platform.system() == 'Linux': + import odrive.utils + odrive.utils.setup_udev_rules(odrive.utils.Logger()) setup( name = 'odrive', packages = ['odrive', 'odrive.dfuse'], # this must be the same as the name above - scripts = ['odrivetool', 'odrive_demo.py'], + scripts = ['odrivetool', 'odrivetool.bat', 'odrive_demo.py'], version = version, description = 'Control utilities for the ODrive high performance motor controller', author = 'Oskar Weigl', @@ -71,13 +77,14 @@ setup( url = 'https://github.com/madcowswe/ODrive', keywords = ['odrive', 'motor', 'motor control'], install_requires = [ + 'ipython', # Used to do the interactive parts of the odrivetool 'PyUSB', # Required to access USB devices from Python through libusb 'PySerial', # Required to access serial devices from Python 'IntelHex', # Used to by DFU to load firmware files - 'matplotlib' # Required to run the liveplotter + 'matplotlib', # Required to run the liveplotter + 'pywin32==222;platform_system=="Windows"' # Required for fancy terminal features on Windows ], package_data={'': ['version.txt']}, - include_package_data=True, classifiers = [], )