[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:
Felix Ruess
2014-08-27 16:07:27 +02:00
parent bf44219206
commit b5470cb794
4 changed files with 16 additions and 11 deletions
@@ -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" />
+4
View File
@@ -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">
+10 -10
View File
@@ -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;
+1 -1
View File
@@ -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;