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