Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
shift_tracking.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
30 
32 #include "autopilot.h"
33 #include "modules/core/abi.h"
35 #include "generated/airframe.h"
36 
37 #include "filters/pid.h"
38 
39 // ABI message binding ID
40 #ifndef SHIFT_TRACKING_ID
41 #define SHIFT_TRACKING_ID ABI_BROADCAST
42 #endif
43 
44 // Send debug message
45 #ifndef SHIFT_TRACKING_DEBUG
46 #define SHIFT_TRACKING_DEBUG FALSE
47 #endif
48 
49 #if SHIFT_TRACKING_DEBUG
51 #include "pprzlink/messages.h"
52 #include "mcu_periph/uart.h"
53 #endif
54 
55 #ifndef SHIFT_TRACKING_DIR
56 #define SHIFT_TRACKING_DIR { -1.0f, 0.f, 0.f }
57 #endif
58 
59 #ifndef SHIFT_TRACKING_KP
60 #define SHIFT_TRACKING_KP 1.5f
61 #endif
62 
63 #ifndef SHIFT_TRACKING_KI
64 #define SHIFT_TRACKING_KI 0.5f
65 #endif
66 
67 #ifndef SHIFT_TRACKING_KD
68 #define SHIFT_TRACKING_KD 1.f
69 #endif
70 
71 #ifndef SHIFT_TRACKING_MAXSHIFT
72 #define SHIFT_TRACKING_MAXSHIFT 30.f
73 #endif
74 
76 
77 // base time on NAV freq
78 static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
79 
80 // internal structure
82  struct FloatVect3 pos;
83  struct FloatVect3 dir;
84  struct PID_f pid;
85  float *shift;
87 };
88 
89 static struct shift_tracking_private stp;
90 
91 static const float dir[] = SHIFT_TRACKING_DIR;
92 
93 // callback on follow target message
94 static void get_pos(uint8_t sender_id __attribute__((unused)),
95  uint32_t id __attribute__((unused)),
96  float x,
97  float y,
98  float z,
99  float noise_x __attribute__((unused)),
100  float noise_y __attribute__((unused)),
101  float noise_z __attribute__((unused)))
102 {
103  stp.pos.x = x;
104  stp.pos.y = y;
105  stp.pos.z = z;
106 }
107 
109 {
113  shift_tracking.shift = 0.f;
114 
116  stp.dir.x = dir[0];
117  stp.dir.y = dir[1];
118  stp.dir.z = dir[2];
119  float_vect3_normalize(&stp.dir); // normalize direction
121  stp.shift = NULL;
122 
123  // Bind to position message
124  AbiBindMsgPOSITION_ESTIMATE(SHIFT_TRACKING_ID, &stp.pos_ev, get_pos);
125 }
126 
128 {
129  reset_pid_f(&stp.pid);
130  shift_tracking.shift = 0.f;
131  if (stp.shift) {
132  *stp.shift = 0.f;
133  }
134 }
135 
137 {
138  // store external shift variable pointer
139  stp.shift = shift;
140 
141  // compute value parameter from pos and dir
142  //
143  // FIXME
144  // for now, assume that Z is normal to the ground
145  // and shift is computed from 2D (X,Y)
146  // the vertical axis in local from should be defined as well
147  // for a correct 3D computation
148  //
149  // normal to dir = (-dir.y, dir.x) where dir is normalized
150  // offset is scalar between pos and normal
151  float value = (- stp.dir.y * stp.pos.x) + (stp.dir.x * stp.pos.y);
152 
153  // shift calculation
156 
157  // pilot actual value
158  if (stp.shift) {
160  }
161 }
162 
164 {
166 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
Core autopilot interface common to all firmwares.
#define FLOAT_VECT3_ZERO(_v)
static void float_vect3_normalize(struct FloatVect3 *v)
normalize 3D vector in place
Fixedwing Navigation library.
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
Several forms of PID controllers.
static void set_gains_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki)
Set gains of the PID struct.
Definition: pid.h:113
static void reset_pid_f(struct PID_f *pid)
Reset PID struture, gains left unchanged.
Definition: pid.h:98
static float update_pid_f(struct PID_f *pid, float value, float dt)
Update PID with a new value and return new command.
Definition: pid.h:68
static void init_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki, float max_sum)
Definition: pid.h:50
Simple PID structure floating point.
Definition: pid.h:42
Paparazzi floating point algebra.
static const float dir[]
#define SHIFT_TRACKING_DIR
void shift_tracking_update_gains(void)
#define SHIFT_TRACKING_KI
static void get_pos(uint8_t sender_id, uint32_t id, float x, float y, float z, float noise_x, float noise_y, float noise_z)
void shift_tracking_run(float *shift)
run function
void shift_tracking_init(void)
init function
struct FloatVect3 pos
last position report
static struct shift_tracking_private stp
#define SHIFT_TRACKING_KD
struct shift_tracking_t shift_tracking
#define SHIFT_TRACKING_ID
float * shift
keep track of the shift variable to change
#define SHIFT_TRACKING_KP
struct PID_f pid
PID controller.
static const float nav_dt
struct FloatVect3 dir
tracking direction
#define SHIFT_TRACKING_MAXSHIFT
void shift_tracking_reset(void)
reset function
float ki
integral gain
float kp
proportional gain
float shift
shift command
float kd
derivative gain
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98