Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Make mode takeoff entry climb to TKOFF_ALT before loitering

This commit is contained in:
Henry Wurzburg 2023-05-09 07:16:35 -05:00 committed by Randy Mackay
parent ea9e051220
commit 23d9d3fd40
1 changed files with 51 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
ArduPlane/mode_takeoff.cpp
View File

@ -68,58 +68,72 @@ bool ModeTakeoff::_enter()
void ModeTakeoff::update()
{
if (!takeoff_started) {
// see if we will skip takeoff as already flying
if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling");
plane.prev_WP_loc = plane.current_loc;
plane.next_WP_loc = plane.current_loc;
takeoff_started = true;
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
}
// don't setup waypoints if we dont have a valid position and home!
if (!(plane.current_loc.initialised() && AP::ahrs().home_is_set())) {
plane.calc_nav_roll();
plane.calc_nav_pitch();
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
return;
}
const float alt = target_alt;
const float dist = target_dist;
if (!takeoff_started) {
// setup target location 1.5 times loiter radius from the
// takeoff point, at a height of TKOFF_ALT
const float dist = target_dist;
const float alt = target_alt;
const uint16_t altitude = plane.relative_ground_altitude(false,true);
const float direction = degrees(ahrs.yaw);
// see if we will skip takeoff as already flying
if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) {
if (altitude >= alt) {
gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering");
plane.next_WP_loc = plane.current_loc;
takeoff_started = true;
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Climbing to TKOFF alt then loitering");
plane.next_WP_loc = plane.current_loc;
plane.next_WP_loc.alt += ((alt - altitude) *100);
plane.next_WP_loc.offset_bearing(direction, dist);
takeoff_started = true;
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
}
// not flying so do a full takeoff sequence
} else {
// setup target waypoint and alt for loiter at dist and alt from start
start_loc = plane.current_loc;
plane.prev_WP_loc = plane.current_loc;
plane.next_WP_loc = plane.current_loc;
plane.next_WP_loc.alt += alt*100.0;
plane.next_WP_loc.offset_bearing(direction, dist);
start_loc = plane.current_loc;
plane.prev_WP_loc = plane.current_loc;
plane.next_WP_loc = plane.current_loc;
plane.next_WP_loc.alt += alt*100.0;
plane.next_WP_loc.offset_bearing(direction, dist);
plane.crash_state.is_crashed = false;
plane.crash_state.is_crashed = false;
plane.auto_state.takeoff_pitch_cd = level_pitch * 100;
plane.auto_state.takeoff_pitch_cd = level_pitch * 100;
plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
if (!plane.throttle_suppressed) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm at %.1fm to %.1f deg",
alt, dist, direction);
takeoff_started = true;
if (!plane.throttle_suppressed) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
alt, dist, direction);
takeoff_started = true;
}
}
}
// we finish the initial level takeoff if we climb past
// TKOFF_LVL_ALT or we pass the target location. The check for
// target location prevents us flying forever if we can't climb
// reset the loiter waypoint target to be correct bearing and dist
// from starting location in case original yaw used to set it was off due to EKF
// reset or compass interference from max throttle
if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF &&
(plane.current_loc.alt - start_loc.alt >= level_alt*100 ||
start_loc.get_distance(plane.current_loc) >= target_dist)) {
// reached level alt, re-calculate bearing to cope with systems with no compass
// or with poor initial compass
float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01;
float dist_done = start_loc.get_distance(plane.current_loc);
const float dist = target_dist;
plane.next_WP_loc = plane.current_loc;
plane.next_WP_loc.offset_bearing(direction, MAX(dist-dist_done, 0));
plane.next_WP_loc.alt = start_loc.alt + target_alt*100.0;
start_loc.get_distance(plane.current_loc) >= dist)) {
// reset the target loiter waypoint using current yaw which should be close to correct starting heading
const float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01;
plane.next_WP_loc = start_loc;
plane.next_WP_loc.offset_bearing(direction, dist);
plane.next_WP_loc.alt += alt*100.0;
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);