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

Copter: move Mode::auto_takeoff_run to takeoff.cpp

Browse Source
This commit is contained in:
Bogdan Grigoruta 2019-11-15 22:28:08 +02:00 committed by Randy Mackay
parent c2f049918b
commit 5183703243
3 changed files with 38 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/mode.cpp
View File

@ -25,8 +25,6 @@ Mode::Mode(void) :
G_Dt(copter.G_Dt) G_Dt(copter.G_Dt)
{ }; { };
float Mode::auto_takeoff_no_nav_alt_cm = 0;
// return the static controller object corresponding to supplied mode // return the static controller object corresponding to supplied mode
Mode *Copter::mode_from_mode_num(const Mode::Number mode) Mode *Copter::mode_from_mode_num(const Mode::Number mode)
{ {

36
ArduCopter/mode_guided.cpp
View File

@ -375,42 +375,6 @@ void ModeGuided::takeoff_run()
} }
} }
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;
}
// 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 {
wp_nav->shift_wp_origin_to_current_pos();
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
copter.pos_control->update_z_controller();
// call attitude controller
auto_takeoff_attitude_run(target_yaw_rate);
}
// guided_pos_control_run - runs the guided position controller // guided_pos_control_run - runs the guided position controller
// called from guided_run // called from guided_run
void ModeGuided::pos_control_run() void ModeGuided::pos_control_run()

38
ArduCopter/takeoff.cpp
View File

@ -2,6 +2,8 @@
Mode::_TakeOff Mode::takeoff; Mode::_TakeOff Mode::takeoff;
float Mode::auto_takeoff_no_nav_alt_cm = 0;
// This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes. // 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 // 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 // A safe takeoff speed is calculated and used to calculate a time_ms
@ -138,6 +140,42 @@ void Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate,
} }
} }
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;
}
// 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 {
wp_nav->shift_wp_origin_to_current_pos();
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
copter.pos_control->update_z_controller();
// call attitude controller
auto_takeoff_attitude_run(target_yaw_rate);
}
void Mode::auto_takeoff_set_start_alt(void) void Mode::auto_takeoff_set_start_alt(void)
{ {
// start with our current altitude // start with our current altitude