Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
rover_guidance_holonomic.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2018 Fabien Bonneval <fabien.bonneval@gmail.com>
3 * Gautier Hattenberger <gautier.hattenberger@enac.fr>
4 *
5 * This file is part of paparazzi.
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, see
19 * <http://www.gnu.org/licenses/>.
20 */
21
28
29#define AUTOPILOT_CORE_GUIDANCE_C
30
32#include "generated/airframe.h"
33#include "generated/autopilot_core_guidance.h"
34#include "state.h"
35
37
38#if PERIODIC_TELEMETRY
40// TODO rover guidance messages
41#endif
42
44{
46 rover_holo_guidance.sp.heading = 0.0f;
47 rover_holo_guidance.speed_pid.p = ROVER_HOLO_GUIDANCE_SPEED_PGAIN;
48 rover_holo_guidance.speed_pid.i = ROVER_HOLO_GUIDANCE_SPEED_IGAIN;
49 rover_holo_guidance.speed_pid.d = ROVER_HOLO_GUIDANCE_SPEED_DGAIN;
50 rover_holo_guidance.speed_pid.err = 0.f;
51 rover_holo_guidance.speed_pid.d_err = 0.f;
52 rover_holo_guidance.speed_pid.sum_err = 0.f;
53 rover_holo_guidance.turn_pid.p = ROVER_HOLO_GUIDANCE_TURN_PGAIN;
54 rover_holo_guidance.turn_pid.i = ROVER_HOLO_GUIDANCE_TURN_IGAIN;
55 rover_holo_guidance.turn_pid.d = ROVER_HOLO_GUIDANCE_TURN_DGAIN;
56 rover_holo_guidance.turn_pid.err = 0.f;
57 rover_holo_guidance.turn_pid.d_err = 0.f;
58 rover_holo_guidance.turn_pid.sum_err = 0.f;
59
60#if PERIODIC_TELEMETRY
61 // TODO register messages
62#endif
63
64 // from code generation
65 autopilot_core_guidance_init();
66}
67
69{
70 // from code generation
71 autopilot_core_guidance_periodic_task();
72}
73
74static float compute_pid(struct RoverHoloGuidancePID *pid)
75{
76 return pid->p * pid->err + pid->d * pid->d_err + pid->i * pid->sum_err;
77}
78
79#define MAX_POS_ERR 10.f // max position error
80#define MAX_SPEED_ERR 10.f // max speed error
81#define MAX_INTEGRAL_CMD (MAX_PPRZ / 10.f) // 10% of max command
82#define PROXIMITY_DIST 0.2f // proximity distance TODO improve
83
84void rover_holo_guidance_run(float *heading_sp)
85{
86 // compute position error
87 struct FloatVect2 pos_err;
89 rover_holo_guidance.speed_pid.err = float_vect2_norm(&pos_err);
90 BoundAbs(rover_holo_guidance.speed_pid.err, MAX_POS_ERR);
91
92 // speed update when far enough
93 if (rover_holo_guidance.speed_pid.err > PROXIMITY_DIST) {
94 // compute speed error
96 BoundAbs(rover_holo_guidance.speed_pid.d_err, MAX_SPEED_ERR);
97 // integral
98 rover_holo_guidance.speed_pid.sum_err = 0.f; // nothing for now
99 // run PID
100 float speed = MAX_PPRZ * compute_pid(&rover_holo_guidance.speed_pid);
101 speed = TRIM_PPRZ(speed);
102
103 float cpsi = cosf(stateGetNedToBodyEulers_f()->psi);
104 float spsi = sinf(stateGetNedToBodyEulers_f()->psi);
105 struct FloatVect2 err_body = {
106 .x = cpsi * pos_err.x + spsi * pos_err.y,
107 .y = -spsi * pos_err.x + cpsi * pos_err.y
108 };
109 // command in rover frame = speed * normalized direction vector
110 // TODO normalize to min/max speed with pprz_t scale
111 rover_holo_guidance.cmd.motor_speed_x = speed * err_body.x / rover_holo_guidance.speed_pid.err;
112 rover_holo_guidance.cmd.motor_speed_y = speed * err_body.y / rover_holo_guidance.speed_pid.err;
113 } else {
114 rover_holo_guidance.cmd.motor_speed_x = 0.f;
115 rover_holo_guidance.cmd.motor_speed_y = 0.f;
116 }
117
118 rover_holo_guidance.sp.heading = *heading_sp;
119 // angular error
121 NormRadAngle(rover_holo_guidance.turn_pid.err);
122 // turn rate error
123 rover_holo_guidance.turn_pid.d_err = - stateGetBodyRates_f()->r;
124 // integral
125 rover_holo_guidance.turn_pid.sum_err = 0.f; // nothing for now
126 // run PID
127 rover_holo_guidance.cmd.motor_turn = MAX_PPRZ * compute_pid(&rover_holo_guidance.turn_pid);
128 rover_holo_guidance.cmd.motor_turn = TRIM_PPRZ(rover_holo_guidance.cmd.motor_turn);
129}
130
132{
133 ClearBit(rover_holo_guidance.sp.mask, 5);
134 ClearBit(rover_holo_guidance.sp.mask, 7);
135
137
138 // TODO reset integral part ?
139}
140
142{
143 rover_holo_guidance.speed_pid.i = igain;
144 rover_holo_guidance.speed_pid.sum_err = 0.f;
145}
146
148{
149 rover_holo_guidance.turn_pid.i = igain;
150 rover_holo_guidance.turn_pid.sum_err = 0.f;
151}
152
153
float r
in rad/s
float psi
in radians
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 NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
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
#define PROXIMITY_DIST
void rover_holo_guidance_init(void)
struct RoverHoloGuidance rover_holo_guidance
void rover_holo_guidance_periodic(void)
void rover_holo_guidance_run(float *heading_sp)
static float compute_pid(struct RoverHoloGuidancePID *pid)
void rover_holo_guidance_enter(void)
void rover_guidance_holonomic_set_turn_igain(float igain)
void rover_guidance_holonomic_set_speed_igain(float igain)
Basic guidance for rover.
API to get/set the generic vehicle states.
Periodic telemetry system header (includes downlink utility and generated code).