Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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>
24#include "autopilot.h"
25#include "generated/flight_plan.h"
26#include "state.h"
27#include "airborne_ant_track.h"
28
29
30typedef struct {
31 float fx;
32 float fy;
33 float fz;
34} VECTOR;
35
36typedef 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
42float airborne_ant_pan;
43static bool ant_pan_positive = 0;
44
45void ant_point(void);
46static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC);
47static 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;*******************************************************************/
54static 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;*******************************************************************/
66static 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
73void airborne_ant_point_init(void)
74{
75
76 return;
77}
78
79void 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:44
#define WaypointX(_wp)
Definition common_nav.h:45
#define WaypointY(_wp)
Definition common_nav.h:46
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition state.h:832
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition state.h:1096
float fz1
Definition max7456.c:118
float fx3
Definition max7456.c:116
float fx1
Definition max7456.c:116
static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
Definition max7456.c:244
static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
Definition max7456.c:256
float fy2
Definition max7456.c:117
float fz2
Definition max7456.c:118
float fy1
Definition max7456.c:117
float fy3
Definition max7456.c:117
float fx
Definition max7456.c:110
float fz3
Definition max7456.c:118
float fy
Definition max7456.c:111
float fx2
Definition max7456.c:116
float fz
Definition max7456.c:112
#define MAX_PPRZ
Definition paparazzi.h:8
#define TRIM_PPRZ(pprz)
Definition paparazzi.h:11
#define MIN_PPRZ
Definition paparazzi.h:9
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.