Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-26 12:27:45 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

[TEMP] refactoring: compile works, motors initialize as usual

Browse Source
This commit is contained in:
Samuel Sadok
2018-03-02 22:03:24 -08:00
parent 037f715dc2
commit b32c87df5a
33 changed files with 2298 additions and 1929 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/.gitignore
+4 -1
View File
@@ -15,8 +15,11 @@ Odrive.xml
.settings/
.project
# VSCode stuff
/.vscode/.cortex-debug.*.state.json
# STM32CubeMX (in case you put it in this folder, or a symlink)
STM32CubeMX
#gdb log
openocd.log
openocd.log
Firmware/Board/v3.3/Inc/gpio.h
+5 -1
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 */
@@ -73,6 +73,10 @@ void MX_GPIO_Init(void);
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.3/Inc/main.h
+1 -1
View File
@@ -54,7 +54,7 @@
#define HW_VERSION_MAJOR 3
#define HW_VERSION_MINOR 4
// #define HW_VERSION_HIGH_VOLTAGE true
#define HW_VERSION_HIGH_VOLTAGE true
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \
|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2
Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
Vendored
+1 -1
View File
@@ -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.3/Src/freertos.c
+6 -28
View File
@@ -53,10 +53,11 @@
/* USER CODE BEGIN Includes */
#include "freertos_vars.h"
#include "low_level.h"
//#include "low_level.h"
#include "axis_c_interface.h"
#include "commands.h"
#include "config.h"
//#include "commands.h"
//#include "config.h"
int odrive_main(void);
/* USER CODE END Includes */
/* Variables -----------------------------------------------------------------*/
@@ -67,8 +68,6 @@ osThreadId defaultTaskHandle;
osSemaphoreId sem_usb_irq;
// List of threads
osThreadId thread_motor_0;
osThreadId thread_motor_1;
osThreadId thread_cmd_parse;
/* USER CODE END Variables */
@@ -110,7 +109,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);
@@ -142,28 +141,7 @@ void StartDefaultTask(void const * argument)
/* 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.3/Src/gpio.c
+95 -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
@@ -158,10 +158,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;
@@ -178,59 +212,73 @@ 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) {
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;
}
}
}
//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.3/Src/stm32f4xx_it.c
+1 -1
View File
@@ -209,7 +209,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.3/Src/syscalls.c
+3
View File
@@ -23,6 +23,7 @@
static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE];
int _write(int file, char* data, int len) {
#if 0 // TODO: revert!
//number of bytes written
int written = 0;
switch (serial_printf_select) {
@@ -57,6 +58,8 @@ int _write(int file, char* data, int len) {
}
return written;
#endif
return len;
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) {
Firmware/Board/v3.3/Src/usbd_cdc_if.c
+1 -1
View File
@@ -274,7 +274,7 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
{
/* USER CODE BEGIN 6 */
set_cmd_buffer(Buf, *Len);
//set_cmd_buffer(Buf, *Len); TODO: revert!
osSemaphoreRelease(sem_usb_rx);
return (USBD_OK);
Firmware/MotorControl/axis.cpp
+234 -64
View File
@@ -1,65 +1,239 @@
#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 "axis.hpp"
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),
encoder(encoder),
sensorless_estimator(sensorless_estimator),
controller(controller),
motor(motor)
{
encoder.axis = this;
sensorless_estimator.axis = this;
controller.axis = this;
motor.axis = this;
}
//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_;
static void step_cb_wrapper(void* ctx) {
reinterpret_cast<Axis*>(ctx)->step_cb();
}
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),
config_(config),
legacy_motor_ref_(legacy_motor_ref) {
SetupLegacyMappings();
void Axis::setup() {
encoder.setup();
motor.setup();
}
void Axis::StateMachineLoop() {
void Axis::start_thread() {
osThreadDef(thread_def, run_state_machine_loop, hw_config.thread_priority, 0, 512);
thread_id = osThreadCreate(osThread(thread_def), this);
thread_id_valid = true;
}
void Axis::signal_thread(thread_signals sig) {
if (thread_id_valid)
osSignalSet(thread_id, sig);
}
// 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;
}
};
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);
}
}
//Returns true if everything is OK (no fault)
bool Axis::check_PSU_brownout() {
if(vbus_voltage < config.dc_bus_brownout_trip_level)
return false;
return true;
}
// Returns true if everything is ok. Sets motor->error and returns false otherwise.
bool Axis::do_checks() {
if (!motor.check_DRV_fault()) {
motor.error = ERROR_DRV_FAULT;
return false;
}
if (!check_PSU_brownout()) {
motor.error = ERROR_DC_BUS_BROWNOUT;
return 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 false;
return x < 1.0f;
});
if (x < 1.0f)
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 false;
return vel < config.spin_up_target_vel;
});
return vel >= config.spin_up_target_vel;
}
// 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() {
run_control_loop([this](){
float pos_estimate, vel_estimate, phase, current_setpoint;
// 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 false;
if (!controller.update(pos_estimate, vel_estimate, &current_setpoint))
return false;
return motor.update(current_setpoint, phase);
});
return false;
}
bool Axis::run_closed_loop_control_loop() {
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 false;
if (!controller.update(pos_estimate, vel_estimate, &current_setpoint))
return false;
return motor.update(current_setpoint, phase);
});
return false;
}
bool Axis::run_idle_loop() {
// TODO: allow preemption
for (;;) {
if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) {
motor.error = ERROR_FOC_MEASUREMENT_TIMEOUT;
break;
}
}
return false;
}
void Axis::run_state_machine_loop() {
//TODO: Move this somewhere else
// TODO: respect changes of CPR
// 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) {
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;
enum AxisState_t {
AXIS_STATE_MOTOR_CALIBRATION,
AXIS_STATE_ENCODER_CALIBRATION,
AXIS_STATE_SENSORLESS_SPINUP,
AXIS_STATE_SENSORLESS_CONTROL,
AXIS_STATE_CLOSED_LOOP_CONTROL,
AXIS_STATE_IDLE
};
AxisState_t axis_state = AXIS_STATE_MOTOR_CALIBRATION;
for (;;) {
switch (axis_state) {
case AXIS_STATE_MOTOR_CALIBRATION:
if (!config.enable_motor_calibration || motor.run_calibration()) {
axis_state = AXIS_STATE_ENCODER_CALIBRATION;
} else {
axis_state = AXIS_STATE_IDLE;
}
break;
case AXIS_STATE_ENCODER_CALIBRATION:
if (!config.enable_encoder_calibration || encoder.run_calibration()) {
axis_state = config.enable_control ?
config.sensorless ?
AXIS_STATE_SENSORLESS_SPINUP :
AXIS_STATE_CLOSED_LOOP_CONTROL :
AXIS_STATE_IDLE;
if (axis_state != AXIS_STATE_IDLE)
set_step_dir_enabled(config.enable_step_dir_after_calibration);
} else {
axis_state = AXIS_STATE_IDLE;
}
break;
case AXIS_STATE_SENSORLESS_SPINUP:
if (run_sensorless_spin_up()) {
axis_state = AXIS_STATE_SENSORLESS_CONTROL;
} else {
axis_state = AXIS_STATE_IDLE;
}
break;
case AXIS_STATE_SENSORLESS_CONTROL:
run_sensorless_control_loop();
axis_state = AXIS_STATE_IDLE; // TODO: restart if desired
break;
case AXIS_STATE_CLOSED_LOOP_CONTROL:
run_closed_loop_control_loop();
axis_state = AXIS_STATE_IDLE;
break;
case AXIS_STATE_IDLE:
default:
run_idle_loop();
break;
}
}
/*
bool calibration_ok = false;
for (;;) {
// Keep rotor estimation up to date while idling
@@ -68,30 +242,26 @@ void Axis::StateMachineLoop() {
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
calibration_ok = motor.do_calibration();
if (calibration_ok)
calibration_ok = encoder.do_calibration();
}
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;
enable_step_dir = true;
if (rotor_mode == ROTOR_MODE_SENSORLESS) {
bool spin_up_ok = do_sensorless_spin_up();
if (spin_up_ok)
do_sensorless_control();
} else {
do_closed_loop_control();
}
if (enable_control_) { // if control is still enabled, we exited because of error
calibration_ok = false;
enable_control_ = false;
}
}
}
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
+209
View File
@@ -0,0 +1,209 @@
#ifndef __AXIS_HPP
#define __AXIS_HPP
#include <utils.h>
#include <cmsis_os.h>
#include <functional>
#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>
#include <board_config_v3.3.h>
/*class Estimator {
public:
virtual float get_position_estimation(void);
virtual float get_velocity_estimation(void);
};*/
/*
class Motor {
public:
// @brief Updates the current control loop
virtual void update(float current_ref);
};*/
// The Axis declaration is needed in the other header files
class Axis;
#include <encoder.hpp>
#include <sensorless_estimator.hpp>
#include <controller.hpp>
#include <motor.hpp>
#include <low_level.h>
//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 float brake_resistance; // [ohm]
constexpr size_t AXIS_COUNT = 2;
extern Axis *axes[AXIS_COUNT];
/*
class Controller {
public:
// @brief Updates the controller loop(s)
virtual void update(void);
};*/
//Outside axis:
//command handler
//callback dispatch
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;
// TODO: decide if we want to consolidate all default configs in one file for ease of use?
struct AxisConfig_t {
bool enable_motor_calibration = true;
bool enable_encoder_calibration = true;
bool enable_control = true;
bool sensorless = false;
bool enable_step_dir_after_calibration = true; // 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]
Rotor_mode_t rotor_mode = ROTOR_MODE_ENCODER;
// 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:
//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
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_thread(thread_signals sig);
// Infinite loop that does calibration and enters main control loop as appropriate
void run_state_machine_loop();
static void run_state_machine_loop(const void* ctx) {
const_cast<Axis*>(reinterpret_cast<const Axis*>(ctx))->run_state_machine_loop();
};
void step_cb();
void set_step_dir_enabled(bool enable);
bool check_DRV_fault();
bool check_PSU_brownout();
bool do_checks();
// TODO: check if this uses dynamic memory
// @brief Runs the update handler at the frequency of the current measurements.
//
// The loop runs until one of the following conditions:
// - the update handler returns false
// - the current measurement times out
// - do_checks() becomes false
// - update_handler doesn't finish in time
//
// The function arms the motor at the beginning of the control loop and disarms it at
// the end of the control loop.
// Note that if this function returns, this should generally be considered an error condition,
// unless the termination was deliberately caused by the update_handler because it was of the
// opinion that the loop's task was completed.
// @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) {
motor.arm();
while (true /*enable_control*/) { // TODO: check for state change
if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) {
motor.error = ERROR_FOC_MEASUREMENT_TIMEOUT;
break;
}
// Proactively set phase voltages to 0. If the control deadline is missed,
// the voltages will go to zero.
motor.enqueue_voltage_timings(0.0f, 0.0f);
if (!do_checks())
break;
if (!update_handler())
break;
update_brake_current();
// Check we meet deadlines after queueing
motor.last_cpu_time = motor.check_timing();
if (!(motor.last_cpu_time < motor.hw_config.control_deadline)) {
motor.error = ERROR_PHASE_RESISTANCE_TIMING;
break;
}
++loop_counter;
// TODO: maybe we should just abort automatically as soon as error is set
}
// We are exiting control: disarm motor, reset Ibus, and update brake current
motor.disarm();
motor.current_control.Ibus = 0.0f;
update_brake_current();
}
bool run_sensorless_spin_up();
bool run_sensorless_control_loop();
bool run_closed_loop_control_loop();
bool run_idle_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;
bool enable_step_dir = false; //auto enabled after calibration
uint32_t loop_counter = 0;
};
#endif /* __AXIS_HPP */
Firmware/MotorControl/board_config_v3.3.h
+112
View File
@@ -0,0 +1,112 @@
#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
+5 -4
View File
@@ -1,4 +1,4 @@
#if 0
/* Includes ------------------------------------------------------------------*/
// TODO: remove this option
@@ -8,7 +8,7 @@
#include "commands.h"
#include "low_level.h"
#include "axis.h"
#include "axis.hpp"
#include "protocol.hpp"
#include "freertos_vars.h"
#include "utils.h"
@@ -145,7 +145,7 @@ const Endpoint endpoints[] = {
Endpoint::make_property("DC_calib.phC", &motors[0].DC_calib.phC),
Endpoint::make_property("shunt_conductance", &motors[0].shunt_conductance),
Endpoint::make_property("phase_current_rev_gain", &motors[0].phase_current_rev_gain),
Endpoint::make_property("thread_ready", &motors[0].thread_ready),
Endpoint::make_property("thread_id_valid", &motors[0].thread_id_valid),
Endpoint::make_property("control_deadline", &motors[0].control_deadline),
Endpoint::make_property("last_cpu_time", &motors[0].last_cpu_time),
Endpoint::make_property("loop_counter", &motors[0].loop_counter),
@@ -232,7 +232,7 @@ const Endpoint endpoints[] = {
Endpoint::make_property("DC_calib.phC", &motors[1].DC_calib.phC),
Endpoint::make_property("shunt_conductance", &motors[1].shunt_conductance),
Endpoint::make_property("phase_current_rev_gain", &motors[1].phase_current_rev_gain),
Endpoint::make_property("thread_ready", &motors[1].thread_ready),
Endpoint::make_property("thread_id_valid", &motors[1].thread_id_valid),
Endpoint::make_property("control_deadline", &motors[1].control_deadline),
Endpoint::make_property("last_cpu_time", &motors[1].last_cpu_time),
Endpoint::make_property("loop_counter", &motors[1].loop_counter),
@@ -509,3 +509,4 @@ void usb_update_thread() {
vTaskDelete(osThreadGetId());
}
#endif
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
+130
View File
@@ -0,0 +1,130 @@
#include "axis.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", motor->pos_setpoint, motor->vel_setpoint, motor->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", motor->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", motor->current_setpoint);
#endif
}
/*
* 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::anti_cogging_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
anti_cogging_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
+72
View File
@@ -0,0 +1,72 @@
// 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_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
bool anti_cogging_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
float pos_setpoint = 0.0f;
float vel_setpoint = 0.0f;
// float vel_setpoint = 800.0f; <sensorless example>
// float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] <sensorless example>
float vel_integrator_current = 0.0f; // [A]
float current_setpoint = 0.0f; // [A]
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,
};
// Cache for remote procedure calls arguments TODO: remove
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;
};
Firmware/MotorControl/encoder.cpp
+204
View File
@@ -0,0 +1,204 @@
//#include "encoder.hpp"
#include "axis.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: disarm 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(uint32_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_duration = 1.0f;
static const float scan_range = 16.0f * M_PI;
static const size_t num_steps = scan_duration * current_meas_hz;
// go to motor zero phase for start_lock_duration to get ready to scan
size_t i = 0;
axis->run_control_loop([&](){
axis->motor.enqueue_voltage_timings(voltage_magnitude, 0.0f);
return ++i < start_lock_duration * current_meas_hz;
});
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_range * (float)i / (float)num_steps - scan_range / 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);
encvaluesum += (int64_t)hw_config.timer->Instance->CNT;
return ++i < num_steps;
});
if (i < num_steps)
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_range / 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)
{
axis->motor.error = ERROR_ENCODER_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
axis->motor.error = ERROR_ENCODER_RESPONSE;
return false;
}
// scan backwards
i = 0;
axis->run_control_loop([&](){
float phase = wrap_pm_pi(-scan_range * (float)i / (float)num_steps + scan_range / 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);
encvaluesum += (int64_t)hw_config.timer->Instance->CNT;
return ++i < num_steps;
});
if (i < num_steps)
return false;
int offset = encvaluesum / (num_steps * 2);
config.offset = offset;
config.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);
// continue until the index is found
return !index_found;
});
return index_found;
}
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.calibrated)
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)) {
axis->motor.error = ERROR_CALIBRATION_TIMING;
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
+42
View File
@@ -0,0 +1,42 @@
#ifndef __ENCODER_HPP
#define __ENCODER_HPP
struct EncoderConfig_t {
bool use_index = false;
bool 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:
Encoder(const EncoderHardwareConfig_t& hw_config,
EncoderConfig_t& config);
void setup();
void enc_index_cb();
void set_count(uint32_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
volatile bool index_found = false;
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]
};
#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
@@ -1,7 +1,7 @@
/* Includes ------------------------------------------------------------------*/
#include "legacy_commands.h"
#include <utils.h>
#if 0
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
@@ -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
-1459
View File
File diff suppressed because it is too large Load Diff
Firmware/MotorControl/low_level.cpp
+270
View File
@@ -0,0 +1,270 @@
/* 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 <axis.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;
// TODO: Migrate to C++, clearly we are actually doing object oriented code here...
float brake_resistance = 0.47f; // [ohm]
/* 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 global_fault(Error_t error) {
// Disable motors NOW!
for (size_t i = 0; i < AXIS_COUNT; ++i) {
axes[i]->motor.disarm();
}
// Set fault codes, etc.
for (size_t i = 0; i < AXIS_COUNT; ++i) {
axes[i]->motor.error = error;
// TODO: update axis_state
}
// disable brake resistor
set_brake_current(0.0f);
}
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;
}
//--------------------------------
// 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)) {
global_fault(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;
if (&axis == axes[1] && counting_down) {
// Load next timings for M0 (only once is sufficient)
if (hadc == &hadc2) {
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];
}
} else if (&axis == axes[0] && !counting_down) {
// Load next timings for M1 (only once is sufficient)
if (hadc == &hadc2) {
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];
}
}
// Check the timing of the sequencing
axis.motor.check_timing();
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_thread(Axis::thread_signals::M_SIGNAL_PH_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;
}
}
}
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;
}
// Note: set_brake_current will clip negative values to 0.0f
set_brake_current(-Ibus_sum);
}
void set_brake_current(float brake_current) {
if (brake_current < 0.0f) brake_current = 0.0f;
float brake_duty = brake_current * brake_resistance / vbus_voltage;
// 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 -231
View File
@@ -8,191 +8,10 @@ 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 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;
int timing_log_index;
uint16_t timing_log[TIMING_LOG_SIZE];
// Cache for remote procedure calls arguments
struct {
float pos_setpoint;
float vel_feed_forward;
float current_feed_forward;
} set_pos_setpoint_args;
struct {
float vel_setpoint;
float current_feed_forward;
} set_vel_setpoint_args;
struct {
float current_setpoint;
} set_current_setpoint_args;
Anticogging_t anticogging;
DRV8301_FaultType_e drv_fault;
} Motor_t;
typedef struct{
int type;
@@ -200,74 +19,26 @@ typedef struct{
} 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);
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
+110
View File
@@ -0,0 +1,110 @@
#include <axis.hpp>
#include <nvm_config.hpp>
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];
bool enable_uart;
typedef Config<AxisConfig_t[2], MotorConfig_t[2], float, bool> ConfigFormat;
void save_configuration(void) {
if (ConfigFormat::safe_store_config(
&axis_configs,
&motor_configs,
&brake_resistance,
&enable_uart)) {
//printf("saving configuration failed\r\n"); osDelay(5);
}
}
void load_configuration() {
if (NVM_init() ||
ConfigFormat::safe_load_config(
&axis_configs,
&motor_configs,
&brake_resistance,
&enable_uart)) {
for (size_t i = 0; i < AXIS_COUNT; ++i) {
axis_configs[i] = AxisConfig_t();
motor_configs[i] = MotorConfig_t();
}
brake_resistance = 0.47f;
enable_uart = true;
}
}
void erase_configuration(void) {
NVM_erase();
}
extern "C" {
int odrive_main(void);
}
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 (enable_uart) {
axes[0]->config.enable_step_dir_after_calibration = false;
axes[0]->set_step_dir_enabled(false);
SetGPIO12toUART();
}
/*
// Init communications (this requires the axis objects to be constructed)
init_communication();
// 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);
*/
// 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
+322
View File
@@ -0,0 +1,322 @@
#include <algorithm>
#include "drv8301.h"
//#include "motor.hpp"
#include <axis.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,
})
{
}
void Motor::arm() {
__HAL_TIM_MOE_ENABLE(hw_config.timer); // enable pwm outputs
}
void Motor::disarm() {
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config.timer); // disables pwm outputs
}
// 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);
}
//Returns true if everything is OK (no fault)
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;
}
uint16_t Motor::check_timing() {
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 (++(timing_log_index) == TIMING_LOG_SIZE) {
timing_log_index = 0;
}
timing_log[timing_log_index] = timing;
return 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) {
error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE;
return false;
}
// Test voltage along phase A
enqueue_voltage_timings(test_voltage, 0.0f);
return ++i < num_test_cycles;
});
//// De-energize motor
//enqueue_voltage_timings(motor, 0.0f, 0.0f);
float R = test_voltage / test_current;
config.phase_resistance = R;
return i == num_test_cycles; // 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);
return ++t < (num_cycles << 1);
});
if (t != (num_cycles << 1))
return false; // the loop aborted prematurely
//// 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) {
error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE;
return 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;
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);
}
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);
}
// 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);
update_brake_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
+147
View File
@@ -0,0 +1,147 @@
#ifndef __MOTOR_HPP
#define __MOTOR_HPP
// The Motor declaration is needed in the axis header
//class Motor;
#include <axis.hpp>
#include "drv8301.h"
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;
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 {
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;
#define TIMING_LOG_SIZE 16
class Motor {
public:
Motor(const MotorHardwareConfig_t& hw_config,
const GateDriverHardwareConfig_t& gate_driver_config,
MotorConfig_t& config);
void arm();
void disarm();
void setup() {
DRV8301_setup();
}
void DRV8301_setup();
bool check_DRV_fault();
uint16_t check_timing();
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
Error_t error = ERROR_NO_ERROR;
// bool enable_control = true; // 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 = true; // trigger motor calibration. will be reset to false after self test
// bool calibration_ok = false;
uint16_t next_timings[3] = {
TIM_1_8_PERIOD_CLOCKS / 2,
TIM_1_8_PERIOD_CLOCKS / 2,
TIM_1_8_PERIOD_CLOCKS / 2
};
uint16_t last_cpu_time = 0;
Iph_BC_t current_meas = {0.0f, 0.0f};
Iph_BC_t DC_calib = {0.0f, 0.0f};
DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers (initialized by DRV8301_setup)
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,
};
int timing_log_index = 0;
uint16_t timing_log[TIMING_LOG_SIZE] = { 0 };
DRV8301_FaultType_e drv_fault = DRV8301_FaultType_NoFault;
};
#endif // __MOTOR_HPP
Firmware/MotorControl/nvm_config.hpp
+142
View File
@@ -0,0 +1,142 @@
/*
* 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 <stdint.h>
#include <stdlib.h>
#include <stm32f405xx.h>
#include "nvm.h"
#include "crc.hpp"
#include "low_level.h"
#include "axis.hpp"
/* Private defines -----------------------------------------------------------*/
#define CONFIG_CRC16_INIT 0xabcd
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* 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.
// The types of these objects are passed as template arguments to Config<Ts...>.
//
// Config<Ts...> has two template specializations to implement template recursion:
// - Config<T, Ts...> handles loading/storing of the first object (type T) and leaves
// the rest of the objects to an "inner" class Config<Ts...>.
// - Config<> represents the leaf of the recursion.
template<typename ... Ts>
struct Config;
template<>
struct Config<> {
static size_t get_size() {
return 0;
}
static int load_config(size_t offset, uint16_t* crc16) {
return 0;
}
static int store_config(size_t offset, uint16_t* crc16) {
return 0;
}
};
template<typename T, typename ... Ts>
struct Config<T, Ts...> {
static size_t get_size() {
return sizeof(T) + Config<Ts...>::get_size();
}
// @brief Loads one or more consecutive objects from the NVM.
// During loading this function also calculates the CRC over the loaded data.
// @param offset: 0 means that the function should start reading at the beginning
// of the last comitted NVM block
// @param crc16: the result of the CRC calculation is written to this address
// @param val0, vals: the values to be loaded
static int load_config(size_t offset, uint16_t* crc16, T* val0, Ts* ... vals) {
size_t size = sizeof(T);
// save current CRC (in case val0 and crc16 point to the same address)
size_t previous_crc16 = *crc16;
if (NVM_read(offset, (uint8_t *)val0, size))
return -1;
*crc16 = calc_crc16(previous_crc16, (uint8_t *)val0, size);
if (Config<Ts...>::load_config(offset + size, crc16, vals...))
return -1;
return 0;
}
// @brief Stores one or more consecutive objects to the NVM.
// During storing this function also calculates the CRC over the stored data.
// @param offset: 0 means that the function should start writing at the beginning
// of the currently active NVM write block
// @param crc16: the result of the CRC calculation is written to this address
// @param val0, vals: the values to be stored
static int store_config(size_t offset, uint16_t* crc16, const T* val0, const Ts* ... vals) {
size_t size = sizeof(T);
if (NVM_write(offset, (uint8_t *)val0, size))
return -1;
// update CRC _after_ writing (in case val0 and crc16 point to the same address)
if (crc16)
*crc16 = calc_crc16(*crc16, (uint8_t *)val0, size);
if (Config<Ts...>::store_config(offset + size, crc16, vals...))
return -1;
return 0;
}
// @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 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 = CONFIG_CRC16_INIT ^ config_version;
if (Config<T, Ts..., uint16_t>::load_config(0, &crc16, val0, vals..., &crc16))
return -1;
if (crc16)
return -1;
return 0;
}
// @brief Stores one or more consecutive objects to the NVM. In addition to the
// provided objects, a CRC of the data is stored.
//
// The CRC includes a version number and thus adds some protection against
// 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 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 = 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))
return -1;
if (NVM_commit())
return -1;
return 0;
}
};
Firmware/MotorControl/sensorless_estimator.cpp
+101
View File
@@ -0,0 +1,101 @@
//#include "sensorless_estimator.hpp"
#include <axis.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)) {
axis->motor.error = ERROR_CALIBRATION_TIMING;
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_linkage * pm_flux_linkage);
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
+24
View File
@@ -0,0 +1,24 @@
#ifndef __SENSORLESS_ESTIMATOR_HPP
#define __SENSORLESS_ESTIMATOR_HPP
class SensorlessEstimator {
public:
SensorlessEstimator();
bool update(float* pos_estimate, float* vel_estimate, float* phase);
Axis* axis = nullptr; // set by Axis constructor
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/Tupfile.lua
+6 -2
View File
@@ -65,12 +65,16 @@ build{
sources={
'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/protocol.cpp',
'MotorControl/config.cpp'
'MotorControl/motor.cpp',
'MotorControl/encoder.cpp',
'MotorControl/controller.cpp',
'MotorControl/sensorless_estimator.cpp',
'MotorControl/main.cpp'
},
includes={
'MotorControl'