mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 13:55:40 +08:00
[modules] fix nav_spiral
no generic variable/enum names in headers... Also add it to nav_modules flight plan and test it with microjet_lisa_m
This commit is contained in:
@@ -59,6 +59,7 @@
|
||||
<load name="nav_survey_poly_osam.xml"/>
|
||||
<load name="nav_survey_zamboni.xml"/>
|
||||
<load name="nav_vertical_raster.xml"/>
|
||||
<load name="nav_spiral.xml"/>
|
||||
|
||||
<load name="digital_cam_servo.xml">
|
||||
<define name="DC_SHUTTER_SERVO" value="COMMAND_SHUTTER" />
|
||||
|
||||
@@ -118,6 +118,10 @@
|
||||
<call fun="nav_vertical_raster_setup()"/>
|
||||
<call fun="nav_vertical_raster_run(WP_S1, WP_S2, nav_radius, 50)"/>
|
||||
</block>
|
||||
<block name="Spiral">
|
||||
<call fun="nav_spiral_setup(WP_S1, WP_S2, 30, 10, 3)"/>
|
||||
<call fun="nav_spiral_run()"/>
|
||||
</block>
|
||||
<!--====================================================================================-->
|
||||
|
||||
<block group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
|
||||
|
||||
@@ -79,7 +79,7 @@ bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start,
|
||||
nav_spiral.fly_from.y = stateGetPositionEnu_f()->y;
|
||||
|
||||
if(nav_spiral.dist_from_center > nav_spiral.radius)
|
||||
nav_spiral.status = Outside;
|
||||
nav_spiral.status = SpiralOutside;
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
@@ -96,7 +96,7 @@ bool_t nav_spiral_run(void)
|
||||
|
||||
switch(nav_spiral.status)
|
||||
{
|
||||
case Outside:
|
||||
case SpiralOutside:
|
||||
//flys until center of the helix is reached an start helix
|
||||
nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y);
|
||||
// center reached?
|
||||
@@ -105,24 +105,24 @@ bool_t nav_spiral_run(void)
|
||||
#ifdef DIGITAL_CAM
|
||||
dc_send_command(DC_SHOOT);
|
||||
#endif
|
||||
nav_spiral.status = StartCircle;
|
||||
nav_spiral.status = SpiralStartCircle;
|
||||
}
|
||||
break;
|
||||
case StartCircle:
|
||||
case SpiralStartCircle:
|
||||
// Starts helix
|
||||
// storage of current coordinates
|
||||
// calculation needed, State switch to Circle
|
||||
// calculation needed, State switch to SpiralCircle
|
||||
nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start);
|
||||
if(nav_spiral.dist_from_center >= nav_spiral.radius_start){
|
||||
VECT2_COPY(nav_spiral.last_circle, pos_enu);
|
||||
nav_spiral.status = Circle;
|
||||
nav_spiral.status = SpiralCircle;
|
||||
// Start helix
|
||||
#ifdef DIGITAL_CAM
|
||||
dc_Circle(360/nav_spiral.segments);
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
case Circle: {
|
||||
case SpiralCircle: {
|
||||
nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start);
|
||||
// Trigonometrische Berechnung des bereits geflogenen Winkels alpha
|
||||
// equation:
|
||||
@@ -136,11 +136,11 @@ bool_t nav_spiral_run(void)
|
||||
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * nav_spiral.radius_start)));
|
||||
if (CircleAlpha >= nav_spiral.alpha_limit) {
|
||||
VECT2_COPY(nav_spiral.last_circle, pos_enu);
|
||||
nav_spiral.status = IncSpiral;
|
||||
nav_spiral.status = SpiralInc;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case IncSpiral:
|
||||
case SpiralInc:
|
||||
// increasing circle radius as long as it is smaller than max helix radius
|
||||
if(nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius)
|
||||
{
|
||||
@@ -160,7 +160,7 @@ bool_t nav_spiral_run(void)
|
||||
dc_stop();
|
||||
#endif
|
||||
}
|
||||
nav_spiral.status = Circle;
|
||||
nav_spiral.status = SpiralCircle;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
||||
@@ -32,7 +32,7 @@
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
enum SpiralStatus { Outside, StartCircle, Circle, IncSpiral };
|
||||
enum SpiralStatus { SpiralOutside, SpiralStartCircle, SpiralCircle, SpiralInc };
|
||||
|
||||
struct NavSpiral {
|
||||
struct FloatVect3 trans_current;
|
||||
|
||||
Reference in New Issue
Block a user