From 104e4413c9d9e00274cf190c0e336ad3acfcc55d Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sat, 25 May 2019 14:07:44 +0200
Subject: [PATCH] Fix input_filter integration issues

---
 Firmware/MotorControl/axis.cpp        |  5 ++++-
 Firmware/MotorControl/controller.cpp  |  5 ++++-
 Firmware/MotorControl/controller.hpp  |  2 +-
 Firmware/communication/can_simple.cpp | 11 +++++++----
 4 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index 6589532e..b2c10ff4 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -288,7 +288,10 @@ bool Axis::run_closed_loop_control_loop() {
         if (homing_state_ == HOMING_STATE_HOMING) {
             if (min_endstop_.getEndstopState()) {
                 encoder_.set_linear_count(min_endstop_.config_.offset);
-                controller_.set_pos_setpoint(0.0f, 0.0f, 0.0f);
+                controller_.pos_setpoint_ = 0.0f;
+                controller_.vel_setpoint_ = 0.0f;
+                controller_.current_setpoint_ = 0.0f;
+                controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;
                 homing_state_ = HOMING_STATE_MOVE_TO_ZERO;
             }
         } else if (homing_state_ == HOMING_STATE_MOVE_TO_ZERO) {
diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 35eb3029..7063f081 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -59,7 +59,10 @@ void Controller::start_anticogging_calibration() {
 // When pressed, set the linear count to the offset (default 0), and then
 bool Controller::home_axis() {
     if (axis_->min_endstop_.config_.enabled) {
-        set_vel_setpoint(-config_.homing_speed, 0.0f);
+        config_.control_mode = CTRL_MODE_VELOCITY_CONTROL;
+        pos_setpoint_ = 0.0f;
+        vel_setpoint_ = -config_.homing_speed;
+        current_setpoint_ = 0.0f;
         axis_->homing_state_ = HOMING_STATE_HOMING;
     } else {
         return false;
diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp
index d008adac..5673a9d4 100644
--- a/Firmware/MotorControl/controller.hpp
+++ b/Firmware/MotorControl/controller.hpp
@@ -133,7 +133,7 @@ public:
                 make_protocol_property("vel_limit", &config_.vel_limit),
                 make_protocol_property("vel_limit_tolerance", &config_.vel_limit_tolerance),
                 make_protocol_property("vel_ramp_rate", &config_.vel_ramp_rate),
-                make_protocol_property("homing_speed", &config_.homing_speed)
+                make_protocol_property("homing_speed", &config_.homing_speed),
                 make_protocol_property("inertia", &config_.inertia),
                 make_protocol_property("input_filter_bandwidth", &config_.input_filter_bandwidth,
                     [](void* ctx) { static_cast<Controller*>(ctx)->update_filter_gains(); }, this)
diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp
index 81502c8e..44f3844f 100644
--- a/Firmware/communication/can_simple.cpp
+++ b/Firmware/communication/can_simple.cpp
@@ -280,15 +280,18 @@ void CANSimple::move_to_pos_callback(Axis* axis, can_Message_t& msg) {
 }
 
 void CANSimple::set_pos_setpoint_callback(Axis* axis, can_Message_t& msg) {
-    axis->controller_.set_pos_setpoint(can_getSignal<int32_t>(msg, 0, 32, true, 1, 0), can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0), can_getSignal<int16_t>(msg, 48, 16, true, 0.01f, 0));
+    axis->controller_.pos_setpoint_ = can_getSignal<int32_t>(msg, 0, 32, true, 1, 0);
+    axis->controller_.vel_setpoint_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0);
+    axis->controller_.current_setpoint_ = can_getSignal<int16_t>(msg, 48, 16, true, 0.01f, 0);
 }
 
 void CANSimple::set_vel_setpoint_callback(Axis* axis, can_Message_t& msg) {
-    axis->controller_.set_vel_setpoint(can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f), can_getSignal<int32_t>(msg, 4, 32, true, 0.01f, 0.0f));
+    axis->controller_.vel_setpoint_ = can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f);
+    axis->controller_.current_setpoint_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.01f, 0.0f);
 }
 
 void CANSimple::set_current_setpoint_callback(Axis* axis, can_Message_t& msg) {
-    axis->controller_.set_current_setpoint(can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0));
+    axis->controller_.current_setpoint_ = can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0);
 }
 
 void CANSimple::set_vel_limit_callback(Axis* axis, can_Message_t& msg) {
@@ -309,7 +312,7 @@ void CANSimple::set_traj_accel_limits_callback(Axis* axis, can_Message_t& msg) {
 }
 
 void CANSimple::set_traj_A_per_css_callback(Axis* axis, can_Message_t& msg) {
-    axis->trap_.config_.A_per_css = can_getSignal<float>(msg, 0, 32, true, 1, 0);
+    axis->controller_.config_.inertia = can_getSignal<float>(msg, 0, 32, true, 1, 0);
 }
 
 void CANSimple::get_iq_callback(Axis* axis, can_Message_t& msg) {