mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-22 12:28:03 +08:00
[nav] takeoff detection module
set the launch variable to true based on pitch angle
This commit is contained in:
Gautier Hattenberger
committed by
Felix Ruess
Felix Ruess
parent
953ef8ea10
commit
bc4df1d4f0
@@ -0,0 +1,30 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="takeoff_detect" dir="nav">
|
||||
<doc>
|
||||
<description>
|
||||
Fixed-wing automatic launch detection
|
||||
|
||||
When loaded, the pitch angle of plane is monitored, and will trigger the launch variable
|
||||
if the angle goes over a specified value (nose up) for 2 seconds.
|
||||
The launch variable can be reset to FALSE by pointing the nose to the ground.
|
||||
The module disables itself within a few seconds (4s) after detecting the launch.
|
||||
It can be re-actiaved by hand from the GCS.
|
||||
</description>
|
||||
<section name="TO_DETECT">
|
||||
<define name="TAKEOFF_DETECT_LAUNCH_PITCH" value="30." unit="deg" description="Picth angle for takeoff detection (set 'launch' to TRUE)"/>
|
||||
<define name="TAKEOFF_DETECT_ABORT_PITCH" value="-20." unit="deg" description="Pitch angle to abort takeoff (set 'launch' to FALSE)"/>
|
||||
<define name="TAKEOFF_DETECT_TIMER" value="2." description="Timer for takeoff detection in seconds (default 2s above pitch angle threshold)"/>
|
||||
<define name="TAKEOFF_DETECT_DISABLE_TIMER" value="4." description="Timer for module de-activation (default 4s after the launch detection)"/>
|
||||
</section>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="takeoff_detect.h"/>
|
||||
</header>
|
||||
<init fun="takeoff_detect_init()"/>
|
||||
<periodic fun="takeoff_detect_periodic()" freq="20" autorun="TRUE" start="takeoff_detect_start()"/>
|
||||
<makefile>
|
||||
<file name="takeoff_detect.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -0,0 +1,131 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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/takeoff_detect.c
|
||||
*
|
||||
* Automatic takeoff assistance for fixed-wing.
|
||||
* The planes's launching can be triggered/aborted
|
||||
* by pointing the nose up or down for a given time.
|
||||
*/
|
||||
|
||||
#include "std.h"
|
||||
#include "modules/nav/takeoff_detect.h"
|
||||
#include "firmwares/fixedwing/autopilot.h"
|
||||
#include "state.h"
|
||||
#include "generated/modules.h" // for status and function's period
|
||||
|
||||
/** Default pitch angle to trigger launch */
|
||||
#ifndef TAKEOFF_DETECT_LAUNCH_PITCH
|
||||
#define TAKEOFF_DETECT_LAUNCH_PITCH RadOfDeg(30.)
|
||||
#endif
|
||||
|
||||
/** Default pitch angle to cancel launch */
|
||||
#ifndef TAKEOFF_DETECT_ABORT_PITCH
|
||||
#define TAKEOFF_DETECT_ABORT_PITCH RadOfDeg(-20.)
|
||||
#endif
|
||||
|
||||
/** Detection timer in seconds */
|
||||
#ifndef TAKEOFF_DETECT_TIMER
|
||||
#define TAKEOFF_DETECT_TIMER 2.
|
||||
#endif
|
||||
|
||||
/** Disable timer in seconds */
|
||||
#ifndef TAKEOFF_DETECT_DISABLE_TIMER
|
||||
#define TAKEOFF_DETECT_DISABLE_TIMER 4.
|
||||
#endif
|
||||
|
||||
/** Takeoff detection states */
|
||||
enum takeoff_detect_state {
|
||||
TO_DETECT_DISABLED,
|
||||
TO_DETECT_ARMED,
|
||||
TO_DETECT_LAUNCHING
|
||||
};
|
||||
|
||||
/** Takeoff detection structure */
|
||||
struct takeoff_detect_struct {
|
||||
enum takeoff_detect_state state;
|
||||
uint32_t timer;
|
||||
};
|
||||
|
||||
static struct takeoff_detect_struct takeoff_detect;
|
||||
|
||||
// Init
|
||||
void takeoff_detect_init(void)
|
||||
{
|
||||
// variable init is done in start function
|
||||
}
|
||||
|
||||
// Start
|
||||
void takeoff_detect_start(void)
|
||||
{
|
||||
takeoff_detect.state = TO_DETECT_ARMED; // always start periodic with ARMED state
|
||||
takeoff_detect.timer = 0; // and reset timer
|
||||
}
|
||||
|
||||
// Periodic
|
||||
void takeoff_detect_periodic(void)
|
||||
{
|
||||
// Run detection state machine here
|
||||
switch (takeoff_detect.state) {
|
||||
case TO_DETECT_ARMED:
|
||||
// test for "nose up" + AP in AUTO2 (+ GPS OK ? FIXME)
|
||||
if (stateGetNedToBodyEulers_f()->theta > TAKEOFF_DETECT_LAUNCH_PITCH
|
||||
&& pprz_mode == PPRZ_MODE_AUTO2) {
|
||||
takeoff_detect.timer++;
|
||||
}
|
||||
else {
|
||||
// else reset timer
|
||||
takeoff_detect.timer = 0;
|
||||
}
|
||||
// if timer is finished, start launching
|
||||
if (takeoff_detect.timer > (int)(TAKEOFF_DETECT_PERIODIC_FREQ * TAKEOFF_DETECT_TIMER)) {
|
||||
launch = TRUE;
|
||||
takeoff_detect.state = TO_DETECT_LAUNCHING;
|
||||
takeoff_detect.timer = 0;
|
||||
}
|
||||
break;
|
||||
case TO_DETECT_LAUNCHING:
|
||||
// abort if pitch goes below threshold while launching
|
||||
if (stateGetNedToBodyEulers_f()->theta < TAKEOFF_DETECT_ABORT_PITCH
|
||||
|| pprz_mode != PPRZ_MODE_AUTO2) {
|
||||
// back to ARMED state
|
||||
launch = FALSE;
|
||||
takeoff_detect.state = TO_DETECT_ARMED;
|
||||
}
|
||||
// increment timer and disable detection after some time
|
||||
takeoff_detect.timer++;
|
||||
if (takeoff_detect.timer > (int)(TAKEOFF_DETECT_PERIODIC_FREQ * TAKEOFF_DETECT_DISABLE_TIMER)) {
|
||||
takeoff_detect.state = TO_DETECT_DISABLED;
|
||||
}
|
||||
break;
|
||||
case TO_DETECT_DISABLED:
|
||||
// stop periodic call
|
||||
takeoff_detect_takeoff_detect_periodic_status = MODULES_STOP;
|
||||
break;
|
||||
default:
|
||||
// No kidding ?!
|
||||
takeoff_detect.state = TO_DETECT_DISABLED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,48 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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/takeoff_detect.h
|
||||
*
|
||||
* Automatic takeoff assistance for fixed-wing.
|
||||
* The planes's launching can be triggered/aborted
|
||||
* by pointing the nose up or down for a given time.
|
||||
*/
|
||||
|
||||
#ifndef TAKEOFF_DETECT_H
|
||||
#define TAKEOFF_DETECT_H
|
||||
|
||||
/** Init function */
|
||||
extern void takeoff_detect_init(void);
|
||||
|
||||
/** Start function called once before periodic
|
||||
*/
|
||||
extern void takeoff_detect_start(void);
|
||||
|
||||
/** Periodic call
|
||||
*
|
||||
* - can be enabled or disabled from settings
|
||||
* - enabled by default, disable himself after launch
|
||||
*/
|
||||
extern void takeoff_detect_periodic(void);
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user