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

Plane: fixed takeoff direction with no yaw source 

in TAKEOFF mode with either very poor yaw source or no yaw source we
need to use ground vector and wait for sufficient ground speed
This commit is contained in:
Andrew Tridgell 2024-11-25 14:53:26 +11:00
parent 4f7a1fc81c
commit c15fa7b943
1 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduPlane/mode_takeoff.cpp
View File

@ -83,7 +83,10 @@ void ModeTakeoff::update()
if (!takeoff_mode_setup) {
plane.auto_state.takeoff_altitude_rel_cm = alt * 100;
const uint16_t altitude = plane.relative_ground_altitude(false,true);
const float direction = degrees(ahrs.get_yaw());
const Vector2f &groundspeed2d = ahrs.groundspeed_vector();
const float direction = wrap_360(degrees(groundspeed2d.angle()));
const float groundspeed = groundspeed2d.length();
// 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) {
@ -115,7 +118,15 @@ void ModeTakeoff::update()
plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
if (!plane.throttle_suppressed) {
/*
don't lock in the takeoff loiter location until we reach
a ground speed of AIRSPEED_MIN*0.3 to cope with aircraft
with no compass or poor compass. If flying in a very
strong headwind then the is_flying() check above will
eventually trigger
*/
if (!plane.throttle_suppressed &&
groundspeed > plane.aparm.airspeed_min*0.3) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
alt, dist, direction);
plane.takeoff_state.start_time_ms = millis();