Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 16:30:07 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[module] add shift_tracking module

Browse Source 
This module is trying to track an axis based on a local position
estimate that can come from an UWB position system or a smart cam like
jevois for instance.
The control idea is to apply a shift around a reference trajectory that
should be close enough from the final desired path.
This was developped to help final approach for landing in a net. See
https://hal-enac.archives-ouvertes.fr/hal-01936955 for more information.
Also include a simple PID library (discrete or "continuous" form).
This commit is contained in:
Gautier Hattenberger
2019-07-01 12:49:58 +02:00
parent 407975cd8e
commit 57b4448205
5 changed files with 690 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/ENAC/dw1000_basic.xml
+97
View File
@@ -0,0 +1,97 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="260" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
<header>
#include "subsystems/datalink/datalink.h"
</header>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint name="STDBY" x="49.5" y="100.1"/>
<waypoint name="1" x="10.1" y="189.9"/>
<waypoint name="2" x="132.3" y="139.1"/>
<waypoint name="MOB" x="137.0" y="-11.6"/>
<waypoint alt="215.0" name="AF" x="177.4" y="45.1"/>
<waypoint alt="185.0" name="TD" x="-22.6" y="45.1"/>
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
<waypoint name="CLIMB" x="-114.5" y="162.3"/>
<waypoint name="ANCHOR_1" x="-15" y="37.5"/>
<waypoint name="ANCHOR_2" x="-20" y="40."/>
<waypoint name="ANCHOR_3" x="-20" y="35."/>
</waypoints>
<modules>
<module name="shift_tracking"/>
</modules>
<exceptions/>
<blocks>
<block name="Wait GPS">
<call_once fun="dw1000_use_ekf = false"/>
<set value="1" var="autopilot.kill_throttle"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Holding point">
<set value="1" var="autopilot.kill_throttle"/>
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
<set value="0" var="autopilot.kill_throttle"/>
<set value="0" var="autopilot.flight_time"/>
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
</block>
<block group="home" 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="Start Shift Tracking">
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="Run Shift Tracking" pre_call="shift_tracking_run(&nav_shift)">
<call_once fun="shift_tracking_reset()"/>
<call_once fun="dw1000_use_ekf = true"/>
<go approaching_time="0" from="AF" hmode="route" wp="TD" alt="WaypointAlt(WP_AF)"/>
<call_once fun="dw1000_use_ekf = false"/>
<deroute block="Start Shift Tracking"/>
</block>
<block group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
<deroute block="land"/>
</block>
<block group="land" name="Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png">
<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() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
<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>
<block name="Set Anchor 1">
<call_once fun="NavSetWaypointHere(WP_ANCHOR_1)"/>
<deroute block="Holding point"/>
</block>
<block name="Set Anchor 2">
<call_once fun="NavSetWaypointHere(WP_ANCHOR_2)"/>
<deroute block="Holding point"/>
</block>
<block name="Set Anchor 3">
<call_once fun="NavSetWaypointHere(WP_ANCHOR_3)"/>
<deroute block="Holding point"/>
</block>
</blocks>
</flight_plan>
conf/modules/shift_tracking.xml
+41
View File
@@ -0,0 +1,41 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="shift_tracking" dir="ctrl">
<doc>
<description>
Pilot nav shift variable to track an offset in navigation
Typical use is to Control a fixed-wing to use both GPS and
local position (from Decawave modules for instance) to land it
with precision or in a net.
Data are coming from POSITION_ESTIMATE ABI message.
</description>
<section name="SHIFT_TRACKING" prefix="SHIFT_TRACKING_">
<define name="ID" value="ABI_BROADCAST" description="ABI binding ID"/>
<define name="DIR" value="1.0, 0., 0." type="float[]" description="direction to track expressed in the local pos frame"/>
<define name="KP" value="1.5" description="proportional gain"/>
<define name="KI" value="0.5" description="integral gain"/>
<define name="KD" value="1.0" description="derivative gain"/>
<define name="MAXSHIFT" value="30." description="maximum offset control"/>
<define name="MAXSUM" value="30." description="maximum integral part"/>
</section>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="shift tracking">
<dl_setting MAX="5." MIN="0." STEP="0.05" VAR="shift_tracking.kp" module="modules/ctrl/shift_tracking" handler="SetKp"/>
<dl_setting MAX="5." MIN="0." STEP="0.05" VAR="shift_tracking.ki" module="modules/ctrl/shift_tracking" handler="SetKi"/>
<dl_setting MAX="5." MIN="0." STEP="0.05" VAR="shift_tracking.kd" module="modules/ctrl/shift_tracking" handler="SetKd"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="shift_tracking.h"/>
</header>
<init fun="shift_tracking_init()"/>
<makefile>
<file name="shift_tracking.c"/>
</makefile>
</module>
sw/airborne/filters/pid.h
+317
View File
@@ -0,0 +1,317 @@
/*
* Copyright (C) 2018 Gautier Hattenberger
*
* 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 filters/pid.h
* @brief Several forms of PID controllers
*
*/
#ifndef PID_H
#define PID_H
/** Simple PID structure
* floating point
*
* u_k = Kp * e_k + Kd * (e_k - e_k-1) / dt + Ki * (sum (e_k * dt))
*
* with:
* u = outputs
* e = inputs
* Kp = proportional gain
* Kd = derivative gain
* Ki = integral gain
* dt = time since last input
*/
struct PID_f {
float u; ///< output
float e[2]; ///< input
float sum; ///< integral of input
float g[3]; ///< controller gains (Kp, Kd, Ki)
float max_sum; ///< windup protection, max of Ki * sum(e_k * dt)
};
static inline void init_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki, float max_sum)
{
*pid = (struct PID_f) {
0.f,
{ 0.f , 0.f },
0.f,
{ Kp, Kd, Ki },
max_sum
};
}
/** Update PID with a new value and return new command.
*
* @param pid pointer to PID structure
* @param value new input value of the PID
* @param dt time since last input (in seconds)
* @return new output command
*/
static inline float update_pid_f(struct PID_f *pid, float value, float dt)
{
pid->e[1] = pid->e[0];
pid->e[0] = value;
float integral = pid->g[2] * (pid->sum + value);
if (integral > pid->max_sum) {
integral = pid->max_sum;
} else if (integral < -pid->max_sum) {
integral = -pid->max_sum;
} else {
pid->sum += value;
}
pid->u = pid->g[0] * pid->e[0] + pid->g[1] * (pid->e[0] - pid->e[1]) / dt + integral;
return pid->u;
}
/** Get current value of the PID command.
*
* @param pid pointer to PID structure
* @return current value of PID command
*/
static inline float get_pid_f(struct PID_f *pid)
{
return pid->u;
}
/** Reset PID struture, gains left unchanged.
*
* @param pid pointer to PID structure
*/
static inline void reset_pid_f(struct PID_f *pid)
{
pid->u = 0.f;
pid->e[0] = 0.f;
pid->e[1] = 0.f;
pid->sum = 0.f;
}
/** Set gains of the PID struct.
*
* @param pid pointer to PID structure
* @param Kp proportional gain
* @param Kd derivative gain
* @param Ki integral gain
*/
static inline void set_gains_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki)
{
pid->g[0] = Kp;
pid->g[1] = Kd;
pid->g[2] = Ki;
}
/** Distcrete time PID structure.
* floating point, fixed frequency.
*
* u_k = u_k-1 + a * e_k + b * e_k-1 + c * e_k-2
*
* with:
* u = outputs
* e = inputs
* a = Kp + Ki Ts/2 + Kd/Ts
* b = -Kp + Ki Ts/2 - 2 Kd/Ts
* c = Kd/Ts
* Kp = proportional gain
* Kd = derivative gain
* Ki = integral gain
* Ts = sampling frequency
*
*/
struct PID_df {
float u[2]; ///< output
float e[3]; ///< input
float g[3]; ///< controller gains
};
/** Init PID struct.
*
* @param pid pointer to PID structure
* @param Kp proportional gain
* @param Kd derivative gain
* @param Ki integral gain
* @param Ts sampling time
*/
static inline void init_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
{
*pid = (struct PID_df) {
{ 0.f, 0.f },
{ 0.f ,0.f , 0.f },
{ Kp + Ki * Ts / 2.f + Kd / Ts,
-Kp + Ki * Ts / 2.f - 2.f * Kd / Ts,
Kd / Ts }
};
}
/** Update PID with a new value and return new command.
*
* @param pid pointer to PID structure
* @param value new input value of the PID
* @return new output command
*/
static inline float update_pid_df(struct PID_df *pid, float value)
{
pid->e[2] = pid->e[1];
pid->e[1] = pid->e[0];
pid->e[0] = value;
pid->u[1] = pid->u[0];
pid->u[0] = pid->u[1] + pid->g[0] * pid->e[0] + pid->g[1] * pid->e[1] + pid->g[2] * pid->e[2];
return pid->u[0];
}
/** Get current value of the PID command.
*
* @param pid pointer to PID structure
* @return current value of PID command
*/
static inline float get_pid_df(struct PID_df *pid)
{
return pid->u[0];
}
/** Reset PID struture, gains left unchanged.
*
* @param pid pointer to PID structure
*/
static inline void reset_pid_df(struct PID_df *pid)
{
pid->u[0] = 0.f;
pid->u[1] = 0.f;
pid->e[0] = 0.f;
pid->e[1] = 0.f;
pid->e[2] = 0.f;
}
/** Set gains of the PID struct.
*
* @param pid pointer to PID structure
* @param Kp proportional gain
* @param Kd derivative gain
* @param Ki integral gain
* @param Ts sampling time
*/
static inline void set_gains_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
{
pid->g[0] = Kp + Ki * Ts / 2.f + Kd / Ts;
pid->g[1] = -Kp + Ki * Ts / 2.f - 2.f * Kd / Ts;
pid->g[2] = Kd / Ts;
}
/** Distcrete time PI-D structure.
* derivative term is directly provided as input
* as it may be available directly from a sensor
* or estimated separately.
* floating point, fixed frequency.
*
* u_k = u_k-1 + a * e_k + b * e_k-1 + Kd * d_k
*
* with:
* u = outputs
* e = inputs
* d = derivative of input
* a = Kp + Ki Ts/2
* b = -Kp + Ki Ts/2
* Kp = proportional gain
* Kd = derivative gain
* Ki = integral gain
* Ts = sampling frequency
*
*/
struct PI_D_df {
float u[2]; ///< output
float e[2]; ///< input
float g[3]; ///< controller gains
};
/** Init PI-D struct.
*
* @param pid pointer to PID structure
* @param Kp proportional gain
* @param Kd derivative gain
* @param Ki integral gain
* @param Ts sampling time
*/
static inline void init_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
{
*pid = (struct PI_D_df) {
{ 0.f, 0.f },
{ 0.f ,0.f },
{ Kp + Ki * Ts / 2.f,
-Kp + Ki * Ts / 2.f,
Kd }
};
}
/** Update PI-D with a new value and return new command.
*
* @param pid pointer to PI-D structure
* @param value new input value of the PI-D
* @param deriv new input derivative
* @return new output command
*/
static inline float update_pi_d_df(struct PI_D_df *pid, float value, float deriv)
{
pid->e[1] = pid->e[0];
pid->e[0] = value;
pid->u[1] = pid->u[0];
pid->u[0] = pid->u[1] + pid->g[0] * pid->e[0] + pid->g[1] * pid->e[1] + pid->g[2] * deriv;
return pid->u[0];
}
/** Get current value of the PI-D command.
*
* @param pid pointer to PI-D structure
* @return current value of PI-D command
*/
static inline float get_pi_d_df(struct PI_D_df *pid)
{
return pid->u[0];
}
/** Reset PI-D struture, gains left unchanged.
*
* @param pid pointer to PI-D structure
*/
static inline void reset_pi_d_df(struct PI_D_df *pid)
{
pid->u[0] = 0.f;
pid->u[1] = 0.f;
pid->e[0] = 0.f;
pid->e[1] = 0.f;
}
/** Set gains PI-D struct.
*
* @param pid pointer to PID structure
* @param Kp proportional gain
* @param Kd derivative gain
* @param Ki integral gain
* @param Ts sampling time
*/
static inline void set_gains_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
{
pid->g[0] = Kp + Ki * Ts / 2.f;
pid->g[1] = -Kp + Ki * Ts / 2.f;
pid->g[2] = Kd;
}
#endif
sw/airborne/modules/ctrl/shift_tracking.c
+166
View File
@@ -0,0 +1,166 @@
/*
* Copyright (C) 2018 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/ctrl/shift_tracking.h"
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* pilot the nav shift variable to track an offset trajectory
* based on the POSITION_ESTIMATE ABI message
*/
#include "modules/ctrl/shift_tracking.h"
#include "firmwares/fixedwing/nav.h"
#include "autopilot.h"
#include "subsystems/abi.h"
#include "math/pprz_algebra_float.h"
#include "generated/airframe.h"
#include "filters/pid.h"
// ABI message binding ID
#ifndef SHIFT_TRACKING_ID
#define SHIFT_TRACKING_ID ABI_BROADCAST
#endif
// Send debug message
#ifndef SHIFT_TRACKING_DEBUG
#define SHIFT_TRACKING_DEBUG FALSE
#endif
#if SHIFT_TRACKING_DEBUG
#include "subsystems/datalink/downlink.h"
#include "pprzlink/messages.h"
#include "mcu_periph/uart.h"
#endif
#ifndef SHIFT_TRACKING_DIR
#define SHIFT_TRACKING_DIR { -1.0f, 0.f, 0.f }
#endif
#ifndef SHIFT_TRACKING_KP
#define SHIFT_TRACKING_KP 1.5f
#endif
#ifndef SHIFT_TRACKING_KI
#define SHIFT_TRACKING_KI 0.5f
#endif
#ifndef SHIFT_TRACKING_KD
#define SHIFT_TRACKING_KD 1.f
#endif
#ifndef SHIFT_TRACKING_MAXSHIFT
#define SHIFT_TRACKING_MAXSHIFT 30.f
#endif
struct shift_tracking_t shift_tracking;
// base time on NAV freq
static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
// internal structure
struct shift_tracking_private {
struct FloatVect3 pos; ///< last position report
struct FloatVect3 dir; ///< tracking direction
struct PID_f pid; ///< PID controller
float *shift; ///< keep track of the shift variable to change
abi_event pos_ev;
};
static struct shift_tracking_private stp;
static const float dir[] = SHIFT_TRACKING_DIR;
// callback on follow target message
static void get_pos(uint8_t sender_id __attribute__((unused)),
uint32_t id __attribute__((unused)),
float x,
float y,
float z,
float noise_x __attribute__((unused)),
float noise_y __attribute__((unused)),
float noise_z __attribute__((unused)))
{
stp.pos.x = x;
stp.pos.y = y;
stp.pos.z = z;
}
void shift_tracking_init(void)
{
shift_tracking.kp = SHIFT_TRACKING_KP;
shift_tracking.ki = SHIFT_TRACKING_KI;
shift_tracking.kd = SHIFT_TRACKING_KD;
shift_tracking.shift = 0.f;
FLOAT_VECT3_ZERO(stp.pos);
stp.dir.x = dir[0];
stp.dir.y = dir[1];
stp.dir.z = dir[2];
float_vect3_normalize(&stp.dir); // normalize direction
init_pid_f(&stp.pid, shift_tracking.kp, shift_tracking.kd, shift_tracking.ki, 30.f);
stp.shift = NULL;
// Bind to position message
AbiBindMsgPOSITION_ESTIMATE(SHIFT_TRACKING_ID, &stp.pos_ev, get_pos);
}
void shift_tracking_reset(void)
{
reset_pid_f(&stp.pid);
shift_tracking.shift = 0.f;
if (stp.shift) {
*stp.shift = 0.f;
}
}
void shift_tracking_run(float *shift)
{
// store external shift variable pointer
stp.shift = shift;
// compute value parameter from pos and dir
//
// FIXME
// for now, assume that Z is normal to the ground
// and shift is computed from 2D (X,Y)
// the vertical axis in local from should be defined as well
// for a correct 3D computation
//
// normal to dir = (-dir.y, dir.x) where dir is normalized
// offset is scalar between pos and normal
float value = (- stp.dir.y * stp.pos.x) + (stp.dir.x * stp.pos.y);
// shift calculation
shift_tracking.shift = update_pid_f(&stp.pid, value, nav_dt);
BoundAbs(shift_tracking.shift, SHIFT_TRACKING_MAXSHIFT);
// pilot actual value
if (stp.shift) {
*stp.shift = shift_tracking.shift;
}
}
void shift_tracking_update_gains(void)
{
set_gains_pid_f(&stp.pid, shift_tracking.kp, shift_tracking.kd, shift_tracking.ki);
}
sw/airborne/modules/ctrl/shift_tracking.h
+69
View File
@@ -0,0 +1,69 @@
/*
* Copyright (C) 2018 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/ctrl/shift_tracking.h"
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* pilot the nav shift variable to track an offset trajectory
* based on the POSITION_ESTIMATE ABI message
*/
#ifndef SHIFT_TRACKING_H
#define SHIFT_TRACKING_H
#include "std.h"
struct shift_tracking_t {
float kp; ///< proportional gain
float kd; ///< derivative gain
float ki; ///< integral gain
float shift; ///< shift command
};
extern struct shift_tracking_t shift_tracking;
/** init function
*/
extern void shift_tracking_init(void);
/** run function
*
* should be called in flight plan pre_call
*
* @param[out] shift pointer to the navigation shift to control
*/
extern void shift_tracking_run(float *shift);
/** reset function
*
* reset integral and offset command
*/
extern void shift_tracking_reset(void);
/** hndlers for gains update
*/
#define shift_tracking_SetKp(_v) { shift_tracking.kp = _v; shift_tracking_update_gains(); }
#define shift_tracking_SetKd(_v) { shift_tracking.kd = _v; shift_tracking_update_gains(); }
#define shift_tracking_SetKi(_v) { shift_tracking.ki = _v; shift_tracking_update_gains(); }
extern void shift_tracking_update_gains(void);
#endif