[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_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" />
+4
View File
@@ -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">
+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; 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;
+1 -1
View File
@@ -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;