mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 21:36:28 +08:00
[module] rewrite nav_bungee_takeoff
shorter, with less trigo and more vectors tested in simulation, to be done in real flight
This commit is contained in:
Gautier Hattenberger
@@ -3,18 +3,20 @@
|
||||
<module name="nav_bungee_takeoff" dir="nav">
|
||||
<doc>
|
||||
<description>
|
||||
Takeoff functions for bungee takeoff.
|
||||
Run initialize function when the plane is on the bungee, the bungee is fully extended and you are ready to
|
||||
launch the plane. After initialized, the plane will follow a line drawn by the position of the plane on initialization and the
|
||||
position of the bungee (given in the arguments). Once the plane crosses the throttle line, which is perpendicular to the line the plane is following,
|
||||
and intersects the position of the bungee (plus or minus a fixed distance (TakeOff_Distance in airframe file) from the bungee just in case the bungee doesn't release directly above the bungee) the prop will come on. The plane will then continue to follow the line until it has reached a specific
|
||||
height (defined in as Takeoff_Height in airframe file) above the bungee waypoint and speed (defined as Takeoff_Speed in the airframe file).
|
||||
Takeoff functions for bungee takeoff.
|
||||
|
||||
Run initialize function when the plane is on the bungee, the bungee is fully extended and you are ready to launch the plane.
|
||||
After initialized, the plane will follow a line drawn by the position of the plane on initialization and the position of the bungee (given in the arguments).
|
||||
Once the plane crosses the throttle line, which is perpendicular to the line the plane is following, and intersects the position of the bungee (plus or minus a fixed distance (BUNGEE_TAKEOFF_DISTANCE in airframe file) from the bungee just in case the bungee doesn't release exactly above the bungee) the prop will come on.
|
||||
The plane will then continue to follow the line until it has reached a specific height (defined in as BUNGEE_TAKEOFF_HEIGHT in airframe file) above the bungee waypoint and speed (defined as BUNGEE_TAKEOFF_SPEED in the airframe file). It is also possible to specify the pitch angle (BUNGEE_TAKEOFF_PITCH) and the throttle (BUNGEE_TAKEOFF_THROTTLE, between 0 and 1).
|
||||
</description>
|
||||
<section name="Takeoff" prefix="Takeoff_">
|
||||
<define name="Height" value="distance" unit="m" description="Takeoff height"/>
|
||||
<define name="Speed" value="speed" unit="m/s" description="Procedures ends above this speed (and above Height)"/>
|
||||
<define name="Distance" value="distance" unit="m" description="After this distance, the throttle is activated (if above MinSpeed)"/>
|
||||
<define name="MinSpeed" value="speed" unit="m/s" description="Throttle is activated if crossing the line above this speed"/>
|
||||
<section name="BUNGEE" prefix="BUNGEE_TAKEOFF_">
|
||||
<define name="HEIGHT" value="distance" unit="m" description="Takeoff height"/>
|
||||
<define name="SPEED" value="speed" unit="m/s" description="Procedures ends above this speed (and above HEIGHT)"/>
|
||||
<define name="DISTANCE" value="distance" unit="m" description="After this distance, the throttle is activated (if above MIN_SPEED)"/>
|
||||
<define name="MIN_SPEED" value="speed" unit="m/s" description="Throttle is activated if crossing the line above this speed"/>
|
||||
<define name="PITCH" value="angle" unit="deg" description="Pitch angle during the complete takeoff phase"/>
|
||||
<define name="THROTTLE" value="throttle" unit="normalized" description="Throttle setpoint (between 0 and 1) used after crossing the throttle line"/>
|
||||
</section>
|
||||
</doc>
|
||||
<header>
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2013 The Paparazzi Team
|
||||
* Copyright (C) 2008-2015 The Paparazzi Team
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -22,198 +22,183 @@
|
||||
/**
|
||||
* @file modules/nav/nav_bungee_takeoff.c
|
||||
*
|
||||
* Takeoff functions for bungee takeoff.
|
||||
*
|
||||
* from OSAM advanced navigation routines
|
||||
*
|
||||
*/
|
||||
|
||||
#include "modules/nav/nav_bungee_takeoff.h"
|
||||
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
#include "state.h"
|
||||
#include "autopilot.h"
|
||||
#include "paparazzi.h"
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
#include "firmwares/fixedwing/autopilot.h"
|
||||
#include "generated/flight_plan.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
/************** Bungee Takeoff **********************************************/
|
||||
|
||||
/** Takeoff functions for bungee takeoff.
|
||||
Run initialize function when the plane is on the bungee, the bungee is fully extended and you are ready to
|
||||
launch the plane. After initialized, the plane will follow a line drawn by the position of the plane on initialization and the
|
||||
position of the bungee (given in the arguments). Once the plane crosses the throttle line, which is perpendicular to the line the plane is following,
|
||||
and intersects the position of the bungee (plus or minus a fixed distance (TakeOff_Distance in airframe file) from the bungee just in case the bungee doesn't release directly above the bungee) the prop will come on. The plane will then continue to follow the line until it has reached a specific
|
||||
height (defined in as Takeoff_Height in airframe file) above the bungee waypoint and speed (defined as Takeoff_Speed in the airframe file).
|
||||
@verbatim
|
||||
<section name="Takeoff" prefix="Takeoff_">
|
||||
<define name="Height" value="30" unit="m"/>
|
||||
<define name="Speed" value="15" unit="m/s"/>
|
||||
<define name="Distance" value="10" unit="m"/>
|
||||
<define name="MinSpeed" value="5" unit="m/s"/>
|
||||
</section>
|
||||
@endverbatim
|
||||
*/
|
||||
/**
|
||||
* Takeoff functions for bungee takeoff.
|
||||
* Run initialize function when the plane is on the bungee, the bungee is
|
||||
* fully extended and you are ready to launch the plane.
|
||||
* After initialized, the plane will follow a line drawn by the position
|
||||
* of the plane on initialization and the position of the bungee (given in
|
||||
* the arguments).
|
||||
* Once the plane crosses the throttle line, which is perpendicular to the line
|
||||
* the plane is following, and intersects the position of the bungee (plus or
|
||||
* minus a fixed distance (BUNGEE_TAKEOFF_DISTANCE in airframe file) from
|
||||
* the bungee just in case the bungee doesn't release exactly above the bungee)
|
||||
* the prop will come on.
|
||||
* The plane will then continue to follow the line until it has reached a
|
||||
* specific height (defined in as BUNGEE_TAKEOFF_HEIGHT in airframe file) above
|
||||
* the bungee waypoint and speed (defined as BUNGEE_TAKEOFF_SPEED in the
|
||||
* airframe file).
|
||||
* It is also possible to specify the pitch angle (BUNGEE_TAKEOFF_PITCH) and
|
||||
* the throttle (BUNGEE_TAKEOFF_THROTTLE, between 0 and 1).
|
||||
*
|
||||
* @verbatim
|
||||
* <section name="BUNGEE" prefix="BUNGEE_TAKEOFF_">
|
||||
* <define name="HEIGHT" value="30" unit="m"/>
|
||||
* <define name="SPEED" value="15" unit="m/s"/>
|
||||
* <define name="DISTANCE" value="10" unit="m"/>
|
||||
* <define name="MIN_SPEED" value="5" unit="m/s"/>
|
||||
* <define name="PITCH" value="15." unit="deg"/>
|
||||
* <define name="THROTTLE" value="1.0"/>
|
||||
* </section>
|
||||
* @endverbatim
|
||||
*/
|
||||
|
||||
#ifndef Takeoff_Distance
|
||||
#define Takeoff_Distance 10
|
||||
#ifndef BUNGEE_TAKEOFF_DISTANCE
|
||||
#define BUNGEE_TAKEOFF_DISTANCE 10.0
|
||||
#endif
|
||||
#ifndef Takeoff_Height
|
||||
#define Takeoff_Height 30
|
||||
#ifndef BUNGEE_TAKEOFF_HEIGHT
|
||||
#define BUNGEE_TAKEOFF_HEIGHT 30.0
|
||||
#endif
|
||||
#ifndef Takeoff_Speed
|
||||
#define Takeoff_Speed 15
|
||||
#ifndef BUNGEE_TAKEOFF_SPEED
|
||||
#define BUNGEE_TAKEOFF_SPEED 15.0
|
||||
#endif
|
||||
#ifndef BUNGEE_TAKEOFF_MIN_SPEED
|
||||
#define BUNGEE_TAKEOFF_MIN_SPEED 5.0
|
||||
#endif
|
||||
#ifndef BUNGEE_TAKEOFF_THROTTLE
|
||||
#define BUNGEE_TAKEOFF_THROTTLE 1.0
|
||||
#endif
|
||||
#ifndef BUNGEE_TAKEOFF_PITCH
|
||||
#ifdef AGR_CLIMB_PITCH
|
||||
#define BUNGEE_TAKEOFF_PITCH AGR_CLIMB_PITCH
|
||||
#else
|
||||
#define BUNGEE_TAKEOFF_PITCH RadOfDeg(15.)
|
||||
#endif
|
||||
#ifndef Takeoff_MinSpeed
|
||||
#define Takeoff_MinSpeed 5
|
||||
#endif
|
||||
|
||||
enum TakeoffStatus { Launch, Throttle, Finished };
|
||||
#ifdef Takeoff_Distance
|
||||
#warning "Takeoff_Distance depreciated, please use BUNGEE_TAKEOFF_DISTANCE instead"
|
||||
#define BUNGEE_TAKEOFF_DISTANCE Takeoff_Distance
|
||||
#endif
|
||||
#ifdef Takeoff_Height
|
||||
#warning "Takeoff_Height depreciated, please use BUNGEE_TAKEOFF_HEIGHT instead"
|
||||
#define BUNGEE_TAKEOFF_HEIGHT Takeoff_Height
|
||||
#endif
|
||||
#ifdef Takeoff_Speed
|
||||
#warning "Takeoff_Speed depreciated, please use BUNGEE_TAKEOFF_SPEED instead"
|
||||
#define BUNGEE_TAKEOFF_SPEED Takeoff_Speed
|
||||
#endif
|
||||
#ifdef Takeoff_MinSpeed
|
||||
#warning "Takeoff_MinSpeed depreciated, please use BUNGEE_TAKEOFF_MIN_SPEED instead"
|
||||
#define BUNGEE_TAKEOFF_MIN_SPEED Takeoff_MinSpeed
|
||||
#endif
|
||||
|
||||
enum TakeoffStatus {
|
||||
Launch,
|
||||
Throttle,
|
||||
Finished
|
||||
};
|
||||
|
||||
static enum TakeoffStatus CTakeoffStatus;
|
||||
static float throttlePx;
|
||||
static float throttlePy;
|
||||
static float initialx;
|
||||
static float initialy;
|
||||
static float ThrottleSlope;
|
||||
static bool_t AboveLine;
|
||||
static float BungeeAlt;
|
||||
static float TDistance;
|
||||
static uint8_t BungeeWaypoint;
|
||||
|
||||
bool_t nav_bungee_takeoff_setup(uint8_t BungeeWP)
|
||||
static struct FloatVect2 init_point;
|
||||
static struct FloatVect2 throttle_point;
|
||||
static struct FloatVect2 takeoff_dir;
|
||||
static struct FloatVect3 bungee_point;
|
||||
|
||||
static void compute_points_from_bungee(void)
|
||||
{
|
||||
float ThrottleB;
|
||||
// Store init point (current position, where the plane will be released)
|
||||
VECT2_ASSIGN(init_point, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y);
|
||||
// Compute unitary 2D vector (bungee_point - init_point) = takeoff direction
|
||||
VECT2_DIFF(takeoff_dir, bungee_point, init_point);
|
||||
float_vect2_normalize(&takeoff_dir);
|
||||
// Find throttle point (the point where the throttle line and launch line intersect)
|
||||
// If TakeOff_Distance is positive, throttle point is after bungee point, before otherwise
|
||||
VECT2_SMUL(throttle_point, takeoff_dir, BUNGEE_TAKEOFF_DISTANCE);
|
||||
VECT2_SUM(throttle_point, bungee_point, throttle_point);
|
||||
}
|
||||
|
||||
initialx = stateGetPositionEnu_f()->x;
|
||||
initialy = stateGetPositionEnu_f()->y;
|
||||
bool_t nav_bungee_takeoff_setup(uint8_t bungee_wp)
|
||||
{
|
||||
// Store bungee point (from WP id, altitude should be ground alt)
|
||||
// FIXME use current alt instead ?
|
||||
VECT3_ASSIGN(bungee_point, WaypointX(bungee_wp), WaypointY(bungee_wp), WaypointAlt(bungee_wp));
|
||||
|
||||
BungeeWaypoint = BungeeWP;
|
||||
// Compute other points
|
||||
compute_points_from_bungee();
|
||||
|
||||
//Takeoff_Distance can only be positive
|
||||
TDistance = fabs(Takeoff_Distance);
|
||||
|
||||
//Translate initial position so that the position of the bungee is (0,0)
|
||||
float Currentx = initialx - (WaypointX(BungeeWaypoint));
|
||||
float Currenty = initialy - (WaypointY(BungeeWaypoint));
|
||||
|
||||
//Record bungee alt (which should be the ground alt at that point)
|
||||
BungeeAlt = waypoints[BungeeWaypoint].a;
|
||||
|
||||
//Find Launch line slope and Throttle line slope
|
||||
float MLaunch = Currenty / Currentx;
|
||||
|
||||
//Find Throttle Point (the point where the throttle line and launch line intersect)
|
||||
if (Currentx < 0) {
|
||||
throttlePx = TDistance / sqrt(MLaunch * MLaunch + 1);
|
||||
} else {
|
||||
throttlePx = -(TDistance / sqrt(MLaunch * MLaunch + 1));
|
||||
}
|
||||
|
||||
if (Currenty < 0) {
|
||||
throttlePy = sqrt((TDistance * TDistance) - (throttlePx * throttlePx));
|
||||
} else {
|
||||
throttlePy = -sqrt((TDistance * TDistance) - (throttlePx * throttlePx));
|
||||
}
|
||||
|
||||
//Find ThrottleLine
|
||||
ThrottleSlope = tan(atan2(Currenty, Currentx) + (3.14 / 2));
|
||||
ThrottleB = (throttlePy - (ThrottleSlope * throttlePx));
|
||||
|
||||
//Determine whether the UAV is below or above the throttle line
|
||||
if (Currenty > ((ThrottleSlope * Currentx) + ThrottleB)) {
|
||||
AboveLine = TRUE;
|
||||
} else {
|
||||
AboveLine = FALSE;
|
||||
}
|
||||
|
||||
//Enable Launch Status and turn kill throttle on
|
||||
// Enable Launch Status and turn kill throttle on
|
||||
CTakeoffStatus = Launch;
|
||||
kill_throttle = 1;
|
||||
|
||||
//Translate the throttle point back
|
||||
throttlePx = throttlePx + (WaypointX(BungeeWP));
|
||||
throttlePy = throttlePy + (WaypointY(BungeeWP));
|
||||
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
bool_t nav_bungee_takeoff_run(void)
|
||||
{
|
||||
//Translate current position so Throttle point is (0,0)
|
||||
float Currentx = stateGetPositionEnu_f()->x - throttlePx;
|
||||
float Currenty = stateGetPositionEnu_f()->y - throttlePy;
|
||||
bool_t CurrentAboveLine;
|
||||
float ThrottleB;
|
||||
float cross = 0.;
|
||||
|
||||
// Get current position
|
||||
struct FloatVect2 pos;
|
||||
VECT2_ASSIGN(pos, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y);
|
||||
|
||||
switch (CTakeoffStatus) {
|
||||
case Launch:
|
||||
//Follow Launch Line
|
||||
NavVerticalAutoThrottleMode(0);
|
||||
NavVerticalAltitudeMode(BungeeAlt + Takeoff_Height, 0.);
|
||||
nav_route_xy(initialx, initialy, throttlePx, throttlePy);
|
||||
// Recalculate lines if below min speed
|
||||
if ((*stateGetHorizontalSpeedNorm_f()) < BUNGEE_TAKEOFF_MIN_SPEED) {
|
||||
compute_points_from_bungee();
|
||||
}
|
||||
|
||||
// Follow Launch Line with takeoff pitch and no throttle
|
||||
NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH);
|
||||
NavVerticalThrottleMode(0);
|
||||
// FIXME previously using altitude mode, maybe not wise without motors
|
||||
//NavVerticalAltitudeMode(bungee_point.z + BUNGEE_TAKEOFF_HEIGHT, 0.);
|
||||
nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y);
|
||||
|
||||
kill_throttle = 1;
|
||||
|
||||
//recalculate lines if below min speed
|
||||
if ((*stateGetHorizontalSpeedNorm_f()) < Takeoff_MinSpeed) {
|
||||
initialx = stateGetPositionEnu_f()->x;
|
||||
initialy = stateGetPositionEnu_f()->y;
|
||||
// Find out if UAV has crossed the line
|
||||
VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point
|
||||
cross = VECT2_DOT_PRODUCT(pos, takeoff_dir);
|
||||
|
||||
//Translate initial position so that the position of the bungee is (0,0)
|
||||
Currentx = initialx - (WaypointX(BungeeWaypoint));
|
||||
Currenty = initialy - (WaypointY(BungeeWaypoint));
|
||||
|
||||
//Find Launch line slope
|
||||
float MLaunch = Currenty / Currentx;
|
||||
|
||||
//Find Throttle Point (the point where the throttle line and launch line intersect)
|
||||
if (Currentx < 0) {
|
||||
throttlePx = TDistance / sqrt(MLaunch * MLaunch + 1);
|
||||
} else {
|
||||
throttlePx = -(TDistance / sqrt(MLaunch * MLaunch + 1));
|
||||
}
|
||||
|
||||
if (Currenty < 0) {
|
||||
throttlePy = sqrt((TDistance * TDistance) - (throttlePx * throttlePx));
|
||||
} else {
|
||||
throttlePy = -sqrt((TDistance * TDistance) - (throttlePx * throttlePx));
|
||||
}
|
||||
|
||||
//Find ThrottleLine
|
||||
ThrottleSlope = tan(atan2(Currenty, Currentx) + (3.14 / 2));
|
||||
ThrottleB = (throttlePy - (ThrottleSlope * throttlePx));
|
||||
|
||||
//Determine whether the UAV is below or above the throttle line
|
||||
if (Currenty > ((ThrottleSlope * Currentx) + ThrottleB)) {
|
||||
AboveLine = TRUE;
|
||||
} else {
|
||||
AboveLine = FALSE;
|
||||
}
|
||||
|
||||
//Translate the throttle point back
|
||||
throttlePx = throttlePx + (WaypointX(BungeeWaypoint));
|
||||
throttlePy = throttlePy + (WaypointY(BungeeWaypoint));
|
||||
}
|
||||
|
||||
//Find out if the UAV is currently above the line
|
||||
if (Currenty > (ThrottleSlope * Currentx)) {
|
||||
CurrentAboveLine = TRUE;
|
||||
} else {
|
||||
CurrentAboveLine = FALSE;
|
||||
}
|
||||
|
||||
//Find out if UAV has crossed the line
|
||||
if (AboveLine != CurrentAboveLine && (*stateGetHorizontalSpeedNorm_f()) > Takeoff_MinSpeed) {
|
||||
if (cross > 0. && (*stateGetHorizontalSpeedNorm_f()) > BUNGEE_TAKEOFF_MIN_SPEED) {
|
||||
CTakeoffStatus = Throttle;
|
||||
kill_throttle = 0;
|
||||
nav_init_stage();
|
||||
} else {
|
||||
// If not crossed stay in this status
|
||||
break;
|
||||
}
|
||||
break;
|
||||
// Start throttle imidiatelly
|
||||
case Throttle:
|
||||
//Follow Launch Line
|
||||
NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH);
|
||||
NavVerticalThrottleMode(9600 * (1));
|
||||
nav_route_xy(initialx, initialy, throttlePx, throttlePy);
|
||||
NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH);
|
||||
NavVerticalThrottleMode(MAX_PPRZ * (BUNGEE_TAKEOFF_THROTTLE));
|
||||
nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y);
|
||||
kill_throttle = 0;
|
||||
|
||||
if ((stateGetPositionUtm_f()->alt > BungeeAlt + Takeoff_Height - 10)
|
||||
&& ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed)) {
|
||||
if ((stateGetPositionUtm_f()->alt > bungee_point.z + BUNGEE_TAKEOFF_HEIGHT)
|
||||
&& ((*stateGetHorizontalSpeedNorm_f()) > BUNGEE_TAKEOFF_SPEED)) {
|
||||
CTakeoffStatus = Finished;
|
||||
return FALSE;
|
||||
} else {
|
||||
@@ -221,7 +206,9 @@ bool_t nav_bungee_takeoff_run(void)
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
// Invalid status or Finished, end function
|
||||
return FALSE;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2013 The Paparazzi Team
|
||||
* Copyright (C) 2008-2015 The Paparazzi Team
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -22,6 +22,8 @@
|
||||
/**
|
||||
* @file modules/nav/nav_bungee_takeoff.h
|
||||
*
|
||||
* Takeoff functions for bungee takeoff.
|
||||
*
|
||||
* from OSAM advanced navigation routines
|
||||
*
|
||||
*/
|
||||
@@ -31,7 +33,7 @@
|
||||
|
||||
#include "std.h"
|
||||
|
||||
extern bool_t nav_bungee_takeoff_setup(uint8_t BungeeWP);
|
||||
extern bool_t nav_bungee_takeoff_setup(uint8_t bungee_wp);
|
||||
extern bool_t nav_bungee_takeoff_run(void);
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user