diff --git a/CHANGELOG.md b/CHANGELOG.md index a8b1bbba..1257724b 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,10 @@ 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` ### Fixed * 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". diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 77692bfd..b4bb288f 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; } @@ -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 aa00f052..5f518adb 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: diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 235ca6a8..051c0228 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -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/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/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.