39 #define RCLost() bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)
76 #ifndef NAV_GLIDE_PITCH_TRIM
77 #define NAV_GLIDE_PITCH_TRIM 0.
96 nav_circle_radians = 0;
97 nav_circle_radians_no_rewind = 0;
98 nav_in_circle =
FALSE;
99 nav_in_segment =
FALSE;
103 #define PowerVoltage() (vsupply/10.)
104 #define RcRoll(travel) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)
106 #define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))
114 nav_circle_trigo_qdr = atan2f(pos->
y - y, pos->
x - x);
115 float sign_radius = radius > 0 ? 1 : -1;
118 float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr;
119 NormRadAngle(trigo_diff);
120 nav_circle_radians += trigo_diff;
121 trigo_diff *= - sign_radius;
122 if (trigo_diff > 0) {
123 nav_circle_radians_no_rewind += trigo_diff;
128 float dist_carrot =
CARROT * NOMINAL_AIRSPEED;
132 float abs_radius = fabs(radius);
136 (dist2_center >
Square(abs_radius + dist_carrot)
137 || dist2_center <
Square(abs_radius - dist_carrot)) ?
141 float carrot_angle = dist_carrot / abs_radius;
142 carrot_angle =
Min(carrot_angle, M_PI / 4);
143 carrot_angle =
Max(carrot_angle, M_PI / 16);
144 float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
146 float radius_carrot = abs_radius;
148 radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius);
150 fly_to_xy(x + cosf(alpha_carrot)*radius_carrot,
151 y + sinf(alpha_carrot)*radius_carrot);
152 nav_in_circle =
TRUE;
155 nav_circle_radius =
radius;
159 #define NavGlide(_last_wp, _wp) { \
160 float start_alt = waypoints[_last_wp].a; \
161 float diff_alt = waypoints[_wp].a - start_alt; \
162 float alt = start_alt + nav_leg_progress * diff_alt; \
163 float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length; \
164 NavVerticalAltitudeMode(alt, pre_climb); \
170 #define MAX_DIST_CARROT 250.
171 #define MIN_HEIGHT_CARROT 50.
172 #define MAX_HEIGHT_CARROT 150.
174 #define Goto3D(radius) { \
175 if (pprz_mode == PPRZ_MODE_AUTO2) { \
176 int16_t yaw = fbw_state->channels[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); \
182 int16_t pitch = fbw_state->channels[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); \
188 v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
189 int16_t roll = fbw_state->channels[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); \
196 nav_circle_XY(carrot_x, carrot_y, radius); \
200 #define NavFollow(_ac_id, _distance, _height) \
201 nav_follow(_ac_id, _distance, _height);
204 static unit_t
unit __attribute__((unused));
208 #ifdef NAV_GROUND_SPEED_PGAIN
211 static void nav_ground_speed_loop(
void)
213 if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint
214 && nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) {
235 float d = sqrtf(x_0 * x_0 + y_0 * y_0);
242 baseleg_out_qdr = M_PI - atan2f(-y_1, -x_1);
243 if (nav_radius < 0) {
244 baseleg_out_qdr += M_PI;
258 float d = sqrtf(x_0 * x_0 + y_0 * y_0);
278 float td_af = sqrtf(td_af_x * td_af_x + td_af_y * td_af_y);
280 wind->
x * wind->
x + wind->
y * wind->
y));
288 #include "generated/flight_plan.h"
291 #ifndef LINE_START_FUNCTION
292 #define LINE_START_FUNCTION {}
294 #ifndef LINE_STOP_FUNCTION
295 #define LINE_STOP_FUNCTION {}
305 float alpha = M_PI / 2 - ac->
course;
306 float ca = cosf(alpha), sa = sinf(alpha);
307 float x = ac->
east - _distance * ca;
308 float y = ac->
north - _distance * sa;
310 #ifdef NAV_FOLLOW_PGAIN
312 nav_ground_speed_setpoint = ac->
gspeed + NAV_FOLLOW_PGAIN * s;
313 nav_ground_speed_loop();
342 if (approaching_time < 0.) {
344 float leg_x = x - from_x;
345 float leg_y = y - from_y;
346 float leg = sqrtf(
Max(leg_x * leg_x + leg_y * leg_y, 1.));
348 float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
349 return (scal_prod < exceed_dist);
357 float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
358 return (scal_prod < 0.);
381 BoundAbs(diff, M_PI / 2.);
382 float s = sinf(diff);
393 void nav_route_xy(
float last_wp_x,
float last_wp_y,
float wp_x,
float wp_y)
395 float leg_x = wp_x - last_wp_x;
396 float leg_y = wp_y - last_wp_y;
397 float leg2 =
Max(leg_x * leg_x + leg_y * leg_y, 1.);
400 nav_leg_length = sqrtf(leg2);
403 float carrot =
CARROT * NOMINAL_AIRSPEED;
405 nav_carrot_leg_progress = nav_leg_progress +
Max(carrot / nav_leg_length, 0.);
406 nav_in_segment =
TRUE;
407 nav_segment_x_1 = last_wp_x;
408 nav_segment_y_1 = last_wp_y;
409 nav_segment_x_2 = wp_x;
410 nav_segment_y_2 = wp_y;
413 fly_to_xy(last_wp_x + nav_carrot_leg_progress * leg_x + nav_shift * leg_y / nav_leg_length,
414 last_wp_y + nav_carrot_leg_progress * leg_y - nav_shift * leg_x / nav_leg_length);
419 #ifndef FAILSAFE_HOME_RADIUS
420 #define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS
425 static float last_nav_altitude;
426 if (fabs(nav_altitude - last_nav_altitude) > 1.) {
452 nav_survey_active =
FALSE;
473 #if PERIODIC_TELEMETRY
478 pprz_msg_send_NAVIGATION_REF(trans, dev, AC_ID,
484 SEND_NAVIGATION(trans, dev);
492 DownlinkSendWp(trans, dev, i);
495 bool_t DownlinkSendWpNr(
uint8_t _wp)
505 pprz_msg_send_CIRCLE(trans, dev, AC_ID,
506 &nav_circle_x, &nav_circle_y, &nav_circle_radius);
512 if (nav_in_segment) {
513 pprz_msg_send_SEGMENT(trans, dev, AC_ID,
514 &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2);
520 if (nav_survey_active) {
521 pprz_msg_send_SURVEY(trans, dev, AC_ID,
522 &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south);
544 #ifdef NAV_GROUND_SPEED_PGAIN
545 nav_ground_speed_pgain = ABS(NAV_GROUND_SPEED_PGAIN);
546 nav_ground_speed_setpoint = NOMINAL_AIRSPEED;
549 #if PERIODIC_TELEMETRY
570 #ifdef SECTION_FAILSAFE
572 nav_pitch = FAILSAFE_DEFAULT_PITCH;
577 nav_throttle_setpoint =
TRIM_UPPRZ((V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE) * MAX_PPRZ);
603 float aradius = fabs(radius);
609 float d = sqrtf(target_c1_x * target_c1_x + target_c1_y * target_c1_y);
613 float u_x = target_c1_x / d;
614 float u_y = target_c1_y / d;
617 if (d > 2 * aradius) {
630 struct point c1_in = {
635 struct point c1_out = {
641 struct point c2_in = {
642 c2.
x + radius * -u_y,
646 struct point c2_out = {
647 c2.
x - radius * -u_y,
652 float qdr_out = M_PI - atan2f(u_y, u_x);
657 switch (eight_status) {
739 float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
742 float u_x = p2_p1_x / d;
743 float u_y = p2_p1_y / d;
764 float qdr_out_2 = M_PI - atan2f(u_y, u_x);
765 float qdr_out_1 = qdr_out_2 + M_PI;
770 float qdr_anticipation = (radius > 0 ? -15 : 15);
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Communication between fbw and ap processes.
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
#define MIN_HEIGHT_CARROT
float v_ctl_altitude_setpoint
in meters above MSL
Generic transmission transport header.
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
void nav_eight(uint8_t target, uint8_t c1, float radius)
Navigation along a figure 8.
Periodic telemetry system header (includes downlink utility and generated code).
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
static bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
float h_ctl_course_setpoint
vector in East North Up coordinates Units: meters
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
float dist2_to_home
squared distance to home waypoint
uint8_t nav_oval_count
Navigation along a figure O.
void nav_home(void)
Home mode navigation (circle around HOME)
#define V_CTL_MODE_AUTO_THROTTLE
#define HORIZONTAL_MODE_CIRCLE
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
struct ac_info_ * get_ac_info(uint8_t id)
static uint8_t last_wp
Index of last waypoint.
#define FAILSAFE_HOME_RADIUS
float v_ctl_auto_throttle_max_cruise_throttle
float nav_circle_radians
Status on the current circle.
#define NavCircleWaypoint(wp, radius)
Fixed wing horizontal control.
#define V_CTL_AUTO_THROTTLE_STANDARD
static float nav_leg_progress
Status on the current leg (percentage, 0.
#define DistanceSquare(p1_x, p1_y, p2_x, p2_y)
#define V_CTL_MODE_AUTO_ALT
static float baseleg_out_qdr
static void nav_set_altitude(void)
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command...
Device independent GPS code (interface)
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
#define DefaultPeriodic
Set default periodic telemetry.
float nav_glide_pitch_trim
float nav_ground_speed_setpoint
#define HORIZONTAL_MODE_ROUTE
uint8_t v_ctl_auto_throttle_submode
float v_ctl_auto_throttle_min_cruise_throttle
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
#define V_CTL_MODE_AUTO_CLIMB
void nav_oval(uint8_t p1, uint8_t p2, float radius)
void nav_init(void)
Periodic telemetry.
const uint8_t nb_waypoint
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
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.
float h_ctl_roll_max_setpoint
static bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
static const struct usb_device_descriptor dev
#define DefaultChannel
SITL.
static bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
float nav_circle_radians_no_rewind
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
float h_ctl_course_pre_bank
float v_ctl_auto_throttle_cruise_throttle
float h_ctl_roll_setpoint
enum oval_status oval_status
void nav_eight_init(void)
float nav_pitch
with INT32_ANGLE_FRAC
void nav_without_gps(void)
Failsafe navigation without position estimation.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
void fly_to_xy(float x, float y)
Computes desired_x, desired_y and desired_course.
static float nav_leg_length
length of the current leg (m)
bool_t 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.
float nav_circle_trigo_qdr
float nav_ground_speed_pgain
#define CARROT
default approaching_time for a wp
#define LATERAL_MODE_ROLL
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
static void send_survey(struct transport_tx *trans, struct link_device *dev)
#define LATERAL_MODE_COURSE
Fixedwing Navigation library.
static void nav_follow(uint8_t _ac_id, float _distance, float _height)
#define NAV_GLIDE_PITCH_TRIM
float dist2_to_wp
squared distance to next waypoint
static float nav_carrot_leg_progress
pprz_t nav_throttle_setpoint
#define LINE_START_FUNCTION
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Information relative to the other aircrafts.
#define LINE_STOP_FUNCTION
Fixedwing autopilot modes.