bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
float alt = WaypointAlt(_climb);
nav_catapult_armed = 1;
// No Roll, Climb Pitch, No motor Phase
if (nav_catapult_launch <= nav_catapult_motor_delay)
{
NavAttitude(RadOfDeg(0));
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);
NavVerticalThrottleMode(9600*(0));
// Store take-off waypoint
WaypointX(_to) = GetPosX();
WaypointY(_to) = GetPosY();
WaypointAlt(_to) = GetPosAlt();
nav_catapult_x = stateGetPositionEnu_f()->x;
nav_catapult_y = stateGetPositionEnu_f()->y;
}
// No Roll, Climb Pitch, Full Power
else if (nav_catapult_launch < nav_catapult_heading_delay)
{
NavAttitude(RadOfDeg(0));
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);
NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle));
}
// Normal Climb: Heading Locked by Waypoint Target
else if (nav_catapult_launch == 0xffff)
{
NavVerticalAltitudeMode(alt, 0); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavGotoWaypoint(_climb); // horizontal mode (stay on localiser)
}
else
{
// Store Heading, move Climb
nav_catapult_launch = 0xffff;
float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;
float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);
WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;
DownlinkSendWp(DefaultChannel, DefaultDevice, _climb);
}
return TRUE;
} // end of gls()
bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
float alt = WaypointAlt(_climb);
nav_catapult_armed = 1;
// No Roll, Climb Pitch, No motor Phase 零滚转 府仰爬行 没有电机阶段
if (nav_catapult_launch <= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
{
NavAttitude(RadOfDeg(0)); //高度设置
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch); //自动油门模式
NavVerticalThrottleMode(9600*(0)); //设定油门
// Store take-off waypoint 存储起飞点
WaypointX(_to) = GetPosX(); //获得ｘ坐标
WaypointY(_to) = GetPosY(); //获得ｙ坐标
WaypointAlt(_to) = GetPosAlt(); //获得高度
nav_catapult_x = stateGetPositionEnu_f()->x; //起飞点ｘ坐标
nav_catapult_y = stateGetPositionEnu_f()->y; //起飞点ｙ坐标
}
// No Roll, Climb Pitch, Full Power 零滚转 府仰爬行 满油门
else if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
{
NavAttitude(RadOfDeg(0)); //高度设置
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch); //自动油门模式
NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle)); //设定油门
}
// Normal Climb: Heading Locked by Waypoint Target
// 正常爬行：锁定给定航点
else if (nav_catapult_launch == 0xffff)
{
NavVerticalAltitudeMode(alt, 0); // vertical mode (folow glideslope) 水平模式（跟随滑坡）
NavVerticalAutoThrottleMode(0); // throttle mode 油门模式
NavGotoWaypoint(_climb); // horizontal mode (stay on localiser) 垂直模式（保持定位）
}
else
{
// Store Heading, move Climb
nav_catapult_launch = 0xffff;
float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;
float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);
WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;
DownlinkSendWp(DefaultChannel, DefaultDevice, _climb);
}
return TRUE;
}
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 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;
}
