From 5cc1a9546def8dd4809bc8a3427c8d205e735fb0 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 1 Jul 2018 01:11:44 -0700 Subject: [PATCH] implement axis temp reading --- CHANGELOG.md | 1 + Firmware/MotorControl/axis.cpp | 6 ++++++ Firmware/MotorControl/axis.hpp | 2 ++ Firmware/MotorControl/board_config_v3.h | 15 +++++++++++++++ Firmware/MotorControl/low_level.cpp | 6 ++++-- Firmware/MotorControl/low_level.h | 4 ++++ Firmware/MotorControl/odrive_main.h | 3 --- Firmware/MotorControl/utils.c | 5 ++++- tools/setup.py | 2 +- 9 files changed, 37 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 359319ae..16cda1ec 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,7 @@ Please add a note of your changes below this heading if you make a Pull Request. ### Added * Hall sensor feedback * Configurable RC PWM input +* Ability to read axis FET temperature * Config settings for: * `motor.config.requested_current_range` * `motor.config.current_control_bandwidth` and `motor.set_current_control_bandwidth`. Latter required to invoke gain recalculation. diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 08828336..270ab15f 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -123,6 +123,12 @@ bool Axis::do_updates() { return error_ == ERROR_NONE; } +float Axis::get_temp() { + float adc = adc_measurements_[hw_config_.thermistor_adc_ch]; + float normalized_voltage = adc / adc_full_scale; + return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs); +} + bool Axis::run_sensorless_spin_up() { // Early Spin-up: spiral up current float x = 0.0f; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 8eda2892..cfa1bd28 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -79,6 +79,7 @@ public: bool check_PSU_brownout(); bool do_checks(); bool do_updates(); + float get_temp(); // @brief Runs the specified update handler at the frequency of the current measurements. // @@ -178,6 +179,7 @@ public: make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration), make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel) ), + make_protocol_function("get_temp", *this, &Axis::get_temp), make_protocol_object("motor", motor_.make_protocol_definitions()), make_protocol_object("controller", controller_.make_protocol_definitions()), make_protocol_object("encoder", encoder_.make_protocol_definitions()), diff --git a/Firmware/MotorControl/board_config_v3.h b/Firmware/MotorControl/board_config_v3.h index 3885727a..1e170fed 100644 --- a/Firmware/MotorControl/board_config_v3.h +++ b/Firmware/MotorControl/board_config_v3.h @@ -25,6 +25,7 @@ typedef struct { uint16_t step_pin; GPIO_TypeDef* dir_port; uint16_t dir_pin; + size_t thermistor_adc_ch; osPriority thread_priority; } AxisHardwareConfig_t; @@ -61,15 +62,23 @@ typedef struct { } BoardHardwareConfig_t; extern const BoardHardwareConfig_t hw_configs[2]; +extern const float thermistor_poly_coeffs[]; +extern const size_t thermistor_num_coeffs; //TODO stick this in a C file #ifdef __MAIN_CPP__ +const float thermistor_poly_coeffs[] = + {363.0172658f, -459.19773008f, 308.29273921f, -28.12731452f}; +const size_t thermistor_num_coeffs = sizeof(thermistor_poly_coeffs)/sizeof(thermistor_poly_coeffs[1]); + const BoardHardwareConfig_t hw_configs[2] = { { + //M0 .axis_config = { .step_port = GPIO_1_GPIO_Port, .step_pin = GPIO_1_Pin, .dir_port = GPIO_2_GPIO_Port, .dir_pin = GPIO_2_Pin, + .thermistor_adc_ch = 15, .thread_priority = (osPriority)(osPriorityHigh + (osPriority)1), }, .encoder_config = { @@ -99,6 +108,7 @@ const BoardHardwareConfig_t hw_configs[2] = { { .nFAULT_pin = nFAULT_Pin, } },{ + //M1 .axis_config = { #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 5 .step_port = GPIO_7_GPIO_Port, @@ -110,6 +120,11 @@ const BoardHardwareConfig_t hw_configs[2] = { { .step_pin = GPIO_3_Pin, .dir_port = GPIO_4_GPIO_Port, .dir_pin = GPIO_4_Pin, +#endif +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 + .thermistor_adc_ch = 4, +#else + .thermistor_adc_ch = 1, #endif .thread_priority = osPriorityHigh, }, diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index e39d8a96..43e59bd0 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -28,6 +28,8 @@ /* Private macros ------------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/ /* Global constant data ------------------------------------------------------*/ +const float adc_full_scale = (float)(1 << 12); +const float adc_ref_voltage = 3.3f; /* Global variables ----------------------------------------------------------*/ // This value is updated by the DC-bus reading ADC. @@ -422,7 +424,7 @@ float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) { channel = 15; } if (channel < ADC_CHANNEL_COUNT) - return ((float)adc_measurements_[channel]) * (3.3f / (float)(1 << 12)); + return ((float)adc_measurements_[channel]) * (adc_ref_voltage / adc_full_scale); else return 0.0f / 0.0f; // NaN } @@ -432,7 +434,7 @@ float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) { //-------------------------------- void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12); + static const float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale; // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); vbus_voltage = ADCValue * voltage_scale; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 41f7f4b5..4af6e3e0 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -17,9 +17,13 @@ extern "C" { /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ +#define ADC_CHANNEL_COUNT 16 +extern const float adc_full_scale; +extern const float adc_ref_voltage; /* Exported variables --------------------------------------------------------*/ extern float vbus_voltage; extern bool brake_resistor_armed; +extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT]; /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index bf35821f..4e4db160 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -36,9 +36,6 @@ extern bool user_config_loaded_; extern uint64_t serial_number; extern char serial_number_str[13]; -#define ADC_CHANNEL_COUNT 16 -extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT]; - typedef struct { bool fully_booted; uint32_t uptime; // [ms] diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index f70f5170..b59af789 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -153,9 +153,12 @@ float fast_atan2(float y, float x) { return r; } +// Evaluate polynomials using Fused Multiply Add intrisic instruction. +// coeffs[0] is highest order, as per numpy.polyfit +// p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg" float horner_fma(float x, const float *coeffs, size_t count) { float result = 0.0f; - for (int idx = count-1; idx >= 0; idx--) + for (int idx = 0; idx < count; ++idx) result = fmaf(result, x, coeffs[idx]); return result; } diff --git a/tools/setup.py b/tools/setup.py index d5e38eff..961ceec6 100644 --- a/tools/setup.py +++ b/tools/setup.py @@ -33,7 +33,7 @@ to publish packages with the name odrive. # Set to true to make an official post-release, rather than dev of new version is_post_release = False -post_rel_num = 5 +post_rel_num = 8 # To test higher numbered releases, bump to the next rev bump_rev = not is_post_release