Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
FLOAT_VECT2_ZERO
#define FLOAT_VECT2_ZERO(_v)
Definition: pprz_algebra_float.h:129
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
stateGetPositionNed_f
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
compute_pid
static float compute_pid(struct RoverHoloGuidancePID *pid)
Definition: rover_guidance_holonomic.c:74
rover_holo_guidance_periodic
void rover_holo_guidance_periodic(void)
Definition: rover_guidance_holonomic.c:68
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
RoverHoloGuidanceSetpoint::mask
uint8_t mask
bit 5: vx & vy, bit 6: vz, bit 7: vyaw
Definition: rover_guidance_holonomic.h:42
RoverHoloGuidancePID::d
float d
Definition: rover_guidance_holonomic.h:49
rover_holo_guidance_init
void rover_holo_guidance_init(void)
Definition: rover_guidance_holonomic.c:43
RoverHoloGuidanceSetpoint::heading
float heading
heading setpoint
Definition: rover_guidance_holonomic.h:41
rover_holo_guidance_enter
void rover_holo_guidance_enter(void)
Definition: rover_guidance_holonomic.c:131
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
FloatVect2
Definition: pprz_algebra_float.h:49
telemetry.h
float_vect2_norm
static float float_vect2_norm(struct FloatVect2 *v)
Definition: pprz_algebra_float.h:139
MAX_POS_ERR
#define MAX_POS_ERR
Definition: rover_guidance_holonomic.c:79
RoverHoloGuidancePID::err
float err
Definition: rover_guidance_holonomic.h:50
rover_guidance_holonomic.h
rover_holo_guidance
struct RoverHoloGuidance rover_holo_guidance
Definition: rover_guidance_holonomic.c:36
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
RoverHoloGuidancePID::i
float i
Definition: rover_guidance_holonomic.h:48
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
RoverHoloGuidancePID::sum_err
float sum_err
Definition: rover_guidance_holonomic.h:52
RoverHoloGuidanceControl::motor_speed_y
float motor_speed_y
Definition: rover_guidance_holonomic.h:58
MAX_SPEED_ERR
#define MAX_SPEED_ERR
Definition: rover_guidance_holonomic.c:80
PROXIMITY_DIST
#define PROXIMITY_DIST
Definition: rover_guidance_holonomic.c:82
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
RoverHoloGuidance::turn_pid
struct RoverHoloGuidancePID turn_pid
turn rate controller
Definition: rover_guidance_holonomic.h:66
RoverHoloGuidanceSetpoint::pos
struct FloatVect2 pos
position setpoint in NED.
Definition: rover_guidance_holonomic.h:39
RoverHoloGuidancePID::d_err
float d_err
Definition: rover_guidance_holonomic.h:51
RoverHoloGuidancePID::p
float p
Definition: rover_guidance_holonomic.h:47
rover_guidance_holonomic_set_speed_igain
void rover_guidance_holonomic_set_speed_igain(float igain)
Definition: rover_guidance_holonomic.c:141
RoverHoloGuidanceControl::motor_speed_x
float motor_speed_x
Definition: rover_guidance_holonomic.h:57
RoverHoloGuidance::sp
struct RoverHoloGuidanceSetpoint sp
setpoints
Definition: rover_guidance_holonomic.h:64
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
RoverHoloGuidance::cmd
struct RoverHoloGuidanceControl cmd
commands
Definition: rover_guidance_holonomic.h:63
TRIM_PPRZ
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
state.h
rover_holo_guidance_run
void rover_holo_guidance_run(float *heading_sp)
Definition: rover_guidance_holonomic.c:84
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
RoverHoloGuidanceControl::motor_turn
float motor_turn
Definition: rover_guidance_holonomic.h:59
RoverHoloGuidancePID
Definition: rover_guidance_holonomic.h:46
RoverHoloGuidance
Definition: rover_guidance_holonomic.h:62
RoverHoloGuidance::speed_pid
struct RoverHoloGuidancePID speed_pid
motor speed controller
Definition: rover_guidance_holonomic.h:65
rover_guidance_holonomic_set_turn_igain
void rover_guidance_holonomic_set_turn_igain(float igain)
Definition: rover_guidance_holonomic.c:147