32#include "generated/flight_plan.h"
38#ifndef LINE_START_FUNCTION
39#define LINE_START_FUNCTION {}
41#ifndef LINE_STOP_FUNCTION
42#define LINE_STOP_FUNCTION {}
61 zs.pre_leave_angle = 2;
69 if (sweep_spacing > 0) {
70 zs.pre_leave_angle = 0;
78 zs.flight_angle = DegOfRad(atan2(flight_vec.
x, flight_vec.
y));
79 zs.return_angle =
zs.flight_angle + 180;
80 if (
zs.return_angle > 359) {
81 zs.return_angle -= 360;
87 zs.sweep_width.x = -flight_vec.
y * sweep_spacing;
88 zs.sweep_width.y = +flight_vec.
x * sweep_spacing;
91 zs.total_laps = (sweep_lines + 1) / 2;
92 zs.turnradius1 = (
zs.total_laps - 1) * sweep_spacing * 0.5;
93 zs.turnradius2 =
zs.total_laps * sweep_spacing * 0.5;
96 VECT2_SMUL(flight_line, flight_vec, sweep_length * 0.5);
110 zs.turn_center1.x = (
zs.seg_end.x +
zs.ret_start.x) / 2.0;
111 zs.turn_center1.y = (
zs.seg_end.y +
zs.ret_start.y) / 2.0;
112 zs.turn_center2.x = (
zs.seg_start.x +
zs.ret_end.x +
zs.sweep_width.x) / 2.0;
113 zs.turn_center2.y = (
zs.seg_start.y +
zs.ret_end.y +
zs.sweep_width.y) / 2.0;
166 zs.current_laps =
zs.current_laps + 1;
188 }
else if (
zs.stage ==
Z_SEG) {
195 zs.turn_center1.x = (
zs.seg_end.x +
zs.ret_start.x) / 2;
196 zs.turn_center1.y = (
zs.seg_end.y +
zs.ret_start.y) / 2;
197 zs.turn_center2.x = (
zs.seg_start.x +
zs.ret_end.x +
zs.sweep_width.x) / 2;
198 zs.turn_center2.y = (
zs.seg_start.y +
zs.ret_end.y +
zs.sweep_width.y) / 2;
208 if (
zs.current_laps >=
zs.total_laps) {
Core autopilot interface common to all firmwares.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Standard Digital Camera Control Interface.
#define FLOAT_VECT2_NORMALIZE(_v)
#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)
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
#define LINE_START_FUNCTION
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_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Fixedwing Navigation library.
#define NavCourseCloseTo(x)
#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.
bool nav_survey_zamboni_run(void)
main navigation routine.
void nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
initializes the variables needed for the survey to start.
Zamboni pattern survey for fixedwings.
#define CARROT
default approaching_time for a wp
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.