18 #if defined(USE_AIRBORNE_ANT_TRACKING) && USE_AIRBORNE_ANT_TRACKING == 1
25 #include "generated/flight_plan.h"
37 float fx1;
float fx2;
float fx3;
38 float fy1;
float fy2;
float fy3;
39 float fz1;
float fz2;
float fz3;
42 float airborne_ant_pan;
43 static bool ant_pan_positive = 0;
73 void airborne_ant_point_init(
void)
79 void airborne_ant_point_periodic(
void)
81 float airborne_ant_pan_servo = 0;
83 static VECTOR svPlanePosition,
85 Home_PositionForPlane,
86 Home_PositionForPlane2;
105 smRotation.
fy1 = -smRotation.
fx2;
106 smRotation.
fy2 = smRotation.
fx1;
147 airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.
fx, (Home_PositionForPlane2.
fy)));
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; }
153 if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0) {
154 airborne_ant_pan = RadOfDeg(-180);
156 }
else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive) {
157 airborne_ant_pan = RadOfDeg(180);
158 ant_pan_positive = 0;
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)));
166 airborne_ant_pan_servo =
MIN_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MIN - ANT_PAN_NEUTRAL)));
170 airborne_ant_pan_servo =
TRIM_PPRZ(airborne_ant_pan_servo);
172 #ifdef COMMAND_ANT_PAN
173 command_set(COMMAND_ANT_PAN, airborne_ant_pan_servo);
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
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
API to get/set the generic vehicle states.