From 556e03ef281743a345216a9239cbeddc45d79afb Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Thu, 14 Sep 2023 17:06:52 +0200 Subject: [PATCH] [module] predict touchdown in case of kill (#3039) * predict touchdown in case of kill * remove message * make variable extern and rename --- conf/modules/ballistic_touchdown.xml | 24 ++++++++ sw/airborne/modules/nav/ballistic_touchdown.c | 61 +++++++++++++++++++ sw/airborne/modules/nav/ballistic_touchdown.h | 26 ++++++++ 3 files changed, 111 insertions(+) create mode 100644 conf/modules/ballistic_touchdown.xml create mode 100644 sw/airborne/modules/nav/ballistic_touchdown.c create mode 100644 sw/airborne/modules/nav/ballistic_touchdown.h diff --git a/conf/modules/ballistic_touchdown.xml b/conf/modules/ballistic_touchdown.xml new file mode 100644 index 0000000000..5b53c6f361 --- /dev/null +++ b/conf/modules/ballistic_touchdown.xml @@ -0,0 +1,24 @@ + + + + + + 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. + + + + @navigation + +
+ +
+ + + + + + + diff --git a/sw/airborne/modules/nav/ballistic_touchdown.c b/sw/airborne/modules/nav/ballistic_touchdown.c new file mode 100644 index 0000000000..67698294db --- /dev/null +++ b/sw/airborne/modules/nav/ballistic_touchdown.c @@ -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); +} diff --git a/sw/airborne/modules/nav/ballistic_touchdown.h b/sw/airborne/modules/nav/ballistic_touchdown.h new file mode 100644 index 0000000000..d6db6ad706 --- /dev/null +++ b/sw/airborne/modules/nav/ballistic_touchdown.h @@ -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;