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

Merge branch 'sam_refactoring' into sam_testing

Browse Source
This commit is contained in:
Samuel Sadok
2018-03-17 22:42:16 -07:00
parent 8a33a6d8ff 95ba621403
commit defab4fe90
41 changed files with 3172 additions and 2852 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/.gitignore
+3
View File
@@ -17,6 +17,9 @@ Odrive.xml
.settings/
.project
# VSCode stuff
/.vscode/.cortex-debug.*.state.json
# STM32CubeMX (in case you put it in this folder, or a symlink)
STM32CubeMX
Firmware/Board/v3/Inc/FreeRTOSConfig.h
+2
View File
@@ -109,6 +109,7 @@
#define configUSE_MUTEXES 1
#define configQUEUE_REGISTRY_SIZE 8
#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
#define configCHECK_FOR_STACK_OVERFLOW 1
/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 0
@@ -124,6 +125,7 @@ to exclude the API function. */
#define INCLUDE_vTaskDelayUntil 1
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
/* Cortex-M specific definitions. */
#ifdef __NVIC_PRIO_BITS
Firmware/Board/v3/Inc/gpio.h
+6 -2
View File
@@ -59,7 +59,7 @@
#include "main.h"
/* USER CODE BEGIN Includes */
#include <stdbool.h>
/* USER CODE END Includes */
/* USER CODE BEGIN Private defines */
@@ -71,8 +71,12 @@ void MX_GPIO_Init(void);
/* USER CODE BEGIN Prototypes */
void SetGPIO12toUART();
void SetGPIO12toStepDir();
void SetupENCIndexGPIO();
bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin,
uint32_t pull_up_down,
void (*callback)(void*), void* ctx);
void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin);
/* USER CODE END Prototypes */
Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
Vendored
+3 -3
View File
@@ -270,11 +270,11 @@ typedef enum {
/// Entry point of a thread.
/// \note MUST REMAIN UNCHANGED: \b os_pthread shall be consistent in every CMSIS-RTOS.
typedef void (*os_pthread) (void const *argument);
typedef void (*os_pthread) (void *argument);
/// Entry point of a timer call back function.
/// \note MUST REMAIN UNCHANGED: \b os_ptimer shall be consistent in every CMSIS-RTOS.
typedef void (*os_ptimer) (void const *argument);
typedef void (*os_ptimer) (void *argument);
// >>> the following data type definitions may shall adapted towards a specific RTOS
@@ -323,7 +323,7 @@ typedef StaticQueue_t osStaticMessageQDef_t;
/// Thread Definition structure contains startup information of a thread.
/// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS.
typedef struct os_thread_def {
char *name; ///< Thread name
const char *name; ///< Thread name
os_pthread pthread; ///< start address of thread function
osPriority tpriority; ///< initial thread priority
uint32_t instances; ///< maximum number of instances of that thread function
Firmware/Board/v3/Src/freertos.c
+6 -29
View File
@@ -53,10 +53,8 @@
/* USER CODE BEGIN Includes */
#include "freertos_vars.h"
#include "low_level.h"
#include "axis_c_interface.h"
#include "commands.h"
#include "config.h"
int odrive_main(void);
/* USER CODE END Includes */
/* Variables -----------------------------------------------------------------*/
@@ -77,7 +75,7 @@ uint8_t ucHeap[configTOTAL_HEAP_SIZE];
/* USER CODE END Variables */
/* Function prototypes -------------------------------------------------------*/
void StartDefaultTask(void const * argument);
void StartDefaultTask(void * argument);
extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
@@ -114,7 +112,7 @@ void MX_FREERTOS_Init(void) {
sem_usb_rx = osSemaphoreCreate(osSemaphore(sem_usb_rx), 1);
osSemaphoreWait(sem_usb_rx, 0); // Remove a token.
// Create a semaphore for USB RX
// Create a semaphore for USB TX
osSemaphoreDef(sem_usb_tx);
sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1);
@@ -126,7 +124,7 @@ void MX_FREERTOS_Init(void) {
/* Create the thread(s) */
/* definition and creation of defaultTask */
osThreadDef(defaultTask, StartDefaultTask, osPriorityIdle, 0, 256);
osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 256);
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
/* USER CODE BEGIN RTOS_THREADS */
@@ -139,35 +137,14 @@ void MX_FREERTOS_Init(void) {
}
/* StartDefaultTask function */
void StartDefaultTask(void const * argument)
void StartDefaultTask(void * argument)
{
/* init code for USB_DEVICE */
MX_USB_DEVICE_Init();
/* USER CODE BEGIN StartDefaultTask */
// Init and load persistent configuration
init_configuration();
// Init communications
init_communication();
// Init motor control
init_motor_control();
// Start motor threads
osThreadDef(task_motor_0, axis_thread_entry, osPriorityHigh+1, 0, 512);
osThreadDef(task_motor_1, axis_thread_entry, osPriorityHigh, 0, 512);
thread_motor_0 = osThreadCreate(osThread(task_motor_0), &motors[0]);
thread_motor_1 = osThreadCreate(osThread(task_motor_1), &motors[1]);
// Start command handling thread
osThreadDef(task_cmd_parse, 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);
odrive_main();
//If we get to here, then the default task is done.
vTaskDelete(defaultTaskHandle);
Firmware/Board/v3/Src/gpio.c
+100 -47
View File
@@ -50,7 +50,7 @@
/* Includes ------------------------------------------------------------------*/
#include "gpio.h"
/* USER CODE BEGIN 0 */
#include "low_level.h"
#include <stdbool.h>
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \
|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2
@@ -140,10 +140,44 @@ void MX_GPIO_Init(void)
/* USER CODE BEGIN 2 */
#endif // End GPIO Include
// @brief Returns the IRQ number associated with a certain pin.
// Note that all GPIOs with the same pin number map to the same IRQn,
// no matter which port they belong to.
IRQn_Type get_irq_number(uint16_t pin) {
uint16_t pin_number = 0;
while (pin) {
pin >>= 1;
pin_number++;
}
switch (pin_number) {
case 0: return EXTI0_IRQn;
case 1: return EXTI1_IRQn;
case 2: return EXTI2_IRQn;
case 3: return EXTI3_IRQn;
case 4: return EXTI4_IRQn;
case 5:
case 6:
case 7:
case 8:
case 9: return EXTI9_5_IRQn;
case 10:
case 11:
case 12:
case 13:
case 14:
case 15: return EXTI15_10_IRQn;
default: return 0; // impossible
}
}
// @brief Puts the GPIO's 1 and 2 into UART mode.
// This will disable any interrupt subscribers of these GPIOs.
void SetGPIO12toUART() {
GPIO_InitTypeDef GPIO_InitStruct;
HAL_NVIC_DisableIRQ(EXTI0_IRQn);
// make sure nothing is hogging the GPIO's
GPIO_unsubscribe(GPIO_1_GPIO_Port, GPIO_1_Pin);
GPIO_unsubscribe(GPIO_2_GPIO_Port, GPIO_2_Pin);
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
@@ -160,59 +194,78 @@ void SetGPIO12toUART() {
HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
}
void SetGPIO12toStepDir() {
// Expected subscriptions: 2x step signal + 2x encoder index signal
#define MAX_SUBSCRIPTIONS 10
struct subscription_t {
GPIO_TypeDef* GPIO_port;
uint16_t GPIO_pin;
void (*callback)(void*);
void* ctx;
} subscriptions[MAX_SUBSCRIPTIONS] = { 0 };
size_t n_subscriptions = 0;
// Sets up the specified GPIO to trigger the specified callback
// on a rising edge of the GPIO.
// @param pull_up_down: one of GPIO_NOPULL, GPIO_PULLUP or GPIO_PULLDOWN
bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin,
uint32_t pull_up_down,
void (*callback)(void*), void* ctx) {
// Register handler (or reuse existing registration)
// TODO: make thread safe
struct subscription_t* subscription = NULL;
for (size_t i = 0; i < n_subscriptions; ++i) {
if (subscriptions[i].GPIO_port == GPIO_port &&
subscriptions[i].GPIO_pin == GPIO_pin)
subscription = &subscriptions[i];
}
if (!subscription) {
if (n_subscriptions >= MAX_SUBSCRIPTIONS)
return false;
subscription = &subscriptions[n_subscriptions++];
}
*subscription = (struct subscription_t){
.GPIO_port = GPIO_port,
.GPIO_pin = GPIO_pin,
.callback = callback,
.ctx = ctx
};
// Set up GPIO
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Pin = GPIO_pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pull = pull_up_down;
HAL_GPIO_Init(GPIO_port, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
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);
// Enable interrupt
HAL_NVIC_SetPriority(get_irq_number(GPIO_pin), 0, 0);
HAL_NVIC_EnableIRQ(get_irq_number(GPIO_pin));
return true;
}
//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);
void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) {
bool is_pin_in_use = false;
for (size_t i = 0; i < n_subscriptions; ++i) {
if (subscriptions[i].GPIO_port == GPIO_port &&
subscriptions[i].GPIO_pin == GPIO_pin) {
subscriptions[i].callback = NULL;
subscriptions[i].ctx = NULL;
} else if (subscriptions[i].GPIO_pin == GPIO_pin) {
is_pin_in_use = true;
}
}
if (!is_pin_in_use)
HAL_NVIC_DisableIRQ(get_irq_number(GPIO_pin));
}
//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);
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_pin) {
for (size_t i = 0; i < n_subscriptions; ++i) {
if (subscriptions[i].GPIO_pin == GPIO_pin) // TODO: check for port
if (subscriptions[i].callback)
subscriptions[i].callback(subscriptions[i].ctx);
}
}
Firmware/Board/v3/Src/main.c
+1 -1
View File
@@ -60,7 +60,7 @@
/* USER CODE BEGIN Includes */
#include "utils.h"
#include "commands.h"
#include "communication.h"
/* USER CODE END Includes */
/* Private variables ---------------------------------------------------------*/
Firmware/Board/v3/Src/stm32f4xx_it.c
+1 -1
View File
@@ -217,7 +217,7 @@ void ADC_IRQHandler(void)
// 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
//@TODO add vbus measurement 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);
Firmware/Board/v3/Src/usbd_cdc_if.c
+1 -1
View File
@@ -54,7 +54,7 @@
#include "cmsis_os.h"
#include "freertos_vars.h"
#include "utils.h"
#include "commands.h"
#include "communication.h"
#include <freertos_vars.h>
/* USER CODE END INCLUDE */
Firmware/Board/v3/Src/usbd_desc.c
+1 -1
View File
@@ -51,7 +51,7 @@
#include "usbd_core.h"
#include "usbd_desc.h"
#include "usbd_conf.h"
#include "commands.h"
#include "communication.h"
/* USER CODE BEGIN INCLUDE */
Firmware/CHANGELOG.md
+3
View File
@@ -7,6 +7,9 @@ Please add a note of your changes below this heading if you make a Pull Request.
* `make erase_config` to erase the configuration with an STLink (the configuration can also be erased from within explore_odrive.py, using `my_odrive.erase_configuration()`)
### Changed
* 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.
* 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
Firmware/MotorControl/axis.cpp
+269 -80
View File
@@ -1,97 +1,286 @@
#include "axis.h"
#include <stdlib.h>
#include "legacy_commands.h"
#include <functional>
#include "gpio.h"
//TODO: goal of refactor is to kick this out completely
extern "C" {
#include "low_level.h"
}
#include "utils.h"
#include "odrive_main.hpp"
//TODO: Make it really clear where this is loaded.
AxisConfig axis_configs[2]; //TODO: get a constexpr for num motors
// C interface
extern "C" {
void axis_thread_entry(void const* temp_motor_ptr) {
Motor_t* motor = (Motor_t*)temp_motor_ptr;
//TODO: explicit axis number assignment
//for now we search for it
uint8_t ax_number = 0;
while (&motors[ax_number] != motor)
++ax_number;
Axis axis(axis_configs[ax_number], ax_number, motor);
axis.StateMachineLoop();
}
} // extern "C"
void Axis::SetupLegacyMappings() {
// Legacy reachability from C
legacy_motor_ref_->axis_legacy.enable_control = &enable_control_;
// override for compatibility with legacy comms paradigm
// TODO next gen comms
exposed_bools[4 * axis_number_ + 1] = &enable_control_;
exposed_bools[4 * axis_number_ + 2] = &do_calibration_;
}
Axis::Axis(AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref)
: axis_number_(axis_number),
enable_control_(config.enable_control_at_start),
do_calibration_(config.do_calibration_at_start),
Axis::Axis(const AxisHardwareConfig_t& hw_config,
AxisConfig_t& config,
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor)
: hw_config_(hw_config),
config_(config),
legacy_motor_ref_(legacy_motor_ref) {
SetupLegacyMappings();
encoder_(encoder),
sensorless_estimator_(sensorless_estimator),
controller_(controller),
motor_(motor)
{
encoder_.axis_ = this;
sensorless_estimator_.axis_ = this;
controller_.axis_ = this;
motor_.axis_ = this;
}
void Axis::StateMachineLoop() {
// @brief Sets up all components of the axis,
// such as gate driver and encoder hardware.
void Axis::setup() {
encoder_.setup();
motor_.setup();
}
static void run_state_machine_loop_wrapper(void* ctx) {
reinterpret_cast<Axis*>(ctx)->run_state_machine_loop();
}
// @brief Starts run_state_machine_loop in a new thread
void Axis::start_thread() {
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4*512);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_valid_ = true;
}
// @brief Unblocks the control loop thread.
// This is called from the current sense interrupt handler.
void Axis::signal_current_meas() {
if (thread_id_valid_)
osSignalSet(thread_id_, M_SIGNAL_PH_CURRENT_MEAS);
}
// @brief Blocks until a current measurement is completed
// @returns True on success, false otherwise
bool Axis::wait_for_current_meas() {
if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal)
return error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false;
return true;
}
static void step_cb_wrapper(void* ctx) {
reinterpret_cast<Axis*>(ctx)->step_cb();
}
// step/direction interface
void Axis::step_cb() {
if (enable_step_dir_) {
GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config_.dir_port, hw_config_.dir_pin);
float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f;
controller_.pos_setpoint_ += dir * config_.counts_per_step;
}
};
// @brief Enables or disables step/dir input
void Axis::set_step_dir_enabled(bool enable) {
if (enable) {
// Set up the direction GPIO as input
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = hw_config_.dir_pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(hw_config_.dir_port, &GPIO_InitStruct);
// Subscribe to rising edges of the step GPIO
GPIO_subscribe(hw_config_.step_port, hw_config_.step_pin, GPIO_PULLDOWN,
step_cb_wrapper, this);
enable_step_dir_ = true;
} else {
enable_step_dir_ = false;
// Unsubscribe from step GPIO
GPIO_unsubscribe(hw_config_.step_port, hw_config_.step_pin);
}
}
// @brief Returns true if the power supply is within range
bool Axis::check_PSU_brownout() {
return vbus_voltage >= config_.dc_bus_brownout_trip_level;
}
// @brief Returns true if everything is ok.
// Sets error and returns false otherwise.
bool Axis::do_checks() {
if (!motor_.do_checks())
return error_ = ERROR_MOTOR_FAILED, false;
if (!check_PSU_brownout())
return error_ = ERROR_DC_BUS_UNDER_VOLTAGE, false;
return true;
}
bool Axis::run_sensorless_spin_up() {
// Early Spin-up: spiral up current
float x = 0.0f;
run_control_loop([&](){
float phase = wrap_pm_pi(config_.ramp_up_distance * x);
float I_mag = config_.spin_up_current * x;
x += current_meas_period / config_.ramp_up_time;
if (!motor_.update(I_mag, phase))
return error_ = ERROR_MOTOR_FAILED, false;
return x < 1.0f;
});
if (error_ != ERROR_NO_ERROR)
return false;
// Late Spin-up: accelerate
float vel = config_.ramp_up_distance / config_.ramp_up_time;
float phase = wrap_pm_pi(config_.ramp_up_distance);
run_control_loop([&](){
vel += config_.spin_up_acceleration * current_meas_period;
phase = wrap_pm_pi(phase + vel * current_meas_period);
float I_mag = config_.spin_up_current;
if (!motor_.update(I_mag, phase))
return error_ = ERROR_MOTOR_FAILED, false;
return vel < config_.spin_up_target_vel;
});
return error_ == ERROR_NO_ERROR;
}
// Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from.
bool Axis::run_sensorless_control_loop() {
set_step_dir_enabled(config_.enable_step_dir);
run_control_loop([this](){
float pos_estimate, vel_estimate, phase, current_setpoint;
if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL)
return error_ = ERROR_POS_CTRL_DURING_SENSORLESS, false;
// We update the encoder just in case someone needs the output for testing
encoder_.update(nullptr, nullptr, nullptr);
if (!sensorless_estimator_.update(&pos_estimate, &vel_estimate, &phase))
return error_ = ERROR_SENSORLESS_ESTIMATOR_FAILED, false;
if (!controller_.update(pos_estimate, vel_estimate, &current_setpoint))
return error_ = ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, phase))
return error_ = ERROR_MOTOR_FAILED, false;
return true;
});
set_step_dir_enabled(false);
return error_ == ERROR_NO_ERROR;
}
bool Axis::run_closed_loop_control_loop() {
set_step_dir_enabled(config_.enable_step_dir);
run_control_loop([this](){
float pos_estimate, vel_estimate, phase, current_setpoint;
// We update the sensorless estimator just in case someone needs the output for testing
sensorless_estimator_.update(nullptr, nullptr, nullptr);
if (!encoder_.update(&pos_estimate, &vel_estimate, &phase))
return error_ = ERROR_ENCODER_FAILED, false;
if (!controller_.update(pos_estimate, vel_estimate, &current_setpoint))
return error_ = ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, phase))
return error_ = ERROR_MOTOR_FAILED, false;
return true;
});
set_step_dir_enabled(false);
return error_ == ERROR_NO_ERROR;
}
bool Axis::run_idle_loop() {
// run_control_loop ignores missed modulation timing updates
// if and only if we're in AXIS_STATE_IDLE
run_control_loop([this](){
sensorless_estimator_.update(nullptr, nullptr, nullptr);
encoder_.update(nullptr, nullptr, nullptr);
return true;
});
return error_ == ERROR_NO_ERROR;
}
// Infinite loop that does calibration and enters main control loop as appropriate
void Axis::run_state_machine_loop() {
//TODO: Move this somewhere else
// Allocate the map for anti-cogging algorithm and initialize all values to 0.0f
int encoder_cpr = legacy_motor_ref_->encoder.encoder_cpr;
legacy_motor_ref_->anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float));
if (legacy_motor_ref_->anticogging.cogging_map != NULL) {
// TODO: Move this somewhere else
// TODO: respect changes of CPR
int encoder_cpr = encoder_.config_.cpr;
controller_.anticogging_.cogging_map = (float*)malloc(encoder_cpr * sizeof(float));
if (controller_.anticogging_.cogging_map != NULL) {
for (int i = 0; i < encoder_cpr; i++) {
legacy_motor_ref_->anticogging.cogging_map[i] = 0.0f;
controller_.anticogging_.cogging_map[i] = 0.0f;
}
}
legacy_motor_ref_->motor_thread = osThreadGetId();
legacy_motor_ref_->thread_ready = true;
bool calibration_ok = false;
// arm!
motor_.arm();
for (;;) {
// Keep rotor estimation up to date while idling
osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever);
loop_updates(legacy_motor_ref_);
if (do_calibration_) {
do_calibration_ = false;
__HAL_TIM_MOE_ENABLE(legacy_motor_ref_->motor_timer); // enable pwm outputs
calibration_ok = motor_calibration(legacy_motor_ref_);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(legacy_motor_ref_->motor_timer); // disables pwm outputs
}
if (calibration_ok && enable_control_) {
legacy_motor_ref_->enable_step_dir = true;
__HAL_TIM_MOE_ENABLE(legacy_motor_ref_->motor_timer);
bool spin_up_ok = true;
if (legacy_motor_ref_->rotor_mode == ROTOR_MODE_SENSORLESS)
spin_up_ok = spin_up_sensorless(legacy_motor_ref_);
if (spin_up_ok)
control_motor_loop(legacy_motor_ref_);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(legacy_motor_ref_->motor_timer);
legacy_motor_ref_->enable_step_dir = false;
if (enable_control_) { // if control is still enabled, we exited because of error
calibration_ok = false;
enable_control_ = false;
// Load the task chain if a specific request is pending
if (requested_state_ != AXIS_STATE_UNDEFINED) {
size_t pos = 0;
if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) {
if (config_.startup_motor_calibration)
task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
if (config_.startup_encoder_calibration)
task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION;
if (config_.startup_closed_loop_control)
task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
else if (config_.startup_sensorless_control)
task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL;
task_chain_[pos++] = AXIS_STATE_IDLE;
} else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) {
task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION;
task_chain_[pos++] = AXIS_STATE_IDLE;
} else if (requested_state_ != AXIS_STATE_UNDEFINED) {
task_chain_[pos++] = requested_state_;
task_chain_[pos++] = AXIS_STATE_IDLE;
}
task_chain_[pos++] = AXIS_STATE_UNDEFINED;
// TODO: bounds checking
requested_state_ = AXIS_STATE_UNDEFINED;
}
// Note that current_state is a reference to task_chain_[0]
// Validate the state before running it
if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_)
current_state_ = AXIS_STATE_UNDEFINED;
if (current_state_ > AXIS_STATE_ENCODER_CALIBRATION && !encoder_.is_calibrated_)
current_state_ = AXIS_STATE_UNDEFINED;
// Run the specified state
// Handlers should exit if requested_state != AXIS_STATE_UNDEFINED
bool status;
switch (current_state_) {
case AXIS_STATE_MOTOR_CALIBRATION:
status = motor_.run_calibration();
break;
case AXIS_STATE_ENCODER_CALIBRATION:
status = encoder_.run_calibration();
break;
case AXIS_STATE_SENSORLESS_CONTROL:
status = run_sensorless_spin_up(); // TODO: restart if desired
if (status)
status = run_sensorless_control_loop();
break;
case AXIS_STATE_CLOSED_LOOP_CONTROL:
status = run_closed_loop_control_loop();
break;
case AXIS_STATE_IDLE:
run_idle_loop();
status = motor_.arm(); // done with idling - try to arm the motor
break;
default:
error_ = ERROR_INVALID_STATE;
status = false; // this will set the state to idle
break;
}
// If the state failed, go to idle, else advance task chain
if (!status)
current_state_ = AXIS_STATE_IDLE;
else
memcpy(task_chain_, task_chain_ + 1, sizeof(task_chain_) - sizeof(task_chain_[0]));
}
legacy_motor_ref_->thread_ready = false;
}
thread_id_valid_ = false;
}
Firmware/MotorControl/axis.h
-63
View File
@@ -1,63 +0,0 @@
#ifndef __AXIS_HPP
#define __AXIS_HPP
//TODO: goal of refactor is to kick this out completely
extern "C" {
#include "low_level.h"
}
//Outside axis:
//command handler
//callback dispatch
// TODO: decide if we want to consolidate all default configs in one file for ease of use?
struct AxisConfig {
bool enable_control_at_start = true;
bool do_calibration_at_start = true;
};
extern AxisConfig axis_configs[];
class Axis {
public:
//thread/os/system management
//timing log
//thread id
//etc.
//state machine
//control mode
//control_en/calib_ok
//error state
//motor
//current controller
//contains rotor phase logic
//motor level calibration routines
//low_level (implementation specifics)
//DRV driver
//adc callback handling
//pwm queueing
//rotor estimator
//kick out rotor phase logic
//pos/vel controller
//step/dir handler
// Object operation requires ptr to legacy object for now, TODO: get rid of this dep
Axis(AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref);
// Infinite loop that does calibration and enters main control loop as appropriate
void StateMachineLoop();
uint8_t axis_number_;
bool enable_control_;
bool do_calibration_;
AxisConfig& config_;
Motor_t* legacy_motor_ref_;
private:
void SetupLegacyMappings();
};
#endif /* __AXIS_HPP */
Firmware/MotorControl/axis.hpp
+188
View File
@@ -0,0 +1,188 @@
#ifndef __AXIS_HPP
#define __AXIS_HPP
#ifndef __ODRIVE_MAIN_HPP
#error "This file should not be included directly. Include odrive_main.hpp instead."
#endif
// Warning: Do not reorder these enum values.
// The state machine uses ">" comparision on them.
enum AxisState_t {
AXIS_STATE_UNDEFINED, //<! will fall through to idle
AXIS_STATE_IDLE, //<! disable PWM and do nothing
AXIS_STATE_STARTUP_SEQUENCE, //<! the actual sequence is defined by the config.startup_... flags
AXIS_STATE_FULL_CALIBRATION_SEQUENCE, //<! run all calibration procedures, then idle
AXIS_STATE_MOTOR_CALIBRATION, //<! run motor calibration
AXIS_STATE_SENSORLESS_CONTROL, //<! run sensorless calibration
AXIS_STATE_ENCODER_CALIBRATION, //<! run encoder calibration
AXIS_STATE_CLOSED_LOOP_CONTROL //<! run closed loop control
};
struct AxisConfig_t {
bool startup_motor_calibration = false; //<! run motor calibration at startup, skip otherwise
bool startup_encoder_calibration = false; //<! run encoder calibration after startup, skip otherwise
bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
bool enable_step_dir = true; //<! enable step/dir input after calibration
// For M0 this has no effect if enable_uart is true
float counts_per_step = 2.0f;
float dc_bus_brownout_trip_level = 8.0f; //<! [V] voltage at which the motor stops operating
// Spinup settings
float ramp_up_time = 0.4f; // [s]
float ramp_up_distance = 4 * M_PI; // [rad]
float spin_up_current = 10.0f; // [A]
float spin_up_acceleration = 400.0f; // [rad/s^2]
float spin_up_target_vel = 400.0f; // [rad/s]
};
class Axis {
public:
enum Error_t {
ERROR_NO_ERROR,
ERROR_INVALID_STATE, //<! an invalid state was requested
ERROR_DC_BUS_UNDER_VOLTAGE,
ERROR_DC_BUS_OVER_VOLTAGE,
ERROR_CURRENT_MEASUREMENT_TIMEOUT,
ERROR_CONTROL_LOOP_TIMEOUT,
ERROR_MOTOR_FAILED,
ERROR_SENSORLESS_ESTIMATOR_FAILED,
ERROR_ENCODER_FAILED,
ERROR_CONTROLLER_FAILED,
ERROR_POS_CTRL_DURING_SENSORLESS,
};
enum thread_signals {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
};
Axis(const AxisHardwareConfig_t& hw_config,
AxisConfig_t& config,
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor);
void setup();
void start_thread();
void signal_current_meas();
bool wait_for_current_meas();
void step_cb();
void set_step_dir_enabled(bool enable);
bool check_DRV_fault();
bool check_PSU_brownout();
bool do_checks();
// @brief Runs the specified update handler at the frequency of the current measurements.
//
// The loop runs until one of the following conditions:
// - update_handler returns false
// - the current measurement times out
// - the health checks fail (brownout, driver fault line)
// - update_handler doesn't update the modulation timings in time
// This criterion is ignored if current_state is AXIS_STATE_IDLE
//
// If update_handler is going to update the motor timings, you must call motor.arm()
// shortly before this function.
//
// If the function returns, it is guaranteed that error is non-zero, except if the cause
// for the exit was a negative return value of update_handler or an external
// state change request (requested_state != AXIS_STATE_DONT_CARE).
// Under all exit conditions the motor is disarmed and the brake current set to zero.
// Furthermore, if the update_handler does not set the phase voltages in time, they will
// go to zero.
//
// @tparam T Must be a callable type that takes no arguments and returns a bool
template<typename T>
void run_control_loop(const T& update_handler) {
while (requested_state_ == AXIS_STATE_UNDEFINED) {
if (motor_.error_ != Motor::ERROR_NO_ERROR) {
error_ = ERROR_MOTOR_FAILED;
break;
}
if ((current_state_ != AXIS_STATE_IDLE) && missed_control_deadline_) {
error_ = ERROR_CONTROL_LOOP_TIMEOUT;
break;
}
if (!do_checks()) // error set during function call
break;
if (!update_handler()) // error set during function call
break;
// Check we meet deadlines after queueing
++loop_counter_;
// Wait until the current measurement interrupt fires
if (!wait_for_current_meas()) { // error set by function call
motor_.disarm(); // maybe the interrupt handler is dead, let's be safe and float all phases
break;
}
}
}
bool run_sensorless_spin_up();
bool run_sensorless_control_loop();
bool run_closed_loop_control_loop();
bool run_idle_loop();
void run_state_machine_loop();
const AxisHardwareConfig_t& hw_config_;
AxisConfig_t& config_;
Encoder& encoder_;
SensorlessEstimator& sensorless_estimator_;
Controller& controller_;
Motor& motor_;
osThreadId thread_id_;
volatile bool thread_id_valid_ = false;
// variables exposed on protocol
Error_t error_ = ERROR_NO_ERROR;
bool missed_control_deadline_ = true; // this flag is raised by the interrupt handler
// whenever there's no active control loop that
// sets the timings. The flag must be explicitly
// cleared by a call to motors.arm().
bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir
AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
AxisState_t& current_state_ = task_chain_[0];
uint32_t loop_counter_ = 0;
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_ro_property("missed_control_deadline", &missed_control_deadline_),
make_protocol_property("enable_step_dir", &enable_step_dir_),
make_protocol_ro_property("current_state", &current_state_),
make_protocol_property("requested_state", &requested_state_),
make_protocol_ro_property("loop_counter", &loop_counter_),
make_protocol_object("config",
make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
make_protocol_property("startup_encoder_calibration", &config_.startup_encoder_calibration),
make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control),
make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
make_protocol_property("enable_step_dir", &config_.enable_step_dir),
make_protocol_property("counts_per_step", &config_.counts_per_step),
make_protocol_property("dc_bus_brownout_trip_level", &config_.dc_bus_brownout_trip_level),
make_protocol_property("ramp_up_time", &config_.ramp_up_time),
make_protocol_property("ramp_up_distance", &config_.ramp_up_distance),
make_protocol_property("spin_up_current", &config_.spin_up_current),
make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration),
make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel)
),
make_protocol_object("motor", motor_.make_protocol_definitions()),
make_protocol_object("controller", controller_.make_protocol_definitions()),
make_protocol_object("encoder", encoder_.make_protocol_definitions())
);
}
};
#endif /* __AXIS_HPP */
Firmware/MotorControl/board_config_v3.h
+115
View File
@@ -0,0 +1,115 @@
/*
* @brief Contains board specific configuration for ODrive v3.x
*/
#ifndef __BOARD_CONFIG_H
#define __BOARD_CONFIG_H
// STM specific includes
#include <gpio.h>
#include <spi.h>
#include <tim.h>
#include <main.h>
#if HW_VERSION_MAJOR == 3
#if HW_VERSION_MINOR <= 3
#define SHUNT_RESISTANCE (675e-6f)
#else
#define SHUNT_RESISTANCE (500e-6f)
#endif
#endif
struct AxisHardwareConfig_t {
GPIO_TypeDef* step_port;
uint16_t step_pin;
GPIO_TypeDef* dir_port;
uint16_t dir_pin;
osPriority thread_priority;
};
struct EncoderHardwareConfig_t {
TIM_HandleTypeDef* timer;
GPIO_TypeDef* index_port;
uint16_t index_pin;
};
struct MotorHardwareConfig_t {
TIM_HandleTypeDef* timer;
uint16_t control_deadline;
float shunt_conductance;
};
struct GateDriverHardwareConfig_t {
SPI_HandleTypeDef* spi;
GPIO_TypeDef* enable_port;
uint16_t enable_pin;
GPIO_TypeDef* nCS_port;
uint16_t nCS_pin;
GPIO_TypeDef* nFAULT_port;
uint16_t nFAULT_pin;
};
struct BoardHardwareConfig_t {
AxisHardwareConfig_t axis_config;
EncoderHardwareConfig_t encoder_config;
MotorHardwareConfig_t motor_config;
GateDriverHardwareConfig_t gate_driver_config;
};
const BoardHardwareConfig_t hw_configs[] = { {
.axis_config = {
.step_port = GPIO_1_GPIO_Port,
.step_pin = GPIO_1_Pin,
.dir_port = GPIO_2_GPIO_Port,
.dir_pin = GPIO_2_Pin,
.thread_priority = (osPriority)(osPriorityHigh + (osPriority)1),
},
.encoder_config = {
.timer = &htim3,
.index_port = M0_ENC_Z_GPIO_Port,
.index_pin = M0_ENC_Z_Pin,
},
.motor_config = {
.timer = &htim1,
.control_deadline = TIM_1_8_PERIOD_CLOCKS,
.shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S]
},
.gate_driver_config = {
.spi = &hspi3,
// Note: this board has the EN_Gate pin shared!
.enable_port = EN_GATE_GPIO_Port,
.enable_pin = EN_GATE_Pin,
.nCS_port = M0_nCS_GPIO_Port,
.nCS_pin = M0_nCS_Pin,
.nFAULT_port = nFAULT_GPIO_Port, // the nFAULT pin is shared between both motors
.nFAULT_pin = nFAULT_Pin,
}
},{
.axis_config = {
.step_port = GPIO_3_GPIO_Port,
.step_pin = GPIO_3_Pin,
.dir_port = GPIO_4_GPIO_Port,
.dir_pin = GPIO_4_Pin,
.thread_priority = osPriorityHigh,
},
.encoder_config = {
.timer = &htim4,
.index_port = M1_ENC_Z_GPIO_Port,
.index_pin = M1_ENC_Z_Pin,
},
.motor_config = {
.timer = &htim8,
.control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2,
.shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S]
},
.gate_driver_config = {
.spi = &hspi3,
// Note: this board has the EN_Gate pin shared!
.enable_port = EN_GATE_GPIO_Port,
.enable_pin = EN_GATE_Pin,
.nCS_port = M1_nCS_GPIO_Port,
.nCS_pin = M1_nCS_Pin,
.nFAULT_port = nFAULT_GPIO_Port, // the nFAULT pin is shared between both motors
.nFAULT_pin = nFAULT_Pin,
}
} };
#endif // __BOARD_CONFIG_H
Firmware/MotorControl/commands.cpp
-551
View File
File diff suppressed because it is too large Load Diff
Firmware/MotorControl/communication.cpp
+314
View File
@@ -0,0 +1,314 @@
/* Includes ------------------------------------------------------------------*/
// TODO: remove this option
// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py
// todo: make clean switches for protocol
#define ENABLE_LEGACY_PROTOCOL
#include "communication.h"
//#include "low_level.h"
#include "odrive_main.hpp"
#include "protocol.hpp"
#include "freertos_vars.h"
#include "utils.h"
#ifdef ENABLE_LEGACY_PROTOCOL
#include "legacy_commands.h"
#endif
#include <cmsis_os.h>
#include <memory>
#include <usbd_cdc_if.h>
#include <usb_device.h>
#include <usart.h>
#include <gpio.h>
#define UART_TX_BUFFER_SIZE 64
/* Private defines -----------------------------------------------------------*/
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
extern USBD_HandleTypeDef hUsbDeviceFS;
uint64_t serial_number;
/* Private constant data -----------------------------------------------------*/
/* 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_NATIVE)
class USBSender : public PacketSink {
public:
int process_packet(const uint8_t* buffer, size_t length) {
// cannot send partial packets
if (length > USB_TX_DATA_SIZE)
return -1;
// wait for USB interface to become ready
if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK)
return -1;
// transmit packet
uint8_t status = CDC_Transmit_FS(
const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
well... it's not actually. Stupid STM. */, length);
return (status == USBD_OK) ? 0 : -1;
}
} usb_sender;
BidirectionalPacketBasedChannel usb_channel(usb_sender);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
class USBSender : public StreamSink {
public:
int process_bytes(const uint8_t* buffer, size_t length) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
// wait for USB interface to become ready
if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK)
return -1;
// transmit chunk
if (CDC_Transmit_FS(
const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
well... it's not actually. Stupid STM. */, chunk) != USBD_OK)
return -1;
buffer += chunk;
length -= chunk;
}
return 0;
}
size_t get_free_space() { return SIZE_MAX; }
} usb_sender;
PacketToStreamConverter usb_packet_sender(usb_sender);
BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender);
StreamToPacketConverter usb_stream_sink(usb_channel);
#endif
#if defined(UART_PROTOCOL_NATIVE)
class UART4Sender : public StreamSink {
public:
int process_bytes(const uint8_t* buffer, size_t length) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data
if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
return -1;
// transmit chunk
memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
return -1;
buffer += chunk;
length -= chunk;
}
return 0;
}
size_t get_free_space() { return SIZE_MAX; }
private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart4_sender;
PacketToStreamConverter uart4_packet_sender(uart4_sender);
BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender);
StreamToPacketConverter UART4_stream_sink(uart4_channel);
#endif
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
void 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, osPriorityNormal, 0, 512);
thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL);
}
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(); }
} 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_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_object("axis0", axes[0]->make_protocol_definitions()),
make_protocol_object("axis1", axes[1]->make_protocol_definitions()),
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_type>(*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);
#if defined(UART_PROTOCOL_NATIVE)
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < last_rcv_idx) {
UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx,
UART_RX_BUFFER_SIZE - last_rcv_idx);
last_rcv_idx = 0;
}
if (new_rcv_idx > last_rcv_idx) {
UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx,
new_rcv_idx - last_rcv_idx);
last_rcv_idx = new_rcv_idx;
}
#elif defined(UART_PROTOCOL_LEGACY)
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < last_rcv_idx) {
legacy_parse_stream(dma_circ_buffer + last_rcv_idx,
UART_RX_BUFFER_SIZE - last_rcv_idx);
last_rcv_idx = 0;
}
if (new_rcv_idx > last_rcv_idx) {
legacy_parse_stream(dma_circ_buffer + last_rcv_idx,
new_rcv_idx - last_rcv_idx);
last_rcv_idx = new_rcv_idx;
}
#endif
#endif
#if !defined(USB_PROTOCOL_NONE)
// When we reach here, we are out of immediate characters to fetch out of UART buffer
// Now we check if there is any USB processing to do: we wait for up to 1 ms,
// before going back to checking UART again.
const uint32_t usb_check_timeout = 1; // ms
osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout);
if (sem_stat == osOK) {
deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
#if defined(USB_PROTOCOL_NATIVE)
usb_channel.process_packet(usb_buf, usb_len);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
usb_stream_sink.process_bytes(usb_buf, usb_len);
#elif defined(USB_PROTOCOL_LEGACY)
legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB);
#endif
USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet
}
#endif
#if 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());
}
Firmware/MotorControl/commands.h → Firmware/MotorControl/communication.h
+2 -2
View File
@@ -14,9 +14,9 @@ extern "C" {
#endif
void init_communication(void);
void communication_task(void const * argument);
void communication_task(void * ctx);
void set_cmd_buffer(uint8_t *buf, uint32_t len);
void usb_update_thread();
void usb_update_thread(void * ctx);
void USB_receive_packet(const uint8_t *buffer, size_t length);
extern uint64_t serial_number;
Firmware/MotorControl/config.h
-16
View File
@@ -1,16 +0,0 @@
#ifndef __CONFIG_H
#define __CONFIG_H
#ifdef __cplusplus
extern "C" {
#endif
void init_configuration(void);
void save_configuration(void);
void erase_configuration(void);
#ifdef __cplusplus
}
#endif
#endif /* __CONFIG_H */
Firmware/MotorControl/controller.cpp
+137
View File
@@ -0,0 +1,137 @@
#include "odrive_main.hpp"
Controller::Controller(ControllerConfig_t& config) :
config_(config)
{}
//--------------------------------
// Command Handling
//--------------------------------
void Controller::set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward) {
pos_setpoint_ = pos_setpoint;
vel_setpoint_ = vel_feed_forward;
current_setpoint_ = current_feed_forward;
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
#ifdef DEBUG_PRINT
printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", pos_setpoint, vel_setpoint_, current_setpoint_);
#endif
}
void Controller::set_vel_setpoint(float vel_setpoint, float current_feed_forward) {
vel_setpoint_ = vel_setpoint;
current_setpoint_ = current_feed_forward;
config_.control_mode = CTRL_MODE_VELOCITY_CONTROL;
#ifdef DEBUG_PRINT
printf("VELOCITY_CONTROL %3.3f %3.3f\n", vel_setpoint_, motor->current_setpoint_);
#endif
}
void Controller::set_current_setpoint(float current_setpoint) {
current_setpoint_ = current_setpoint;
config_.control_mode = CTRL_MODE_CURRENT_CONTROL;
#ifdef DEBUG_PRINT
printf("CURRENT_CONTROL %3.3f\n", current_setpoint_);
#endif
}
void Controller::start_anticogging_calibration() {
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NO_ERROR) {
anticogging_.calib_anticogging = true;
}
}
/*
* This anti-cogging implementation iterates through each encoder position,
* waits for zero velocity & position error,
* then samples the current required to maintain that position.
*
* This holding current is added as a feedforward term in the control loop.
*/
bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
if (anticogging_.calib_anticogging && anticogging_.cogging_map != NULL) {
float pos_err = anticogging_.index - pos_estimate;
if (fabsf(pos_err) <= anticogging_.calib_pos_threshold &&
fabsf(vel_estimate) < anticogging_.calib_vel_threshold) {
anticogging_.cogging_map[anticogging_.index++] = vel_integrator_current_;
}
if (anticogging_.index < axis_->encoder_.config_.cpr) { // TODO: remove the dependency on encoder CPR
set_pos_setpoint(anticogging_.index, 0.0f, 0.0f);
return false;
} else {
anticogging_.index = 0;
set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home
anticogging_.use_anticogging = true; // We're good to go, enable anti-cogging
anticogging_.calib_anticogging = false;
return true;
}
}
return false;
}
bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) {
// Only runs if anticogging_.calib_anticogging is true; non-blocking
anticogging_calibration(pos_estimate, vel_estimate);
// Position control
// TODO Decide if we want to use encoder or pll position here
float vel_des = vel_setpoint_;
if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) {
float pos_err = pos_setpoint_ - pos_estimate;
vel_des += config_.pos_gain * pos_err;
}
// Velocity limiting
float vel_lim = config_.vel_limit;
if (vel_des > vel_lim) vel_des = vel_lim;
if (vel_des < -vel_lim) vel_des = -vel_lim;
// Velocity control
float Iq = current_setpoint_;
// 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 == motor->encoder.encoder_cpr - 1)
if (anticogging_.use_anticogging) {
Iq += anticogging_.cogging_map[mod(pos_estimate, axis_->encoder_.config_.cpr)];
}
float v_err = vel_des - vel_estimate;
if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) {
Iq += config_.vel_gain * v_err;
}
// Velocity integral action before limiting
Iq += vel_integrator_current_;
// Current limiting
float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current);
bool limited = false;
if (Iq > Ilim) {
limited = true;
Iq = Ilim;
}
if (Iq < -Ilim) {
limited = true;
Iq = -Ilim;
}
// Velocity integrator (behaviour dependent on limiting)
if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL) {
// reset integral if not in use
vel_integrator_current_ = 0.0f;
} else {
if (limited) {
// TODO make decayfactor configurable
vel_integrator_current_ *= 0.99f;
} else {
vel_integrator_current_ += (config_.vel_integrator_gain * current_meas_period) * v_err;
}
}
if (current_setpoint_output) *current_setpoint_output = Iq;
return true;
}
Firmware/MotorControl/controller.hpp
+101
View File
@@ -0,0 +1,101 @@
#ifndef __CONTROLLER_HPP
#define __CONTROLLER_HPP
#ifndef __ODRIVE_MAIN_HPP
#error "This file should not be included directly. Include odrive_main.hpp instead."
#endif
// Note: these should be sorted from lowest level of control to
// highest level of control, to allow "<" style comparisons.
typedef enum {
CTRL_MODE_VOLTAGE_CONTROL = 0,
CTRL_MODE_CURRENT_CONTROL = 1,
CTRL_MODE_VELOCITY_CONTROL = 2,
CTRL_MODE_POSITION_CONTROL = 3
} Motor_control_mode_t;
struct ControllerConfig_t {
Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t
float pos_gain = 20.0f; // [(counts/s) / counts]
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
// float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] <sensorless example>
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
float vel_limit = 20000.0f; // [counts/s]
};
class Controller {
public:
Controller(ControllerConfig_t& config);
void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward);
void set_vel_setpoint(float vel_setpoint, float current_feed_forward);
void set_current_setpoint(float current_setpoint);
// TODO: make this more similar to other calibration loops
void start_anticogging_calibration();
bool anticogging_calibration(float pos_estimate, float vel_estimate);
bool update(float pos_estimate, float vel_estimate, float* current_setpoint);
ControllerConfig_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
// TODO: anticogging overhaul:
// - expose selected (all?) variables on protocol
// - make calibration user experience similar to motor & encoder calibration
// - use python tools to Fourier transform and write back the smoothed map or Fourier coefficients
// - make the calibration persistent
typedef struct {
int index;
float *cogging_map;
bool use_anticogging;
bool calib_anticogging;
float calib_pos_threshold;
float calib_vel_threshold;
} Anticogging_t;
Anticogging_t anticogging_ = {
.index = 0,
.cogging_map = nullptr,
.use_anticogging = false,
.calib_anticogging = false,
.calib_pos_threshold = 1.0f,
.calib_vel_threshold = 1.0f,
};
// variables exposed on protocol
float pos_setpoint_ = 0.0f;
float vel_setpoint_ = 0.0f;
// float vel_setpoint = 800.0f; <sensorless example>
float vel_integrator_current_ = 0.0f; // [A]
float current_setpoint_ = 0.0f; // [A]
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("pos_setpoint", &pos_setpoint_),
make_protocol_property("vel_setpoint", &vel_setpoint_),
make_protocol_property("vel_integrator_current", &vel_integrator_current_),
make_protocol_property("current_setpoint", &current_setpoint_),
make_protocol_object("config",
make_protocol_property("control_mode", &config_.control_mode),
make_protocol_property("pos_gain", &config_.pos_gain),
make_protocol_property("vel_gain", &config_.vel_gain),
make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain),
make_protocol_property("vel_limit", &config_.vel_limit)
),
make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint,
"pos_setpoint",
"vel_feed_forward",
"current_feed_forward"),
make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint,
"vel_setpoint",
"current_feed_forward"),
make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint,
"current_setpoint"),
make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration)
);
}
};
#endif // __CONTROLLER_HPP
Firmware/MotorControl/encoder.cpp
+210
View File
@@ -0,0 +1,210 @@
//#include "encoder.hpp"
#include "odrive_main.hpp"
Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
EncoderConfig_t& config) :
hw_config_(hw_config),
config_(config)
{
// Calculate encoder pll gains
// This calculation is currently identical to the PLL in SensorlessEstimator
float pll_bandwidth = 1000.0f; // [rad/s]
pll_kp_ = 2.0f * pll_bandwidth;
// Critically damped
pll_ki_ = 0.25f * (pll_kp_ * pll_kp_);
}
static void enc_index_cb_wrapper(void* ctx) {
reinterpret_cast<Encoder*>(ctx)->enc_index_cb();
}
void Encoder::setup() {
HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL,
enc_index_cb_wrapper, this);
}
//--------------------
// Hardware Dependent
//--------------------
// Triggered when an encoder passes over the "Index" pin
// TODO: only arm index edge interrupt when we know encoder has powered up
// TODO: disable interrupt once we found the index
void Encoder::enc_index_cb() {
if (!index_found_) {
set_count(0);
index_found_ = true;
}
}
// Function that sets the current encoder count to a desired 32-bit value.
void Encoder::set_count(int32_t count) {
// Disable interrupts to make a critical section to avoid race condition
uint32_t prim = __get_PRIMASK();
__disable_irq();
state_ = count;
hw_config_.timer->Instance->CNT = count;
pll_pos_ = (float)count;
__set_PRIMASK(prim);
}
// TODO: Do the scan with current, not voltage!
// TODO: add check_timing
bool Encoder::calib_enc_offset(float voltage_magnitude) {
static const float start_lock_duration = 1.0f;
static const float scan_omega = 4.0f * M_PI;
static const float scan_distance = 16.0f * M_PI;
static const int num_steps = scan_distance / scan_omega * current_meas_hz;
// go to motor zero phase for start_lock_duration to get ready to scan
int i = 0;
axis_->run_control_loop([&](){
axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f);
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
return ++i < start_lock_duration * current_meas_hz;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
return false;
int32_t init_enc_val = (int16_t)hw_config_.timer->Instance->CNT;
int64_t encvaluesum = 0;
// scan forward
i = 0;
axis_->run_control_loop([&](){
float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f);
float v_alpha = voltage_magnitude * arm_cos_f32(phase);
float v_beta = voltage_magnitude * arm_sin_f32(phase);
axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta);
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
encvaluesum += (int16_t)hw_config_.timer->Instance->CNT;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
return false;
//TODO avoid recomputing elec_rad_per_enc every time
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float expected_encoder_delta = scan_distance / elec_rad_per_enc;
float actual_encoder_delta_abs = fabsf((int16_t)hw_config_.timer->Instance->CNT-init_enc_val);
if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
{
error_ = ERROR_CPR_OUT_OF_RANGE;
return false;
}
// check direction
if ((int16_t)hw_config_.timer->Instance->CNT > init_enc_val + 8) {
// motor same dir as encoder
axis_->motor_.config_.direction = 1;
} else if ((int16_t)hw_config_.timer->Instance->CNT < init_enc_val - 8) {
// motor opposite dir as encoder
axis_->motor_.config_.direction = -1;
} else {
// Encoder response error
error_ = ERROR_RESPONSE;
return false;
}
// scan backwards
i = 0;
axis_->run_control_loop([&](){
float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f);
float v_alpha = voltage_magnitude * arm_cos_f32(phase);
float v_beta = voltage_magnitude * arm_sin_f32(phase);
axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta);
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
encvaluesum += (int16_t)hw_config_.timer->Instance->CNT;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
return false;
int offset = encvaluesum / (num_steps * 2);
config_.offset = offset;
is_calibrated_ = true;
return true;
}
bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) {
index_found_ = false;
float phase = 0.0f;
axis_->run_control_loop([&](){
phase = wrap_pm_pi(phase + omega * current_meas_period);
float v_alpha = voltage_magnitude * arm_cos_f32(phase);
float v_beta = voltage_magnitude * arm_sin_f32(phase);
axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta);
axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH);
// continue until the index is found
return !index_found_;
});
return axis_->error_ == Axis::ERROR_NO_ERROR;
}
bool Encoder::run_calibration() {
float enc_calibration_voltage;
if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
enc_calibration_voltage = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL)
enc_calibration_voltage = axis_->motor_.config_.calibration_current;
else
return false;
if (config_.use_index && !index_found_)
if (!scan_for_enc_idx(
(float)(axis_->motor_.config_.direction) * config_.idx_search_speed,
enc_calibration_voltage))
return false;
if (!config_.hand_calibrated) // TODO: discuss what logic we want here
if (!calib_enc_offset(enc_calibration_voltage))
return false;
return true;
}
bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) {
// Check that we don't get problems with discrete time approximation
if (!(current_meas_period * pll_kp_ < 1.0f)) {
error_ = ERROR_NUMERICAL;
return false;
}
// update internal encoder state
int16_t delta_enc = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)state_;
state_ += (int32_t)delta_enc;
// compute electrical phase
int corrected_enc = state_ % config_.cpr;
corrected_enc -= config_.offset;
//corrected_enc *= axis_->motor_.config_.direction; TODO: verify if this still works
//TODO avoid recomputing elec_rad_per_enc every time
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float ph = elec_rad_per_enc * (float)corrected_enc;
// ph = fmodf(ph, 2*M_PI);
phase_ = wrap_pm_pi(ph);
// run pll (for now pll is in units of encoder counts)
// TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part?
// Predict current pos
pll_pos_ += current_meas_period * pll_vel_;
// discrete phase detector
float delta_pos = (float)(state_ - (int32_t)floorf(pll_pos_));
// pll feedback
pll_pos_ += current_meas_period * pll_kp_ * delta_pos;
pll_vel_ += current_meas_period * pll_ki_ * delta_pos;
// Assign output arguments
if (pos_estimate) *pos_estimate = pll_pos_;
if (vel_estimate) *vel_estimate = pll_vel_;
if (phase_output) *phase_output = phase_;
return true;
}
Firmware/MotorControl/encoder.hpp
+78
View File
@@ -0,0 +1,78 @@
#ifndef __ENCODER_HPP
#define __ENCODER_HPP
#ifndef __ODRIVE_MAIN_HPP
#error "This file should not be included directly. Include odrive_main.hpp instead."
#endif
struct EncoderConfig_t {
bool use_index = false;
bool hand_calibrated = false;
float idx_search_speed = 10.0f; // [rad/s electrical]
int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
int32_t offset = 0;
float calib_range = 0.02;
};
class Encoder {
public:
enum Error_t {
ERROR_NONE,
ERROR_NUMERICAL,
ERROR_CPR_OUT_OF_RANGE,
ERROR_RESPONSE,
};
Encoder(const EncoderHardwareConfig_t& hw_config,
EncoderConfig_t& config);
void setup();
void enc_index_cb();
void set_count(int32_t count);
bool calib_enc_offset(float voltage_magnitude);
bool scan_for_enc_idx(float omega, float voltage_magnitude);
bool update(float* pos_estimate, float* vel_estimate, float* phase);
bool run_calibration();
const EncoderHardwareConfig_t& hw_config_;
EncoderConfig_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
Error_t error_ = ERROR_NONE;
bool index_found_ = false;
bool is_calibrated_ = config_.hand_calibrated;
int32_t state_ = 0;
float phase_ = 0.0f; // [rad]
float pll_pos_ = 0.0f; // [rad]
float pll_vel_ = 0.0f; // [rad/s]
float pll_kp_ = 0.0f; // [rad/s / rad]
float pll_ki_ = 0.0f; // [(rad/s^2) / rad]
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_ro_property("is_calibrated", &is_calibrated_),
make_protocol_ro_property("index_found", const_cast<bool*>(&index_found_)),
make_protocol_property("state", &state_),
make_protocol_property("phase", &phase_),
make_protocol_property("pll_pos", &pll_pos_),
make_protocol_property("pll_vel", &pll_vel_),
make_protocol_property("pll_kp", &pll_kp_),
make_protocol_property("pll_ki", &pll_ki_),
make_protocol_object("config",
make_protocol_property("use_index", &config_.use_index),
make_protocol_property("hand_calibrated", &config_.hand_calibrated),
make_protocol_property("idx_search_speed", &config_.idx_search_speed),
make_protocol_property("cpr", &config_.cpr),
make_protocol_property("offset", &config_.offset),
make_protocol_property("calib_range", &config_.calib_range)
)
);
}
};
#endif // __ENCODER_HPP
Firmware/MotorControl/example.json
+37
View File
@@ -0,0 +1,37 @@
[
{
"name": "",
"id": 0,
"type": "json"
},
{
"name": "subscriptions",
"id": 1,
"type": "int32[]"
},
{
"name": "motor0",
"id": 2,
"type": "tree",
"content": [
{
"name": "pos_setpoint",
"id": 3,
"type": "float",
"access": "rw"
},
{
"name": "pos_gain",
"id": 4,
"type": "float",
"access": "rw"
},
{
"name": "vel_setpoint",
"id": 5,
"type": "float",
"access": "rw"
}
]
}
]
Firmware/MotorControl/legacy_commands.c
+8 -7
View File
@@ -10,7 +10,7 @@
// recently recieved a command. In the future we may want to separate
// debug printf and the main serial comms.
SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_UART;
#if 0
/* Private constant data -----------------------------------------------------*/
// variables exposed to usb/serial interface via set/get/monitor
@@ -88,14 +88,14 @@ int* exposed_ints[] = {
};
bool* exposed_bools[] = {
&motors[0].thread_ready, // ro
&motors[0].thread_id_valid, // ro
//For now these are written by Axis::SetupLegacyMappings
NULL, // &motors[0].enable_control, // rw
NULL, // &motors[0].do_calibration, // rw
&axis[0].enable_control, // rw
&axis[0].do_calibration, // rw
NULL, // &motors[0].calibration_ok, // ro
&motors[1].thread_ready, // ro
NULL, // &motors[1].enable_control, // rw
NULL, // &motors[1].do_calibration, // rw
&motors[1].thread_id_valid, // ro
&axis[1].enable_control, // rw
&axis[1].do_calibration, // rw
NULL, // &motors[1].calibration_ok, // ro
};
@@ -289,3 +289,4 @@ static void print_monitoring(int limit) {
}
printf("\n");
}
#endif
Firmware/MotorControl/low_level.c
-1455
View File
File diff suppressed because it is too large Load Diff
Firmware/MotorControl/low_level.cpp
+271
View File
@@ -0,0 +1,271 @@
/* Includes ------------------------------------------------------------------*/
// Because of broken cmsis_os.h, we need to include arm_math first,
// otherwise chip specific defines are ommited
#include <stm32f405xx.h>
#include <stm32f4xx_hal.h> // Sets up the correct chip specifc defines required by arm_math
#define ARM_MATH_CM4
#include <arm_math.h>
#include <low_level.h>
#include <cmsis_os.h>
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <adc.h>
#include <gpio.h>
#include <main.h>
#include <spi.h>
#include <tim.h>
#include <utils.h>
#include "odrive_main.hpp"
/* Private defines -----------------------------------------------------------*/
// #define DEBUG_PRINT
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
// This value is updated by the DC-bus reading ADC.
// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late
float vbus_voltage = 12.0f;
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Function implementations --------------------------------------------------*/
void start_adc_pwm() {
// Enable ADC and interrupts
__HAL_ADC_ENABLE(&hadc1);
__HAL_ADC_ENABLE(&hadc2);
__HAL_ADC_ENABLE(&hadc3);
// Warp field stabilize.
osDelay(2);
__HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC);
__HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC);
__HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC);
__HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC);
__HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC);
// Ensure that debug halting of the core doesn't leave the motor PWM running
__HAL_DBGMCU_FREEZE_TIM1();
__HAL_DBGMCU_FREEZE_TIM8();
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);
// Motor output starts in the disabled state
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8);
// Start brake resistor PWM in floating output configuration
htim2.Instance->CCR3 = 0;
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) {
// Init PWM
int half_load = TIM_1_8_PERIOD_CLOCKS / 2;
htim->Instance->CCR1 = half_load;
htim->Instance->CCR2 = half_load;
htim->Instance->CCR3 = half_load;
// This hardware obfustication layer really is getting on my nerves
HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1);
HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2);
HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3);
HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3);
htim->Instance->CCR4 = 1;
HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4);
}
void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) {
// Store intial timer configs
uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE);
uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE);
uint16_t CR2_store = htim_a->Instance->CR2;
uint16_t SMCR_store = htim_b->Instance->SMCR;
// Turn off output
htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE);
htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE);
// Disable both timer counters
htim_a->Instance->CR1 &= ~TIM_CR1_CEN;
htim_b->Instance->CR1 &= ~TIM_CR1_CEN;
// Set first timer to send TRGO on counter enable
htim_a->Instance->CR2 &= ~TIM_CR2_MMS;
htim_a->Instance->CR2 |= TIM_TRGO_ENABLE;
// Set Trigger Source of second timer to the TRGO of the first timer
htim_b->Instance->SMCR &= ~TIM_SMCR_TS;
htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx;
// Set 2nd timer to start on trigger
htim_b->Instance->SMCR &= ~TIM_SMCR_SMS;
htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER;
// Dir bit is read only in center aligned mode, so we clear the mode for now
uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS;
uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS;
htim_a->Instance->CR1 &= ~TIM_CR1_CMS;
htim_b->Instance->CR1 &= ~TIM_CR1_CMS;
// Set both timers to up-counting state
htim_a->Instance->CR1 &= ~TIM_CR1_DIR;
htim_b->Instance->CR1 &= ~TIM_CR1_DIR;
// Restore center aligned mode
htim_a->Instance->CR1 |= CMS_store_a;
htim_b->Instance->CR1 |= CMS_store_b;
// set counter offset
htim_a->Instance->CNT = count_offset;
htim_b->Instance->CNT = 0;
// Start Timer a
htim_a->Instance->CR1 |= (TIM_CR1_CEN);
// Restore timer configs
htim_a->Instance->CR2 = CR2_store;
htim_b->Instance->SMCR = SMCR_store;
// restore output
htim_a->Instance->BDTR |= MOE_store_a;
htim_b->Instance->BDTR |= MOE_store_b;
}
// @brief Floats ALL phases immediately and sets the brake current to 0.
void disable_all_pwms(Motor::Error_t error) {
// Disable all motors NOW!
for (size_t i = 0; i < AXIS_COUNT; ++i) {
axes[i]->motor_.disarm();
axes[i]->motor_.error_ = error;
}
}
//--------------------------------
// IRQ Callbacks
//--------------------------------
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
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;
}
// 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
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)) {
disable_all_pwms(Motor::ERROR_ADC_FAILED);
return;
};
// Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion
// Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion
// If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
// If we are counting down, we just sampled in SVM vector 7, with zero current
Axis& axis = injected ? *axes[0] : *axes[1];
Axis& other_axis = injected ? *axes[1] : *axes[0];
bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR;
bool current_meas_not_DC_CAL = !counting_down;
bool update_timings = false;
if (hadc == &hadc2) {
if (&axis == axes[1] && counting_down)
update_timings = true; // update timings of M0
else if (&axis == axes[0] && !counting_down)
update_timings = true; // update timings of M1
}
// Load next timings for the motor that we're not currently sampling
if (update_timings) {
if (other_axis.motor_.next_timings_valid_ && !other_axis.missed_control_deadline_) {
other_axis.motor_.next_timings_valid_ = false;
other_axis.motor_.hw_config_.timer->Instance->CCR1 = other_axis.motor_.next_timings_[0];
other_axis.motor_.hw_config_.timer->Instance->CCR2 = other_axis.motor_.next_timings_[1];
other_axis.motor_.hw_config_.timer->Instance->CCR3 = other_axis.motor_.next_timings_[2];
__HAL_TIM_MOE_ENABLE(other_axis.motor_.hw_config_.timer); // enable pwm outputs
update_brake_current();
} else {
// the motor control loop failed to update the timings in time
// we must assume that it died and therefore float all phases
other_axis.motor_.disarm();
}
}
// Check the timing of the sequencing
if (current_meas_not_DC_CAL)
axis.motor_.log_timing(Motor::TIMING_LOG_ADC_CB_I);
else
axis.motor_.log_timing(Motor::TIMING_LOG_ADC_CB_DC);
uint32_t ADCValue;
if (injected) {
ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
} else {
ADCValue = HAL_ADC_GetValue(hadc);
}
float current = axis.motor_.phase_current_from_adcval(ADCValue);
if (current_meas_not_DC_CAL) {
// ADC2 and ADC3 record the phB and phC currents concurrently,
// and their interrupts should arrive on the same clock cycle.
// We dispatch the callbacks in order, so ADC2 will always be processed before ADC3.
// Therefore we store the value from ADC2 and signal the thread that the
// measurement is ready when we receive the ADC3 measurement
// return or continue
if (hadc == &hadc2) {
axis.motor_.current_meas_.phB = current - axis.motor_.DC_calib_.phB;
return;
} else {
axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC;
}
// Trigger axis thread
axis.signal_current_meas();
} else {
// DC_CAL measurement
if (hadc == &hadc2) {
axis.motor_.DC_calib_.phB += (current - axis.motor_.DC_calib_.phB) * calib_filter_k;
} else {
axis.motor_.DC_calib_.phC += (current - axis.motor_.DC_calib_.phC) * calib_filter_k;
}
}
}
// @brief Sums up the Ibus contribution of each motor and updates the
// brake resistor PWM accordingly.
void update_brake_current() {
float Ibus_sum = 0.0f;
for (size_t i = 0; i < AXIS_COUNT; ++i) {
Ibus_sum += axes[i]->motor_.current_control_.Ibus;
}
float brake_current = -Ibus_sum;
// Clip negative values to 0.0f
if (brake_current < 0.0f) brake_current = 0.0f;
float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage;
// Duty limit at 90% to allow bootstrap caps to charge
if (brake_duty > 0.9f) brake_duty = 0.9f;
int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty);
int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS;
if (low_off < 0) low_off = 0;
// Safe update of low and high side timings
// 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->CCR3 = low_off;
htim2.Instance->CCR4 = high_on;
}
Firmware/MotorControl/low_level.h
+2 -254
View File
@@ -8,279 +8,27 @@ extern "C" {
/* Includes ------------------------------------------------------------------*/
#include <cmsis_os.h>
#include "drv8301.h"
//default timeout waiting for phase measurement signals
#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms]
#include <stdbool.h>
#include <adc.h>
/* Exported types ------------------------------------------------------------*/
typedef enum {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
} Motor_thread_signals_t;
typedef struct {
int index;
float *cogging_map;
bool use_anticogging;
bool calib_anticogging;
float calib_pos_threshold;
float calib_vel_threshold;
} Anticogging_t;
typedef enum {
ERROR_NO_ERROR,
ERROR_PHASE_RESISTANCE_TIMING,
ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT,
ERROR_PHASE_RESISTANCE_OUT_OF_RANGE,
ERROR_PHASE_INDUCTANCE_TIMING,
ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT,
ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE,
ERROR_ENCODER_RESPONSE,
ERROR_ENCODER_MEASUREMENT_TIMEOUT,
ERROR_ADC_FAILED,
ERROR_CALIBRATION_TIMING,
ERROR_FOC_TIMING,
ERROR_FOC_MEASUREMENT_TIMEOUT,
ERROR_SCAN_MOTOR_TIMING,
ERROR_FOC_VOLTAGE_TIMING,
ERROR_GATEDRIVER_INVALID_GAIN,
ERROR_PWM_SRC_FAIL,
ERROR_UNEXPECTED_STEP_SRC,
ERROR_POS_CTRL_DURING_SENSORLESS,
ERROR_SPIN_UP_TIMEOUT,
ERROR_DRV_FAULT,
ERROR_NOT_IMPLEMENTED_MOTOR_TYPE,
ERROR_ENCODER_CPR_OUT_OF_RANGE,
ERROR_DC_BUS_BROWNOUT,
} Error_t;
// Note: these should be sorted from lowest level of control to
// highest level of control, to allow "<" style comparisons.
typedef enum {
CTRL_MODE_VOLTAGE_CONTROL = 0,
CTRL_MODE_CURRENT_CONTROL = 1,
CTRL_MODE_VELOCITY_CONTROL = 2,
CTRL_MODE_POSITION_CONTROL = 3
} Motor_control_mode_t;
typedef enum {
MOTOR_TYPE_HIGH_CURRENT = 0,
// MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented
MOTOR_TYPE_GIMBAL = 2
} Motor_type_t;
typedef struct {
float phB;
float phC;
} Iph_BC_t;
typedef struct {
float current_lim; // [A]
float p_gain; // [V/A]
float i_gain; // [V/As]
float v_current_control_integral_d; // [V]
float v_current_control_integral_q; // [V]
float Ibus; // DC bus current [A]
// Voltage applied at end of cycle:
float final_v_alpha; // [V]
float final_v_beta; // [V]
float Iq_setpoint;
float Iq_measured;
float max_allowed_current;
} Current_control_t;
typedef enum {
ROTOR_MODE_ENCODER,
ROTOR_MODE_SENSORLESS,
ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS //Run on encoder, but still run estimator for testing
} Rotor_mode_t;
typedef struct {
float phase;
float pll_pos;
float pll_vel;
float pll_kp;
float pll_ki;
float observer_gain; // [rad/s]
float flux_state[2]; // [Vs]
float V_alpha_beta_memory[2]; // [V]
float pm_flux_linkage; // [V / (rad/s)]
bool estimator_good;
float spin_up_current; // [A]
float spin_up_acceleration; // [rad/s^2]
float spin_up_target_vel; // [rad/s]
} Sensorless_t;
typedef struct {
TIM_HandleTypeDef* encoder_timer;
bool use_index;
bool index_found;
bool manually_calibrated;
float idx_search_speed;
int32_t encoder_cpr;
int32_t encoder_offset;
int32_t encoder_state;
int32_t motor_dir; // 1/-1 for fwd/rev alignment to encoder.
float encoder_calib_range;
float phase;
float pll_pos;
float pll_vel;
float pll_kp;
float pll_ki;
} Encoder_t;
typedef struct {
bool* enable_control;
} Axis_legacy_t;
#define TIMING_LOG_SIZE 16
typedef struct {
Axis_legacy_t axis_legacy;
Motor_control_mode_t control_mode;
bool enable_step_dir;
float counts_per_step;
Error_t error;
int32_t pole_pairs;
float pos_setpoint;
float pos_gain;
float vel_setpoint;
float vel_gain;
float vel_integrator_gain;
float vel_integrator_current;
float vel_limit;
float current_setpoint;
float calibration_current;
float resistance_calib_max_voltage;
float dc_bus_brownout_trip_level;
float phase_inductance;
float phase_resistance;
osThreadId motor_thread;
bool thread_ready;
// bool enable_control; // enable/disable via usb to start motor control. will be set to false again in case of errors.requires calibration_ok=true
// bool do_calibration; // trigger motor calibration. will be reset to false after self test
// bool calibration_ok;
TIM_HandleTypeDef* motor_timer;
uint16_t next_timings[3];
uint16_t control_deadline;
uint16_t last_cpu_time;
Iph_BC_t current_meas;
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;
Rotor_mode_t rotor_mode;
Encoder_t encoder;
Sensorless_t sensorless;
uint32_t loop_counter;
uint16_t timing_log[TIMING_LOG_SIZE];
// Cache for remote procedure calls arguments
struct {
float pos_setpoint;
float vel_feed_forward;
float current_feed_forward;
} set_pos_setpoint_args;
struct {
float vel_setpoint;
float current_feed_forward;
} set_vel_setpoint_args;
struct {
float current_setpoint;
} set_current_setpoint_args;
Anticogging_t anticogging;
DRV8301_FaultType_e drv_fault;
} Motor_t;
typedef enum {
TIMING_LOG_GENERAL,
TIMING_LOG_ADC_CB_M0_I,
TIMING_LOG_ADC_CB_M0_DC,
TIMING_LOG_ADC_CB_M1_I,
TIMING_LOG_ADC_CB_M1_DC,
TIMING_LOG_MEAS_R,
TIMING_LOG_MEAS_L,
TIMING_LOG_ENC_CALIB,
TIMING_LOG_IDX_SEARCH,
TIMING_LOG_FOC_VOLTAGE,
TIMING_LOG_FOC_CURRENT,
} TimingLog_t;
typedef struct{
int type;
int index;
} monitoring_slot;
/* Exported constants --------------------------------------------------------*/
extern const size_t num_motors;
extern const float elec_rad_per_enc;
/* Exported variables --------------------------------------------------------*/
extern float vbus_voltage;
extern float brake_resistance;
extern Motor_t motors[];
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
//Note: to control without feed forward, set feed forward terms to 0.0f.
void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward);
void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward);
void set_current_setpoint(Motor_t* motor, float current_setpoint);
void 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);
//// Old private:
// Utility
uint16_t check_timing(Motor_t* motor, TimingLog_t log_idx);
void global_fault(int error);
float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue);
// Initalisation
void DRV8301_setup(Motor_t* motor);
void start_adc_pwm();
void start_pwm(TIM_HandleTypeDef* htim);
void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset);
// IRQ Callbacks (are all public)
// Measurement and calibrationa
bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage);
bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high);
bool calib_enc_offset(Motor_t* motor, float voltage_magnitude);
bool 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);
// Main motor control
bool do_checks(Motor_t* motor);
bool loop_updates(Motor_t* motor);
void update_rotor(Motor_t* motor);
bool using_encoder(Motor_t* motor);
bool using_sensorless(Motor_t* motor);
float get_rotor_phase(Motor_t* motor);
float get_pll_vel(Motor_t* motor);
bool spin_up_sensorless(Motor_t* motor);
void update_brake_current();
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);
//motor thread moved to axis object
//void motor_thread(void const * argument);
#ifdef __cplusplus
}
Firmware/MotorControl/main.cpp
+101
View File
@@ -0,0 +1,101 @@
#include "odrive_main.hpp"
#include "nvm_config.hpp"
#include "communication.h"
BoardConfig_t board_config;
EncoderConfig_t encoder_configs[AXIS_COUNT];
ControllerConfig_t controller_configs[AXIS_COUNT];
MotorConfig_t motor_configs[AXIS_COUNT];
AxisConfig_t axis_configs[AXIS_COUNT];
Axis *axes[AXIS_COUNT];
typedef Config<BoardConfig_t, AxisConfig_t[AXIS_COUNT], MotorConfig_t[AXIS_COUNT]> ConfigFormat;
void save_configuration(void) {
if (ConfigFormat::safe_store_config(
&board_config,
&axis_configs,
&motor_configs)) {
//printf("saving configuration failed\r\n"); osDelay(5);
}
}
void load_configuration(void) {
if (NVM_init() ||
ConfigFormat::safe_load_config(
&board_config,
&axis_configs,
&motor_configs)) {
for (size_t i = 0; i < AXIS_COUNT; ++i) {
axis_configs[i] = AxisConfig_t();
motor_configs[i] = MotorConfig_t();
}
board_config = BoardConfig_t();
}
}
void erase_configuration(void) {
NVM_erase();
}
extern "C" {
int odrive_main(void);
void vApplicationStackOverflowHook(void) { for(;;); }
}
int odrive_main(void) {
// Load persistent configuration (or defaults)
load_configuration();
// Construct all objects.
for (size_t i = 0; i < AXIS_COUNT; ++i) {
Encoder *encoder = new Encoder(hw_configs[i].encoder_config,
encoder_configs[i]);
SensorlessEstimator *sensorless_estimator = new SensorlessEstimator();
Controller *controller = new Controller(controller_configs[i]);
Motor *motor = new Motor(hw_configs[i].motor_config,
hw_configs[i].gate_driver_config,
motor_configs[i]);
axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i],
*encoder, *sensorless_estimator, *controller, *motor);
}
// TODO: make dynamically reconfigurable
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
if (board_config.enable_uart) {
axes[0]->config_.enable_step_dir = false;
axes[0]->set_step_dir_enabled(false);
SetGPIO12toUART();
}
#endif
//osDelay(100);
// Init communications (this requires the axis objects to be constructed)
init_communication();
// Setup hardware for all components
for (size_t i = 0; i < AXIS_COUNT; ++i) {
axes[i]->setup();
}
// Start PWM and enable adc interrupts/callbacks
start_adc_pwm();
// This delay serves two purposes:
// - Let the current sense calibration converge (the current
// sense interrupts are firing in background by now)
// - Allow a user to interrupt the code, e.g. by flashing a new code,
// before it does anything crazy
// TODO make timing a function of calibration filter tau
osDelay(1500);
// Start state machine threads. Each thread will go through various calibration
// procedures and then run the actual controller loops.
// TODO: generalize for AXIS_COUNT != 2
for (size_t i = 0; i < AXIS_COUNT; ++i) {
axes[i]->start_thread();
}
return 0;
}
Firmware/MotorControl/motor.cpp
+358
View File
@@ -0,0 +1,358 @@
#include <algorithm>
#include "drv8301.h"
//#include "motor.hpp"
#include "odrive_main.hpp"
Motor::Motor(const MotorHardwareConfig_t& hw_config,
const GateDriverHardwareConfig_t& gate_driver_config,
MotorConfig_t& config) :
hw_config_(hw_config),
gate_driver_config_(gate_driver_config),
config_(config),
gate_driver_({
.spiHandle = gate_driver_config_.spi,
.EngpioHandle = gate_driver_config_.enable_port,
.EngpioNumber = gate_driver_config_.enable_pin,
.nCSgpioHandle = gate_driver_config_.nCS_port,
.nCSgpioNumber = gate_driver_config_.nCS_pin,
})
{
}
// @brief Arms the PWM outputs that belong to this motor.
//
// Note that this does not yet activate the PWM outputs, it just unlocks them.
//
// While the motor is armed, the control loop must set new modulation timings
// between any two interrupts (that is, enqueue_modulation_timings must be executed).
// If the control loop fails to do so, the next interrupt handler floats the
// phases. Once this happens, missed_control_deadline is set to true and
// the motor can be considered disarmed.
//
// @returns: True on success, false otherwise
bool Motor::arm() {
// Wait until the interrupt handler triggers twice. After the first wait there is an
// undefined period until the next trigger. After the second wait we know for sure
// that we have exactly one full interrupt period until the third trigger. This gives
// the control loop the correct time quota to set up modulation timings.
if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas()))
return false;
next_timings_valid_ = false;
axis_->missed_control_deadline_ = false;
return true;
}
// @brief Floats the phases of this motor immediately and updates
// the brake current accordingly.
void Motor::disarm() {
// disable pwm
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config_.timer);
// set this motor's contribution to 0
current_control_.Ibus = 0.0f;
update_brake_current();
// ensure the PWM is not re-enabled without the state machine explicitly
// calling motor.arm()
axis_->missed_control_deadline_ = true;
}
// @brief Set up the gate drivers
void Motor::DRV8301_setup() {
DRV_SPI_8301_Vars_t* local_regs = &gate_driver_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
// 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:
phase_current_rev_gain_ = 1.0f / 10.0f;
break;
case DRV8301_ShuntAmpGain_20VpV:
phase_current_rev_gain_ = 1.0f / 20.0f;
break;
case DRV8301_ShuntAmpGain_40VpV:
phase_current_rev_gain_ = 1.0f / 40.0f;
break;
case DRV8301_ShuntAmpGain_80VpV:
phase_current_rev_gain_ = 1.0f / 80.0f;
break;
}
float margin = 0.90f;
float max_input = margin * 0.3f * hw_config_.shunt_conductance;
float max_swing = margin * 1.6f * hw_config_.shunt_conductance * phase_current_rev_gain_;
current_control_.max_allowed_current = std::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);
}
// @brief Checks if the gate driver is in operational state.
// @returns: true if the gate driver is OK (no fault), false otherwise
bool Motor::check_DRV_fault() {
//TODO: make this pin configurable per motor ch
GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config_.nFAULT_port, gate_driver_config_.nFAULT_pin);
if (nFAULT_state == GPIO_PIN_RESET) {
// Update DRV Fault Code
drv_fault_ = DRV8301_getFaultType(&gate_driver_);
// Update/Cache all SPI device registers
DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
local_regs->RcvCmd = true;
DRV8301_readData(&gate_driver_, local_regs);
return false;
};
return true;
}
bool Motor::do_checks() {
if (!check_DRV_fault()) {
error_ = ERROR_DRV_FAULT;
return false;
}
return true;
}
void Motor::log_timing(TimingLog_t log_idx) {
TIM_HandleTypeDef* htim = hw_config_.timer;
uint16_t timing = htim->Instance->CNT;
bool down = htim->Instance->CR1 & TIM_CR1_DIR;
if (down) {
uint16_t delta = TIM_1_8_PERIOD_CLOCKS - timing;
timing = TIM_1_8_PERIOD_CLOCKS + delta;
}
if (log_idx < TIMING_LOG_NUM_SLOTS) {
timing_log_[log_idx] = timing;
}
}
float Motor::phase_current_from_adcval(uint32_t ADCValue) {
int adcval_bal = (int)ADCValue - (1 << 11);
float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal;
float shunt_volt = amp_out_volt * phase_current_rev_gain_;
float current = shunt_volt * hw_config_.shunt_conductance;
return current;
}
//--------------------------------
// Measurement and calibration
//--------------------------------
// TODO check Ibeta balance to verify good motor connection
bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
static const float kI = 10.0f; // [(V/s)/A]
static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s
float test_voltage = 0.0f;
size_t i = 0;
axis_->run_control_loop([&](){
float Ialpha = -(current_meas_.phB + current_meas_.phC);
test_voltage += (kI * current_meas_period) * (test_current - Ialpha);
if (test_voltage > max_voltage || test_voltage < -max_voltage)
return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false;
// Test voltage along phase A
enqueue_voltage_timings(test_voltage, 0.0f);
log_timing(TIMING_LOG_MEAS_R);
return ++i < num_test_cycles;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
return false;
//// De-energize motor
//enqueue_voltage_timings(motor, 0.0f, 0.0f);
float R = test_voltage / test_current;
config_.phase_resistance = R;
return true; // if we ran to completion that means success
}
bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
float test_voltages[2] = {voltage_low, voltage_high};
float Ialphas[2] = {0.0f};
static const int num_cycles = 5000;
size_t t = 0;
axis_->run_control_loop([&](){
int i = t & 1;
Ialphas[i] += -current_meas_.phB - current_meas_.phC;
// Test voltage along phase A
enqueue_voltage_timings(test_voltages[i], 0.0f);
log_timing(TIMING_LOG_MEAS_L);
return ++t < (num_cycles << 1);
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
return false;
//// De-energize motor
//enqueue_voltage_timings(motor, 0.0f, 0.0f);
float v_L = 0.5f * (voltage_high - voltage_low);
// Note: A more correct formula would also take into account that there is a finite timestep.
// 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;
config_.phase_inductance = L;
// TODO arbitrary values set for now
if (L < 1e-6f || L > 500e-6f)
return error_ = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false;
return true;
}
bool Motor::run_calibration() {
error_ = ERROR_NO_ERROR;
float R_calib_max_voltage = config_.resistance_calib_max_voltage;
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage))
return false;
if (!measure_phase_inductance(-R_calib_max_voltage, R_calib_max_voltage))
return false;
} else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
// no calibration needed
} else {
return false;
}
// Calculate current control gains
float current_control_bandwidth = 1000.0f; // [rad/s]
current_control_.p_gain = current_control_bandwidth * config_.phase_inductance;
float plant_pole = config_.phase_resistance / config_.phase_inductance;
current_control_.i_gain = plant_pole * current_control_.p_gain;
is_calibrated_ = true;
return true;
}
void Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
float tA, tB, tC;
SVM(mod_alpha, mod_beta, &tA, &tB, &tC);
next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_valid_ = true;
}
void Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage);
float mod_alpha = vfactor * v_alpha;
float mod_beta = vfactor * v_beta;
enqueue_modulation_timings(mod_alpha, mod_beta);
log_timing(TIMING_LOG_FOC_VOLTAGE);
}
// TODO: This doesn't update brake current
// We should probably make FOC Current call FOC Voltage to avoid duplication.
bool Motor::FOC_voltage(float v_d, float v_q, float phase) {
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;
enqueue_voltage_timings(v_alpha, v_beta);
return true;
}
bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
Current_control_t* ictrl = &current_control_;
// For Reporting
ictrl->Iq_setpoint = Iq_des;
// Clarke transform
float Ialpha = -current_meas_.phB - current_meas_.phC;
float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC);
// Park transform
float c = arm_cos_f32(phase);
float s = arm_sin_f32(phase);
float Id = c * Ialpha + s * Ibeta;
float Iq = c * Ibeta - s * Ialpha;
ictrl->Iq_measured = Iq;
// Current error
float Ierr_d = Id_des - Id;
float Ierr_q = Iq_des - Iq;
// TODO look into feed forward terms (esp omega, since PI pole maps to RL tau)
// Apply PI control
float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain;
float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain;
float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
float V_to_mod = 1.0f / mod_to_V;
float mod_d = V_to_mod * Vd;
float mod_q = V_to_mod * Vq;
// 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) {
mod_d *= mod_scalefactor;
mod_q *= mod_scalefactor;
// TODO make decayfactor configurable
ictrl->v_current_control_integral_d *= 0.99f;
ictrl->v_current_control_integral_q *= 0.99f;
} else {
ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period);
ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period);
}
// Compute estimated bus current
ictrl->Ibus = mod_d * Id + mod_q * Iq;
// Inverse park transform
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;
ictrl->final_v_beta = mod_to_V * mod_beta;
// Apply SVM
enqueue_modulation_timings(mod_alpha, mod_beta);
log_timing(TIMING_LOG_FOC_CURRENT);
return true;
}
bool Motor::update(float current_setpoint, float phase) {
current_setpoint *= config_.direction;
phase *= config_.direction;
// Execute current command
// TODO: move this into the mot
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
if(!FOC_current(0.0f, current_setpoint, phase)){
return false;
}
} else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
//In gimbal motor mode, current is reinterptreted as voltage.
if(!FOC_voltage(0.0f, current_setpoint, phase))
return false;
} else {
error_ = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE;
return false;
}
return true;
}
Firmware/MotorControl/motor.hpp
+196
View File
@@ -0,0 +1,196 @@
#ifndef __MOTOR_HPP
#define __MOTOR_HPP
#ifndef __ODRIVE_MAIN_HPP
#error "This file should not be included directly. Include odrive_main.hpp instead."
#endif
#include "drv8301.h"
typedef enum {
MOTOR_TYPE_HIGH_CURRENT = 0,
// MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented
MOTOR_TYPE_GIMBAL = 2
} Motor_type_t;
typedef struct {
float phB;
float phC;
} Iph_BC_t;
typedef struct {
float p_gain; // [V/A]
float i_gain; // [V/As]
float v_current_control_integral_d; // [V]
float v_current_control_integral_q; // [V]
float Ibus; // DC bus current [A]
// Voltage applied at end of cycle:
float final_v_alpha; // [V]
float final_v_beta; // [V]
float Iq_setpoint;
float Iq_measured;
float max_allowed_current;
} Current_control_t;
// 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.
typedef struct {
bool hand_calibrated = false; // can be set to true to indicate that all values here are valid
int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series.
float calibration_current = 10.0f; // [A]
float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
float phase_inductance = 0.0f; // to be set by measure_phase_inductance
float phase_resistance = 0.0f; // to be set by measure_phase_resistance
int32_t direction = 1; // 1 or -1
Motor_type_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
// Read out max_allowed_current to see max supported value for current_lim.
// You can change DRV8301_ShuntAmpGain to get a different range.
// float current_lim = 75.0f; //[A]
float current_lim = 10.0f; //[A]
} MotorConfig_t;
class Motor {
public:
enum Error_t {
ERROR_NO_ERROR,
ERROR_PHASE_RESISTANCE_OUT_OF_RANGE,
ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE,
ERROR_ADC_FAILED,
ERROR_DRV_FAULT,
ERROR_NOT_IMPLEMENTED_MOTOR_TYPE,
};
enum TimingLog_t {
TIMING_LOG_GENERAL,
TIMING_LOG_ADC_CB_I,
TIMING_LOG_ADC_CB_DC,
TIMING_LOG_MEAS_R,
TIMING_LOG_MEAS_L,
TIMING_LOG_ENC_CALIB,
TIMING_LOG_IDX_SEARCH,
TIMING_LOG_FOC_VOLTAGE,
TIMING_LOG_FOC_CURRENT,
TIMING_LOG_NUM_SLOTS
};
Motor(const MotorHardwareConfig_t& hw_config,
const GateDriverHardwareConfig_t& gate_driver_config,
MotorConfig_t& config);
bool arm();
void disarm();
void setup() {
DRV8301_setup();
}
void DRV8301_setup();
bool check_DRV_fault();
bool do_checks();
void log_timing(TimingLog_t log_idx);
float phase_current_from_adcval(uint32_t ADCValue);
bool measure_phase_resistance(float test_current, float max_voltage);
bool measure_phase_inductance(float voltage_low, float voltage_high);
bool run_calibration();
void enqueue_modulation_timings(float mod_alpha, float mod_beta);
void enqueue_voltage_timings(float v_alpha, float v_beta);
bool FOC_voltage(float v_d, float v_q, float phase);
bool FOC_current(float Id_des, float Iq_des, float phase);
bool update(float current_setpoint, float phase);
const MotorHardwareConfig_t& hw_config_;
const GateDriverHardwareConfig_t gate_driver_config_;
MotorConfig_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
//private:
DRV8301_Obj gate_driver_; // initialized in constructor
uint16_t next_timings_[3] = {
TIM_1_8_PERIOD_CLOCKS / 2,
TIM_1_8_PERIOD_CLOCKS / 2,
TIM_1_8_PERIOD_CLOCKS / 2
};
bool next_timings_valid_ = false;
uint16_t last_cpu_time_ = 0;
int timing_log_index_ = 0;
uint16_t timing_log_[TIMING_LOG_NUM_SLOTS] = { 0 };
// variables exposed on protocol
Error_t error_ = ERROR_NO_ERROR;
bool is_calibrated_ = config_.hand_calibrated;
Iph_BC_t current_meas_ = {0.0f, 0.0f};
Iph_BC_t DC_calib_ = {0.0f, 0.0f};
const float shunt_conductance_ = 1.0f / SHUNT_RESISTANCE; //[S]
float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup)
Current_control_t current_control_ = {
.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_setpoint = 0.0f,
.Iq_measured = 0.0f,
.max_allowed_current = 0.0f,
};
DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault;
DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup)
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_ro_property("is_calibrated", &is_calibrated_),
make_protocol_ro_property("current_meas_phB", &current_meas_.phB),
make_protocol_ro_property("current_meas_phC", &current_meas_.phC),
make_protocol_property("DC_calib_phB", &DC_calib_.phB),
make_protocol_property("DC_calib_phC", &DC_calib_.phC),
make_protocol_property("shunt_conductance", &shunt_conductance_),
make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_),
make_protocol_object("current_control",
make_protocol_property("p_gain", &current_control_.p_gain),
make_protocol_property("i_gain", &current_control_.i_gain),
make_protocol_property("v_current_control_integral_d", &current_control_.v_current_control_integral_d),
make_protocol_property("v_current_control_integral_q", &current_control_.v_current_control_integral_q),
make_protocol_property("Ibus", &current_control_.Ibus),
make_protocol_property("final_v_alpha", &current_control_.final_v_alpha),
make_protocol_property("final_v_beta", &current_control_.final_v_beta),
make_protocol_property("Iq_setpoint", &current_control_.Iq_setpoint),
make_protocol_property("Iq_measured", &current_control_.Iq_measured),
make_protocol_property("max_allowed_current", &current_control_.max_allowed_current)
),
make_protocol_object("gate_driver",
make_protocol_ro_property("drv_fault", &drv_fault_),
make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value),
make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value),
make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value),
make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value)
),
make_protocol_object("timing_log",
make_protocol_ro_property("TIMING_LOG_GENERAL", &timing_log_[TIMING_LOG_GENERAL]),
make_protocol_ro_property("TIMING_LOG_ADC_CB_I", &timing_log_[TIMING_LOG_ADC_CB_I]),
make_protocol_ro_property("TIMING_LOG_ADC_CB_DC", &timing_log_[TIMING_LOG_ADC_CB_DC]),
make_protocol_ro_property("TIMING_LOG_MEAS_R", &timing_log_[TIMING_LOG_MEAS_R]),
make_protocol_ro_property("TIMING_LOG_MEAS_L", &timing_log_[TIMING_LOG_MEAS_L]),
make_protocol_ro_property("TIMING_LOG_ENC_CALIB", &timing_log_[TIMING_LOG_ENC_CALIB]),
make_protocol_ro_property("TIMING_LOG_IDX_SEARCH", &timing_log_[TIMING_LOG_IDX_SEARCH]),
make_protocol_ro_property("TIMING_LOG_FOC_VOLTAGE", &timing_log_[TIMING_LOG_FOC_VOLTAGE]),
make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT])
),
make_protocol_object("config",
make_protocol_property("pole_pairs", &config_.pole_pairs),
make_protocol_property("calibration_current", &config_.calibration_current),
make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage),
make_protocol_property("phase_inductance", &config_.phase_inductance),
make_protocol_property("phase_resistance", &config_.phase_resistance),
make_protocol_property("direction", &config_.direction),
make_protocol_property("motor_type", &config_.motor_type),
make_protocol_property("current_lim", &config_.current_lim)
)
);
}
};
#endif // __MOTOR_HPP
Firmware/MotorControl/config.cpp → Firmware/MotorControl/nvm_config.hpp
+17 -129
View File
@@ -1,57 +1,38 @@
/*
* Convenience functions to load and store multiple objects from and to NVM.
*
* The NVM stores consecutive one-to-one copies of arbitrary objects.
* The types of these objects are passed as template arguments to Config<Ts...>.
*/
/* Includes ------------------------------------------------------------------*/
#include "config.h"
#include <stdint.h>
#include <stdlib.h>
#include <stm32f405xx.h>
#include "nvm.h"
#include "crc.hpp"
#include "low_level.h"
#include "axis.h"
// IMPORTANT: if you change, reorder or otherwise modify any of the fields in
// the config structs, make sure to increment this number:
uint16_t config_version = 0x0001;
/* Private defines -----------------------------------------------------------*/
#define CRC16_INIT 0xabcd
#define CONFIG_CRC16_INIT 0xabcd
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
typedef struct {
Motor_control_mode_t control_mode;
float counts_per_step;
int32_t pole_pairs;
float pos_gain;
float vel_gain;
float vel_integrator_gain;
float vel_limit;
float calibration_current;
float resistance_calib_max_voltage;
float phase_inductance;
float phase_resistance;
Motor_type_t motor_type;
Rotor_mode_t rotor_mode;
float current_control_current_lim;
bool encoder_use_index;
bool encoder_manually_calibrated;
float encoder_idx_search_speed;
int32_t encoder_cpr;
int32_t encoder_offset;
int32_t encoder_motor_dir;
} MotorConfig_t;
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
// IMPORTANT: if you change, reorder or otherwise modify any of the fields in
// the config structs, make sure to increment this number:
static constexpr uint16_t config_version = 0x0001;
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
// @brief Manages configuration load and store operations from and to NVM
//
// The NVM stores consecutive one-to-one copies of arbitrary objects.
@@ -121,11 +102,11 @@ struct Config<T, Ts...> {
// @brief Loads one or more consecutive objects from the NVM. The loaded data
// is validated using a CRC value that is stored at the beginning of the data.
static int load_config(T* val0, Ts* ... vals) {
static int safe_load_config(T* val0, Ts* ... vals) {
//printf("have %d bytes\r\n", NVM_get_max_read_length()); osDelay(5);
if (Config<T, Ts..., uint16_t>::get_size() > NVM_get_max_read_length())
return -1;
uint16_t crc16 = CRC16_INIT ^ config_version;
uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version;
if (Config<T, Ts..., uint16_t>::load_config(0, &crc16, val0, vals..., &crc16))
return -1;
if (crc16)
@@ -140,14 +121,14 @@ struct Config<T, Ts...> {
// changes of the config structs during firmware update. Note that if the total
// config data length changes, the CRC validation will fail even if the developer
// forgets to update the config version number.
static int store_config(const T* val0, const Ts* ... vals) {
static int safe_store_config(const T* val0, const Ts* ... vals) {
size_t size = Config<T, Ts...>::get_size() + 2;
//printf("config is %d bytes\r\n", size); osDelay(5);
if (size > NVM_get_max_write_length())
return -1;
if (NVM_start_write(size))
return -1;
uint16_t crc16 = CRC16_INIT ^ config_version;
uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version;
if (Config<T, Ts...>::store_config(0, &crc16, val0, vals...))
return -1;
if (Config<uint8_t, uint8_t>::store_config(size - 2, nullptr, (uint8_t *)&crc16 + 1, (uint8_t *)&crc16))
@@ -157,96 +138,3 @@ struct Config<T, Ts...> {
return 0;
}
};
// This function is obviously stupid and should go away (make MotorConfig_t a member of Motor_t)
// TODO: make this go away as part of the C++ refactoring
void set_motor_config(const MotorConfig_t* config, Motor_t* motor) {
motor->control_mode = config->control_mode;
motor->counts_per_step = config->counts_per_step;
motor->pole_pairs = config->pole_pairs;
motor->pos_gain = config->pos_gain;
motor->vel_gain = config->vel_gain;
motor->vel_integrator_gain = config->vel_integrator_gain;
motor->vel_limit = config->vel_limit;
motor->calibration_current = config->calibration_current;
motor->resistance_calib_max_voltage = config->resistance_calib_max_voltage;
motor->phase_inductance = config->phase_inductance;
motor->phase_resistance = config->phase_resistance;
motor->motor_type = config->motor_type;
motor->rotor_mode = config->rotor_mode;
motor->current_control.current_lim = config->current_control_current_lim;
motor->encoder.use_index = config->encoder_use_index;
motor->encoder.manually_calibrated = config->encoder_manually_calibrated;
motor->encoder.idx_search_speed = config->encoder_idx_search_speed;
motor->encoder.encoder_cpr = config->encoder_cpr;
motor->encoder.encoder_offset = config->encoder_offset;
motor->encoder.motor_dir = config->encoder_motor_dir;
}
// This function is obviously stupid and should go away (make MotorConfig_t a member of Motor_t)
// TODO: make this go away as part of the C++ refactoring
void get_motor_config(const Motor_t* motor, MotorConfig_t* config) {
config->control_mode = motor->control_mode;
config->counts_per_step = motor->counts_per_step;
config->pole_pairs = motor->pole_pairs;
config->pos_gain = motor->pos_gain;
config->vel_gain = motor->vel_gain;
config->vel_integrator_gain = motor->vel_integrator_gain;
config->vel_limit = motor->vel_limit;
config->calibration_current = motor->calibration_current;
config->resistance_calib_max_voltage = motor->resistance_calib_max_voltage;
config->phase_inductance = motor->phase_inductance;
config->phase_resistance = motor->phase_resistance;
config->motor_type = motor->motor_type;
config->rotor_mode = motor->rotor_mode;
config->current_control_current_lim = motor->current_control.current_lim;
config->encoder_use_index = motor->encoder.use_index;
config->encoder_manually_calibrated = motor->encoder.manually_calibrated;
config->encoder_idx_search_speed = motor->encoder.idx_search_speed;
config->encoder_cpr = motor->encoder.encoder_cpr;
config->encoder_offset = motor->encoder.encoder_offset;
config->encoder_motor_dir = motor->encoder.motor_dir;
}
void init_configuration(void) {
MotorConfig_t motor_config[2];
//TODO: we really shouldn't be hardcoding like this
if (NVM_init() || Config<MotorConfig_t, MotorConfig_t, AxisConfig, AxisConfig, float>::load_config(&motor_config[0], &motor_config[1], &axis_configs[0], &axis_configs[1], &brake_resistance)) {
//printf("no config found\r\n"); osDelay(5);
// load default config
// motor_config[0] = MotorConfig_t();
// motor_config[1] = MotorConfig_t();
// TODO: temporary hack, this is gonna change after refactoring
axis_configs[0] = AxisConfig();
axis_configs[1] = AxisConfig();
brake_resistance = 0.47f;
// Default config coming from flashed Motor_t
return;
} else {
//printf("load config successful\r\n"); osDelay(5);
}
set_motor_config(&motor_config[0], &motors[0]);
set_motor_config(&motor_config[1], &motors[1]);
}
void save_configuration(void) {
MotorConfig_t motor_config[2];
get_motor_config(&motors[0], &motor_config[0]);
get_motor_config(&motors[1], &motor_config[1]);
//TODO: we really shouldn't be hardcoding like this
if (Config<MotorConfig_t, MotorConfig_t, AxisConfig, AxisConfig, float>::store_config(&motor_config[0], &motor_config[1], &axis_configs[0], &axis_configs[1], &brake_resistance)) {
//printf("saving configuration failed\r\n"); osDelay(5);
}
}
void erase_configuration(void) {
NVM_erase();
}
Firmware/MotorControl/odrive_main.hpp
+57
View File
@@ -0,0 +1,57 @@
#ifndef __ODRIVE_MAIN_HPP
#define __ODRIVE_MAIN_HPP
// stdlib includes
#include <functional>
// System includes
#include <cmsis_os.h>
// STM specific includes
#include <stm32f4xx_hal.h> // Sets up the correct chip specifc defines required by arm_math
#define ARM_MATH_CM4 // TODO: might change in future board versions
#include <arm_math.h>
// Hardware configuration
#if HW_VERSION_MAJOR == 3
#include <board_config_v3.h>
#else
#error "unknown board version"
#endif
// @brief general user configurable board configuration
struct BoardConfig_t {
bool enable_uart = true;
float brake_resistance = 0.47f; // [ohm]
};
class Axis;
//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 const float elec_rad_per_enc;
extern BoardConfig_t board_config;
constexpr size_t AXIS_COUNT = 2;
extern Axis *axes[AXIS_COUNT];
// ODrive specific includes
#include <protocol.hpp>
#include <utils.h>
#include <low_level.h>
#include <encoder.hpp>
#include <sensorless_estimator.hpp>
#include <controller.hpp>
#include <motor.hpp>
#include <axis.hpp>
// defined in main.cpp
void save_configuration(void);
void erase_configuration(void);
#endif /* __ODRIVE_MAIN_HPP */
Firmware/MotorControl/protocol.cpp
+62 -69
View File
@@ -8,16 +8,7 @@
#include <stdlib.h>
/* Private defines -----------------------------------------------------------*/
// Note that this option cannot be used to debug UART because it prints on UART
//#define DEGUG_PROTOCOL
/* Private macros ------------------------------------------------------------*/
#ifdef DEGUG_PROTOCOL
#define LOG_PROTO(...) do { printf(__VA_ARGS__); osDelay(10); } while (0)
#else
#define LOG_PROTO(...) ((void) 0)
#endif
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
@@ -46,48 +37,6 @@ void hexdump(const uint8_t* buf, size_t len) {
}
#endif
static inline int write_string(const char* str, StreamSink* output) {
return output->process_bytes(reinterpret_cast<const uint8_t*>(str), strlen(str));
}
void Endpoint::write_json(size_t id, bool* need_comma, StreamSink* output) const {
if (type_ == CLOSE_TREE) {
write_string("]}", output);
*need_comma = true;
} else {
if (*need_comma)
write_string(",", output);
// write name
write_string("{\"name\":\"", output);
if (name_)
write_string(name_, output);
// write endpoint ID
write_string("\",\"id\":", output);
char id_buf[10];
snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf
write_string(id_buf, output);
// write additional JSON data
if (json_modifier_ && json_modifier_[0]) {
write_string(",", output);
write_string(json_modifier_, output);
}
if (type_ == BEGIN_OBJECT) {
write_string(",\"members\":[", output);
*need_comma = false;
} else if (type_ == BEGIN_FUNCTION) {
write_string(",\"arguments\":[", output);
*need_comma = false;
} else if (type_ == PROPERTY) {
write_string("}", output);
*need_comma = true;
}
}
}
int StreamToPacketConverter::process_bytes(const uint8_t *buffer, size_t length) {
@@ -157,36 +106,75 @@ int PacketToStreamConverter::process_packet(const uint8_t *buffer, size_t length
}
// Calculates the CRC16 of the JSON interface descriptor.
// The init value is the protocol version.
uint16_t BidirectionalPacketBasedChannel::calculate_json_crc16(void) {
CRC16Calculator crc16_calculator(PROTOCOL_VERSION);
class JSONDescriptorEndpoint : Endpoint {
public:
static constexpr size_t endpoint_count = 1;
void write_json(size_t id, StreamSink* output);
void register_endpoints(Endpoint** list, size_t id, size_t length);
void handle(const uint8_t* input, size_t input_length, StreamSink* output);
};
uint8_t offset[4] = { 0 };
interface_query(offset, sizeof(offset), &crc16_calculator);
JSONDescriptorEndpoint json_file_endpoint = JSONDescriptorEndpoint();
EndpointProvider* application_endpoints;
uint16_t json_crc_;
return crc16_calculator.get_crc16();
void JSONDescriptorEndpoint::write_json(size_t id, StreamSink* output) {
write_string("{\"name\":\"\",", output);
// write endpoint ID
write_string("\"id\":", output);
char id_buf[10];
snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf
write_string(id_buf, output);
write_string(",\"type\":\"json\",\"access\":\"r\"}", output);
}
void JSONDescriptorEndpoint::register_endpoints(Endpoint** list, size_t id, size_t length) {
if (id < length)
list[id] = this;
};
// Returns part of the JSON interface definition.
void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size_t input_length, StreamSink* output) {
void JSONDescriptorEndpoint::handle(const uint8_t* input, size_t input_length, StreamSink* output) {
// The request must contain a 32 bit integer to specify an offset
if (input_length < 4)
return;
uint32_t offset = 0;
read_le<uint32_t>(&offset, input);
NullStreamSink output_with_offset = NullStreamSink(offset, *output);
bool need_comma = false;
size_t id = 0;
write_string("[", &output_with_offset);
for (size_t i = 0; i < n_endpoints_; ++i) {
get_endpoint(i)->write_json(i, &need_comma, &output_with_offset);
if (!output->get_free_space())
return; // return early if the output cannot take more bytes
}
json_file_endpoint.write_json(id, &output_with_offset);
id += decltype(json_file_endpoint)::endpoint_count;
write_string(",", &output_with_offset);
application_endpoints->write_json(id, &output_with_offset);
write_string("]", &output_with_offset);
}
void set_application_endpoints(EndpointProvider* endpoints) {
application_endpoints = endpoints;
n_endpoints_ = 0;
json_file_endpoint.register_endpoints(endpoints_, 0, max_endpoints_);
n_endpoints_ += decltype(json_file_endpoint)::endpoint_count;
application_endpoints->register_endpoints(endpoints_, n_endpoints_, max_endpoints_);
n_endpoints_ += application_endpoints->get_endpoint_count();
// Calculates the CRC16 of the JSON file.
// The init value is the protocol version.
CRC16Calculator crc16_calculator(PROTOCOL_VERSION);
uint8_t offset[4] = { 0 };
json_file_endpoint.handle(offset, sizeof(offset), &crc16_calculator);
json_crc_ = crc16_calculator.get_crc16();
CRC16Calculator crc16_calculator2(PROTOCOL_VERSION);
endpoints_[0]->handle(offset, sizeof(offset), &crc16_calculator2);
json_crc_ = crc16_calculator2.get_crc16();
}
int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_t length) {
LOG_PROTO("got packet of length %d: \r\n", length);
hexdump(buffer, length);
@@ -205,10 +193,15 @@ int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_
bool expect_response = endpoint_id & 0x8000;
endpoint_id &= 0x7fff;
const Endpoint* endpoint = get_endpoint(endpoint_id);
if (!endpoint)
if (endpoint_id >= n_endpoints_)
return -1;
Endpoint* endpoint = endpoints_[endpoint_id];
if (!endpoint) {
LOG_PROTO("critical: no endpoint at %d", endpoint_id);
return -1;
}
// Verify packet trailer. The expected trailer value depends on the selected endpoint.
// For endpoint 0 this is just the protocol version, for all other endpoints it's a
// CRC over the entire JSON descriptor tree (this may change in future versions).
@@ -218,7 +211,7 @@ int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_
LOG_PROTO("trailer mismatch for endpoint %d: expected %04x, got %04x\r\n", endpoint_id, expected_trailer, actual_trailer);
return -1;
}
LOG_PROTO("trailer ok\r\n");
LOG_PROTO("trailer ok for endpoint %d\r\n", endpoint_id);
// TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible?
Firmware/MotorControl/protocol.hpp
+379 -134
View File
File diff suppressed because it is too large Load Diff
Firmware/MotorControl/sensorless_estimator.cpp
+101
View File
@@ -0,0 +1,101 @@
//#include "sensorless_estimator.hpp"
#include "odrive_main.hpp"
SensorlessEstimator::SensorlessEstimator()
{
// Calculate pll gains
// This calculation is currently identical to the PLL in Encoder
float pll_bandwidth = 1000.0f; // [rad/s]
pll_kp_ = 2.0f * pll_bandwidth;
// Critically damped
pll_ki_ = 0.25f * (pll_kp_ * pll_kp_);
}
bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float* phase_output) {
// 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).
// The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
// is the one computed two cycles ago. To get the correct measurement, it was stored twice:
// once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.
// Check that we don't get problems with discrete time approximation
if (!(current_meas_period * pll_kp_ < 1.0f)) {
error_ = ERROR_NUMERICAL;
return false;
}
// Clarke transform
float I_alpha_beta[2] = {
-axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC,
one_by_sqrt3 * (axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC)};
// alpha-beta vector operations
float eta[2];
for (int i = 0; i <= 1; ++i) {
// y is the total flux-driving voltage (see paper eqn 4)
float y = -axis_->motor_.config_.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];
// flux dynamics (prediction)
float x_dot = y;
// integrate prediction to current timestep
flux_state_[i] += x_dot * current_meas_period;
// eta is the estimated permanent magnet flux (see paper eqn 6)
eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i];
}
// Non-linear observer (see paper eqn 8):
float pm_flux_sqr = pm_flux_linkage_ * pm_flux_linkage_;
float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1];
float bandwidth_factor = 1.0f / pm_flux_sqr;
float eta_factor = 0.5f * (observer_gain_ * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);
static float eta_factor_avg_test = 0.0f;
eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test);
// alpha-beta vector operations
for (int i = 0; i <= 1; ++i) {
// add observer action to flux estimate dynamics
float x_dot = eta_factor * eta[i];
// convert action to discrete-time
flux_state_[i] += x_dot * current_meas_period;
// update new eta
eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i];
}
// Flux state estimation done, store V_alpha_beta for next timestep
V_alpha_beta_memory_[0] = axis_->motor_.current_control_.final_v_alpha;
V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta;
// PLL
// TODO: the PLL part has some code duplication with the encoder PLL
// predict PLL phase with velocity
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_vel_);
// update PLL phase with observer permanent magnet phase
phase_ = fast_atan2(eta[1], eta[0]);
float delta_phase = wrap_pm_pi(phase_ - pll_pos_);
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp_ * delta_phase);
// update PLL velocity
pll_vel_ += current_meas_period * pll_ki_ * delta_phase;
//TODO TEMP TEST HACK
// static int trigger_ctr = 0;
// if (++trigger_ctr >= 3*current_meas_hz) {
// trigger_ctr = 0;
// //Change to sensorless units
// motor->vel_gain = 15.0f / 200.0f;
// motor->vel_setpoint = 800.0f * motor->encoder.motor_dir;
// //Change mode
// motor->rotor_mode = ROTOR_MODE_SENSORLESS;
// }
if (pos_estimate) *pos_estimate = pll_pos_;
if (vel_estimate) *vel_estimate = pll_vel_;
if (phase_output) *phase_output = phase_;
return true;
};
Firmware/MotorControl/sensorless_estimator.hpp
+31
View File
@@ -0,0 +1,31 @@
#ifndef __SENSORLESS_ESTIMATOR_HPP
#define __SENSORLESS_ESTIMATOR_HPP
class SensorlessEstimator {
public:
enum Error_t {
ERROR_NONE,
ERROR_NUMERICAL,
};
SensorlessEstimator();
bool update(float* pos_estimate, float* vel_estimate, float* phase);
Axis* axis_ = nullptr; // set by Axis constructor
// TODO: expose on protocol
Error_t error_ = ERROR_NONE;
float phase_ = 0.0f; // [rad]
float pll_pos_ = 0.0f; // [rad]
float pll_vel_ = 0.0f; // [rad/s]
float pll_kp_ = 0.0f; // [rad/s / rad]
float pll_ki_ = 0.0f; // [(rad/s^2) / rad]
float observer_gain_ = 1000.0f; // [rad/s]
float flux_state_[2] = {0.0f, 0.0f}; // [Vs]
float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V]
float pm_flux_linkage_ = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / (<pole pairs> * <rpm/v>) }
bool estimator_good_ = false;
};
#endif /* __SENSORLESS_ESTIMATOR_HPP */
Firmware/MotorControl/utils.c
-2
View File
@@ -4,8 +4,6 @@
#include <cmsis_os.h>
#include <stm32f4xx_hal.h>
static const float one_by_sqrt3 = 0.57735026919f;
static const float two_by_sqrt3 = 1.15470053838f;
int SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
int Sextant;
Firmware/MotorControl/utils.h
+4
View File
@@ -78,6 +78,10 @@ extern "C" {
#define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y))
#define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y))
static const float one_by_sqrt3 = 0.57735026919f;
static const float two_by_sqrt3 = 1.15470053838f;
static const float sqrt3_by_2 = 0.86602540378f;
// Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta
// as per the magnitude invariant clarke transform
// The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2
Firmware/README.md
-2
View File
@@ -43,8 +43,6 @@ __CONFIG_UART_PROTOCOL__: Defines which protocol the ODrive should use on the UA
* `ascii`: The ASCII protocol. Use this option if you control the ODrive with an Arduino. The ODrive Arduino library is not yet updated to the native protocol.
* `none`: Disable UART.
__CONFIG_STEP_DIR__: Set to `y` to use the GPIO1 and GPIO2 for step/direction input. Set to `n` otherwise. To use this, `CONFIG_UART_PROTOCOL` must be `none` because UART uses the same pins.
<br><br>
## Downloading and Installing Tools
Firmware/Tupfile.lua
+7 -3
View File
@@ -128,12 +128,16 @@ build{
'Drivers/DRV8301/drv8301.c',
'MotorControl/utils.c',
'MotorControl/legacy_commands.c',
'MotorControl/low_level.c',
'MotorControl/low_level.cpp',
'MotorControl/nvm.c',
'MotorControl/axis.cpp',
'MotorControl/commands.cpp',
'MotorControl/communication.cpp',
'MotorControl/protocol.cpp',
'MotorControl/config.cpp'
'MotorControl/motor.cpp',
'MotorControl/encoder.cpp',
'MotorControl/controller.cpp',
'MotorControl/sensorless_estimator.cpp',
'MotorControl/main.cpp'
},
includes={
'Drivers/DRV8301',