Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/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 
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 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
#define SHIFT_TRACKING_KI
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
struct FloatVect3 dir
tracking direction
struct PID_f pid
PID controller.
static const float nav_dt
Main include for ABI (AirBorneInterface).
#define SHIFT_TRACKING_ID
static void init_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki, float max_sum)
Definition: pid.h:50
struct FloatVect3 pos
last position report
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
void shift_tracking_update_gains(void)
static struct shift_tracking_private stp
static void float_vect3_normalize(struct FloatVect3 *v)
normalize 3D vector in place
#define FLOAT_VECT3_ZERO(_v)
struct shift_tracking_t shift_tracking
#define SHIFT_TRACKING_DIR
void shift_tracking_init(void)
init function
void shift_tracking_reset(void)
reset function
Paparazzi floating point algebra.
#define SHIFT_TRACKING_MAXSHIFT
unsigned long uint32_t
Definition: types.h:18
#define NAVIGATION_FREQUENCY
static const float dir[]
void shift_tracking_run(float *shift)
run function
#define SHIFT_TRACKING_KD
Core autopilot interface common to all firmwares.
float kd
derivative gain
static void reset_pid_f(struct PID_f *pid)
Reset PID struture, gains left unchanged.
Definition: pid.h:98
unsigned char uint8_t
Definition: types.h:14
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)
float kp
proportional gain
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
#define SHIFT_TRACKING_KP
Simple PID structure floating point.
Definition: pid.h:42
float ki
integral gain
float shift
shift command
float * shift
keep track of the shift variable to change
Several forms of PID controllers.