Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
airborne_ant_track.c
Go to the documentation of this file.
1 /*
2  * Determines antenna pan angle.
3  *
4  * project: Paparazzi
5  * description: Determines antenna pan angle from
6  * plane's and home's positions and plane's heading angle.
7  * Software might be optimized
8  * by removing multiplications with 0, it is left this
9  * way for better understandabilty and changeability.
10  *
11  * authors: Arnold Schroeter, Martin Mueller, Chris Efstathiou
12  *
13  *
14  *
15  *
16  */
17 
18 #if defined(USE_AIRBORNE_ANT_TRACKING) && USE_AIRBORNE_ANT_TRACKING == 1
19 
20 #include <math.h>
21 #include <inttypes.h>
22 #include "inter_mcu.h"
24 #include "autopilot.h"
25 #include "generated/flight_plan.h"
26 #include "state.h"
28 #include "airborne_ant_track.h"
29 
30 
31 typedef struct {
32  float fx;
33  float fy;
34  float fz;
35 } VECTOR;
36 
37 typedef struct {
38  float fx1; float fx2; float fx3;
39  float fy1; float fy2; float fy3;
40  float fz1; float fz2; float fz3;
41 } MATRIX;
42 
43 float airborne_ant_pan;
44 static bool_t ant_pan_positive = 0;
45 
46 void ant_point(void);
47 static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC);
48 static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC);
49 
50 /*******************************************************************
51 ; function name: vSubtractVectors
52 ; description: subtracts two vectors a = b - c
53 ; parameters:
54 ;*******************************************************************/
55 static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
56 {
57  svA->fx = svB.fx - svC.fx;
58  svA->fy = svB.fy - svC.fy;
59  svA->fz = svB.fz - svC.fz;
60 }
61 
62 /*******************************************************************
63 ; function name: vMultiplyMatrixByVector
64 ; description: multiplies matrix by vector svA = smB * svC
65 ; parameters:
66 ;*******************************************************************/
67 static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
68 {
69  svA->fx = smB.fx1 * svC.fx + smB.fx2 * svC.fy + smB.fx3 * svC.fz;
70  svA->fy = smB.fy1 * svC.fx + smB.fy2 * svC.fy + smB.fy3 * svC.fz;
71  svA->fz = smB.fz1 * svC.fx + smB.fz2 * svC.fy + smB.fz3 * svC.fz;
72 }
73 
74 void airborne_ant_point_init(void)
75 {
76 
77  return;
78 }
79 
80 void airborne_ant_point_periodic(void)
81 {
82  float airborne_ant_pan_servo = 0;
83 
84  static VECTOR svPlanePosition,
85  Home_Position,
86  Home_PositionForPlane,
87  Home_PositionForPlane2;
88 
89  static MATRIX smRotation;
90 
91  svPlanePosition.fx = stateGetPositionEnu_f()->y;
92  svPlanePosition.fy = stateGetPositionEnu_f()->x;
93  svPlanePosition.fz = stateGetPositionUtm_f()->alt;
94 
95  Home_Position.fx = WaypointY(WP_HOME);
96  Home_Position.fy = WaypointX(WP_HOME);
97  Home_Position.fz = waypoints[WP_HOME].a;
98 
99  /* distance between plane and object */
100  vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
101 
102  /* yaw */
103  smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f());
104  smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f());
105  smRotation.fx3 = 0.;
106  smRotation.fy1 = -smRotation.fx2;
107  smRotation.fy2 = smRotation.fx1;
108  smRotation.fy3 = 0.;
109  smRotation.fz1 = 0.;
110  smRotation.fz2 = 0.;
111  smRotation.fz3 = 1.;
112 
113  vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane);
114 
115 
116  /*
117  * This is for one axis pan antenna mechanisms. The default is to
118  * circle clockwise so view is right. The pan servo neutral makes
119  * the antenna look to the right with 0° given, 90° is to the back and
120  * -90° is to the front.
121  *
122  *
123  *
124  * plane front
125  *
126  * 90
127  ^
128  * I
129  * 135 I 45°
130  * \ I /
131  * \I/
132  * 180-------I------- 0°
133  * /I\
134  * / I \
135  * -135 I -45°
136  * I
137  * -90
138  * plane back
139  *
140  *
141  */
142 
143  /* fPan = 0 -> antenna looks along the wing
144  90 -> antenna looks in flight direction
145  -90 -> antenna looks backwards
146  */
147  /* fixed to the plane*/
148  airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy)));
149 
150  // I need to avoid oscillations around the 180 degree mark.
151  if (airborne_ant_pan > 0 && airborne_ant_pan <= RadOfDeg(175)) { ant_pan_positive = 1; }
152  if (airborne_ant_pan < 0 && airborne_ant_pan >= RadOfDeg(-175)) { ant_pan_positive = 0; }
153 
154  if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0) {
155  airborne_ant_pan = RadOfDeg(-180);
156 
157  } else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive) {
158  airborne_ant_pan = RadOfDeg(180);
159  ant_pan_positive = 0;
160  }
161 
162 #ifdef ANT_PAN_NEUTRAL
163  airborne_ant_pan = airborne_ant_pan - RadOfDeg(ANT_PAN_NEUTRAL);
164  if (airborne_ant_pan > 0) {
165  airborne_ant_pan_servo = MAX_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MAX - ANT_PAN_NEUTRAL)));
166  } else {
167  airborne_ant_pan_servo = MIN_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MIN - ANT_PAN_NEUTRAL)));
168  }
169 #endif
170 
171  airborne_ant_pan_servo = TRIM_PPRZ(airborne_ant_pan_servo);
172 
173 #ifdef COMMAND_ANT_PAN
174  ap_state->commands[COMMAND_ANT_PAN] = airborne_ant_pan_servo;
175 #endif
176 
177 
178  return;
179 }
180 
181 #endif
float fx2
Definition: point.c:90
Communication between fbw and ap processes.
float alt
in meters above WGS84 reference ellipsoid
#define MIN_PPRZ
Definition: paparazzi.h:9
float fz
Definition: point.c:86
void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
Definition: point.c:115
float fz1
Definition: point.c:92
float fy1
Definition: point.c:91
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:702
struct ap_state * ap_state
Definition: inter_mcu.c:37
float fz2
Definition: point.c:92
Definition: point.c:83
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
Definition: point.c:127
Definition: point.c:89
float fy2
Definition: point.c:91
#define WaypointY(_wp)
Definition: common_nav.h:46
float fx1
Definition: point.c:90
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:921
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:675
API to get/set the generic vehicle states.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float a
Definition: common_nav.h:42
float fx3
Definition: point.c:90
float fy
Definition: point.c:85
#define MAX_PPRZ
Definition: paparazzi.h:8
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
float y
in meters
float fy3
Definition: point.c:91
float fz3
Definition: point.c:92
float fx
Definition: point.c:84
Information relative to the other aircrafts.