Merge branch 'devel' into Endstops

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

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 <stm32f4xx_hal.h>  // 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 <code>(1.0f-fract)*a + fract*b;</code>
+ *
+ * where
+ * <pre>
+ *    b=Table[index+0];
+ *    c=Table[index+1];
+ * </pre>
+ */
+
+ /**
+ * @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
+ */

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 <stm32f4xx_hal.h>  // 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 <code>(1.0f-fract)*a + fract*b;</code>
+ *
+ * where
+ * <pre>
+ *    b=Table[index+0];
+ *    c=Table[index+1];
+ * </pre>
+ */
+
+/**
+ * @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
+ */

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() {

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

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

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:
@@ -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;

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

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 <fibre/protocol.hpp>
 extern "C" {

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

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',

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

ODrive_Workspace.code-workspace

@@ -44,7 +44,8 @@
 			"algorithm": "cpp",
 			"chrono": "cpp",
 			"condition_variable": "cpp",
-			"future": "cpp"
+			"future": "cpp",
+			"arm_math.h": "c"
 		}
 	}
 }

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.