Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nav.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 */
21
28#include <math.h>
29#include "std.h"
30
31static unit_t unit __attribute__((unused));
32
33#define NAV_C
36#include "autopilot.h"
37#include "modules/gps/gps.h"
38
39#include "generated/flight_plan.h"
40
41#if !USE_GENERATED_AUTOPILOT
43#endif
44
46
48
51
53float nav_circle_radians; /* Cumulated */
54float nav_circle_radians_no_rewind; /* Cumulated */
55float nav_circle_trigo_qdr; /* Angle from center to mobile */
57
58
60static float nav_leg_progress;
62
64static float nav_leg_length;
65
66bool nav_in_circle = false;
67bool nav_in_segment = false;
71float circle_bank = 0;
72
75
77#ifndef NAV_GLIDE_PITCH_TRIM
78#define NAV_GLIDE_PITCH_TRIM 0.
79#endif
80
81
82
84
85/* Used in nav_survey_rectangle. Defined here for downlink and uplink */
89
91
103
104#define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))
105
106
108void nav_circle_XY(float x, float y, float radius)
109{
110 struct EnuCoor_f *pos = stateGetPositionEnu_f();
112 nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x);
113 float sign_radius = radius > 0 ? 1 : -1;
114
115 if (nav_in_circle) {
120 if (trigo_diff > 0) { // do not rewind if the change in angle is in the opposite sense than nav_radius
122 }
123 }
124
125 float dist2_center = DistanceSquare(pos->x, pos->y, x, y);
127
128 radius += -nav_shift;
129
130 float abs_radius = fabs(radius);
131
136 0 :
138
145 if (nav_mode == NAV_MODE_COURSE) {
147 }
150 nav_in_circle = true;
151 nav_in_segment = false;
152 nav_circle_x = x;
153 nav_circle_y = y;
154 nav_circle_radius = radius;
155}
156
157
162
169
170#define MAX_DIST_CARROT 250.
171#define MIN_HEIGHT_CARROT 50.
172#define MAX_HEIGHT_CARROT 150.
173
174#define Goto3D(radius) { \
175 if (autopilot_get_mode() == AP_MODE_AUTO2) { \
176 int16_t yaw = radio_control_get(RADIO_YAW); \
177 if (yaw > MIN_DX || yaw < -MIN_DX) { \
178 carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \
179 carrot_x = Min(carrot_x, MAX_DIST_CARROT); \
180 carrot_x = Max(carrot_x, -MAX_DIST_CARROT); \
181 } \
182 int16_t pitch = radio_control_get(RADIO_PITCH); \
183 if (pitch > MIN_DX || pitch < -MIN_DX) { \
184 carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \
185 carrot_y = Min(carrot_y, MAX_DIST_CARROT); \
186 carrot_y = Max(carrot_y, -MAX_DIST_CARROT); \
187 } \
188 v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
189 int16_t roll = radio_control_get(RADIO_ROLL); \
190 if (roll > MIN_DX || roll < -MIN_DX) { \
191 nav_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \
192 nav_altitude = Max(nav_altitude, MIN_HEIGHT_CARROT+ground_alt); \
193 nav_altitude = Min(nav_altitude, MAX_HEIGHT_CARROT+ground_alt); \
194 } \
195 } \
196 nav_circle_XY(carrot_x, carrot_y, radius); \
197 }
198
199
200
201#ifdef NAV_GROUND_SPEED_PGAIN
204static void nav_ground_speed_loop(void)
205{
212 } else {
213 /* Reset cruise throttle to nominal value */
215 }
216}
217#endif
218
221{
222 nav_radius = radius;
223
224 float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
225 float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
226
227 /* Unit vector from AF to TD */
228 float d = sqrtf(x_0 * x_0 + y_0 * y_0);
229 float x_1 = x_0 / d;
230 float y_1 = y_0 / d;
231
236 if (nav_radius < 0) {
238 }
239}
240
242{
243
244 float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
245 float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
246 float h_0 = waypoints[wp_td].a - waypoints[wp_af].a;
247
248 /* Unit vector from AF to TD */
249 float d = sqrtf(x_0 * x_0 + y_0 * y_0);
250 float x_1 = x_0 / d;
251 float y_1 = y_0 / d;
252
255}
256
257
258/* For a landing UPWIND.
259 Computes Top Of Descent waypoint from Touch Down and Approach Fix
260 waypoints, using glide airspeed, glide vertical speed and wind */
262{
264 float td_af_x = WaypointX(_af) - WaypointX(_td);
265 float td_af_y = WaypointY(_af) - WaypointY(_td);
266 float td_af = sqrtf(td_af_x * td_af_x + td_af_y * td_af_y);
268 wind->x * wind->x + wind->y * wind->y));
272}
273
274
275#ifndef LINE_START_FUNCTION
276#define LINE_START_FUNCTION {}
277#endif
278#ifndef LINE_STOP_FUNCTION
279#define LINE_STOP_FUNCTION {}
280#endif
281
282#ifdef TRAFFIC_INFO
284
285void nav_follow(uint8_t ac_id, float distance, float height)
286{
287 struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ac_id);
290 float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
291 float ca = cosf(alpha), sa = sinf(alpha);
292 float x = ac->x - distance * ca;
293 float y = ac->y - distance * sa;
294 fly_to_xy(x, y);
295#ifdef NAV_FOLLOW_PGAIN
296 float s = (stateGetPositionEnu_f()->x - x) * ca + (stateGetPositionEnu_f()->y - y) * sa;
299#endif
300}
301#else
302void nav_follow(uint8_t __attribute__((unused)) _ac_id, float __attribute__((unused)) distance,
303 float __attribute__((unused)) height) {}
304#endif // TRAFFIC_INFO
305
306
310float nav_pitch; /* Rad */
311float fp_pitch; /* deg */
312float fp_throttle; /* [0-1] */
313float fp_climb; /* m/s */
314
315
325bool nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
326{
328 float pw_x = x - stateGetPositionEnu_f()->x;
330 float pw_y = y - stateGetPositionEnu_f()->y;
331
332 if (approaching_time < 0.) {
333 // fly after the destination waypoint
334 float leg_x = x - from_x;
335 float leg_y = y - from_y;
336 float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));
337 float exceed_dist = approaching_time * stateGetHorizontalSpeedNorm_f(); // negative value
338 float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
339 return (scal_prod < exceed_dist);
340 } else {
341 // fly close enough of the waypoint or cross it
342 dist2_to_wp = pw_x * pw_x + pw_y * pw_y;
344 if (dist2_to_wp < min_dist * min_dist) {
345 return true;
346 }
347 float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
348 return (scal_prod < 0.);
349 }
350}
351
355//static inline void fly_to_xy(float x, float y) {
356void fly_to_xy(float x, float y)
357{
358 struct EnuCoor_f *pos = stateGetPositionEnu_f();
359 desired_x = x;
360 desired_y = y;
361 if (nav_mode == NAV_MODE_COURSE) {
362 h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y);
363 if (h_ctl_course_setpoint < 0.) {
365 }
367 } else {
368 float diff = atan2f(x - pos->x, y - pos->y) - stateGetHorizontalSpeedDir_f();
370 BoundAbs(diff, M_PI / 2.);
371 float s = sinf(diff);
372 float speed = stateGetHorizontalSpeedNorm_f();
373 h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81));
376 }
377}
378
406
408
409#ifndef FAILSAFE_HOME_RADIUS
410#define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS
411#endif
412
422
440
446{
447 nav_survey_active = false;
448
450 dist2_to_wp = 0.;
451
452 auto_nav(); /* From flight_plan.h */
453
455
456#ifdef AGR_CLIMB
459 }
460#endif
461
463}
464
468#if PERIODIC_TELEMETRY
470
476
477static void send_nav(struct transport_tx *trans, struct link_device *dev)
478{
479 SEND_NAVIGATION(trans, dev);
480}
481
482static void DownlinkSendWp(struct transport_tx *trans, struct link_device *dev, uint8_t _wp)
483{
484 float x = nav_utm_east0 + waypoints[_wp].x;
485 float y = nav_utm_north0 + waypoints[_wp].y;
487}
488
489static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
490{
491 static uint8_t i;
492 i++;
493 if (i >= nb_waypoint) { i = 0; }
494 DownlinkSendWp(trans, dev, i);
495}
496
498{
499 if (_wp >= nb_waypoint) return;
500 DownlinkSendWp(&(DefaultChannel).trans_tx, &(DefaultDevice).device, _wp);
501}
502
503
504static void send_circle(struct transport_tx *trans, struct link_device *dev)
505{
506 if (nav_in_circle) {
509 }
510}
511
512static void send_segment(struct transport_tx *trans, struct link_device *dev)
513{
514 if (nav_in_segment) {
517 }
518}
519
520static void send_survey(struct transport_tx *trans, struct link_device *dev)
521{
522 if (nav_survey_active) {
525 }
526}
527#endif
528
562
584
585void nav_parse_BLOCK(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
586{
587 if (DL_BLOCK_ac_id(buf) != AC_ID) { return; }
589 SEND_NAVIGATION(trans, dev);
590}
591
592void nav_parse_MOVE_WP(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
593{
594 if (DL_MOVE_WP_ac_id(buf) != AC_ID) { return; }
595 uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
596
597 /* Computes from (lat, long) in the referenced UTM zone */
598 struct LlaCoor_f lla;
599 lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(buf) / 1e7f));
600 lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(buf) / 1e7f));
601 lla.alt = (float)(DL_MOVE_WP_alt(buf) / 1000.f); // mm to m
602 struct UtmCoor_f utm;
604 utm_of_lla_f(&utm, &lla);
605 nav_move_waypoint(wp_id, utm.east, utm.north, utm.alt);
606
607 /* Waypoint range is limited. Computes the UTM pos back from the relative
608 coordinates */
609 utm.east = waypoints[wp_id].x + nav_utm_east0;
610 utm.north = waypoints[wp_id].y + nav_utm_north0;
611 pprz_msg_send_WP_MOVED(trans, dev, AC_ID, &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0);
612}
613
614/**************** 8 Navigation **********************************************/
615
616
618
621{
623}
624
633void nav_eight(uint8_t target, uint8_t c1, float radius)
634{
635 float aradius = fabs(radius);
636 float alt = waypoints[target].a;
637 waypoints[c1].a = alt;
638
642 d = Max(d, 1.); /* To prevent a division by zero */
643
644 /* Unit vector from target to c1 */
645 float u_x = target_c1_x / d;
646 float u_y = target_c1_y / d;
647
648 /* Move [c1] closer if needed */
649 if (d > 2 * aradius) {
650 d = 2 * aradius;
651 waypoints[c1].x = waypoints[target].x + d * u_x;
652 waypoints[c1].y = waypoints[target].y + d * u_y;
653 }
654
655 /* The other center */
656 struct point c2 = {
657 waypoints[target].x - d * u_x,
658 waypoints[target].y - d * u_y,
659 alt
660 };
661
662 struct point c1_in = {
663 waypoints[c1].x + radius * -u_y,
664 waypoints[c1].y + radius * u_x,
665 alt
666 };
667 struct point c1_out = {
668 waypoints[c1].x - radius * -u_y,
669 waypoints[c1].y - radius * u_x,
670 alt
671 };
672
673 struct point c2_in = {
674 c2.x + radius * -u_y,
675 c2.y + radius * u_x,
676 alt
677 };
678 struct point c2_out = {
679 c2.x - radius * -u_y,
680 c2.y - radius * u_x,
681 alt
682 };
683
684 float qdr_out = M_PI - atan2f(u_y, u_x);
685 if (radius < 0) {
686 qdr_out += M_PI;
687 }
688
689 switch (eight_status) {
690 case C1 :
691 NavCircleWaypoint(c1, radius);
692 if (NavQdrCloseTo(DegOfRad(qdr_out) - 10)) {
694 InitStage();
695 }
696 return;
697
698 case R1T:
702 InitStage();
703 }
704 return;
705
706 case RT2:
710 InitStage();
711 }
712 return;
713
714 case C2 :
715 nav_circle_XY(c2.x, c2.y, -radius);
716 if (NavQdrCloseTo(DegOfRad(qdr_out) + 10)) {
718 InitStage();
719 }
720 return;
721
722 case R2T:
726 InitStage();
727 }
728 return;
729
730 case RT1:
734 InitStage();
735 }
736 return;
737
738 default:/* Should not occur !!! Doing nothing */
739 return;
740 } /* switch */
741}
742
743/************** Oval Navigation **********************************************/
744
755
757{
759 nav_oval_count = 0;
760}
761
762void nav_oval(uint8_t p1, uint8_t p2, float radius)
763{
764 radius = - radius; /* Historical error ? */
765
766 float alt = waypoints[p1].a;
767 waypoints[p2].a = alt;
768
769 float p2_p1_x = waypoints[p1].x - waypoints[p2].x;
770 float p2_p1_y = waypoints[p1].y - waypoints[p2].y;
771 float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
772
773 /* Unit vector from p1 to p2 */
774 float u_x = p2_p1_x / d;
775 float u_y = p2_p1_y / d;
776
777 /* The half circle centers and the other leg */
778 struct point p1_center = { waypoints[p1].x + radius * -u_y,
779 waypoints[p1].y + radius * u_x,
780 alt
781 };
782 struct point p1_out = { waypoints[p1].x + 2 * radius * -u_y,
783 waypoints[p1].y + 2 * radius * u_x,
784 alt
785 };
786
787 struct point p2_in = { waypoints[p2].x + 2 * radius * -u_y,
788 waypoints[p2].y + 2 * radius * u_x,
789 alt
790 };
791 struct point p2_center = { waypoints[p2].x + radius * -u_y,
792 waypoints[p2].y + radius * u_x,
793 alt
794 };
795
796 float qdr_out_2 = M_PI - atan2f(u_y, u_x);
797 float qdr_out_1 = qdr_out_2 + M_PI;
798 if (radius < 0) {
799 qdr_out_2 += M_PI;
800 qdr_out_1 += M_PI;
801 }
802 float qdr_anticipation = (radius > 0 ? -15 : 15);
803
804 switch (oval_status) {
805 case OC1 :
806 nav_circle_XY(p1_center.x, p1_center.y, -radius);
809 InitStage();
811 }
812 return;
813
814 case OR12:
819 InitStage();
821 }
822 return;
823
824 case OC2 :
825 nav_circle_XY(p2_center.x, p2_center.y, -radius);
828 InitStage();
830 }
831 return;
832
833 case OR21:
837 InitStage();
839 }
840 return;
841
842 default: /* Should not occur !!! Doing nothing */
843 return;
844 }
845}
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:49
Core autopilot interface common to all firmwares.
bool launch
request launch
Definition autopilot.h:71
static uint16_t c1
static uint16_t c2
void common_flight_plan_init(void)
uint16_t stage_time
In s.
void nav_goto_block(uint8_t b)
#define InitStage()
float dist2_to_wp
Definition common_nav.c:39
int32_t nav_utm_east0
Definition common_nav.c:48
uint8_t nav_utm_zone0
Definition common_nav.c:50
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:46
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:44
float dist2_to_home
Definition common_nav.c:38
const uint8_t nb_waypoint
Definition common_nav.c:43
int32_t nav_utm_north0
Definition common_nav.c:49
void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt)
Move a waypoint to given UTM coordinates.
Definition common_nav.c:180
#define WaypointX(_wp)
Definition common_nav.h:45
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition common_nav.h:48
float y
Definition common_nav.h:41
float a
Definition common_nav.h:42
float x
Definition common_nav.h:40
#define WaypointY(_wp)
Definition common_nav.h:46
pprz_t v_ctl_throttle_setpoint
uint8_t v_ctl_auto_throttle_submode
Definition energy_ctrl.c:76
float v_ctl_altitude_setpoint
in meters above MSL
Definition energy_ctrl.c:88
float v_ctl_auto_throttle_cruise_throttle
uint8_t v_ctl_mode
Definition energy_ctrl.c:74
#define Min(x, y)
Definition esc_dshot.c:109
uint8_t lateral_mode
#define LATERAL_MODE_COURSE
#define LATERAL_MODE_ROLL
float v_ctl_auto_throttle_min_cruise_throttle
Definition guidance_v.c:58
float v_ctl_auto_throttle_max_cruise_throttle
Definition guidance_v.c:59
Fixed wing horizontal control.
Device independent GPS code (interface)
static float acInfoGetCourse(uint8_t ac_id)
Get vehicle course (float).
static float acInfoGetGspeed(uint8_t ac_id)
Get vehicle ground speed (float).
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:848
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition state.h:1076
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition state.h:1085
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition state.h:1560
#define V_CTL_MODE_AUTO_THROTTLE
#define V_CTL_AUTO_THROTTLE_STANDARD
#define V_CTL_MODE_AUTO_ALT
#define V_CTL_MODE_AUTO_CLIMB
static uint32_t s
uint16_t foo
Definition main_demo5.c:58
bool nav_in_segment
Definition nav.c:67
static unit_t unit
Definition nav.c:31
void nav_home(void)
Home mode navigation (circle around HOME)
Definition nav.c:424
static float nav_carrot_leg_progress
Definition nav.c:61
float baseleg_out_qdr
Definition nav.c:219
float nav_glide_pitch_trim
Definition nav.c:76
bool nav_survey_active
Definition nav.c:88
int nav_mode
Definition nav.c:90
float nav_segment_x_2
Definition nav.c:69
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition nav.c:74
float nav_survey_east
Definition nav.c:87
bool nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
Decide if the UAV is approaching the current waypoint.
Definition nav.c:325
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition nav.c:92
#define LINE_STOP_FUNCTION
Definition nav.c:279
void nav_eight(uint8_t target, uint8_t c1, float radius)
Navigation along a figure 8.
Definition nav.c:633
float nav_segment_y_2
Definition nav.c:69
float fp_throttle
Definition nav.c:312
float carrot_x
Definition nav.c:50
static void send_survey(struct transport_tx *trans, struct link_device *dev)
Definition nav.c:520
float last_y
Definition nav.c:47
float nav_circle_radius
Definition nav.c:68
uint8_t nav_oval_count
Navigation along a figure O.
Definition nav.c:754
float nav_ground_speed_pgain
Definition nav.c:83
float circle_bank
Definition nav.c:71
float carrot_y
Definition nav.c:50
float nav_climb
Definition nav.c:56
static void DownlinkSendWp(struct transport_tx *trans, struct link_device *dev, uint8_t _wp)
Definition nav.c:482
float nav_survey_north
Definition nav.c:87
float nav_radius
Definition nav.c:56
void nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
Definition nav.c:220
float nav_survey_south
Definition nav.c:87
pprz_t nav_throttle_setpoint
Definition nav.c:309
static float nav_leg_progress
Status on the current leg (percentage, 0.
Definition nav.c:60
void nav_init(void)
Navigation Initialisation.
Definition nav.c:532
float nav_altitude
Definition nav.c:307
float fp_pitch
Definition nav.c:311
float nav_circle_y
Definition nav.c:68
enum oval_status oval_status
Definition nav.c:45
static void send_circle(struct transport_tx *trans, struct link_device *dev)
Definition nav.c:504
#define LINE_START_FUNCTION
Definition nav.c:276
float desired_x
Definition nav.c:308
float nav_segment_x_1
Definition nav.c:69
static void compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
Definition nav.c:261
void DownlinkSendWpNr(uint8_t _wp)
Definition nav.c:497
bool nav_in_circle
Definition nav.c:66
static void send_nav(struct transport_tx *trans, struct link_device *dev)
Definition nav.c:477
float nav_segment_y_1
Definition nav.c:69
float nav_circle_trigo_qdr
Definition nav.c:55
float nav_circle_radians_no_rewind
Definition nav.c:54
void nav_glide_alt(float start_alt, float final_alt)
Definition nav.c:163
#define MIN_HEIGHT_CARROT
Definition nav.c:171
void nav_oval_init(void)
Definition nav.c:756
eight_status
Definition nav.c:617
@ C2
Definition nav.c:617
@ RT2
Definition nav.c:617
@ RT1
Definition nav.c:617
@ R1T
Definition nav.c:617
@ R2T
Definition nav.c:617
@ C1
Definition nav.c:617
static float nav_leg_length
length of the current leg (m)
Definition nav.c:64
float nav_shift
Definition nav.c:56
float last_x
Definition nav.c:47
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
Computes the carrot position along the desired segment.
Definition nav.c:382
void nav_parse_MOVE_WP(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
Definition nav.c:592
void nav_without_gps(void)
Failsafe navigation without position estimation.
Definition nav.c:569
float desired_y
Definition nav.c:308
static void send_segment(struct transport_tx *trans, struct link_device *dev)
Definition nav.c:512
#define FAILSAFE_HOME_RADIUS
Definition nav.c:410
float nav_pitch
Definition nav.c:310
float nav_circle_x
Definition nav.c:68
static void send_nav_ref(struct transport_tx *trans, struct link_device *dev)
Periodic telemetry.
Definition nav.c:471
float nav_survey_shift
Definition nav.c:86
float rc_pitch
Definition nav.c:49
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition nav.c:108
uint8_t horizontal_mode
Definition nav.c:70
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition nav.c:445
float fp_climb
Definition nav.c:313
void nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
Definition nav.c:241
void nav_follow(uint8_t _ac_id, float distance, float height)
Definition nav.c:302
#define NAV_GLIDE_PITCH_TRIM
Definition nav.c:78
float nav_circle_radians
Status on the current circle.
Definition nav.c:53
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Definition nav.c:762
void nav_eight_init(void)
Definition nav.c:620
void fly_to_xy(float x, float y)
Computes desired_x, desired_y and desired_course.
Definition nav.c:356
float nav_ground_speed_setpoint
Definition nav.c:83
float nav_survey_west
Definition nav.c:87
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
Definition nav.c:489
float nav_course
Definition nav.c:56
void nav_glide(uint8_t start_wp, uint8_t wp)
Definition nav.c:158
void nav_parse_BLOCK(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
Definition nav.c:585
static void nav_set_altitude(void)
Definition nav.c:413
Fixedwing Navigation library.
#define NavCircleWaypoint(wp, radius)
Definition nav.h:151
#define Square(_x)
Definition nav.h:53
#define SEND_NAVIGATION(_trans, _dev)
Definition nav.h:245
#define HORIZONTAL_MODE_CIRCLE
Definition nav.h:92
#define HORIZONTAL_MODE_ROUTE
Definition nav.h:91
oval_status
Definition nav.h:60
@ OC2
Definition nav.h:60
@ OC1
Definition nav.h:60
@ OR12
Definition nav.h:60
@ OR21
Definition nav.h:60
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
Definition nav.h:161
#define NAV_GRAVITY
Definition nav.h:52
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition nav.h:49
#define DistanceSquare(p1_x, p1_y, p2_x, p2_y)
Definition nav.h:54
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition nav.h:191
#define NAV_MODE_COURSE
Definition nav.h:86
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition nav.h:177
#define MAXIMUM_AIRSPEED
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#define TRIM_UPPRZ(pprz)
Definition paparazzi.h:14
int16_t pprz_t
Definition paparazzi.h:6
#define MAX_PPRZ
Definition paparazzi.h:8
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
float alt
in meters (normally above WGS84 reference ellipsoid)
float y
in meters
float x
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone
UTM zone number.
float lon
in radians
float lat
in radians
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
position in UTM coordinates Units: meters
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition navigation.c:326
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition navigation.h:42
#define CARROT
default approaching_time for a wp
Definition navigation.h:70
float h_ctl_roll_max_setpoint
float h_ctl_roll_setpoint
float h_ctl_course_pgain
float h_ctl_course_pre_bank
float h_ctl_course_setpoint
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
struct target_t target
Definition target_pos.c:65
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
float alpha
Definition textons.c:133
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.