|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Specific navigation functions for hybrid aircraft. More...
#include "modules/nav/nav_rotorcraft_hybrid.h"#include "firmwares/rotorcraft/navigation.h"#include "math/pprz_isa.h"#include "generated/flight_plan.h"Go to the source code of this file.
Macros | |
| #define | NAV_HYBRID_MAX_AIRSPEED 15.0f |
| #define | NAV_HYBRID_SPEED_MARGIN 10.0f |
| #define | NAV_MAX_SPEED (NAV_HYBRID_MAX_AIRSPEED + NAV_HYBRID_SPEED_MARGIN) |
| #define | NAV_HYBRID_GOTO_MAX_SPEED NAV_MAX_SPEED |
| #define | NAV_HYBRID_MAX_DECELERATION 1.0 |
| #define | NAV_HYBRID_MAX_ACCELERATION 4.0 |
| #define | NAV_HYBRID_SOFT_ACCELERATION NAV_HYBRID_MAX_ACCELERATION |
| #define | NAV_HYBRID_MAX_EXPECTED_WIND 5.0f |
| #define | NAV_HYBRID_NAV_LINE_DIST 50.f |
| #define | NAV_HYBRID_NAV_CIRCLE_DIST 40.f |
| #define | NAV_HYBRID_EXT_VISION_SETPOINT_MODE FALSE |
| #define | NAV_HYBRID_LIMIT_CIRCLE_RADIUS FALSE |
Functions | |
| static float | max_speed_for_deceleration (float dist_to_wp) |
| Calculate max speed when decelerating at MAX capacity a_max distance travelled d = 1/2 a_max t^2 The time in which it does this is: T = V / a_max The maximum speed at which to fly to still allow arriving with zero speed at the waypoint given maximum deceleration is: V = sqrt(2 * a_max * d). | |
| static void | nav_hybrid_goto (struct EnuCoor_f *wp) |
| Implement basic nav function for the hybrid case. | |
| static void | nav_hybrid_route (struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end) |
| static bool | nav_hybrid_approaching (struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time) |
| static void | nav_hybrid_circle (struct EnuCoor_f *wp_center, float radius) |
| void | nav_rotorcraft_hybrid_init (void) |
| Init and register nav functions. | |
Variables | |
| float | nav_hybrid_max_bank = 0.52f |
| float | nav_max_speed = NAV_MAX_SPEED |
| float | nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION |
| float | nav_max_acceleration_sp = NAV_HYBRID_MAX_ACCELERATION |
| float | nav_hybrid_soft_acceleration = NAV_HYBRID_SOFT_ACCELERATION |
| float | nav_hybrid_max_acceleration = NAV_HYBRID_MAX_ACCELERATION |
| float | nav_hybrid_max_expected_wind = NAV_HYBRID_MAX_EXPECTED_WIND |
| float | nav_hybrid_line_gain = 1.0f |
| float | nav_hybrid_pos_gain = 1.0f |
| bool | force_forward = 0.0f |
| forward flight for hybrid nav | |
Specific navigation functions for hybrid aircraft.
Definition in file nav_rotorcraft_hybrid.c.
| #define NAV_HYBRID_EXT_VISION_SETPOINT_MODE FALSE |
Definition at line 108 of file nav_rotorcraft_hybrid.c.
| #define NAV_HYBRID_GOTO_MAX_SPEED NAV_MAX_SPEED |
Definition at line 51 of file nav_rotorcraft_hybrid.c.
| #define NAV_HYBRID_LIMIT_CIRCLE_RADIUS FALSE |
Definition at line 113 of file nav_rotorcraft_hybrid.c.
| #define NAV_HYBRID_MAX_ACCELERATION 4.0 |
Definition at line 59 of file nav_rotorcraft_hybrid.c.
| #define NAV_HYBRID_MAX_AIRSPEED 15.0f |
Definition at line 40 of file nav_rotorcraft_hybrid.c.
| #define NAV_HYBRID_MAX_DECELERATION 1.0 |
Definition at line 55 of file nav_rotorcraft_hybrid.c.
| #define NAV_HYBRID_MAX_EXPECTED_WIND 5.0f |
Definition at line 72 of file nav_rotorcraft_hybrid.c.
| #define NAV_HYBRID_NAV_CIRCLE_DIST 40.f |
Definition at line 87 of file nav_rotorcraft_hybrid.c.
Referenced by nav_hybrid_circle().
| #define NAV_HYBRID_NAV_LINE_DIST 50.f |
Definition at line 83 of file nav_rotorcraft_hybrid.c.
Referenced by nav_hybrid_route().
| #define NAV_HYBRID_SOFT_ACCELERATION NAV_HYBRID_MAX_ACCELERATION |
Definition at line 63 of file nav_rotorcraft_hybrid.c.
| #define NAV_HYBRID_SPEED_MARGIN 10.0f |
Definition at line 44 of file nav_rotorcraft_hybrid.c.
| #define NAV_MAX_SPEED (NAV_HYBRID_MAX_AIRSPEED + NAV_HYBRID_SPEED_MARGIN) |
Definition at line 47 of file nav_rotorcraft_hybrid.c.
|
static |
Calculate max speed when decelerating at MAX capacity a_max distance travelled d = 1/2 a_max t^2 The time in which it does this is: T = V / a_max The maximum speed at which to fly to still allow arriving with zero speed at the waypoint given maximum deceleration is: V = sqrt(2 * a_max * d).
Definition at line 214 of file nav_rotorcraft_hybrid.c.
References Min, nav_max_deceleration_sp, and nav_max_speed.
Referenced by nav_hybrid_goto(), and nav_hybrid_route().
|
static |
Definition at line 222 of file nav_rotorcraft_hybrid.c.
References ARRIVED_AT_WAYPOINT, float_vect2_norm(), stateGetPositionEnu_f(), stateGetSpeedEnu_f(), VECT2_DIFF, VECT2_SMUL, VECT2_SUM, FloatVect2::x, and FloatVect2::y.
Referenced by nav_rotorcraft_hybrid_init().
|
static |
Definition at line 261 of file nav_rotorcraft_hybrid.c.
References alpha, float_vect2_norm(), float_vect2_normalize(), force_forward, nav, NAV_HORIZONTAL_MODE_CIRCLE, nav_hybrid_max_acceleration, nav_hybrid_max_bank, nav_hybrid_max_expected_wind, NAV_HYBRID_NAV_CIRCLE_DIST, nav_hybrid_pos_gain, nav_hybrid_soft_acceleration, nav_max_acceleration_sp, nav_max_speed, nav_rotorcraft_base, NAV_SETPOINT_MODE_SPEED, pos_diff, PPRZ_ISA_GRAVITY, stateGetPositionEnu_f(), VECT2_COPY, VECT2_DIFF, VECT2_SMUL, EnuCoor_f::x, and EnuCoor_f::y.
Referenced by nav_rotorcraft_hybrid_init().
|
static |
Implement basic nav function for the hybrid case.
Definition at line 121 of file nav_rotorcraft_hybrid.c.
References float_vect2_bound_in_2d(), float_vect2_norm(), force_forward, get_dist2_to_point(), max_speed_for_deceleration(), nav, NAV_HORIZONTAL_MODE_WAYPOINT, nav_hybrid_pos_gain, nav_hybrid_soft_acceleration, nav_max_acceleration_sp, nav_max_speed, nav_rotorcraft_base, NAV_SETPOINT_MODE_POS, NAV_SETPOINT_MODE_SPEED, speed_sp, stateGetPositionEnu_f(), VECT2_COPY, VECT2_DIFF, and VECT2_SMUL.
Referenced by nav_rotorcraft_hybrid_init().
Definition at line 157 of file nav_rotorcraft_hybrid.c.
References direction, float_vect2_norm(), force_forward, get_dist2_to_point(), max_speed_for_deceleration(), nav, NAV_HORIZONTAL_MODE_ROUTE, nav_hybrid_line_gain, NAV_HYBRID_NAV_LINE_DIST, nav_hybrid_pos_gain, nav_max_speed, nav_rotorcraft_base, NAV_SETPOINT_MODE_SPEED, pos_diff, stateGetPositionEnu_f(), VECT2_ADD, VECT2_ASSIGN, VECT2_DIFF, VECT2_SMUL, VECT2_SUM, FloatVect2::x, and FloatVect2::y.
Referenced by nav_rotorcraft_hybrid_init().
| void nav_rotorcraft_hybrid_init | ( | void | ) |
Init and register nav functions.
For hybrid vehicle nav Init should be called after the normal rotorcraft nav_init as we are reusing some of the functions and overwritting others
Definition at line 330 of file nav_rotorcraft_hybrid.c.
References DEFAULT_CIRCLE_RADIUS, nav_hybrid_approaching(), nav_hybrid_circle(), nav_hybrid_goto(), nav_hybrid_route(), nav_register_circle(), nav_register_goto_wp(), and nav_rotorcraft_base.
| bool force_forward = 0.0f |
forward flight for hybrid nav
Definition at line 103 of file nav_rotorcraft_hybrid.c.
| float nav_hybrid_line_gain = 1.0f |
Definition at line 79 of file nav_rotorcraft_hybrid.c.
Referenced by nav_hybrid_route().
| float nav_hybrid_max_acceleration = NAV_HYBRID_MAX_ACCELERATION |
Definition at line 69 of file nav_rotorcraft_hybrid.c.
Referenced by nav_hybrid_circle().
| float nav_hybrid_max_bank = 0.52f |
Definition at line 33 of file nav_rotorcraft_hybrid.c.
Referenced by init_controller_gains(), and nav_hybrid_circle().
| float nav_hybrid_max_expected_wind = NAV_HYBRID_MAX_EXPECTED_WIND |
Definition at line 74 of file nav_rotorcraft_hybrid.c.
Referenced by nav_hybrid_circle().
| float nav_hybrid_pos_gain = 1.0f |
Definition at line 95 of file nav_rotorcraft_hybrid.c.
Referenced by init_controller_gains(), nav_hybrid_circle(), nav_hybrid_goto(), and nav_hybrid_route().
| float nav_hybrid_soft_acceleration = NAV_HYBRID_SOFT_ACCELERATION |
Definition at line 68 of file nav_rotorcraft_hybrid.c.
Referenced by nav_hybrid_circle(), and nav_hybrid_goto().
| float nav_max_acceleration_sp = NAV_HYBRID_MAX_ACCELERATION |
Definition at line 67 of file nav_rotorcraft_hybrid.c.
Referenced by init_controller_gains(), nav_hybrid_circle(), and nav_hybrid_goto().
| float nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION |
Definition at line 66 of file nav_rotorcraft_hybrid.c.
Referenced by max_speed_for_deceleration(), and rotwing_state_periodic().
| float nav_max_speed = NAV_MAX_SPEED |
Definition at line 48 of file nav_rotorcraft_hybrid.c.
Referenced by init_controller_gains(), max_speed_for_deceleration(), nav_hybrid_circle(), nav_hybrid_goto(), and nav_hybrid_route().