Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
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;
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;
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 
73 static 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 
83 void rover_guidance_run(float *heading_sp)
84 {
85  // compute position error
86  struct FloatVect2 pos_err;
90 
91  // speed and heading update when far enough
93  // compute speed error
96  // integral
97  rover_guidance.speed_pid.sum_err = 0.f; // nothing for now
98  // run PID
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 {
108  // use nav heading ref and run angular control
109  rover_guidance.sp.heading = *heading_sp;
110  }
111 
112  // angular error
114  NormRadAngle(rover_guidance.turn_pid.err);
115  // turn rate error
117  // integral
118  rover_guidance.turn_pid.sum_err = 0.f; // nothing for now
119  // run PID
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_MANUAL) {
146 // stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
147 // stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
148 // stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
149 // } else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
150 // struct Int32Eulers sp_cmd_i;
151 // sp_cmd_i.phi = nav_roll;
152 // sp_cmd_i.theta = nav_pitch;
153 // sp_cmd_i.psi = nav_heading;
154 // stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
155 // stabilization_attitude_run(in_flight);
156 // } else {
157 // INT32_VECT2_NED_OF_ENU(rover_guidance.sp.pos, navigation_carrot);
158 //
159 // /* set psi command */
160 // rover_guidance.sp.heading = ANGLE_FLOAT_OF_BFP(nav_heading);
161 // FLOAT_ANGLE_NORMALIZE(rover_guidance.sp.heading);
162 //
163 // /* compute x,y earth commands */
164 // rover_guidance_traj_run(in_flight);
165 // /* set final attitude setpoint */
166 // int32_t heading_sp_i = ANGLE_BFP_OF_REAL(rover_guidance.sp.heading);
167 // stabilization_attitude_set_earth_cmd_i(&rover_guidance_cmd_earth,
168 // heading_sp_i);
169 // stabilization_attitude_run(in_flight);
170 // }
171 //}
172 
174 {
175  rover_guidance.speed_pid.i = igain;
177 }
178 
180 {
181  rover_guidance.turn_pid.i = igain;
183 }
184 
185 
186 #ifdef ROVER_GUIDANCE_MODE_GUIDED
187 void rover_guidance_guided_run(bool in_flight)
188 {
189  /* rover_guidance.sp.pos and rover_guidance.sp.heading need to be set from external source */
190  if (!in_flight) {
191  rover_guidance_hover_enter();
192  }
193 
194  /* compute x,y earth commands */
195  rover_guidance_traj_run(in_flight);
196 }
197 
198 bool rover_guidance_set_guided_pos(float x, float y)
199 {
200  if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
201  ClearBit(rover_guidance.sp.mask, 5);
202  rover_guidance.sp.pos.x = x;
203  rover_guidance.sp.pos.y = y;
204  return true;
205  }
206  return false;
207 }
208 
209 bool rover_guidance_set_guided_heading(float heading)
210 {
211  if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
212  ClearBit(rover_guidance.sp.mask, 7);
215  return true;
216  }
217  return false;
218 }
219 
220 bool rover_guidance_set_guided_body_vel(float vx, float vy)
221 {
222  float psi = stateGetNedToBodyEulers_f()->psi;
223  float newvx = cosf(-psi) * vx + sinf(-psi) * vy;
224  float newvy = -sinf(-psi) * vx + cosf(-psi) * vy;
225  return rover_guidance_set_guided_vel(newvx, newvy);
226 }
227 
228 bool rover_guidance_set_guided_vel(float vx, float vy)
229 {
230  if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
231  SetBit(rover_guidance.sp.mask, 5);
232  rover_guidance.sp.speed.x = vx;
233  rover_guidance.sp.speed.y = vy;
234  return true;
235  }
236  return false;
237 }
238 
239 bool rover_guidance_set_guided_heading_rate(float rate)
240 {
241  if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
242  SetBit(rover_guidance.sp.mask, 7);
243  rover_guidance.sp.heading_rate = rate;
244  return true;
245  }
246  return false;
247 }
248 
249 #endif
250 
FLOAT_VECT2_ZERO
#define FLOAT_VECT2_ZERO(_v)
Definition: pprz_algebra_float.h:129
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
RoverGuidancePID::p
float p
Definition: rover_guidance.h:47
FLOAT_ANGLE_NORMALIZE
#define FLOAT_ANGLE_NORMALIZE(_a)
Definition: pprz_algebra_float.h:99
RoverGuidanceControl::motor_turn
float motor_turn
Definition: rover_guidance.h:57
RoverGuidancePID::d
float d
Definition: rover_guidance.h:49
PROXIMITY_DIST
#define PROXIMITY_DIST
Definition: rover_guidance.c:81
RoverGuidancePID::d_err
float d_err
Definition: rover_guidance.h:51
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
MAX_SPEED_ERR
#define MAX_SPEED_ERR
Definition: rover_guidance.c:79
RoverGuidanceSetpoint::mask
uint8_t mask
bit 5: vx & vy, bit 6: vz, bit 7: vyaw
Definition: rover_guidance.h:42
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
RoverGuidanceSetpoint::speed
struct FloatVect2 speed
speed setpoint
Definition: rover_guidance.h:40
uint32_t
unsigned long uint32_t
Definition: types.h:18
rover_guidance
struct RoverGuidance rover_guidance
Definition: rover_guidance.c:35
RoverGuidanceControl::motor_speed
float motor_speed
Definition: rover_guidance.h:56
RoverGuidance::speed_pid
struct RoverGuidancePID speed_pid
motor speed controller
Definition: rover_guidance.h:63
RoverGuidancePID::err
float err
Definition: rover_guidance.h:50
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
RoverGuidancePID::sum_err
float sum_err
Definition: rover_guidance.h:52
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
RoverGuidance::turn_pid
struct RoverGuidancePID turn_pid
turn rate controller
Definition: rover_guidance.h:64
rover_guidance_set_turn_igain
void rover_guidance_set_turn_igain(uint32_t igain)
Definition: rover_guidance.c:179
RoverGuidancePID
Definition: rover_guidance.h:46
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
rover_guidance_run
void rover_guidance_run(float *heading_sp)
Definition: rover_guidance.c:83
RoverGuidanceSetpoint::pos
struct FloatVect2 pos
position setpoint in NED.
Definition: rover_guidance.h:39
rover_guidance_enter
void rover_guidance_enter(void)
Definition: rover_guidance.c:124
RoverGuidance
Definition: rover_guidance.h:60
RoverGuidanceSetpoint::heading
float heading
heading setpoint
Definition: rover_guidance.h:41
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
RoverGuidance::cmd
struct RoverGuidanceControl cmd
commands
Definition: rover_guidance.h:62
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
MAX_POS_ERR
#define MAX_POS_ERR
Definition: rover_guidance.c:78
RoverGuidance::sp
struct RoverGuidanceSetpoint sp
setpoints
Definition: rover_guidance.h:61
RoverGuidancePID::i
float i
Definition: rover_guidance.h:48
rover_guidance.h
compute_pid
static float compute_pid(struct RoverGuidancePID *pid)
Definition: rover_guidance.c:73
rover_guidance_periodic
void rover_guidance_periodic(void)
Definition: rover_guidance.c:67
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
rover_guidance_set_speed_igain
void rover_guidance_set_speed_igain(uint32_t igain)
Definition: rover_guidance.c:173
TRIM_PPRZ
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
state.h
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
rover_guidance_init
void rover_guidance_init(void)
Definition: rover_guidance.c:42
heading
float heading
Definition: wedgebug.c:258