Paparazzi UAS  v7.0_unstable
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_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;
173 }
174 
176 {
177  rover_guidance.turn_pid.i = igain;
179 }
180 
181 
182 #ifdef ROVER_GUIDANCE_MODE_GUIDED
183 void 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 
194 bool 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 
205 bool rover_guidance_set_guided_heading(float heading)
206 {
207  if (rover_guidance.mode == ROVER_GUIDANCE_MODE_GUIDED) {
208  ClearBit(rover_guidance.sp.mask, 7);
211  return true;
212  }
213  return false;
214 }
215 
216 bool 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 
224 bool 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 
235 bool 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(struct FloatVect2 *v)
#define FLOAT_VECT2_ZERO(_v)
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
#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)
#define MAX_SPEED_ERR
void rover_guidance_set_turn_igain(uint32_t igain)
#define PROXIMITY_DIST
void rover_guidance_enter(void)
static float compute_pid(struct RoverGuidancePID *pid)
#define MAX_POS_ERR
void rover_guidance_periodic(void)
void rover_guidance_init(void)
struct RoverGuidance rover_guidance
Basic guidance for rover.
uint8_t mask
bit 5: vx & vy, bit 6: vz, bit 7: vyaw
struct RoverGuidanceSetpoint sp
setpoints
struct RoverGuidancePID turn_pid
turn rate controller
float heading
heading setpoint
struct FloatVect2 speed
speed setpoint
struct RoverGuidancePID speed_pid
motor speed controller
struct FloatVect2 pos
position setpoint in NED.
struct RoverGuidanceControl cmd
commands
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.
Definition: vl53l1_types.h:78
float heading
Definition: wedgebug.c:258