Copter: support for acceleration-based AttitudeControl
This commit is contained in:
Leonard Hall
Randy Mackay
parent
75411afd21
commit
635d13a106
@ -791,7 +791,7 @@ private:
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
void Log_Write_Heli(void);
|
||||
#endif
|
||||
void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, const Vector3f& vel_target);
|
||||
void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, const Vector3f& vel_target, const Vector3f& accel_target);
|
||||
void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);
|
||||
void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z);
|
||||
void Log_Write_Vehicle_Startup_Messages();
|
||||
|
||||
@ -119,30 +119,37 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
|
||||
const ModeGuided::SubMode guided_mode = copter.mode_guided.submode();
|
||||
Vector3f target_pos;
|
||||
Vector3f target_vel;
|
||||
Vector3f target_accel;
|
||||
uint16_t type_mask = 0;
|
||||
|
||||
switch (guided_mode) {
|
||||
case ModeGuided::SubMode::Angle:
|
||||
// we don't have a local target when in angle mode
|
||||
return;
|
||||
case ModeGuided::SubMode::TakeOff:
|
||||
case ModeGuided::SubMode::WP:
|
||||
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
|
||||
target_pos = copter.wp_nav->get_wp_destination() * 0.01f; // convert to metres
|
||||
target_pos = copter.mode_guided.get_target_pos().tofloat() * 0.01; // convert to metres
|
||||
break;
|
||||
case ModeGuided::SubMode::Velocity:
|
||||
case ModeGuided::SubMode::PosVelAccel:
|
||||
type_mask = POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position, velocity & acceleration
|
||||
target_pos = copter.mode_guided.get_target_pos().tofloat() * 0.01; // convert to metres
|
||||
target_vel = copter.mode_guided.get_target_vel() * 0.01f; // convert to metres/s
|
||||
target_accel = copter.mode_guided.get_target_accel() * 0.01f; // convert to metres/s/s
|
||||
break;
|
||||
case ModeGuided::SubMode::VelAccel:
|
||||
type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_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 velocity
|
||||
target_vel = copter.flightmode->get_vel_desired_cms() * 0.01f; // convert to m/s
|
||||
POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity & acceleration
|
||||
target_vel = copter.mode_guided.get_target_vel() * 0.01f; // convert to metres/s
|
||||
target_accel = copter.mode_guided.get_target_accel() * 0.01f; // convert to metres/s/s
|
||||
break;
|
||||
case ModeGuided::SubMode::TakeOff:
|
||||
case ModeGuided::SubMode::PosVel:
|
||||
type_mask = 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 & velocity
|
||||
target_pos = copter.wp_nav->get_wp_destination() * 0.01f;
|
||||
target_vel = copter.flightmode->get_vel_desired_cms() * 0.01f;
|
||||
case ModeGuided::SubMode::Accel:
|
||||
type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE |
|
||||
POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
|
||||
POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity & acceleration
|
||||
target_accel = copter.mode_guided.get_target_accel() * 0.01f; // convert to metres/s/s
|
||||
break;
|
||||
}
|
||||
|
||||
@ -151,15 +158,15 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
|
||||
AP_HAL::millis(), // time boot ms
|
||||
MAV_FRAME_LOCAL_NED,
|
||||
type_mask,
|
||||
target_pos.x, // x in metres
|
||||
target_pos.y, // y in metres
|
||||
-target_pos.z, // z in metres NED frame
|
||||
target_vel.x, // vx in m/s
|
||||
target_vel.y, // vy in m/s
|
||||
-target_vel.z, // vz in m/s NED frame
|
||||
0.0f, // afx
|
||||
0.0f, // afy
|
||||
0.0f, // afz
|
||||
target_pos.x, // x in metres
|
||||
target_pos.y, // y in metres
|
||||
-target_pos.z, // z in metres NED frame
|
||||
target_vel.x, // vx in m/s
|
||||
target_vel.y, // vy in m/s
|
||||
-target_vel.z, // vz in m/s NED frame
|
||||
target_accel.x, // afx in m/s/s
|
||||
target_accel.y, // afy in m/s/s
|
||||
-target_accel.z,// afz in m/s/s NED frame
|
||||
0.0f, // yaw
|
||||
0.0f); // yaw_rate
|
||||
#endif
|
||||
@ -1111,8 +1118,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
|
||||
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
|
||||
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
|
||||
|
||||
// exit immediately if acceleration provided
|
||||
if (!acc_ignore) {
|
||||
// exit immediately if neither position nor velocity is provided
|
||||
if (pos_ignore && vel_ignore) {
|
||||
break;
|
||||
}
|
||||
|
||||
@ -1145,6 +1152,17 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
|
||||
}
|
||||
}
|
||||
|
||||
// prepare acceleration
|
||||
Vector3f accel_vector;
|
||||
if (!acc_ignore) {
|
||||
// convert to cm
|
||||
accel_vector = Vector3f(packet.afx * 100.0f, packet.afy * 100.0f, -packet.afz * 100.0f);
|
||||
// rotate to body-frame if necessary
|
||||
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
|
||||
copter.rotate_body_frame_to_NE(accel_vector.x, accel_vector.y);
|
||||
}
|
||||
}
|
||||
|
||||
// prepare yaw
|
||||
float yaw_cd = 0.0f;
|
||||
bool yaw_relative = false;
|
||||
@ -1159,11 +1177,13 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
|
||||
|
||||
// send request
|
||||
if (!pos_ignore && !vel_ignore) {
|
||||
copter.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||
copter.mode_guided.set_destination_posvelaccel(pos_vector, vel_vector, accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||
} else if (pos_ignore && vel_ignore && !acc_ignore) {
|
||||
copter.mode_guided.set_accel(accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||
} else if (pos_ignore && !vel_ignore) {
|
||||
copter.mode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||
} else if (!pos_ignore && vel_ignore) {
|
||||
copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||
copter.mode_guided.set_velaccel(vel_vector, accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||
} else if (!pos_ignore && vel_ignore && acc_ignore) {
|
||||
copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, false);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
@ -377,12 +377,15 @@ struct PACKED log_GuidedTarget {
|
||||
float vel_target_x;
|
||||
float vel_target_y;
|
||||
float vel_target_z;
|
||||
float accel_target_x;
|
||||
float accel_target_y;
|
||||
float accel_target_z;
|
||||
};
|
||||
|
||||
// Write a Guided mode target
|
||||
// pos_target is lat, lon, alt OR offset from ekf origin in cm OR roll, pitch, yaw target in centi-degrees
|
||||
// vel_target is cm/s
|
||||
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, const Vector3f& vel_target)
|
||||
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, const Vector3f& vel_target, const Vector3f& accel_target)
|
||||
{
|
||||
struct log_GuidedTarget pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
|
||||
@ -393,7 +396,10 @@ void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vecto
|
||||
pos_target_z : pos_target.z,
|
||||
vel_target_x : vel_target.x,
|
||||
vel_target_y : vel_target.y,
|
||||
vel_target_z : vel_target.z
|
||||
vel_target_z : vel_target.z,
|
||||
accel_target_x : accel_target.x,
|
||||
accel_target_y : accel_target.y,
|
||||
accel_target_z : accel_target.z
|
||||
};
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
@ -539,9 +545,12 @@ const struct LogStructure Copter::log_structure[] = {
|
||||
// @Field: vX: Target velocity, X-Axis
|
||||
// @Field: vY: Target velocity, Y-Axis
|
||||
// @Field: vZ: Target velocity, Z-Axis
|
||||
// @Field: aX: Target acceleration, X-Axis
|
||||
// @Field: aY: Target acceleration, Y-Axis
|
||||
// @Field: aZ: Target acceleration, Z-Axis
|
||||
|
||||
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
|
||||
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-BBBBBB" },
|
||||
"GUID", "QBfffffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ,aX,aY,aZ", "s-mmmnnnooo", "F-BBBBBBBBB" },
|
||||
};
|
||||
|
||||
void Copter::Log_Write_Vehicle_Startup_Messages()
|
||||
@ -573,7 +582,7 @@ void Copter::Log_Write_Data(LogDataID id, uint16_t value) {}
|
||||
void Copter::Log_Write_Data(LogDataID id, float value) {}
|
||||
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {}
|
||||
void Copter::Log_Sensor_Health() {}
|
||||
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
|
||||
void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, const Vector3f& vel_target, const Vector3f& accel_target) {}
|
||||
void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {}
|
||||
void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {}
|
||||
void Copter::Log_Write_Vehicle_Startup_Messages() {}
|
||||
|
||||
@ -1027,7 +1027,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_ThrustAsThrust
|
||||
// @Bitmask: 0:Allow Arming from Transmitter,2:Ignore pilot yaw,3:SetAttitudeTarget_ThrustAsThrust,4:DoNotStabilizePositionXY,5:DoNotStabilizeVelocityXY
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GUID_OPTIONS", 41, ParametersG2, guided_options, 0),
|
||||
#endif
|
||||
|
||||
@ -845,11 +845,21 @@ public:
|
||||
bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool terrain_alt = false);
|
||||
bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||
bool get_wp(Location &loc) override;
|
||||
void set_accel(const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
|
||||
void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
|
||||
void set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
|
||||
bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||
bool set_destination_posvelaccel(const Vector3f& destination, const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||
|
||||
// get position, velocity and acceleration targets
|
||||
const Vector3p& get_target_pos() const;
|
||||
const Vector3f& get_target_vel() const;
|
||||
const Vector3f& get_target_accel() const;
|
||||
|
||||
// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
|
||||
bool set_attitude_target_provides_thrust() const;
|
||||
bool stabilizing_pos_xy() const;
|
||||
bool stabilizing_vel_xy() const;
|
||||
|
||||
void limit_clear();
|
||||
void limit_init_time_and_pos();
|
||||
@ -863,8 +873,9 @@ public:
|
||||
enum class SubMode {
|
||||
TakeOff,
|
||||
WP,
|
||||
Velocity,
|
||||
PosVel,
|
||||
PosVelAccel,
|
||||
VelAccel,
|
||||
Accel,
|
||||
Angle,
|
||||
};
|
||||
|
||||
@ -886,19 +897,23 @@ private:
|
||||
|
||||
// enum for GUID_OPTIONS parameter
|
||||
enum class Options : int32_t {
|
||||
AllowArmingFromTX = (1U << 0),
|
||||
AllowArmingFromTX = (1U << 0),
|
||||
// this bit is still available, pilot yaw was mapped to bit 2 for symmetry with auto
|
||||
IgnorePilotYaw = (1U << 2),
|
||||
IgnorePilotYaw = (1U << 2),
|
||||
SetAttitudeTarget_ThrustAsThrust = (1U << 3),
|
||||
DoNotStabilizePositionXY = (1U << 4),
|
||||
DoNotStabilizeVelocityXY = (1U << 5),
|
||||
};
|
||||
|
||||
void pos_control_start();
|
||||
void vel_control_start();
|
||||
void posvel_control_start();
|
||||
void accel_control_start();
|
||||
void velaccel_control_start();
|
||||
void posvelaccel_control_start();
|
||||
void takeoff_run();
|
||||
void pos_control_run();
|
||||
void vel_control_run();
|
||||
void posvel_control_run();
|
||||
void accel_control_run();
|
||||
void velaccel_control_run();
|
||||
void posvelaccel_control_run();
|
||||
void set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des);
|
||||
void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
|
||||
bool use_pilot_yaw(void) const;
|
||||
|
||||
@ -14,9 +14,9 @@
|
||||
#define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates
|
||||
|
||||
static Vector3p guided_pos_target_cm; // position target (used by posvel controller only)
|
||||
static Vector3f guided_vel_target_cms; // velocity target (used by velocity controller and posvel controller)
|
||||
static uint32_t posvel_update_time_ms; // system time of last target update to posvel controller (i.e. position and velocity update)
|
||||
static uint32_t vel_update_time_ms; // system time of last target update to velocity controller
|
||||
static Vector3f guided_vel_target_cms; // velocity target (used by pos_vel_accel controller and vel_accel controller)
|
||||
static Vector3f guided_accel_target_cmss; // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller)
|
||||
static uint32_t update_time_ms; // system time of last target update to pos_vel_accel, vel_accel or accel controller
|
||||
|
||||
struct {
|
||||
uint32_t update_time_ms;
|
||||
@ -39,16 +39,18 @@ struct Guided_Limit {
|
||||
Vector3f start_pos; // start position as a distance from home in cm. used for checking horiz_max limit
|
||||
} guided_limit;
|
||||
|
||||
// guided_init - initialise guided controller
|
||||
// init - initialise guided controller
|
||||
bool ModeGuided::init(bool ignore_checks)
|
||||
{
|
||||
// start in position control mode
|
||||
pos_control_start();
|
||||
// start in velaccel control mode
|
||||
velaccel_control_start();
|
||||
guided_vel_target_cms.zero();
|
||||
guided_accel_target_cmss.zero();
|
||||
send_notification = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
// guided_run - runs the guided controller
|
||||
// run - runs the guided controller
|
||||
// should be called at 100hz or more
|
||||
void ModeGuided::run()
|
||||
{
|
||||
@ -69,18 +71,19 @@ void ModeGuided::run()
|
||||
}
|
||||
break;
|
||||
|
||||
case SubMode::Velocity:
|
||||
// run velocity controller
|
||||
vel_control_run();
|
||||
case SubMode::Accel:
|
||||
accel_control_run();
|
||||
break;
|
||||
|
||||
case SubMode::PosVel:
|
||||
// run position-velocity controller
|
||||
posvel_control_run();
|
||||
case SubMode::VelAccel:
|
||||
velaccel_control_run();
|
||||
break;
|
||||
|
||||
case SubMode::PosVelAccel:
|
||||
posvelaccel_control_run();
|
||||
break;
|
||||
|
||||
case SubMode::Angle:
|
||||
// run angle controller
|
||||
angle_control_run();
|
||||
break;
|
||||
}
|
||||
@ -141,25 +144,25 @@ void ModeGuided::pos_control_start()
|
||||
// set to position control mode
|
||||
guided_mode = SubMode::WP;
|
||||
|
||||
// initialise waypoint and spline controller
|
||||
wp_nav->wp_and_spline_init();
|
||||
// initialise horizontal speed, acceleration
|
||||
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
|
||||
|
||||
// initialise wpnav to stopping point
|
||||
Vector3f stopping_point;
|
||||
wp_nav->get_wp_stopping_point(stopping_point);
|
||||
// initialize vertical speeds and acceleration
|
||||
pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
|
||||
|
||||
// no need to check return status because terrain data is not used
|
||||
wp_nav->set_wp_destination(stopping_point, false);
|
||||
// initialise velocity controller
|
||||
pos_control->init_z_controller();
|
||||
pos_control->init_xy_controller();
|
||||
|
||||
// initialise yaw
|
||||
auto_yaw.set_mode_to_default(false);
|
||||
}
|
||||
|
||||
// initialise guided mode's velocity controller
|
||||
void ModeGuided::vel_control_start()
|
||||
void ModeGuided::accel_control_start()
|
||||
{
|
||||
// set guided_mode to velocity controller
|
||||
guided_mode = SubMode::Velocity;
|
||||
guided_mode = SubMode::Accel;
|
||||
|
||||
// initialise horizontal speed, acceleration
|
||||
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
|
||||
@ -170,13 +173,36 @@ void ModeGuided::vel_control_start()
|
||||
// initialise the position controller
|
||||
pos_control->init_z_controller();
|
||||
pos_control->init_xy_controller();
|
||||
|
||||
// pilot always controls yaw
|
||||
auto_yaw.set_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
|
||||
// initialise guided mode's velocity controller
|
||||
void ModeGuided::velaccel_control_start()
|
||||
{
|
||||
// set guided_mode to velocity controller
|
||||
guided_mode = SubMode::VelAccel;
|
||||
|
||||
// initialise horizontal speed, acceleration
|
||||
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
|
||||
|
||||
// set vertical speed and acceleration limits
|
||||
pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
|
||||
|
||||
// initialise the position controller
|
||||
pos_control->init_z_controller();
|
||||
pos_control->init_xy_controller();
|
||||
|
||||
// pilot always controls yaw
|
||||
auto_yaw.set_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
|
||||
// initialise guided mode's posvel controller
|
||||
void ModeGuided::posvel_control_start()
|
||||
void ModeGuided::posvelaccel_control_start()
|
||||
{
|
||||
// set guided_mode to velocity controller
|
||||
guided_mode = SubMode::PosVel;
|
||||
guided_mode = SubMode::PosVelAccel;
|
||||
|
||||
// initialise horizontal speed, acceleration
|
||||
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
|
||||
@ -224,7 +250,7 @@ void ModeGuided::angle_control_start()
|
||||
auto_yaw.set_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
|
||||
// guided_set_destination - sets guided mode's target destination
|
||||
// set_destination - sets guided mode's target destination
|
||||
// Returns true if the fence is enabled and guided waypoint is within the fence
|
||||
// else return false if the waypoint is outside the fence
|
||||
bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool terrain_alt)
|
||||
@ -247,11 +273,14 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
|
||||
// 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);
|
||||
// set position target and zero velocity and acceleration
|
||||
guided_pos_target_cm = destination.topostype();
|
||||
guided_vel_target_cms.zero();
|
||||
guided_accel_target_cmss.zero();
|
||||
update_time_ms = millis();
|
||||
|
||||
// log target
|
||||
copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss);
|
||||
|
||||
send_notification = true;
|
||||
|
||||
@ -286,47 +315,89 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
|
||||
pos_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);
|
||||
|
||||
// set position target and zero velocity and acceleration
|
||||
Vector3f pos_target_f;
|
||||
if (!dest_loc.get_vector_from_origin_NEU(pos_target_f)) {
|
||||
guided_pos_target_cm = pos_control->get_pos_target_cm();
|
||||
} else {
|
||||
guided_pos_target_cm = pos_target_f.topostype();
|
||||
}
|
||||
|
||||
guided_vel_target_cms.zero();
|
||||
guided_accel_target_cmss.zero();
|
||||
|
||||
// log target
|
||||
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
|
||||
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_vel_target_cms, guided_accel_target_cmss);
|
||||
|
||||
send_notification = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// guided_set_velocity - sets guided mode's target velocity
|
||||
void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
|
||||
// set_velaccel - sets guided mode's target velocity and acceleration
|
||||
void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
|
||||
{
|
||||
// check we are in velocity control mode
|
||||
if (guided_mode != SubMode::Velocity) {
|
||||
vel_control_start();
|
||||
if (guided_mode != SubMode::Accel) {
|
||||
accel_control_start();
|
||||
}
|
||||
|
||||
// set yaw state
|
||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
|
||||
// record velocity target
|
||||
guided_vel_target_cms = velocity;
|
||||
vel_update_time_ms = millis();
|
||||
// set velocity and acceleration targets and zero position
|
||||
guided_pos_target_cm.zero();
|
||||
guided_vel_target_cms.zero();
|
||||
guided_accel_target_cmss = acceleration;
|
||||
update_time_ms = millis();
|
||||
|
||||
// log target
|
||||
if (log_request) {
|
||||
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss);
|
||||
}
|
||||
}
|
||||
|
||||
// set guided mode posvel target
|
||||
// set_velocity - sets guided mode's target velocity
|
||||
void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
|
||||
{
|
||||
set_velaccel(velocity, Vector3f(), use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw, log_request);
|
||||
}
|
||||
|
||||
// set_velaccel - sets guided mode's target velocity and acceleration
|
||||
void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
|
||||
{
|
||||
// check we are in velocity control mode
|
||||
if (guided_mode != SubMode::VelAccel) {
|
||||
velaccel_control_start();
|
||||
}
|
||||
|
||||
// set yaw state
|
||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
|
||||
// set velocity and acceleration targets and zero position
|
||||
guided_pos_target_cm.zero();
|
||||
guided_vel_target_cms = velocity;
|
||||
guided_accel_target_cmss = acceleration;
|
||||
update_time_ms = millis();
|
||||
|
||||
// log target
|
||||
if (log_request) {
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss);
|
||||
}
|
||||
}
|
||||
|
||||
// set_destination_posvel - set guided mode position and velocity target
|
||||
bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
||||
{
|
||||
set_destination_posvelaccel(destination, velocity, Vector3f(), use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
return true;
|
||||
}
|
||||
|
||||
// set_destination_posvelaccel - set guided mode position, velocity and acceleration target
|
||||
bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
||||
{
|
||||
#if AC_FENCE == ENABLED
|
||||
// reject destination if outside the fence
|
||||
@ -339,21 +410,20 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto
|
||||
#endif
|
||||
|
||||
// check we are in velocity control mode
|
||||
if (guided_mode != SubMode::PosVel) {
|
||||
posvel_control_start();
|
||||
if (guided_mode != SubMode::PosVelAccel) {
|
||||
posvelaccel_control_start();
|
||||
}
|
||||
|
||||
// set yaw state
|
||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
|
||||
posvel_update_time_ms = millis();
|
||||
update_time_ms = millis();
|
||||
guided_pos_target_cm = destination.topostype();
|
||||
guided_vel_target_cms = velocity;
|
||||
|
||||
copter.pos_control->set_pos_vel_accel(guided_pos_target_cm, guided_vel_target_cms, Vector3f());
|
||||
guided_accel_target_cmss = acceleration;
|
||||
|
||||
// log target
|
||||
copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
|
||||
copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss);
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -363,6 +433,18 @@ bool ModeGuided::set_attitude_target_provides_thrust() const
|
||||
return ((copter.g2.guided_options.get() & uint32_t(Options::SetAttitudeTarget_ThrustAsThrust)) != 0);
|
||||
}
|
||||
|
||||
// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
|
||||
bool ModeGuided:: stabilizing_pos_xy() const
|
||||
{
|
||||
return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizePositionXY)) != 0);
|
||||
}
|
||||
|
||||
// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
|
||||
bool ModeGuided:: stabilizing_vel_xy() const
|
||||
{
|
||||
return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizeVelocityXY)) != 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)
|
||||
{
|
||||
@ -393,10 +475,10 @@ void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust,
|
||||
// log target
|
||||
copter.Log_Write_GuidedTarget(guided_mode,
|
||||
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
|
||||
Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust));
|
||||
Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust), Vector3f());
|
||||
}
|
||||
|
||||
// guided_takeoff_run - takeoff in guided mode
|
||||
// takeoff_run - takeoff in guided mode
|
||||
// called by guided_run at 100hz or more
|
||||
void ModeGuided::takeoff_run()
|
||||
{
|
||||
@ -408,17 +490,18 @@ void ModeGuided::takeoff_run()
|
||||
#endif
|
||||
|
||||
// switch to position control mode but maintain current target
|
||||
const Vector3f target = wp_nav->get_wp_destination();
|
||||
const Vector3f target = pos_control->get_pos_target_cm().tofloat();
|
||||
set_destination(target, false, 0, false, 0, false, wp_nav->origin_and_destination_are_terrain_alt());
|
||||
}
|
||||
}
|
||||
|
||||
// guided_pos_control_run - runs the guided position controller
|
||||
// pos_control_run - runs the guided position controller
|
||||
// called from guided_run
|
||||
void ModeGuided::pos_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());
|
||||
@ -436,29 +519,31 @@ void ModeGuided::pos_control_run()
|
||||
// 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());
|
||||
// send position and velocity targets to position controller
|
||||
guided_accel_target_cmss.zero();
|
||||
guided_vel_target_cms.zero();
|
||||
pos_control->input_pos_xyz(guided_pos_target_cm);
|
||||
|
||||
// WP_Nav has set the vertical position control targets
|
||||
// run the vertical position controller and set output throttle
|
||||
// run position controllers
|
||||
pos_control->update_xy_controller();
|
||||
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);
|
||||
// roll & pitch from position controller, yaw rate from pilot
|
||||
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 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());
|
||||
// roll & pitch from position controller, yaw rate from mavlink command or mission item
|
||||
attitude_control->input_thrust_vector_rate_heading(pos_control->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());
|
||||
// roll & pitch from position controller, yaw heading from GCS or auto_heading()
|
||||
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
|
||||
}
|
||||
}
|
||||
|
||||
// guided_vel_control_run - runs the guided velocity controller
|
||||
// velaccel_control_run - runs the guided velocity controller
|
||||
// called from guided_run
|
||||
void ModeGuided::vel_control_run()
|
||||
void ModeGuided::accel_control_run()
|
||||
{
|
||||
// process pilot's yaw input
|
||||
float target_yaw_rate = 0;
|
||||
@ -492,37 +577,129 @@ void ModeGuided::vel_control_run()
|
||||
|
||||
// set velocity to zero and stop rotating if no updates received for 3 seconds
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
|
||||
if (!pos_control->get_vel_desired_cms().is_zero()) {
|
||||
set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f));
|
||||
}
|
||||
if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
|
||||
guided_accel_target_cmss.zero();
|
||||
if (auto_yaw.mode() == AUTO_YAW_RATE) {
|
||||
auto_yaw.set_rate(0.0f);
|
||||
}
|
||||
} else {
|
||||
set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
|
||||
}
|
||||
|
||||
// update position controller with new target
|
||||
pos_control->input_accel_xy(guided_accel_target_cmss);
|
||||
if (!stabilizing_vel_xy()) {
|
||||
// set position and velocity errors to zero
|
||||
pos_control->stop_vel_xy_stabilisation();
|
||||
} else if (!stabilizing_pos_xy()) {
|
||||
// set position errors to zero
|
||||
pos_control->stop_pos_xy_stabilisation();
|
||||
}
|
||||
pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false);
|
||||
|
||||
// call velocity controller which includes z axis controller
|
||||
pos_control->update_xy_controller();
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
// roll & pitch from position controller, yaw rate from pilot
|
||||
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 velocity controller, yaw rate from mavlink command or mission item
|
||||
// roll & pitch from position controller, yaw rate from mavlink command or mission item
|
||||
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
|
||||
} else {
|
||||
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
|
||||
// roll & pitch from position controller, yaw heading from GCS or auto_heading()
|
||||
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
|
||||
}
|
||||
}
|
||||
|
||||
// guided_posvel_control_run - runs the guided spline controller
|
||||
// velaccel_control_run - runs the guided velocity and acceleration controller
|
||||
// called from guided_run
|
||||
void ModeGuided::posvel_control_run()
|
||||
void ModeGuided::velaccel_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);
|
||||
}
|
||||
}
|
||||
|
||||
// landed with positive desired climb rate, initiate takeoff
|
||||
if (motors->armed() && copter.ap.auto_armed && copter.ap.land_complete && is_positive(guided_vel_target_cms.z)) {
|
||||
zero_throttle_and_relax_ac();
|
||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
||||
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
|
||||
set_land_complete(false);
|
||||
set_throttle_takeoff();
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// if not armed set throttle to zero and exit immediately
|
||||
if (is_disarmed_or_landed()) {
|
||||
make_safe_spool_down();
|
||||
return;
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
||||
|
||||
// set velocity to zero and stop rotating if no updates received for 3 seconds
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
|
||||
guided_vel_target_cms.zero();
|
||||
guided_accel_target_cmss.zero();
|
||||
if (auto_yaw.mode() == AUTO_YAW_RATE) {
|
||||
auto_yaw.set_rate(0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
bool do_avoid = false;
|
||||
#if AC_AVOID_ENABLED
|
||||
// limit the velocity for obstacle/fence avoidance
|
||||
copter.avoid.adjust_velocity(guided_vel_target_cms, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt);
|
||||
do_avoid = copter.avoid.limits_active();
|
||||
#endif
|
||||
|
||||
// update position controller with new target
|
||||
|
||||
if (!stabilizing_vel_xy() && !do_avoid) {
|
||||
// set the current commanded xy vel to the desired vel
|
||||
guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x;
|
||||
guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y;
|
||||
}
|
||||
pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy());
|
||||
if (!stabilizing_vel_xy() && !do_avoid) {
|
||||
// set position and velocity errors to zero
|
||||
pos_control->stop_vel_xy_stabilisation();
|
||||
} else if (!stabilizing_pos_xy() && !do_avoid) {
|
||||
// set position errors to zero
|
||||
pos_control->stop_pos_xy_stabilisation();
|
||||
}
|
||||
pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false);
|
||||
|
||||
// call velocity controller which includes z axis controller
|
||||
pos_control->update_xy_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(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(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(pos_control->get_thrust_vector(), auto_yaw.yaw());
|
||||
}
|
||||
}
|
||||
|
||||
// posvelaccel_control_run - runs the guided position, velocity and acceleration controller
|
||||
// called from guided_run
|
||||
void ModeGuided::posvelaccel_control_run()
|
||||
{
|
||||
// process pilot's yaw input
|
||||
float target_yaw_rate = 0;
|
||||
@ -546,20 +723,37 @@ void ModeGuided::posvel_control_run()
|
||||
|
||||
// set velocity to zero and stop rotating if no updates received for 3 seconds
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
|
||||
if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
|
||||
guided_vel_target_cms.zero();
|
||||
guided_accel_target_cmss.zero();
|
||||
if (auto_yaw.mode() == AUTO_YAW_RATE) {
|
||||
auto_yaw.set_rate(0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
// advance position target using velocity target
|
||||
guided_pos_target_cm += (guided_vel_target_cms * pos_control->get_dt()).topostype();
|
||||
|
||||
// send position and velocity targets to position controller
|
||||
pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), Vector2f());
|
||||
if (!stabilizing_vel_xy()) {
|
||||
// set the current commanded xy pos to the target pos and xy vel to the desired vel
|
||||
guided_pos_target_cm.x = pos_control->get_pos_target_cm().x;
|
||||
guided_pos_target_cm.y = pos_control->get_pos_target_cm().y;
|
||||
guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x;
|
||||
guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y;
|
||||
} else if (!stabilizing_pos_xy()) {
|
||||
// set the current commanded xy pos to the target pos
|
||||
guided_pos_target_cm.x = pos_control->get_pos_target_cm().x;
|
||||
guided_pos_target_cm.y = pos_control->get_pos_target_cm().y;
|
||||
}
|
||||
pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), guided_accel_target_cmss.xy());
|
||||
if (!stabilizing_vel_xy()) {
|
||||
// set position and velocity errors to zero
|
||||
pos_control->stop_vel_xy_stabilisation();
|
||||
} else if (!stabilizing_pos_xy()) {
|
||||
// set position errors to zero
|
||||
pos_control->stop_pos_xy_stabilisation();
|
||||
}
|
||||
|
||||
float pz = guided_pos_target_cm.z;
|
||||
pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, 0);
|
||||
pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, guided_accel_target_cmss.z);
|
||||
guided_pos_target_cm.z = pz;
|
||||
|
||||
// run position controllers
|
||||
@ -568,18 +762,18 @@ void ModeGuided::posvel_control_run()
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
// roll & pitch from position controller, yaw rate from pilot
|
||||
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-velocity controller, yaw rate from mavlink command or mission item
|
||||
// roll & pitch from position controller, yaw rate from mavlink command or mission item
|
||||
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
|
||||
} else {
|
||||
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
|
||||
// roll & pitch from position controller, yaw heading from GCS or auto_heading()
|
||||
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
|
||||
}
|
||||
}
|
||||
|
||||
// guided_angle_control_run - runs the guided angle controller
|
||||
// angle_control_run - runs the guided angle controller
|
||||
// called from guided_run
|
||||
void ModeGuided::angle_control_run()
|
||||
{
|
||||
@ -663,17 +857,6 @@ void ModeGuided::angle_control_run()
|
||||
// helper function to update position controller's desired velocity while respecting acceleration limits
|
||||
void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
|
||||
{
|
||||
// get current desired velocity
|
||||
Vector3f curr_vel_des = vel_des;
|
||||
|
||||
#if AC_AVOID_ENABLED
|
||||
// limit the velocity for obstacle/fence avoidance
|
||||
copter.avoid.adjust_velocity(curr_vel_des, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt);
|
||||
#endif
|
||||
|
||||
// update position controller with new target
|
||||
pos_control->input_vel_accel_xy(curr_vel_des.xy(), Vector2f());
|
||||
pos_control->input_vel_accel_z(curr_vel_des.z, 0, false);
|
||||
}
|
||||
|
||||
// helper function to set yaw state and targets
|
||||
@ -694,7 +877,7 @@ bool ModeGuided::use_pilot_yaw(void) const
|
||||
|
||||
// Guided Limit code
|
||||
|
||||
// guided_limit_clear - clear/turn off guided limits
|
||||
// limit_clear - clear/turn off guided limits
|
||||
void ModeGuided::limit_clear()
|
||||
{
|
||||
guided_limit.timeout_ms = 0;
|
||||
@ -703,7 +886,7 @@ void ModeGuided::limit_clear()
|
||||
guided_limit.horiz_max_cm = 0.0f;
|
||||
}
|
||||
|
||||
// guided_limit_set - set guided timeout and movement limits
|
||||
// limit_set - set guided timeout and movement limits
|
||||
void ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
|
||||
{
|
||||
guided_limit.timeout_ms = timeout_ms;
|
||||
@ -712,7 +895,7 @@ void ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_
|
||||
guided_limit.horiz_max_cm = horiz_max_cm;
|
||||
}
|
||||
|
||||
// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
|
||||
// limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
|
||||
// only called from AUTO mode's auto_nav_guided_start function
|
||||
void ModeGuided::limit_init_time_and_pos()
|
||||
{
|
||||
@ -723,7 +906,7 @@ void ModeGuided::limit_init_time_and_pos()
|
||||
guided_limit.start_pos = inertial_nav.get_position();
|
||||
}
|
||||
|
||||
// guided_limit_check - returns true if guided mode has breached a limit
|
||||
// limit_check - returns true if guided mode has breached a limit
|
||||
// used when guided is invoked from the NAV_GUIDED_ENABLE mission command
|
||||
bool ModeGuided::limit_check()
|
||||
{
|
||||
@ -757,14 +940,28 @@ bool ModeGuided::limit_check()
|
||||
return false;
|
||||
}
|
||||
|
||||
const Vector3p &ModeGuided::get_target_pos() const
|
||||
{
|
||||
return guided_pos_target_cm;
|
||||
}
|
||||
|
||||
const Vector3f& ModeGuided::get_target_vel() const
|
||||
{
|
||||
return guided_vel_target_cms;
|
||||
}
|
||||
|
||||
const Vector3f& ModeGuided::get_target_accel() const
|
||||
{
|
||||
return guided_accel_target_cmss;
|
||||
}
|
||||
|
||||
uint32_t ModeGuided::wp_distance() const
|
||||
{
|
||||
switch(guided_mode) {
|
||||
case SubMode::WP:
|
||||
return wp_nav->get_wp_distance_to_destination();
|
||||
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::PosVel:
|
||||
case SubMode::PosVelAccel:
|
||||
return pos_control->get_pos_error_xy_cm();
|
||||
break;
|
||||
default:
|
||||
@ -776,13 +973,14 @@ int32_t ModeGuided::wp_bearing() const
|
||||
{
|
||||
switch(guided_mode) {
|
||||
case SubMode::WP:
|
||||
return wp_nav->get_wp_bearing_to_destination();
|
||||
return get_bearing_cd(inertial_nav.get_position(), guided_pos_target_cm.tofloat());
|
||||
break;
|
||||
case SubMode::PosVel:
|
||||
case SubMode::PosVelAccel:
|
||||
return pos_control->get_bearing_to_target_cd();
|
||||
break;
|
||||
case SubMode::TakeOff:
|
||||
case SubMode::Velocity:
|
||||
case SubMode::Accel:
|
||||
case SubMode::VelAccel:
|
||||
case SubMode::Angle:
|
||||
// these do not have bearings
|
||||
return 0;
|
||||
@ -795,10 +993,10 @@ float ModeGuided::crosstrack_error() const
|
||||
{
|
||||
switch (guided_mode) {
|
||||
case SubMode::WP:
|
||||
return wp_nav->crosstrack_error();
|
||||
case SubMode::TakeOff:
|
||||
case SubMode::Velocity:
|
||||
case SubMode::PosVel:
|
||||
case SubMode::Accel:
|
||||
case SubMode::VelAccel:
|
||||
case SubMode::PosVelAccel:
|
||||
case SubMode::Angle:
|
||||
// no track to have a crosstrack to
|
||||
return 0;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user