29 #include "generated/flight_plan.h"
32 #ifndef NAV_HYBRID_MAX_BANK
33 #define NAV_HYBRID_MAX_BANK 0.52f
37 #ifndef NAV_HYBRID_MAX_AIRSPEED
38 #define NAV_HYBRID_MAX_AIRSPEED 15.0f
41 #ifndef NAV_HYBRID_SPEED_MARGIN
42 #define NAV_HYBRID_SPEED_MARGIN 10.0f
45 #define NAV_MAX_SPEED (NAV_HYBRID_MAX_AIRSPEED + NAV_HYBRID_SPEED_MARGIN)
48 #ifndef NAV_HYBRID_GOTO_MAX_SPEED
49 #define NAV_HYBRID_GOTO_MAX_SPEED NAV_MAX_SPEED
54 #ifndef NAV_HYBRID_MAX_DECELERATION
55 #define NAV_HYBRID_MAX_DECELERATION 1.0
60 #ifdef NAV_HYBRID_LINE_GAIN
66 #ifndef NAV_HYBRID_NAV_LINE_DIST
67 #define NAV_HYBRID_NAV_LINE_DIST 50.f
70 #ifndef NAV_HYBRID_NAV_CIRCLE_DIST
71 #define NAV_HYBRID_NAV_CIRCLE_DIST 40.f
74 #ifdef NAV_HYBRID_POS_GAIN
80 #ifndef GUIDANCE_INDI_HYBRID
113 float max_speed_decel = sqrtf(max_speed_decel2);
146 float length_normalv = length_line;
180 if (approaching_time > 0.f) {
184 VECT2_SMUL(estimated_progress, *speed, approaching_time);
185 VECT2_SUM(estimated_pos, *pos, estimated_progress);
206 return (diff.
x * from_diff.
x + diff.
y * from_diff.
y < 0.f);
219 float sign_radius = radius > 0.f ? 1.f : -1.f;
221 float abs_radius = fabsf(radius);
223 if (abs_radius > 0.1f) {
230 NormRadAngle(trigo_diff);
234 Bound(progress_angle, M_PI / 16.f, M_PI / 4.f);
void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound)
static float float_vect2_norm(struct FloatVect2 *v)
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
#define VECT2_ADD(_a, _b)
#define VECT2_SMUL(_vo, _vi, _s)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_COPY(_a, _b)
#define VECT2_SUM(_c, _a, _b)
#define VECT2_ASSIGN(_a, _x, _y)
#define PPRZ_ISA_GRAVITY
earth-surface gravitational acceleration in m/s^2
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
struct FloatVect3 speed_sp
float leg_progress
progress over leg
struct EnuCoor_f center
center WP position
float leg_length
leg length
struct EnuCoor_f to
end WP position
float dist2_to_wp
squared distance to next waypoint
struct NavCircle_t circle
struct EnuCoor_f from
start WP position
float radians
incremental angular distance
float radius
radius in meters
struct NavBase_t nav_rotorcraft_base
Basic Nav struct.
float nav_max_deceleration_sp
bool force_forward
forward flight for hybrid nav
static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
static void nav_hybrid_goto(struct EnuCoor_f *wp)
Implement basic nav function for the hybrid case.
#define NAV_HYBRID_MAX_BANK
#define NAV_HYBRID_NAV_LINE_DIST
#define NAV_HYBRID_NAV_CIRCLE_DIST
static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
#define NAV_HYBRID_GOTO_MAX_SPEED
float nav_hybrid_line_gain
float nav_hybrid_pos_gain
static bool nav_hybrid_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
void nav_rotorcraft_hybrid_init(void)
Init and register nav functions.
#define NAV_HYBRID_MAX_DECELERATION
Specific navigation functions for hybrid aircraft.
struct FloatVect2 pos_diff
vector in East North Up coordinates Units: meters
Paparazzi atmospheric pressure conversion utilities.
struct RotorcraftNavigation nav
void nav_register_circle(navigation_circle nav_circle)
void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
Registering functions.
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
Rotorcraft navigation functions.
struct EnuCoor_f speed
speed setpoint (in m/s)
#define NAV_HORIZONTAL_MODE_WAYPOINT
Nav modes these modes correspond to the flight plan instructions used to set the high level navigatio...
#define NAV_HORIZONTAL_MODE_CIRCLE
#define NAV_HORIZONTAL_MODE_ROUTE
#define NAV_SETPOINT_MODE_SPEED
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
struct EnuCoor_f target
final target position (in meters)
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived