Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
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 {
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;
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;
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 
74 static 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 
84 void rover_holo_guidance_run(float *heading_sp)
85 {
86  // compute position error
87  struct FloatVect2 pos_err;
91 
92  // speed update when far enough
94  // compute speed error
97  // integral
98  rover_holo_guidance.speed_pid.sum_err = 0.f; // nothing for now
99  // run 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
113  } else {
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
124  // integral
125  rover_holo_guidance.turn_pid.sum_err = 0.f; // nothing for now
126  // run PID
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 {
145 }
146 
148 {
151 }
152 
153 
struct RoverHoloGuidance rover_holo_guidance
struct RoverHoloGuidanceSetpoint sp
setpoints
#define FLOAT_VECT2_ZERO(_v)
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
float heading
heading setpoint
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
Periodic telemetry system header (includes downlink utility and generated code).
float r
in rad/s
float psi
in radians
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
void rover_holo_guidance_init(void)
struct RoverHoloGuidancePID speed_pid
motor speed controller
uint8_t mask
bit 5: vx & vy, bit 6: vz, bit 7: vyaw
#define MAX_SPEED_ERR
static float float_vect2_norm(struct FloatVect2 *v)
Basic guidance for rover.
void rover_holo_guidance_periodic(void)
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
void rover_guidance_holonomic_set_turn_igain(float igain)
void rover_holo_guidance_enter(void)
API to get/set the generic vehicle states.
void rover_holo_guidance_run(float *heading_sp)
struct RoverHoloGuidancePID turn_pid
turn rate controller
#define MAX_POS_ERR
struct RoverHoloGuidanceControl cmd
commands
struct FloatVect2 pos
position setpoint in NED.
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
#define PROXIMITY_DIST
static float compute_pid(struct RoverHoloGuidancePID *pid)
void rover_guidance_holonomic_set_speed_igain(float igain)