add reset for flight controller

toppers · Nov 27, 2024 · 7d26ba8 · 7d26ba8
1 parent ade88e7
commit 7d26ba8
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 1 deletion.
diff --git a/drone_control/common/include/drone_controller.hpp b/drone_control/common/include/drone_controller.hpp
@@ -30,6 +30,13 @@ class DroneController {
         head = std::make_unique<DroneHeadingController>(loader);
         angle = std::make_unique<DroneAngleController>(loader);
     }
+    void reset() {
+        this->loader.reload();
+        alt->reset();
+        pos->reset();
+        head->reset();
+        angle->reset();
+    }
 };
 
 #endif /* _DRONE_CONTROLLER_HPP_ */
diff --git a/drone_control/workspace/FlightController/hako_module_drone_controller_impl.cpp b/drone_control/workspace/FlightController/hako_module_drone_controller_impl.cpp
@@ -19,8 +19,13 @@ int hako_module_drone_controller_impl_is_operation_doing(void *)
     return true;
 }
 
-int hako_module_drone_controller_impl_init(void*)
+int hako_module_drone_controller_impl_init(void* context)
 {
+    if (context != nullptr) {
+        DroneController* ctrl = (DroneController*)context;
+        ctrl->reset();
+        return 0;
+    }
     return 0;
 }
 mi_drone_control_out_t hako_module_drone_controller_impl_run(mi_drone_control_in_t *in)