From 745dd865bbe00340c431a4102e9305b547c38bd4 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 11 May 2021 14:03:13 +0930 Subject: [PATCH 01/34] Copter: support for acceleration-based AttitudeControl --- ArduCopter/Copter.h | 2 +- ArduCopter/GCS_Mavlink.cpp | 72 ++++--- ArduCopter/Log.cpp | 17 +- ArduCopter/Parameters.cpp | 2 +- ArduCopter/mode.h | 31 ++- ArduCopter/mode_guided.cpp | 416 +++++++++++++++++++++++++++---------- 6 files changed, 391 insertions(+), 149 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0ed08e0663f9e..c2dac776649b3 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -791,7 +791,7 @@ class Copter : public AP_Vehicle { #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(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 953a2d1ca82ec..4be486ff2e9f7 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -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::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::Velocity: + 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; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 344e36d556697..8e4354b241407 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -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() {} diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index b9f19baf945bc..418370b34bfa0 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -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 diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 8665424119fcc..75eb4531dd461 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -845,11 +845,21 @@ class ModeGuided : public Mode { 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 @@ class ModeGuided : public Mode { enum class SubMode { TakeOff, WP, - Velocity, - PosVel, + PosVelAccel, + VelAccel, + Accel, Angle, }; @@ -886,19 +897,23 @@ class ModeGuided : public Mode { // 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; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index f9e612a628e7c..879f00f2d860f 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -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,45 @@ 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::Accel; + + // 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 velocity controller +void ModeGuided::velaccel_control_start() { // set guided_mode to velocity controller - guided_mode = SubMode::Velocity; + 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()); @@ -170,13 +193,16 @@ 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 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 +// 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::Accel) { + accel_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.zero(); + 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_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::Velocity) { - vel_control_start(); + 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); - // record velocity target + // set velocity and acceleration targets and zero position + guided_pos_target_cm.zero(); guided_vel_target_cms = velocity; - vel_update_time_ms = millis(); + 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_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); } + } + + // 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 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 { - set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms); + // 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()); + } +} + +// velaccel_control_run - runs the guided velocity and acceleration controller +// called from guided_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 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 +// posvelaccel_control_run - runs the guided position, velocity and acceleration controller // called from guided_run -void ModeGuided::posvel_control_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; From f16b6dd24f81049524c6dbab55e10607b027937d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 31 May 2021 10:27:52 +0930 Subject: [PATCH 02/34] AC_AttitudeControl: AC_PosControl: Change input_pos_xyz name --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 4 ++-- libraries/AC_AttitudeControl/AC_PosControl.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 9b59f6d4f7ae9..b9d735b75318c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -328,13 +328,13 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav, /// 3D position shaper /// -/// input_pos_vel_accel_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. +/// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant. /// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// The time constant also defines the time taken to achieve the maximum acceleration. /// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. -void AC_PosControl::input_pos_vel_accel_xyz(const Vector3p& pos) +void AC_PosControl::input_pos_xyz(const Vector3p& pos) { Vector3f dest_vector = (pos - _pos_target).tofloat(); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 484b646a1d8d4..8087f37fafb5f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -54,10 +54,10 @@ class AC_PosControl /// 3D position shaper /// - /// input_pos_vel_accel_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. + /// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant. - void input_pos_vel_accel_xyz(const Vector3p& pos); + void input_pos_xyz(const Vector3p& pos); /// /// Lateral position controller From 1b506b9e08232c16110ea71b8e1a4a0f89883766 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 20 Jun 2021 15:32:24 +0930 Subject: [PATCH 03/34] AC_AttitudeControl: AC_PosControl: Support Accel only input --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 14 ++++++++++++++ libraries/AC_AttitudeControl/AC_PosControl.h | 7 +++++++ 2 files changed, 21 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index b9d735b75318c..51444aa5a7d79 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -481,6 +481,20 @@ void AC_PosControl::init_xy() _last_update_xy_us = AP_HAL::micros64(); } +/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. +/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. +/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant. +/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. +/// The time constant also defines the time taken to achieve the maximum acceleration. +void AC_PosControl::input_accel_xy(const Vector3f& accel) +{ + // check for ekf xy position reset + handle_ekf_xy_reset(); + + update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy()); + shape_accel_xy(accel, _accel_desired, _accel_max_xy_cmss, _tc_xy_s, _dt); +} + /// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. /// The vel is projected forwards in time based on a time step of dt and acceleration accel. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 8087f37fafb5f..7fc729ce93667 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -89,6 +89,13 @@ class AC_PosControl /// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration. void relax_velocity_controller_xy(); + /// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. + /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. + /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant. + /// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. + /// The time constant also defines the time taken to achieve the maximum acceleration. + void input_accel_xy(const Vector3f& accel); + /// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant. From a83b356d6052d0885f819e455958beca9c6f1a68 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 20 Jun 2021 15:32:52 +0930 Subject: [PATCH 04/34] AC_Math: Control: Support Accel only input --- libraries/AP_Math/control.cpp | 11 +++++++++++ libraries/AP_Math/control.h | 3 +++ 2 files changed, 14 insertions(+) diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index 05e60e19526fc..d53d7d8261961 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -155,6 +155,17 @@ void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel, } } +void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel, + float accel_max, float tc, float dt) +{ + const Vector2f accel_input_2f {accel_input.x, accel_input.y}; + Vector2f accel_2f {accel.x, accel.y}; + + shape_accel_xy(accel_input_2f, accel_2f, accel_max, tc, dt); + accel.x = accel_2f.x; + accel.y = accel_2f.y; +} + /* shape_vel_accel and shape_vel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input velocity. The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h index 29058070531c0..b6c5f9c5d7709 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -53,6 +53,9 @@ void shape_accel(const float accel_input, float& accel, void shape_accel_xy(const Vector2f& accel_input, Vector2f& accel, const float accel_max, const float tc, const float dt); +void shape_accel_xy(const Vector3f& accel_input, Vector3f& accel, + float accel_max, float tc, float dt); + /* shape_vel calculates a jerk limited path from the current velocity and acceleration to an input velocity. The function takes the current velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. The kinematic path is constrained by : From 4e3aecd4a8b8908f6b1ae0fc67e99ce66bfb3adc Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 22 Jun 2021 13:56:01 +0930 Subject: [PATCH 05/34] Tools: Autotest update guided bitbask to include acceleration --- Tools/autotest/arducopter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 23b495d0a9187..8da54bc2342c1 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -3693,8 +3693,8 @@ def test_guided_local_velocity_target(self, vx, vy, vz_up, timeout=3): self.progress("Received local target: %s" % str(m)) # Check the last received message - if not (m.type_mask == 0xFFC7 or m.type_mask == 0x0FC7): - raise NotAchievedException("Did not receive proper mask: expected=65479 or 4039, got=%u" % m.type_mask) + if not (m.type_mask == 0xFE07 or m.type_mask == 0x0E07): + raise NotAchievedException("Did not receive proper mask: expected=65031 or 3591, got=%u" % m.type_mask) if vx - m.vx > 0.1: raise NotAchievedException("Did not receive proper target velocity vx: wanted=%f got=%f" % (vx, m.vx)) From f1c69fcccd0e693bf3602cf2b4278ec79431febf Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 23 Jun 2021 23:42:08 +0930 Subject: [PATCH 06/34] Sub: adjust for AttitudeControl library changes --- ArduSub/control_guided.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index e7b393d77f8f2..13337b61b4e7f 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -13,8 +13,7 @@ static Vector3p posvel_pos_target_cm; static Vector3f posvel_vel_target_cms; -static uint32_t posvel_update_time_ms; -static uint32_t vel_update_time_ms; +static uint32_t update_time_ms; struct { uint32_t update_time_ms; @@ -189,7 +188,7 @@ void Sub::guided_set_velocity(const Vector3f& velocity) guided_vel_control_start(); } - vel_update_time_ms = AP_HAL::millis(); + update_time_ms = AP_HAL::millis(); // set position controller velocity target pos_control.set_vel_desired_cms(velocity); @@ -213,7 +212,7 @@ bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vecto } #endif - posvel_update_time_ms = AP_HAL::millis(); + update_time_ms = AP_HAL::millis(); posvel_pos_target_cm = destination.topostype(); posvel_vel_target_cms = velocity; @@ -358,7 +357,7 @@ void Sub::guided_vel_control_run() // set velocity to zero if no updates received for 3 seconds uint32_t tnow = AP_HAL::millis(); - if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_vel_desired_cms().is_zero()) { + if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_vel_desired_cms().is_zero()) { pos_control.set_vel_desired_cms(Vector3f(0,0,0)); } @@ -415,7 +414,7 @@ void Sub::guided_posvel_control_run() // set velocity to zero if no updates received for 3 seconds uint32_t tnow = AP_HAL::millis(); - if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) { + if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) { posvel_vel_target_cms.zero(); } From f071bd2597bfbe2e803076aeb2e43837b43c1d3b Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 24 Jun 2021 00:10:46 +0930 Subject: [PATCH 07/34] AC_WPNav: move code to generate terrain following kinematic path --- libraries/AC_WPNav/AC_WPNav.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index bb93552a64062..7126d4fe64713 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -421,6 +421,14 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) return false; } + // input shape the terrain offset + shape_pos_vel_accel(terr_offset, 0.0f, 0.0f, + _pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, + 0.0f, _pos_control.get_max_speed_down_cms(), _pos_control.get_max_speed_up_cms(), + -_pos_control.get_max_accel_z_cmss(), _pos_control.get_max_accel_z_cmss(), _pos_control.get_shaping_tc_z_s(), dt); + + update_pos_vel_accel(_pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, dt, 0.0f); + // get current position and adjust altitude to origin and destination's frame (i.e. _frame) const Vector3f &curr_pos = _inav.get_position() - Vector3f{0, 0, terr_offset}; @@ -465,14 +473,6 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) s_finished = _spline_this_leg.reached_destination(); } - // input shape the terrain offset - shape_pos_vel_accel(terr_offset, 0.0f, 0.0f, - _pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, - 0.0f, _pos_control.get_max_speed_down_cms(), _pos_control.get_max_speed_up_cms(), - -_pos_control.get_max_accel_z_cmss(), _pos_control.get_max_accel_z_cmss(), _pos_control.get_shaping_tc_z_s(), dt); - - update_pos_vel_accel(_pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, dt, 0.0f); - // convert final_target.z to altitude above the ekf origin target_pos.z += _pos_terrain_offset; target_vel.z += _vel_terrain_offset; From bdc37a2a392e768a011f7a4708d632431321334c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 26 Jun 2021 10:08:49 +0900 Subject: [PATCH 08/34] AC_WPNav: get_terrain_offset and get_vector_NEU made public --- libraries/AC_WPNav/AC_WPNav.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index f8afa706c83b6..1dc653f3cdd2c 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -49,6 +49,13 @@ class AC_WPNav }; AC_WPNav::TerrainSource get_terrain_source() const; + // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) + bool get_terrain_offset(float& offset_cm); + + // convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain + // returns false if conversion failed (likely because terrain data was not available) + bool get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_alt); + /// /// waypoint controller /// @@ -216,13 +223,6 @@ class AC_WPNav uint8_t wp_yaw_set : 1; // true if yaw target has been set } _flags; - // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) - bool get_terrain_offset(float& offset_cm); - - // convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain - // returns false if conversion failed (likely because terrain data was not available) - bool get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_alt); - // helper function to calculate scurve jerk and jerk_time values // updates _scurve_jerk and _scurve_jerk_time void calc_scurve_jerk_and_jerk_time(); From 0e7c08565dc58e434c78bf511b2418f393a1878c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 26 Jun 2021 10:10:03 +0900 Subject: [PATCH 09/34] Copter: guided accepts terrain alt position targets --- ArduCopter/Copter.h | 2 +- ArduCopter/Log.cpp | 10 ++++-- ArduCopter/mode_guided.cpp | 74 +++++++++++++++++++++++++++++++++----- 3 files changed, 73 insertions(+), 13 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c2dac776649b3..adcf2a3b371a5 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -791,7 +791,7 @@ class Copter : public AP_Vehicle { #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, const Vector3f& accel_target); + void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, bool terrain_alt, 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(); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 8e4354b241407..1954a683bf606 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -374,6 +374,7 @@ struct PACKED log_GuidedTarget { float pos_target_x; float pos_target_y; float pos_target_z; + uint8_t terrain; float vel_target_x; float vel_target_y; float vel_target_z; @@ -384,8 +385,9 @@ struct PACKED log_GuidedTarget { // 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 +// terrain should be 0 if pos_target.z is alt-above-ekf-origin, 1 if alt-above-terrain // vel_target is cm/s -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_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target) { struct log_GuidedTarget pkt = { LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG), @@ -394,6 +396,7 @@ void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vecto pos_target_x : pos_target.x, pos_target_y : pos_target.y, pos_target_z : pos_target.z, + terrain : terrain_alt, vel_target_x : vel_target.x, vel_target_y : vel_target.y, vel_target_z : vel_target.z, @@ -542,6 +545,7 @@ const struct LogStructure Copter::log_structure[] = { // @Field: pX: Target position, X-Axis // @Field: pY: Target position, Y-Axis // @Field: pZ: Target position, Z-Axis +// @Field: Terrain: Target position, Z-Axis is alt above terrain // @Field: vX: Target velocity, X-Axis // @Field: vY: Target velocity, Y-Axis // @Field: vZ: Target velocity, Z-Axis @@ -550,7 +554,7 @@ const struct LogStructure Copter::log_structure[] = { // @Field: aZ: Target acceleration, Z-Axis { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), - "GUID", "QBfffffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ,aX,aY,aZ", "s-mmmnnnooo", "F-BBBBBBBBB" }, + "GUID", "QBfffbffffff", "TimeUS,Type,pX,pY,pZ,Terrain,vX,vY,vZ,aX,aY,aZ", "s-mmm-nnnooo", "F-BBB-BBBBBB" }, }; void Copter::Log_Write_Vehicle_Startup_Messages() @@ -582,7 +586,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, const Vector3f& accel_target) {} +void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, bool terrain_alt, 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() {} diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 879f00f2d860f..04b010f2627be 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -14,6 +14,7 @@ #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) +bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain 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 @@ -270,17 +271,36 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa pos_control_start(); } + // initialise terrain following if needed + if (terrain_alt) { + // get current alt above terrain + float origin_terr_offset; + if (!wp_nav->get_terrain_offset(origin_terr_offset)) { + // if we don't have terrain altitude then stop + init(true); + return false; + } + // convert origin to alt-above-terrain if necessary + if (!guided_pos_terrain_alt) { + // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin + pos_control->set_pos_offset_z_cm(origin_terr_offset); + } + } else { + pos_control->set_pos_offset_z_cm(0.0); + } + // 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 guided_pos_target_cm = destination.topostype(); + guided_pos_terrain_alt = terrain_alt; guided_vel_target_cms.zero(); guided_accel_target_cmss.zero(); update_time_ms = millis(); // log target - copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss); + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); send_notification = true; @@ -320,17 +340,36 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y // 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(); + bool terrain_alt; + if (!wp_nav->get_vector_NEU(dest_loc, pos_target_f, terrain_alt)) { + return false; + } + + // initialise terrain following if needed + if (terrain_alt) { + // get current alt above terrain + float origin_terr_offset; + if (!wp_nav->get_terrain_offset(origin_terr_offset)) { + // if we don't have terrain altitude then stop + init(true); + return false; + } + // convert origin to alt-above-terrain if necessary + if (!guided_pos_terrain_alt) { + // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin + pos_control->set_pos_offset_z_cm(origin_terr_offset); + } } else { - guided_pos_target_cm = pos_target_f.topostype(); + pos_control->set_pos_offset_z_cm(0.0); } + guided_pos_target_cm = pos_target_f.topostype(); + guided_pos_terrain_alt = terrain_alt; 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), guided_vel_target_cms, guided_accel_target_cmss); + copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); send_notification = true; @@ -350,13 +389,14 @@ void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw // set velocity and acceleration targets and zero position guided_pos_target_cm.zero(); + guided_pos_terrain_alt = false; 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, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss); + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); } } @@ -379,13 +419,14 @@ void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& accelera // set velocity and acceleration targets and zero position guided_pos_target_cm.zero(); + guided_pos_terrain_alt = false; 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); + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); } } @@ -419,11 +460,12 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const update_time_ms = millis(); guided_pos_target_cm = destination.topostype(); + guided_pos_terrain_alt = false; guided_vel_target_cms = velocity; guided_accel_target_cmss = acceleration; // log target - copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss); + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); return true; } @@ -475,6 +517,7 @@ 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), + false, Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust), Vector3f()); } @@ -516,13 +559,21 @@ void ModeGuided::pos_control_run() return; } + // calculate terrain adjustments + float terr_offset = 0.0f; + if (guided_pos_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) { + // if we don't have terrain altitude then stop + init(true); + return; + } + // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // 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); + pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset); // run position controllers pos_control->update_xy_controller(); @@ -752,6 +803,11 @@ void ModeGuided::posvelaccel_control_run() pos_control->stop_pos_xy_stabilisation(); } + // guided_pos_target z-axis should never be a terrain altitude + if (guided_pos_terrain_alt) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + float pz = guided_pos_target_cm.z; pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, guided_accel_target_cmss.z); guided_pos_target_cm.z = pz; From 9840279b4ea2bf1f2a7d846d61f0d0d9ca007e2a Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 26 Jun 2021 23:19:12 +0930 Subject: [PATCH 10/34] AC_AttitudeControl: Add terain following to guided --- .../AC_AttitudeControl/AC_PosControl.cpp | 50 ++++++++++++++++--- libraries/AC_AttitudeControl/AC_PosControl.h | 20 ++++++-- 2 files changed, 59 insertions(+), 11 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 51444aa5a7d79..69f5fbdcaabf0 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -334,9 +334,12 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav, /// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// The time constant also defines the time taken to achieve the maximum acceleration. /// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. -void AC_PosControl::input_pos_xyz(const Vector3p& pos) +void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z) { - Vector3f dest_vector = (pos - _pos_target).tofloat(); + // remove terrain offsets for flat earth assumption + _pos_target.z -= _pos_offset_z; + _vel_desired.z -= _vel_offset_z; + _accel_desired.z -= _accel_offset_z; // calculated increased maximum acceleration if over speed float accel_z_cmss = _accel_max_z_cmss; @@ -349,31 +352,41 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos) update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy()); - // adjust desired alt if motors have not hit their limits + // adjust desired altitude if motors have not hit their limits update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z); - float vel_max_xy_cms = _vel_max_xy_cms; + // calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos + float vel_max_xy_cms = 0.0f; float vel_max_z_cms = 0.0f; + Vector3f dest_vector = (pos - _pos_target).tofloat(); if (is_positive(dest_vector.length_squared()) ) { dest_vector.normalize(); float dest_vector_xy_length = dest_vector.xy().length(); float vel_max_cms = kinematic_limit(dest_vector, _vel_max_xy_cms, _vel_max_up_cms, _vel_max_down_cms); vel_max_xy_cms = vel_max_cms * dest_vector_xy_length; - vel_max_z_cms = vel_max_cms * dest_vector.z; + vel_max_z_cms = fabsf(vel_max_cms * dest_vector.z); } - Vector2f vel; Vector2f accel; shape_pos_vel_accel_xy(pos.xy(), vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt); + float posz = pos.z; shape_pos_vel_accel(posz, 0, 0, _pos_target.z, _vel_desired.z, _accel_desired.z, vel_max_z_cms, _vel_max_down_cms, _vel_max_up_cms, -constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss, _tc_z_s, _dt); + + // update the vertical position, velocity and acceleration offsets + update_pos_offset_z(pos_offset_z); + + // add terrain offsets + _pos_target.z += _pos_offset_z; + _vel_desired.z += _vel_offset_z; + _accel_desired.z += _accel_offset_z; } /// @@ -743,6 +756,11 @@ void AC_PosControl::init_z() _accel_target.z = -(curr_accel.z + GRAVITY_MSS) * 100.0f; _pid_accel_z.reset_filter(); + // initialise vertical offsets + _pos_offset_z = 0.0; + _vel_offset_z = 0.0; + _accel_offset_z = 0.0; + // initialise ekf z reset handler init_ekf_z_reset(); @@ -806,7 +824,7 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float ac accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_up_cms; } - // adjust desired alt if motors have not hit their limits + // adjust desired altitude if motors have not hit their limits update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z); shape_pos_vel_accel(pos, vel, accel, @@ -829,6 +847,21 @@ void AC_PosControl::set_alt_target_with_slew(const float& pos) input_pos_vel_accel_z(posf, zero, 0); } +/// update_pos_offset_z - updates the vertical offsets used by terrain following +void AC_PosControl::update_pos_offset_z(float pos_offset_z) +{ + + postype_t posp_z = _pos_offset_z; + update_pos_vel_accel(posp_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f)); + _pos_offset_z = posp_z; + + // input shape the terrain offset + shape_pos_vel_accel(pos_offset_z, 0.0f, 0.0f, + _pos_offset_z, _vel_offset_z, _accel_offset_z, + 0.0f, get_max_speed_down_cms(), get_max_speed_up_cms(), + -get_max_accel_z_cmss(), get_max_accel_z_cmss(), _tc_z_s, _dt); +} + // is_active_z - returns true if the z position controller has been run in the previous 5 loop times bool AC_PosControl::is_active_z() const { @@ -857,7 +890,9 @@ void AC_PosControl::update_z_controller() const float curr_alt = _inav.get_position().z; // calculate the target velocity correction float pos_target_zf = _pos_target.z; + _vel_target.z = _p_pos_z.update_all(pos_target_zf, curr_alt, _limit.pos_down, _limit.pos_up); + _pos_target.z = pos_target_zf; // add feed forward component @@ -868,6 +903,7 @@ void AC_PosControl::update_z_controller() const Vector3f& curr_vel = _inav.get_velocity(); _accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel.z, _motors.limit.throttle_lower, _motors.limit.throttle_upper); + // add feed forward component _accel_target.z += _accel_desired.z; // Acceleration Controller diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 7fc729ce93667..592b957f68c39 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -57,7 +57,7 @@ class AC_PosControl /// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant. - void input_pos_xyz(const Vector3p& pos); + void input_pos_xyz(const Vector3p& pos, float pos_offset_z); /// /// Lateral position controller @@ -184,6 +184,9 @@ class AC_PosControl /// using the default position control kinimatic path. void set_alt_target_with_slew(const float& pos); + /// update_pos_offset_z - updates the vertical offsets used by terrain following + void update_pos_offset_z(float pos_offset); + // is_active_z - returns true if the z position controller has been run in the previous 5 loop times bool is_active_z() const; @@ -215,7 +218,7 @@ class AC_PosControl /// set_pos_target_z_cm - set altitude target in cm above home void set_pos_target_z_cm(float pos_target) { _pos_target.z = pos_target; } - /// get_pos_target_z_cm - get desired altitude (in cm above home) + /// get_pos_target_z_cm - get target altitude (in cm above home) float get_pos_target_z_cm() const { return _pos_target.z; } /// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration @@ -264,6 +267,12 @@ class AC_PosControl const Vector3f& get_accel_target_cmss() const { return _accel_target; } + /// Offset + + /// set_pos_offset_z_cm - set altitude offset in cm above home + void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset_z = pos_offset_z; } + + /// Outputs /// get desired roll and pitch to be passed to the attitude controller @@ -413,10 +422,13 @@ class AC_PosControl Vector3f _accel_target; // acceleration target in NEU cm/s/s Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set + float _pos_offset_z; // vertical position offset in NEU cm from home + float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step + float _accel_offset_z; // vertical acceleration offset in NEU cm/s/s // ekf reset handling - uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset - uint32_t _ekf_z_reset_ms; // system time of last recorded ekf altitude reset + uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset + uint32_t _ekf_z_reset_ms; // system time of last recorded ekf altitude reset // high vibration handling bool _vibe_comp_enabled; // true when high vibration compensation is on From df13f6bb4a670749087444435e4666ce299b5a20 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:39:36 +0930 Subject: [PATCH 11/34] Copter: Guided prevent takeoff without takeoff command. --- ArduCopter/mode_guided.cpp | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 04b010f2627be..7ee4b2b67132b 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -606,17 +606,6 @@ void ModeGuided::accel_control_run() } } - // 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(); @@ -677,17 +666,6 @@ void ModeGuided::velaccel_control_run() } } - // 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(); From c7ef92e8f0470580a27d54e9b4e1cd5eabb2892f Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:41:54 +0930 Subject: [PATCH 12/34] Copter: Guided: use common initialisation --- ArduCopter/mode.h | 1 + ArduCopter/mode_guided.cpp | 54 ++++++++++++++------------------------ 2 files changed, 21 insertions(+), 34 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 75eb4531dd461..fbdaef97056ec 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -905,6 +905,7 @@ class ModeGuided : public Mode { DoNotStabilizeVelocityXY = (1U << 5), }; + void pva_control_start(); void pos_control_start(); void accel_control_start(); void velaccel_control_start(); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 7ee4b2b67132b..af8a86ca09c46 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -139,12 +139,9 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) return true; } -// initialise guided mode's position controller -void ModeGuided::pos_control_start() +// initialise position controller +void ModeGuided::pva_control_start() { - // set to position control mode - guided_mode = SubMode::WP; - // initialise horizontal speed, acceleration pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); @@ -154,6 +151,16 @@ void ModeGuided::pos_control_start() // initialise velocity controller pos_control->init_z_controller(); pos_control->init_xy_controller(); +} + +// initialise guided mode's position controller +void ModeGuided::pos_control_start() +{ + // set to position control mode + guided_mode = SubMode::WP; + + // initialise position controller + pva_control_start(); // initialise yaw auto_yaw.set_mode_to_default(false); @@ -165,55 +172,34 @@ void ModeGuided::accel_control_start() // set guided_mode to velocity controller 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()); - - // 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(); + // initialise position controller + pva_control_start(); // pilot always controls yaw auto_yaw.set_mode(AUTO_YAW_HOLD); } -// initialise guided mode's velocity controller +// initialise guided mode's velocity and acceleration 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(); + // initialise position controller + pva_control_start(); // pilot always controls yaw auto_yaw.set_mode(AUTO_YAW_HOLD); } -// initialise guided mode's posvel controller +// initialise guided mode's position, velocity and acceleration controller void ModeGuided::posvelaccel_control_start() { // set guided_mode to velocity controller 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()); - - // 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(); + // initialise position controller + pva_control_start(); // pilot always controls yaw auto_yaw.set_mode(AUTO_YAW_HOLD); From 7844d829747ecde8cf11009253c7ee2f877bfd24 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:43:05 +0930 Subject: [PATCH 13/34] AC_AttitudeControl: AC_PosControl: seperate kinimatic shaping from pid limit setting --- .../AC_AttitudeControl/AC_PosControl.cpp | 34 ++++++++++++++----- libraries/AC_AttitudeControl/AC_PosControl.h | 24 ++++++++++--- 2 files changed, 45 insertions(+), 13 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 69f5fbdcaabf0..d01f84e61d7a9 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -393,7 +393,10 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z) /// Lateral position controller /// -/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s and position controller correction acceleration limit +/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s +/// This function only needs to be called if using the kinimatic shaping. +/// This can be done at any time as changes in these parameters are handled smoothly +/// by the kinimatic shaping. void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss) { // return immediately if no change @@ -403,8 +406,6 @@ void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss) _vel_max_xy_cms = speed_cms; _accel_max_xy_cmss = accel_cmss; - _p_pos_xy.set_limits(_vel_max_xy_cms, _accel_max_xy_cmss, 0.0f); - // ensure the horizontal time constant is not less than the vehicle is capable of const float lean_angle = _accel_max_xy_cmss / (GRAVITY_MSS * 100.0 * M_PI / 18000.0); const float angle_accel = MIN(_attitude_control.get_accel_pitch_max(), _attitude_control.get_accel_roll_max()); @@ -415,6 +416,13 @@ void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss) } } +/// set_max_speed_accel_xy - set the position controller correction velocity and acceleration limit +/// This should be done only during initialisation to avoid discontinuities +void AC_PosControl::set_correction_speed_accel_xy(float speed_cms, float accel_cmss) +{ + _p_pos_xy.set_limits(speed_cms, accel_cmss, 0.0f); +} + /// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude. /// This function is the default initialisation for any position control that provides position, velocity and acceleration. void AC_PosControl::init_xy_controller() @@ -646,8 +654,11 @@ void AC_PosControl::update_xy_controller() /// Vertical position controller /// -/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s and position controller correction acceleration limit -/// speed_down can be positive or negative but will always be interpreted as a descent speed +/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s +/// speed_down can be positive or negative but will always be interpreted as a descent speed. +/// This function only needs to be called if using the kinimatic shaping. +/// This can be done at any time as changes in these parameters are handled smoothly +/// by the kinimatic shaping. void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, float accel_cmss) { // ensure speed_down is always negative @@ -668,8 +679,6 @@ void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, floa if (is_positive(accel_cmss)) { _accel_max_z_cmss = accel_cmss; } - // define maximum position error and maximum first and second differential limits - _p_pos_z.set_limits(-fabsf(_vel_max_down_cms), _vel_max_up_cms, _accel_max_z_cmss, 0.0f); // ensure the vertical time constant is not less than the filters in the _pid_accel_z object _tc_z_s = _shaping_tc_z_s; @@ -681,6 +690,15 @@ void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, floa } } +/// set_correction_speed_accel_z - set the position controller correction velocity and acceleration limit +/// speed_down can be positive or negative but will always be interpreted as a descent speed. +/// This should be done only during initialisation to avoid discontinuities +void AC_PosControl::set_correction_speed_accel_z(float speed_down, float speed_up, float accel_cmss) +{ + // define maximum position error and maximum first and second differential limits + _p_pos_z.set_limits(-fabsf(speed_down), _vel_max_up_cms, _accel_max_z_cmss, 0.0f); +} + /// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude. /// This function is the default initialisation for any position control that provides position, velocity and acceleration. void AC_PosControl::init_z_controller() @@ -1165,7 +1183,7 @@ bool AC_PosControl::calculate_yaw_and_rate_yaw() } } - // update the target yaw if origin and destination are at least 2m apart horizontally + // update the target yaw if velocity is greater than 5% _vel_max_xy_cms if (vel_desired_xy_len > _vel_max_xy_cms * 0.05f) { _yaw_target = degrees(vel_desired_xy.angle()) * 100.0f; _yaw_rate_target = turn_rate*degrees(100.0f); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 592b957f68c39..d0decc7ca3be9 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -64,8 +64,15 @@ class AC_PosControl /// /// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s + /// This function only needs to be called if using the kinimatic shaping. + /// This can be done at any time as changes in these parameters are handled smoothly + /// by the kinimatic shaping. void set_max_speed_accel_xy(float speed_cms, float accel_cmss); + /// set_max_speed_accel_xy - set the position controller correction velocity and acceleration limit + /// This should be done only during initialisation to avoid discontinuities + void set_correction_speed_accel_xy(float speed_cms, float accel_cmss); + /// get_max_speed_xy_cms - get the maximum horizontal speed in cm/s float get_max_speed_xy_cms() const { return _vel_max_xy_cms; } @@ -129,8 +136,15 @@ class AC_PosControl /// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s /// speed_down can be positive or negative but will always be interpreted as a descent speed + /// This can be done at any time as changes in these parameters are handled smoothly + /// by the kinimatic shaping. void set_max_speed_accel_z(float speed_down, float speed_up, float accel_cmss); + /// set_correction_speed_accel_z - set the position controller correction velocity and acceleration limit + /// speed_down can be positive or negative but will always be interpreted as a descent speed + /// This should be done only during initialisation to avoid discontinuities + void set_correction_speed_accel_z(float speed_down, float speed_up, float accel_cmss); + /// get_max_accel_z_cmss - get the maximum vertical acceleration in cm/s/s float get_max_accel_z_cmss() const { return _accel_max_z_cmss; } @@ -401,11 +415,11 @@ class AC_PosControl uint64_t _last_update_z_us; // system time (in microseconds) since last update_z_controller call float _tc_xy_s; // time constant of the xy kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target float _tc_z_s; // time constant of the z kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target - float _vel_max_xy_cms; // max horizontal speed in cm/s - float _vel_max_up_cms; // max climb rate in cm/s - float _vel_max_down_cms; // max descent rate in cm/s - float _accel_max_xy_cmss; // max horizontal acceleration in cm/s/s - float _accel_max_z_cmss; // max vertical acceleration in cm/s/s + float _vel_max_xy_cms; // max horizontal speed in cm/s used for kinematic shaping + float _vel_max_up_cms; // max climb rate in cm/s used for kinematic shaping + float _vel_max_down_cms; // max descent rate in cm/s used for kinematic shaping + float _accel_max_xy_cmss; // max horizontal acceleration in cm/s/s used for kinematic shaping + float _accel_max_z_cmss; // max vertical acceleration in cm/s/s used for kinematic shaping float _vel_z_control_ratio = 2.0f; // confidence that we have control in the vertical axis // output from controller From d408c7908aa6917741021092dc78093be47e1e48 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:44:01 +0930 Subject: [PATCH 14/34] AC_WPNav: seperate kinimatic shaping from pid limit setting --- libraries/AC_WPNav/AC_Loiter.cpp | 16 ++++++---------- libraries/AC_WPNav/AC_WPNav.cpp | 3 ++- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 7d426ec771da1..4a903ef6af74c 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -90,8 +90,10 @@ void AC_Loiter::init_target(const Vector2f& position) { sanity_check_params(); - // initialise pos controller speed, acceleration - _pos_control.set_max_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); + // initialise position controller speed and acceleration + _pos_control.set_correction_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); + _pos_control.set_pos_error_max_xy_cm(LOITER_POS_CORRECTION_MAX); + // initialise position controller _pos_control.init_xy_controller_stopping_point(); @@ -111,11 +113,11 @@ void AC_Loiter::init_target() sanity_check_params(); // initialise position controller speed and acceleration - _pos_control.set_max_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); + _pos_control.set_correction_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); + _pos_control.set_pos_error_max_xy_cm(LOITER_POS_CORRECTION_MAX); // initialise position controller _pos_control.init_xy_controller(); - _pos_control.set_pos_error_max_xy_cm(LOITER_POS_CORRECTION_MAX); // initialise predicted acceleration and angles from the position controller _predicted_accel.x = _pos_control.get_accel_target_cmss().x; @@ -191,9 +193,6 @@ float AC_Loiter::get_angle_max_cd() const /// run the loiter controller void AC_Loiter::update(bool avoidance_on) { - // initialise pos controller speed and acceleration - _pos_control.set_max_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); - calc_desired_velocity(_pos_control.get_dt(), avoidance_on); _pos_control.update_xy_controller(); } @@ -224,9 +223,6 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on) return; } - // initialise pos controller speed, acceleration - _pos_control.set_max_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss); - // get loiters desired velocity from the position controller where it is being stored. const Vector3f &desired_vel_3d = _pos_control.get_vel_desired_cms(); Vector2f desired_vel{desired_vel_3d.x,desired_vel_3d.y}; diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 7126d4fe64713..47e3986b9a5f4 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -141,7 +141,9 @@ void AC_WPNav::wp_and_spline_init(float speed_cms) // initialise position controller speed and acceleration _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); + _pos_control.set_correction_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); _pos_control.set_max_speed_accel_z(-_wp_speed_down_cms, _wp_speed_up_cms, _wp_accel_z_cmss); + _pos_control.set_correction_speed_accel_z(-_wp_speed_down_cms, _wp_speed_up_cms, _wp_accel_z_cmss); // calculate scurve jerk and jerk time if (!is_positive(_wp_jerk)) { @@ -177,7 +179,6 @@ void AC_WPNav::set_speed_xy(float speed_cms) // range check target speed if (speed_cms >= WPNAV_WP_SPEED_MIN) { _wp_desired_speed_xy_cms = speed_cms; - _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); update_track_with_speed_accel_limits(); } } From 7f9195a2a44d5a3bc9aa5f634cc38f7890b967d2 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:46:08 +0930 Subject: [PATCH 15/34] Sub: seperate kinimatic shaping from pid limit setting --- ArduSub/control_althold.cpp | 4 +--- ArduSub/control_auto.cpp | 1 + ArduSub/control_circle.cpp | 2 ++ ArduSub/control_guided.cpp | 3 +++ ArduSub/control_poshold.cpp | 2 ++ ArduSub/control_surface.cpp | 1 + 6 files changed, 10 insertions(+), 3 deletions(-) diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 268c06a17051c..c72a2401e8628 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -15,9 +15,7 @@ bool Sub::althold_init() // initialize vertical maximum speeds and acceleration // sets the maximum speed up and down returned by position controller pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - - // initialize vertical speeds and acceleration - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity pos_control.init_z_controller(); diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index a381cb4e0d196..8f100a5432fc9 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -572,6 +572,7 @@ bool Sub::auto_terrain_recover_start() // initialize vertical maximum 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()); + pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery"); return true; diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index 0dec0cd2b85bd..9f057d9111322 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -15,7 +15,9 @@ bool Sub::circle_init() // initialize speeds and accelerations pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); + pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise circle controller including setting the circle center based on vehicle speed circle_nav.init(); diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 13337b61b4e7f..987c9b2383caa 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -74,6 +74,7 @@ void Sub::guided_vel_control_start() // initialize vertical maximum speeds and acceleration pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise velocity controller pos_control.init_z_controller(); @@ -88,6 +89,7 @@ void Sub::guided_posvel_control_start() // set vertical speed 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()); + pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); // initialise velocity controller pos_control.init_z_controller(); @@ -105,6 +107,7 @@ void Sub::guided_angle_control_start() // set vertical speed 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()); + pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); // initialise velocity controller pos_control.init_z_controller(); diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index e70d3589a34ee..46121952ae054 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -16,7 +16,9 @@ bool Sub::poshold_init() // initialize vertical speeds and acceleration pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); + pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity pos_control.init_xy_controller_stopping_point(); diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index d3becc5366132..6c1cca1109543 100644 --- a/ArduSub/control_surface.cpp +++ b/ArduSub/control_surface.cpp @@ -9,6 +9,7 @@ bool Sub::surface_init() // initialize vertical speeds and acceleration pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity pos_control.init_z_controller(); From dd4827f93a5b631e960c935d5722633c011cab80 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:46:41 +0930 Subject: [PATCH 16/34] Plane: seperate kinimatic shaping from pid limit setting --- ArduPlane/qautotune.cpp | 3 +++ ArduPlane/quadplane.cpp | 8 ++++++++ 2 files changed, 11 insertions(+) diff --git a/ArduPlane/qautotune.cpp b/ArduPlane/qautotune.cpp index 3f9a385202aa5..141ae621d88bf 100644 --- a/ArduPlane/qautotune.cpp +++ b/ArduPlane/qautotune.cpp @@ -45,6 +45,9 @@ void QAutoTune::init_z_limits() plane.quadplane.pos_control->set_max_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(), plane.quadplane.pilot_velocity_z_max_up, plane.quadplane.pilot_accel_z); + plane.quadplane.pos_control->set_correction_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(), + plane.quadplane.pilot_velocity_z_max_up, + plane.quadplane.pilot_accel_z); } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 11a0f41b5b47f..64800a4fb2954 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1134,6 +1134,7 @@ void QuadPlane::init_hover(void) { // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); set_climb_rate_cms(0, false); init_throttle_wait(); @@ -1320,6 +1321,7 @@ void QuadPlane::init_loiter(void) // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); init_throttle_wait(); @@ -2562,6 +2564,7 @@ void QuadPlane::run_xy_controller(void) { if (!pos_control->is_active_xy()) { pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); pos_control->init_xy_controller(); } pos_control->update_xy_controller(); @@ -2600,6 +2603,8 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) // back to normal qp.pos_control->set_max_speed_accel_xy(qp.wp_nav->get_default_speed_xy(), qp.wp_nav->get_wp_acceleration()); + qp.pos_control->set_correction_speed_accel_xy(qp.wp_nav->get_default_speed_xy(), + qp.wp_nav->get_wp_acceleration()); } else if (s == QPOS_AIRBRAKE) { // start with zero integrator on vertical throttle qp.pos_control->get_accel_z_pid().set_integrator(0); @@ -2907,6 +2912,7 @@ void QuadPlane::vtol_position_controller(void) const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle())); pos_control->set_max_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration()); run_xy_controller(); @@ -3098,6 +3104,7 @@ void QuadPlane::setup_target_position(void) // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); } /* @@ -3275,6 +3282,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); // initialise the vertical position controller pos_control->init_z_controller(); From 9d503130fb573372ce51d4e5964c41c3531166ab Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:47:41 +0930 Subject: [PATCH 17/34] Copter: seperate kinimatic shaping from pid limit setting --- ArduCopter/mode_althold.cpp | 5 +++++ ArduCopter/mode_autorotate.cpp | 1 + ArduCopter/mode_autotune.cpp | 1 + ArduCopter/mode_brake.cpp | 2 ++ ArduCopter/mode_circle.cpp | 2 ++ ArduCopter/mode_flowhold.cpp | 1 + ArduCopter/mode_guided.cpp | 3 +++ ArduCopter/mode_land.cpp | 1 + ArduCopter/mode_loiter.cpp | 4 ++++ ArduCopter/mode_poshold.cpp | 1 + ArduCopter/mode_sport.cpp | 1 + ArduCopter/mode_throw.cpp | 2 ++ ArduCopter/mode_zigzag.cpp | 4 ++++ 13 files changed, 28 insertions(+) diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 8bc8ed5da900d..b272cdd823aa0 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -8,11 +8,16 @@ // althold_init - initialise althold controller bool ModeAltHold::init(bool ignore_checks) { + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + return true; } diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index a60ab9ea5192d..5b47f2fb78a00 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -239,6 +239,7 @@ void ModeAutorotate::run() // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); + pos_control->set_correction_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 1cd0074193fd4..e1ca2680bbb2e 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -95,6 +95,7 @@ void AutoTune::init_z_limits() { // set vertical speed and acceleration limits copter.pos_control->set_max_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z); + copter.pos_control->set_correction_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z); } void AutoTune::log_pids() diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 30da6448ae405..a11bec1aac80b 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -11,12 +11,14 @@ bool ModeBrake::init(bool ignore_checks) { // initialise pos controller speed and acceleration pos_control->set_max_speed_accel_xy(inertial_nav.get_velocity().length(), BRAKE_MODE_DECEL_RATE); + pos_control->set_correction_speed_accel_xy(inertial_nav.get_velocity().length(), BRAKE_MODE_DECEL_RATE); // initialise position controller pos_control->init_xy_controller(); // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE); + pos_control->set_correction_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE); // initialise the vertical position controller if (!pos_control->is_active_z()) { diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 6a0b8822ed198..f34b9263f1858 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -14,7 +14,9 @@ bool ModeCircle::init(bool ignore_checks) // set speed and acceleration limits pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise circle controller including setting the circle center based on vehicle speed copter.circle_nav->init(); diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 31050a3d743d2..d1f654fe23198 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -90,6 +90,7 @@ bool ModeFlowHold::init(bool ignore_checks) // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise the vertical position controller if (!copter.pos_control->is_active_z()) { diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index af8a86ca09c46..742a5c419a52b 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -144,9 +144,11 @@ void ModeGuided::pva_control_start() { // initialise horizontal speed, acceleration pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); // 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()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); // initialise velocity controller pos_control->init_z_controller(); @@ -218,6 +220,7 @@ void ModeGuided::angle_control_start() // 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()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); // initialise the vertical position controller if (!pos_control->is_active_z()) { diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index ff6cb0c999ed9..9f656314b93b8 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -14,6 +14,7 @@ bool ModeLand::init(bool ignore_checks) // 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()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); // initialise the vertical position controller if (!pos_control->is_active_z()) { diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index dcb397cb79273..1ee740d6130ae 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -30,6 +30,10 @@ bool ModeLoiter::init(bool ignore_checks) pos_control->init_z_controller(); } + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + return true; } diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 430b71f10533c..d43dfb386a279 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -29,6 +29,7 @@ bool ModePosHold::init(bool ignore_checks) { // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise the vertical position controller if (!pos_control->is_active_z()) { diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index d495334d27aaf..9051c97eb877d 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -11,6 +11,7 @@ bool ModeSport::init(bool ignore_checks) { // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise the vertical position controller if (!pos_control->is_active_z()) { diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 7743d4b53886e..34d954c643dc7 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -21,9 +21,11 @@ bool ModeThrow::init(bool ignore_checks) // initialise pos controller speed and acceleration pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), BRAKE_MODE_DECEL_RATE); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), BRAKE_MODE_DECEL_RATE); // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE); + pos_control->set_correction_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE); return true; } diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 3941c715583db..cd666e7909562 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -83,6 +83,10 @@ bool ModeZigZag::init(bool ignore_checks) } loiter_nav->init_target(); + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); From 2a83f49c6d9a2f512ec6e7accc7ece60e1de5a3e Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:51:38 +0930 Subject: [PATCH 18/34] Copter: Guided close gap between TARGET_LOCAL_NED and TARGET_GLOBAL_INT --- ArduCopter/GCS_Mavlink.cpp | 44 +++++++++++++++++++++++++------------- 1 file changed, 29 insertions(+), 15 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 4be486ff2e9f7..806d2051dc81e 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1118,11 +1118,6 @@ 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 neither position nor velocity is provided - if (pos_ignore && vel_ignore) { - break; - } - // prepare position Vector3f pos_vector; if (!pos_ignore) { @@ -1178,12 +1173,15 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) // send request if (!pos_ignore && !vel_ignore) { 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_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_accel(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); + } else { + // input is not valid so stop + copter.mode_guided.init(true); } break; @@ -1200,17 +1198,14 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) break; } + // todo: do we need to check for supported coordinate frames + bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; 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) { - break; - } - // extract location from message Location loc; if (!pos_ignore) { @@ -1226,6 +1221,20 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame}; } + // prepare velocity + Vector3f vel_vector; + if (!vel_ignore) { + // convert to cm + vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f); + } + + // 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); + } + // prepare yaw float yaw_cd = 0.0f; bool yaw_relative = false; @@ -1249,11 +1258,16 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) { break; } - copter.mode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } else if (pos_ignore && !vel_ignore) { - copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); - } else if (!pos_ignore && vel_ignore) { + 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_accel(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(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + } else { + // input is not valid so stop + copter.mode_guided.init(true); } break; From 7c1f843d95face26d971dea677952721397c1fd4 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 14:20:42 +0930 Subject: [PATCH 19/34] Copter: Guided: fix waypoint track reporting --- ArduCopter/mode_guided.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 742a5c419a52b..1be591832b81e 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -301,7 +301,8 @@ bool ModeGuided::get_wp(Location& destination) if (guided_mode != SubMode::WP) { return false; } - return wp_nav->get_oa_wp_destination(destination); + destination = Location(guided_pos_target_cm.tofloat(), guided_pos_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + return true; } // sets guided mode's target from a Location object From 415a9504443a243a73d02a80a4699efa83cb8890 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 16:39:05 +0930 Subject: [PATCH 20/34] AC_AttitudeControl: AC_PosControl: support terrain following --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 17 ++++++++++++++++- libraries/AC_AttitudeControl/AC_PosControl.h | 5 ++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index d01f84e61d7a9..80effd1b01faa 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -334,7 +334,7 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav, /// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// The time constant also defines the time taken to achieve the maximum acceleration. /// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. -void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z) +void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer) { // remove terrain offsets for flat earth assumption _pos_target.z -= _pos_offset_z; @@ -368,6 +368,9 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z) vel_max_z_cms = fabsf(vel_max_cms * dest_vector.z); } + // reduce speed if we are reaching the edge of our vertical buffer + vel_max_xy_cms *= pos_offset_z_scaler(pos_offset_z, pos_offset_z_buffer); + Vector2f vel; Vector2f accel; shape_pos_vel_accel_xy(pos.xy(), vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), @@ -389,6 +392,18 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z) _accel_desired.z += _accel_offset_z; } + +/// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range +float AC_PosControl::pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer) +{ + if (is_zero(pos_offset_z_buffer)) { + return 1.0; + } + const Vector3f curr_pos = _inav.get_position(); + float pos_offset_error_z = curr_pos.z - (_pos_target.z + pos_offset_z); + return constrain_float((1.0 - (fabsf(pos_offset_error_z) - 0.5 * pos_offset_z_buffer) / (0.5 * pos_offset_z_buffer)), 0.01, 1.0); +} + /// /// Lateral position controller /// diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index d0decc7ca3be9..baf4409fc2684 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -57,7 +57,10 @@ class AC_PosControl /// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant. - void input_pos_xyz(const Vector3p& pos, float pos_offset_z); + void input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer); + + /// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range + float pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer); /// /// Lateral position controller From 12505cf9e885974f78ecf41f5700f92970bc54e2 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 18:16:52 +0930 Subject: [PATCH 21/34] Copter: Guided: support terrain following --- ArduCopter/mode_guided.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 1be591832b81e..e256289bc4029 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -563,7 +563,14 @@ void ModeGuided::pos_control_run() // 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, terr_offset); + + // todo: Randy to convert to parameter TERRAIN_MARGIN (in m) + float TERRAIN_MARGIN = 10; + float pos_offset_z_buffer = 0.0; // Vertical buffer size in m + if (guided_pos_terrain_alt) { + pos_offset_z_buffer = MIN(TERRAIN_MARGIN * 100.0, 0.5 * fabsf(guided_pos_target_cm.z)); + } + pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset, pos_offset_z_buffer); // run position controllers pos_control->update_xy_controller(); From 9e6ac6708dd9bee93a9719e80eac3da89a4d5e1d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 9 Jul 2021 11:20:29 +0930 Subject: [PATCH 22/34] Copter: Guided add terrain failsafe --- ArduCopter/mode_guided.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index e256289bc4029..f52ca9987a02a 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -552,8 +552,8 @@ void ModeGuided::pos_control_run() // calculate terrain adjustments float terr_offset = 0.0f; if (guided_pos_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) { - // if we don't have terrain altitude then stop - init(true); + // failure to set destination can only be because of missing terrain data + copter.failsafe_terrain_on_event(); return; } From 5e163f7aaf31bca21e51e26a361501c0f041b15a Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 9 Jul 2021 14:55:35 +0930 Subject: [PATCH 23/34] Copter: Guided: make aircraft stop on accel time out --- ArduCopter/mode_guided.cpp | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index f52ca9987a02a..94b611985d30f 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -615,23 +615,26 @@ void ModeGuided::accel_control_run() // 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); } + pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy()); + pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false); + } else { + // 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_accel_z(guided_accel_target_cmss.z); } - // 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(); From 6d4b59b6f21e8c5b27c135aeb173031ae0443bd7 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 9 Jul 2021 14:57:15 +0930 Subject: [PATCH 24/34] Copter: Guided: stop aircraft if an invalid command is sent --- ArduCopter/GCS_Mavlink.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 806d2051dc81e..9bad2ad6cd11d 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1109,6 +1109,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && packet.coordinate_frame != MAV_FRAME_BODY_NED && packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { + // input is not valid so stop + copter.mode_guided.init(true); break; } @@ -1211,11 +1213,15 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) if (!pos_ignore) { // sanity check location if (!check_latlng(packet.lat_int, packet.lon_int)) { + // input is not valid so stop + copter.mode_guided.init(true); break; } Location::AltFrame frame; if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) { // unknown coordinate frame + // input is not valid so stop + copter.mode_guided.init(true); break; } loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame}; @@ -1252,10 +1258,14 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) // convert Location to vector from ekf origin for posvel controller if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) { // posvel controller does not support alt-above-terrain + // input is not valid so stop + copter.mode_guided.init(true); break; } Vector3f pos_neu_cm; if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) { + // input is not valid so stop + copter.mode_guided.init(true); break; } copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); From 7082b21f26c5665f3f4659b8e53a2e74fbe476d9 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 9 Jul 2021 14:57:54 +0930 Subject: [PATCH 25/34] Copter: Guided: use default yaw mode for all gps based sub modes. --- ArduCopter/mode_guided.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 94b611985d30f..a82de8e3b406d 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -153,6 +153,9 @@ void ModeGuided::pva_control_start() // 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 position controller @@ -163,9 +166,6 @@ void ModeGuided::pos_control_start() // initialise position controller pva_control_start(); - - // initialise yaw - auto_yaw.set_mode_to_default(false); } // initialise guided mode's velocity controller @@ -176,9 +176,6 @@ void ModeGuided::accel_control_start() // initialise position controller pva_control_start(); - - // pilot always controls yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); } // initialise guided mode's velocity and acceleration controller @@ -189,9 +186,6 @@ void ModeGuided::velaccel_control_start() // initialise position controller pva_control_start(); - - // pilot always controls yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); } // initialise guided mode's position, velocity and acceleration controller @@ -202,9 +196,6 @@ void ModeGuided::posvelaccel_control_start() // initialise position controller pva_control_start(); - - // pilot always controls yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); } bool ModeGuided::is_taking_off() const From 6034d6a1d51225b0d94848aa1831b245b193e6fc Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 9 Jul 2021 14:58:58 +0930 Subject: [PATCH 26/34] AC_AttitudeControl: AC_PosControl: support accel only input in the vertical --- .../AC_AttitudeControl/AC_PosControl.cpp | 22 +++++++++++++++++++ libraries/AC_AttitudeControl/AC_PosControl.h | 5 +++++ 2 files changed, 27 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 80effd1b01faa..dc95c979af842 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -805,6 +805,28 @@ void AC_PosControl::init_z() /// The vel is projected forwards in time based on a time step of dt and acceleration accel. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The function alters the vel to be the kinematic path based on accel +void AC_PosControl::input_accel_z(const float accel) +{ + // calculated increased maximum acceleration if over speed + float accel_z_cmss = _accel_max_z_cmss; + if (_vel_desired.z < _vel_max_down_cms && !is_zero(_vel_max_down_cms)) { + accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_down_cms; + } + if (_vel_desired.z > _vel_max_up_cms && !is_zero(_vel_max_up_cms)) { + accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_up_cms; + } + + // adjust desired alt if motors have not hit their limits + update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z); + + shape_accel(accel, _accel_desired.z, + -constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss, + _tc_z_s, _dt); +} + +/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. +/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. +/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_z and time constant. void AC_PosControl::input_vel_accel_z(float &vel, const float accel, bool ignore_descent_limit) { if (ignore_descent_limit) { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index baf4409fc2684..f579882d3578e 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -181,6 +181,11 @@ class AC_PosControl /// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration. void relax_z_controller(float throttle_setting); + /// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. + /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. + /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_z and time constant. + virtual void input_accel_z(const float accel); + /// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_z and time constant. From 31780fdfe034b1ccb122a8df55e6615dd428979b Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 9 Jul 2021 18:13:33 +0930 Subject: [PATCH 27/34] AC_AttitudeControl: Add accessor for yaw slew limit --- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 49f7002d02faa..4cc87fd7fc59b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -111,6 +111,9 @@ class AC_AttitudeControl { // get the pitch angular velocity limit in radians/s float get_ang_vel_pitch_max_radss() const { return _ang_vel_pitch_max; } + // get the yaw slew limit + float get_slew_yaw_cds() const { return _slew_yaw; } + // get the rate control input smoothing time constant float get_input_tc() const { return _input_tc; } @@ -120,7 +123,7 @@ class AC_AttitudeControl { // Ensure attitude controller have zero errors to relax rate controller output void relax_attitude_controllers(); - // Used by child class AC_AttitudeControl_TS to change behavior for tailsitter quadplanes + // Used by child class AC_AttitudeControl_TS to change behaviour for tailsitter quadplanes virtual void relax_attitude_controllers(bool exclude_pitch) { relax_attitude_controllers(); } // reset rate controller I terms From c9fa1c64c53b46df1dda10fab945691a2c1295cc Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 9 Jul 2021 18:14:07 +0930 Subject: [PATCH 28/34] Copter: additional yaw modes and fixes --- ArduCopter/GCS_Mavlink.cpp | 12 ++---- ArduCopter/autoyaw.cpp | 75 +++++++++++++++++++++++++++----------- ArduCopter/defines.h | 5 ++- ArduCopter/mode.h | 11 ++++-- ArduCopter/mode_auto.cpp | 2 +- ArduCopter/mode_guided.cpp | 14 ++++--- 6 files changed, 78 insertions(+), 41 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 9bad2ad6cd11d..68403001459a5 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -704,11 +704,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && !copter.camera_mount.has_pan_control()) { - copter.flightmode->auto_yaw.set_fixed_yaw( + copter.flightmode->auto_yaw.set_yaw_angle_rate( (float)packet.param3 * 0.01f, - 0.0f, - 0, - false); + 0.0f); } break; #endif @@ -974,11 +972,9 @@ void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg) // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && !copter.camera_mount.has_pan_control()) { - copter.flightmode->auto_yaw.set_fixed_yaw( + copter.flightmode->auto_yaw.set_yaw_angle_rate( mavlink_msg_mount_control_get_input_c(&msg) * 0.01f, - 0.0f, - 0, - false); + 0.0f); break; } diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 78bec2dc50b3b..cfb358fe1f8db 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -85,7 +85,7 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode) case AUTO_YAW_RATE: // initialise target yaw rate to zero - _rate_cds = 0.0f; + _yaw_rate_cds = 0.0f; break; case AUTO_YAW_CIRCLE: @@ -95,35 +95,52 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode) } // set_fixed_yaw - sets the yaw look at heading for auto mode -void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle) +void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t direction, bool relative_angle) { - const int32_t curr_yaw_target = copter.attitude_control->get_att_target_euler_cd().z; + fixed_last_update = millis(); + + _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z; + _yaw_rate_cds = 0.0; // calculate final angle as relative to vehicle heading or absolute - if (!relative_angle) { - // absolute angle - _fixed_yaw = wrap_360_cd(angle_deg * 100); + if (relative_angle) { + if (direction == 0) { + _fixed_yaw_offset_cd = angle_deg; + } else { + _fixed_yaw_offset_cd = angle_deg * direction; + } } else { - // relative angle - if (direction < 0) { - angle_deg = -angle_deg; + // absolute angle + _fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd); + if ( direction < 0 && is_positive(_fixed_yaw_offset_cd) ) { + _fixed_yaw_offset_cd -= 36000.0; + } else if ( direction > 0 && is_negative(_fixed_yaw_offset_cd) ) { + _fixed_yaw_offset_cd += 36000.0; } - _fixed_yaw = wrap_360_cd((angle_deg * 100) + curr_yaw_target); } // get turn speed - if (is_zero(turn_rate_dps)) { - // default to regular auto slew rate - _fixed_yaw_slewrate = AUTO_YAW_SLEW_RATE; + if (!is_positive(turn_rate_ds)) { + // default to default slew rate + _fixed_yaw_slewrate_cds = copter.attitude_control->get_slew_yaw_cds(); } else { - const int32_t turn_rate = (wrap_180_cd(_fixed_yaw - curr_yaw_target) / 100) / turn_rate_dps; - _fixed_yaw_slewrate = constrain_int32(turn_rate, 1, 360); // deg / sec + _fixed_yaw_slewrate_cds = MIN(copter.attitude_control->get_slew_yaw_cds(), turn_rate_ds * 100.0); } // set yaw mode set_mode(AUTO_YAW_FIXED); +} - // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise +// set_fixed_yaw - sets the yaw look at heading for auto mode +void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds) +{ + fixed_last_update = millis(); + + _yaw_angle_cd = yaw_angle_d * 100.0; + _yaw_rate_cds = yaw_rate_ds * 100.0; + + // set yaw mode + set_mode(AUTO_YAW_ANGLE_RATE); } // set_roi - sets the yaw to look at roi for auto mode @@ -169,7 +186,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location) void Mode::AutoYaw::set_rate(float turn_rate_cds) { set_mode(AUTO_YAW_RATE); - _rate_cds = turn_rate_cds; + _yaw_rate_cds = turn_rate_cds; } // yaw - returns target heading depending upon auto_yaw.mode() @@ -181,10 +198,16 @@ float Mode::AutoYaw::yaw() // point towards a location held in roi return roi_yaw(); - case AUTO_YAW_FIXED: + case AUTO_YAW_FIXED: { // keep heading pointing in the direction held in fixed_yaw // with no pilot input allowed - return _fixed_yaw; + float dt = millis() - fixed_last_update; + fixed_last_update = millis(); + float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds); + _fixed_yaw_offset_cd -= yaw_angle_step; + _yaw_angle_cd += yaw_angle_step; + return _yaw_angle_cd; + } case AUTO_YAW_LOOK_AHEAD: // Commanded Yaw to automatically look ahead. @@ -203,11 +226,18 @@ float Mode::AutoYaw::yaw() // return the current attitude target return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z); + case AUTO_YAW_ANGLE_RATE:{ + float dt = millis() - fixed_last_update; + fixed_last_update = millis(); + _yaw_angle_cd += _yaw_rate_cds * dt; + return _yaw_angle_cd; + } + case AUTO_YAW_LOOK_AT_NEXT_WP: default: // point towards next waypoint. // we don't use wp_bearing because we don't want the copter to turn too much during flight - return copter.wp_nav->get_yaw(); + return copter.pos_control->get_yaw_cd(); } } @@ -225,11 +255,12 @@ float Mode::AutoYaw::rate_cds() const case AUTO_YAW_CIRCLE: return 0.0f; + case AUTO_YAW_ANGLE_RATE: case AUTO_YAW_RATE: - return _rate_cds; + return _yaw_rate_cds; case AUTO_YAW_LOOK_AT_NEXT_WP: - return copter.wp_nav->get_yaw_rate_cds(); + return copter.pos_control->get_yaw_rate_cds(); } // return zero turn rate (this should never happen) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index d7f6d43b13e8a..a45df46cdc401 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -18,8 +18,9 @@ enum autopilot_yaw_mode { AUTO_YAW_FIXED = 3, // point towards a particular angle (no pilot input accepted) AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the copter is moving AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed - AUTO_YAW_RATE = 6, // turn at a specified rate (held in auto_yaw_rate) - AUTO_YAW_CIRCLE = 7, // use AC_Circle's provided yaw (used during Loiter-Turns commands) + AUTO_YAW_ANGLE_RATE = 6, // turn at a specified rate from a starting angle + AUTO_YAW_RATE = 7, // turn at a specified rate (held in auto_yaw_rate) + AUTO_YAW_CIRCLE = 8, // use AC_Circle's provided yaw (used during Loiter-Turns commands) }; // Frame types diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index fbdaef97056ec..b9f88e5b7cfa8 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -207,6 +207,8 @@ class Mode { int8_t direction, bool relative_angle); + void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds); + private: float look_ahead_yaw(); @@ -219,16 +221,19 @@ class Mode { Vector3f roi; // yaw used for YAW_FIXED yaw_mode - int32_t _fixed_yaw; + float _fixed_yaw_offset_cd; // Deg/s we should turn - int16_t _fixed_yaw_slewrate; + float _fixed_yaw_slewrate_cds; + + uint32_t fixed_last_update; // heading when in yaw_look_ahead_yaw float _look_ahead_yaw; // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE - float _rate_cds; + float _yaw_angle_cd; + float _yaw_rate_cds; }; static AutoYaw auto_yaw; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 3185157a57f53..520fb5ce034da 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1501,7 +1501,7 @@ void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && !copter.camera_mount.has_pan_control()) { - auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,false); + auto_yaw.set_yaw_angle_rate(cmd.content.mount_control.yaw,0.0f); } // pass the target angles to the camera mount copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index a82de8e3b406d..dd6fa1a6dfcce 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -576,7 +576,7 @@ void ModeGuided::pos_control_run() 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()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } @@ -639,7 +639,7 @@ void ModeGuided::accel_control_run() 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()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } @@ -713,7 +713,7 @@ void ModeGuided::velaccel_control_run() 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()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } @@ -794,7 +794,7 @@ void ModeGuided::posvelaccel_control_run() 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()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } @@ -887,8 +887,12 @@ void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f // helper function to set yaw state and targets void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) { - if (use_yaw) { + if (use_yaw && relative_angle) { auto_yaw.set_fixed_yaw(yaw_cd * 0.01f, 0.0f, 0, relative_angle); + } else if (use_yaw && use_yaw_rate) { + auto_yaw.set_yaw_angle_rate(yaw_cd * 0.01f, yaw_rate_cds * 0.01f); + } else if (use_yaw && !use_yaw_rate) { + auto_yaw.set_yaw_angle_rate(yaw_cd * 0.01f, 0.0f); } else if (use_yaw_rate) { auto_yaw.set_rate(yaw_rate_cds); } From 008b4fe6a380b6c6dea40b11f729445281b60c6f Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 9 Jul 2021 19:14:14 +0930 Subject: [PATCH 29/34] AC_WPNav: Remove unused function --- libraries/AC_WPNav/AC_WPNav.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 1dc653f3cdd2c..9c2b9c5a22764 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -201,8 +201,6 @@ class AC_WPNav // get target yaw in centi-degrees float get_yaw() const { return _pos_control.get_yaw_cd(); } - float get_yaw_rate_cds() const { return _pos_control.get_yaw_rate_cds(); } - /// advance_wp_target_along_track - move target location along track from origin to destination bool advance_wp_target_along_track(float dt); From da71d49f45c02dbe58a804903c7a39dd0c6abe8e Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 10 Jul 2021 10:55:19 +0930 Subject: [PATCH 30/34] Copter: Guided Angle init Z controller on time out --- ArduCopter/mode_guided.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index dd6fa1a6dfcce..122ebb629aa76 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -833,7 +833,11 @@ void ModeGuided::angle_control_run() pitch_in = 0.0f; climb_rate_cms = 0.0f; yaw_rate_in = 0.0f; - guided_angle_state.use_thrust = false; + if (guided_angle_state.use_thrust) { + // initialise vertical velocity controller + pos_control->init_z_controller(); + guided_angle_state.use_thrust = false; + } } // interpret positive climb rate or thrust as triggering take-off From 3b203c92416a6082c6aa651b50df44fd2a9f5d7f Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 10 Jul 2021 10:55:52 +0930 Subject: [PATCH 31/34] Copter: Auto Yaw variable names and comments --- ArduCopter/autoyaw.cpp | 20 +++++++++----------- ArduCopter/mode.h | 3 ++- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index cfb358fe1f8db..ec2d1d3c080ab 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -97,18 +97,14 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode) // set_fixed_yaw - sets the yaw look at heading for auto mode void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t direction, bool relative_angle) { - fixed_last_update = millis(); + _last_update_ms = millis(); _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z; _yaw_rate_cds = 0.0; // calculate final angle as relative to vehicle heading or absolute if (relative_angle) { - if (direction == 0) { - _fixed_yaw_offset_cd = angle_deg; - } else { - _fixed_yaw_offset_cd = angle_deg * direction; - } + _fixed_yaw_offset_cd = angle_deg * 100.0 * (direction >= 0 ? 1.0 : -1.0); } else { // absolute angle _fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd); @@ -134,7 +130,7 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di // set_fixed_yaw - sets the yaw look at heading for auto mode void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds) { - fixed_last_update = millis(); + _last_update_ms = millis(); _yaw_angle_cd = yaw_angle_d * 100.0; _yaw_rate_cds = yaw_rate_ds * 100.0; @@ -201,8 +197,9 @@ float Mode::AutoYaw::yaw() case AUTO_YAW_FIXED: { // keep heading pointing in the direction held in fixed_yaw // with no pilot input allowed - float dt = millis() - fixed_last_update; - fixed_last_update = millis(); + const uint32_t now_ms = millis(); + float dt = now_ms - _last_update_ms; + _last_update_ms = now_ms; float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds); _fixed_yaw_offset_cd -= yaw_angle_step; _yaw_angle_cd += yaw_angle_step; @@ -227,8 +224,9 @@ float Mode::AutoYaw::yaw() return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z); case AUTO_YAW_ANGLE_RATE:{ - float dt = millis() - fixed_last_update; - fixed_last_update = millis(); + const uint32_t now_ms = millis(); + float dt = now_ms - _last_update_ms; + _last_update_ms = now_ms; _yaw_angle_cd += _yaw_rate_cds * dt; return _yaw_angle_cd; } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index b9f88e5b7cfa8..04bb224b47ead 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -226,7 +226,8 @@ class Mode { // Deg/s we should turn float _fixed_yaw_slewrate_cds; - uint32_t fixed_last_update; + // time of the last yaw update + uint32_t _last_update_ms; // heading when in yaw_look_ahead_yaw float _look_ahead_yaw; From a52fece2d67d9c907dff4859034c5a76f802e86b Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 10 Jul 2021 13:36:59 +0930 Subject: [PATCH 32/34] Copter: Guided: move to zero velocity after takeoff --- ArduCopter/mode_guided.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 122ebb629aa76..3f33175f58762 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -457,13 +457,13 @@ bool ModeGuided::set_attitude_target_provides_thrust() const } // 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 +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 +bool ModeGuided::stabilizing_vel_xy() const { return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizeVelocityXY)) != 0); } @@ -513,9 +513,8 @@ void ModeGuided::takeoff_run() copter.landinggear.retract_after_takeoff(); #endif - // switch to position control mode but maintain current target - 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()); + // change to velocity control after take off. + init(true); } } From ea50cc01960fcfa9ee07c22e831c38b02b1fd91e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 9 Jul 2021 14:02:07 +0900 Subject: [PATCH 33/34] Copter: add GUID_TIMEOUT for guided mode vel, accel and angle control --- ArduCopter/Parameters.cpp | 10 ++++++++++ ArduCopter/Parameters.h | 4 ++++ ArduCopter/mode.h | 3 +++ ArduCopter/mode_guided.cpp | 17 ++++++++++------- 4 files changed, 27 insertions(+), 7 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 418370b34bfa0..c5e580e30125b 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1069,6 +1069,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_GROUPINFO("RNGFND_FILT", 45, ParametersG2, rangefinder_filt, RANGEFINDER_FILT_DEFAULT), #endif +#if MODE_GUIDED_ENABLED == ENABLED + // @Param: GUID_TIMEOUT + // @DisplayName: Guided mode timeout + // @Description: Guided mode timeout after which vehicle will stop or return to level if no updates are received from caller. Only applicable during velocity, acceleration or angle control + // @Units: s + // @Range: 0.1 5 + // @User: Advanced + AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0), +#endif + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e10366787224c..2432455a268b8 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -649,6 +649,10 @@ class ParametersG2 { AP_Float rangefinder_filt; #endif +#if MODE_GUIDED_ENABLED == ENABLED + AP_Float guided_timeout; +#endif + }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 04bb224b47ead..16a94eec43d5f 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -890,6 +890,9 @@ class ModeGuided : public Mode { void angle_control_start(); void angle_control_run(); + // return guided mode timeout in milliseconds. Only used for velocity, acceleration and angle control + uint32_t get_timeout_ms() const; + protected: const char *name() const override { return "GUIDED"; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 3f33175f58762..7f70ad9f12e48 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -10,9 +10,6 @@ # define GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM 500 // point nose at target if it is more than 5m away #endif -#define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates -#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) bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain static Vector3f guided_vel_target_cms; // velocity target (used by pos_vel_accel controller and vel_accel controller) @@ -604,7 +601,7 @@ void ModeGuided::accel_control_run() // 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) { + if (tnow - update_time_ms > get_timeout_ms()) { guided_vel_target_cms.zero(); guided_accel_target_cmss.zero(); if (auto_yaw.mode() == AUTO_YAW_RATE) { @@ -667,7 +664,7 @@ void ModeGuided::velaccel_control_run() // 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) { + if (tnow - update_time_ms > get_timeout_ms()) { guided_vel_target_cms.zero(); guided_accel_target_cmss.zero(); if (auto_yaw.mode() == AUTO_YAW_RATE) { @@ -742,7 +739,7 @@ void ModeGuided::posvelaccel_control_run() // 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) { + if (tnow - update_time_ms > get_timeout_ms()) { guided_vel_target_cms.zero(); guided_accel_target_cmss.zero(); if (auto_yaw.mode() == AUTO_YAW_RATE) { @@ -827,7 +824,7 @@ void ModeGuided::angle_control_run() // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds uint32_t tnow = millis(); - if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) { + if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) { roll_in = 0.0f; pitch_in = 0.0f; climb_rate_cms = 0.0f; @@ -1037,4 +1034,10 @@ float ModeGuided::crosstrack_error() const return 0; } +// return guided mode timeout in milliseconds. Only used for velocity, acceleration and angle control +uint32_t ModeGuided::get_timeout_ms() const +{ + return MAX(copter.g2.guided_timeout, 0.1) * 1000; +} + #endif From 974d1369360fc4123a59f0ddc14a00c9e98ccf9c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 9 Jul 2021 14:18:28 +0900 Subject: [PATCH 34/34] Copter: add TERRAIN_MARGIN parameter --- ArduCopter/Copter.h | 1 + ArduCopter/Parameters.cpp | 10 ++++++++++ ArduCopter/Parameters.h | 3 +++ ArduCopter/mode_guided.cpp | 4 +--- ArduCopter/terrain.cpp | 10 ++++++++++ 5 files changed, 25 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index adcf2a3b371a5..1b6b30e093c5d 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -884,6 +884,7 @@ class Copter : public AP_Vehicle { // terrain.cpp void terrain_update(); void terrain_logging(); + float get_terrain_margin() const; // tuning.cpp void tuning(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index c5e580e30125b..6f4133b65f806 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1079,6 +1079,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0), #endif +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + // @Param: TERRAIN_MARGIN + // @DisplayName: Terrain following altitude margin + // @Description: Terrain following altitude margin. Vehicle will stop if distance from target altitude is larger than this margin (in meters) + // @Units: m + // @Range: 0.1 100 + // @User: Advanced + AP_GROUPINFO("TERRAIN_MARGIN", 47, ParametersG2, terrain_margin, 10.0), +#endif + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 2432455a268b8..9e91f52240720 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -653,6 +653,9 @@ class ParametersG2 { AP_Float guided_timeout; #endif +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + AP_Float terrain_margin; +#endif }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 7f70ad9f12e48..848251babdaaa 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -551,11 +551,9 @@ void ModeGuided::pos_control_run() guided_accel_target_cmss.zero(); guided_vel_target_cms.zero(); - // todo: Randy to convert to parameter TERRAIN_MARGIN (in m) - float TERRAIN_MARGIN = 10; float pos_offset_z_buffer = 0.0; // Vertical buffer size in m if (guided_pos_terrain_alt) { - pos_offset_z_buffer = MIN(TERRAIN_MARGIN * 100.0, 0.5 * fabsf(guided_pos_target_cm.z)); + pos_offset_z_buffer = MIN(copter.get_terrain_margin() * 100.0, 0.5 * fabsf(guided_pos_target_cm.z)); } pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset, pos_offset_z_buffer); diff --git a/ArduCopter/terrain.cpp b/ArduCopter/terrain.cpp index 09649eb26fb93..43ea931f60343 100644 --- a/ArduCopter/terrain.cpp +++ b/ArduCopter/terrain.cpp @@ -26,3 +26,13 @@ void Copter::terrain_logging() } #endif } + +// return terrain following altitude margin. Vehicle will stop if distance from target altitude is larger than this margin (in meters) +float Copter::get_terrain_margin() const +{ +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + return MAX(g2.terrain_margin.get(), 0.1); +#else + return 10; +#endif +}