Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: auto and guided takeoff use postion controller

This commit is contained in:
Leonard Hall 2022-02-11 13:35:43 +10:30 committed by Randy Mackay
parent 0ce44ad1ba
commit fb6c3ebb72
4 changed files with 150 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/mode.h
View File

@ -197,15 +197,23 @@ protected:
virtual bool do_user_takeoff_start(float takeoff_alt_cm);
// method shared by both Guided and Auto for takeoff. This is
// waypoint navigation but the user can control the yaw.
// method shared by both Guided and Auto for takeoff.
// position controller controls vehicle but the user can control the yaw.
void auto_takeoff_run();
void auto_takeoff_set_start_alt(void);
void auto_takeoff_start(float complete_alt_cm, bool terrain_alt);
bool auto_takeoff_get_position(Vector3p& completion_pos);
// altitude above-ekf-origin below which auto takeoff does not control horizontal position
static bool auto_takeoff_no_nav_active;
static float auto_takeoff_no_nav_alt_cm;
// auto takeoff variables
static float auto_take_off_start_alt_cm; // start altitude expressed as cm above ekf origin
static float auto_take_off_complete_alt_cm; // completion altitude expressed as cm above ekf origin
static bool auto_takeoff_terrain_alt; // true if altitudes are above terrain
static bool auto_takeoff_complete; // true when takeoff is complete
static Vector3p auto_takeoff_complete_pos; // target takeoff position as offset from ekf origin in cm
public:
// Navigation Yaw control
class AutoYaw {
@ -940,6 +948,8 @@ public:
bool is_taking_off() const override;
// initialises position controller to implement take-off
// takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
bool do_user_takeoff_start(float takeoff_alt_cm) override;
enum class SubMode {

85
ArduCopter/mode_auto.cpp
View File

@ -262,39 +262,43 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
{
_mode = SubMode::TAKEOFF;
Location dest(dest_loc);
if (!copter.current_loc.initialised()) {
// vehicle doesn't know where it is ATM. We should not
// initialise our takeoff destination without knowing this!
return;
}
// set horizontal target
dest.lat = copter.current_loc.lat;
dest.lng = copter.current_loc.lng;
// calculate current and target altitudes
// by default current_alt_cm and alt_target_cm are alt-above-EKF-origin
int32_t alt_target_cm;
bool alt_target_terrain = false;
float current_alt_cm = inertial_nav.get_position_z_up_cm();
float terrain_offset; // terrain's altitude in cm above the ekf origin
if ((dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) && wp_nav->get_terrain_offset(terrain_offset)) {
// subtract terrain offset to convert vehicle's alt-above-ekf-origin to alt-above-terrain
current_alt_cm -= terrain_offset;
// get altitude target
int32_t alt_target;
if (!dest.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_target)) {
// this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
// fall back to altitude above current altitude
alt_target = copter.current_loc.alt + dest.alt;
// specify alt_target_cm as alt-above-terrain
alt_target_cm = dest_loc.alt;
alt_target_terrain = true;
} else {
// set horizontal target
Location dest(dest_loc);
dest.lat = copter.current_loc.lat;
dest.lng = copter.current_loc.lng;
// get altitude target above EKF origin
if (!dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) {
// this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
// fall back to altitude above current altitude
alt_target_cm = current_alt_cm + dest.alt;
}
}
// sanity check target
int32_t alt_target_min_cm = copter.current_loc.alt + (copter.ap.land_complete ? 100 : 0);
if (alt_target < alt_target_min_cm ) {
dest.set_alt_cm(alt_target_min_cm , Location::AltFrame::ABOVE_HOME);
}
// set waypoint controller target
if (!wp_nav->set_wp_destination_loc(dest)) {
// failure to set destination can only be because of missing terrain data
copter.failsafe_terrain_on_event();
return;
}
int32_t alt_target_min_cm = current_alt_cm + (copter.ap.land_complete ? 100 : 0);
alt_target_cm = MAX(alt_target_cm, alt_target_min_cm);
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
@ -302,13 +306,25 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
// clear i term when we're taking off
set_throttle_takeoff();
// get initial alt for WP_NAVALT_MIN
auto_takeoff_set_start_alt();
// initialise alt for WP_NAVALT_MIN and set completion alt
auto_takeoff_start(alt_target_cm, alt_target_terrain);
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void ModeAuto::wp_start(const Location& dest_loc)
{
// init wpnav and set origin if transitioning from takeoff
if (!wp_nav->is_active()) {
Vector3f stopping_point;
if (_mode == SubMode::TAKEOFF) {
Vector3p takeoff_complete_pos;
if (auto_takeoff_get_position(takeoff_complete_pos)) {
stopping_point = takeoff_complete_pos.tofloat();
}
}
wp_nav->wp_and_spline_init(0, stopping_point);
}
// send target to waypoint controller
if (!wp_nav->set_wp_destination_loc(dest_loc)) {
// failure to set destination can only be because of missing terrain data
@ -1194,6 +1210,18 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
}
}
// init wpnav and set origin if transitioning from takeoff
if (!wp_nav->is_active()) {
Vector3f stopping_point;
if (_mode == SubMode::TAKEOFF) {
Vector3p takeoff_complete_pos;
if (auto_takeoff_get_position(takeoff_complete_pos)) {
stopping_point = takeoff_complete_pos.tofloat();
}
}
wp_nav->wp_and_spline_init(0, stopping_point);
}
// get waypoint's location from command and send to wp_nav
const Location dest_loc = loc_from_cmd(cmd, default_loc);
if (!wp_nav->set_wp_destination_loc(dest_loc)) {
@ -1637,18 +1665,15 @@ void ModeAuto::do_RTL(void)
// verify_takeoff - check if we have completed the takeoff
bool ModeAuto::verify_takeoff()
{
// have we reached our target altitude?
const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
#if LANDING_GEAR_ENABLED == ENABLED
// if we have reached our destination
if (reached_wp_dest) {
if (auto_takeoff_complete) {
// retract the landing gear
copter.landinggear.retract_after_takeoff();
}
#endif
return reached_wp_dest;
return auto_takeoff_complete;
}
// verify_land - returns true if landing has been completed

35
ArduCopter/mode_guided.cpp
View File

@ -98,14 +98,15 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const
return (copter.g2.guided_options & (uint32_t)Options::AllowArmingFromTX) != 0;
};
// do_user_takeoff_start - initialises waypoint controller to implement take-off
// initialises position controller to implement take-off
// takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
{
guided_mode = SubMode::TakeOff;
// initialise wpnav destination
Location target_loc = copter.current_loc;
Location::AltFrame frame = Location::AltFrame::ABOVE_HOME;
// calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain)
int32_t alt_target_cm;
bool alt_target_terrain = false;
if (wp_nav->rangefinder_used_and_healthy() &&
wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER &&
takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
@ -113,15 +114,19 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) {
return false;
}
frame = Location::AltFrame::ABOVE_TERRAIN;
}
target_loc.set_alt_cm(takeoff_alt_cm, frame);
// provide target altitude as alt-above-terrain
alt_target_cm = takeoff_alt_cm;
alt_target_terrain = true;
} else {
// interpret altitude as alt-above-home
Location target_loc = copter.current_loc;
target_loc.set_alt_cm(takeoff_alt_cm, Location::AltFrame::ABOVE_HOME);
if (!wp_nav->set_wp_destination_loc(target_loc)) {
// failure to set destination can only be because of missing terrain data
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;
// provide target altitude as alt-above-ekf-origin
if (!target_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) {
// this should never happen but we reject the command just in case
return false;
}
}
// initialise yaw
@ -130,8 +135,8 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
// clear i term when we're taking off
set_throttle_takeoff();
// get initial alt for WP_NAVALT_MIN
auto_takeoff_set_start_alt();
// initialise alt for WP_NAVALT_MIN and set completion alt
auto_takeoff_start(alt_target_cm, alt_target_terrain);
// record takeoff has not completed
takeoff_complete = false;
@ -629,7 +634,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_
void ModeGuided::takeoff_run()
{
auto_takeoff_run();
if (!takeoff_complete && wp_nav->reached_wp_destination()) {
if (auto_takeoff_complete && !takeoff_complete) {
takeoff_complete = true;
#if LANDING_GEAR_ENABLED == ENABLED
// optionally retract landing gear

87
ArduCopter/takeoff.cpp
View File

@ -4,6 +4,11 @@ Mode::_TakeOff Mode::takeoff;
bool Mode::auto_takeoff_no_nav_active = false;
float Mode::auto_takeoff_no_nav_alt_cm = 0;
float Mode::auto_take_off_start_alt_cm = 0;
float Mode::auto_take_off_complete_alt_cm = 0;
bool Mode::auto_takeoff_terrain_alt = false;
bool Mode::auto_takeoff_complete = false;
Vector3p Mode::auto_takeoff_complete_pos;
// 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
@ -93,13 +98,21 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
}
}
// auto_takeoff_run - controls the vertical position controller during the process of taking off in auto modes
// auto_takeoff_complete set to true when target altitude is within 10% of the take off altitude and less than 50% max climb rate
void Mode::auto_takeoff_run()
{
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !copter.ap.auto_armed) {
// do not spool down tradheli when on the ground with motor interlock enabled
make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
wp_nav->shift_wp_origin_and_destination_to_current_pos_xy();
return;
}
// get terrain offset
float terr_offset = 0.0f;
if (auto_takeoff_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "auto takeoff: failed to get terrain offset");
return;
}
@ -116,17 +129,18 @@ void Mode::auto_takeoff_run()
}
}
// aircraft stays in landed state until rotor speed runup has finished
// aircraft stays in landed state until rotor speed run up 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();
pos_control->relax_velocity_controller_xy();
pos_control->update_xy_controller();
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);
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
return;
}
@ -135,51 +149,74 @@ void Mode::auto_takeoff_run()
// check if vehicle has reached no_nav_alt threshold
if (inertial_nav.get_position_z_up_cm() >= 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_externally_limited_xy();
pos_control->relax_velocity_controller_xy();
} else {
Vector2f vel;
Vector2f accel;
pos_control->input_vel_accel_xy(vel, accel);
}
pos_control->update_xy_controller();
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
Vector3f thrustvector{0, 0, -GRAVITY_MSS * 100.0f};
if (!auto_takeoff_no_nav_active) {
thrustvector = wp_nav->get_thrust_vector();
}
// WP_Nav has set the vertical position control targets
// command the aircraft to the take off altitude
float pos_z = auto_take_off_complete_alt_cm + terr_offset;
float vel_z = 0.0;
copter.pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0.0);
// run the vertical position controller and set output throttle
copter.pos_control->update_z_controller();
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from position controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(thrustvector, target_yaw_rate);
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from position controller, yaw rate from mavlink command or mission item
attitude_control->input_thrust_vector_rate_heading(thrustvector, auto_yaw.rate_cds());
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
} else {
// roll & pitch from position controller, yaw heading from GCS or auto_heading()
attitude_control->input_thrust_vector_heading(thrustvector, auto_yaw.yaw(), auto_yaw.rate_cds());
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
}
// handle takeoff completion
bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_take_off_start_alt_cm) >= ((auto_take_off_complete_alt_cm - auto_take_off_start_alt_cm) * 0.90);
bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.5;
auto_takeoff_complete = reached_altitude && reached_climb_rate;
// calculate completion for location in case it is needed for a smooth transition to wp_nav
if (auto_takeoff_complete) {
const Vector3p& complete_pos = copter.pos_control->get_pos_target_cm();
auto_takeoff_complete_pos = Vector3p{complete_pos.x, complete_pos.y, pos_z};
}
}
void Mode::auto_takeoff_set_start_alt(void)
void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt)
{
auto_take_off_start_alt_cm = inertial_nav.get_position_z_up_cm();
auto_take_off_complete_alt_cm = complete_alt_cm;
auto_takeoff_terrain_alt = terrain_alt;
auto_takeoff_complete = false;
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_position_z_up_cm() + g2.wp_navalt_min * 100;
auto_takeoff_no_nav_alt_cm = auto_take_off_start_alt_cm + g2.wp_navalt_min * 100;
auto_takeoff_no_nav_active = true;
} else {
auto_takeoff_no_nav_active = false;
}
}
// return takeoff final position if takeoff has completed successfully
bool Mode::auto_takeoff_get_position(Vector3p& complete_pos)
{
// only provide location if takeoff has completed
if (!auto_takeoff_complete) {
return false;
}
complete_pos = auto_takeoff_complete_pos;
return true;
}
bool Mode::is_taking_off() const
{
if (!has_user_takeoff(false)) {