ardupilot/ArduCopter/takeoff.cpp

183 lines
6.3 KiB
C++
Raw Normal View History

#include "Copter.h"
Mode::_TakeOff Mode::takeoff;
2019-11-28 09:19:17 -04:00
bool Mode::auto_takeoff_no_nav_active = false;
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
bool Mode::do_user_takeoff_start(float takeoff_alt_cm)
2017-12-12 06:09:48 -04: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
bool Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
{
2017-12-12 06:09:48 -04:00
if (!copter.motors->armed()) {
return false;
}
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;
}
2017-12-12 06:09:48 -04:00
// Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && copter.ap.using_interlock) {
2017-12-12 06:09:48 -04:00
return false;
}
2017-12-12 06:09:48 -04:00
if (!do_user_takeoff_start(takeoff_alt_cm)) {
return false;
}
2017-12-12 06:09:48 -04:00
copter.set_auto_armed(true);
return true;
}
2021-05-19 11:07:38 -03:00
// start takeoff to specified altitude above home in centimeters
void Mode::_TakeOff::start(float alt_cm)
{
// indicate we are taking off
copter.set_land_complete(false);
// tell position controller to reset alt target and reset I terms
copter.flightmode->set_throttle_takeoff();
2015-04-29 23:52:19 -03:00
// initialise takeoff state
_running = true;
2021-05-13 02:40:05 -03:00
take_off_start_alt = copter.pos_control->get_pos_target_z_cm();
2021-05-19 11:07:38 -03:00
take_off_complete_alt = take_off_start_alt + alt_cm;
}
2015-04-30 03:52:32 -03:00
// stop takeoff
void Mode::_TakeOff::stop()
2015-04-30 03:52:32 -03:00
{
_running = false;
2015-04-30 03:52:32 -03:00
}
2021-05-19 11:07:38 -03:00
// do_pilot_takeoff - controls the vertical position controller during the process of taking off
// take off is complete when the vertical target reaches the take off altitude.
2021-05-13 02:40:05 -03:00
// climb is cancelled if pilot_climb_rate_cm becomes negative
// sets take off to complete when target altitude is within 1% of the take off altitude
void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
{
// return pilot_climb_rate if take-off inactive
if (!_running) {
return;
}
2021-05-13 02:40:05 -03:00
Vector3f pos;
Vector3f vel;
Vector3f accel;
2015-10-19 21:56:49 -03:00
2021-05-19 11:07:38 -03:00
pos.z = take_off_complete_alt ;
2021-05-13 02:40:05 -03:00
vel.z = pilot_climb_rate_cm;
2021-05-13 02:40:05 -03:00
// command the aircraft to the take off altitude and current pilot climb rate
copter.pos_control->input_pos_vel_accel_z(pos, vel, accel);
2021-05-13 02:40:05 -03:00
// stop take off early and return if negative climb rate is commanded or we are within 0.1% of our take off altitude
if (is_negative(pilot_climb_rate_cm) ||
2021-05-19 11:07:38 -03:00
(take_off_complete_alt - take_off_start_alt) * 0.999f < copter.pos_control->get_pos_target_z_cm() - take_off_start_alt) {
2021-05-13 02:40:05 -03:00
stop();
}
}
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_and_destination_to_current_pos_xy();
return;
}
2019-11-28 09:19:17 -04:00
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// 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 {
// motors have not completed spool up yet so relax navigation and position controllers
wp_nav->shift_wp_origin_and_destination_to_current_pos_xy();
2021-05-19 11:07:38 -03:00
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
pos_control->update_z_controller();
attitude_control->reset_yaw_target_and_rate();
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-28 09:19:17 -04:00
// check if we are not navigating because of low altitude
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
2021-05-19 11:07:38 -03:00
pos_control->set_externally_limited_xy();
2019-11-28 09:19:17 -04:00
}
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
Vector3f thrustvector{0, 0, -GRAVITY_MSS * 100.0f};
2019-11-28 09:19:17 -04:00
if (!auto_takeoff_no_nav_active) {
thrustvector = wp_nav->get_thrust_vector();
2019-11-28 09:19:17 -04:00
}
2021-05-19 11:07:38 -03:00
// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
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_thrust_vector_rate_heading(thrustvector, target_yaw_rate);
}
void Mode::auto_takeoff_set_start_alt(void)
{
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;
} else {
2019-11-28 09:19:17 -04:00
auto_takeoff_no_nav_active = false;
}
}
bool Mode::is_taking_off() const
{
if (!has_user_takeoff(false)) {
return false;
}
if (copter.ap.land_complete) {
return false;
}
return takeoff.running();
}