Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane:add TKOFF_TIMEOUT to MODE TAKEOFF

Browse Source
This commit is contained in:
Henry Wurzburg 2024-01-17 11:39:20 -06:00 committed by Andrew Tridgell
parent 4bcbb13f79
commit 3973c28f15
5 changed files with 43 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -1090,6 +1090,7 @@ private:
int8_t takeoff_tail_hold(void);
int16_t get_takeoff_pitch_min_cd(void);
void landing_gear_update(void);
bool check_takeoff_timeout(void);
// avoidance_adsb.cpp
void avoidance_adsb_update(void);

18
ArduPlane/commands_logic.cpp
View File

@ -595,22 +595,8 @@ bool Plane::verify_takeoff()
}
// check for optional takeoff timeout
if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) {
const float ground_speed = gps.ground_speed();
const float takeoff_min_ground_speed = 4;
if (!arming.is_armed_and_safety_off()) {
return false;
}
if (ground_speed >= takeoff_min_ground_speed) {
takeoff_state.start_time_ms = 0;
} else {
uint32_t now = AP_HAL::millis();
if (now - takeoff_state.start_time_ms > (uint32_t)(1000U * g2.takeoff_timeout)) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff timeout at %.1f m/s", ground_speed);
plane.arming.disarm(AP_Arming::Method::TAKEOFFTIMEOUT);
mission.reset();
}
}
if (plane.check_takeoff_timeout()) {
mission.reset();
}
// see if we have reached takeoff altitude

2
ArduPlane/mode.h
View File

@ -788,7 +788,7 @@ protected:
AP_Int16 target_dist;
AP_Int8 level_pitch;
bool takeoff_started;
bool takeoff_mode_setup;
Location start_loc;
bool _enter() override;

19
ArduPlane/mode_takeoff.cpp
View File

@ -62,7 +62,7 @@ ModeTakeoff::ModeTakeoff() :
bool ModeTakeoff::_enter()
{
takeoff_started = false;
takeoff_mode_setup = false;
return true;
}
@ -79,7 +79,7 @@ void ModeTakeoff::update()
const float alt = target_alt;
const float dist = target_dist;
if (!takeoff_started) {
if (!takeoff_mode_setup) {
const uint16_t altitude = plane.relative_ground_altitude(false,true);
const float direction = degrees(ahrs.get_yaw());
// see if we will skip takeoff as already flying
@ -87,20 +87,19 @@ void ModeTakeoff::update()
if (altitude >= alt) {
gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering");
plane.next_WP_loc = plane.current_loc;
takeoff_started = true;
takeoff_mode_setup = 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;
takeoff_mode_setup = 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;
@ -116,11 +115,19 @@ void ModeTakeoff::update()
if (!plane.throttle_suppressed) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
alt, dist, direction);
takeoff_started = true;
plane.takeoff_state.start_time_ms = millis();
takeoff_mode_setup = true;
}
}
}
// check for optional takeoff timeout
if (plane.check_takeoff_timeout()) {
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
takeoff_mode_setup = false;
}
// 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

26
ArduPlane/takeoff.cpp
View File

@ -305,3 +305,29 @@ void Plane::landing_gear_update(void)
g2.landing_gear.update(relative_ground_altitude(g.rangefinder_landing));
}
#endif
/*
check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred and disarms on timeout
*/
bool Plane::check_takeoff_timeout(void)
{
if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) {
const float ground_speed = AP::gps().ground_speed();
const float takeoff_min_ground_speed = 4;
if (ground_speed >= takeoff_min_ground_speed) {
takeoff_state.start_time_ms = 0;
return false;
} else {
uint32_t now = AP_HAL::millis();
if (now - takeoff_state.start_time_ms > (uint32_t)(1000U * g2.takeoff_timeout)) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff timeout: %.1f m/s speed < 4m/s", ground_speed);
arming.disarm(AP_Arming::Method::TAKEOFFTIMEOUT);
takeoff_state.start_time_ms = 0;
return true;
}
}
}
return false;
}