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

56
ArduPlane/mode_takeoff.cpp
View File

@ -68,23 +68,37 @@ bool ModeTakeoff::_enter()
void ModeTakeoff::update() void ModeTakeoff::update()
{ {
// 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) { if (!takeoff_started) {
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 // see if we will skip takeoff as already flying
if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) { if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling"); if (altitude >= alt) {
plane.prev_WP_loc = plane.current_loc; gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering");
plane.next_WP_loc = plane.current_loc; plane.next_WP_loc = plane.current_loc;
takeoff_started = true; takeoff_started = true;
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); 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 {
if (!takeoff_started) { // setup target waypoint and alt for loiter at dist and alt from start
// 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 float direction = degrees(ahrs.yaw);
start_loc = plane.current_loc; start_loc = plane.current_loc;
plane.prev_WP_loc = plane.current_loc; plane.prev_WP_loc = plane.current_loc;
@ -99,27 +113,27 @@ void ModeTakeoff::update()
plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF); plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
if (!plane.throttle_suppressed) { if (!plane.throttle_suppressed) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm at %.1fm to %.1f deg", gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
alt, dist, direction); alt, dist, direction);
takeoff_started = true; takeoff_started = true;
} }
} }
}
// we finish the initial level takeoff if we climb past // we finish the initial level takeoff if we climb past
// TKOFF_LVL_ALT or we pass the target location. The check for // TKOFF_LVL_ALT or we pass the target location. The check for
// target location prevents us flying forever if we can't climb // 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 && if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF &&
(plane.current_loc.alt - start_loc.alt >= level_alt*100 || (plane.current_loc.alt - start_loc.alt >= level_alt*100 ||
start_loc.get_distance(plane.current_loc) >= target_dist)) { start_loc.get_distance(plane.current_loc) >= dist)) {
// reached level alt, re-calculate bearing to cope with systems with no compass // reset the target loiter waypoint using current yaw which should be close to correct starting heading
// or with poor initial compass const float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01;
float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01; plane.next_WP_loc = start_loc;
float dist_done = start_loc.get_distance(plane.current_loc); plane.next_WP_loc.offset_bearing(direction, dist);
const float dist = target_dist; plane.next_WP_loc.alt += alt*100.0;
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;
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);