wait for ODrive to boot up before sending control mode

zauberzeug · Aug 8, 2023 · 4659fb6 · 4659fb6
1 parent 047f720
commit 4659fb6
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 0 deletions.
diff --git a/main/modules/odrive_motor.cpp b/main/modules/odrive_motor.cpp
@@ -15,6 +15,9 @@ void ODriveMotor::subscribe_to_can() {
 }
 
 void ODriveMotor::set_mode(const uint8_t state, const uint8_t control_mode, const uint8_t input_mode) {
+    if (!this->is_boot_complete) {
+        return;
+    }
     if (this->axis_state != state) {
         this->can->send(this->can_id + 0x007, state, 0, 0, 0, 0, 0, 0, 0);
         this->axis_state = state;
@@ -53,6 +56,7 @@ void ODriveMotor::call(const std::string method_name, const std::vector<ConstExp
 }
 
 void ODriveMotor::handle_can_msg(const uint32_t id, const int count, const uint8_t *const data) {
+    this->is_boot_complete = true;
     switch (id - this->can_id) {
     case 0x009: {
         float tick;

diff --git a/main/modules/odrive_motor.h b/main/modules/odrive_motor.h
@@ -11,6 +11,7 @@ class ODriveMotor : public Module, public std::enable_shared_from_this<ODriveMot
 private:
     const uint32_t can_id;
     const Can_ptr can;
+    bool is_boot_complete = false;
     uint8_t axis_state = -1;
     uint8_t axis_control_mode = -1;
     uint8_t axis_input_mode = -1;