Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
34 
35 bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
36 {
41  return true;
42  }
43  return false;
44 }
45 
46 bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
47 {
49  float x = stateGetPositionNed_f()->x + dx;
50  float y = stateGetPositionNed_f()->y + dy;
51  float z = stateGetPositionNed_f()->z + dz;
52  float heading = stateGetNedToBodyEulers_f()->psi + dyaw;
53  return autopilot_guided_goto_ned(x, y, z, heading);
54  }
55  return false;
56 }
57 
58 bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
59 {
62  float x = stateGetPositionNed_f()->x + cosf(-psi) * dx + sinf(-psi) * dy;
63  float y = stateGetPositionNed_f()->y - sinf(-psi) * dx + cosf(-psi) * dy;
64  float z = stateGetPositionNed_f()->z + dz;
65  float heading = psi + dyaw;
66  return autopilot_guided_goto_ned(x, y, z, heading);
67  }
68  return false;
69 }
70 
71 bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
72 {
77  return true;
78  }
79  return false;
80 }
81 
82 /* Set guided mode setpoint
83  * Note: Offset position command in NED frame or body frame will only be implemented if
84  * local reference frame has been initialised.
85  * Flag definition:
86  bit 0: x,y as offset coordinates
87  bit 1: x,y in body coordinates
88  bit 2: z as offset coordinates
89  bit 3: yaw as offset coordinates
90  bit 4: free
91  bit 5: x,y as vel
92  bit 6: z as vel
93  bit 7: yaw as rate
94  */
95 void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
96 {
97  /* only update setpoints when in guided mode */
99  return;
100  }
101 
102  // handle x,y
103  struct FloatVect2 setpoint = {.x = x, .y = y};
104  if (bit_is_set(flags, 5)) { // velocity setpoint
105  if (bit_is_set(flags, 1)) { // set velocity in body frame
106  guidance_h_set_guided_body_vel(setpoint.x, setpoint.y);
107  } else {
108  guidance_h_set_guided_vel(setpoint.x, setpoint.y);
109  }
110  } else { // position setpoint
111  if (!bit_is_set(flags, 0) && !bit_is_set(flags, 1)) { // set absolute position setpoint
112  guidance_h_set_guided_pos(setpoint.x, setpoint.y);
113  } else {
115  if (bit_is_set(flags, 1)) { // set position as offset in body frame
116  float psi = stateGetNedToBodyEulers_f()->psi;
117 
118  setpoint.x = stateGetPositionNed_f()->x + cosf(-psi) * x + sinf(-psi) * y;
119  setpoint.y = stateGetPositionNed_f()->y - sinf(-psi) * x + cosf(-psi) * y;
120  } else { // set position as offset in NED frame
121  setpoint.x += stateGetPositionNed_f()->x;
122  setpoint.y += stateGetPositionNed_f()->y;
123  }
124  guidance_h_set_guided_pos(setpoint.x, setpoint.y);
125  }
126  }
127  }
128 
129  //handle z
130  if (bit_is_set(flags, 6)) { // speed set-point
132  } else { // position set-point
133  if (bit_is_set(flags, 2)) { // set position as offset in NED frame
135  z += stateGetPositionNed_f()->z;
137  }
138  } else {
140  }
141  }
142 
143  //handle yaw
144  if (bit_is_set(flags, 7)) { // speed set-point
146  } else { // position set-point
147  if (bit_is_set(flags, 3)) { // set yaw as offset
148  yaw += stateGetNedToBodyEulers_f()->psi; // will be wrapped to [-pi,pi] later
149  }
151  }
152 }
153 
bool guidance_v_set_guided_z(float z)
Set z setpoint in GUIDED mode.
Definition: guidance_v.c:551
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
float y
in meters
bool guidance_h_set_guided_heading_rate(float rate)
Set heading rate setpoint in GUIDED mode.
Definition: guidance_h.c:738
bool guidance_v_set_guided_vz(float vz)
Set z velocity setpoint in GUIDED mode.
Definition: guidance_v.c:568
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
Set guided setpoints using flag mask in GUIDED mode.
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
bool guidance_h_set_guided_pos(float x, float y)
Set horizontal position setpoint in GUIDED mode.
Definition: guidance_h.c:697
float psi
in radians
bool guidance_h_set_guided_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint in GUIDED mode.
Definition: guidance_h.c:719
Autopilot guided mode interface.
float z
in meters
static float heading
Definition: ahrs_infrared.c:45
#define AP_MODE_GUIDED
Core autopilot interface common to all firmwares.
bool guidance_h_set_guided_vel(float vx, float vy)
Set horizontal velocity setpoint in GUIDED mode.
Definition: guidance_h.c:727
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
Set velocity and heading setpoints in GUIDED mode.
bool guidance_h_set_guided_heading(float heading)
Set heading setpoint in GUIDED mode.
Definition: guidance_h.c:708
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
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.
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
float x
in meters