mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 05:42:49 +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_poly_osam.xml"/>
|
||||||
<load name="nav_survey_zamboni.xml"/>
|
<load name="nav_survey_zamboni.xml"/>
|
||||||
<load name="nav_vertical_raster.xml"/>
|
<load name="nav_vertical_raster.xml"/>
|
||||||
|
<load name="nav_spiral.xml"/>
|
||||||
|
|
||||||
<load name="digital_cam_servo.xml">
|
<load name="digital_cam_servo.xml">
|
||||||
<define name="DC_SHUTTER_SERVO" value="COMMAND_SHUTTER" />
|
<define name="DC_SHUTTER_SERVO" value="COMMAND_SHUTTER" />
|
||||||
|
|||||||
@@ -118,6 +118,10 @@
|
|||||||
<call fun="nav_vertical_raster_setup()"/>
|
<call fun="nav_vertical_raster_setup()"/>
|
||||||
<call fun="nav_vertical_raster_run(WP_S1, WP_S2, nav_radius, 50)"/>
|
<call fun="nav_vertical_raster_run(WP_S1, WP_S2, nav_radius, 50)"/>
|
||||||
</block>
|
</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">
|
<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;
|
nav_spiral.fly_from.y = stateGetPositionEnu_f()->y;
|
||||||
|
|
||||||
if(nav_spiral.dist_from_center > nav_spiral.radius)
|
if(nav_spiral.dist_from_center > nav_spiral.radius)
|
||||||
nav_spiral.status = Outside;
|
nav_spiral.status = SpiralOutside;
|
||||||
return FALSE;
|
return FALSE;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -96,7 +96,7 @@ bool_t nav_spiral_run(void)
|
|||||||
|
|
||||||
switch(nav_spiral.status)
|
switch(nav_spiral.status)
|
||||||
{
|
{
|
||||||
case Outside:
|
case SpiralOutside:
|
||||||
//flys until center of the helix is reached an start helix
|
//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);
|
nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y);
|
||||||
// center reached?
|
// center reached?
|
||||||
@@ -105,24 +105,24 @@ bool_t nav_spiral_run(void)
|
|||||||
#ifdef DIGITAL_CAM
|
#ifdef DIGITAL_CAM
|
||||||
dc_send_command(DC_SHOOT);
|
dc_send_command(DC_SHOOT);
|
||||||
#endif
|
#endif
|
||||||
nav_spiral.status = StartCircle;
|
nav_spiral.status = SpiralStartCircle;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case StartCircle:
|
case SpiralStartCircle:
|
||||||
// Starts helix
|
// Starts helix
|
||||||
// storage of current coordinates
|
// 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);
|
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){
|
if(nav_spiral.dist_from_center >= nav_spiral.radius_start){
|
||||||
VECT2_COPY(nav_spiral.last_circle, pos_enu);
|
VECT2_COPY(nav_spiral.last_circle, pos_enu);
|
||||||
nav_spiral.status = Circle;
|
nav_spiral.status = SpiralCircle;
|
||||||
// Start helix
|
// Start helix
|
||||||
#ifdef DIGITAL_CAM
|
#ifdef DIGITAL_CAM
|
||||||
dc_Circle(360/nav_spiral.segments);
|
dc_Circle(360/nav_spiral.segments);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case Circle: {
|
case SpiralCircle: {
|
||||||
nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start);
|
nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start);
|
||||||
// Trigonometrische Berechnung des bereits geflogenen Winkels alpha
|
// Trigonometrische Berechnung des bereits geflogenen Winkels alpha
|
||||||
// equation:
|
// equation:
|
||||||
@@ -136,11 +136,11 @@ bool_t nav_spiral_run(void)
|
|||||||
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * nav_spiral.radius_start)));
|
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * nav_spiral.radius_start)));
|
||||||
if (CircleAlpha >= nav_spiral.alpha_limit) {
|
if (CircleAlpha >= nav_spiral.alpha_limit) {
|
||||||
VECT2_COPY(nav_spiral.last_circle, pos_enu);
|
VECT2_COPY(nav_spiral.last_circle, pos_enu);
|
||||||
nav_spiral.status = IncSpiral;
|
nav_spiral.status = SpiralInc;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case IncSpiral:
|
case SpiralInc:
|
||||||
// increasing circle radius as long as it is smaller than max helix radius
|
// 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)
|
if(nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius)
|
||||||
{
|
{
|
||||||
@@ -160,7 +160,7 @@ bool_t nav_spiral_run(void)
|
|||||||
dc_stop();
|
dc_stop();
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
nav_spiral.status = Circle;
|
nav_spiral.status = SpiralCircle;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
|
|||||||
@@ -32,7 +32,7 @@
|
|||||||
#include "std.h"
|
#include "std.h"
|
||||||
#include "math/pprz_algebra_float.h"
|
#include "math/pprz_algebra_float.h"
|
||||||
|
|
||||||
enum SpiralStatus { Outside, StartCircle, Circle, IncSpiral };
|
enum SpiralStatus { SpiralOutside, SpiralStartCircle, SpiralCircle, SpiralInc };
|
||||||
|
|
||||||
struct NavSpiral {
|
struct NavSpiral {
|
||||||
struct FloatVect3 trans_current;
|
struct FloatVect3 trans_current;
|
||||||
|
|||||||
Reference in New Issue
Block a user