Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 "estimator.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 = estimator_y;
89  svPlanePosition.fy = estimator_x;
90  svPlanePosition.fz = estimator_z;
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(estimator_hspeed_dir));
101  smRotation.fx2 = (float)(sin(estimator_hspeed_dir));
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){
152  airborne_ant_pan = RadOfDeg(-180);
153 
154  }else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive){
155  airborne_ant_pan = RadOfDeg(180);
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:88
float fz1
Definition: point.c:93
float fx
Definition: point.c:86
#define MIN_PPRZ
Definition: paparazzi.h:9
float fy
Definition: point.c:87
float fx3
Definition: point.c:91
float fz2
Definition: point.c:93
float estimator_y
north position in meters
Definition: estimator.c:43
void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
Definition: point.c:115
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
struct ap_state * ap_state
Definition: inter_mcu.c:35
float fz3
Definition: point.c:93
float fy3
Definition: point.c:92
Definition: point.c:90
float fy1
Definition: point.c:92
float estimator_x
east position in meters
Definition: estimator.c:42
void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
Definition: point.c:127
float fx2
Definition: point.c:91
float fy2
Definition: point.c:92
float fx1
Definition: point.c:91
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
State estimation, fusioning sensors.
#define MAX_PPRZ
Definition: paparazzi.h:8
Definition: point.c:85
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
Informations relative to the other aircrafts.