Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/core/commands.h"
23 #include "modules/nav/common_nav.h"
24 #include "autopilot.h"
25 #include "generated/flight_plan.h"
26 #include "state.h"
27 #include "airborne_ant_track.h"
28 
29 
30 typedef struct {
31  float fx;
32  float fy;
33  float fz;
34 } VECTOR;
35 
36 typedef struct {
37  float fx1; float fx2; float fx3;
38  float fy1; float fy2; float fy3;
39  float fz1; float fz2; float fz3;
40 } MATRIX;
41 
42 float airborne_ant_pan;
43 static bool ant_pan_positive = 0;
44 
45 void ant_point(void);
46 static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC);
47 static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC);
48 
49 /*******************************************************************
50 ; function name: vSubtractVectors
51 ; description: subtracts two vectors a = b - c
52 ; parameters:
53 ;*******************************************************************/
54 static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
55 {
56  svA->fx = svB.fx - svC.fx;
57  svA->fy = svB.fy - svC.fy;
58  svA->fz = svB.fz - svC.fz;
59 }
60 
61 /*******************************************************************
62 ; function name: vMultiplyMatrixByVector
63 ; description: multiplies matrix by vector svA = smB * svC
64 ; parameters:
65 ;*******************************************************************/
66 static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
67 {
68  svA->fx = smB.fx1 * svC.fx + smB.fx2 * svC.fy + smB.fx3 * svC.fz;
69  svA->fy = smB.fy1 * svC.fx + smB.fy2 * svC.fy + smB.fy3 * svC.fz;
70  svA->fz = smB.fz1 * svC.fx + smB.fz2 * svC.fy + smB.fz3 * svC.fz;
71 }
72 
73 void airborne_ant_point_init(void)
74 {
75 
76  return;
77 }
78 
79 void airborne_ant_point_periodic(void)
80 {
81  float airborne_ant_pan_servo = 0;
82 
83  static VECTOR svPlanePosition,
84  Home_Position,
85  Home_PositionForPlane,
86  Home_PositionForPlane2;
87 
88  static MATRIX smRotation;
89 
90  svPlanePosition.fx = stateGetPositionEnu_f()->y;
91  svPlanePosition.fy = stateGetPositionEnu_f()->x;
92  svPlanePosition.fz = stateGetPositionUtm_f()->alt;
93 
94  Home_Position.fx = WaypointY(WP_HOME);
95  Home_Position.fy = WaypointX(WP_HOME);
96  Home_Position.fz = waypoints[WP_HOME].a;
97 
98  /* distance between plane and object */
99  vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
100 
101  /* yaw */
102  smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f());
103  smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f());
104  smRotation.fx3 = 0.;
105  smRotation.fy1 = -smRotation.fx2;
106  smRotation.fy2 = smRotation.fx1;
107  smRotation.fy3 = 0.;
108  smRotation.fz1 = 0.;
109  smRotation.fz2 = 0.;
110  smRotation.fz3 = 1.;
111 
112  vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane);
113 
114 
115  /*
116  * This is for one axis pan antenna mechanisms. The default is to
117  * circle clockwise so view is right. The pan servo neutral makes
118  * the antenna look to the right with 0˚ given, 90˚ is to the back and
119  * -90˚ is to the front.
120  *
121  *
122  *
123  * plane front
124  *
125  * 90˚
126  ^
127  * I
128  * 135˚ I 45˚
129  * \ I /
130  * \I/
131  * 180˚-------I------- 0˚
132  * /I\
133  * / I \
134  * -135˚ I -45˚
135  * I
136  * -90
137  * plane back
138  *
139  *
140  */
141 
142  /* fPan = 0˚ -> antenna looks along the wing
143  90˚ -> antenna looks in flight direction
144  -90˚ -> antenna looks backwards
145  */
146  /* fixed to the plane*/
147  airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy)));
148 
149  // I need to avoid oscillations around the 180 degree mark.
150  if (airborne_ant_pan > 0 && airborne_ant_pan <= RadOfDeg(175)) { ant_pan_positive = 1; }
151  if (airborne_ant_pan < 0 && airborne_ant_pan >= RadOfDeg(-175)) { ant_pan_positive = 0; }
152 
153  if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0) {
154  airborne_ant_pan = RadOfDeg(-180);
155 
156  } else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive) {
157  airborne_ant_pan = RadOfDeg(180);
158  ant_pan_positive = 0;
159  }
160 
161 #ifdef ANT_PAN_NEUTRAL
162  airborne_ant_pan = airborne_ant_pan - RadOfDeg(ANT_PAN_NEUTRAL);
163  if (airborne_ant_pan > 0) {
164  airborne_ant_pan_servo = MAX_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MAX - ANT_PAN_NEUTRAL)));
165  } else {
166  airborne_ant_pan_servo = MIN_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MIN - ANT_PAN_NEUTRAL)));
167  }
168 #endif
169 
170  airborne_ant_pan_servo = TRIM_PPRZ(airborne_ant_pan_servo);
171 
172 #ifdef COMMAND_ANT_PAN
173  command_set(COMMAND_ANT_PAN, airborne_ant_pan_servo);
174 #endif
175 
176 
177  return;
178 }
179 
180 #endif
Core autopilot interface common to all firmwares.
Hardware independent code for commands handling.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
#define WaypointX(_wp)
Definition: common_nav.h:45
float a
Definition: common_nav.h:42
#define WaypointY(_wp)
Definition: common_nav.h:46
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
@ VECTOR
Definition: guidance_OA.h:52
float fz1
Definition: point.c:93
float fx3
Definition: point.c:91
float fx1
Definition: point.c:91
float fy2
Definition: point.c:92
float fz2
Definition: point.c:93
float fy1
Definition: point.c:92
float fy3
Definition: point.c:92
float fx
Definition: point.c:85
float fz3
Definition: point.c:93
float fy
Definition: point.c:86
float fx2
Definition: point.c:91
float fz
Definition: point.c:87
Definition: point.c:90
Definition: point.c:84
#define MAX_PPRZ
Definition: paparazzi.h:8
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
#define MIN_PPRZ
Definition: paparazzi.h:9
void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
Definition: point.c:116
void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
Definition: point.c:128
float y
in meters
float x
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
API to get/set the generic vehicle states.