|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "modules/nav/nav_trinity.h"#include "firmwares/fixedwing/nav.h"#include "state.h"#include "autopilot.h"#include "generated/flight_plan.h"#include "modules/core/abi.h"Go to the source code of this file.
Data Structures | |
| struct | NavTrinity |
Macros | |
| #define | NAV_TRINITY_RECOVER_MAX_TURN 1.5f |
| #define | NAV_TRINITY_BORDER_FILTER 20.f |
| #define | NAV_TRINITY_LWC_ID ABI_BROADCAST |
Enumerations | |
| enum | TrinityStatus { TRINITY_ENTER , TRINITY_INSIDE_FIRST , TRINITY_INSIDE , TRINITY_OUTSIDE_START , TRINITY_OUTSIDE , TRINITY_RECOVER_START , TRINITY_RECOVER_INSIDE , TRINITY_RECOVER_OUTSIDE } |
| enum | RotationDir { TRINITY_LEFT , TRINITY_RIGHT } |
Functions | |
| static float | change_rep (float dir) |
| static struct EnuCoor_f | process_new_point_trinity (struct EnuCoor_f *position, float alt_sp, float uav_direction) |
| static struct EnuCoor_f | process_first_point_trinity (struct EnuCoor_f *position, float alt_sp, float uav_direction) |
| static void | update_target_point (struct EnuCoor_f *target, struct EnuCoor_f *border, float dt, float tau) |
| static void | lwc_cb (uint8_t sender_id UNUSED, uint32_t stamp UNUSED, int32_t data_type, uint32_t size, uint8_t *data) |
| void | nav_trinity_init (void) |
| Init function called by modules init. | |
| void | nav_trinity_setup (float init_x, float init_y, float init_z, int turn, float desired_radius, float vx, float vy, float vz) |
| Initialized the exploration with a first target point inside the cloud Called from flight plan or with mission parameters. | |
| bool | nav_trinity_run (void) |
| Navigation function Called by flight plan or mission run function. | |
Variables | |
| static struct NavTrinity | nav_trinity |
| static const float | nav_dt = 1.f / NAVIGATION_FREQUENCY |
| static abi_event | lwc_ev |
Definition in file nav_trinity.c.
| struct NavTrinity |
Definition at line 60 of file nav_trinity.c.
| Data Fields | ||
|---|---|---|
| struct EnuCoor_f | actual | |
| struct EnuCoor_f | circle | |
| float | direction | |
| struct EnuCoor_f | estim_border | |
| bool | inside_cloud | |
| float | last_border_time | |
| float | max_recover_radius | |
| struct FloatVect3 | pos_incr | |
| float | radius | |
| float | radius_sign | |
| struct EnuCoor_f | recover_circle | |
| float | recover_radius | |
| enum RotationDir | rotation | |
| enum TrinityStatus | status | |
| struct EnuCoor_f | target | |
| #define NAV_TRINITY_BORDER_FILTER 20.f |
Definition at line 41 of file nav_trinity.c.
Referenced by nav_trinity_run().
| #define NAV_TRINITY_LWC_ID ABI_BROADCAST |
Definition at line 178 of file nav_trinity.c.
Referenced by nav_trinity_init().
| #define NAV_TRINITY_RECOVER_MAX_TURN 1.5f |
Definition at line 37 of file nav_trinity.c.
Referenced by nav_trinity_run().
| enum RotationDir |
| Enumerator | |
|---|---|
| TRINITY_LEFT | |
| TRINITY_RIGHT | |
Definition at line 55 of file nav_trinity.c.
| enum TrinityStatus |
| Enumerator | |
|---|---|
| TRINITY_ENTER | |
| TRINITY_INSIDE_FIRST | |
| TRINITY_INSIDE | |
| TRINITY_OUTSIDE_START | |
| TRINITY_OUTSIDE | |
| TRINITY_RECOVER_START | |
| TRINITY_RECOVER_INSIDE | |
| TRINITY_RECOVER_OUTSIDE | |
Definition at line 44 of file nav_trinity.c.
|
static |
Definition at line 82 of file nav_trinity.c.
References dir.
Referenced by nav_trinity_run().
|
static |
Definition at line 183 of file nav_trinity.c.
References nav_trinity, and UNUSED.
| void nav_trinity_init | ( | void | ) |
Init function called by modules init.
Definition at line 189 of file nav_trinity.c.
References DEFAULT_CIRCLE_RADIUS, lwc_cb(), lwc_ev, mission_register(), nav_trinity, NAV_TRINITY_LWC_ID, and TRINITY_ENTER.
| bool nav_trinity_run | ( | void | ) |
Navigation function Called by flight plan or mission run function.
Definition at line 230 of file nav_trinity.c.
References change_rep(), get_sys_time_float(), ground_alt, nav_circle_XY(), nav_dt, nav_init_stage(), nav_move_waypoint_enu(), nav_route_xy(), nav_send_waypoint(), nav_trinity, NAV_TRINITY_BORDER_FILTER, NAV_TRINITY_RECOVER_MAX_TURN, NavCircleCountNoRewind, NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, process_first_point_trinity(), process_new_point_trinity(), stateGetHorizontalSpeedDir_f(), stateGetPositionEnu_f(), TRINITY_ENTER, TRINITY_INSIDE, TRINITY_INSIDE_FIRST, TRINITY_OUTSIDE, TRINITY_OUTSIDE_START, TRINITY_RECOVER_INSIDE, TRINITY_RECOVER_OUTSIDE, TRINITY_RECOVER_START, update_target_point(), and VECT3_ADD.
| void nav_trinity_setup | ( | float | init_x, |
| float | init_y, | ||
| float | init_z, | ||
| int | turn, | ||
| float | desired_radius, | ||
| float | vx, | ||
| float | vy, | ||
| float | vz ) |
Initialized the exploration with a first target point inside the cloud Called from flight plan or with mission parameters.
Definition at line 205 of file nav_trinity.c.
References nav_dt, nav_trinity, stateGetPositionEnu_f(), TRINITY_ENTER, TRINITY_LEFT, TRINITY_RIGHT, and VECT3_ASSIGN.
|
static |
Definition at line 105 of file nav_trinity.c.
References nav_trinity, process_first_point_trinity(), process_new_point_trinity(), TRINITY_LEFT, and TRINITY_RIGHT.
Referenced by nav_trinity_run(), and process_first_point_trinity().
|
static |
Definition at line 87 of file nav_trinity.c.
References nav_trinity, process_new_point_trinity(), TRINITY_RIGHT, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.
Referenced by nav_trinity_run(), process_first_point_trinity(), and process_new_point_trinity().
|
static |
Definition at line 116 of file nav_trinity.c.
References alpha, target, EnuCoor_f::x, and EnuCoor_f::y.
Referenced by nav_trinity_run().
|
static |
Definition at line 181 of file nav_trinity.c.
|
static |
Definition at line 80 of file nav_trinity.c.
|
static |
Definition at line 78 of file nav_trinity.c.
Referenced by lwc_cb(), nav_trinity_init(), nav_trinity_run(), nav_trinity_setup(), process_first_point_trinity(), and process_new_point_trinity().