2019-05-04 06:56:07 -03:00
# include "mode.h"
# include "Plane.h"
2023-08-08 10:24:11 -03:00
# include <GCS_MAVLink/GCS.h>
2019-05-04 06:56:07 -03:00
/*
mode takeoff parameters
*/
const AP_Param : : GroupInfo ModeTakeoff : : var_info [ ] = {
// @Param: ALT
// @DisplayName: Takeoff mode altitude
// @Description: This is the target altitude for TAKEOFF mode
// @Range: 0 200
// @Increment: 1
// @Units: m
2020-09-13 19:42:54 -03:00
// @User: Standard
2019-05-04 06:56:07 -03:00
AP_GROUPINFO ( " ALT " , 1 , ModeTakeoff , target_alt , 50 ) ,
// @Param: LVL_ALT
// @DisplayName: Takeoff mode altitude level altitude
2023-12-19 13:24:08 -04:00
// @Description: This is the altitude below which the wings are held level for TAKEOFF and AUTO modes. Below this altitude, roll demand is restricted to LEVEL_ROLL_LIMIT. Normal-flight roll restriction resumes above TKOFF_LVL_ALT*3 or TKOFF_ALT, whichever is lower. Roll limits are scaled while between TKOFF_LVL_ALT and those altitudes for a smooth transition.
2019-05-04 06:56:07 -03:00
// @Range: 0 50
// @Increment: 1
// @Units: m
2020-09-13 19:42:54 -03:00
// @User: Standard
2024-05-24 06:38:42 -03:00
AP_GROUPINFO ( " LVL_ALT " , 2 , ModeTakeoff , level_alt , 10 ) ,
2019-05-04 06:56:07 -03:00
// @Param: LVL_PITCH
// @DisplayName: Takeoff mode altitude initial pitch
2024-05-24 06:38:42 -03:00
// @Description: This is the target pitch during the takeoff.
2019-05-04 06:56:07 -03:00
// @Range: 0 30
// @Increment: 1
// @Units: deg
2020-09-13 19:42:54 -03:00
// @User: Standard
2019-05-04 06:56:07 -03:00
AP_GROUPINFO ( " LVL_PITCH " , 3 , ModeTakeoff , level_pitch , 15 ) ,
// @Param: DIST
// @DisplayName: Takeoff mode distance
2023-09-06 22:37:48 -03:00
// @Description: This is the distance from the takeoff location where the plane will loiter. The loiter point will be in the direction of takeoff (the direction the plane is facing when the plane begins takeoff)
2019-05-04 06:56:07 -03:00
// @Range: 0 500
// @Increment: 1
// @Units: m
2020-09-13 19:42:54 -03:00
// @User: Standard
2019-05-04 06:56:07 -03:00
AP_GROUPINFO ( " DIST " , 4 , ModeTakeoff , target_dist , 200 ) ,
2023-05-09 09:16:35 -03:00
2021-02-19 21:00:38 -04:00
// @Param: GND_PITCH
// @DisplayName: Takeoff run pitch demand
// @Description: Degrees of pitch angle demanded during the takeoff run before speed reaches TKOFF_ROTATE_SPD. For taildraggers set to 3-point ground pitch angle and use TKOFF_TDRAG_ELEV to prevent nose tipover. For nose-wheel steer aircraft set to the ground pitch angle and if a reduction in nose-wheel load is required as speed rises, use a positive offset in TKOFF_GND_PITCH of up to 5 degrees above the angle on ground, starting at the mesured pitch angle and incrementing in 1 degree steps whilst checking for premature rotation and takeoff with each increment. To increase nose-wheel load, use a negative TKOFF_TDRAG_ELEV and refer to notes on TKOFF_TDRAG_ELEV before making adjustments.
// @Units: deg
// @Range: -5.0 10.0
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO ( " GND_PITCH " , 5 , ModeTakeoff , ground_pitch , 5 ) ,
2019-05-04 06:56:07 -03:00
AP_GROUPEND
} ;
2021-07-24 11:04:10 -03:00
ModeTakeoff : : ModeTakeoff ( ) :
Mode ( )
2019-05-04 06:56:07 -03:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
}
bool ModeTakeoff : : _enter ( )
{
2024-01-17 13:39:20 -04:00
takeoff_mode_setup = false ;
2024-02-16 16:54:56 -04:00
plane . have_autoenabled_fences = false ;
2019-05-04 06:56:07 -03:00
return true ;
}
void ModeTakeoff : : update ( )
{
2023-05-09 09:16:35 -03:00
// 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 ;
2024-01-17 13:39:20 -04:00
if ( ! takeoff_mode_setup ) {
2023-05-09 09:16:35 -03:00
const uint16_t altitude = plane . relative_ground_altitude ( false , true ) ;
2024-01-12 08:40:24 -04:00
const float direction = degrees ( ahrs . get_yaw ( ) ) ;
2019-10-09 23:17:36 -03:00
// see if we will skip takeoff as already flying
2023-02-12 09:33:27 -04:00
if ( plane . is_flying ( ) & & ( millis ( ) - plane . started_flying_ms > 10000U ) & & ahrs . groundspeed ( ) > 3 ) {
2023-05-09 09:16:35 -03:00
if ( altitude > = alt ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Above TKOFF alt - loitering " ) ;
plane . next_WP_loc = plane . current_loc ;
2024-01-17 13:39:20 -04:00
takeoff_mode_setup = true ;
2023-05-09 09:16:35 -03:00
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 ) ;
2024-01-17 13:39:20 -04:00
takeoff_mode_setup = true ;
2023-05-09 09:16:35 -03:00
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 ;
2019-10-09 23:17:36 -03:00
plane . prev_WP_loc = plane . current_loc ;
plane . next_WP_loc = plane . current_loc ;
2023-05-09 09:16:35 -03:00
plane . next_WP_loc . alt + = alt * 100.0 ;
plane . next_WP_loc . offset_bearing ( direction , dist ) ;
2019-05-04 06:56:07 -03:00
2023-05-09 09:16:35 -03:00
plane . crash_state . is_crashed = false ;
2019-05-04 06:56:07 -03:00
2023-05-09 09:16:35 -03:00
plane . auto_state . takeoff_pitch_cd = level_pitch * 100 ;
2019-05-04 06:56:07 -03:00
2023-05-09 09:16:35 -03:00
plane . set_flight_stage ( AP_FixedWing : : FlightStage : : TAKEOFF ) ;
2019-05-04 06:56:07 -03:00
2023-05-09 09:16:35 -03:00
if ( ! plane . throttle_suppressed ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Takeoff to %.0fm for %.1fm heading %.1f deg " ,
alt , dist , direction ) ;
2024-01-17 13:39:20 -04:00
plane . takeoff_state . start_time_ms = millis ( ) ;
takeoff_mode_setup = true ;
2023-05-09 09:16:35 -03:00
}
2019-05-04 06:56:07 -03:00
}
}
2024-01-17 13:39:20 -04:00
// check for optional takeoff timeout
if ( plane . check_takeoff_timeout ( ) ) {
plane . set_flight_stage ( AP_FixedWing : : FlightStage : : NORMAL ) ;
takeoff_mode_setup = false ;
2019-05-04 06:56:07 -03:00
2024-01-17 13:39:20 -04:00
}
2019-05-04 06:56:07 -03:00
// 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
2023-05-09 09:16:35 -03:00
// 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
2023-12-19 15:42:24 -04:00
const float altitude_cm = plane . current_loc . alt - start_loc . alt ;
2022-09-29 20:10:41 -03:00
if ( plane . flight_stage = = AP_FixedWing : : FlightStage : : TAKEOFF & &
2023-12-19 15:42:24 -04:00
( altitude_cm > = level_alt * 100 | |
2023-05-09 09:16:35 -03:00
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 ;
2022-09-29 20:10:41 -03:00
plane . set_flight_stage ( AP_FixedWing : : FlightStage : : NORMAL ) ;
2019-05-04 06:56:07 -03:00
}
2022-09-29 20:10:41 -03:00
if ( plane . flight_stage = = AP_FixedWing : : FlightStage : : TAKEOFF ) {
2023-12-19 15:42:24 -04:00
//below TAKOFF_LVL_ALT
2019-05-04 06:56:07 -03:00
plane . takeoff_calc_roll ( ) ;
plane . takeoff_calc_pitch ( ) ;
2024-05-24 06:38:42 -03:00
plane . takeoff_calc_throttle ( true ) ;
2019-05-04 06:56:07 -03:00
} else {
2024-05-24 06:38:42 -03:00
if ( ( altitude_cm > = alt * 100 - 200 ) ) { //within 2m of TKOFF_ALT, or above and loitering
2023-12-19 15:42:24 -04:00
# if AP_FENCE_ENABLED
2024-02-16 16:54:56 -04:00
if ( ! plane . have_autoenabled_fences ) {
plane . fence . auto_enable_fence_after_takeoff ( ) ;
plane . have_autoenabled_fences = true ;
}
2023-12-19 15:42:24 -04:00
# endif
plane . calc_nav_roll ( ) ;
plane . calc_nav_pitch ( ) ;
plane . calc_throttle ( ) ;
} else { // still climbing to TAKEOFF_ALT; may be loitering
2024-05-24 06:38:42 -03:00
plane . takeoff_calc_throttle ( ) ;
2023-12-19 15:42:24 -04:00
plane . takeoff_calc_roll ( ) ;
plane . takeoff_calc_pitch ( ) ;
}
//check if in long failsafe due to being in initial TAKEOFF stage; if it is, recall long failsafe now to get fs action via events call
2023-08-08 10:24:11 -03:00
if ( plane . long_failsafe_pending ) {
2023-12-19 15:42:24 -04:00
plane . long_failsafe_pending = false ;
plane . failsafe_long_on_event ( FAILSAFE_LONG , ModeReason : : MODE_TAKEOFF_FAILSAFE ) ;
2023-08-08 10:24:11 -03:00
}
2019-05-04 06:56:07 -03:00
}
}
2020-08-19 04:51:00 -03:00
void ModeTakeoff : : navigate ( )
2020-07-29 12:01:34 -03:00
{
2020-08-11 07:54:28 -03:00
// Zero indicates to use WP_LOITER_RAD
plane . update_loiter ( 0 ) ;
2020-07-29 12:01:34 -03:00
}