Wait for shaking to take-off (#3487)

* Wait for shaking to take-off * Update sw/airborne/modules/nav/nav_shakestart.c * Use Shake-Start in Flightplan * Remove disable-GPS, ahrsAligned, DSM_On/Off, Wind * fix make tests
2026-05-09 22:49:53 +08:00 · 2025-07-07 10:15:56 +02:00
parent 6497ecf431
commit 0d9a3a1402
5 changed files with 297 additions and 1 deletions
@@ -0,0 +1,130 @@
+<?xml version="1.0"?>
+<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
+<flight_plan alt="75" ground_alt="1" qfu="340" lat0="52.1683" lon0="4.4133" max_dist_from_home="1000" name="Basic" security_height="70">
+  <header>
+#include "modules/datalink/datalink.h"
+</header>
+  <waypoints>
+    <waypoint name="HOME" x="8.067" y="47.748"/>
+    <waypoint name="STDBY" x="-67.392" y="15.998"/>
+    <waypoint name="1" x="-268.538" y="2.34"/>
+    <waypoint name="2" x="230.06" y="188.096"/>
+    <waypoint name="MOB" x="137.0" y="-11.6"/>
+    <waypoint alt="30.0" name="AF" x="177.4" y="45.1"/>
+    <waypoint alt="0.0" name="TD" x="-9.629" y="78.603"/>
+    <waypoint name="_BASELEG" x="168.8" y="-13.8"/>
+    <waypoint name="DESTINATION" lat="52.1632" lon="4.41162"/>
+    <waypoint name="EST" x="10" y="10"/>
+  </waypoints>
+  <variables>
+    <variable var="flightplan_disable_gps_seconds" init="30" type="int" min="1" max="180" step="1"/>
+  </variables>
+  <modules>
+  	<module name="nav_shakestart"/>
+  </modules>
+  <exceptions>
+    <!-- WARNING: No exceptions: this flightplan is not safe /-->
+  </exceptions>
+  <blocks>
+    <block name="Wait GPS">
+      <!-- call_once fun="iomcu_dsm_off()"/ -->
+      <set value="1" var="autopilot.kill_throttle"/>
+      <call_once fun="nav_shakestart_reset()"/>
+      <while cond="!GpsFixValid()"/> <!-- || !ahrsIsAligned()" -->
+    </block>
+    <block name="Geo init">
+      <set value="1" var="autopilot.kill_throttle"/>
+      <call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
+      <while cond="LessThan(NavBlockTime(), 2)"/>
+      <call_once fun="nav_shakestart_reset()"/>
+      <call_once fun="NavSetGroundReferenceHere()"/>
+    </block>
+    <block name="WaitForShake">
+      <!-- Re-initialize if GPS was not good... -->
+      <!--exception cond="GetPosAlt() @GT (GetAltRef()+7)" deroute="Wait GPS"/>
+      <exception cond="GetPosAlt() @LT (GetAltRef()-7)" deroute="Wait GPS"/-->
+      <set value="1" var="autopilot.kill_throttle"/>
+      <call_once fun="nav_shakestart_reset()"/>
+      <call fun="nav_shakestart_run()"/>
+    </block>
+    <block key="t" name="Takeoff" strip_button="Takeoff (phi0,theta20)" strip_icon="takeoff.png" group="home">
+      <exception cond="GetPosAlt() @GT GetAltRef()+25" deroute="Climb"/>
+      <set value="0" var="autopilot.kill_throttle"/>
+      <set value="1" var="autopilot.launch"/>
+      <set value="0" var="autopilot.flight_time"/>
+      <call_once fun="NavSetWaypointHere(WP_TD)"/>
+      <call_once fun="NavSetWaypointDistBehind(WP_AF,WP_TD,200)"/>
+      <call_once fun="NavSetWaypointDistBehind(WP_STDBY,WP_TD,-200)"/>
+      <attitude roll="0" pitch="20" throttle="0.8" vmode="throttle"/>
+    </block>
+    <block name="Climb">
+      <exception cond="(GetPosAlt() @GT  (WaypointAlt(WP_STDBY)-10)) || block_time @GT 60" deroute="Wind"/>
+      <circle radius="nav_radius" wp="STDBY"/>
+    </block>
+    <block name="Holding point">
+      <!--set var="nav_mode" value="NAV_MODE_ROLL"/-->
+      <set value="1" var="autopilot.kill_throttle"/>
+      <attitude roll="0" throttle="0" vmode="throttle"/>
+    </block>
+    <block key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
+      <circle radius="nav_radius" wp="STDBY"/>
+    </block>
+    <block name="Oval 1-2" strip_button="Oval (wp 1-2)" strip_icon="oval.png" group="base_pattern">
+      <oval p1="1" p2="2" radius="nav_radius"/>
+    </block>
+    <block name="Oval 1-2 X-sec No GPS StoreHeading" strip_button="Oval (wp 1-2) X-sec StoreHeading" on_exit="//gps_disable_fix=0" strip_icon="oval.png" group="base_pattern">
+      <exception cond="(block_time @GT flightplan_disable_gps_seconds)" deroute="Oval 1-2"/>
+      <!-- set value="flightplan_disable_gps_seconds" var="gps_disable_fix"/ -->
+      <oval p1="1" p2="2" radius="nav_radius" pre_call="NavSetWaypointHereSend(WP_EST)"/>
+    </block>
+    <block name="Oval 1-2 X-sec No GPS RLS" strip_button="Oval (wp 1-2) X-sec RLS" on_exit="//gps_disable_fix=0" strip_icon="oval.png" group="base_pattern">
+      <exception cond="(block_time @GT flightplan_disable_gps_seconds)" deroute="Oval 1-2"/>
+      <!-- set value="flightplan_disable_gps_seconds" var="gps_disable_fix"/ -->
+      <oval p1="1" p2="2" radius="nav_radius" pre_call="NavSetWaypointHereSend(WP_EST)"/>
+    </block>
+    <block name="MOB" strip_button="Circle around here" strip_icon="mob.png" group="base_pattern">
+      <call_once fun="NavSetWaypointHere(WP_MOB)"/>
+      <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
+      <circle radius="nav_radius" wp="MOB"/>
+    </block>
+    <block key="Ctrl+w" name="Wind" strip_button="Wind" strip_icon="circle_cw_fwd.png" group="home">
+      <!-- call_once fun="gps_propagate_reset_rls()"/ -->
+      <!--set value="true" var="wind_estimation_enabled"/-->
+      <circle radius="nav_radius" until="NavCircleCount() @GT 1" wp="STDBY"/>
+      <deroute block="Oval 1-2"/>
+    </block>
+    <block name="Mission" strip_button="Mission" strip_icon="path.png" group="extra_pattern">
+      <path wpts="1,2 DESTINATION" approaching_time="20"/>
+    </block>
+    <block name="Observe">
+      <circle radius="nav_radius" until="NavCircleCount() @GT 0.7" wp="DESTINATION"/>
+      <!-- comment out to return instead - deroute block="roof"/-->
+    </block>
+    <block name="Return">
+      <path wpts="DESTINATION, 2 1"/>
+      <deroute block="Land Right AF-TD"/>
+    </block>
+    <block name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png" group="land">
+      <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
+      <deroute block="land"/>
+    </block>
+    <block name="Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png" group="land">
+      <set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
+      <deroute block="land"/>
+    </block>
+    <block name="land">
+      <call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
+      <circle radius="nav_radius" until="NavCircleCount() @GT 0.5" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) @AND (fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)) @LT 10)" wp="_BASELEG"/>
+    </block>
+    <block name="final">
+      <exception cond="GetAltRef() + 10 @GT GetPosAlt()" deroute="flare"/>
+      <go from="AF" hmode="route" vmode="glide" wp="TD"/>
+    </block>
+    <block name="flare">
+      <!-- call_once fun="iomcu_dsm_on()"/ -->
+      <go approaching_time="0" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
+      <attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
+    </block>
+  </blocks>
+</flight_plan>
@@ -0,0 +1,17 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+<module name="nav_shakestart" dir="nav">
+  <doc>
+    <description>Shake the RPAS for 1 second to start engines</description>
+  </doc>
+  <header>
+    <file name="nav_shakestart.h"/>
+  </header>
+  <init fun="nav_shakestart_init()"/>
+  <periodic fun="nav_shakestart_periodic()" freq="20.0" autorun="TRUE"/>
+  <makefile>
+    <file name="nav_shakestart.c"/>
+    <test firmware="fixedwing">
+      <define name="CTRL_TYPE_H" value="firmwares/fixedwing/guidance/guidance_v.h" type="string"/>
+    </test>
+  </makefile>
+</module>
@@ -698,7 +698,7 @@
   airframe="airframes/tudelft/easystar3_fw.xml"
   radio="radios/crossfire_sbus.xml"
   telemetry="telemetry/fixedwing_flight_recorder.xml"
-   flight_plan="flight_plans/basic.xml"
+   flight_plan="flight_plans/tudelft/fixedwing_no_gps.xml"
   settings="settings/fixedwing_basic.xml"
   settings_modules="modules/ahrs_float_dcm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/logger_sd_chibios.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
   gui_color="blue"
@@ -0,0 +1,112 @@
+/*
+ * Copyright (C) 2025 MAVLab <microuav@gmail.com>
+ *
+ * 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, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/nav/nav_shakestart.c"
+ * @author MAVLab <microuav@gmail.com>
+ * Shake the RPAS for 1 second to start engines
+ */
+
+#include "modules/nav/nav_shakestart.h"
+#include "modules/core/abi.h"
+
+#include "firmwares/fixedwing/nav.h"
+#include "autopilot.h"
+#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
+
+
+#ifndef NAV_SHAKE_START_ACC_ID
+#define NAV_SHAKE_START_ACC_ID ABI_BROADCAST
+#endif
+
+
+static bool nav_shakestart_detected = false;
+static float nav_shakestart_accel = 0;
+
+
+static abi_event nav_shake_start_acc_ev;
+
+static void nav_shake_start_acc_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Vect3 * accel)
+{
+  // your abi callback code here
+  // Convert to float
+  float nav_shakestart_accel_x = ACCEL_FLOAT_OF_BFP(accel->x);
+  float nav_shakestart_accel_y = ACCEL_FLOAT_OF_BFP(accel->y);
+  float nav_shakestart_accel_z = ACCEL_FLOAT_OF_BFP(accel->z);
+
+  nav_shakestart_accel = nav_shakestart_accel_x + nav_shakestart_accel_y + nav_shakestart_accel_z;
+}
+
+void nav_shakestart_init(void)
+{
+  // Abi messages bindings
+  AbiBindMsgIMU_ACCEL(NAV_SHAKE_START_ACC_ID, &nav_shake_start_acc_ev, nav_shake_start_acc_cb);
+}
+
+void nav_shakestart_reset(void)
+{
+  // Reset the shake start detection
+  nav_shakestart_detected = false;
+  nav_shakestart_accel = 0.0f;
+}
+
+void nav_shakestart_periodic(void)
+{
+  // your periodic code here.
+  // freq = 20.0 Hz
+
+  static uint32_t timer = 0;
+  static int shake_state = 0;
+
+  // If shake_state is even, we are waiting for a positive acceleration
+  if (shake_state % 2 == 0) {
+    // Check if the acceleration is above a threshold
+    if (nav_shakestart_accel > 12.0f) { // Threshold of 2g
+      timer = 15;
+      shake_state++;
+    }
+  } else {
+    // Check is the acceleration is below a threshold
+    if (nav_shakestart_accel <= 12.0f) { // Threshold of 1.2g
+      timer = 15;
+      shake_state++;
+    }
+  }
+
+  if (timer > 0) {
+    timer--;
+  } else {
+    shake_state = 0; // Reset shake_state
+  }
+
+  if (shake_state >= 9) {
+    nav_shakestart_detected = true;
+  }
+
+}
+
+bool nav_shakestart_run(void)
+{
+  NavAttitude(RadOfDeg(0.f));
+  NavVerticalAutoThrottleMode(0.5f);
+  NavVerticalThrottleMode(0.f);
+  return !nav_shakestart_detected; // return true (continue) while the shake was not detected
+}
+
+
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2025 MAVLab <microuav@gmail.com>
+ *
+ * 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, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/nav/nav_shakestart.h"
+ * @author MAVLab <microuav@gmail.com>
+ * Shake the RPAS for 1 second to start engines
+ */
+
+#ifndef NAV_SHAKESTART_H
+#define NAV_SHAKESTART_H
+
+
+#include "std.h"
+
+extern void nav_shakestart_init(void);
+extern void nav_shakestart_periodic(void);
+extern bool nav_shakestart_run(void);
+extern void nav_shakestart_reset(void);
+
+#endif  // NAV_SHAKESTART_H