Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nav_survey_hybrid.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3 * Based on OSAM poly survey
4 *
5 * This file is part of paparazzi.
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, see
19 * <http://www.gnu.org/licenses/>.
20 */
21
30
34#include "state.h"
35#include "autopilot.h"
36#include "generated/flight_plan.h"
37
38#ifdef DIGITAL_CAM
40#endif
41
42// turn anticipation
43#ifndef SURVEY_HYBRID_APPROACHING_TIME
44#define SURVEY_HYBRID_APPROACHING_TIME 3.f
45#endif
46
47// maximum number of polygon corners
48#ifndef SURVEY_HYBRID_MAX_POLYGON_SIZE
49#define SURVEY_HYBRID_MAX_POLYGON_SIZE 20
50#endif
51
52// use half sweep at the end of polygon
53#ifndef SURVEY_HYBRID_HALF_SWEEP_ENABLED
54#define SURVEY_HYBRID_HALF_SWEEP_ENABLED true
55#endif
56
57// maximum number of sweep lines (0 for unlimited)
58#ifndef SURVEY_HYBRID_MAX_SWEEP
59#define SURVEY_HYBRID_MAX_SWEEP 0
60#endif
61
62// maximum number of sweep back (0 for unlimited)
63#ifndef SURVEY_HYBRID_MAX_SWEEP_BACK
64#define SURVEY_HYBRID_MAX_SWEEP_BACK 0
65#endif
66
67// entry distance (default, half sweep distance)
68#ifndef SURVEY_HYBRID_ENTRY_DISTANCE
69#define SURVEY_HYBRID_ENTRY_DISTANCE (survey_private.sweep_distance / 2.f)
70#endif
71
72// make a circle at entry point if radius is not 0
73#ifndef SURVEY_HYBRID_ENTRY_CIRCLE
74#define SURVEY_HYBRID_ENTRY_CIRCLE TRUE
75#endif
76
77struct Line {float m; float b; float x;};
79
102
105
106static void nav_survey_hybrid_setup(float orientation, float sweep, float radius, float height);
107
108static void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans);
109static void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans);
110static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2);
111static float EvaluateLineForX(float y, struct Line L);
112static float CrossProductZ(struct EnuCoor_f *p1_start, struct EnuCoor_f *p1_end, struct EnuCoor_f *p2_start, struct EnuCoor_f *p2_end);
113
114#define MaxFloat 1000000000
115#define MinFloat -1000000000
116
117#ifndef LINE_START_FUNCTION
118#define LINE_START_FUNCTION {}
119#endif
120#ifndef LINE_STOP_FUNCTION
121#define LINE_STOP_FUNCTION {}
122#endif
123
124#if USE_MISSION
126
127static bool nav_survey_hybrid_mission_local(uint8_t nb, float *params, enum MissionRunFlag flag)
128{
129 if (flag == MissionInit) {
130 if (nb == 10 || nb == 12) {
131 float orientation = params[0];
132 float sweep = params[1];
133 float radius = params[2];
134 float height = params[3];
135 if (nb == 10) { survey_private.size = 3; }
136 else { survey_private.size = 4; }
137 for (int i = 0; i < survey_private.size; i++) {
138 survey_private.corners[i].x = params[4+2*i];
139 survey_private.corners[i].y = params[5+2*i+1];
140 survey_private.corners[i].z = height;
141 }
143 return nav_survey_hybrid_run();
144 }
145 }
146 else if (flag == MissionRun) {
147 return nav_survey_hybrid_run();
148 }
149 return false; // not a valid case
150}
151
152static bool nav_survey_hybrid_mission_global(uint8_t nb, float *params, enum MissionRunFlag flag)
153{
154 if (flag == MissionInit) {
155 if (nb == 10 || nb == 12) {
156 float orientation = params[0];
157 float sweep = params[1];
158 float radius = params[2];
159 float height = params[3];
160 if (nb == 10) { survey_private.size = 3; }
161 else { survey_private.size = 4; }
162 for (int i = 0; i < survey_private.size; i++) {
163 struct LlaCoor_f lla = {
164 .lat = RadOfDeg(params[4+2*i]),
165 .lon = RadOfDeg(params[4+2*i+1]),
166 .alt = stateGetLlaOrigin_f().alt + height
167 };
168 struct EnuCoor_f corner;
169 enu_of_lla_point_f(&corner, stateGetNedOrigin_f(), &lla);
170 survey_private.corners[i] = corner;
171 }
172 nav_survey_hybrid_setup(orientation, sweep, radius, height);
173 return nav_survey_hybrid_run();
174 }
175 }
176 else if (flag == MissionRun) {
177 return nav_survey_hybrid_run();
178 }
179 return false; // not a valid case
180}
181
182#endif
183
185{
188 survey_hybrid.sweep_nb = 0;
189 survey_hybrid.sweep_back_nb = 0;
191
192 memset(&survey_private, 0, sizeof(struct SurveyHybridPrivate));
193
194#if USE_MISSION
195 mission_register(nav_survey_hybrid_mission_local, "SRVHL");
196 mission_register(nav_survey_hybrid_mission_global, "SRVHG");
197#endif
198}
199
202static void nav_survey_hybrid_setup(float orientation, float sweep, float radius, float height)
203{
204 FLOAT_VECT2_ZERO(survey_private.smallest_corner);
205 int i = 0;
206 float ys = 0.f;
207 float LeftYInt;
208 float RightYInt;
209 float temp;
210 float XIntercept1 = 0.f;
211 float XIntercept2 = 0.f;
212
213 // cap orientation angle to [-pi; +pi]
214 survey_private.orientation = RadOfDeg(orientation);
215 NormRadAngle(survey_private.orientation);
216 survey_private.sweep_distance = sweep;
217
218 // set auto radius mode if needed
219 if (radius < -0.1f) {
220 survey_private.radius = sweep / 2.f;
221 survey_private.circle_turns = true;
222 } else if (radius > 0.1f) {
223 survey_private.radius = radius;
224 survey_private.circle_turns = true;
225 } else {
226 survey_private.radius = 0.f;
227 survey_private.circle_turns = false;
228 }
229
230 float entry_distance = SURVEY_HYBRID_ENTRY_DISTANCE;
231 survey_private.sweep = survey_private.sweep_distance;
232 survey_hybrid.sweep_nb = 0;
233 survey_hybrid.sweep_back_nb = 0;
234 survey_private.status = Init;
235
236 // Rotate Corners so sweeps are parellel with x axis
237 for (i = 0; i < survey_private.size; i++) {
238 struct EnuCoor_f zero = { 0 };
239 TranslateAndRotateFromWorld(&survey_private.corners[i], survey_private.orientation, &zero);
240 }
241
242 // Find min x and min y
243 VECT2_COPY(survey_private.smallest_corner, survey_private.corners[0]);
244 for (i = 1; i < survey_private.size; i++) {
245 if (survey_private.corners[i].y < survey_private.smallest_corner.y) {
246 survey_private.smallest_corner.y = survey_private.corners[i].y;
247 }
248 if (survey_private.corners[i].x < survey_private.smallest_corner.x) {
249 survey_private.smallest_corner.x = survey_private.corners[i].x;
250 }
251 }
252
253 // Translate Corners all exist in quad #1
254 for (i = 0; i < survey_private.size; i++) {
255 TranslateAndRotateFromWorld(&survey_private.corners[i], 0.f, &survey_private.smallest_corner);
256 }
257
258 // Find max y
259 survey_private.max_y = survey_private.corners[0].y;
260 for (i = 1; i < survey_private.size; i++) {
261 if (survey_private.corners[i].y > survey_private.max_y) {
262 survey_private.max_y = survey_private.corners[i].y;
263 }
264 }
265
266 // Find polygon edges
267 for (i = 0; i < survey_private.size; i++) {
268 if (i == 0) {
269 if (survey_private.corners[survey_private.size - 1].x == survey_private.corners[i].x) { // Don't divide by zero!
270 survey_private.edges[i].m = MaxFloat;
271 } else {
272 survey_private.edges[i].m = ((survey_private.corners[survey_private.size - 1].y - survey_private.corners[i].y) / (survey_private.corners[survey_private.size - 1].x - survey_private.corners[i].x));
273 }
274 }
275 else if (survey_private.corners[i].x == survey_private.corners[i - 1].x) {
276 survey_private.edges[i].m = MaxFloat;
277 } else {
278 survey_private.edges[i].m = ((survey_private.corners[i].y - survey_private.corners[i - 1].y) / (survey_private.corners[i].x - survey_private.corners[i - 1].x));
279 }
280
281 survey_private.edges[i].b = (survey_private.corners[i].y - (survey_private.corners[i].x * survey_private.edges[i].m));
282 }
283
284 // Find Min and Max y for each line
285 FindInterceptOfTwoLines(&temp, &LeftYInt, survey_private.edges[0], survey_private.edges[1]);
286 FindInterceptOfTwoLines(&temp, &RightYInt, survey_private.edges[0], survey_private.edges[survey_private.size - 1]);
287
288 if (LeftYInt > RightYInt) {
289 survey_private.edge_max_y[0] = LeftYInt;
290 survey_private.edge_min_y[0] = RightYInt;
291 } else {
292 survey_private.edge_max_y[0] = RightYInt;
293 survey_private.edge_min_y[0] = LeftYInt;
294 }
295
296 for (i = 1; i < survey_private.size - 1; i++) {
297 FindInterceptOfTwoLines(&temp, &LeftYInt, survey_private.edges[i], survey_private.edges[i + 1]);
298 FindInterceptOfTwoLines(&temp, &RightYInt, survey_private.edges[i], survey_private.edges[i - 1]);
299
300 if (LeftYInt > RightYInt) {
301 survey_private.edge_max_y[i] = LeftYInt;
302 survey_private.edge_min_y[i] = RightYInt;
303 } else {
304 survey_private.edge_max_y[i] = RightYInt;
305 survey_private.edge_min_y[i] = LeftYInt;
306 }
307 }
308
309 FindInterceptOfTwoLines(&temp, &LeftYInt, survey_private.edges[survey_private.size - 1], survey_private.edges[0]);
310 FindInterceptOfTwoLines(&temp, &RightYInt, survey_private.edges[survey_private.size - 1], survey_private.edges[survey_private.size - 2]);
311
312 if (LeftYInt > RightYInt) {
313 survey_private.edge_max_y[survey_private.size - 1] = LeftYInt;
314 survey_private.edge_min_y[survey_private.size - 1] = RightYInt;
315 } else {
316 survey_private.edge_max_y[survey_private.size - 1] = RightYInt;
317 survey_private.edge_min_y[survey_private.size - 1] = LeftYInt;
318 }
319
320 // Find amount to increment by every sweep
321 if (survey_private.corners[0].y >= survey_private.max_y / 2.f) {
322 entry_distance = -entry_distance;
323 survey_private.sweep = -survey_private.sweep_distance;
324 } else {
325 survey_private.sweep = survey_private.sweep_distance;
326 }
327
328 // Find y value of the first sweep
329 ys = survey_private.corners[0].y + entry_distance;
330
331 // Find the edges which intercet the sweep line first
332 for (i = 0; i < survey_private.size; i++) {
333 if (survey_private.edge_min_y[i] <= ys && survey_private.edge_max_y[i] > ys) {
334 XIntercept2 = XIntercept1;
335 XIntercept1 = EvaluateLineForX(ys, survey_private.edges[i]);
336 }
337 }
338
339 // Find point to come from and point to go to
340 if (fabsf(survey_private.corners[0].x - XIntercept2) <= fabsf(survey_private.corners[0].x - XIntercept1)) {
341 survey_private.to_wp.x = XIntercept1;
342 survey_private.to_wp.y = ys;
343 survey_private.from_wp.x = XIntercept2;
344 survey_private.from_wp.y = ys;
345 } else {
346 survey_private.to_wp.x = XIntercept2;
347 survey_private.to_wp.y = ys;
348 survey_private.from_wp.x = XIntercept1;
349 survey_private.from_wp.y = ys;
350 }
351
352 // Find the entry point
353 survey_private.entry.x = survey_private.from_wp.x;
354 survey_private.entry.y = survey_private.corners[0].y + entry_distance;
355 survey_private.entry.z = height;
356
357 // Go into entry state
358 survey_private.status = Entry;
359
362 survey_private.valid = true;
363}
364
365void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius, float height)
366{
367 survey_private.valid = false;
368 if (size < 3 || size > SURVEY_HYBRID_MAX_POLYGON_SIZE) {
369 return; // polygon is too small or too big
370 }
371 for (int i = 0; i < size; i++) {
372 struct EnuCoor_f *wp = waypoint_get_enu_f(start_wp + i);
373 if (wp == NULL) {
374 return; // not a valid waypoint
375 }
376 survey_private.corners[i] = *wp;
377 }
378 survey_private.size = size;
379
380 nav_survey_hybrid_setup(orientation, sweep, radius, height);
381}
382
383void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius, float height)
384{
385 survey_private.valid = false;
386 struct EnuCoor_f *start = waypoint_get_enu_f(start_wp);
387 struct EnuCoor_f *second = waypoint_get_enu_f(second_wp);
388 if (start == NULL || second == NULL) {
389 return;
390 }
391
392 float dx = second->x - start->x;
393 float dy = second->y - start->y;
394 float angle = DegOfRad(atan2f(dy, dx));
395 nav_survey_hybrid_setup_orientation(start_wp, angle, size, sweep, radius, height);
396}
397
398//=========================================================================================================================
400{
401 if (!survey_private.valid) {
402 return false; // don't start survey
403 }
404
405 struct EnuCoor_f C;
406 float ys = 0.f;
407 static struct EnuCoor_f LastPoint;
408 int i = 0;
409 bool LastHalfSweep = false;
410 static bool HalfSweep = false;
411 float XIntercept1 = 0.f;
412 float XIntercept2 = 0.f;
413 float DInt1 = 0.f;
414 float DInt2 = 0.f;
415 struct EnuCoor_f zero = { 0 };
416 float qdr = 0.f;
417 float qdr_out = 0.f;
418
419 switch (survey_private.status) {
420 case Entry:
421 C = survey_private.entry;
422 RotateAndTranslateToWorld(&C, 0.f, &survey_private.smallest_corner);
423 RotateAndTranslateToWorld(&C, survey_private.orientation, &zero);
424
425 if (survey_private.circle_turns && SURVEY_HYBRID_ENTRY_CIRCLE) {
426 // align segment at entry point with a circle
427 survey_private.circle.x = C.x - (cosf(survey_private.orientation + M_PI_2) * survey_private.radius);
428 survey_private.circle.y = C.y - (sinf(survey_private.orientation + M_PI_2) * survey_private.radius);
429 survey_private.circle.z = C.z;
430 nav.nav_circle(&survey_private.circle, survey_private.radius);
431
432 qdr = atan2f(stateGetPositionEnu_f()->x - survey_private.circle.x, stateGetPositionEnu_f()->y - survey_private.circle.y);
433 qdr_out = - survey_private.orientation;
434 if (CloseRadAngles(qdr, qdr_out) && NavCircleCount() > 0.5f) {
435 survey_private.status = Sweep;
438 }
439 } else {
440 // goto entry point
441 VECT3_COPY(survey_private.segment_to, C);
442 nav.nav_goto(&survey_private.segment_to);
443
444 if (((nav.nav_approaching(&survey_private.segment_to, NULL, SURVEY_HYBRID_APPROACHING_TIME))
445 && (fabsf(stateGetPositionEnu_f()->z - survey_private.entry.z)) < 5.f)) {
446 survey_private.status = Sweep;
449 }
450 }
451 break;
452 case Sweep:
453 LastHalfSweep = HalfSweep;
454 survey_private.segment_to = survey_private.to_wp;
455 survey_private.segment_from = survey_private.from_wp;
456
457 // Rotate and Translate Line points into real world
458 RotateAndTranslateToWorld(&survey_private.segment_to, 0.f, &survey_private.smallest_corner);
459 RotateAndTranslateToWorld(&survey_private.segment_to, survey_private.orientation, &zero);
460 RotateAndTranslateToWorld(&survey_private.segment_from, 0.f, &survey_private.smallest_corner);
461 RotateAndTranslateToWorld(&survey_private.segment_from, survey_private.orientation, &zero);
462
463 // follow the line
464 nav.nav_route(&survey_private.segment_from, &survey_private.segment_to);
465
466 if (nav.nav_approaching(&survey_private.segment_to, &survey_private.segment_from, SURVEY_HYBRID_APPROACHING_TIME)) {
467 LastPoint = survey_private.to_wp;
468
469#ifdef DIGITAL_CAM
470 // if last shot is more than shot_distance/2 from the corner and in survey autoshoot mode,
471 // then take a picture in the corner before go to the next sweep
473 float line_length = fabsf((fabsf(survey_private.segment_from.x) - fabsf(survey_private.segment_to.x)));
474 double inteiro;
475 double fract = modf(line_length / dc_distance_interval, &inteiro);
476 if (fract > .5) {
478 }
479 }
480#endif
481
482 if (LastPoint.y + survey_private.sweep >= survey_private.max_y || LastPoint.y + survey_private.sweep <= 0) {
483 // Your out of the Polygon so Sweep Back or Half Sweep
484 if ((LastPoint.y + (survey_private.sweep / 2.f)) <= survey_private.max_y ||
485 (LastPoint.y + (survey_private.sweep / 2.f)) >= 0.f ||
486 !survey_hybrid.half_sweep_enabled) {
487 // Sweep back
488 survey_private.sweep = -survey_private.sweep;
489 }
490 if (LastHalfSweep) {
491 HalfSweep = false;
492 ys = LastPoint.y + survey_private.sweep;
493 } else {
494 HalfSweep = true;
495 ys = LastPoint.y + (survey_private.sweep / 2.f);
496 }
497 survey_hybrid.sweep_back_nb++;
498 } else { // Normal sweep
499 // Find y value of the first sweep
500 HalfSweep = false;
501 ys = LastPoint.y + survey_private.sweep;
502 }
503
504 // Find the edges which intercet the sweep line first
505 for (i = 0; i < survey_private.size; i++) {
506 if (survey_private.edge_min_y[i] < ys && survey_private.edge_max_y[i] >= ys) {
507 XIntercept2 = XIntercept1;
508 XIntercept1 = EvaluateLineForX(ys, survey_private.edges[i]);
509 }
510 }
511
512 // Find point to come from and point to go to
513 DInt1 = XIntercept1 - LastPoint.x;
514 DInt2 = XIntercept2 - LastPoint.x;
515
516 if (DInt1 *DInt2 >= 0) {
517 if (fabsf(DInt2) <= fabsf(DInt1)) {
518 survey_private.to_wp.x = XIntercept1;
519 survey_private.to_wp.y = ys;
520 survey_private.from_wp.x = XIntercept2;
521 survey_private.from_wp.y = ys;
522 } else {
523 survey_private.to_wp.x = XIntercept2;
524 survey_private.to_wp.y = ys;
525 survey_private.from_wp.x = XIntercept1;
526 survey_private.from_wp.y = ys;
527 }
528 } else {
529 if ((survey_private.to_wp.x - survey_private.from_wp.x) > 0 && DInt2 > 0) {
530 survey_private.to_wp.x = XIntercept1;
531 survey_private.to_wp.y = ys;
532 survey_private.from_wp.x = XIntercept2;
533 survey_private.from_wp.y = ys;
534 } else if ((survey_private.to_wp.x - survey_private.from_wp.x) < 0 && DInt2 < 0) {
535 survey_private.to_wp.x = XIntercept1;
536 survey_private.to_wp.y = ys;
537 survey_private.from_wp.x = XIntercept2;
538 survey_private.from_wp.y = ys;
539 } else {
540 survey_private.to_wp.x = XIntercept2;
541 survey_private.to_wp.y = ys;
542 survey_private.from_wp.x = XIntercept1;
543 survey_private.from_wp.y = ys;
544 }
545 }
546
547 // Go into Turn state
548 survey_private.status = Turn;
551
552 survey_hybrid.sweep_nb++;
553 if ((survey_hybrid.sweep_nb_max > 0 && survey_hybrid.sweep_nb >= survey_hybrid.sweep_nb_max) ||
554 (survey_hybrid.sweep_back_nb_max > 0 && survey_hybrid.sweep_back_nb >= survey_hybrid.sweep_back_nb_max)) {
555 // end survey if nb > nb_max and nb_max is not 0
556 // or if nb_back > nb_back_max and nb_back_max is not 0
557 return false;
558 }
559 }
560
561 break;
562 case Turn:
563 survey_private.segment_from = LastPoint;
564 survey_private.segment_to = survey_private.from_wp;
565
566 // Rotate and Translate Line points into real world
567 RotateAndTranslateToWorld(&survey_private.segment_to, 0.f, &survey_private.smallest_corner);
568 RotateAndTranslateToWorld(&survey_private.segment_to, survey_private.orientation, &zero);
569 RotateAndTranslateToWorld(&survey_private.segment_from, 0.f, &survey_private.smallest_corner);
570 RotateAndTranslateToWorld(&survey_private.segment_from, survey_private.orientation, &zero);
571
572 if (survey_private.circle_turns) {
573 if (CrossProductZ(&LastPoint, &survey_private.from_wp, &survey_private.from_wp, &survey_private.to_wp) < 0.f) {
574 survey_private.radius = fabsf(survey_private.radius);
575 } else {
576 survey_private.radius = - fabsf(survey_private.radius);
577 }
578 float dir = (survey_private.sweep < 0.f ? -1.f : 1.f) * fabsf(survey_private.radius);
579 survey_private.circle.x = survey_private.segment_to.x - (cosf(survey_private.orientation + M_PI_2) * dir);
580 survey_private.circle.y = survey_private.segment_to.y - (sinf(survey_private.orientation + M_PI_2) * dir);
581 survey_private.circle.z = survey_private.segment_to.z;
582 // circle turns
583 nav.nav_circle(&survey_private.circle, survey_private.radius);
584
585 qdr = atan2f(stateGetPositionEnu_f()->x - survey_private.circle.x, stateGetPositionEnu_f()->y - survey_private.circle.y);
586 qdr_out = (survey_private.sweep > 0 ? -survey_private.orientation : -survey_private.orientation + M_PI);
587 if (CloseRadAngles(qdr, qdr_out)) {
588 survey_private.status = Sweep;
591 }
592 } else {
593 // use straight lines to reach next point
594 nav.nav_route(&survey_private.segment_from, &survey_private.segment_to);
595
596 if (nav.nav_approaching(&survey_private.segment_to, &survey_private.segment_from, SURVEY_HYBRID_APPROACHING_TIME)) {
597 survey_private.status = Sweep;
600 }
601 }
602
603 break;
604 case Init:
605 return false;
606 default:
607 return false;
608 }
609
610 return true;
611
612}
613
614//============================================================================================================================================
615/*
616 Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
617*/
618void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans)
619{
620 float temp;
621
622 p->x = p->x - trans->x;
623 p->y = p->y - trans->y;
624
625 temp = p->x;
626 p->x = p->x * cosf(Zrot) + p->y * sinf(Zrot);
627 p->y = -temp * sinf(Zrot) + p->y * cosf(Zrot);
628}
629
631void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans)
632{
633 float temp = p->x;
634
635 p->x = p->x * cosf(Zrot) - p->y * sinf(Zrot);
636 p->y = temp * sinf(Zrot) + p->y * cosf(Zrot);
637
638 p->x = p->x + trans->x;
639 p->y = p->y + trans->y;
640}
641
642void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
643{
644 *x = ((L2.b - L1.b) / (L1.m - L2.m));
645 *y = L1.m * (*x) + L1.b;
646}
647
648
649float EvaluateLineForX(float y, struct Line L)
650{
651 return ((y - L.b) / L.m);
652}
653
654float CrossProductZ(struct EnuCoor_f *p1_start, struct EnuCoor_f *p1_end, struct EnuCoor_f *p2_start, struct EnuCoor_f *p2_end)
655{
656 float d1x = p1_end->x - p1_start->x;
657 float d1y = p1_end->y - p1_start->y;
658 float d2x = p2_end->x - p2_start->x;
659 float d2y = p2_end->y - p2_start->y;
660 return d1x * d2y - d1y * d2x;
661}
662
void dc_send_command(uint8_t cmd)
Send Command To Camera.
Core autopilot interface common to all firmwares.
float dc_distance_interval
AutoShoot photos on distance to last shot in meters.
Definition dc.c:85
dc_autoshoot_type dc_autoshoot
Definition dc.c:67
Standard Digital Camera Control Interface.
@ DC_SHOOT
Definition dc.h:102
@ DC_AUTOSHOOT_SURVEY
Definition dc.h:137
#define FLOAT_VECT2_ZERO(_v)
#define VECT2_COPY(_a, _b)
#define VECT3_COPY(_a, _b)
static struct LtpDef_f * stateGetNedOrigin_f(void)
Get the coordinate NED frame origin (float).
Definition state.h:577
struct LlaCoor_f stateGetLlaOrigin_f(void)
Get the LLA position of the frame origin (float).
Definition state.c:149
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static float p[2][2]
bool mission_register(mission_custom_cb cb, char *type)
Register a new navigation or action callback function.
mission planner library
MissionRunFlag
@ MissionInit
first exec
@ MissionRun
normal run
struct EnuCoor_f * waypoint_get_enu_f(uint8_t wp_id)
Get ENU coordinates (float).
Definition waypoints.c:406
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition nav.c:92
#define LINE_STOP_FUNCTION
Definition nav.c:279
#define LINE_START_FUNCTION
Definition nav.c:276
#define NavCircleCount()
Definition nav.h:155
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition nav.h:191
#define CloseRadAngles(_c1, _c2)
#define SURVEY_HYBRID_MAX_SWEEP_BACK
uint8_t size
size of the polygon
#define SURVEY_HYBRID_APPROACHING_TIME
struct EnuCoor_f to_wp
tmp point in rotated frame
struct EnuCoor_f from_wp
tmp point in rotated frame
float orientation
requested orientation in radians
static float CrossProductZ(struct EnuCoor_f *p1_start, struct EnuCoor_f *p1_end, struct EnuCoor_f *p2_start, struct EnuCoor_f *p2_end)
#define SURVEY_HYBRID_ENTRY_DISTANCE
#define SURVEY_HYBRID_MAX_SWEEP
float sweep
oriented sweep distance
static void nav_survey_hybrid_setup(float orientation, float sweep, float radius, float height)
finish preparation of survey based on private structure
static float EvaluateLineForX(float y, struct Line L)
static void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans)
Rotates point round z by -Zrot then translates so (0,0) becomes (transX,transY).
struct EnuCoor_f entry
entry point
struct Line edges[SURVEY_HYBRID_MAX_POLYGON_SIZE]
polygon edges
void nav_survey_hybrid_init(void)
Init function.
static void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans)
static struct SurveyHybridPrivate survey_private
struct EnuCoor_f circle
circle center
#define MaxFloat
#define SURVEY_HYBRID_ENTRY_CIRCLE
float radius
turn radius
static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2)
#define SURVEY_HYBRID_MAX_POLYGON_SIZE
bool circle_turns
turns with circles (or lines between points otherwise)
bool valid
setup is valid
SurveyStatus
@ Entry
@ Sweep
float edge_min_y[SURVEY_HYBRID_MAX_POLYGON_SIZE]
tmp point in rotated frame
bool nav_survey_hybrid_run(void)
Run polygon hybrid survey.
void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius, float height)
Setup polygon survey.
float sweep_distance
requested sweep distance
#define SURVEY_HYBRID_HALF_SWEEP_ENABLED
struct EnuCoor_f segment_to
end of current segment
void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius, float height)
Setup "dynamic" polygon survey with sweep orientation towards a waypoint.
float edge_max_y[SURVEY_HYBRID_MAX_POLYGON_SIZE]
tmp point in rotated frame
enum SurveyStatus status
current state
struct EnuCoor_f corners[SURVEY_HYBRID_MAX_POLYGON_SIZE]
corners location
struct SurveyHybrid survey_hybrid
struct EnuCoor_f segment_from
start of current segment
struct EnuCoor_f smallest_corner
tmp point in rotated frame
Paparazzi floating point algebra.
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
float alt
in meters (normally above WGS84 reference ellipsoid)
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
struct RotorcraftNavigation nav
Definition navigation.c:51
Rotorcraft navigation functions.
static const float dir[]
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.