From 48b80fb9edff20841d62217fceb423ac287575e1 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <21057728+passervr@users.noreply.github.com>
Date: Tue, 14 Nov 2023 17:37:08 +0100
Subject: [PATCH] Update quadcopter support

---
 Propulsion.cpp | 9 +++++++--
 Propulsion.h   | 3 ++-
 Quadcopter.cpp | 5 +++++
 Quadcopter.h   | 1 +
 4 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/Propulsion.cpp b/Propulsion.cpp
index 4899031..3408dff 100644
--- a/Propulsion.cpp
+++ b/Propulsion.cpp
@@ -33,8 +33,8 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
 
 }
 
-void Propulsion::AddQuadcopter() {
-    this->quadcopter = new Quadcopter();
+void Propulsion::AddQuadcopter(Quadcopter* quadcoper) {
+    this->quadcopter = quadcopter;
 }
 
 unsigned int Propulsion::GetMotorCount() {
@@ -105,6 +105,11 @@ void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) {
   }
 }
 
+void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
+    if (quadcopter != nullptr)
+        quadcopter->SetTwistSpeed(linear, yaw);
+}
+
 void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) {
   float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1, 1);
   float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity, -1, 1);
diff --git a/Propulsion.h b/Propulsion.h
index 5e21f32..3d6c70f 100644
--- a/Propulsion.h
+++ b/Propulsion.h
@@ -22,7 +22,7 @@ class Propulsion {
 
   //void AddMotors(MotorPlacement* motors, unsigned int motorCount);
   void AddMotors(Placement* motors, unsigned int motorCount);
-  void AddQuadcopter();
+  void AddQuadcopter(Quadcopter* quadcopter);
 
   unsigned int GetMotorCount();
   Motor* GetMotor(unsigned int motorIx);
@@ -32,6 +32,7 @@ class Propulsion {
 
   void SetTwistSpeed(float forward, float yaw);
   void SetTwistSpeed(float forward, float yaw, float pitch);
+  void SetTwistSpeed(Vector3 linear, float yaw);
   void SetTwistVelocity(float forward, float yaw);
 
   // Think: drones
diff --git a/Quadcopter.cpp b/Quadcopter.cpp
index 1e9607d..1326a91 100644
--- a/Quadcopter.cpp
+++ b/Quadcopter.cpp
@@ -9,6 +9,11 @@ void Quadcopter::LinearMotion(Vector3 velocity, float yawSpeed, float rollSpeed)
 	this->rollSpeed = rollSpeed;
 }
 
+void Quadcopter::SetTwistSpeed(Vector3 velocity, float yawSpeed) {
+	this->velocity = velocity;
+	this->yawSpeed = yawSpeed;
+}
+
 void Quadcopter::SetTwistSpeed(float forward, float yaw, float pitch) {
 	this->velocity = Vector3::forward * forward;
 	this->yawSpeed = yaw;
diff --git a/Quadcopter.h b/Quadcopter.h
index 81db84b..5d62f78 100644
--- a/Quadcopter.h
+++ b/Quadcopter.h
@@ -9,6 +9,7 @@ public:
 
 	void LinearMotion(Vector3 velocity, float yawSpeed = 0.0F, float rollSpeed = 0.0);
 	void SetTwistSpeed(float forward, float yaw, float pitch);
+    void SetTwistSpeed(Vector3 velocity, float yawSpeed = 0.0F);
 
 	Vector3 GetTargetVelocity();
 	float GetYawSpeed();