39#include "generated/flight_plan.h"
41#if !USE_GENERATED_AUTOPILOT
77#ifndef NAV_GLIDE_PITCH_TRIM
78#define NAV_GLIDE_PITCH_TRIM 0.
104#define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))
170#define MAX_DIST_CARROT 250.
171#define MIN_HEIGHT_CARROT 50.
172#define MAX_HEIGHT_CARROT 150.
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); \
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); \
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); \
196 nav_circle_XY(carrot_x, carrot_y, radius); \
201#ifdef NAV_GROUND_SPEED_PGAIN
268 wind->
x * wind->
x + wind->
y * wind->
y));
275#ifndef LINE_START_FUNCTION
276#define LINE_START_FUNCTION {}
278#ifndef LINE_STOP_FUNCTION
279#define LINE_STOP_FUNCTION {}
292 float x =
ac->x - distance *
ca;
293 float y =
ac->y - distance *
sa;
295#ifdef NAV_FOLLOW_PGAIN
409#ifndef FAILSAFE_HOME_RADIUS
410#define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS
468#if PERIODIC_TELEMETRY
545#ifdef NAV_GROUND_SPEED_PGAIN
550#if PERIODIC_TELEMETRY
574#ifdef SECTION_FAILSAFE
struct pprz_autopilot autopilot
Global autopilot structure.
Core autopilot interface common to all firmwares.
bool launch
request launch
void common_flight_plan_init(void)
void nav_goto_block(uint8_t b)
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
const uint8_t nb_waypoint
void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt)
Move a waypoint to given UTM coordinates.
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
pprz_t v_ctl_throttle_setpoint
uint8_t v_ctl_auto_throttle_submode
float v_ctl_altitude_setpoint
in meters above MSL
float v_ctl_auto_throttle_cruise_throttle
#define LATERAL_MODE_COURSE
#define LATERAL_MODE_ROLL
float v_ctl_auto_throttle_min_cruise_throttle
float v_ctl_auto_throttle_max_cruise_throttle
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).
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
#define V_CTL_MODE_AUTO_THROTTLE
#define V_CTL_AUTO_THROTTLE_STANDARD
#define V_CTL_MODE_AUTO_ALT
#define V_CTL_MODE_AUTO_CLIMB
void nav_home(void)
Home mode navigation (circle around HOME)
static float nav_carrot_leg_progress
float nav_glide_pitch_trim
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
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.
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
#define LINE_STOP_FUNCTION
void nav_eight(uint8_t target, uint8_t c1, float radius)
Navigation along a figure 8.
static void send_survey(struct transport_tx *trans, struct link_device *dev)
uint8_t nav_oval_count
Navigation along a figure O.
float nav_ground_speed_pgain
static void DownlinkSendWp(struct transport_tx *trans, struct link_device *dev, uint8_t _wp)
void nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
pprz_t nav_throttle_setpoint
static float nav_leg_progress
Status on the current leg (percentage, 0.
void nav_init(void)
Navigation Initialisation.
enum oval_status oval_status
static void send_circle(struct transport_tx *trans, struct link_device *dev)
#define LINE_START_FUNCTION
static void compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
void DownlinkSendWpNr(uint8_t _wp)
static void send_nav(struct transport_tx *trans, struct link_device *dev)
float nav_circle_trigo_qdr
float nav_circle_radians_no_rewind
void nav_glide_alt(float start_alt, float final_alt)
#define MIN_HEIGHT_CARROT
static float nav_leg_length
length of the current leg (m)
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.
void nav_parse_MOVE_WP(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
void nav_without_gps(void)
Failsafe navigation without position estimation.
static void send_segment(struct transport_tx *trans, struct link_device *dev)
#define FAILSAFE_HOME_RADIUS
static void send_nav_ref(struct transport_tx *trans, struct link_device *dev)
Periodic telemetry.
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
void nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
void nav_follow(uint8_t _ac_id, float distance, float height)
#define NAV_GLIDE_PITCH_TRIM
float nav_circle_radians
Status on the current circle.
void nav_oval(uint8_t p1, uint8_t p2, float radius)
void nav_eight_init(void)
void fly_to_xy(float x, float y)
Computes desired_x, desired_y and desired_course.
float nav_ground_speed_setpoint
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
void nav_glide(uint8_t start_wp, uint8_t wp)
void nav_parse_BLOCK(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
static void nav_set_altitude(void)
Fixedwing Navigation library.
#define NavCircleWaypoint(wp, radius)
#define SEND_NAVIGATION(_trans, _dev)
#define HORIZONTAL_MODE_CIRCLE
#define HORIZONTAL_MODE_ROUTE
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
#define DistanceSquare(p1_x, p1_y, p2_x, p2_y)
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
float alt
in meters (normally above WGS84 reference ellipsoid)
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone
UTM zone number.
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.
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
#define CARROT
default approaching_time for a wp
float h_ctl_roll_max_setpoint
float h_ctl_roll_setpoint
float h_ctl_course_pre_bank
float h_ctl_course_setpoint
static const struct usb_device_descriptor dev
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.