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: guided supports using wpnav to reach position targets

Browse Source
This commit is contained in:
Randy Mackay 2021-09-07 21:30:25 +09:00
parent f201ef57ff
commit 9ee28542da
4 changed files with 146 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/GCS_Mavlink.cpp
View File

@ -138,6 +138,7 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
return;
case ModeGuided::SubMode::TakeOff:
case ModeGuided::SubMode::WP:
case ModeGuided::SubMode::Pos:
type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position

2
ArduCopter/Parameters.cpp
View File

@ -1017,7 +1017,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Param: GUID_OPTIONS
// @DisplayName: Guided mode options
// @Description: Options that can be applied to change guided mode behaviour
// @Bitmask: 0:Allow Arming from Transmitter,2:Ignore pilot yaw,3:SetAttitudeTarget interprets Thrust As Thrust,4:Do not stabilize PositionXY,5:Do not stabilize VelocityXY
// @Bitmask: 0:Allow Arming from Transmitter,2:Ignore pilot yaw,3:SetAttitudeTarget interprets Thrust As Thrust,4:Do not stabilize PositionXY,5:Do not stabilize VelocityXY,6:Waypoint navigation used for position targets
// @User: Advanced
AP_GROUPINFO("GUID_OPTIONS", 41, ParametersG2, guided_options, 0),
#endif

7
ArduCopter/mode.h
View File

@ -866,6 +866,7 @@ public:
bool set_attitude_target_provides_thrust() const;
bool stabilizing_pos_xy() const;
bool stabilizing_vel_xy() const;
bool use_wpnav_for_position_control() const;
void limit_clear();
void limit_init_time_and_pos();
@ -879,6 +880,7 @@ public:
enum class SubMode {
TakeOff,
WP,
Pos,
PosVelAccel,
VelAccel,
Accel,
@ -912,8 +914,13 @@ private:
SetAttitudeTarget_ThrustAsThrust = (1U << 3),
DoNotStabilizePositionXY = (1U << 4),
DoNotStabilizeVelocityXY = (1U << 5),
WPNavUsedForPosControl = (1U << 6),
};
// wp controller
void wp_control_start();
void wp_control_run();
void pva_control_start();
void pos_control_start();
void accel_control_start();

145
ArduCopter/mode_guided.cpp
View File

@ -61,14 +61,19 @@ void ModeGuided::run()
break;
case SubMode::WP:
// run position controller
pos_control_run();
// run waypoint controller
wp_control_run();
if (send_notification && wp_nav->reached_wp_destination()) {
send_notification = false;
gcs().send_mission_item_reached_message(0);
}
break;
case SubMode::Pos:
// run position controller
pos_control_run();
break;
case SubMode::Accel:
accel_control_run();
break;
@ -136,6 +141,68 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
return true;
}
// initialise guided mode's waypoint navigation controller
void ModeGuided::wp_control_start()
{
// set to position control mode
guided_mode = SubMode::WP;
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
// initialise wpnav to stopping point
Vector3f stopping_point;
wp_nav->get_wp_stopping_point(stopping_point);
// no need to check return status because terrain data is not used
wp_nav->set_wp_destination(stopping_point, false);
// initialise yaw
auto_yaw.set_mode_to_default(false);
}
// run guided mode's waypoint navigation controller
void ModeGuided::wp_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled
make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
return;
}
// 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)
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), auto_yaw.rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
}
}
// initialise position controller
void ModeGuided::pva_control_start()
{
@ -162,7 +229,7 @@ void ModeGuided::pva_control_start()
void ModeGuided::pos_control_start()
{
// set to position control mode
guided_mode = SubMode::WP;
guided_mode = SubMode::Pos;
// initialise position controller
pva_control_start();
@ -246,8 +313,28 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
}
#endif
// if configured to use wpnav for position control
if (use_wpnav_for_position_control()) {
// ensure we are in position control mode
if (guided_mode != SubMode::WP) {
wp_control_start();
}
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// no need to check return status because terrain data is not used
wp_nav->set_wp_destination(destination, terrain_alt);
// log target
copter.Log_Write_GuidedTarget(guided_mode, destination, terrain_alt, Vector3f(), Vector3f());
send_notification = true;
return true;
}
// if configured to use position controller for position control
// ensure we are in position control mode
if (guided_mode != SubMode::Pos) {
pos_control_start();
}
@ -289,11 +376,18 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
bool ModeGuided::get_wp(Location& destination)
{
if (guided_mode != SubMode::WP) {
return false;
}
switch (guided_mode) {
case SubMode::WP:
return wp_nav->get_oa_wp_destination(destination);
case SubMode::Pos:
destination = Location(guided_pos_target_cm.tofloat(), guided_pos_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
return true;
default:
return false;
}
// should never get here but just in case
return false;
}
// sets guided mode's target from a Location object
@ -311,8 +405,31 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
}
#endif
// ensure we are in position control mode
// if using wpnav for position control
if (use_wpnav_for_position_control()) {
if (guided_mode != SubMode::WP) {
wp_control_start();
}
if (!wp_nav->set_wp_destination_loc(dest_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;
}
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// log target
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), (dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN), Vector3f(), Vector3f());
send_notification = true;
return true;
}
// if configured to use position controller for position control
// ensure we are in position control mode
if (guided_mode != SubMode::Pos) {
pos_control_start();
}
@ -467,6 +584,12 @@ bool ModeGuided::stabilizing_vel_xy() const
return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizeVelocityXY)) != 0);
}
// returns true if GUIDED_OPTIONS param specifies waypoint navigation should be used for position control (allow path planning to be used but updates must be slower)
bool ModeGuided::use_wpnav_for_position_control() const
{
return ((copter.g2.guided_options.get() & uint32_t(Options::WPNavUsedForPosControl)) != 0);
}
// set guided mode angle target and climbrate
void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust)
{
@ -988,8 +1111,9 @@ uint32_t ModeGuided::wp_distance() const
{
switch(guided_mode) {
case SubMode::WP:
return wp_nav->get_wp_distance_to_destination();
case SubMode::Pos:
return norm(guided_pos_target_cm.x - inertial_nav.get_position().x, guided_pos_target_cm.y - inertial_nav.get_position().y);
break;
case SubMode::PosVelAccel:
return pos_control->get_pos_error_xy_cm();
break;
@ -1002,8 +1126,9 @@ int32_t ModeGuided::wp_bearing() const
{
switch(guided_mode) {
case SubMode::WP:
return wp_nav->get_wp_bearing_to_destination();
case SubMode::Pos:
return get_bearing_cd(inertial_nav.get_position(), guided_pos_target_cm.tofloat());
break;
case SubMode::PosVelAccel:
return pos_control->get_bearing_to_target_cd();
break;
@ -1022,6 +1147,8 @@ float ModeGuided::crosstrack_error() const
{
switch (guided_mode) {
case SubMode::WP:
return wp_nav->crosstrack_error();
case SubMode::Pos:
case SubMode::TakeOff:
case SubMode::Accel:
case SubMode::VelAccel: