2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
Mode::_TakeOff Mode::takeoff;
|
2017-12-12 06:34:49 -04:00
|
|
|
|
2019-11-28 09:19:17 -04:00
|
|
|
bool Mode::auto_takeoff_no_nav_active = false;
|
2019-11-15 16:28:08 -04:00
|
|
|
float Mode::auto_takeoff_no_nav_alt_cm = 0;
|
|
|
|
|
2015-04-29 23:52:19 -03:00
|
|
|
// This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes.
|
|
|
|
// The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude
|
|
|
|
// A safe takeoff speed is calculated and used to calculate a time_ms
|
|
|
|
// the pos_control target is then slowly increased until time_ms expires
|
2015-04-30 03:06:55 -03:00
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
bool Mode::do_user_takeoff_start(float takeoff_alt_cm)
|
2017-12-12 06:09:48 -04:00
|
|
|
{
|
2018-03-16 03:22:14 -03:00
|
|
|
copter.flightmode->takeoff.start(takeoff_alt_cm);
|
2017-12-12 06:09:48 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2015-04-29 23:52:19 -03:00
|
|
|
// initiate user takeoff - called when MAVLink TAKEOFF command is received
|
2019-05-09 23:18:49 -03:00
|
|
|
bool Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
|
2015-04-30 03:06:55 -03:00
|
|
|
{
|
2017-12-12 06:09:48 -04:00
|
|
|
if (!copter.motors->armed()) {
|
|
|
|
return false;
|
|
|
|
}
|
2019-05-09 23:18:49 -03:00
|
|
|
if (!copter.ap.land_complete) {
|
2017-12-12 06:09:48 -04:00
|
|
|
// can't takeoff again!
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (!has_user_takeoff(must_navigate)) {
|
|
|
|
// this mode doesn't support user takeoff
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (takeoff_alt_cm <= copter.current_loc.alt) {
|
|
|
|
// can't takeoff downwards...
|
|
|
|
return false;
|
|
|
|
}
|
2015-07-01 09:32:40 -03:00
|
|
|
|
2017-12-12 06:09:48 -04:00
|
|
|
// Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
|
2019-05-09 23:18:49 -03:00
|
|
|
if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && copter.ap.using_interlock) {
|
2017-12-12 06:09:48 -04:00
|
|
|
return false;
|
|
|
|
}
|
2015-07-01 09:32:40 -03:00
|
|
|
|
2017-12-12 06:09:48 -04:00
|
|
|
if (!do_user_takeoff_start(takeoff_alt_cm)) {
|
|
|
|
return false;
|
2015-04-30 03:06:55 -03:00
|
|
|
}
|
2017-12-12 06:09:48 -04:00
|
|
|
|
|
|
|
copter.set_auto_armed(true);
|
|
|
|
return true;
|
2015-04-30 03:06:55 -03:00
|
|
|
}
|
|
|
|
|
2015-06-21 23:59:43 -03:00
|
|
|
// start takeoff to specified altitude above home in centimeters
|
2019-05-09 23:18:49 -03:00
|
|
|
void Mode::_TakeOff::start(float alt_cm)
|
2015-04-30 03:06:55 -03:00
|
|
|
{
|
2019-04-08 03:37:41 -03:00
|
|
|
// indicate we are taking off
|
|
|
|
copter.set_land_complete(false);
|
|
|
|
// tell position controller to reset alt target and reset I terms
|
2020-06-16 03:38:36 -03:00
|
|
|
copter.flightmode->set_throttle_takeoff();
|
2019-04-08 03:37:41 -03:00
|
|
|
|
2015-04-29 23:52:19 -03:00
|
|
|
// calculate climb rate
|
2019-01-24 01:01:46 -04:00
|
|
|
const float speed = MIN(copter.wp_nav->get_default_speed_up(), MAX(copter.g.pilot_speed_up*2.0f/3.0f, copter.g.pilot_speed_up-50.0f));
|
2015-04-30 03:06:55 -03:00
|
|
|
|
2015-04-29 23:52:19 -03:00
|
|
|
// sanity check speed and target
|
2019-04-08 03:37:41 -03:00
|
|
|
if (speed <= 0.0f || alt_cm <= 0.0f) {
|
2015-04-30 03:06:55 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-04-29 23:52:19 -03:00
|
|
|
// initialise takeoff state
|
2018-03-16 03:22:14 -03:00
|
|
|
_running = true;
|
|
|
|
max_speed = speed;
|
|
|
|
start_ms = millis();
|
|
|
|
alt_delta = alt_cm;
|
2015-04-30 03:06:55 -03:00
|
|
|
}
|
|
|
|
|
2015-04-30 03:52:32 -03:00
|
|
|
// stop takeoff
|
2019-05-09 23:18:49 -03:00
|
|
|
void Mode::_TakeOff::stop()
|
2015-04-30 03:52:32 -03:00
|
|
|
{
|
2018-03-16 03:22:14 -03:00
|
|
|
_running = false;
|
|
|
|
start_ms = 0;
|
2015-04-30 03:52:32 -03:00
|
|
|
}
|
|
|
|
|
2015-04-30 04:40:38 -03:00
|
|
|
// returns pilot and takeoff climb rates
|
|
|
|
// pilot_climb_rate is both an input and an output
|
|
|
|
// takeoff_climb_rate is only an output
|
|
|
|
// has side-effect of turning takeoff off when timeout as expired
|
2019-05-09 23:18:49 -03:00
|
|
|
void Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate,
|
2018-03-16 03:22:14 -03:00
|
|
|
float& takeoff_climb_rate)
|
2015-04-30 03:06:55 -03:00
|
|
|
{
|
2015-04-30 04:40:38 -03:00
|
|
|
// return pilot_climb_rate if take-off inactive
|
2018-03-16 03:22:14 -03:00
|
|
|
if (!_running) {
|
2015-04-30 04:40:38 -03:00
|
|
|
takeoff_climb_rate = 0.0f;
|
|
|
|
return;
|
2015-04-30 03:06:55 -03:00
|
|
|
}
|
|
|
|
|
2015-11-04 02:25:19 -04:00
|
|
|
// acceleration of 50cm/s/s
|
2018-03-16 03:22:14 -03:00
|
|
|
static constexpr float TAKEOFF_ACCEL = 50.0f;
|
|
|
|
const float takeoff_minspeed = MIN(50.0f, max_speed);
|
|
|
|
const float time_elapsed = (millis() - start_ms) * 1.0e-3f;
|
|
|
|
const float speed = MIN(time_elapsed * TAKEOFF_ACCEL + takeoff_minspeed, max_speed);
|
2015-10-19 21:56:49 -03:00
|
|
|
|
2018-03-16 03:22:14 -03:00
|
|
|
const float time_to_max_speed = (max_speed - takeoff_minspeed) / TAKEOFF_ACCEL;
|
2015-10-19 21:56:49 -03:00
|
|
|
float height_gained;
|
|
|
|
if (time_elapsed <= time_to_max_speed) {
|
2018-03-16 03:22:14 -03:00
|
|
|
height_gained = 0.5f * TAKEOFF_ACCEL * sq(time_elapsed) + takeoff_minspeed * time_elapsed;
|
2015-10-19 21:56:49 -03:00
|
|
|
} else {
|
2018-03-16 03:22:14 -03:00
|
|
|
height_gained = 0.5f * TAKEOFF_ACCEL * sq(time_to_max_speed) + takeoff_minspeed * time_to_max_speed +
|
|
|
|
(time_elapsed - time_to_max_speed) * max_speed;
|
2015-10-19 21:56:49 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// check if the takeoff is over
|
2018-03-16 03:22:14 -03:00
|
|
|
if (height_gained >= alt_delta) {
|
|
|
|
stop();
|
2015-04-30 03:06:55 -03:00
|
|
|
}
|
|
|
|
|
2015-04-30 04:40:38 -03:00
|
|
|
// if takeoff climb rate is zero return
|
2015-10-19 21:56:49 -03:00
|
|
|
if (speed <= 0.0f) {
|
2015-04-30 04:40:38 -03:00
|
|
|
takeoff_climb_rate = 0.0f;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// default take-off climb rate to maximum speed
|
2015-10-19 21:56:49 -03:00
|
|
|
takeoff_climb_rate = speed;
|
2015-04-30 04:40:38 -03:00
|
|
|
|
|
|
|
// if pilot's commands descent
|
|
|
|
if (pilot_climb_rate < 0.0f) {
|
|
|
|
// if overall climb rate is still positive, move to take-off climb rate
|
|
|
|
if (takeoff_climb_rate + pilot_climb_rate > 0.0f) {
|
|
|
|
takeoff_climb_rate = takeoff_climb_rate + pilot_climb_rate;
|
2018-03-16 03:22:14 -03:00
|
|
|
pilot_climb_rate = 0.0f;
|
2015-04-30 04:40:38 -03:00
|
|
|
} else {
|
|
|
|
// if overall is negative, move to pilot climb rate
|
|
|
|
pilot_climb_rate = pilot_climb_rate + takeoff_climb_rate;
|
|
|
|
takeoff_climb_rate = 0.0f;
|
|
|
|
}
|
|
|
|
} else { // pilot commands climb
|
|
|
|
// pilot climb rate is zero until it surpasses the take-off climb rate
|
|
|
|
if (pilot_climb_rate > takeoff_climb_rate) {
|
|
|
|
pilot_climb_rate = pilot_climb_rate - takeoff_climb_rate;
|
|
|
|
} else {
|
|
|
|
pilot_climb_rate = 0.0f;
|
|
|
|
}
|
|
|
|
}
|
2015-04-30 03:06:55 -03:00
|
|
|
}
|
2016-06-04 23:37:55 -03:00
|
|
|
|
2019-11-15 16:28:08 -04:00
|
|
|
void Mode::auto_takeoff_run()
|
|
|
|
{
|
|
|
|
// if not armed set throttle to zero and exit immediately
|
|
|
|
if (!motors->armed() || !copter.ap.auto_armed) {
|
|
|
|
make_safe_spool_down();
|
|
|
|
wp_nav->shift_wp_origin_to_current_pos();
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2019-11-28 09:19:17 -04:00
|
|
|
// set motors to full range
|
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
|
|
|
|
2019-11-15 16:28:08 -04:00
|
|
|
// process pilot's yaw input
|
|
|
|
float target_yaw_rate = 0;
|
|
|
|
if (!copter.failsafe.radio) {
|
|
|
|
// get pilot's desired yaw rate
|
|
|
|
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
|
|
|
}
|
|
|
|
|
|
|
|
// aircraft stays in landed state until rotor speed runup has finished
|
|
|
|
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
|
|
|
|
set_land_complete(false);
|
|
|
|
} else {
|
2020-09-30 08:05:37 -03:00
|
|
|
// motors have not completed spool up yet so relax navigation and position controllers
|
2019-11-15 16:28:08 -04:00
|
|
|
wp_nav->shift_wp_origin_to_current_pos();
|
2020-09-30 08:05:37 -03:00
|
|
|
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
|
|
|
pos_control->update_z_controller();
|
|
|
|
attitude_control->set_yaw_target_to_current_heading();
|
|
|
|
attitude_control->reset_rate_controller_I_terms();
|
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
|
|
|
|
return;
|
2019-11-15 16:28:08 -04:00
|
|
|
}
|
|
|
|
|
2019-11-28 09:19:17 -04:00
|
|
|
// check if we are not navigating because of low altitude
|
|
|
|
float nav_roll = 0.0f, nav_pitch = 0.0f;
|
|
|
|
if (auto_takeoff_no_nav_active) {
|
|
|
|
// check if vehicle has reached no_nav_alt threshold
|
|
|
|
if (inertial_nav.get_altitude() >= auto_takeoff_no_nav_alt_cm) {
|
|
|
|
auto_takeoff_no_nav_active = false;
|
|
|
|
wp_nav->shift_wp_origin_and_destination_to_stopping_point_xy();
|
|
|
|
} else {
|
|
|
|
// shift the navigation target horizontally to our current position
|
|
|
|
wp_nav->shift_wp_origin_and_destination_to_current_pos_xy();
|
|
|
|
}
|
|
|
|
// tell the position controller that we have limited roll/pitch demand to prevent integrator buildup
|
|
|
|
pos_control->set_limit_accel_xy();
|
|
|
|
}
|
2019-11-15 16:28:08 -04:00
|
|
|
|
|
|
|
// run waypoint controller
|
|
|
|
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
|
|
|
|
|
2019-11-28 09:19:17 -04:00
|
|
|
if (!auto_takeoff_no_nav_active) {
|
|
|
|
nav_roll = wp_nav->get_roll();
|
|
|
|
nav_pitch = wp_nav->get_pitch();
|
|
|
|
}
|
|
|
|
|
2019-11-15 16:28:08 -04:00
|
|
|
// call z-axis position controller (wpnav should have already updated it's alt target)
|
|
|
|
copter.pos_control->update_z_controller();
|
|
|
|
|
2019-11-28 09:19:17 -04:00
|
|
|
// roll & pitch from waypoint controller, yaw rate from pilot
|
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate);
|
2019-11-15 16:28:08 -04:00
|
|
|
}
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
void Mode::auto_takeoff_set_start_alt(void)
|
2016-06-04 23:37:55 -03:00
|
|
|
{
|
2019-11-28 09:19:17 -04:00
|
|
|
if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) {
|
|
|
|
// we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min
|
|
|
|
auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude() + g2.wp_navalt_min * 100;
|
|
|
|
auto_takeoff_no_nav_active = true;
|
2016-06-04 23:37:55 -03:00
|
|
|
} else {
|
2019-11-28 09:19:17 -04:00
|
|
|
auto_takeoff_no_nav_active = false;
|
2016-06-04 23:37:55 -03:00
|
|
|
}
|
|
|
|
}
|
2018-04-30 06:50:04 -03:00
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
bool Mode::is_taking_off() const
|
2018-04-30 06:50:04 -03:00
|
|
|
{
|
|
|
|
if (!has_user_takeoff(false)) {
|
|
|
|
return false;
|
|
|
|
}
|
2019-05-09 23:18:49 -03:00
|
|
|
if (copter.ap.land_complete) {
|
2018-04-30 06:50:04 -03:00
|
|
|
return false;
|
|
|
|
}
|
2021-02-03 10:04:38 -04:00
|
|
|
return takeoff.running();
|
2018-04-30 06:50:04 -03:00
|
|
|
}
|