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

Merge branch 'devel' of github.com:madcowswe/ODrive into RazorsFrozenTesting

Browse Source
This commit is contained in:
camrbuss
2020-05-23 15:13:06 -06:00
parent 573ad8f7f2 2a1d95dbd4
commit e9d636d13c
83 changed files with 15699 additions and 3211 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2
View File
@@ -54,3 +54,5 @@ ODrive\.creator\.user
ODrive\.files
ODrive\.includes
Firmware/Tests/bin/
CHANGELOG.md
+14 -3
View File
@@ -5,6 +5,7 @@ Please add a note of your changes below this heading if you make a Pull Request.
* AC Induction Motor support.
* Tracking of rotor flux through rotor time constant
* Automatic d axis current for Maximum Torque Per Amp (MTPA)
* ASCII "w" commands now execute write hooks.
* Simplified control interface ("Input Filter" branch)
* New input variables: `input_pos`, `input_vel`, and `input_current`
* New setting `input_mode` to switch between different input behaviours
@@ -18,19 +19,29 @@ Please add a note of your changes below this heading if you make a Pull Request.
* [CAN Communication with CANSimple stack](can-protocol.md)
* Gain scheduling for anti-hunt when close to 0 position error
* Velocity Limiting in Current Control mode according to `vel_limit` and `vel_gain`
* Regen current limiting according to `max_regen_limit`, in Amps
* DC Bus hard current limiting according to `power_supply_min_current` and `power_supply_max_current`
* Regen current limiting according to `max_regen_current`, in Amps
* DC Bus hard current limiting according to `dc_max_negative_current` and `dc_max_positive_current`
* Brake resistor logic now attempts to clamp voltage according to `odrv.config.dc_bus_overvoltage_ramp_start` and `odrv.config.dc_bus_overvoltage_ramp_end`
* Unit Testing with Doctest has been started for select algorithms, see [Firmware/Tests/test_runner.cpp](Firmware/Tests/test_runner.cpp)
* Added support for Flylint VSCode Extension for static code analysis
* Using an STM32F405 .svd file allows CortexDebug to view registers during debugging
* Added scripts for building via docker.
* Added ability to change uart baudrate via fibre
### Changed
* Changed ratiometric `motor.config.current_lim_tolerance` to absolute `motor.config.current_lim_margin`
* Moved `controller.vel_ramp_enable` into `controller.config`.
* Moved `controller.vel_ramp_enable` to INPUT_MODE_VEL_RAMP.
* Anticogging map is temporarily forced to 0.1 deg precision, but saves with the config
* Some Encoder settings have been made read-only
* Cleaned up VSCode C/C++ Configuration settings on Windows with recursive includePath
* Now compiling with C++17
* Fixed a firmware hang that could occur from unlikely but possible user input
* Added JSON caching to Fibre. This drastically reduces the time odrivetool needs to connect to an ODrive (except for the first time or after firmware updates).
* Fix IPython `RuntimeWarning` that would occur every time `odrivetool` was started.
* Reboot on `erase_configuration()`. This avoids unexpected behavior of a subsequent `save_configuration()` call, since the configuration is only erased from NVM, not from RAM.
* Change `motor.get_inverter_temp()` to use a property which was already being sampled at `motor.inverter_temp`
* Fixed a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint
# Releases
## [0.4.11] - 2019-07-25
### Added
Dockerfile
+22
View File
@@ -0,0 +1,22 @@
FROM ubuntu:bionic
# Prepare the build environment and dependencies
RUN apt-get update
RUN apt-get -y install software-properties-common
RUN add-apt-repository ppa:team-gcc-arm-embedded/ppa
RUN add-apt-repository ppa:jonathonf/tup
RUN apt-get update
RUN apt-get -y upgrade
RUN apt-get -y install gcc-arm-embedded openocd tup python3.7 build-essential git
# Build step below does not know about debian's python naming schemme
RUN ln -s /usr/bin/python3.7 /usr/bin/python
# Copy the firmware tree into the container
RUN mkdir ODrive
COPY . ODrive
WORKDIR ODrive/Firmware
# Hack around Tup's dependency on FUSE
RUN tup generate build.sh
RUN ./build.sh
Firmware/.vscode/launch.json
Vendored
+35 -2
View File
@@ -9,7 +9,7 @@
"type": "cortex-debug",
"servertype": "openocd",
"request": "launch",
"name": "Debug ODrive",
"name": "Debug ODrive - ST-Link",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"configFiles": [
"interface/stlink-v2.cfg",
@@ -23,7 +23,7 @@
"type": "cortex-debug",
"servertype": "openocd",
"request": "launch",
"name": "Debug ODrive - FreeRTOS",
"name": "Debug ODrive - ST-Link - FreeRTOS",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"rtos": "FreeRTOS",
"configFiles": [
@@ -33,5 +33,38 @@
"svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd",
"cwd": "${workspaceRoot}"
},
{
// For the Cortex-Debug extension
// ssh -t odrv -L3333:localhost:3333 bash -c "\"openocd '-f' 'interface/stlink-v2.cfg' '-f' 'target/stm32f4x_stlink.cfg'\""
"type": "cortex-debug",
"servertype": "external",
"gdbTarget": "localhost:3333",
"preLaunchCommands": [
"load"
],
"request": "launch",
"name": "Debug ODrive via external server",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"configFiles": [
"interface/stlink-v2.cfg",
"target/stm32f4x_stlink.cfg",
],
"svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd",
"cwd": "${workspaceRoot}"
},
{
// For the Cortex-Debug extensions
"type": "cortex-debug",
"servertype": "bmp",
"request": "launch",
"name": "Debug ODrive - Black Magic Probe",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"device": "STM32F4xx",
"BMPGDBSerialPort": "${env:BMP_PORT}",
"interface": "swd",
"targetId": 1,
"armToolchainPath": "${env:ARM_GCC_ROOT}/bin/",
"cwd": "${workspaceRoot}"
}
]
}
Firmware/.vscode/settings.json
Vendored
-3
View File
@@ -1,9 +1,6 @@
{
"C_Cpp.intelliSenseEngine": "Default",
"C_Cpp.intelliSenseEngineFallback": "Disabled",
"files.exclude": {
"build": true
},
"files.associations": {
"memory": "cpp",
"utility": "cpp",
Firmware/.vscode/tasks.json
Vendored
+7 -1
View File
@@ -19,11 +19,17 @@
]
},
{
"label": "flash",
"label": "flash - ST-Link",
"type": "shell",
"command": "make flash",
"problemMatcher": []
},
{
"label": "flash - Black Magic Probe",
"type": "shell",
"command": "make flashbmp",
"problemMatcher": []
},
{
"label": "openocd",
"type": "shell",
Firmware/Board/v3/Inc/gpio.h
-2
View File
@@ -76,8 +76,6 @@ bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin,
void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin);
void GPIO_set_to_analog(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin);
uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin);
GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin);
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 4
#define GPIO_COUNT 5
Firmware/Board/v3/Inc/main.h
-2
View File
@@ -173,10 +173,8 @@
#if HW_VERSION_VOLTAGE >= 48
#define VBUS_S_DIVIDER_RATIO 19.0f
#define VBUS_OVERVOLTAGE_LEVEL 52.0f
#elif HW_VERSION_VOLTAGE == 24
#define VBUS_S_DIVIDER_RATIO 11.0f
#define VBUS_OVERVOLTAGE_LEVEL 26.0f
#else
#error "unknown board voltage"
#endif
Firmware/Board/v3/Src/gpio.c
-42
View File
@@ -278,49 +278,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_pin) {
}
}
GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){
switch(GPIO_pin){
case 1: return GPIO_1_GPIO_Port; break;
case 2: return GPIO_2_GPIO_Port; break;
case 3: return GPIO_3_GPIO_Port; break;
case 4: return GPIO_4_GPIO_Port; break;
#ifdef GPIO_5_GPIO_Port
case 5: return GPIO_5_GPIO_Port; break;
#endif
#ifdef GPIO_6_GPIO_Port
case 6: return GPIO_6_GPIO_Port; break;
#endif
#ifdef GPIO_7_GPIO_Port
case 7: return GPIO_7_GPIO_Port; break;
#endif
#ifdef GPIO_8_GPIO_Port
case 8: return GPIO_8_GPIO_Port; break;
#endif
default: return GPIO_1_GPIO_Port;
}
}
uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){
switch(GPIO_pin){
case 1: return GPIO_1_Pin; break;
case 2: return GPIO_2_Pin; break;
case 3: return GPIO_3_Pin; break;
case 4: return GPIO_4_Pin; break;
#ifdef GPIO_5_Pin
case 5: return GPIO_5_Pin; break;
#endif
#ifdef GPIO_6_Pin
case 6: return GPIO_6_Pin; break;
#endif
#ifdef GPIO_7_Pin
case 7: return GPIO_7_Pin; break;
#endif
#ifdef GPIO_8_Pin
case 8: return GPIO_8_Pin; break;
#endif
default: return GPIO_1_Pin;
}
}
/* USER CODE END 2 */
Firmware/Board/v3/Src/spi.c
+1 -1
View File
@@ -103,7 +103,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
*/
GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Pull = GPIO_PULLUP; // required for disconnect detection on SPI encoders
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF6_SPI3;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
Firmware/Makefile
+9
View File
@@ -20,6 +20,15 @@ flash: all
-c 'reset run' \
-c exit
flashbmp: all
arm-none-eabi-gdb --ex 'target extended-remote $(BMP_PORT)' \
--ex 'monitor swdp_scan' \
--ex 'attach 1' \
--ex 'load' \
--ex 'detach' \
--ex 'quit' \
$(FIRMWARE)
gdb: all
arm-none-eabi-gdb $(FIRMWARE) -x openocd.gdbinit
Firmware/MotorControl/axis.cpp
+11 -23
View File
@@ -5,6 +5,7 @@
#include "odrive_main.h"
#include "utils.hpp"
#include "gpio_utils.hpp"
#include "communication/interface_can.hpp"
Axis::Axis(int axis_num,
@@ -86,7 +87,7 @@ static void run_state_machine_loop_wrapper(void* ctx) {
// @brief Starts run_state_machine_loop in a new thread
void Axis::start_thread() {
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, stack_size_ / sizeof(StackType_t));
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_valid_ = true;
}
@@ -165,21 +166,6 @@ bool Axis::do_checks() {
if (!(vbus_voltage <= board_config.dc_bus_overvoltage_trip_level))
error_ |= ERROR_DC_BUS_OVER_VOLTAGE;
// This is the same math that's used in update_brake_current(). Should we calculate IBus globally?
float Ibus_sum = 0.0f;
for (size_t i = 0; i < AXIS_COUNT; ++i) {
if (axes[i]->motor_.armed_state_ == Motor::ARMED_STATE_ARMED) {
Ibus_sum += axes[i]->motor_.current_control_.Ibus;
}
}
if (Ibus_sum > board_config.power_supply_max_current) {
error_ |= ERROR_DC_BUS_OVER_CURRENT;
}
if (Ibus_sum < board_config.power_supply_min_current) {
error_ |= ERROR_DC_BUS_UNDER_CURRENT;
}
// Sub-components should use set_error which will propegate to this error_
motor_.do_checks();
// encoder_.do_checks();
@@ -216,9 +202,7 @@ void Axis::watchdog_feed() {
// @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired.
bool Axis::watchdog_check() {
// reset value = 0 means watchdog disabled.
if (!config_.enable_watchdog) return true;
if (get_watchdog_reset() == 0) return true;
// explicit check here to ensure that we don't underflow back to UINT32_MAX
if (watchdog_current_value_ > 0) {
@@ -337,7 +321,7 @@ bool Axis::run_closed_loop_control_loop() {
return true;
});
set_step_dir_active(false);
set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
return check_for_errors();
}
@@ -393,7 +377,7 @@ bool Axis::run_homing() {
controller_.vel_setpoint_ = 0.0f; // Change directions without decelerating
// Set our current position in encoder counts to make control more logical
encoder_.set_linear_count(static_cast<int32_t>(controller_.pos_setpoint_));
encoder_.set_linear_count((int32_t)controller_.pos_setpoint_);
controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;
controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
@@ -427,6 +411,7 @@ bool Axis::run_idle_loop() {
// run_control_loop ignores missed modulation timing updates
// if and only if we're in AXIS_STATE_IDLE
safety_critical_disarm_motor_pwm(motor_);
set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
run_control_loop([this]() {
return true;
});
@@ -549,9 +534,12 @@ void Axis::run_state_machine_loop() {
}
// If the state failed, go to idle, else advance task chain
if (!status)
if (!status) {
std::fill(task_chain_.begin(), task_chain_.end(), AXIS_STATE_UNDEFINED);
current_state_ = AXIS_STATE_IDLE;
else
memmove(task_chain_, task_chain_ + 1, sizeof(task_chain_) - sizeof(task_chain_[0]));
} else {
std::rotate(task_chain_.begin(), task_chain_.begin() + 1, task_chain_.end());
task_chain_.back() = AXIS_STATE_UNDEFINED;
}
}
}
Firmware/MotorControl/axis.hpp
+16 -10
View File
@@ -24,8 +24,6 @@ public:
ERROR_MIN_ENDSTOP_PRESSED = 0x1000,
ERROR_MAX_ENDSTOP_PRESSED = 0x2000,
ERROR_ESTOP_REQUESTED = 0x4000,
ERROR_DC_BUS_UNDER_CURRENT = 0x8000, // too much current pushed into the power supply
ERROR_DC_BUS_OVER_CURRENT = 0x10000, // too much current pulled out of the power supply
ERROR_HOMING_WITHOUT_ENDSTOP = 0x20000, // the min endstop was not enabled during homing
};
@@ -68,11 +66,17 @@ public:
bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
bool startup_homing = false; //<! enable homing after calibration/startup
bool enable_step_dir = false; //<! enable step/dir input after calibration
// For M0 this has no effect if enable_uart is true
bool step_dir_always_on = false; //<! Keep step/dir enabled while the motor is disabled.
//<! This is ignored if enable_step_dir is false.
//<! This setting only takes effect on a state transition
//<! into idle or out of closed loop control.
float counts_per_step = 2.0f;
float watchdog_timeout = 0.0f; // [s] (0 disables watchdog)
float watchdog_timeout = 0.0f; // [s]
bool enable_watchdog = false;
// Defaults loaded from hw_config in load_configuration in main.cpp
@@ -82,7 +86,8 @@ public:
LockinConfig_t calibration_lockin = default_calibration();
LockinConfig_t sensorless_ramp = default_sensorless();
LockinConfig_t lockin;
uint8_t can_node_id = 0; // Both axes will have the same id to start
uint32_t can_node_id = 0; // Both axes will have the same id to start
bool can_node_id_extended = false;
uint32_t can_heartbeat_rate_ms = 100;
};
@@ -120,7 +125,6 @@ public:
void step_cb();
void set_step_dir_active(bool enable);
void decode_step_dir_pins();
void update_watchdog_settings();
static void load_default_step_dir_pin_config(
const AxisHardwareConfig_t& hw_config, Config_t* config);
@@ -135,10 +139,10 @@ public:
bool watchdog_check();
void clear_errors() {
motor_.error_ = Motor::ERROR_NONE;
controller_.error_ = Controller::ERROR_NONE;
motor_.error_ = Motor::ERROR_NONE;
controller_.error_ = Controller::ERROR_NONE;
sensorless_estimator_.error_ = SensorlessEstimator::ERROR_NONE;
encoder_.error_ = Encoder::ERROR_NONE;
encoder_.error_ = Encoder::ERROR_NONE;
error_ = Axis::ERROR_NONE;
}
@@ -248,8 +252,8 @@ public:
uint16_t dir_pin_;
State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
State_t& current_state_ = task_chain_[0];
std::array<State_t, 10> task_chain_ = { AXIS_STATE_UNDEFINED };
State_t& current_state_ = task_chain_.front();
uint32_t loop_counter_ = 0;
LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
Homing_t homing_;
@@ -276,6 +280,7 @@ public:
make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
make_protocol_property("startup_homing", &config_.startup_homing),
make_protocol_property("enable_step_dir", &config_.enable_step_dir),
make_protocol_property("step_dir_always_on", &config_.step_dir_always_on),
make_protocol_property("counts_per_step", &config_.counts_per_step),
make_protocol_property("watchdog_timeout", &config_.watchdog_timeout),
make_protocol_property("enable_watchdog", &config_.enable_watchdog),
@@ -310,6 +315,7 @@ public:
make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance),
make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)),
make_protocol_property("can_node_id", &config_.can_node_id),
make_protocol_property("can_node_id_extended", &config_.can_node_id_extended),
make_protocol_property("can_heartbeat_rate_ms", &config_.can_heartbeat_rate_ms)),
make_protocol_object("motor", motor_.make_protocol_definitions()),
make_protocol_object("controller", controller_.make_protocol_definitions()),
Firmware/MotorControl/controller.cpp
+13 -14
View File
@@ -54,7 +54,7 @@ void Controller::move_to_pos(float goal_point) {
axis_->trap_.config_.vel_limit,
axis_->trap_.config_.accel_limit,
axis_->trap_.config_.decel_limit);
traj_start_loop_count_ = axis_->loop_counter_;
axis_->trap_.t_ = 0.0f;
trajectory_done_ = false;
}
@@ -110,7 +110,8 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate)
}
void Controller::update_filter_gains() {
input_filter_ki_ = 2.0f * config_.input_filter_bandwidth; // basic conversion to discrete time
float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz);
input_filter_ki_ = 2.0f * bandwidth; // basic conversion to discrete time
input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped
}
@@ -160,12 +161,12 @@ bool Controller::update(float* current_setpoint_output) {
float step = std::clamp(full_step, -max_step_size, max_step_size);
vel_setpoint_ += step;
current_setpoint_ = step / current_meas_period * config_.inertia;
current_setpoint_ = (step / current_meas_period) * config_.inertia;
} break;
case INPUT_MODE_CURRENT_RAMP: {
float max_step_size = std::abs(current_meas_period * config_.current_ramp_rate);
float full_step = input_current_ - current_setpoint_;
float step = std::clamp(full_step, -max_step_size, max_step_size);
float full_step = input_current_ - current_setpoint_;
float step = std::clamp(full_step, -max_step_size, max_step_size);
current_setpoint_ += step;
} break;
@@ -175,7 +176,7 @@ bool Controller::update(float* current_setpoint_output) {
float delta_vel = input_vel_ - vel_setpoint_; // Vel error
float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback
current_setpoint_ = accel * config_.inertia; // Accel
vel_setpoint_ += std::clamp(current_meas_period * accel, 2.0f * std::abs(delta_vel), -2.0f * std::abs(delta_vel)); // delta vel
vel_setpoint_ += current_meas_period * accel; // delta vel
pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos
} break;
case INPUT_MODE_MIRROR: {
@@ -198,10 +199,8 @@ bool Controller::update(float* current_setpoint_output) {
// Avoid updating uninitialized trajectory
if (trajectory_done_)
break;
// Note: uint32_t loop count delta is OK across overflow
// Beware of negative deltas, as they will not be well behaved due to uint!
float t = (axis_->loop_counter_ - traj_start_loop_count_) * current_meas_period;
if (t > axis_->trap_.Tf_) {
if (axis_->trap_.t_ > axis_->trap_.Tf_) {
// Drop into position control mode when done to avoid problems on loop counter delta overflow
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
pos_setpoint_ = input_pos_;
@@ -209,10 +208,11 @@ bool Controller::update(float* current_setpoint_output) {
current_setpoint_ = 0.0f;
trajectory_done_ = true;
} else {
TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t);
TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(axis_->trap_.t_);
pos_setpoint_ = traj_step.Y;
vel_setpoint_ = traj_step.Yd;
current_setpoint_ = traj_step.Ydd * config_.inertia;
axis_->trap_.t_ += current_meas_period;
}
anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate
} break;
@@ -256,8 +256,7 @@ bool Controller::update(float* current_setpoint_output) {
// Velocity limiting
float vel_lim = config_.vel_limit;
if (config_.enable_vel_limit) {
if (vel_des > vel_lim) vel_des = vel_lim;
if (vel_des < -vel_lim) vel_des = -vel_lim;
vel_des = std::clamp(vel_des, -vel_lim, vel_lim);
}
// Check for overspeed fault (done in this module (controller) for cohesion with vel_lim)
@@ -294,7 +293,7 @@ bool Controller::update(float* current_setpoint_output) {
// 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_valid_ && config_.anticogging.enable) {
Iq += config_.anticogging.cogging_map[std::clamp(mod(static_cast<int>(anticogging_pos), 3600), 0, 3600)];
Iq += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)];
}
float v_err = 0.0f;
Firmware/MotorControl/controller.hpp
+22 -23
View File
@@ -44,34 +44,34 @@ public:
bool calib_anticogging = false;
float calib_pos_threshold = 1.0f;
float calib_vel_threshold = 1.0f;
float cogging_ratio = 1.0f;
bool enable = true;
float cogging_ratio = 1.0f;
bool enable = true;
} Anticogging_t;
struct Config_t {
ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: ControlMode_t
InputMode_t input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t
float pos_gain = 20.0f; // [(counts/s) / counts]
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
// float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
float vel_limit = 20000.0f; // [counts/s] Infinity to disable.
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable.
float vel_ramp_rate = 10000.0f; // [(counts/s) / s]
float current_ramp_rate = 1.0f; // A / sec
bool setpoints_in_cpr = false;
float inertia = 0.0f; // [A/(count/s^2)]
float input_filter_bandwidth = 2.0f; // [1/s]
float homing_speed = 2000.0f; // [counts/s]
float pos_gain = 20.0f; // [(counts/s) / counts]
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
// float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
float vel_limit = 20000.0f; // [counts/s] Infinity to disable.
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable.
float vel_ramp_rate = 10000.0f; // [(counts/s) / s]
float current_ramp_rate = 1.0f; // A / sec
bool setpoints_in_cpr = false;
float inertia = 0.0f; // [A/(count/s^2)]
float input_filter_bandwidth = 2.0f; // [1/s]
float homing_speed = 2000.0f; // [counts/s]
Anticogging_t anticogging;
float gain_scheduling_width = 10.0f;
bool enable_gain_scheduling = false;
bool enable_vel_limit = true;
bool enable_overspeed_error = true;
bool enable_current_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
uint8_t axis_to_mirror = -1;
float mirror_ratio = 1.0f;
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration()
float gain_scheduling_width = 10.0f;
bool enable_gain_scheduling = false;
bool enable_vel_limit = true;
bool enable_overspeed_error = true;
bool enable_current_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
uint8_t axis_to_mirror = -1;
float mirror_ratio = 1.0f;
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration()
};
explicit Controller(Config_t& config);
@@ -117,7 +117,6 @@ public:
bool input_pos_updated_ = false;
uint32_t traj_start_loop_count_ = 0;
bool trajectory_done_ = true;
bool anticogging_valid_ = false;
Firmware/MotorControl/encoder.cpp
+72 -48
View File
@@ -3,7 +3,7 @@
Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
Config_t& config, Motor::Config_t motor_config) :
Config_t& config, const Motor::Config_t& motor_config) :
hw_config_(hw_config),
config_(config)
{
@@ -109,8 +109,8 @@ void Encoder::set_linear_count(int32_t count) {
uint32_t prim = cpu_enter_critical();
// Update states
shadow_count_ = count;
pos_estimate_ = static_cast<float>(count);
shadow_count_ = count;
pos_estimate_ = (float)count;
tim_cnt_sample_ = count;
//Write hardware last
@@ -132,7 +132,7 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
// Update states
count_in_cpr_ = mod(count, config_.cpr);
pos_cpr_ = static_cast<float>(count_in_cpr_);
pos_cpr_ = (float)count_in_cpr_;
cpu_exit_critical(prim);
}
@@ -151,11 +151,14 @@ bool Encoder::run_index_search() {
bool Encoder::run_direction_find() {
int32_t init_enc_val = shadow_count_;
bool orig_finish_on_distance = axis_->config_.calibration_lockin.finish_on_distance;
axis_->config_.calibration_lockin.finish_on_distance = true;
axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin);
axis_->config_.calibration_lockin.finish_on_distance = orig_finish_on_distance;
Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
lockin_config.finish_distance = lockin_config.vel * 3.0f; // run for 3 seconds
lockin_config.finish_on_distance = true;
lockin_config.finish_on_enc_idx = false;
lockin_config.finish_on_vel = false;
bool status = axis_->run_lockin_spin(lockin_config);
if (status) {
// Check response and direction
@@ -179,7 +182,7 @@ bool Encoder::run_direction_find() {
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
static const float start_lock_duration = 1.0f;
static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * static_cast<float>(current_meas_hz));
static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);
// Require index found if enabled
if (config_.use_index && !index_found_) {
@@ -216,7 +219,7 @@ bool Encoder::run_offset_calibration() {
// scan forward
i = 0;
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(config_.calib_scan_distance * static_cast<float>(i) / static_cast<float>(num_steps) - config_.calib_scan_distance / 2.0f);
float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
@@ -245,9 +248,9 @@ bool Encoder::run_offset_calibration() {
//TODO avoid recomputing elec_rad_per_enc every time
// Check CPR
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast<float>(config_.cpr));
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
return false;
@@ -256,7 +259,7 @@ bool Encoder::run_offset_calibration() {
// scan backwards
i = 0;
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(-config_.calib_scan_distance * static_cast<float>(i) / static_cast<float>(num_steps) + config_.calib_scan_distance / 2.0f);
float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
@@ -270,9 +273,9 @@ bool Encoder::run_offset_calibration() {
if (axis_->error_ != Axis::ERROR_NONE)
return false;
config_.offset = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
config_.offset_float = static_cast<float>(residual) / static_cast<float>(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase
config_.offset = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase
is_ready_ = true;
return true;
@@ -301,13 +304,15 @@ void Encoder::sample_now() {
} break;
case MODE_SINCOS: {
sincos_sample_s_ = (get_adc_voltage(GPIO_3_GPIO_Port, GPIO_3_Pin) / 3.3f) - 0.5f;
sincos_sample_c_ = (get_adc_voltage(GPIO_4_GPIO_Port, GPIO_4_Pin) / 3.3f) - 0.5f;
sincos_sample_s_ = (get_adc_voltage(get_gpio_port_by_pin(config_.sincos_gpio_pin_sin), get_gpio_pin_by_pin(config_.sincos_gpio_pin_sin)) / 3.3f) - 0.5f;
sincos_sample_c_ = (get_adc_voltage(get_gpio_port_by_pin(config_.sincos_gpio_pin_cos), get_gpio_pin_by_pin(config_.sincos_gpio_pin_cos)) / 3.3f) - 0.5f;
} break;
case MODE_SPI_ABS_AMS:
case MODE_SPI_ABS_CUI:
case MODE_SPI_ABS_AEAT:
{
axis_->motor_.log_timing(Motor::TIMING_LOG_SAMPLE_NOW);
// Do nothing
} break;
@@ -329,12 +334,12 @@ bool Encoder::abs_spi_init(){
spi->Init.CLKPhase = SPI_PHASE_2EDGE;
spi->Init.NSS = SPI_NSS_SOFT;
spi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
spi->Init.FirstBit = SPI_FIRSTBIT_MSB;
spi->Init.TIMode = SPI_TIMODE_DISABLE;
spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
spi->Init.CRCPolynomial = 10;
spi->Init.FirstBit = SPI_FIRSTBIT_MSB;
spi->Init.TIMode = SPI_TIMODE_DISABLE;
spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
spi->Init.CRCPolynomial = 10;
if (mode_ == MODE_SPI_ABS_AEAT) {
spi->Init.CLKPolarity = SPI_POLARITY_HIGH;
spi->Init.CLKPolarity = SPI_POLARITY_HIGH;
}
HAL_SPI_DeInit(spi);
HAL_SPI_Init(spi);
@@ -343,17 +348,18 @@ bool Encoder::abs_spi_init(){
bool Encoder::abs_spi_start_transaction(){
if (mode_ & MODE_FLAG_ABS){
axis_->motor_.log_timing(Motor::TIMING_LOG_SPI_START);
if(hw_config_.spi->State != HAL_SPI_STATE_READY){
set_error(ERROR_ABS_SPI_NOT_READY);
return false;
}
HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_RESET);
HAL_SPI_TransmitReceive_DMA(hw_config_.spi,(uint8_t*)abs_spi_dma_tx_,(uint8_t*)abs_spi_dma_rx_,1);
HAL_SPI_TransmitReceive_DMA(hw_config_.spi, (uint8_t*)abs_spi_dma_tx_, (uint8_t*)abs_spi_dma_rx_, 1);
}
return true;
}
uint8_t parity(uint16_t v){
uint8_t ams_parity(uint16_t v) {
v ^= v >> 8;
v ^= v >> 4;
v ^= v >> 2;
@@ -361,29 +367,50 @@ uint8_t parity(uint16_t v){
return v & 1;
}
uint8_t cui_parity(uint16_t v) {
v ^= v >> 8;
v ^= v >> 4;
v ^= v >> 2;
return ~v & 3;
}
void Encoder::abs_spi_cb(){
HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_SET);
axis_->motor_.log_timing(Motor::TIMING_LOG_SPI_END);
uint16_t pos;
switch (mode_) {
case MODE_SPI_ABS_AMS: {
uint8_t parity_calc, parity_bit;
auto rawVal = abs_spi_dma_rx_[0];
parity_calc = parity(rawVal & 0x7FFF);
parity_bit = rawVal >> 15;
if (parity_calc == parity_bit) {
pos_abs_ = rawVal & 0x3FFF;
abs_spi_pos_updated_ = true;
uint16_t rawVal = abs_spi_dma_rx_[0];
// check if parity is correct (even) and error flag clear
if (ams_parity(rawVal) || ((rawVal >> 14) & 1)) {
return;
}
pos = rawVal & 0x3fff;
} break;
case MODE_SPI_ABS_AEAT: {
pos_abs_ = abs_spi_dma_rx_[0];
abs_spi_pos_updated_ = true;
case MODE_SPI_ABS_CUI: {
uint16_t rawVal = abs_spi_dma_rx_[0];
// check if parity is correct
if (cui_parity(rawVal)) {
return;
}
pos = rawVal & 0x3fff;
} break;
default: {
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
return;
} break;
}
is_ready_ = true;
pos_abs_ = pos;
abs_spi_pos_updated_ = true;
if (config_.pre_calibrated) {
is_ready_ = true;
}
}
void Encoder::abs_spi_cs_pin_init(){
@@ -445,7 +472,7 @@ bool Encoder::update() {
case MODE_SPI_ABS_AMS:
case MODE_SPI_ABS_CUI:
case MODE_SPI_ABS_AEAT: {
if (abs_spi_pos_updated_ == false && abs_spi_pos_init_once_) {
if (abs_spi_pos_updated_ == false) {
// Low pass filter the error
spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
if (spi_error_rate_ > 0.005f)
@@ -461,9 +488,6 @@ bool Encoder::update() {
if (delta_enc > config_.cpr/2) {
delta_enc -= config_.cpr;
}
if (!abs_spi_pos_init_once_ && delta_enc != 0) {
abs_spi_pos_init_once_ = true;
}
}break;
default: {
@@ -484,17 +508,17 @@ bool Encoder::update() {
pos_estimate_ += current_meas_period * vel_estimate_;
pos_cpr_ += current_meas_period * vel_estimate_;
// discrete phase detector
float delta_pos = static_cast<float>(shadow_count_) - static_cast<int32_t>(std::floor(pos_estimate_));
float delta_pos_cpr = static_cast<float>(count_in_cpr_) - static_cast<int32_t>(std::floor(pos_cpr_));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast<float>(config_.cpr));
float delta_pos = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_));
float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr));
// pll feedback
pos_estimate_ += current_meas_period * pll_kp_ * delta_pos;
pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr;
pos_cpr_ = fmodf_pos(pos_cpr_, static_cast<float>(config_.cpr));
pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr));
vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr;
bool snap_to_zero_vel = false;
if (std::abs(vel_estimate_) < 0.5f * current_meas_period * pll_ki_) {
vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter
vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter
snap_to_zero_vel = true;
}
@@ -519,8 +543,8 @@ bool Encoder::update() {
//// compute electrical phase
//TODO avoid recomputing elec_rad_per_enc every time
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast<float>(config_.cpr));
float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
// ph = fmodf(ph, 2*M_PI);
phase_ = wrap_pm_pi(ph);
Firmware/MotorControl/encoder.hpp
+15 -12
View File
@@ -8,10 +8,10 @@
class Encoder {
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_UNSTABLE_GAIN = 0x01,
ERROR_CPR_POLEPAIRS_MISMATCH = 0x02,
ERROR_NO_RESPONSE = 0x04,
ERROR_NONE = 0,
ERROR_UNSTABLE_GAIN = 0x01,
ERROR_CPR_POLEPAIRS_MISMATCH = 0x02,
ERROR_NO_RESPONSE = 0x04,
ERROR_UNSUPPORTED_ENCODER_MODE = 0x08,
ERROR_ILLEGAL_HALL_STATE = 0x10,
ERROR_INDEX_NOT_FOUND_YET = 0x20,
@@ -24,9 +24,9 @@ public:
MODE_INCREMENTAL,
MODE_HALL,
MODE_SINCOS,
MODE_SPI_ABS_CUI = 0x100,
MODE_SPI_ABS_AMS = 0x101,
MODE_SPI_ABS_AEAT = 0x102,
MODE_SPI_ABS_CUI = 0x100, //!< compatible with CUI AMT23xx
MODE_SPI_ABS_AMS = 0x101, //!< compatible with AMS AS5047P, AS5048A/AS5048B (no daisy chain support)
MODE_SPI_ABS_AEAT = 0x102, //!< not yet implemented
};
const uint32_t MODE_FLAG_ABS = 0x100;
@@ -51,10 +51,12 @@ public:
bool idx_search_unidirectional = false; // Only allow index search in known direction
bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
uint16_t abs_spi_cs_gpio_pin = 1;
uint16_t sincos_gpio_pin_sin = 3;
uint16_t sincos_gpio_pin_cos = 4;
};
Encoder(const EncoderHardwareConfig_t& hw_config,
Config_t& config, Motor::Config_t motor_config);
Config_t& config, const Motor::Config_t& motor_config);
void setup();
void set_error(Error_t error);
@@ -108,10 +110,9 @@ public:
bool abs_spi_start_transaction();
void abs_spi_cb();
void abs_spi_cs_pin_init();
uint16_t abs_spi_dma_tx_[2] = {0xFFFF, 0x0000};
uint16_t abs_spi_dma_rx_[2];
uint16_t abs_spi_dma_tx_[1] = {0xFFFF};
uint16_t abs_spi_dma_rx_[1];
bool abs_spi_pos_updated_ = false;
bool abs_spi_pos_init_once_ = false;
Mode_t mode_ = MODE_INCREMENTAL;
GPIO_TypeDef* abs_spi_cs_port_;
uint16_t abs_spi_cs_pin_;
@@ -161,7 +162,9 @@ public:
make_protocol_property("calib_scan_distance", &config_.calib_scan_distance),
make_protocol_property("calib_scan_omega", &config_.calib_scan_omega),
make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional),
make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state)
make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state),
make_protocol_property("sincos_gpio_pin_sin", &config_.sincos_gpio_pin_sin),
make_protocol_property("sincos_gpio_pin_cos", &config_.sincos_gpio_pin_cos)
),
make_protocol_function("set_linear_count", *this, &Encoder::set_linear_count, "count")
);
Firmware/MotorControl/endstop.cpp
+20 -15
View File
@@ -3,24 +3,25 @@
Endstop::Endstop(Endstop::Config_t& config)
: config_(config) {
update_config();
debounceTimer_.setIncrement(current_meas_period);
}
void Endstop::update() {
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
bool last_pin_state = pin_state_;
pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
float now = axis_->loop_counter_ * current_meas_period;
if (pin_state_ != last_pin_state) {
debounce_timer_ = now;
}
debounceTimer_.update();
if (config_.enabled) {
if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state
bool last_pin_state = pin_state_;
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
// If the pin state has changed, reset the timer
if (pin_state_ != last_pin_state)
debounceTimer_.reset();
if (debounceTimer_.expired())
endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state
debounce_timer_ = now - (config_.debounce_ms * 0.001f); // Ensure timer doesn't have overflow issues
} else {
endstop_state_ = endstop_state_; // Do nothing
}
} else {
endstop_state_ = false;
}
@@ -30,11 +31,13 @@ bool Endstop::get_state() {
return endstop_state_;
}
void Endstop::update_config(){
void Endstop::update_config() {
set_enabled(config_.enabled);
debounceTimer_.setIncrement(config_.debounce_ms * 0.001f);
}
void Endstop::set_enabled(bool enable) {
debounceTimer_.reset();
if (config_.gpio_num != 0) {
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
@@ -45,6 +48,8 @@ void Endstop::set_enabled(bool enable) {
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = config_.pullup ? GPIO_PULLUP : GPIO_PULLDOWN;
HAL_GPIO_Init(gpio_port, &GPIO_InitStruct);
}
debounceTimer_.start();
} else
debounceTimer_.stop();
}
}
Firmware/MotorControl/endstop.hpp
+8 -6
View File
@@ -1,15 +1,16 @@
#ifndef __ENDSTOP_HPP
#define __ENDSTOP_HPP
#include "timer.hpp"
class Endstop {
public:
struct Config_t {
float offset = 0;
float debounce_ms = 50.0f;
uint32_t debounce_ms = 50;
uint16_t gpio_num = 0;
bool enabled = false;
bool is_active_high = false;
bool pullup = false;
bool pullup = true;
};
explicit Endstop(Endstop::Config_t& config);
@@ -36,12 +37,13 @@ class Endstop {
make_protocol_property("offset", &config_.offset),
make_protocol_property("is_active_high", &config_.is_active_high),
make_protocol_property("pullup", &config_.pullup),
make_protocol_property("debounce_ms", &config_.debounce_ms)));
make_protocol_property("debounce_ms", &config_.debounce_ms,
[](void* ctx) { static_cast<Endstop*>(ctx)->update_config(); }, this)));
}
private:
bool pin_state_ = false;
float pos_when_pressed_ = 0.0f;
volatile float debounce_timer_ = 0;
bool pin_state_ = false;
float pos_when_pressed_ = 0.0f;
Timer<float> debounceTimer_;
};
#endif
Firmware/MotorControl/gpio_utils.hpp
+46
View File
@@ -0,0 +1,46 @@
#pragma once
#include "gpio.h"
constexpr GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){
switch(GPIO_pin){
case 1: return GPIO_1_GPIO_Port; break;
case 2: return GPIO_2_GPIO_Port; break;
case 3: return GPIO_3_GPIO_Port; break;
case 4: return GPIO_4_GPIO_Port; break;
#ifdef GPIO_5_GPIO_Port
case 5: return GPIO_5_GPIO_Port; break;
#endif
#ifdef GPIO_6_GPIO_Port
case 6: return GPIO_6_GPIO_Port; break;
#endif
#ifdef GPIO_7_GPIO_Port
case 7: return GPIO_7_GPIO_Port; break;
#endif
#ifdef GPIO_8_GPIO_Port
case 8: return GPIO_8_GPIO_Port; break;
#endif
default: return GPIO_1_GPIO_Port;
}
}
constexpr uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){
switch(GPIO_pin){
case 1: return GPIO_1_Pin; break;
case 2: return GPIO_2_Pin; break;
case 3: return GPIO_3_Pin; break;
case 4: return GPIO_4_Pin; break;
#ifdef GPIO_5_Pin
case 5: return GPIO_5_Pin; break;
#endif
#ifdef GPIO_6_Pin
case 6: return GPIO_6_Pin; break;
#endif
#ifdef GPIO_7_Pin
case 7: return GPIO_7_Pin; break;
#endif
#ifdef GPIO_8_Pin
case 8: return GPIO_8_Pin; break;
#endif
default: return GPIO_1_Pin;
}
}
Firmware/MotorControl/low_level.cpp
+58 -13
View File
@@ -35,7 +35,9 @@ const float adc_ref_voltage = 3.3f;
// 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;
float ibus_ = 0.0f; // exposed for monitoring only
bool brake_resistor_armed = false;
bool brake_resistor_saturated = false;
/* Private constant data -----------------------------------------------------*/
static const GPIO_TypeDef* GPIOs_to_samp[] = { GPIOA, GPIOB, GPIOC };
static const int num_GPIO = sizeof(GPIOs_to_samp) / sizeof(GPIOs_to_samp[0]);
@@ -212,6 +214,7 @@ void start_adc_pwm() {
// Ensure that debug halting of the core doesn't leave the motor PWM running
__HAL_DBGMCU_FREEZE_TIM1();
__HAL_DBGMCU_FREEZE_TIM8();
__HAL_DBGMCU_FREEZE_TIM13();
start_pwm(&htim1);
start_pwm(&htim8);
@@ -259,6 +262,20 @@ void start_pwm(TIM_HandleTypeDef* htim) {
HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4);
}
/*
* Initial intention of this function:
* Synchronize TIM1, TIM8 and TIM13 such that:
* 1. The triangle waveform of TIM1 leads the triangle waveform of TIM8 by a
* 90° phase shift.
* 2. The timer update events of TIM1 and TIM8 are symmetrically interleaved.
* 3. Each TIM13 reload coincides with a TIM1 lower update event.
*
* However right now this function only ensures point (1) and (3) but because
* TIM1 and TIM3 only trigger an update on every third reload, this does not
* imply (or even allow for) (2).
*
* TODO: revisit the timing topic in general.
*/
void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset,
TIM_HandleTypeDef* htim_refbase) {
@@ -492,6 +509,10 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
else if (&axis == axes[0] && !counting_down)
update_timings = true; // update timings of M1
// TODO: this is out of place here. However when moving it somewhere
// else we have to consider the timing requirements to prevent the SPI
// transfers of axis0 and axis1 from conflicting.
// Also see comment on sync_timers.
if((current_meas_not_DC_CAL && !axis_num) ||
(axis_num && !current_meas_not_DC_CAL)){
axis.encoder_.abs_spi_start_transaction();
@@ -592,20 +613,44 @@ void update_brake_current() {
}
// Don't start braking until -Ibus > regen_current_allowed
float brake_current = std::max(-Ibus_sum - board_config.max_regen_current, 0.0f);
float brake_duty = std::max(brake_current * std::abs(board_config.brake_resistance) / vbus_voltage, 0.0f);
// Duty limit at 90% to allow bootstrap caps to charge
// If brake_duty is NaN, this expression will also evaluate to false
if ((brake_duty >= 0.0f) && (brake_duty <= 0.9f)) {
int high_on = static_cast<int>(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty));
int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS;
if (low_off < 0) low_off = 0;
safety_critical_apply_brake_resistor_timings(low_off, high_on);
} else {
//shuts off all motors AND brake resistor, sets error code on all motors.
low_level_fault(Motor::ERROR_BRAKE_CURRENT_OUT_OF_RANGE);
float brake_current = -Ibus_sum - board_config.max_regen_current;
float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage;
if (board_config.enable_dc_bus_overvoltage_ramp && (board_config.brake_resistance > 0.0f) && (board_config.dc_bus_overvoltage_ramp_start < board_config.dc_bus_overvoltage_ramp_end)) {
brake_duty += std::fmax((vbus_voltage - board_config.dc_bus_overvoltage_ramp_start) / (board_config.dc_bus_overvoltage_ramp_end - board_config.dc_bus_overvoltage_ramp_start), 0.0f);
}
if (std::isnan(brake_duty)) {
// Shuts off all motors AND brake resistor, sets error code on all motors.
low_level_fault(Motor::ERROR_BRAKE_DUTY_CYCLE_NAN);
return;
}
if (brake_duty >= 0.95f) {
brake_resistor_saturated = true;
}
// Duty limit at 95% to allow bootstrap caps to charge
brake_duty = std::clamp(brake_duty, 0.0f, 0.95f);
// Special handling to avoid the case 0.0/0.0 == NaN.
Ibus_sum += brake_duty ? (brake_duty * vbus_voltage / board_config.brake_resistance) : 0.0f;
ibus_ = Ibus_sum;
if (Ibus_sum > board_config.dc_max_positive_current) {
low_level_fault(Motor::ERROR_DC_BUS_OVER_CURRENT);
return;
}
if (Ibus_sum < board_config.dc_max_negative_current) {
low_level_fault(Motor::ERROR_DC_BUS_OVER_REGEN_CURRENT);
return;
}
int high_on = (int)(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty));
int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS;
if (low_off < 0) low_off = 0;
safety_critical_apply_brake_resistor_timings(low_off, high_on);
}
Firmware/MotorControl/low_level.h
+2
View File
@@ -22,7 +22,9 @@ extern const float adc_full_scale;
extern const float adc_ref_voltage;
/* Exported variables --------------------------------------------------------*/
extern float vbus_voltage;
extern float ibus_;
extern bool brake_resistor_armed;
extern bool brake_resistor_saturated;
extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT];
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
Firmware/MotorControl/main.cpp
+15 -3
View File
@@ -3,6 +3,7 @@
#include "odrive_main.h"
#include "nvm_config.hpp"
#include "usart.h"
#include "freertos_vars.h"
#include <communication/interface_usb.h>
#include <communication/interface_uart.h>
@@ -23,7 +24,7 @@ bool user_config_loaded_;
SystemStats_t system_stats_;
Axis *axes[AXIS_COUNT];
std::array<Axis*, AXIS_COUNT> axes;
ODriveCAN *odCAN = nullptr;
typedef Config<
@@ -95,6 +96,13 @@ extern "C" int load_configuration(void) {
void erase_configuration(void) {
NVM_erase();
// FIXME: this reboot is a workaround because we don't want the next save_configuration
// to write back the old configuration from RAM to NVM. The proper action would
// be to reset the values in RAM to default. However right now that's not
// practical because several startup actions depend on the config. The
// other problem is that the stack overflows if we reset to default here.
NVIC_SystemReset();
}
void enter_dfu_mode() {
@@ -116,7 +124,7 @@ void enter_dfu_mode() {
}
extern "C" int construct_objects(){
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
if (board_config.enable_i2c_instead_of_can) {
// Set up the direction GPIO as input
GPIO_InitTypeDef GPIO_InitStruct;
@@ -140,6 +148,10 @@ extern "C" int construct_objects(){
#endif
MX_CAN1_Init();
HAL_UART_DeInit(&huart4);
huart4.Init.BaudRate = board_config.uart_baudrate;
HAL_UART_Init(&huart4);
// Init general user ADC on some GPIOs.
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
@@ -235,7 +247,7 @@ int odrive_main(void) {
axes[i]->setup();
}
for(auto axis : axes){
for(auto& axis : axes){
axis->encoder_.setup();
}
Firmware/MotorControl/motor.cpp
+14 -26
View File
@@ -150,8 +150,7 @@ float Motor::get_inverter_temp() {
return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs);
}
bool Motor::update_thermal_limits() {
float fet_temp = get_inverter_temp();
bool Motor::update_thermal_limits(float fet_temp) {
float temp_margin = config_.inverter_temp_limit_upper - fet_temp;
float derating_range = config_.inverter_temp_limit_upper - config_.inverter_temp_limit_lower;
thermal_current_lim_ = config_.current_lim * (temp_margin / derating_range);
@@ -170,7 +169,8 @@ bool Motor::do_checks() {
set_error(ERROR_DRV_FAULT);
return false;
}
if (!update_thermal_limits()) {
inverter_temp_ = get_inverter_temp();
if (!update_thermal_limits(inverter_temp_)) {
//error already set in function
return false;
}
@@ -216,7 +216,7 @@ float Motor::phase_current_from_adcval(uint32_t ADCValue) {
// 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 = static_cast<int>(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
static const int num_test_cycles = (int)(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
float test_voltage = 0.0f;
size_t i = 0;
@@ -329,7 +329,7 @@ bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) {
float c = our_arm_cos_f32(pwm_phase);
float s = our_arm_sin_f32(pwm_phase);
float v_alpha = c*v_d - s*v_q;
float v_beta = c*v_q + s*v_d;
float v_beta = c*v_q + s*v_d;
return enqueue_voltage_timings(v_alpha, v_beta);
}
@@ -400,7 +400,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
float c_p = our_arm_cos_f32(pwm_phase);
float s_p = our_arm_sin_f32(pwm_phase);
float mod_alpha = c_p * mod_d - s_p * mod_q;
float mod_beta = c_p * mod_q + s_p * mod_d;
float mod_beta = c_p * mod_q + s_p * mod_d;
// Report final applied voltage in stationary frame (for sensorles estimator)
ictrl.final_v_alpha = mod_to_V * mod_alpha;
@@ -447,9 +447,8 @@ bool Motor::update(float current_setpoint, float phase, float phase_vel) {
// TODO: 2-norm vs independent clamping (current could be sqrt(2) bigger)
float ilim = effective_current_lim();
// TODO: use std::clamp (C++17)
float id = MACRO_MIN(MACRO_MAX(current_control_.Id_setpoint, -ilim), ilim);
float iq = MACRO_MIN(MACRO_MAX(current_setpoint, -ilim), ilim);
float id = std::clamp(current_control_.Id_setpoint, -ilim, ilim);
float iq = std::clamp(current_setpoint, -ilim, ilim);
if (config_.motor_type == MOTOR_TYPE_ACIM) {
// Note that the effect of the current commands on the real currents is actually 1.5 PWM cycles later
@@ -460,7 +459,7 @@ bool Motor::update(float current_setpoint, float phase, float phase_vel) {
float abs_iq = fabsf(iq);
float gain = abs_iq > id ? config_.acim_autoflux_attack_gain : config_.acim_autoflux_decay_gain;
id += gain * (abs_iq - id) * current_meas_period;
id = MACRO_MIN(MACRO_MAX(id, config_.acim_autoflux_min_Id), ilim);
id = std::clamp(id, config_.acim_autoflux_min_Id, ilim);
current_control_.Id_setpoint = id;
}
@@ -485,22 +484,11 @@ bool Motor::update(float current_setpoint, float phase, float phase_vel) {
float pwm_phase = phase + 1.5f * current_meas_period * phase_vel;
// Execute current command
// TODO: move this into the mot
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
if(!FOC_current(id, iq, phase, pwm_phase)){
return false;
}
} else if (config_.motor_type == MOTOR_TYPE_ACIM) {
if(!FOC_current(id, iq, phase, pwm_phase)){
return false;
}
} else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
//In gimbal motor mode, current is reinterptreted as voltage.
if(!FOC_voltage(id, iq, pwm_phase))
return false;
} else {
set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE);
return false;
switch(config_.motor_type){
case MOTOR_TYPE_HIGH_CURRENT: return FOC_current(id, iq, phase, pwm_phase); break;
case MOTOR_TYPE_ACIM: return FOC_current(id, iq, phase, pwm_phase); break;
case MOTOR_TYPE_GIMBAL: return FOC_voltage(id, iq, pwm_phase); break;
default: set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE); return false; break;
}
return true;
}
Firmware/MotorControl/motor.hpp
+21 -7
View File
@@ -23,7 +23,10 @@ public:
ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200,
ERROR_CURRENT_SENSE_SATURATION = 0x0400,
ERROR_INVERTER_OVER_TEMP = 0x0800,
ERROR_CURRENT_LIMIT_VIOLATION = 0x1000
ERROR_CURRENT_LIMIT_VIOLATION = 0x1000,
ERROR_BRAKE_DUTY_CYCLE_NAN = 0x2000,
ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x4000, // too much current pushed into the power supply
ERROR_DC_BUS_OVER_CURRENT = 0x8000, // too much current pulled out of the power supply
};
enum MotorType_t {
@@ -98,6 +101,9 @@ public:
TIMING_LOG_IDX_SEARCH,
TIMING_LOG_FOC_VOLTAGE,
TIMING_LOG_FOC_CURRENT,
TIMING_LOG_SPI_START,
TIMING_LOG_SAMPLE_NOW,
TIMING_LOG_SPI_END,
TIMING_LOG_NUM_SLOTS
};
@@ -125,7 +131,7 @@ public:
void set_error(Error_t error);
bool do_checks();
float get_inverter_temp();
bool update_thermal_limits();
bool update_thermal_limits(float fet_temp);
float effective_current_lim();
void log_timing(TimingLog_t log_idx);
float phase_current_from_adcval(uint32_t ADCValue);
@@ -187,6 +193,7 @@ public:
DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault;
DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup)
float thermal_current_lim_ = 10.0f; //[A]
float inverter_temp_ = NAN; // [°C] NaN while the ODrive is initializing.
// Communication protocol definitions
auto make_protocol_definitions() {
@@ -200,7 +207,7 @@ public:
make_protocol_property("DC_calib_phC", &DC_calib_.phC),
make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_),
make_protocol_ro_property("thermal_current_lim", &thermal_current_lim_),
make_protocol_function("get_inverter_temp", *this, &Motor::get_inverter_temp),
make_protocol_ro_property("inverter_temp", &inverter_temp_),
make_protocol_object("current_control",
make_protocol_property("p_gain", &current_control_.p_gain),
make_protocol_property("i_gain", &current_control_.i_gain),
@@ -236,15 +243,22 @@ public:
make_protocol_ro_property("TIMING_LOG_ENC_CALIB", &timing_log_[TIMING_LOG_ENC_CALIB]),
make_protocol_ro_property("TIMING_LOG_IDX_SEARCH", &timing_log_[TIMING_LOG_IDX_SEARCH]),
make_protocol_ro_property("TIMING_LOG_FOC_VOLTAGE", &timing_log_[TIMING_LOG_FOC_VOLTAGE]),
make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT])
make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT]),
make_protocol_ro_property("TIMING_LOG_SPI_START", &timing_log_[TIMING_LOG_SPI_START]),
make_protocol_ro_property("TIMING_LOG_SAMPLE_NOW", &timing_log_[TIMING_LOG_SAMPLE_NOW]),
make_protocol_ro_property("TIMING_LOG_SPI_END", &timing_log_[TIMING_LOG_SPI_END])
),
make_protocol_object("config",
make_protocol_property("pre_calibrated", &config_.pre_calibrated),
make_protocol_property("pre_calibrated", &config_.pre_calibrated,
[](void* ctx) { static_cast<Motor*>(ctx)->is_calibrated_ =
static_cast<Motor*>(ctx)->is_calibrated_ || static_cast<Motor*>(ctx)->config_.pre_calibrated; }, this),
make_protocol_property("pole_pairs", &config_.pole_pairs),
make_protocol_property("calibration_current", &config_.calibration_current),
make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage),
make_protocol_property("phase_inductance", &config_.phase_inductance),
make_protocol_property("phase_resistance", &config_.phase_resistance),
make_protocol_property("phase_inductance", &config_.phase_inductance,
[](void* ctx) { static_cast<Motor*>(ctx)->update_current_controller_gains(); }, this),
make_protocol_property("phase_resistance", &config_.phase_resistance,
[](void* ctx) { static_cast<Motor*>(ctx)->update_current_controller_gains(); }, this),
make_protocol_property("direction", &config_.direction),
make_protocol_property("motor_type", &config_.motor_type),
make_protocol_property("current_lim", &config_.current_lim),
Firmware/MotorControl/odrive_main.h
+56 -4
View File
@@ -33,9 +33,12 @@ extern "C" {
//default timeout waiting for phase measurement signals
#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms]
//TODO clean this up
// Period in [s]
static const float current_meas_period = CURRENT_MEAS_PERIOD;
// Frequency in [Hz]
static const int current_meas_hz = CURRENT_MEAS_HZ;
// extern const float elec_rad_per_enc;
extern uint32_t _reboot_cookie;
extern bool user_config_loaded_;
@@ -92,10 +95,58 @@ struct BoardConfig_t {
//<! This protects against cases in which the power supply fails to dissipate
//<! the brake power if the brake resistor is disabled.
//<! The default is 26V for the 24V board version and 52V for the 48V board version.
float power_supply_max_current = INFINITY; // Max current [A] the power supply can source
float power_supply_min_current = -INFINITY; // Max current [A] the power supply can sink
/**
* If enabled, if the measured DC voltage exceeds `dc_bus_overvoltage_ramp_start`,
* the ODrive will sink more power than usual into the the brake resistor
* in an attempt to bring the voltage down again.
*
* The brake duty cycle is increased by the following amount:
* vbus_voltage == dc_bus_overvoltage_ramp_start => brake_duty_cycle += 0%
* vbus_voltage == dc_bus_overvoltage_ramp_end => brake_duty_cycle += 100%
*
* Remarks:
* - This feature is active even when all motors are disarmed.
* - This feature is disabled if `brake_resistance` is non-positive.
*/
bool enable_dc_bus_overvoltage_ramp = false;
float dc_bus_overvoltage_ramp_start = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`.
//!< Do not set this lower than your usual vbus_voltage,
//!< unless you like fried brake resistors.
float dc_bus_overvoltage_ramp_end = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`.
//!< Must be larger than `dc_bus_overvoltage_ramp_start`,
//!< otherwise the ramp feature is disabled.
float dc_max_positive_current = INFINITY; // Max current [A] the power supply can source
float dc_max_negative_current = -0.000001f; // Max current [A] the power supply can sink. You most likely want a non-positive value here. Set to -INFINITY to disable.
PWMMapping_t pwm_mappings[GPIO_COUNT];
PWMMapping_t analog_mappings[GPIO_COUNT];
/**
* Defines the baudrate used on the UART interface.
* Some baudrates will have a small timing error due to hardware limitations.
*
* Here's an (incomplete) list of baudrates for ODrive v3.x:
*
* Configured | Actual | Error [%]
* -------------|---------------|-----------
* 1.2 KBps | 1.2 KBps | 0
* 2.4 KBps | 2.4 KBps | 0
* 9.6 KBps | 9.6 KBps | 0
* 19.2 KBps | 19.195 KBps | 0.02
* 38.4 KBps | 38.391 KBps | 0.02
* 57.6 KBps | 57.613 KBps | 0.02
* 115.2 KBps | 115.068 KBps | 0.11
* 230.4 KBps | 230.769 KBps | 0.16
* 460.8 KBps | 461.538 KBps | 0.16
* 921.6 KBps | 913.043 KBps | 0.93
* 1.792 MBps | 1.826 MBps | 1.9
* 1.8432 MBps | 1.826 MBps | 0.93
*
* For more information refer to Section 30.3.4 and Table 142 (the column with f_PCLK = 42 MHz) in the STM datasheet:
* https://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf
*/
uint32_t uart_baudrate = 115200;
};
extern BoardConfig_t board_config;
extern bool user_config_loaded_;
@@ -106,7 +157,7 @@ class Motor;
class ODriveCAN;
constexpr size_t AXIS_COUNT = 2;
extern Axis *axes[AXIS_COUNT];
extern std::array<Axis*, AXIS_COUNT> axes;
extern ODriveCAN *odCAN;
// if you use the oscilloscope feature you can bump up this value
@@ -128,6 +179,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_c
// ODrive specific includes
#include <utils.hpp>
#include <gpio_utils.hpp>
#include <low_level.h>
#include <motor.hpp>
#include <encoder.hpp>
Firmware/MotorControl/timer.hpp
+42
View File
@@ -0,0 +1,42 @@
#pragma once
#include <algorithm>
template <class T>
class Timer {
public:
void setTimeout(const T timeout) {
timeout_ = timeout;
}
void setIncrement(const T increment) {
increment_ = increment;
}
void start() {
running_ = true;
}
void stop() {
running_ = false;
}
// If the timer is started, increment the timer
void update() {
if (running_)
timer_ = std::min<T>(timer_ + increment_, timeout_);
}
void reset() {
timer_ = static_cast<T>(0);
}
bool expired() {
return timer_ >= timeout_;
}
private:
T timer_ = static_cast<T>(0); // Current state
T timeout_ = static_cast<T>(0); // Time to count
T increment_ = static_cast<T>(0); // Amount to increment each time update() is called
bool running_ = false; // update() only increments if runing_ is true
};
Firmware/MotorControl/trapTraj.cpp
+1 -1
View File
@@ -44,7 +44,7 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
// Are we displacing enough to reach cruising speed?
if (s*dX < s*dXmin) {
// Short move (triangle profile)
Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
Td_ = std::max(0.0f, -Vr_ / Dr_);
Tv_ = 0.0f;
Firmware/MotorControl/trapTraj.hpp
+2
View File
@@ -47,6 +47,8 @@ public:
float Tf_;
float yAccel_;
float t_;
};
#endif
Firmware/MotorControl/utils.hpp
+13 -12
View File
@@ -69,18 +69,6 @@ static const float one_by_sqrt3 = 0.57735026919f;
static const float two_by_sqrt3 = 1.15470053838f;
static const float sqrt3_by_2 = 0.86602540378f;
//beware of inserting large values!
static inline float wrap_pm(float x, float pm_range) {
while (x >= pm_range) x -= (2.0f * pm_range);
while (x < -pm_range) x += (2.0f * pm_range);
return x;
}
//beware of inserting large angles!
static inline float wrap_pm_pi(float theta) {
return wrap_pm(theta, M_PI);
}
// like fmodf, but always positive
static inline float fmodf_pos(float x, float y) {
float out = fmodf(x, y);
@@ -89,6 +77,19 @@ static inline float fmodf_pos(float x, float y) {
return out;
}
/**
* @brief Similar to modulo operator, except that the output range is centered
* around zero.
* The returned value is always in the range [-pm_range, pm_range).
*/
static inline float wrap_pm(float x, float pm_range) {
return fmodf_pos(x + pm_range, 2.0f * pm_range) - pm_range;
}
static inline float wrap_pm_pi(float theta) {
return wrap_pm(theta, M_PI);
}
// 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/Tests/test_can.cpp
-1
View File
@@ -1,5 +1,4 @@
#define DOCTEST_IMPLEMENT
#include <doctest.h>
#include <algorithm>
#include <cstring>
Firmware/Tests/test_rotate.cpp
+32
View File
@@ -0,0 +1,32 @@
#include <doctest.h>
#include <algorithm>
#include <array>
#include <iostream>
TEST_CASE("Rotate Axis State"){
std::array<int, 10> testArr = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
const int& currentVal = testArr.front();
CHECK(currentVal == testArr[0]);
std::rotate(testArr.begin(), testArr.begin() + 1, testArr.end());
CHECK(currentVal == testArr[0]);
CHECK(currentVal == 1);
CHECK(testArr.back() == 0);
std::rotate(testArr.begin(), testArr.begin() + 1, testArr.end());
CHECK(currentVal == testArr[0]);
CHECK(currentVal == 2);
CHECK(testArr.back() == 1);
}
TEST_CASE("Fill Test"){
std::array<int, 10> testArr = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
std::fill(testArr.begin(), testArr.end(), 6);
for(const auto& val : testArr){
CHECK(val == 6);
}
}
Firmware/Tests/test_runner.cpp
+1 -5
View File
@@ -16,10 +16,6 @@
using std::cout;
using std::endl;
TEST_SUITE("delta_enc") {
// Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
int mod(int dividend, int divisor) {
@@ -131,7 +127,7 @@ TEST_SUITE("vel_ramp") {
return v & 1;
}
TEST_CASE("Blah") {
TEST_CASE("Equivalence") {
float vel_setpoint = 0.0f;
float vel_ramp_rate = 8000;
float input_vel = 0.0f;
Firmware/Tests/test_timer.cpp
+29
View File
@@ -0,0 +1,29 @@
#include <doctest.h>
#include "MotorControl/timer.hpp"
#include <stdint.h>
TEST_CASE_TEMPLATE("Timer2", T, float, int, char, uint32_t){
Timer<T> myTimer;
myTimer.setTimeout(10);
myTimer.setIncrement(1);
CHECK(!myTimer.expired());
myTimer.start();
CHECK(!myTimer.expired());
for(int i = 0; i < 9; ++i){
myTimer.update();
CHECK(!myTimer.expired());
}
myTimer.update();
CHECK(myTimer.expired());
myTimer.stop();
CHECK(myTimer.expired());
myTimer.start();
CHECK(myTimer.expired());
myTimer.reset();
CHECK(!myTimer.expired());
}
Firmware/Tests/test_trap_traj.cpp
+235
View File
@@ -0,0 +1,235 @@
#include <doctest.h>
#include <limits.h>
#include <cmath>
#include <iostream>
#include <random>
#include "MotorControl/utils.hpp"
// TODO: This is currently a copy-paste of the real code due to non-trivial
// include dependencies. Should include real code.
class TrapezoidalTrajectory {
public:
struct Step_t {
float Y;
float Yd;
float Ydd;
};
explicit TrapezoidalTrajectory();
bool planTrapezoidal(float Xf, float Xi, float Vi,
float Vmax, float Amax, float Dmax);
Step_t eval(float t);
float Xi_;
float Xf_;
float Vi_;
float Ar_;
float Vr_;
float Dr_;
float Ta_;
float Tv_;
float Td_;
float Tf_;
float yAccel_;
float t_;
};
// A sign function where input 0 has positive sign (not 0)
float sign_hard(float val) {
return (std::signbit(val)) ? -1.0f : 1.0f;
}
// Symbol Description
// Ta, Tv and Td Duration of the stages of the AL profile
// Xi and Vi Adapted initial conditions for the AL profile
// Xf Position set-point
// s Direction (sign) of the trajectory
// Vmax, Amax, Dmax and jmax Kinematic bounds
// Ar, Dr and Vr Reached values of acceleration and velocity
TrapezoidalTrajectory::TrapezoidalTrajectory() {}
bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
float Vmax, float Amax, float Dmax) {
float dX = Xf - Xi; // Distance to travel
float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement
float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)
Ar_ = s * Amax; // Maximum Acceleration (signed)
Dr_ = -s * Dmax; // Maximum Deceleration (signed)
Vr_ = s * Vmax; // Maximum Velocity (signed)
// If we start with a speed faster than cruising, then we need to decel instead of accel
// aka "double deceleration move" in the paper
if ((s * Vi) > (s * Vr_)) {
Ar_ = -s * Amax;
}
// Time to accel/decel to/from Vr (cruise speed)
Ta_ = (Vr_ - Vi) / Ar_;
Td_ = -Vr_ / Dr_;
// Integral of velocity ramps over the full accel and decel times to get
// minimum displacement required to reach cuising speed
float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;
// Are we displacing enough to reach cruising speed?
if (s*dX < s*dXmin) {
// Short move (triangle profile)
Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
//Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
Td_ = std::max(0.0f, -Vr_ / Dr_);
Tv_ = 0.0f;
} else {
// Long move (trapezoidal profile)
Tv_ = (dX - dXmin) / Vr_;
}
// Fill in the rest of the values used at evaluation-time
Tf_ = Ta_ + Tv_ + Td_;
Xi_ = Xi;
Xf_ = Xf;
Vi_ = Vi;
yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase
return true;
}
TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) {
Step_t trajStep;
if (t < 0.0f) { // Initial Condition
trajStep.Y = Xi_;
trajStep.Yd = Vi_;
trajStep.Ydd = 0.0f;
} else if (t < Ta_) { // Accelerating
trajStep.Y = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);
trajStep.Yd = Vi_ + Ar_*t;
trajStep.Ydd = Ar_;
} else if (t < Ta_ + Tv_) { // Coasting
trajStep.Y = yAccel_ + Vr_*(t - Ta_);
trajStep.Yd = Vr_;
trajStep.Ydd = 0.0f;
} else if (t < Tf_) { // Deceleration
float td = t - Tf_;
trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td);
trajStep.Yd = Dr_*td;
trajStep.Ydd = Dr_;
} else if (t >= Tf_) { // Final Condition
trajStep.Y = Xf_;
trajStep.Yd = 0.0f;
trajStep.Ydd = 0.0f;
} else {
// TODO: report error here
}
return trajStep;
}
static_assert(sizeof(float) * CHAR_BIT == 32);
void run_trajectory_test(float goal, float position, float velocity, float Vmax, float Amax, float Dmax) {
float dt = 0.000125f;
int replan_interval = 10; // must be > 2 (see note below)
float t = 0.0f;
float Vmax_test = std::max(Vmax, std::abs(velocity));
TrapezoidalTrajectory traj{};
int replan_counter = 0;
do {
if (replan_counter <= 0) {
CHECK(traj.planTrapezoidal(goal, position, velocity, Vmax, Amax, Dmax));
t = 0.0f;
replan_counter = replan_interval;
} else {
replan_counter--;
}
TrapezoidalTrajectory::Step_t step = traj.eval(t);
t += dt;
//std::cerr << "vel: " << step.Yd << ", pos: " << step.Y << "\n";
// Check if acceleration within bounds
if (velocity >= 0.0f) {
CHECK(step.Ydd <= Amax);
CHECK(step.Ydd >= -Dmax);
CHECK((step.Yd - velocity) / dt <= Amax * 1.002f);
CHECK((step.Yd - velocity) / dt >= -Dmax * 1.002f);
} else {
CHECK(step.Ydd <= Dmax);
CHECK(step.Ydd >= -Amax);
CHECK((step.Yd - velocity) / dt <= Dmax * 1.002f);
CHECK((step.Yd - velocity) / dt >= -Amax * 1.002f);
}
// Check if velocity within bounds
CHECK(step.Yd >= -Vmax_test);
CHECK(step.Yd <= Vmax_test);
CHECK((step.Y - position) / dt >= -Vmax_test * 1.002f);
CHECK((step.Y - position) / dt <= Vmax_test * 1.002f);
velocity = step.Yd;
// Check if position is making progress
// TODO: the trajectory planner currently needs three "warm-up" iterations
// until its position makes progress. This should probably be revisited.
// TODO: this is disabled currently because there are legitimate trajectories
// where the position first moves in the wrong direction.
//if ((replan_counter < replan_interval - 2) && (t <= traj.Tf_)) {
// CHECK(std::abs(step.Y - goal) < std::abs(position - goal));
//}
position = step.Y;
} while (t <= traj.Tf_);
CHECK(position >= goal - 1.0f);
CHECK(position <= goal + 1.0f);
CHECK(velocity >= -Dmax * dt);
CHECK(velocity <= Dmax * dt);
}
TEST_SUITE("Trajectory Planner") {
// these form a triangle trajectory because 2*v^2/(2*a) = 2 * 27712^2 / (2*22288) = 34456 > 16384
TEST_CASE("neg-dir-triangle") {
run_trajectory_test(-8192.0f, 8192.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f);
}
TEST_CASE("pos-dir-triangle") {
run_trajectory_test(8192.0f, -8192.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f);
}
// these form a trapezoid trajectory because 2*v^2/(2*a) = 2 * 27712^2 / (2*22288) = 34456 < 16384
TEST_CASE("neg-dir-trapezoid") {
run_trajectory_test(-25000.0f, 25000.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f);
}
TEST_CASE("pos-dir-trapezoid") {
run_trajectory_test(25000.0f, -25000.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f);
}
// for the following tests note that v^2/(2*a) = 27712^2 / (2*22288) = 17227 > 16384
TEST_CASE("neg-dir-not-enough-braking-distance") {
run_trajectory_test(-8192.0f, 8192.0f, -27712.0f, 27712.0f, 22288.0f, 22288.0f);
}
TEST_CASE("pos-dir-not-enough-braking-distance") {
run_trajectory_test(8192.0f, -8192.0f, 27712.0f, 27712.0f, 22288.0f, 22288.0f);
}
TEST_CASE("neg-dir-over-speed") {
run_trajectory_test(-8192.0f, 8192.0f, -40000.0f, 27712.0f, 22288.0f, 22288.0f);
}
TEST_CASE("pos-dir-over-speed") {
run_trajectory_test(8192.0f, -8192.0f, 40000.0f, 27712.0f, 22288.0f, 22288.0f);
}
}
Firmware/Tupfile.lua
+5 -4
View File
@@ -185,12 +185,13 @@ build{
'MotorControl',
'fibre/cpp/include',
'.',
"C:/Tools/doctest/doctest"
"doctest"
}
}
if tup.getconfig('DOCTEST') == 'true' then
TEST_INCLUDES = '-I. -I./MotorControl -I./fibre/cpp/include -I./Drivers/DRV8301 -IC:/Tools/doctest/doctest'
tup.frule{inputs='Tests/*.cpp', command='g++ -O3 -std=gnu++17 '..TEST_INCLUDES..' %f -o %o', outputs='Tests/test_runner.exe'}
TEST_INCLUDES = '-I. -I./MotorControl -I./fibre/cpp/include -I./Drivers/DRV8301 -I./doctest'
tup.foreach_rule('Tests/*.cpp', 'g++ -O3 -std=c++17 '..TEST_INCLUDES..' -c %f -o %o', 'Tests/bin/%B.o')
tup.frule{inputs='Tests/bin/*.o', command='g++ %f -o %o', outputs='Tests/test_runner.exe'}
tup.frule{inputs='Tests/test_runner.exe', command='%f'}
end
end
Firmware/communication/ascii_protocol.cpp
+5 -4
View File
@@ -72,18 +72,19 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
char cmd[MAX_LINE_LENGTH + 1];
if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH;
memcpy(cmd, buffer, len);
cmd[len] = 0; // null-terminate
// optional checksum validation
bool use_checksum = (checksum_start < len);
if (use_checksum) {
unsigned int received_checksum;
sscanf((const char *)cmd + checksum_start, "%u", &received_checksum);
if (received_checksum != checksum)
int numscan = sscanf((const char *)cmd + checksum_start, "%u", &received_checksum);
if ((numscan < 1) || (received_checksum != checksum))
return;
len = checksum_start - 1; // prune checksum and asterisk
cmd[len] = 0; // null-terminate
}
cmd[len] = 0; // null-terminate
// check incoming packet type
if (cmd[0] == 'p') { // position control
@@ -256,7 +257,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
}
}
}else if (cmd[0] == 'u') { // Update axis watchdog.
} else if (cmd[0] == 'u') { // Update axis watchdog.
unsigned motor_number;
int numscan = sscanf(cmd, "u %u", &motor_number);
if(numscan < 1){
Firmware/communication/can_helpers.hpp
+2 -2
View File
@@ -26,7 +26,7 @@ struct can_Signal_t {
template <typename T>
T can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel) {
uint64_t tempVal = 0;
uint64_t mask = (1ULL << length) - 1;
uint64_t mask = (1ULL << length) - 1;
if (isIntel) {
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
@@ -50,7 +50,7 @@ float can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t len
template <typename T>
void can_setSignal(can_Message_t& msg, const T& val, const uint8_t startBit, const uint8_t length, const bool isIntel, const float factor, const float offset) {
T scaledVal = (val - offset) / factor;
T scaledVal = (val - offset) / factor;
uint64_t valAsBits = 0;
std::memcpy(&valAsBits, &scaledVal, sizeof(scaledVal));
Firmware/communication/can_simple.cpp
+16 -16
View File
@@ -26,7 +26,7 @@ void CANSimple::handle_can_message(can_Message_t& msg) {
bool validAxis = false;
for (uint8_t i = 0; i < AXIS_COUNT; i++) {
if (axes[i]->config_.can_node_id == nodeID) {
if ((axes[i]->config_.can_node_id == nodeID) && (axes[i]->config_.can_node_id_extended == msg.isExt)) {
axis = axes[i];
if (!validAxis) {
validAxis = true;
@@ -137,7 +137,7 @@ void CANSimple::get_motor_error_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_MOTOR_ERROR; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->motor_.error_;
@@ -154,7 +154,7 @@ void CANSimple::get_encoder_error_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_ERROR; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->encoder_.error_;
@@ -171,7 +171,7 @@ void CANSimple::get_sensorless_error_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_SENSORLESS_ERROR; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->sensorless_estimator_.error_;
@@ -184,7 +184,7 @@ void CANSimple::get_sensorless_error_callback(Axis* axis, can_Message_t& msg) {
}
void CANSimple::set_axis_nodeid_callback(Axis* axis, can_Message_t& msg) {
axis->config_.can_node_id = msg.buf[0] & 0x3F; // Node ID bitmask
axis->config_.can_node_id = can_getSignal<uint32_t>(msg, 0, 32, true);
}
void CANSimple::set_axis_requested_state_callback(Axis* axis, can_Message_t& msg) {
@@ -199,7 +199,7 @@ void CANSimple::get_encoder_estimates_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_ESTIMATES; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
// Undefined behaviour!
@@ -230,7 +230,7 @@ void CANSimple::get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_SENSORLESS_ESTIMATES; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
// Undefined behaviour!
@@ -261,7 +261,7 @@ void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_COUNT;
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->encoder_.shadow_count_;
@@ -279,14 +279,14 @@ void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) {
}
void CANSimple::set_input_pos_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.input_pos_ = can_getSignal<int32_t>(msg, 0, 32, true);
axis->controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0);
axis->controller_.input_pos_ = can_getSignal<int32_t>(msg, 0, 32, true);
axis->controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0);
axis->controller_.input_current_ = can_getSignal<int16_t>(msg, 48, 16, true, 0.01f, 0);
axis->controller_.input_pos_updated();
}
void CANSimple::set_input_vel_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.input_vel_ = can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f);
axis->controller_.input_vel_ = can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f);
axis->controller_.input_current_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.01f, 0.0f);
}
@@ -325,7 +325,7 @@ void CANSimple::get_iq_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_IQ;
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
uint32_t floatBytes;
@@ -354,7 +354,7 @@ void CANSimple::get_vbus_voltage_callback(Axis* axis, can_Message_t& msg) {
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_VBUS_VOLTAGE;
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
uint32_t floatBytes;
@@ -386,7 +386,7 @@ void CANSimple::send_heartbeat(Axis* axis) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_ODRIVE_HEARTBEAT; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
// Axis errors in 1st 32-bit value
@@ -403,8 +403,8 @@ void CANSimple::send_heartbeat(Axis* axis) {
odCAN->write(txmsg);
}
uint8_t CANSimple::get_node_id(uint32_t msgID) {
return ((msgID >> NUM_CMD_ID_BITS) & 0x03F); // Upper 6 bits
uint32_t CANSimple::get_node_id(uint32_t msgID) {
return (msgID >> NUM_CMD_ID_BITS); // Upper 6 or more bits
}
uint8_t CANSimple::get_cmd_id(uint32_t msgID) {
Firmware/communication/can_simple.hpp
+1 -1
View File
@@ -64,7 +64,7 @@ class CANSimple {
static void clear_errors_callback(Axis* axis, can_Message_t& msg);
// Utility functions
static uint8_t get_node_id(uint32_t msgID);
static uint32_t get_node_id(uint32_t msgID);
static uint8_t get_cmd_id(uint32_t msgID);
// Fetch a specific signal from the message
Firmware/communication/communication.cpp
+9 -2
View File
@@ -11,6 +11,7 @@
#include "odrive_main.h"
#include "freertos_vars.h"
#include "utils.hpp"
#include "gpio_utils.hpp"
#include "../build/version.h" // autogenerated based on Git state
@@ -115,6 +116,7 @@ public:
static inline auto make_obj_tree() {
return make_protocol_member_list(
make_protocol_ro_property("vbus_voltage", &vbus_voltage),
make_protocol_ro_property("ibus", &ibus_),
make_protocol_ro_property("serial_number", &serial_number),
make_protocol_ro_property("hw_version_major", &hw_version_major),
make_protocol_ro_property("hw_version_minor", &hw_version_minor),
@@ -125,6 +127,7 @@ static inline auto make_obj_tree() {
make_protocol_ro_property("fw_version_unreleased", &fw_version_unreleased),
make_protocol_ro_property("user_config_loaded", const_cast<const bool *>(&user_config_loaded_)),
make_protocol_ro_property("brake_resistor_armed", &brake_resistor_armed),
make_protocol_property("brake_resistor_saturated", &brake_resistor_saturated),
make_protocol_object("system_stats",
make_protocol_ro_property("uptime", &system_stats_.uptime),
make_protocol_ro_property("min_heap_space", &system_stats_.min_heap_space),
@@ -161,12 +164,16 @@ static inline auto make_obj_tree() {
make_protocol_property("max_regen_current", &board_config.max_regen_current),
// TODO: changing this currently requires a reboot - fix this
make_protocol_property("enable_uart", &board_config.enable_uart),
make_protocol_property("uart_baudrate", &board_config.uart_baudrate), // requires a reboot
make_protocol_property("enable_i2c_instead_of_can" , &board_config.enable_i2c_instead_of_can), // requires a reboot
make_protocol_property("enable_ascii_protocol_on_usb", &board_config.enable_ascii_protocol_on_usb),
make_protocol_property("dc_bus_undervoltage_trip_level", &board_config.dc_bus_undervoltage_trip_level),
make_protocol_property("dc_bus_overvoltage_trip_level", &board_config.dc_bus_overvoltage_trip_level),
make_protocol_property("power_supply_min_current", &board_config.power_supply_min_current),
make_protocol_property("power_supply_max_current", &board_config.power_supply_max_current),
make_protocol_property("enable_dc_bus_overvoltage_ramp", &board_config.enable_dc_bus_overvoltage_ramp),
make_protocol_property("dc_bus_overvoltage_ramp_start", &board_config.dc_bus_overvoltage_ramp_start),
make_protocol_property("dc_bus_overvoltage_ramp_end", &board_config.dc_bus_overvoltage_ramp_end),
make_protocol_property("dc_max_negative_current", &board_config.dc_max_negative_current),
make_protocol_property("dc_max_positive_current", &board_config.dc_max_positive_current),
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
make_protocol_object("gpio1_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[0])),
make_protocol_object("gpio2_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[1])),
Firmware/communication/interface_uart.cpp
+8 -4
View File
@@ -62,13 +62,19 @@ static void uart_server_thread(void * ctx) {
(void) ctx;
for (;;) {
osDelay(1);
// Check for UART errors and restart recieve DMA transfer if required
if (huart4.ErrorCode != HAL_UART_ERROR_NONE) {
if (huart4.RxState != HAL_UART_STATE_BUSY_RX) {
HAL_UART_AbortReceive(&huart4);
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer));
dma_last_rcv_idx = 0;
}
// Fetch the circular buffer "write pointer", where it would write next
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
if (new_rcv_idx > UART_RX_BUFFER_SIZE) { // defensive programming
continue;
}
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
// Process bytes in one or two chunks (two in case there was a wrap)
@@ -86,8 +92,6 @@ static void uart_server_thread(void * ctx) {
new_rcv_idx - dma_last_rcv_idx, uart4_stream_output);
dma_last_rcv_idx = new_rcv_idx;
}
osDelay(1);
};
}
@@ -96,7 +100,7 @@ void start_uart_server() {
// We dont use interrupts to fetch the data, instead we periodically read
// data out of the circular buffer into a parse buffer, controlled by a state machine
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer));
dma_last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
dma_last_rcv_idx = 0;
// Start UART communication thread
osThreadDef(uart_server_thread_def, uart_server_thread, osPriorityNormal, 0, stack_size_uart_thread / sizeof(StackType_t) /* the ascii protocol needs considerable stack space */);
Firmware/doctest/doctest.h
+5956
View File
File diff suppressed because it is too large Load Diff
Firmware/doctest/parts/doctest.cpp
+3344
View File
File diff suppressed because it is too large Load Diff
Firmware/doctest/parts/doctest_fwd.h
+2604
View File
File diff suppressed because it is too large Load Diff
Firmware/fibre/cpp/include/fibre/protocol.hpp
+12 -1
View File
@@ -825,7 +825,11 @@ public:
// special-purpose function - to be moved
bool set_string(char * buffer, size_t length) final {
return from_string(buffer, length, property_, 0);
bool wrote = from_string(buffer, length, property_, 0);
if (wrote && written_hook_ != nullptr) {
written_hook_(ctx_);
}
return wrote;
}
bool set_from_float(float value) final {
@@ -1098,6 +1102,7 @@ public:
extern Endpoint** endpoint_list_;
extern size_t n_endpoints_;
extern uint16_t json_crc_;
extern uint32_t json_version_id_; // exposed to hosts to facilitate cache lookup
extern JSONDescriptorEndpoint json_file_endpoint_;
extern EndpointProvider* application_endpoints_;
@@ -1124,10 +1129,16 @@ int fibre_publish(T& application_objects) {
// Calculate the CRC16 of the JSON file.
// The init value is the protocol version.
CRC16Calculator crc16_calculator(PROTOCOL_VERSION);
uint8_t offset[4] = { 0 };
json_file_endpoint_.handle(offset, sizeof(offset), &crc16_calculator);
json_crc_ = crc16_calculator.get_crc16();
// Add entropy for fibre cache
json_file_endpoint_.handle(offset, sizeof(offset), &crc16_calculator);
json_version_id_ = (uint32_t) crc16_calculator.get_crc16();
json_version_id_ += json_crc_ << 16;
return 0;
}
Firmware/fibre/cpp/protocol.cpp
+18 -11
View File
@@ -13,9 +13,10 @@
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
Endpoint** endpoint_list_ = nullptr; // initialized by calling fibre_publish
size_t n_endpoints_ = 0; // initialized by calling fibre_publish
uint16_t json_crc_; // initialized by calling fibre_publish
Endpoint** endpoint_list_ = nullptr; // initialized by calling fibre_publish
size_t n_endpoints_ = 0; // initialized by calling fibre_publish
uint16_t json_crc_; // initialized by calling fibre_publish
uint32_t json_version_id_; // initialized by calling fibre_publish
JSONDescriptorEndpoint json_file_endpoint_ = JSONDescriptorEndpoint();
EndpointProvider* application_endpoints_;
@@ -140,15 +141,21 @@ void JSONDescriptorEndpoint::handle(const uint8_t* input, size_t input_length, S
return;
uint32_t offset = 0;
read_le<uint32_t>(&offset, input);
NullStreamSink output_with_offset = NullStreamSink(offset, *output);
size_t id = 0;
write_string("[", &output_with_offset);
json_file_endpoint_.write_json(id, &output_with_offset);
id += decltype(json_file_endpoint_)::endpoint_count;
write_string(",", &output_with_offset);
application_endpoints_->write_json(id, &output_with_offset);
write_string("]", &output_with_offset);
// If the offset is special value 0xFFFFFFFF, send back the JSON version ID instead
if (offset == 0xffffffff) {
default_readwrite_endpoint_handler(&json_version_id_, nullptr, 0, output);
} else {
NullStreamSink output_with_offset = NullStreamSink(offset, *output);
size_t id = 0;
write_string("[", &output_with_offset);
json_file_endpoint_.write_json(id, &output_with_offset);
id += decltype(json_file_endpoint_)::endpoint_count;
write_string(",", &output_with_offset);
application_endpoints_->write_json(id, &output_with_offset);
write_string("]", &output_with_offset);
}
}
int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_t length) {
Firmware/fibre/python/fibre/discovery.py
+54 -16
View File
@@ -7,11 +7,14 @@ import json
import time
import threading
import traceback
import struct
import fibre.protocol
import fibre.utils
import fibre.remote_object
from fibre.utils import Event, Logger
from fibre.protocol import ChannelBrokenException, TimeoutError
import appdirs
import os
# Load all installed transport layers
@@ -64,25 +67,57 @@ def find_all(path, serial_number,
"""
try:
logger.debug("Connecting to device on " + channel._name)
cache_dir = appdirs.user_cache_dir("odrivetool")
cache_path = None
# Fetch the json version tag to check cache (only supported on firmware v0.5 or later)
try:
json_version_tag = channel.remote_endpoint_operation(0, struct.pack("<I", 0xffffffff), True, 4)
json_version_tag = struct.unpack("<I", json_version_tag)[0]
logger.debug("Device reported JSON version ID: {:08d}".format(json_version_tag))
cache_path = os.path.join(cache_dir, 'fibre_schema_cache_{:08d}'.format(json_version_tag))
except:
logger.debug("Failed to get JSON checksum")
# Check cache
json_data = None
try:
if not cache_path is None:
with open(cache_path, 'rb') as fp:
json_crc16 = fibre.protocol.calc_crc16(fibre.protocol.PROTOCOL_VERSION, fp.read())
fp.seek(0)
json_data = json.load(fp)
except:
logger.debug(f"Failed load JSON cache file {cache_path}")
# Fallback to loading JSON from device
if json_data is None:
# Downloading json data
logger.info("Downloading json data from ODrive... (this might take a while)")
json_bytes = channel.remote_endpoint_read_buffer(0)
except (TimeoutError, ChannelBrokenException):
logger.debug("no response - probably incompatible")
return
json_crc16 = fibre.protocol.calc_crc16(fibre.protocol.PROTOCOL_VERSION, json_bytes)
channel._interface_definition_crc = json_crc16
try:
json_string = json_bytes.decode("ascii")
except UnicodeDecodeError:
logger.debug("device responded on endpoint 0 with something that is not ASCII")
return
logger.debug("JSON: " + json_string.replace('{"name"', '\n{"name"'))
logger.debug("JSON checksum: 0x{:02X} 0x{:02X}".format(json_crc16 & 0xff, (json_crc16 >> 8) & 0xff))
try:
try:
json_string = json_bytes.decode("ascii")
except UnicodeDecodeError:
logger.debug("Device responded on endpoint 0 with something that is not ASCII")
raise UnicodeDecodeError
json_crc16 = fibre.protocol.calc_crc16(fibre.protocol.PROTOCOL_VERSION, json_bytes)
json_data = json.loads(json_string)
except json.decoder.JSONDecodeError as error:
logger.debug("device responded on endpoint 0 with something that is not JSON: " + str(error))
return
# Save JSON to cache
if not cache_path is None:
logger.debug(f"Creating new JSON cache file {cache_path}")
os.makedirs(cache_dir, exist_ok=True)
with open(cache_path, 'w+') as json_cache:
json_cache.write(json_string)
logger.debug(f"Saved JSON to cache file {cache_path}")
channel._interface_definition_crc = json_crc16
logger.debug("JSON: " + str(json_data).replace("{'name'", "\n{'name'"))
json_data = {"name": "fibre_node", "members": json_data}
obj = fibre.remote_object.RemoteObject(json_data, None, channel, logger)
@@ -93,7 +128,10 @@ def find_all(path, serial_number,
if serial_number != None and device_serial_number != serial_number:
logger.debug("Ignoring device with serial number {}".format(device_serial_number))
return
did_discover_object_callback(obj)
except Exception:
logger.debug("Unexpected exception after discovering channel: " + traceback.format_exc())
Firmware/fibre/python/fibre/shell.py
+22 -10
View File
@@ -80,11 +80,23 @@ def launch_shell(args,
# If IPython is installed, embed IPython shell, otherwise embed regular shell
if use_ipython:
help = lambda: print_help(args, len(discovered_devices) > 0) # Override help function # pylint: disable=W0612
locals()['__name__'] = globals()['__name__'] # to fix broken "%run -i script.py"
# Override help function # pylint: disable=W0612
help = lambda: print_help(args, len(discovered_devices) > 0)
# to fix broken "%run -i script.py"
locals()['__name__'] = globals()['__name__']
console = IPython.terminal.embed.InteractiveShellEmbed(banner1='')
console.runcode = console.run_code # hack to make IPython look like the regular console
# hack to make IPython look like the regular console
console.runcode = console.run_cell
interact = console
# Catch ChannelBrokenException (since disconnect is not always an error)
default_exception_hook = console._showtraceback
def filtered_exception_hook(ex_class, ex, trace):
if(ex_class.__module__+'.'+ex_class.__name__ != 'fibre.protocol.ChannelBrokenException'):
default_exception_hook(ex_class,ex,trace)
console._showtraceback = filtered_exception_hook
else:
# Enable tab complete if possible
try:
@@ -100,13 +112,13 @@ def launch_shell(args,
console = code.InteractiveConsole(locals=interactive_variables)
interact = lambda: console.interact(banner='')
# install hook to hide ChannelBrokenException
console.runcode('import sys')
console.runcode('superexcepthook = sys.excepthook')
console.runcode('def newexcepthook(ex_class,ex,trace):\n'
' if ex_class.__module__ + "." + ex_class.__name__ != "fibre.ChannelBrokenException":\n'
' superexcepthook(ex_class,ex,trace)')
console.runcode('sys.excepthook=newexcepthook')
# Catch ChannelBrokenException (since disconnect is not alway an error)
console.runcode("import sys")
console.runcode("default_exception_hook = sys.excepthook")
console.runcode("def filtered_exception_hook(ex_class, ex, trace):\n"
" if ex_class.__module__ + '.' + ex_class.__name__ != 'fibre.protocol.ChannelBrokenException':\n"
" default_exception_hook(ex_class,ex,trace)")
console.runcode("sys.excepthook=filtered_exception_hook")
# Launch shell
Firmware/fibre/python/setup.py
+3 -1
View File
@@ -76,7 +76,9 @@ setup(
license='MIT',
url = 'https://github.com/samuelsadok/fibre',
keywords = ['communication', 'transport-layer', 'rpc'],
install_requires = [],
install_requires = [
'appdirs', # Used to find caching directory
],
#package_data={'': ['version.txt']},
classifiers = [],
)

Some files were not shown because too many files have changed in this diff Show More