 |
Paparazzi UAS
v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Go to the documentation of this file.
31 static unit_t
unit __attribute__((unused));
39 #include "generated/flight_plan.h"
41 #if !USE_GENERATED_AUTOPILOT
80 #ifndef NAV_GLIDE_PITCH_TRIM
81 #define NAV_GLIDE_PITCH_TRIM 0.
107 #define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))
116 float sign_radius = radius > 0 ? 1 : -1;
120 NormRadAngle(trigo_diff);
122 trigo_diff *= - sign_radius;
123 if (trigo_diff > 0) {
129 float dist_carrot =
CARROT * NOMINAL_AIRSPEED;
133 float abs_radius = fabs(radius);
137 (dist2_center >
Square(abs_radius + dist_carrot)
138 || dist2_center <
Square(abs_radius - dist_carrot)) ?
142 float carrot_angle = dist_carrot / abs_radius;
143 carrot_angle =
Min(carrot_angle, M_PI / 4);
144 carrot_angle = Max(carrot_angle, M_PI / 16);
147 float radius_carrot = abs_radius;
149 radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius);
151 fly_to_xy(
x + cosf(alpha_carrot)*radius_carrot,
152 y + sinf(alpha_carrot)*radius_carrot);
171 #define MAX_DIST_CARROT 250.
172 #define MIN_HEIGHT_CARROT 50.
173 #define MAX_HEIGHT_CARROT 150.
175 #define Goto3D(radius) { \
176 if (autopilot_get_mode() == AP_MODE_AUTO2) { \
177 int16_t yaw = radio_control_get(RADIO_YAW); \
178 if (yaw > MIN_DX || yaw < -MIN_DX) { \
179 carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \
180 carrot_x = Min(carrot_x, MAX_DIST_CARROT); \
181 carrot_x = Max(carrot_x, -MAX_DIST_CARROT); \
183 int16_t pitch = radio_control_get(RADIO_PITCH); \
184 if (pitch > MIN_DX || pitch < -MIN_DX) { \
185 carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \
186 carrot_y = Min(carrot_y, MAX_DIST_CARROT); \
187 carrot_y = Max(carrot_y, -MAX_DIST_CARROT); \
189 v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
190 int16_t roll = radio_control_get(RADIO_ROLL); \
191 if (roll > MIN_DX || roll < -MIN_DX) { \
192 nav_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \
193 nav_altitude = Max(nav_altitude, MIN_HEIGHT_CARROT+ground_alt); \
194 nav_altitude = Min(nav_altitude, MAX_HEIGHT_CARROT+ground_alt); \
197 nav_circle_XY(carrot_x, carrot_y, radius); \
202 #ifdef NAV_GROUND_SPEED_PGAIN
205 static void nav_ground_speed_loop(
void)
229 float d = sqrtf(x_0 * x_0 + y_0 * y_0);
250 float d = sqrtf(x_0 * x_0 + y_0 * y_0);
267 float td_af = sqrtf(td_af_x * td_af_x + td_af_y * td_af_y);
269 wind->
x * wind->
x + wind->
y * wind->
y));
276 #ifndef LINE_START_FUNCTION
277 #define LINE_START_FUNCTION {}
279 #ifndef LINE_STOP_FUNCTION
280 #define LINE_STOP_FUNCTION {}
293 float x = ac->
x - distance * ca;
294 float y = ac->
y - distance * sa;
296 #ifdef NAV_FOLLOW_PGAIN
299 nav_ground_speed_loop();
304 float __attribute__((unused)) height) {}
305 #endif // TRAFFIC_INFO
333 if (approaching_time < 0.) {
335 float leg_x =
x - from_x;
336 float leg_y =
y - from_y;
337 float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));
339 float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
340 return (scal_prod < exceed_dist);
348 float scal_prod = (
x - from_x) * pw_x + (
y - from_y) * pw_y;
349 return (scal_prod < 0.);
371 BoundAbs(diff, M_PI / 2.);
372 float s = sinf(diff);
383 void nav_route_xy(
float last_wp_x,
float last_wp_y,
float wp_x,
float wp_y)
385 float leg_x = wp_x - last_wp_x;
386 float leg_y = wp_y - last_wp_y;
387 float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.);
393 float carrot =
CARROT * NOMINAL_AIRSPEED;
410 #ifndef FAILSAFE_HOME_RADIUS
411 #define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS
416 static float last_nav_altitude;
469 #if PERIODIC_TELEMETRY
474 pprz_msg_send_NAVIGATION_REF(trans,
dev, AC_ID,
478 static void send_nav(
struct transport_tx *trans,
struct link_device *
dev)
508 pprz_msg_send_CIRCLE(trans,
dev, AC_ID,
516 pprz_msg_send_SEGMENT(trans,
dev, AC_ID,
524 pprz_msg_send_SURVEY(trans,
dev, AC_ID,
547 #ifdef NAV_GROUND_SPEED_PGAIN
552 #if PERIODIC_TELEMETRY
576 #ifdef SECTION_FAILSAFE
589 if (DL_BLOCK_ac_id(buf) != AC_ID) {
return; }
596 if (DL_MOVE_WP_ac_id(buf) != AC_ID) {
return; }
597 uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
601 lla.
lat = RadOfDeg((
float)(DL_MOVE_WP_lat(buf) / 1e7f));
602 lla.
lon = RadOfDeg((
float)(DL_MOVE_WP_lon(buf) / 1e7f));
603 lla.
alt = (float)(DL_MOVE_WP_alt(buf) / 1000.f);
637 float aradius = fabs(radius);
643 float d = sqrtf(target_c1_x * target_c1_x + target_c1_y * target_c1_y);
647 float u_x = target_c1_x / d;
648 float u_y = target_c1_y / d;
651 if (d > 2 * aradius) {
664 struct point c1_in = {
669 struct point c1_out = {
675 struct point c2_in = {
676 c2.x + radius * -u_y,
680 struct point c2_out = {
681 c2.x - radius * -u_y,
686 float qdr_out = M_PI - atan2f(u_y, u_x);
773 float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
776 float u_x = p2_p1_x / d;
777 float u_y = p2_p1_y / d;
798 float qdr_out_2 = M_PI - atan2f(u_y, u_x);
799 float qdr_out_1 = qdr_out_2 + M_PI;
804 float qdr_anticipation = (radius > 0 ? -15 : 15);
static void send_circle(struct transport_tx *trans, struct link_device *dev)
void nav_follow(uint8_t _ac_id, float distance, float height)
#define LINE_START_FUNCTION
static float acInfoGetGspeed(uint8_t ac_id)
Get vehicle ground speed (float).
float nav_circle_trigo_qdr
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
#define V_CTL_MODE_AUTO_ALT
enum oval_status oval_status
static float nav_leg_progress
Status on the current leg (percentage, 0.
static void nav_set_altitude(void)
float nav_ground_speed_pgain
static void send_nav_ref(struct transport_tx *trans, struct link_device *dev)
Periodic telemetry.
float dist2_to_home
squared distance to home waypoint
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
float v_ctl_auto_throttle_max_cruise_throttle
#define LATERAL_MODE_ROLL
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
static float acInfoGetCourse(uint8_t ac_id)
Get vehicle course (float).
float h_ctl_roll_max_setpoint
#define NAV_GLIDE_PITCH_TRIM
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
float nav_circle_radians
Status on the current circle.
void nav_init(void)
Navigation Initialisation.
void nav_home(void)
Home mode navigation (circle around HOME)
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
float v_ctl_auto_throttle_min_cruise_throttle
void nav_glide(uint8_t start_wp, uint8_t wp)
float nav_ground_speed_setpoint
float h_ctl_roll_setpoint
float v_ctl_auto_throttle_cruise_throttle
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
#define NavCircleWaypoint(wp, radius)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
#define CARROT
default approaching_time for a wp
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
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.
Device independent GPS code (interface)
struct pprz_autopilot autopilot
Global autopilot structure.
static float nav_leg_length
length of the current leg (m)
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
const uint8_t nb_waypoint
uint8_t zone
UTM zone number.
void nav_eight(uint8_t target, uint8_t c1, float radius)
Navigation along a figure 8.
uint8_t last_wp
Index of last waypoint.
void nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
static const struct usb_device_descriptor dev
void DownlinkSendWpNr(uint8_t _wp)
#define HORIZONTAL_MODE_CIRCLE
void nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
vector in East North Up coordinates Units: meters
#define FAILSAFE_HOME_RADIUS
void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt)
Move a waypoint to given UTM coordinates.
void nav_oval(uint8_t p1, uint8_t p2, float radius)
#define V_CTL_AUTO_THROTTLE_STANDARD
void nav_parse_MOVE_WP(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
float nav_glide_pitch_trim
float alt
in meters (normally above WGS84 reference ellipsoid)
static void DownlinkSendWp(struct transport_tx *trans, struct link_device *dev, uint8_t _wp)
#define V_CTL_MODE_AUTO_THROTTLE
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
float h_ctl_course_pre_bank
void nav_parse_BLOCK(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
pprz_t v_ctl_throttle_setpoint
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
#define DistanceSquare(p1_x, p1_y, p2_x, p2_y)
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
float nav_circle_radians_no_rewind
#define SEND_NAVIGATION(_trans, _dev)
float v_ctl_altitude_setpoint
in meters above MSL
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.
static void send_nav(struct transport_tx *trans, struct link_device *dev)
#define MIN_HEIGHT_CARROT
position in UTM coordinates Units: meters
#define LATERAL_MODE_COURSE
static void send_survey(struct transport_tx *trans, struct link_device *dev)
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
#define LINE_STOP_FUNCTION
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
static float nav_carrot_leg_progress
pprz_t nav_throttle_setpoint
#define HORIZONTAL_MODE_ROUTE
static void compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
#define V_CTL_MODE_AUTO_CLIMB
uint8_t v_ctl_auto_throttle_submode
uint8_t nav_oval_count
Navigation along a figure O.
float dist2_to_wp
squared distance to next waypoint
#define DefaultPeriodic
Set default periodic telemetry.
bool launch
request launch
void nav_goto_block(uint8_t b)
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
void nav_eight_init(void)
void nav_without_gps(void)
Failsafe navigation without position estimation.
static void send_segment(struct transport_tx *trans, struct link_device *dev)
vector in Latitude, Longitude and Altitude
void fly_to_xy(float x, float y)
Computes desired_x, desired_y and desired_course.
float h_ctl_course_setpoint