[module] predict touchdown in case of kill (#3039)

* predict touchdown in case of kill * remove message * make variable extern and rename
2026-10-02 12:23:17 +08:00 · 2023-09-14 17:06:52 +02:00
parent 6435c9fc43
commit 556e03ef28
3 changed files with 111 additions and 0 deletions
@@ -0,0 +1,24 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ballistic_touchdown" dir="nav">
+  <doc>
+    <description>
+      Module that predicts the crash location if the motors would be
+      turned off, assuming a ballistic trajectory based on the current
+      velocity. Suggested use: trigger exceptions in the flight plan with
+      the ballistic_pos variable.
+    </description>
+  </doc>
+  <dep>
+    <depends>@navigation</depends>
+  </dep>
+  <header>
+    <file name="ballistic_touchdown.h"/>
+  </header>
+  <init fun="ballistic_touchdown_init()"/>
+  <periodic fun="ballistic_touchdown_run()" autorun="TRUE"/>
+  <makefile target="ap|nps">
+    <file name="ballistic_touchdown.c"/>
+  </makefile>
+</module>
+
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2023 Ewoud Smeur
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "state.h"
+
+float g = -9.81f;
+
+struct FloatVect2 ballistic_pos;
+
+void ballistic_touchdown_init(void) {
+  // nothing to be done here
+}
+
+/**
+ * Function that predicts the ballistic crash location
+ *
+ * Uses ENU coordinates (vertical up!)
+ **/
+void ballistic_touchdown_run(void) {
+
+  struct EnuCoor_f * v = stateGetSpeedEnu_f();
+  float vz = v->z;
+
+  struct FloatVect2 vh;
+  VECT2_ASSIGN(vh, v->x, v->y);
+
+  float h = fabsf(stateGetPositionEnu_f()->z); // Should be height above ground, make sure to initialize local frame on ground
+
+  // With h always larger than 0, the sqrt can never give nan
+  float time_fall = (-vz - sqrtf(vz*vz -2.f*h*g))/g;
+
+  struct FloatVect2 crash_offset;
+
+  VECT2_SMUL(crash_offset, vh, time_fall);
+
+  struct FloatVect2 pos;
+  pos.x = stateGetPositionEnu_f()->x;
+  pos.y = stateGetPositionEnu_f()->y;
+
+  // The predicted crash position is the current drone position + fall distance
+  VECT2_SUM(ballistic_pos, pos, crash_offset);
+}
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) 2023 Ewoud Smeur
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+void ballistic_touchdown_init(void);
+void ballistic_touchdown_run(void);
+
+extern struct FloatVect2 ballistic_pos;