bool nav_launcher_run(void)
{
//Find distance from laucher
float dist_x = stateGetPositionEnu_f()->x - launch_x;
float dist_y = stateGetPositionEnu_f()->y - launch_y;
float launch_dist = sqrtf(dist_x * dist_x + dist_y * dist_y);
if (launch_dist <= 0.01) {
launch_dist = 0.01;
}
switch (CLaunch_Status) {
case L_Pitch_Nav:
//Follow Launch Line
NavVerticalAltitudeMode(launch_alt, 0);
NavVerticalAutoThrottleMode(LAUNCHER_TAKEOFF_PITCH);
NavVerticalThrottleMode(MAX_PPRZ * (1));
NavAttitude(0);
kill_throttle = 0;
//If the plane has been launched and has traveled for more than a specified distance, switch to line nav
if (stateGetHorizontalSpeedNorm_f() > LAUNCHER_TAKEOFF_MIN_SPEED_LINE) {
if (launch_dist > LAUNCHER_TAKEOFF_DISTANCE) {
launch_line_x = stateGetPositionEnu_f()->x;
launch_line_y = stateGetPositionEnu_f()->y;
CLaunch_Status = L_Line_Nav;
}
}
break;
case L_Line_Nav:
//Follow Launch Line
NavVerticalAltitudeMode(launch_alt, 0);
NavVerticalAutoThrottleMode(LAUNCHER_TAKEOFF_PITCH);
NavVerticalThrottleMode(MAX_PPRZ * (1));
nav_route_xy(launch_x, launch_y, launch_line_x, launch_line_y);
kill_throttle = 0;
//If the aircraft is above a specific alt, greater than a specific speed or too far away, circle up
if (((stateGetPositionUtm_f()->alt
> (launch_alt - LAUNCHER_TAKEOFF_HEIGHT_THRESHOLD))
&& (stateGetHorizontalSpeedNorm_f()
> LAUNCHER_TAKEOFF_MIN_SPEED_CIRCLE))
|| (launch_dist > LAUNCHER_TAKEOFF_MAX_CIRCLE_DISTANCE)) {
CLaunch_Status = L_CircleUp;
//Find position of circle
float x_1 = dist_x / launch_dist;
float y_1 = dist_y / launch_dist;
launch_circle.x = stateGetPositionEnu_f()->x
+ y_1 * LAUNCHER_TAKEOFF_CIRCLE_RADIUS;
launch_circle.y = stateGetPositionEnu_f()->y
- x_1 * LAUNCHER_TAKEOFF_CIRCLE_RADIUS;
}
break;
case L_CircleUp:
NavVerticalAutoThrottleMode(0);
NavVerticalAltitudeMode(launch_circle_alt, 0);
nav_circle_XY(launch_circle.x, launch_circle.y,
LAUNCHER_TAKEOFF_CIRCLE_RADIUS);
if (stateGetPositionUtm_f()->alt
> (launch_circle_alt - LAUNCHER_TAKEOFF_HEIGHT_THRESHOLD)) {
CLaunch_Status = L_Finished;
return FALSE;
}
break;
default:
break;
}
return TRUE;
}
bool_t BungeeTakeoff(void)
{
//Translate current position so Throttle point is (0,0)
float Currentx = estimator_x-throttlePx;
float Currenty = estimator_y-throttlePy;
bool_t CurrentAboveLine;
float ThrottleB;
switch(CTakeoffStatus)
{
case Launch:
//Follow Launch Line
NavVerticalAutoThrottleMode(0); //Set the climb control to auto-throttle with the specified pitch pre-command (navigation.h) -> No Pitch
NavVerticalAltitudeMode(BungeeAlt+Takeoff_Height, 0.); //Vorgabe der Sollhöhe
nav_route_xy(initialx,initialy,throttlePx,throttlePy); //Vorgabe der Route durch Methodenaufruf
kill_throttle = 1; //Motor ausgeschaltet
//recalculate lines if below min speed
if(estimator_hspeed_mod < Takeoff_MinSpeed)
{
initialx = estimator_x;
initialy = estimator_y;
//Translate initial position so that the position of the bungee is (0,0)
Currentx = initialx-(waypoints[BungeeWaypoint].x);
Currenty = initialy-(waypoints[BungeeWaypoint].y);
//Find Launch line slope
float MLaunch = Currenty/Currentx;
//Find Throttle Point (the point where the throttle line and launch line intersect)
if(Currentx < 0)
throttlePx = TDistance/sqrt(MLaunch*MLaunch+1);
else
throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1));
if(Currenty < 0)
throttlePy = sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
else
throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
//Find ThrottleLine
ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2));
ThrottleB = (throttlePy - (ThrottleSlope*throttlePx));
//Determine whether the UAV is below or above the throttle line
if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB))
AboveLine = TRUE;
else
AboveLine = FALSE;
//Translate the throttle point back
throttlePx = throttlePx+(waypoints[BungeeWaypoint].x);
throttlePy = throttlePy+(waypoints[BungeeWaypoint].y);
}
//Find out if the UAV is currently above the line
if(Currenty > (ThrottleSlope*Currentx))
CurrentAboveLine = TRUE;
else
CurrentAboveLine = FALSE;
//Find out if UAV has crossed the line
if(AboveLine != CurrentAboveLine && estimator_hspeed_mod > Takeoff_MinSpeed)
{
CTakeoffStatus = Throttle;
kill_throttle = 0;
nav_init_stage();
}
break;
case Throttle:
//Follow Launch Line
NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH);
NavVerticalThrottleMode(9600*(1));
nav_route_xy(initialx,initialy,throttlePx,throttlePy);
kill_throttle = 0;
if((estimator_z > BungeeAlt+Takeoff_Height-10) && (estimator_hspeed_mod > Takeoff_Speed))
{
CTakeoffStatus = Finished;
return FALSE;
}
else
{
return TRUE;
}
break;
default:
break;
}
return TRUE;
}
bool_t nav_bungee_takeoff_run(void)
{
//Translate current position so Throttle point is (0,0)
float Currentx = stateGetPositionEnu_f()->x-throttlePx;
float Currenty = stateGetPositionEnu_f()->y-throttlePy;
bool_t CurrentAboveLine;
float ThrottleB;
switch(CTakeoffStatus)
{
case Launch:
//Follow Launch Line
NavVerticalAutoThrottleMode(0);
NavVerticalAltitudeMode(BungeeAlt+Takeoff_Height, 0.);
nav_route_xy(initialx,initialy,throttlePx,throttlePy);
kill_throttle = 1;
//recalculate lines if below min speed
if((*stateGetHorizontalSpeedNorm_f()) < Takeoff_MinSpeed)
{
initialx = stateGetPositionEnu_f()->x;
initialy = stateGetPositionEnu_f()->y;
//Translate initial position so that the position of the bungee is (0,0)
Currentx = initialx-(WaypointX(BungeeWaypoint));
Currenty = initialy-(WaypointY(BungeeWaypoint));
//Find Launch line slope
float MLaunch = Currenty/Currentx;
//Find Throttle Point (the point where the throttle line and launch line intersect)
if(Currentx < 0)
throttlePx = TDistance/sqrt(MLaunch*MLaunch+1);
else
throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1));
if(Currenty < 0)
throttlePy = sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
else
throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
//Find ThrottleLine
ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2));
ThrottleB = (throttlePy - (ThrottleSlope*throttlePx));
//Determine whether the UAV is below or above the throttle line
if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB))
AboveLine = TRUE;
else
AboveLine = FALSE;
//Translate the throttle point back
throttlePx = throttlePx+(WaypointX(BungeeWaypoint));
throttlePy = throttlePy+(WaypointY(BungeeWaypoint));
}
//Find out if the UAV is currently above the line
if(Currenty > (ThrottleSlope*Currentx))
CurrentAboveLine = TRUE;
else
CurrentAboveLine = FALSE;
//Find out if UAV has crossed the line
if(AboveLine != CurrentAboveLine && (*stateGetHorizontalSpeedNorm_f()) > Takeoff_MinSpeed)
{
CTakeoffStatus = Throttle;
kill_throttle = 0;
nav_init_stage();
}
break;
case Throttle:
//Follow Launch Line
NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH);
NavVerticalThrottleMode(9600*(1));
nav_route_xy(initialx,initialy,throttlePx,throttlePy);
kill_throttle = 0;
if((stateGetPositionUtm_f()->alt > BungeeAlt+Takeoff_Height-10) && ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed))
{
CTakeoffStatus = Finished;
return FALSE;
}
else
{
return TRUE;
}
break;
default:
break;
}
return TRUE;
}
bool nav_catapult_run(uint8_t _climb)
{
switch (nav_catapult.status) {
case NAV_CATAPULT_UNINIT:
// start high freq function if not done
if (nav_catapult_nav_catapult_highrate_module_status != MODULES_RUN) {
nav_catapult_nav_catapult_highrate_module_status = MODULES_START;
}
// arm catapult
nav_catapult.status = NAV_CATAPULT_ARMED;
break;
case NAV_CATAPULT_ARMED:
// store initial position
nav_catapult.pos.x = stateGetPositionEnu_f()->x;
nav_catapult.pos.y = stateGetPositionEnu_f()->y;
nav_catapult.pos.z = stateGetPositionUtm_f()->alt; // useful ?
nav_catapult.status = NAV_CATAPULT_WAIT_ACCEL;
break;
case NAV_CATAPULT_WAIT_ACCEL:
// no throttle, zero attitude
NavAttitude(RadOfDeg(0.f));
NavVerticalAutoThrottleMode(nav_catapult.initial_pitch);
NavVerticalThrottleMode(0.f);
// wait for acceleration from high speed function
break;
case NAV_CATAPULT_MOTOR_ON:
// fixed attitude and motor
NavAttitude(RadOfDeg(0.f));
NavVerticalAutoThrottleMode(nav_catapult.initial_pitch);
NavVerticalThrottleMode(MAX_PPRZ * nav_catapult.initial_throttle);
if (nav_catapult.timer >= nav_catapult.heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
// store heading, move climb waypoint
float dir_x = stateGetPositionEnu_f()->x - nav_catapult.pos.x;
float dir_y = stateGetPositionEnu_f()->y - nav_catapult.pos.y;
float dir_L = sqrtf(dir_x * dir_x + dir_y * dir_y);
WaypointX(_climb) = nav_catapult.pos.x + (dir_x / dir_L) * NAV_CATAPULT_CLIMB_DISTANCE;
WaypointY(_climb) = nav_catapult.pos.y + (dir_y / dir_L) * NAV_CATAPULT_CLIMB_DISTANCE;
DownlinkSendWpNr(_climb);
// next step
nav_catapult.status = NAV_CATAPULT_MOTOR_CLIMB;
}
break;
case NAV_CATAPULT_MOTOR_CLIMB:
// normal climb: heading locked by waypoint target
NavVerticalAltitudeMode(WaypointAlt(_climb), 0.f); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0.f); // throttle mode
NavGotoWaypoint(_climb); // horizontal mode (stay on localiser)
if (nav_approaching_xy(WaypointX(_climb), WaypointY(_climb), nav_catapult.pos.x, nav_catapult.pos.y, 0.f)) {
// reaching climb waypoint, end procedure
nav_catapult.status = NAV_CATAPULT_DISARM;
}
break;
case NAV_CATAPULT_DISARM:
// end procedure
nav_catapult.status = NAV_CATAPULT_UNINIT;
nav_catapult_nav_catapult_highrate_module_status = MODULES_STOP;
return false;
default:
return false;
}
// procedure still running
return true;
}
