Merge branch 'devel'

Arduino/ODriveArduino/ODriveArduino.cpp

@@ -38,6 +38,14 @@ void ODriveArduino::SetVelocity(int motor_number, float velocity, float current_
     serial_ << "v " << motor_number  << " " << velocity << " " << current_feedforward << "\n";
 }
 
+void ODriveArduino::SetCurrent(int motor_number, float current) {
+    serial_ << "c " << motor_number << " " << current << "\n";
+}
+
+void ODriveArduino::TrapezoidalMove(int motor_number, float position){
+    serial_ << "t " << motor_number << " " << position << "\n";
+}
+
 float ODriveArduino::readFloat() {
     return readString().toFloat();
 }

Arduino/ODriveArduino/ODriveArduino.h

@@ -26,7 +26,8 @@ class ODriveArduino {
     void SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward);
     void SetVelocity(int motor_number, float velocity);
     void SetVelocity(int motor_number, float velocity, float current_feedforward);
-
+    void SetCurrent(int motor_number, float current);
+    void TrapezoidalMove(int motor_number, float position);
     // General params
     float readFloat();
     int32_t readInt();

CHANGELOG.md

@@ -1,12 +1,27 @@
 # Unreleased Features
 Please add a note of your changes below this heading if you make a Pull Request.
 
+### Added
+* `dump_errors()` utility function in odrivetool to dump, decode and optionally clear errors.
+* `f` command to ascii protocol to get encoder position and velocity feedback.
+* `q` command to ascii protocol. It is like the old `p` command, but velocity and current mean limits, not feed-forward.
+* `ss`, `se`, `sr` commands to ascii protocol, for save config, erase config and reboot.
+* `move_incremental` function for relative trajectory moves.
+* `encoder.config.ignore_illegal_hall_state` option.
+* `encoder.config.enable_phase_interpolation` option. Setting to false may reduce jerky pulsations at low speed when using hall sensor feedback.
+* Analog input. Used the same way as the PWM input mappings.
+* Voltage limit soft clamping instead of ERROR_MODULATION_MAGNITUDE in gimbal motor closed loop.
+* Thermal current limit with linear derating.
+
+### Fixed
+* Added required 1.5 cycle phase shift between ADC and PWM, lack thereof caused unstable current controller at high eRPM.
+
 # Releases
 ## [0.4.7] - 2018-11-28
 ### Added
 * Overspeed fault
 * Current sense saturation fault.
-* Supress startup transients by sampling encoder estimate into position setpoint when entering closed loop control.
+* Suppress startup transients by sampling encoder estimate into position setpoint when entering closed loop control.
 * Make step dir gpio pins configurable.
 * Configuration variable `encoder.config.zero_count_on_find_idx`, true by default. Set to false to leave the initial encoder count to be where the axis was at boot.
 * Circular position setpoint mode: position setpoints wrapped [0, cpr). Useful for infinite incremental position control.

Firmware/MotorControl/axis.cpp

@@ -141,20 +141,14 @@ bool Axis::do_updates() {
     return check_for_errors();
 }
 
-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;
     run_control_loop([&](){
         float phase = wrap_pm_pi(config_.ramp_up_distance * x);
         float I_mag = config_.spin_up_current * x;
         x += current_meas_period / config_.ramp_up_time;
-        if (!motor_.update(I_mag, phase))
+        if (!motor_.update(I_mag, phase, 0.0f))
             return error_ |= ERROR_MOTOR_FAILED, false;
         return x < 1.0f;
     });
@@ -168,7 +162,7 @@ bool Axis::run_sensorless_spin_up() {
         vel += config_.spin_up_acceleration * current_meas_period;
         phase = wrap_pm_pi(phase + vel * current_meas_period);
         float I_mag = config_.spin_up_current;
-        if (!motor_.update(I_mag, phase))
+        if (!motor_.update(I_mag, phase, vel))
             return error_ |= ERROR_MOTOR_FAILED, false;
         return vel < config_.spin_up_target_vel;
     });
@@ -190,7 +184,7 @@ bool Axis::run_sensorless_control_loop() {
         float current_setpoint;
         if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, &current_setpoint))
             return error_ |= ERROR_CONTROLLER_FAILED, false;
-        if (!motor_.update(current_setpoint, sensorless_estimator_.phase_))
+        if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_))
             return false; // set_error should update axis.error_
         return true;
     });
@@ -206,7 +200,8 @@ bool Axis::run_closed_loop_control_loop() {
         float current_setpoint;
         if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, &current_setpoint))
             return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
-        if (!motor_.update(current_setpoint, encoder_.phase_))
+        float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
+        if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel))
             return false; // set_error should update axis.error_
         return true;
     });

Firmware/MotorControl/axis.hpp

@@ -86,7 +86,6 @@ class Axis {
     bool check_PSU_brownout();
     bool do_checks();
     bool do_updates();
-    float get_temp();
 
 
     // True if there are no errors
@@ -212,7 +211,6 @@ class Axis {
                 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()),

Firmware/MotorControl/board_config_v3.h

@@ -23,7 +23,6 @@
 typedef struct {
     uint16_t step_gpio_pin;
     uint16_t dir_gpio_pin;
-    size_t thermistor_adc_ch;
     osPriority thread_priority;
 } AxisHardwareConfig_t;
 
@@ -42,6 +41,7 @@ typedef struct {
     TIM_HandleTypeDef* timer;
     uint16_t control_deadline;
     float shunt_conductance;
+    size_t inverter_thermistor_adc_ch;
 } MotorHardwareConfig_t;
 typedef struct {
     SPI_HandleTypeDef* spi;
@@ -74,7 +74,6 @@ const BoardHardwareConfig_t hw_configs[2] = { {
     .axis_config = {
         .step_gpio_pin = 1,
         .dir_gpio_pin = 2,
-        .thermistor_adc_ch = 15,
         .thread_priority = (osPriority)(osPriorityHigh + (osPriority)1),
     },
     .encoder_config = {
@@ -92,6 +91,7 @@ const BoardHardwareConfig_t hw_configs[2] = { {
         .timer = &htim1,
         .control_deadline = TIM_1_8_PERIOD_CLOCKS,
         .shunt_conductance = 1.0f / SHUNT_RESISTANCE,  //[S]
+        .inverter_thermistor_adc_ch = 15,
     },
     .gate_driver_config = {
         .spi = &hspi3,
@@ -112,11 +112,6 @@ const BoardHardwareConfig_t hw_configs[2] = { {
 #else
         .step_gpio_pin = 3,
         .dir_gpio_pin = 4,
-#endif
-#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
-        .thermistor_adc_ch = 4,
-#else
-        .thermistor_adc_ch = 1,
 #endif
         .thread_priority = osPriorityHigh,
     },
@@ -135,6 +130,11 @@ const BoardHardwareConfig_t hw_configs[2] = { {
         .timer = &htim8,
         .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2,
         .shunt_conductance = 1.0f / SHUNT_RESISTANCE,  //[S]
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
+        .inverter_thermistor_adc_ch = 4,
+#else
+        .inverter_thermistor_adc_ch = 1,
+#endif
     },
     .gate_driver_config = {
         .spi = &hspi3,

Firmware/MotorControl/controller.cpp

@@ -56,6 +56,15 @@ void Controller::move_to_pos(float goal_point) {
                                  axis_->trap_.config_.decel_limit);
     traj_start_loop_count_ = axis_->loop_counter_;
     config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL;
+    goal_point_ = goal_point;
+}
+
+void Controller::move_incremental(float displacement, bool from_goal_point = true){
+    if(from_goal_point){
+        move_to_pos(goal_point_ + displacement);
+    } else{
+        move_to_pos(pos_setpoint_ + displacement);
+    }
 }
 
 void Controller::start_anticogging_calibration() {
@@ -183,8 +192,8 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
     Iq += vel_integrator_current_;
 
     // Current limiting
-    float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current);
     bool limited = false;
+    float Ilim = axis_->motor_.effective_current_lim();
     if (Iq > Ilim) {
         limited = true;
         Iq = Ilim;

Firmware/MotorControl/controller.hpp

@@ -44,6 +44,7 @@ class Controller {
 
     // Trajectory-Planned control
     void move_to_pos(float goal_point);
+    void move_incremental(float displacement, bool from_goal_point);
 
     // TODO: make this more similar to other calibration loops
     void start_anticogging_calibration();
@@ -89,6 +90,8 @@ class Controller {
 
     uint32_t traj_start_loop_count_ = 0;
 
+    float goal_point_ = 0.0f;
+
     // Communication protocol definitions
     auto make_protocol_definitions() {
         return make_protocol_member_list(
@@ -114,8 +117,9 @@ class Controller {
             make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint,
                 "vel_setpoint", "current_feed_forward"),
             make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint,
-                "current_setpoint"),
-            make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "goal_point"),
+                                   "current_setpoint"),
+            make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"),
+            make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"),
             make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration)
         );
     }

Firmware/MotorControl/encoder.cpp

@@ -252,14 +252,35 @@ void Encoder::update_pll_gains() {
     }
 }
 
+void Encoder::sample_now() {
+    switch (config_.mode) {
+        case MODE_INCREMENTAL: {
+            tim_cnt_sample_ = (int16_t)hw_config_.timer->Instance->CNT;
+        } break;
+
+        case MODE_HALL: {
+            // do nothing: samples already captured in general GPIO capture
+        } break;
+
+        case MODE_SINCOS: {
+            sincos_sample_s_ = get_adc_voltage(GPIO_3_GPIO_Port, GPIO_3_Pin) / 3.3f;
+            sincos_sample_c_ = get_adc_voltage(GPIO_4_GPIO_Port, GPIO_4_Pin) / 3.3f;
+        } break;
+
+        default: {
+           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
+        } break;
+    }
+}
+
 bool Encoder::update() {
     // update internal encoder state.
     int32_t delta_enc = 0;
     switch (config_.mode) {
         case MODE_INCREMENTAL: {
             //TODO: use count_in_cpr_ instead as shadow_count_ can overflow
             //or use 64 bit
-            int16_t delta_enc_16 = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_;
+            int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_;
             delta_enc = (int32_t)delta_enc_16; //sign extend
         } break;
 
@@ -271,10 +292,23 @@ bool Encoder::update() {
                 if (delta_enc > 3)
                     delta_enc -= 6;
             } else {
-                set_error(ERROR_ILLEGAL_HALL_STATE);
-                return false;
+                if (!config_.ignore_illegal_hall_state) {
+                    set_error(ERROR_ILLEGAL_HALL_STATE);
+                    return false;
+                }
             }
         } break;
+
+        case MODE_SINCOS: {
+            float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_);
+            int fake_count = (int)(1000.0f * phase);
+            //CPR = 6283 = 2pi * 1k
+
+            delta_enc = fake_count - count_in_cpr_;
+            delta_enc = mod(delta_enc, 6283);
+            if (delta_enc > 6283/2)
+                delta_enc -= 6283;
+        } break;
 
         default: {
            set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
@@ -308,7 +342,7 @@ bool Encoder::update() {
     //// run encoder count interpolation
     int32_t corrected_enc = count_in_cpr_ - config_.offset;
     // if we are stopped, make sure we don't randomly drift
-    if (snap_to_zero_vel) {
+    if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
         interpolation_ = 0.5f;
     // reset interpolation if encoder edge comes
     } else if (delta_enc > 0) {

Firmware/MotorControl/encoder.hpp

@@ -19,7 +19,8 @@ class Encoder {
 
     enum Mode_t {
         MODE_INCREMENTAL,
-        MODE_HALL
+        MODE_HALL,
+        MODE_SINCOS
     };
 
     struct Config_t {
@@ -35,8 +36,10 @@ class Encoder {
         int32_t cpr = (2048 * 4);   // Default resolution of CUI-AMT102 encoder,
         int32_t offset = 0;        // Offset between encoder count and rotor electrical phase
         float offset_float = 0.0f; // Sub-count phase alignment offset
-        float calib_range = 0.02f;
+        bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state
+        float calib_range = 0.02f; // Accuracy required to pass encoder cpr check
         float bandwidth = 1000.0f;
+        bool ignore_illegal_hall_state = false;
     };
 
     Encoder(const EncoderHardwareConfig_t& hw_config,
@@ -55,6 +58,7 @@ class Encoder {
 
     bool run_index_search();
     bool run_offset_calibration();
+    void sample_now();
     bool update();
 
     void update_pll_gains();
@@ -76,8 +80,11 @@ class Encoder {
     float pll_kp_ = 0.0f;   // [count/s / count]
     float pll_ki_ = 0.0f;   // [(count/s^2) / count]
 
+    int16_t tim_cnt_sample_ = 0; // 
     // Updated by low_level pwm_adc_cb
     uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC
+    float sincos_sample_s_ = 0.0f;
+    float sincos_sample_c_ = 0.0f;
 
     // Communication protocol definitions
     auto make_protocol_definitions() {
@@ -104,9 +111,11 @@ class Encoder {
                 make_protocol_property("cpr", &config_.cpr),
                 make_protocol_property("offset", &config_.offset),
                 make_protocol_property("offset_float", &config_.offset_float),
+                make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation),
                 make_protocol_property("bandwidth", &config_.bandwidth,
                     [](void* ctx) { static_cast<Encoder*>(ctx)->update_pll_gains(); }, this),
-                make_protocol_property("calib_range", &config_.calib_range)
+                make_protocol_property("calib_range", &config_.calib_range),
+                make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state)
             )
         );
     }