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

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".

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;
     }

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<Encoder*>(ctx)->update_pll_gains(); }, this),
                 make_protocol_property("calib_range", &config_.calib_range)
             )
         );

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:

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<Motor*>(ctx)->update_current_controller_gains(); }, this)
+            )
         );
     }
 };

Firmware/fibre/cpp/include/fibre/protocol.hpp

@@ -431,8 +431,42 @@ private:
 typedef std::function<void(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output)> EndpointHandler;
 
 
+// @brief Default endpoint handler for const types
+// @return: True if endpoint was written to, False otherwise
 template<typename T>
-void default_readwrite_endpoint_handler(endpoint_ref_t* value, const uint8_t* input, size_t input_length, StreamSink* output) {
+std::enable_if_t<!std::is_same<T, endpoint_ref_t>::value && std::is_const<T>::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<T>(*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<typename T>
+std::enable_if_t<!std::is_same<T, endpoint_ref_t>::value && !std::is_const<T>::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 T>(const_cast<const T*>(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<T>(value, input);
+        return true;
+    } else {
+        return false;
+    }
+}
+
+// @brief Default endpoint handler for endpoint_ref_t types
+template<typename T>
+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_le<decltype(value->endpoint_id)>(&value->endpoint_id, input);
         read_le<decltype(value->json_crc)>(&value->json_crc, input + 2);
+        return true;
+    } else {
+        return false;
     }
 }
 
-
-// @brief Default endpoint handler for const types
-template<typename T>
-std::enable_if_t<!std::is_same<T, endpoint_ref_t>::value && std::is_const<T>::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<T>(*value, buffer);
-        if (cnt <= output->get_free_space())
-            output->process_bytes(buffer, cnt, nullptr);
-    }
-}
-
-// @brief Default endpoint handler for non-const types
-template<typename T>
-std::enable_if_t<!std::is_same<T, endpoint_ref_t>::value && !std::is_const<T>::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 T>(const_cast<const T*>(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<T>(value, input);
-}
-
-
-
 template<typename T>
 static inline const char* get_default_json_modifier();
 
@@ -815,8 +822,9 @@ public:
     static constexpr const char * json_modifier = get_default_json_modifier<TProperty>();
     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<TProperty>(property_, input, input_length, output);
+        bool wrote = default_readwrite_endpoint_handler<TProperty>(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<typename TProperty, ENABLE_IF(!std::is_enum<TProperty>::value)>
-ProtocolProperty<TProperty> make_protocol_property(const char * name, TProperty* property) {
-    return ProtocolProperty<TProperty>(name, property);
+ProtocolProperty<TProperty> make_protocol_property(const char * name, TProperty* property,
+        void (*written_hook)(void*) = nullptr, void* ctx = nullptr) {
+    return ProtocolProperty<TProperty>(name, property, written_hook, ctx);
 };
 
 // Const non-enum types
 template<typename TProperty, ENABLE_IF(!std::is_enum<TProperty>::value)>
-ProtocolProperty<const TProperty> make_protocol_ro_property(const char * name, const TProperty* property) {
-    return ProtocolProperty<const TProperty>(name, property);
+ProtocolProperty<const TProperty> make_protocol_ro_property(const char * name, TProperty* property,
+        void (*written_hook)(void*) = nullptr, void* ctx = nullptr) {
+    return ProtocolProperty<const TProperty>(name, property, written_hook, ctx);
 };
 
 // Non-const enum types
 template<typename TProperty, ENABLE_IF(std::is_enum<TProperty>::value)>
-ProtocolProperty<std::underlying_type_t<TProperty>> make_protocol_property(const char * name, TProperty* property) {
-    return ProtocolProperty<std::underlying_type_t<TProperty>>(name, reinterpret_cast<std::underlying_type_t<TProperty>*>(property));
+ProtocolProperty<std::underlying_type_t<TProperty>> make_protocol_property(const char * name, TProperty* property,
+        void (*written_hook)(void*) = nullptr, void* ctx = nullptr) {
+    return ProtocolProperty<std::underlying_type_t<TProperty>>(
+            name, reinterpret_cast<std::underlying_type_t<TProperty>*>(property), written_hook, ctx);
 };
 
 // Const enum types
 template<typename TProperty, ENABLE_IF(std::is_enum<TProperty>::value)>
-ProtocolProperty<const std::underlying_type_t<TProperty>> make_protocol_ro_property(const char * name, const TProperty* property) {
-    return ProtocolProperty<const std::underlying_type_t<TProperty>>(name, reinterpret_cast<const std::underlying_type_t<TProperty>*>(property));
+ProtocolProperty<const std::underlying_type_t<TProperty>> make_protocol_ro_property(const char * name, TProperty* property,
+        void (*written_hook)(void*) = nullptr, void* ctx = nullptr) {
+    return ProtocolProperty<const std::underlying_type_t<TProperty>>(
+            name, reinterpret_cast<const std::underlying_type_t<TProperty>*>(property), written_hook, ctx);
 };
 
 

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.