Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
autopilot_guided.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 The Paparazzi Team
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
30 #include "autopilot.h"
32 #include "state.h"
33 #include "pprzlink/dl_protocol.h"
34 
35 
36 bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
37 {
39  guidance_h_set_pos(x, y);
42  return true;
43  }
44  return false;
45 }
46 
47 bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
48 {
50  float x = stateGetPositionNed_f()->x + dx;
51  float y = stateGetPositionNed_f()->y + dy;
52  float z = stateGetPositionNed_f()->z + dz;
53  float heading = stateGetNedToBodyEulers_f()->psi + dyaw;
54  return autopilot_guided_goto_ned(x, y, z, heading);
55  }
56  return false;
57 }
58 
59 bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
60 {
62  float psi = stateGetNedToBodyEulers_f()->psi;
63  float x = stateGetPositionNed_f()->x + cosf(-psi) * dx + sinf(-psi) * dy;
64  float y = stateGetPositionNed_f()->y - sinf(-psi) * dx + cosf(-psi) * dy;
65  float z = stateGetPositionNed_f()->z + dz;
66  float heading = psi + dyaw;
67  return autopilot_guided_goto_ned(x, y, z, heading);
68  }
69  return false;
70 }
71 
72 bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
73 {
75  guidance_h_set_vel(vx, vy);
78  return true;
79  }
80  return false;
81 }
82 
83 /* Set guided mode setpoint
84  * Note: Offset position command in NED frame or body frame will only be implemented if
85  * local reference frame has been initialised.
86  * Flag definition:
87  bit 0: x,y as offset coordinates
88  bit 1: x,y in body coordinates
89  bit 2: z as offset coordinates
90  bit 3: yaw as offset coordinates
91  bit 4: free
92  bit 5: x,y as vel
93  bit 6: z as vel
94  bit 7: yaw as rate
95  */
96 void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
97 {
98  // handle x,y
99  struct FloatVect2 setpoint = {.x = x, .y = y};
100  if (bit_is_set(flags, 5)) { // velocity setpoint
101  if (bit_is_set(flags, 1)) { // set velocity in body frame
102  guidance_h_set_body_vel(setpoint.x, setpoint.y);
103  } else {
104  guidance_h_set_vel(setpoint.x, setpoint.y);
105  }
106  } else { // position setpoint
107  if (!bit_is_set(flags, 0) && !bit_is_set(flags, 1)) { // set absolute position setpoint
108  guidance_h_set_pos(setpoint.x, setpoint.y);
109  } else {
111  if (bit_is_set(flags, 1)) { // set position as offset in body frame
112  float psi = stateGetNedToBodyEulers_f()->psi;
113 
114  setpoint.x = stateGetPositionNed_f()->x + cosf(-psi) * x + sinf(-psi) * y;
115  setpoint.y = stateGetPositionNed_f()->y - sinf(-psi) * x + cosf(-psi) * y;
116  } else { // set position as offset in NED frame
117  setpoint.x += stateGetPositionNed_f()->x;
118  setpoint.y += stateGetPositionNed_f()->y;
119  }
120  guidance_h_set_pos(setpoint.x, setpoint.y);
121  }
122  }
123  }
124 
125  //handle z
126  if (bit_is_set(flags, 6)) { // speed set-point
128  } else { // position set-point
129  if (bit_is_set(flags, 2)) { // set position as offset in NED frame
131  z += stateGetPositionNed_f()->z;
132  guidance_v_set_z(z);
133  }
134  } else {
135  guidance_v_set_z(z);
136  }
137  }
138 
139  //handle yaw
140  if (bit_is_set(flags, 7)) { // speed set-point
142  } else { // position set-point
143  if (bit_is_set(flags, 3)) { // set yaw as offset
144  yaw += stateGetNedToBodyEulers_f()->psi; // will be wrapped to [-pi,pi] later
145  }
147  }
148 }
149 
153  if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID || autopilot_get_mode() != AP_MODE_GUIDED) {
154  return;
155  }
156 
158  DL_GUIDED_SETPOINT_NED_flags(buf),
159  DL_GUIDED_SETPOINT_NED_x(buf),
160  DL_GUIDED_SETPOINT_NED_y(buf),
161  DL_GUIDED_SETPOINT_NED_z(buf),
162  DL_GUIDED_SETPOINT_NED_yaw(buf));
163 }
164 
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:217
Core autopilot interface common to all firmwares.
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
Set guided setpoints using flag mask in GUIDED mode.
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
Set velocity and heading setpoints in GUIDED mode.
void autopilot_guided_parse_GUIDED(uint8_t *buf)
Parse GUIDED_SETPOINT_NED messages from datalink.
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
Autopilot guided mode interface.
float psi
in radians
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
float z
in meters
float x
in meters
float y
in meters
#define AP_MODE_GUIDED
void guidance_h_set_pos(float x, float y)
Set horizontal position setpoint.
Definition: guidance_h.c:578
void guidance_h_set_heading(float heading)
Set heading setpoint.
Definition: guidance_h.c:588
void guidance_h_set_vel(float vx, float vy)
Set horizontal acceleration setpoint.
Definition: guidance_h.c:603
void guidance_h_set_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint.
Definition: guidance_h.c:595
void guidance_h_set_heading_rate(float rate)
Set heading rate setpoint.
Definition: guidance_h.c:631
void guidance_v_set_vz(float vz)
Set z velocity setpoint.
Definition: guidance_v.c:403
void guidance_v_set_z(float z)
Set z position setpoint.
Definition: guidance_v.c:393
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float heading
Definition: wedgebug.c:258