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; }