From 3b949b2bf62bf3ff0b3e56e14b3b695ddc7f6537 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tam=C3=A1s=20Nepusz?= <ntamas@gmail.com>
Date: Thu, 19 Nov 2020 10:48:16 +0100
Subject: [PATCH] Added takeoff/land with average velocity commands to public
 API of the HL commander (#651)

---
 .../interface/crtp_commander_high_level.h     | 21 ++++++++++++++
 src/modules/src/crtp_commander_high_level.c   | 28 +++++++++++++++++++
 2 files changed, 49 insertions(+)

diff --git a/src/modules/interface/crtp_commander_high_level.h b/src/modules/interface/crtp_commander_high_level.h
index 8b859ddf4b..d4fc18a105 100644
--- a/src/modules/interface/crtp_commander_high_level.h
+++ b/src/modules/interface/crtp_commander_high_level.h
@@ -81,6 +81,17 @@ bool crtpCommanderHighLevelIsStopped();
  */
 void crtpCommanderHighLevelTakeoff(const float absoluteHeight_m, const float duration_s);
 
+/**
+ * @brief vertical takeoff from current x-y position to given absolute or relative
+ * height with given velocity
+ *
+ * @param height_m         absolute or relative target height (m)
+ * @param velocity_m_s     takeoff velocity (m/s)
+ * @param relative         whether the height is relative to the current position
+ */
+void crtpCommanderHighLevelTakeoffWithVelocity(const float height_m, const float velocity_m_s, bool relative);
+
+
 /**
  * @brief vertical takeoff from current x-y position to given height
  *
@@ -98,6 +109,16 @@ void crtpCommanderHighLevelTakeoffYaw(const float absoluteHeight_m, const float
  */
 void crtpCommanderHighLevelLand(const float absoluteHeight_m, const float duration_s);
 
+/**
+ * @brief vertical land from current x-y position to given absolute or relative
+ * height with given velocity
+ *
+ * @param height_m         absolute or relative target height (m)
+ * @param velocity_m_s     landing velocity (m/s)
+ * @param relative         whether the height is relative to the current position
+ */
+void crtpCommanderHighLevelLandWithVelocity(const float height_m, const float velocity_m_s, bool relative);
+
 /**
  * @brief vertical land from current x-y position to given height
  *
diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c
index 5672b362fe..02fa21f40f 100644
--- a/src/modules/src/crtp_commander_high_level.c
+++ b/src/modules/src/crtp_commander_high_level.c
@@ -665,6 +665,20 @@ void crtpCommanderHighLevelTakeoffYaw(const float absoluteHeight_m, const float
   handleCommand(COMMAND_TAKEOFF_2, (const uint8_t*)&data);
 }
 
+void crtpCommanderHighLevelTakeoffWithVelocity(const float height_m, const float velocity_m_s, bool relative)
+{
+  struct data_takeoff_with_velocity data =
+  {
+    .height = height_m,
+    .heightIsRelative = relative,
+    .velocity = velocity_m_s,
+    .useCurrentYaw = true,
+    .groupMask = ALL_GROUPS,
+  };
+
+  handleCommand(COMMAND_TAKEOFF_WITH_VELOCITY, (const uint8_t*)&data);
+}
+
 void crtpCommanderHighLevelLand(const float absoluteHeight_m, const float duration_s)
 {
   struct data_land_2 data =
@@ -678,6 +692,20 @@ void crtpCommanderHighLevelLand(const float absoluteHeight_m, const float durati
   handleCommand(COMMAND_LAND_2, (const uint8_t*)&data);
 }
 
+void crtpCommanderHighLevelLandWithVelocity(const float height_m, const float velocity_m_s, bool relative)
+{
+  struct data_land_with_velocity data =
+  {
+    .height = height_m,
+    .heightIsRelative = relative,
+    .velocity = velocity_m_s,
+    .useCurrentYaw = true,
+    .groupMask = ALL_GROUPS,
+  };
+
+  handleCommand(COMMAND_LAND_WITH_VELOCITY, (const uint8_t*)&data);
+}
+
 void crtpCommanderHighLevelLandYaw(const float absoluteHeight_m, const float duration_s, const float yaw)
 {
   struct data_land_2 data =