-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
guidance_h.h
177 lines (147 loc) · 5.35 KB
/
guidance_h.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** @file firmwares/rotorcraft/guidance/guidance_h.h
* Horizontal guidance for rotorcrafts.
*
*/
#ifndef GUIDANCE_H_H
#define GUIDANCE_H_H
#include "math/pprz_algebra_int.h"
#include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
#include "generated/airframe.h"
#include "std.h"
/** Use horizontal guidance reference trajectory.
* Default is TRUE, define to FALSE to always disable it.
*/
#ifndef GUIDANCE_H_USE_REF
#define GUIDANCE_H_USE_REF TRUE
#endif
/** Use horizontal guidance speed reference.
* This also allows to give velocity commands via RC in GUIDANCE_H_MODE_HOVER.
* Default is TRUE, define to FALSE to always disable it.
*/
#ifndef GUIDANCE_H_USE_SPEED_REF
#define GUIDANCE_H_USE_SPEED_REF TRUE
#endif
#define GUIDANCE_H_MODE_KILL 0
#define GUIDANCE_H_MODE_RATE 1
#define GUIDANCE_H_MODE_ATTITUDE 2
#define GUIDANCE_H_MODE_HOVER 3
#define GUIDANCE_H_MODE_NAV 4
#define GUIDANCE_H_MODE_RC_DIRECT 5
#define GUIDANCE_H_MODE_CARE_FREE 6
#define GUIDANCE_H_MODE_FORWARD 7
#define GUIDANCE_H_MODE_MODULE 8
#define GUIDANCE_H_MODE_FLIP 9
#define GUIDANCE_H_MODE_GUIDED 10
struct HorizontalGuidanceSetpoint {
/** horizontal position setpoint in NED.
* fixed point representation: Q23.8
* accuracy 0.0039, range 8388km
*/
struct Int32Vect2 pos;
struct Int32Vect2 speed; ///< only used in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode
int32_t heading; ///< with #INT32_ANGLE_FRAC
int32_t heading_rate; ///< with #INT32_RATE_FRAC
uint8_t mask; ///< bit 5: vx & vy, bit 6: vz, bit 7: vyaw
};
struct HorizontalGuidanceReference {
struct Int32Vect2 pos; ///< with #INT32_POS_FRAC
struct Int32Vect2 speed; ///< with #INT32_SPEED_FRAC
struct Int32Vect2 accel; ///< with #INT32_ACCEL_FRAC
};
struct HorizontalGuidanceGains {
int32_t p;
int32_t d;
int32_t i;
int32_t v;
int32_t a;
};
struct HorizontalGuidance {
uint8_t mode;
/* configuration options */
bool use_ref;
bool approx_force_by_thrust;
/* gains */
struct HorizontalGuidanceGains gains;
struct HorizontalGuidanceSetpoint sp; ///< setpoints
struct HorizontalGuidanceReference ref; ///< reference calculated from setpoints
struct Int32Eulers rc_sp; ///< with #INT32_ANGLE_FRAC
};
extern struct HorizontalGuidance guidance_h;
extern int32_t transition_percentage;
extern int32_t transition_theta_offset;
extern void guidance_h_init(void);
extern void guidance_h_mode_changed(uint8_t new_mode);
extern void guidance_h_read_rc(bool in_flight);
extern void guidance_h_run(bool in_flight);
extern void guidance_h_set_igain(uint32_t igain);
/** Set horizontal position setpoint in GUIDED mode.
* @param x North position (local NED frame) in meters.
* @param y East position (local NED frame) in meters.
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool guidance_h_set_guided_pos(float x, float y);
/** Set heading setpoint in GUIDED mode.
* @param heading Setpoint in radians.
* @return TRUE if setpoint was set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool guidance_h_set_guided_heading(float heading);
/** Set body relative horizontal velocity setpoint in GUIDED mode.
* @param vx forward velocity (body frame) in meters/sec.
* @param vy right velocity (body frame) in meters/sec.
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool guidance_h_set_guided_body_vel(float vx, float vy);
/** Set horizontal velocity setpoint in GUIDED mode.
* @param vx North velocity (local NED frame) in meters/sec.
* @param vy East velocity (local NED frame) in meters/sec.
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool guidance_h_set_guided_vel(float vx, float vy);
/** Set heading rate setpoint in GUIDED mode.
* @param rate Heading rate in radians.
* @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
*/
extern bool guidance_h_set_guided_heading_rate(float rate);
/* Make sure that ref can only be temporarily disabled for testing,
* but not enabled if GUIDANCE_H_USE_REF was defined to FALSE.
*/
#define guidance_h_SetUseRef(_val) { \
guidance_h.use_ref = _val && GUIDANCE_H_USE_REF; \
}
static inline void guidance_h_SetMaxSpeed(float speed)
{
gh_set_max_speed(speed);
}
static inline void guidance_h_SetOmega(float omega)
{
gh_set_omega(omega);
}
static inline void guidance_h_SetZeta(float zeta)
{
gh_set_zeta(zeta);
}
static inline void guidance_h_SetTau(float tau)
{
gh_set_tau(tau);
}
#endif /* GUIDANCE_H_H */