diff --git a/CHANGELOG.md b/CHANGELOG.md index 6c8f96c4..b8d11285 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 * **Trapezoidal Trajectory Planner** +* Hook to execute protocol property written callback * -Wdouble-promotion warning to compilation ### Changed @@ -13,6 +14,15 @@ Please add a note of your changes below this heading if you make a Pull Request. * `TimeoutError` isn't defined, but it makes for more readable code, so I defined it as an OSError subclass. * `ModuleNotFoundError` is replaced by the older ImportError. * Print function imported from future +* Using new hooks to calculate: + * `motor.config.current_control_bandwidth` + * This deprecates `motor.set_current_control_bandwidth()` + * `encoder.config.bandwidth` +* Default value for `motor.resistance_calib_max_voltage` changed to 2.0 + +### Fixed +* An issue where the axis state machine would jump in and out of idle when there is an error +* There is a [bug](https://github.com/ARM-software/CMSIS_5/issues/267) in the arm fast math library, which gives spikes in the output of arm_cos_f32 for input values close to -pi/2. We fixed the bug locally, and hence are using "our_arm_cos_f32". # Releases ## [0.4.4] - 2018-09-18 diff --git a/Firmware/MotorControl/arm_cos_f32.c b/Firmware/MotorControl/arm_cos_f32.c new file mode 100644 index 00000000..a63d14cb --- /dev/null +++ b/Firmware/MotorControl/arm_cos_f32.c @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cos_f32.c + * Description: Fast cosine calculation for floating-point values + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 // TODO: might change in future board versions +#include "arm_math.h" +#include "arm_common_tables.h" +/** + * @ingroup groupFastMath + */ + +/** + * @defgroup cos Cosine + * + * Computes the trigonometric cosine function using a combination of table lookup + * and linear interpolation. There are separate functions for + * Q15, Q31, and floating-point data types. + * The input to the floating-point version is in radians and in the range [0 2*pi) while the + * fixed-point Q15 and Q31 have a scaled input with the range + * [0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a + * value of 2*pi wraps around to 0. + * + * The implementation is based on table lookup using 256 values together with linear interpolation. + * The steps used are: + * -# Calculation of the nearest integer table index + * -# Compute the fractional portion (fract) of the table index. + * -# The final result equals (1.0f-fract)*a + fract*b; + * + * where + * 
+ *    b=Table[index+0];
+ *    c=Table[index+1];
+ *
+ */ + + /** + * @addtogroup cos + * @{ + */ + +/** + * @brief Fast approximation to the trigonometric cosine function for floating-point data. + * @param[in] x input value in radians. + * @return cos(x). + */ + +float32_t our_arm_cos_f32( + float32_t x) +{ + float32_t cosVal, fract, in; /* Temporary variables for input, output */ + uint16_t index; /* Index variable */ + float32_t a, b; /* Two nearest output values */ + int32_t n; + float32_t findex; + + /* input x is in radians */ + /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi, add 0.25 (pi/2) to read sine table */ + in = x * 0.159154943092f + 0.25f; + + /* Calculation of floor value of input */ + n = (int32_t) in; + + /* Make negative values towards -infinity */ + if (in < 0.0f) + { + n--; + } + + /* Map input value to [0 1] */ + in = in - (float32_t) n; + + /* Calculation of index of the table */ + findex = (float32_t)FAST_MATH_TABLE_SIZE * in; + index = (uint16_t)findex; + + /* when "in" is exactly 1, we need to rotate the index down to 0 */ + if (index >= FAST_MATH_TABLE_SIZE) { + index = 0; + findex -= (float32_t)FAST_MATH_TABLE_SIZE; + } + + /* fractional value calculation */ + fract = findex - (float32_t) index; + + /* Read two nearest values of input value from the cos table */ + a = sinTable_f32[index]; + b = sinTable_f32[index+1]; + + /* Linear interpolation process */ + cosVal = (1.0f-fract)*a + fract*b; + + /* Return the output value */ + return (cosVal); +} + +/** + * @} end of cos group + */ diff --git a/Firmware/MotorControl/arm_sin_f32.c b/Firmware/MotorControl/arm_sin_f32.c new file mode 100644 index 00000000..f037248f --- /dev/null +++ b/Firmware/MotorControl/arm_sin_f32.c @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sin_f32.c + * Description: Fast sine calculation for floating-point values + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 // TODO: might change in future board versions +#include "arm_math.h" +#include "arm_common_tables.h" + +/** + * @ingroup groupFastMath + */ + +/** + * @defgroup sin Sine + * + * Computes the trigonometric sine function using a combination of table lookup + * and linear interpolation. There are separate functions for + * Q15, Q31, and floating-point data types. + * The input to the floating-point version is in radians and in the range [0 2*pi) while the + * fixed-point Q15 and Q31 have a scaled input with the range + * [0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a + * value of 2*pi wraps around to 0. + * + * The implementation is based on table lookup using 256 values together with linear interpolation. + * The steps used are: + * -# Calculation of the nearest integer table index + * -# Compute the fractional portion (fract) of the table index. + * -# The final result equals (1.0f-fract)*a + fract*b; + * + * where + * 
+ *    b=Table[index+0];
+ *    c=Table[index+1];
+ *
+ */ + +/** + * @addtogroup sin + * @{ + */ + +/** + * @brief Fast approximation to the trigonometric sine function for floating-point data. + * @param[in] x input value in radians. + * @return sin(x). + */ + +float32_t our_arm_sin_f32( + float32_t x) +{ + float32_t sinVal, fract, in; /* Temporary variables for input, output */ + uint16_t index; /* Index variable */ + float32_t a, b; /* Two nearest output values */ + int32_t n; + float32_t findex; + + /* input x is in radians */ + /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */ + in = x * 0.159154943092f; + + /* Calculation of floor value of input */ + n = (int32_t) in; + + /* Make negative values towards -infinity */ + if (x < 0.0f) + { + n--; + } + + /* Map input value to [0 1] */ + in = in - (float32_t) n; + + /* Calculation of index of the table */ + findex = (float32_t)FAST_MATH_TABLE_SIZE * in; + index = (uint16_t)findex; + + /* when "in" is exactly 1, we need to rotate the index down to 0 */ + if (index >= FAST_MATH_TABLE_SIZE) { + index = 0; + findex -= (float32_t)FAST_MATH_TABLE_SIZE; + } + + /* fractional value calculation */ + fract = findex - (float32_t) index; + + /* Read two nearest values of input value from the sin table */ + a = sinTable_f32[index]; + b = sinTable_f32[index+1]; + + /* Linear interpolation process */ + sinVal = (1.0f-fract)*a + fract*b; + + /* Return the output value */ + return (sinVal); +} + +/** + * @} end of sin group + */ diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 59ba16a0..3ddf3cb1 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -103,19 +103,6 @@ void Axis::set_step_dir_enabled(bool enable) { } } -bool Axis::check_for_errors() { - // Maybe we should update this to only trigger on new errors? - // The danger with that is we could fail to bail on uncleared errors that still prevent - // correct opreation. - - // For now: we treat ERROR_INVALID_STATE in idle loop special, or we could never stay - // in idle after this kind of error. - if (current_state_ == AXIS_STATE_IDLE) - return (error_ & ~ERROR_INVALID_STATE) == ERROR_NONE; - else - return error_ == ERROR_NONE; -} - // @brief Do axis level checks and call subcomponent do_checks // Returns true if everything is ok. bool Axis::do_checks() { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index eac76f8c..e3d5ff2a 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -94,9 +94,14 @@ public: bool check_PSU_brownout(); bool do_checks(); bool do_updates(); - bool check_for_errors(); float get_temp(); + + // True if there are no errors + bool inline check_for_errors() { + return error_ == ERROR_NONE; + } + // @brief Runs the specified update handler at the frequency of the current measurements. // // The loop runs until one of the following conditions: @@ -126,8 +131,12 @@ public: // Note: updates run even if checks fail bool updates_ok = do_updates(); - if (!checks_ok || !updates_ok) - break; + if (!checks_ok || !updates_ok) { + // It's not useful to quit idle since that is the safe action + // Also leaving idle would rearm the motors + if (current_state_ != AXIS_STATE_IDLE) + break; + } // Run main loop function, defer quitting for after wait // TODO: change arming logic to arm after waiting diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 503ace83..f9b00849 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -7,6 +7,8 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, hw_config_(hw_config), config_(config) { + update_pll_gains(); + if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL)) { is_ready_ = true; } @@ -109,8 +111,8 @@ bool Encoder::run_index_search() { axis_->run_control_loop([&](){ phase = wrap_pm_pi(phase + omega * current_meas_period); - float v_alpha = voltage_magnitude * arm_cos_f32(phase); - float v_beta = voltage_magnitude * arm_sin_f32(phase); + 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)) return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); @@ -167,8 +169,8 @@ bool Encoder::run_offset_calibration() { i = 0; axis_->run_control_loop([&](){ float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); - float v_alpha = voltage_magnitude * arm_cos_f32(phase); - float v_beta = voltage_magnitude * arm_sin_f32(phase); + 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)) return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); @@ -208,8 +210,8 @@ bool Encoder::run_offset_calibration() { i = 0; axis_->run_control_loop([&](){ float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); - float v_alpha = voltage_magnitude * arm_cos_f32(phase); - float v_beta = voltage_magnitude * arm_sin_f32(phase); + 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)) return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); @@ -241,17 +243,17 @@ static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) { } } -bool Encoder::update() { - // Calculate encoder pll gains - float pll_kp = 2.0f * config_.bandwidth; // basic conversion to discrete time - float pll_ki = 0.25f * (pll_kp * pll_kp); // Critically damped +void Encoder::update_pll_gains() { + pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time + pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * pll_kp < 1.0f)) { + if (!(current_meas_period * pll_kp_ < 1.0f)) { set_error(ERROR_UNSTABLE_GAIN); - return false; } +} +bool Encoder::update() { // update internal encoder state. int32_t delta_enc = 0; switch (config_.mode) { @@ -294,12 +296,12 @@ bool Encoder::update() { float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(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_estimate_ += current_meas_period * pll_kp_ * delta_pos; + pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr; pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr)); - vel_estimate_ += current_meas_period * pll_ki * delta_pos_cpr; + vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr; bool snap_to_zero_vel = false; - if (fabsf(vel_estimate_) < 0.5f * current_meas_period * pll_ki) { + if (fabsf(vel_estimate_) < 0.5f * current_meas_period * pll_ki_) { vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter snap_to_zero_vel = true; } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 2162d10b..6eca0536 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -56,6 +56,8 @@ public: bool run_offset_calibration(); bool update(); + void update_pll_gains(); + const EncoderHardwareConfig_t& hw_config_; Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor @@ -70,8 +72,8 @@ public: float pos_estimate_ = 0.0f; // [rad] float pos_cpr_ = 0.0f; // [rad] float vel_estimate_ = 0.0f; // [rad/s] - // float pll_kp_ = 0.0f; // [rad/s / rad] - // float pll_ki_ = 0.0f; // [(rad/s^2) / rad] + float pll_kp_ = 0.0f; // [rad/s / rad] + float pll_ki_ = 0.0f; // [(rad/s^2) / rad] // Updated by low_level pwm_adc_cb uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC @@ -100,7 +102,8 @@ public: make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), make_protocol_property("offset_float", &config_.offset_float), - make_protocol_property("bandwidth", &config_.bandwidth), + make_protocol_property("bandwidth", &config_.bandwidth, + [](void* ctx) { static_cast(ctx)->update_pll_gains(); }, this), make_protocol_property("calib_range", &config_.calib_range) ) ); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 1782414c..ee71c98a 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -17,8 +17,8 @@ Motor::Motor(const MotorHardwareConfig_t& hw_config, .EngpioNumber = gate_driver_config_.enable_pin, .nCSgpioHandle = gate_driver_config_.nCS_port, .nCSgpioNumber = gate_driver_config_.nCS_pin, - }) -{ + }) { + update_current_controller_gains(); } // @brief Arms the PWM outputs that belong to this motor. @@ -62,11 +62,6 @@ void Motor::update_current_controller_gains() { current_control_.i_gain = plant_pole * current_control_.p_gain; } -void Motor::set_current_control_bandwidth(float current_control_bandwidth) { - config_.current_control_bandwidth = current_control_bandwidth; - update_current_controller_gains(); -} - // @brief Set up the gate drivers void Motor::DRV8301_setup() { // for reference: @@ -290,8 +285,8 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { // TODO: This doesn't update brake current // We should probably make FOC Current call FOC Voltage to avoid duplication. bool Motor::FOC_voltage(float v_d, float v_q, float phase) { - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); + float c = our_arm_cos_f32(phase); + float s = our_arm_sin_f32(phase); float v_alpha = c*v_d - s*v_q; float v_beta = c*v_q + s*v_d; return enqueue_voltage_timings(v_alpha, v_beta); @@ -309,8 +304,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC); // Park transform - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); + float c = our_arm_cos_f32(phase); + float s = our_arm_sin_f32(phase); float Id = c * Ialpha + s * Ibeta; float Iq = c * Ibeta - s * Ialpha; ictrl.Iq_measured = Iq; @@ -348,7 +343,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { // Inverse park transform float mod_alpha = c * mod_d - s * mod_q; - float mod_beta = c * mod_q + s * mod_d; + float mod_beta = c * mod_q + s * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) ictrl.final_v_alpha = mod_to_V * mod_alpha; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 235ca6a8..ebfd3a12 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -55,7 +55,7 @@ public: bool pre_calibrated = false; // can be set to true to indicate that all values here are valid int32_t pole_pairs = 7; float calibration_current = 10.0f; // [A] - float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. + float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. float phase_inductance = 0.0f; // to be set by measure_phase_inductance float phase_resistance = 0.0f; // to be set by measure_phase_resistance int32_t direction = 1; // 1 or -1 @@ -95,13 +95,11 @@ public: bool arm(); void disarm(); void setup() { - update_current_controller_gains(); DRV8301_setup(); } void reset_current_control(); void update_current_controller_gains(); - void set_current_control_bandwidth(float current_control_bandwidth); void DRV8301_setup(); bool check_DRV_fault(); void set_error(Error_t error); @@ -211,10 +209,9 @@ public: make_protocol_property("motor_type", &config_.motor_type), make_protocol_property("current_lim", &config_.current_lim), make_protocol_property("requested_current_range", &config_.requested_current_range), - make_protocol_ro_property("current_control_bandwidth", &config_.current_control_bandwidth) - ), - make_protocol_function("set_current_control_bandwidth", *this, &Motor::set_current_control_bandwidth, - "current_control_bandwidth") + make_protocol_property("current_control_bandwidth", &config_.current_control_bandwidth, + [](void* ctx) { static_cast(ctx)->update_current_controller_gains(); }, this) + ) ); } }; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 4f829564..74cce4ac 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -1,6 +1,13 @@ #ifndef __ODRIVE_MAIN_H #define __ODRIVE_MAIN_H +// Note on central include scheme by Samuel: +// there are circular dependencies between some of the header files, +// e.g. the Motor header needs a forward declaration of Axis and vice versa +// so I figured I'd make one main header that takes care of +// the forward declarations and right ordering +// btw this pattern is not so uncommon, for instance IIRC the stdlib uses it too + #ifdef __cplusplus #include extern "C" { diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 0ab35668..3145c19a 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -101,13 +101,14 @@ int mod(int dividend, int divisor); uint32_t deadline_to_timeout(uint32_t deadline_ms); uint32_t timeout_to_deadline(uint32_t timeout_ms); - int is_in_the_future(uint32_t time_ms); uint32_t micros(void); - void delay_us(uint32_t us); +float our_arm_sin_f32(float x); +float our_arm_cos_f32(float x); + #ifdef __cplusplus } #endif diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 2f81338e..49c325b3 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -105,6 +105,7 @@ LDFLAGS += '-Wl,--undefined=uxTopUsedPriority' -- common flags for ASM, C and C++ OPT += '-Og' +-- OPT += '-O0' OPT += '-ffast-math -fno-finite-math-only' tup.append_table(FLAGS, OPT) tup.append_table(LDFLAGS, OPT) @@ -148,6 +149,8 @@ build{ sources={ 'Drivers/DRV8301/drv8301.c', 'MotorControl/utils.c', + 'MotorControl/arm_sin_f32.c', + 'MotorControl/arm_cos_f32.c', 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp index d203e76d..e599c0d9 100644 --- a/Firmware/fibre/cpp/include/fibre/protocol.hpp +++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp @@ -431,8 +431,42 @@ private: typedef std::function EndpointHandler; +// @brief Default endpoint handler for const types +// @return: True if endpoint was written to, False otherwise template -void default_readwrite_endpoint_handler(endpoint_ref_t* value, const uint8_t* input, size_t input_length, StreamSink* output) { +std::enable_if_t::value && std::is_const::value, bool> +default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) { + // If the old value was requested, call the corresponding little endian serialization function + if (output) { + // TODO: make buffer size dependent on the type + uint8_t buffer[sizeof(T)]; + size_t cnt = write_le(*value, buffer); + if (cnt <= output->get_free_space()) + output->process_bytes(buffer, cnt, nullptr); + } + return false; // We don't ever write to const types +} + +// @brief Default endpoint handler for non-const types +template +std::enable_if_t::value && !std::is_const::value, bool> +default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) { + // Read the endpoint value into output + default_readwrite_endpoint_handler(const_cast(value), input, input_length, output); + + // If a new value was passed, call the corresponding little endian deserialization function + uint8_t buffer[sizeof(T)] = { 0 }; // TODO: make buffer size dependent on the type + if (input_length >= sizeof(buffer)) { + read_le(value, input); + return true; + } else { + return false; + } +} + +// @brief Default endpoint handler for endpoint_ref_t types +template +bool default_readwrite_endpoint_handler(endpoint_ref_t* value, const uint8_t* input, size_t input_length, StreamSink* output) { constexpr size_t size = sizeof(value->endpoint_id) + sizeof(value->json_crc); if (output) { // TODO: make buffer size dependent on the type @@ -447,39 +481,12 @@ void default_readwrite_endpoint_handler(endpoint_ref_t* value, const uint8_t* in if (input_length >= size) { read_leendpoint_id)>(&value->endpoint_id, input); read_lejson_crc)>(&value->json_crc, input + 2); + return true; + } else { + return false; } } - -// @brief Default endpoint handler for const types -template -std::enable_if_t::value && std::is_const::value> -default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) { - // If the old value was requested, call the corresponding little endian serialization function - if (output) { - // TODO: make buffer size dependent on the type - uint8_t buffer[sizeof(T)]; - size_t cnt = write_le(*value, buffer); - if (cnt <= output->get_free_space()) - output->process_bytes(buffer, cnt, nullptr); - } -} - -// @brief Default endpoint handler for non-const types -template -std::enable_if_t::value && !std::is_const::value> -default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) { - // Read the endpoint value into output - default_readwrite_endpoint_handler(const_cast(value), input, input_length, output); - - // If a new value was passed, call the corresponding little endian deserialization function - uint8_t buffer[sizeof(T)] = { 0 }; // TODO: make buffer size dependent on the type - if (input_length >= sizeof(buffer)) - read_le(value, input); -} - - - template static inline const char* get_default_json_modifier(); @@ -815,8 +822,9 @@ public: static constexpr const char * json_modifier = get_default_json_modifier(); static constexpr size_t endpoint_count = 1; - ProtocolProperty(const char * name, TProperty* property) - : name_(name), property_(property) + ProtocolProperty(const char * name, TProperty* property, + void (*written_hook)(void*), void* ctx) + : name_(name), property_(property), written_hook_(written_hook), ctx_(ctx) {} /* TODO: find out why the move constructor is not used when it could be @@ -892,38 +900,49 @@ public: list[id] = this; } void handle(const uint8_t* input, size_t input_length, StreamSink* output) final { - default_readwrite_endpoint_handler(property_, input, input_length, output); + bool wrote = default_readwrite_endpoint_handler(property_, input, input_length, output); + if (wrote && written_hook_ != nullptr) { + written_hook_(ctx_); + } } /*void handle(const uint8_t* input, size_t input_length, StreamSink* output) { handle(input, input_length, output); }*/ - const char * name_; + const char* name_; TProperty* property_; + void (*written_hook_)(void*); + void* ctx_; }; // Non-const non-enum types template::value)> -ProtocolProperty make_protocol_property(const char * name, TProperty* property) { - return ProtocolProperty(name, property); +ProtocolProperty make_protocol_property(const char * name, TProperty* property, + void (*written_hook)(void*) = nullptr, void* ctx = nullptr) { + return ProtocolProperty(name, property, written_hook, ctx); }; // Const non-enum types template::value)> -ProtocolProperty make_protocol_ro_property(const char * name, const TProperty* property) { - return ProtocolProperty(name, property); +ProtocolProperty make_protocol_ro_property(const char * name, TProperty* property, + void (*written_hook)(void*) = nullptr, void* ctx = nullptr) { + return ProtocolProperty(name, property, written_hook, ctx); }; // Non-const enum types template::value)> -ProtocolProperty> make_protocol_property(const char * name, TProperty* property) { - return ProtocolProperty>(name, reinterpret_cast*>(property)); +ProtocolProperty> make_protocol_property(const char * name, TProperty* property, + void (*written_hook)(void*) = nullptr, void* ctx = nullptr) { + return ProtocolProperty>( + name, reinterpret_cast*>(property), written_hook, ctx); }; // Const enum types template::value)> -ProtocolProperty> make_protocol_ro_property(const char * name, const TProperty* property) { - return ProtocolProperty>(name, reinterpret_cast*>(property)); +ProtocolProperty> make_protocol_ro_property(const char * name, TProperty* property, + void (*written_hook)(void*) = nullptr, void* ctx = nullptr) { + return ProtocolProperty>( + name, reinterpret_cast*>(property), written_hook, ctx); }; diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace index 250eb601..d858dcc3 100644 --- a/ODrive_Workspace.code-workspace +++ b/ODrive_Workspace.code-workspace @@ -44,7 +44,8 @@ "algorithm": "cpp", "chrono": "cpp", "condition_variable": "cpp", - "future": "cpp" + "future": "cpp", + "arm_math.h": "c" } } } diff --git a/docs/hoverboard.md b/docs/hoverboard.md index 5cf64332..76695be4 100644 --- a/docs/hoverboard.md +++ b/docs/hoverboard.md @@ -16,7 +16,7 @@ The motors are also fairly high inductance, so we need to reduce the bandwidth o ```txt odrv0.axis0.motor.config.resistance_calib_max_voltage = 4 odrv0.axis0.motor.config.requested_current_range = 25 #Requires config save and reboot -odrv0.axis0.motor.set_current_control_bandwidth(100) +odrv0.axis0.motor.config.current_control_bandwidth = 100 ``` Set the encoder to hall mode (instead of incremental). See the [pinout](interfaces.md#hall-feedback-pinout) for instructions on how to plug in the hall feedback.