Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
rover_guidance.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
27
28#define AUTOPILOT_CORE_GUIDANCE_C
29
31#include "generated/airframe.h"
32#include "generated/autopilot_core_guidance.h"
33#include "state.h"
34
36
37#if PERIODIC_TELEMETRY
39// TODO rover guidance messages
40#endif
41
43{
45 rover_guidance.sp.heading = 0.0f;
46 rover_guidance.speed_pid.p = ROVER_GUIDANCE_SPEED_PGAIN;
47 rover_guidance.speed_pid.i = ROVER_GUIDANCE_SPEED_IGAIN;
48 rover_guidance.speed_pid.d = ROVER_GUIDANCE_SPEED_DGAIN;
49 rover_guidance.speed_pid.err = 0.f;
50 rover_guidance.speed_pid.d_err = 0.f;
51 rover_guidance.speed_pid.sum_err = 0.f;
52 rover_guidance.turn_pid.p = ROVER_GUIDANCE_TURN_PGAIN;
53 rover_guidance.turn_pid.i = ROVER_GUIDANCE_TURN_IGAIN;
54 rover_guidance.turn_pid.d = ROVER_GUIDANCE_TURN_DGAIN;
55 rover_guidance.turn_pid.err = 0.f;
56 rover_guidance.turn_pid.d_err = 0.f;
57 rover_guidance.turn_pid.sum_err = 0.f;
58
59#if PERIODIC_TELEMETRY
60 // TODO register messages
61#endif
62
63 // from code generation
64 autopilot_core_guidance_init();
65}
66
68{
69 // from code generation
70 autopilot_core_guidance_periodic_task();
71}
72
73static float compute_pid(struct RoverGuidancePID *pid)
74{
75 return pid->p * pid->err + pid->d * pid->d_err + pid->i * pid->sum_err;
76}
77
78#define MAX_POS_ERR 10.f // max position error
79#define MAX_SPEED_ERR 10.f // max speed error
80#define MAX_INTEGRAL_CMD (MAX_PPRZ / 10.f) // 10% of max command
81#define PROXIMITY_DIST 0.5f // proximity distance TODO improve
82
83void rover_guidance_run(float *heading_sp)
84{
85 // compute position error
86 struct FloatVect2 pos_err;
88 rover_guidance.speed_pid.err = float_vect2_norm(&pos_err);
89 BoundAbs(rover_guidance.speed_pid.err, MAX_POS_ERR);
90
91 // speed and heading update when far enough
92 if (rover_guidance.speed_pid.err > PROXIMITY_DIST) {
93 // compute speed error
95 BoundAbs(rover_guidance.speed_pid.d_err, MAX_SPEED_ERR);
96 // integral
97 rover_guidance.speed_pid.sum_err = 0.f; // nothing for now
98 // run PID
99 rover_guidance.cmd.motor_speed = MAX_PPRZ * compute_pid(&rover_guidance.speed_pid);
100 rover_guidance.cmd.motor_speed = TRIM_PPRZ(rover_guidance.cmd.motor_speed);
101
102 // if not close to WP, compute desired heading
103 rover_guidance.sp.heading = atan2f(pos_err.x, pos_err.y);
104 *heading_sp = rover_guidance.sp.heading; // update nav sp
105 }
106 else {
107 rover_guidance.cmd.motor_speed = 0.f;
108 // use nav heading ref and run angular control
109 rover_guidance.sp.heading = *heading_sp;
110 }
111
112 // angular error
113 rover_guidance.turn_pid.err = rover_guidance.sp.heading - stateGetNedToBodyEulers_f()->psi;
114 NormRadAngle(rover_guidance.turn_pid.err);
115 // turn rate error
116 rover_guidance.turn_pid.d_err = - stateGetBodyRates_f()->r;
117 // integral
118 rover_guidance.turn_pid.sum_err = 0.f; // nothing for now
119 // run PID
120 rover_guidance.cmd.motor_turn = MAX_PPRZ * compute_pid(&rover_guidance.turn_pid);
121 rover_guidance.cmd.motor_turn = TRIM_PPRZ(rover_guidance.cmd.motor_turn);
122}
123
125{
126 ClearBit(rover_guidance.sp.mask, 5);
127 ClearBit(rover_guidance.sp.mask, 7);
128
129 /* horizontal position setpoint from navigation/flightplan */
130 //INT32_VECT2_NED_OF_ENU(rover_guidance.sp.pos, navigation_carrot); TODO move to AP xml
131
132 //nav_heading = stateGetNedToBodyEulers_i()->psi; TODO move to AP xml
134
135 // TODO reset integral part ?
136}
137
138// TODO from AP xml
139//void rover_guidance_from_nav(bool in_flight)
140//{
141// if (!in_flight) {
142// rover_guidance_nav_enter();
143// }
144//
145// if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
146// struct Int32Eulers sp_cmd_i;
147// sp_cmd_i.phi = nav_roll;
148// sp_cmd_i.theta = nav_pitch;
149// sp_cmd_i.psi = nav_heading;
150// stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
151// stabilization_attitude_run(in_flight);
152// } else {
153// INT32_VECT2_NED_OF_ENU(rover_guidance.sp.pos, navigation_carrot);
154//
155// /* set psi command */
156// rover_guidance.sp.heading = ANGLE_FLOAT_OF_BFP(nav_heading);
157// FLOAT_ANGLE_NORMALIZE(rover_guidance.sp.heading);
158//
159// /* compute x,y earth commands */
160// rover_guidance_traj_run(in_flight);
161// /* set final attitude setpoint */
162// int32_t heading_sp_i = ANGLE_BFP_OF_REAL(rover_guidance.sp.heading);
163// stabilization_attitude_set_earth_cmd_i(&rover_guidance_cmd_earth,
164// heading_sp_i);
165// stabilization_attitude_run(in_flight);
166// }
167//}
168
170{
171 rover_guidance.speed_pid.i = igain;
172 rover_guidance.speed_pid.sum_err = 0.f;
173}
174
176{
177 rover_guidance.turn_pid.i = igain;
178 rover_guidance.turn_pid.sum_err = 0.f;
179}
180
181
182#ifdef ROVER_GUIDANCE_MODE_GUIDED
183void rover_guidance_guided_run(bool in_flight)
184{
185 /* rover_guidance.sp.pos and rover_guidance.sp.heading need to be set from external source */
186 if (!in_flight) {
187 rover_guidance_hover_enter();
188 }
189
190 /* compute x,y earth commands */
191 rover_guidance_traj_run(in_flight);
192}
193
194bool rover_guidance_set_guided_pos(float x, float y)
195{
196 if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
197 ClearBit(rover_guidance.sp.mask, 5);
198 rover_guidance.sp.pos.x = x;
199 rover_guidance.sp.pos.y = y;
200 return true;
201 }
202 return false;
203}
204
205bool rover_guidance_set_guided_heading(float heading)
206{
207 if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
208 ClearBit(rover_guidance.sp.mask, 7);
209 rover_guidance.sp.heading = heading;
211 return true;
212 }
213 return false;
214}
215
216bool rover_guidance_set_guided_body_vel(float vx, float vy)
217{
218 float psi = stateGetNedToBodyEulers_f()->psi;
219 float newvx = cosf(-psi) * vx + sinf(-psi) * vy;
220 float newvy = -sinf(-psi) * vx + cosf(-psi) * vy;
221 return rover_guidance_set_guided_vel(newvx, newvy);
222}
223
224bool rover_guidance_set_guided_vel(float vx, float vy)
225{
226 if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
227 SetBit(rover_guidance.sp.mask, 5);
228 rover_guidance.sp.speed.x = vx;
229 rover_guidance.sp.speed.y = vy;
230 return true;
231 }
232 return false;
233}
234
235bool rover_guidance_set_guided_heading_rate(float rate)
236{
237 if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
238 SetBit(rover_guidance.sp.mask, 7);
239 rover_guidance.sp.heading_rate = rate;
240 return true;
241 }
242 return false;
243}
244
245#endif
246
float r
in rad/s
float psi
in radians
#define FLOAT_ANGLE_NORMALIZE(_a)
static float float_vect2_norm(const struct FloatVect2 *v)
#define FLOAT_VECT2_ZERO(_v)
#define VECT2_DIFF(_c, _a, _b)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1392
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition state.h:1087
#define MAX_SPEED_ERR
#define MAX_POS_ERR
#define MAX_PPRZ
Definition paparazzi.h:8
#define TRIM_PPRZ(pprz)
Definition paparazzi.h:11
void rover_guidance_run(float *heading_sp)
void rover_guidance_set_speed_igain(uint32_t igain)
void rover_guidance_set_turn_igain(uint32_t igain)
#define PROXIMITY_DIST
void rover_guidance_enter(void)
static float compute_pid(struct RoverGuidancePID *pid)
void rover_guidance_periodic(void)
void rover_guidance_init(void)
struct RoverGuidance rover_guidance
Basic guidance for rover.
API to get/set the generic vehicle states.
Periodic telemetry system header (includes downlink utility and generated code).
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
float heading
Definition wedgebug.c:258