547 lines
18 KiB
C
547 lines
18 KiB
C
/* This file has been generated from conf/flight_plans/braunschweig.xml */
|
|
/* Please DO NOT EDIT */
|
|
|
|
#ifndef FLIGHT_PLAN_H
|
|
#define FLIGHT_PLAN_H
|
|
|
|
#define FLIGHT_PLAN_NAME "EMAV 2004, 8 shape"
|
|
#define NAV_UTM_EAST0 605530
|
|
#define NAV_UTM_NORTH0 5797350
|
|
#define QFU 270.0
|
|
#define WP_HOME 0
|
|
#define WAYPOINTS { \
|
|
{0.0, 0.0, 200},\
|
|
{0.0, 0.0, 200},\
|
|
{115.0, -75.0, 200},\
|
|
{156.7, -41.7, 200},\
|
|
{115.0, 0.0, 200},\
|
|
{0.0, -75.0, 200},\
|
|
{-51.7, -36.7, 200},\
|
|
};
|
|
#define NB_WAYPOINT 7
|
|
#define GROUND_ALT 125.
|
|
#define SECURITY_ALT 150.
|
|
#define MAX_DIST_FROM_HOME 500.
|
|
#ifdef NAV_C
|
|
|
|
static inline void auto_nav( void )
|
|
{
|
|
/*#ifdef WITH_SWITCH
|
|
switch (nav_block) {
|
|
Block(0) // init
|
|
switch(nav_stage) {
|
|
Label(while_1)
|
|
Stage(0)
|
|
if (! (!(estimator_flight_time))) Goto(endwhile_2) else NextStage();
|
|
Stage(1)
|
|
Goto(while_1)
|
|
Label(endwhile_2)
|
|
Stage(2)
|
|
if ((estimator_flight_time>8)) NextStage() else {
|
|
desired_course = RadOfDeg(QFU);
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.150000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
|
|
nav_desired_gaz = TRIM_UPPRZ(0.800000*MAX_PPRZ);
|
|
}
|
|
return;
|
|
Stage(3)
|
|
if ((estimator_z>SECURITY_ALT)) NextStage() else {
|
|
desired_course = RadOfDeg(QFU);
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_CLIMB;
|
|
desired_climb = 8.000000;
|
|
}
|
|
return;
|
|
Stage(4)
|
|
NextBlock()
|
|
}
|
|
|
|
Block(1) // two
|
|
if RcEvent1() { GotoBlock(2) }
|
|
switch(nav_stage) {
|
|
Label(while_3)
|
|
Stage(0)
|
|
if (! (TRUE)) Goto(endwhile_4) else NextStage();
|
|
Stage(1)
|
|
if (approaching(1)) NextStageFrom(1) else {
|
|
fly_to(1);
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 1 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
Stage(2)
|
|
if (approaching(4)) NextStageFrom(4) else {
|
|
fly_to(4);
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 4 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
Stage(3)
|
|
Goto(while_3)
|
|
Label(endwhile_4)
|
|
Stage(4)
|
|
NextBlock()
|
|
}
|
|
|
|
Block(2) // height
|
|
if RcEvent1() { GotoBlock(3) }
|
|
switch(nav_stage) {
|
|
Label(while_5)
|
|
Stage(0)
|
|
if (! (TRUE)) Goto(endwhile_6) else NextStage();
|
|
Stage(1)
|
|
if (approaching(6)) NextStageFrom(6) else {
|
|
fly_to(6);
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 6 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
Stage(2)
|
|
if (approaching(1)) NextStageFrom(1) else {
|
|
fly_to(1);
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 1 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
Stage(3)
|
|
if (approaching(2)) NextStageFrom(2) else {
|
|
route_to(last_wp, 2);
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 2 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
Stage(4)
|
|
if (approaching(3)) NextStageFrom(3) else {
|
|
fly_to(3);
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 3 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
Stage(5)
|
|
if (approaching(4)) NextStageFrom(4) else {
|
|
fly_to(4);
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 4 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
Stage(6)
|
|
if (approaching(5)) NextStageFrom(5) else {
|
|
route_to(last_wp, 5);
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 5 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
Stage(7)
|
|
Goto(while_5)
|
|
Label(endwhile_6)
|
|
Stage(8)
|
|
NextBlock()
|
|
}
|
|
|
|
Block(3) // xyz
|
|
if RcEvent1() { GotoBlock(4) }
|
|
switch(nav_stage) {
|
|
Stage(0)
|
|
|
|
|
|
//NormCourse(x) { while (x < 0) x += 360; while (x >= 360) x -= 360;}
|
|
|
|
|
|
//CircleXY(x,y,radius) { float alpha = atan2(estimator_y - y, estimator_x - x);float alpha_carrot = alpha + CARROT / (-radius * estimator_hspeed_mod);
|
|
//fly_to_xy(x+cos(alpha_carrot)*fabs(radius),y+sin(alpha_carrot)*fabs(radius)); qdr = DegOfRad(M_PI/2 - alpha_carrot); NormCourse(qdr);}
|
|
|
|
|
|
//Goto3D(50)
|
|
{static float carrot_x, carrot_y; int16_t pitch; int16_t roll; if (pprz_mode == PPRZ_MODE_AUTO2) { int16_t yaw = from_fbw.channels[ RADIO_YAW ]; if (yaw >
|
|
MIN_DX || yaw < -MIN_DX) { carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.);carrot_x = Min(carrot_x, MAX_DIST_CARROT); carrot_x = Max(carrot_x, -MAX_DIST_CARROT); }
|
|
pitch = from_fbw.channels[ RADIO_PITCH ]; if (pitch > MIN_DX || pitch < -MIN_DX) { carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); carrot_y = Min(carrot_y,
|
|
MAX_DIST_CARROT); carrot_y = Max(carrot_y, -MAX_DIST_CARROT);} vertical_mode = VERTICAL_MODE_AUTO_ALT; roll = from_fbw.channels[ RADIO_ROLL ]; if (roll >
|
|
MIN_DX || roll < -MIN_DX) { desired_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); desired_altitude = Max(desired_altitude, MIN_HEIGHT_CARROT+GROUND_ALT);
|
|
desired_altitude = Min(desired_altitude, MAX_HEIGHT_CARROT+GROUND_ALT); } } { float alpha = atan2(estimator_y - carrot_y, estimator_x - carrot_x);float alpha_carrot = alpha + CARROT / (-50 * estimator_hspeed_mod);
|
|
fly_to_xy(carrot_x+cos(alpha_carrot)*fabs(50),carrot_y+sin(alpha_carrot)*fabs(50)); qdr = DegOfRad(M_PI/2 - alpha_carrot); { while (qdr < 0) qdr += 360;
|
|
while (qdr >= 360) qdr -= 360;}} }
|
|
|
|
|
|
return;
|
|
Stage(1)
|
|
NextBlock()
|
|
}
|
|
|
|
Block(4) // circle
|
|
if RcEvent1() { GotoBlock(5) }
|
|
switch(nav_stage) {
|
|
Stage(0)
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 0 ].a;
|
|
pre_climb = 0.;
|
|
Circle(0, 150);
|
|
return;
|
|
Stage(1)
|
|
NextBlock()
|
|
}
|
|
|
|
Block(5) // hippo
|
|
if RcEvent1() { GotoBlock(1) }
|
|
switch(nav_stage) {
|
|
Label(while_7)
|
|
Stage(0)
|
|
if (! (TRUE)) Goto(endwhile_8) else NextStage();
|
|
Stage(1)
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 1 ].a;
|
|
pre_climb = 0.;
|
|
Circle(1, 100);
|
|
if (Qdr(0)) NextStage();
|
|
return;
|
|
Stage(2)
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 4 ].a;
|
|
pre_climb = 0.;
|
|
Circle(4, 100);
|
|
if (Qdr(180)) NextStage();
|
|
return;
|
|
Stage(3)
|
|
Goto(while_7)
|
|
Label(endwhile_8)
|
|
Stage(4)
|
|
NextBlock()
|
|
}
|
|
|
|
}
|
|
#else*/
|
|
if ( nav_block == 0 ) {
|
|
// init
|
|
|
|
if ( nav_stage == 0 ) {
|
|
Label( while_1 )
|
|
nav_stage = 0;
|
|
if ( ! ( !( estimator_flight_time ) ) ) Goto( endwhile_2 )
|
|
else NextStage();
|
|
} else
|
|
if ( nav_stage == 1 ) {
|
|
Goto( while_1 )
|
|
} else
|
|
if ( nav_stage == 2 ) {
|
|
Label( endwhile_2 )
|
|
nav_stage = 2;
|
|
if ( ( estimator_flight_time > 8 ) ) NextStage() else {
|
|
desired_course = RadOfDeg( QFU );
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.150000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
|
|
nav_desired_gaz = TRIM_UPPRZ( 0.800000 * MAX_PPRZ );
|
|
}
|
|
return;
|
|
} else
|
|
if ( nav_stage == 3 ) {
|
|
nav_stage = 3;
|
|
if ( ( estimator_z > SECURITY_ALT ) ) NextStage() else {
|
|
desired_course = RadOfDeg( QFU );
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_CLIMB;
|
|
desired_climb = 8.000000;
|
|
}
|
|
return;
|
|
} else
|
|
if ( nav_stage == 4 )
|
|
NextBlock()
|
|
else { }
|
|
} else
|
|
if ( nav_block == 1 ) { // two
|
|
nav_block = 1;
|
|
if RcEvent1() {
|
|
GotoBlock( 2 )
|
|
}
|
|
if ( nav_stage == 0 ) {
|
|
Label( while_3 )
|
|
if ( ! ( TRUE ) ) Goto( endwhile_4 ) else NextStage();
|
|
} else
|
|
if ( nav_stage == 1 ) {
|
|
nav_stage = 1;
|
|
if ( approaching( 1 ) ) NextStageFrom( 1 ) else {
|
|
fly_to( 1 );
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 1 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
} else
|
|
if ( nav_stage == 2 ) {
|
|
nav_stage = 2;
|
|
if ( approaching( 4 ) ) NextStageFrom( 4 ) else {
|
|
fly_to( 4 );
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 4 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
} else
|
|
if ( nav_stage == 3 ) {
|
|
nav_stage = 3;
|
|
Goto( while_3 )
|
|
} else
|
|
if ( nav_stage == 4 ) {
|
|
Label( endwhile_4 )
|
|
nav_stage = 4;
|
|
NextBlock()
|
|
} else { }
|
|
} else
|
|
if ( nav_block == 2 ) { // height
|
|
nav_block = 2;
|
|
if RcEvent1() {
|
|
GotoBlock( 3 )
|
|
}
|
|
|
|
if ( nav_stage == 0 ) {
|
|
Label( while_5 )
|
|
nav_stage = 0;
|
|
if ( ! ( TRUE ) ) Goto( endwhile_6 ) else NextStage();
|
|
} else
|
|
if ( nav_stage == 1 ) {
|
|
nav_stage = 1;
|
|
if ( approaching( 6 ) ) NextStageFrom( 6 ) else {
|
|
fly_to( 6 );
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 6 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
} else
|
|
if ( nav_stage == 2 ) {
|
|
nav_stage = 2;
|
|
if ( approaching( 1 ) ) NextStageFrom( 1 ) else {
|
|
fly_to( 1 );
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 1 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
} else
|
|
if ( nav_stage == 3 ) {
|
|
nav_stage = 3;
|
|
if ( approaching( 2 ) ) NextStageFrom( 2 ) else {
|
|
route_to( last_wp, 2 );
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 2 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
} else
|
|
if ( nav_stage == 4 ) {
|
|
nav_stage = 4;
|
|
if ( approaching( 3 ) ) NextStageFrom( 3 ) else {
|
|
fly_to( 3 );
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 3 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
} else
|
|
if ( nav_stage == 5 ) {
|
|
nav_stage = 5;
|
|
if ( approaching( 4 ) ) NextStageFrom( 4 ) else {
|
|
fly_to( 4 );
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 4 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
} else
|
|
if ( nav_stage == 6 ) {
|
|
nav_stage = 6;
|
|
if ( approaching( 5 ) ) NextStageFrom( 5 ) else {
|
|
route_to( last_wp, 5 );
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 5 ].a;
|
|
pre_climb = 0.;
|
|
}
|
|
return;
|
|
} else
|
|
if ( nav_stage == 7 ) {
|
|
nav_stage = 7;
|
|
Goto( while_5 )
|
|
}
|
|
|
|
else
|
|
if ( nav_stage == 8 ) {
|
|
Label( endwhile_6 )
|
|
nav_stage = 8;
|
|
NextBlock();
|
|
} else { }
|
|
} else
|
|
if ( nav_block == 3 ) { // xyz
|
|
nav_block = 3;
|
|
if RcEvent1() {
|
|
GotoBlock( 4 )
|
|
}
|
|
if ( nav_stage == 0 ) {
|
|
nav_stage = 0;
|
|
//Goto3D(50)
|
|
{
|
|
static float carrot_x, carrot_y;
|
|
int16_t pitch;
|
|
int16_t roll;
|
|
if ( pprz_mode == PPRZ_MODE_AUTO2 ) {
|
|
int16_t yaw = from_fbw.channels[ RADIO_YAW ];
|
|
if ( yaw > MIN_DX || yaw < -MIN_DX ) {
|
|
carrot_x += FLOAT_OF_PPRZ( yaw, 0, -20. );
|
|
carrot_x = Min( carrot_x, MAX_DIST_CARROT );
|
|
carrot_x = Max( carrot_x, -MAX_DIST_CARROT );
|
|
}
|
|
pitch = from_fbw.channels[ RADIO_PITCH ];
|
|
if ( pitch > MIN_DX || pitch < -MIN_DX ) {
|
|
carrot_y += FLOAT_OF_PPRZ( pitch, 0, -20. );
|
|
carrot_y = Min( carrot_y,
|
|
MAX_DIST_CARROT );
|
|
carrot_y = Max( carrot_y, -MAX_DIST_CARROT );
|
|
}
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
roll = from_fbw.channels[ RADIO_ROLL ];
|
|
if ( roll > MIN_DX || roll < -MIN_DX ) {
|
|
desired_altitude += FLOAT_OF_PPRZ( roll, 0, -1.0 );
|
|
desired_altitude = Max( desired_altitude, MIN_HEIGHT_CARROT + GROUND_ALT );
|
|
desired_altitude = Min( desired_altitude, MAX_HEIGHT_CARROT + GROUND_ALT );
|
|
}
|
|
}
|
|
{
|
|
float alpha = atan2( estimator_y - carrot_y, estimator_x - carrot_x );
|
|
float alpha_carrot = alpha + CARROT / ( -50 * estimator_hspeed_mod );
|
|
fly_to_xy( carrot_x + cos( alpha_carrot )*fabs( 50 ),
|
|
carrot_y + sin( alpha_carrot )*fabs( 50 ) );
|
|
qdr = DegOfRad( M_PI / 2 - alpha_carrot );
|
|
{
|
|
_Pragma( "loopbounds min 0 max 1" )
|
|
while ( qdr < 0 ) qdr += 360;
|
|
_Pragma( "loopbounds min 0 max 1" )
|
|
while ( qdr >= 360 ) qdr -= 360;
|
|
}
|
|
}
|
|
}
|
|
|
|
return;
|
|
} else
|
|
if ( nav_stage == 1 ) {
|
|
nav_stage = 1;
|
|
NextBlock()
|
|
} else { }
|
|
} else
|
|
if ( nav_block == 4 ) {
|
|
nav_block = 4;
|
|
if RcEvent1() {
|
|
GotoBlock( 5 )
|
|
}
|
|
if ( nav_stage == 0 ) {
|
|
nav_stage = 0;
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 0 ].a;
|
|
pre_climb = 0.;
|
|
Circle( 0, 150 );
|
|
return;
|
|
} else
|
|
if ( nav_stage == 1 ) {
|
|
nav_stage = 1;
|
|
NextBlock()
|
|
} else {}
|
|
} else
|
|
if ( nav_block == 5 ) {
|
|
nav_block = 5;
|
|
if RcEvent1() {
|
|
GotoBlock( 1 )
|
|
}
|
|
if ( nav_stage == 0 ) {
|
|
Label( while_7 )
|
|
nav_stage = 0;
|
|
if ( ! ( TRUE ) ) Goto( endwhile_8 ) else NextStage();
|
|
}
|
|
|
|
else
|
|
if ( nav_stage == 1 ) {
|
|
nav_stage = 1;
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 1 ].a;
|
|
pre_climb = 0.;
|
|
Circle( 1, 100 );
|
|
if ( Qdr( 0 ) ) NextStage();
|
|
return;
|
|
} else
|
|
if ( nav_stage == 2 ) {
|
|
nav_stage = 2;
|
|
auto_pitch = FALSE;
|
|
nav_pitch = 0.000000;
|
|
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
|
desired_altitude = waypoints[ 4 ].a;
|
|
pre_climb = 0.;
|
|
Circle( 4, 100 );
|
|
if ( Qdr( 180 ) ) NextStage();
|
|
return;
|
|
} else
|
|
if ( nav_stage == 3 ) {
|
|
nav_stage = 3;
|
|
Goto( while_7 )
|
|
} else
|
|
if ( nav_stage == 4 ) {
|
|
Label( endwhile_8 )
|
|
nav_stage = 4;
|
|
NextBlock()
|
|
} else { }
|
|
} else { }
|
|
|
|
//#endif
|
|
}
|
|
#endif // NAV_C
|
|
|
|
#endif // FLIGHT_PLAN_H
|