diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0ed08e0663..c2dac77664 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -791,7 +791,7 @@ private: #if FRAME_CONFIG == HELI_FRAME void Log_Write_Heli(void); #endif - void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, const Vector3f& vel_target); + void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, const Vector3f& vel_target, const Vector3f& accel_target); void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); void Log_Write_Vehicle_Startup_Messages(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 953a2d1ca8..4be486ff2e 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::Velocity: + case ModeGuided::SubMode::PosVelAccel: + type_mask = POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position, velocity & acceleration + target_pos = copter.mode_guided.get_target_pos().tofloat() * 0.01; // convert to metres + target_vel = copter.mode_guided.get_target_vel() * 0.01f; // convert to metres/s + target_accel = copter.mode_guided.get_target_accel() * 0.01f; // convert to metres/s/s + break; + case ModeGuided::SubMode::VelAccel: type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE | - POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | - POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity - target_vel = copter.flightmode->get_vel_desired_cms() * 0.01f; // convert to m/s + POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity & acceleration + target_vel = copter.mode_guided.get_target_vel() * 0.01f; // convert to metres/s + target_accel = copter.mode_guided.get_target_accel() * 0.01f; // convert to metres/s/s break; - case ModeGuided::SubMode::TakeOff: - case ModeGuided::SubMode::PosVel: - type_mask = POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | - POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position & velocity - target_pos = copter.wp_nav->get_wp_destination() * 0.01f; - target_vel = copter.flightmode->get_vel_desired_cms() * 0.01f; + case ModeGuided::SubMode::Accel: + type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity & acceleration + target_accel = copter.mode_guided.get_target_accel() * 0.01f; // convert to metres/s/s break; } @@ -151,15 +158,15 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned() AP_HAL::millis(), // time boot ms MAV_FRAME_LOCAL_NED, type_mask, - target_pos.x, // x in metres - target_pos.y, // y in metres - -target_pos.z, // z in metres NED frame - target_vel.x, // vx in m/s - target_vel.y, // vy in m/s - -target_vel.z, // vz in m/s NED frame - 0.0f, // afx - 0.0f, // afy - 0.0f, // afz + target_pos.x, // x in metres + target_pos.y, // y in metres + -target_pos.z, // z in metres NED frame + target_vel.x, // vx in m/s + target_vel.y, // vy in m/s + -target_vel.z, // vz in m/s NED frame + target_accel.x, // afx in m/s/s + target_accel.y, // afy in m/s/s + -target_accel.z,// afz in m/s/s NED frame 0.0f, // yaw 0.0f); // yaw_rate #endif @@ -1111,8 +1118,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; - // exit immediately if acceleration provided - if (!acc_ignore) { + // exit immediately if neither position nor velocity is provided + if (pos_ignore && vel_ignore) { break; } @@ -1145,6 +1152,17 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) } } + // prepare acceleration + Vector3f accel_vector; + if (!acc_ignore) { + // convert to cm + accel_vector = Vector3f(packet.afx * 100.0f, packet.afy * 100.0f, -packet.afz * 100.0f); + // rotate to body-frame if necessary + if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { + copter.rotate_body_frame_to_NE(accel_vector.x, accel_vector.y); + } + } + // prepare yaw float yaw_cd = 0.0f; bool yaw_relative = false; @@ -1159,11 +1177,13 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) // send request if (!pos_ignore && !vel_ignore) { - copter.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.mode_guided.set_destination_posvelaccel(pos_vector, vel_vector, accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + } else if (pos_ignore && vel_ignore && !acc_ignore) { + copter.mode_guided.set_accel(accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } else if (pos_ignore && !vel_ignore) { - copter.mode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); - } else if (!pos_ignore && vel_ignore) { - copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.mode_guided.set_velaccel(vel_vector, accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + } else if (!pos_ignore && vel_ignore && acc_ignore) { + copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, false); } break; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 344e36d556..8e4354b241 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 b9f19baf94..418370b34b 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 8665424119..75eb4531dd 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -845,11 +845,21 @@ public: bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool terrain_alt = false); bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); bool get_wp(Location &loc) override; + void set_accel(const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true); void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true); + void set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true); bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); + bool set_destination_posvelaccel(const Vector3f& destination, const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); + + // get position, velocity and acceleration targets + const Vector3p& get_target_pos() const; + const Vector3f& get_target_vel() const; + const Vector3f& get_target_accel() const; // returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate bool set_attitude_target_provides_thrust() const; + bool stabilizing_pos_xy() const; + bool stabilizing_vel_xy() const; void limit_clear(); void limit_init_time_and_pos(); @@ -863,8 +873,9 @@ public: enum class SubMode { TakeOff, WP, - Velocity, - PosVel, + PosVelAccel, + VelAccel, + Accel, Angle, }; @@ -886,19 +897,23 @@ private: // enum for GUID_OPTIONS parameter enum class Options : int32_t { - AllowArmingFromTX = (1U << 0), + AllowArmingFromTX = (1U << 0), // this bit is still available, pilot yaw was mapped to bit 2 for symmetry with auto - IgnorePilotYaw = (1U << 2), + IgnorePilotYaw = (1U << 2), SetAttitudeTarget_ThrustAsThrust = (1U << 3), + DoNotStabilizePositionXY = (1U << 4), + DoNotStabilizeVelocityXY = (1U << 5), }; void pos_control_start(); - void vel_control_start(); - void posvel_control_start(); + void accel_control_start(); + void velaccel_control_start(); + void posvelaccel_control_start(); void takeoff_run(); void pos_control_run(); - void vel_control_run(); - void posvel_control_run(); + void accel_control_run(); + void velaccel_control_run(); + void posvelaccel_control_run(); void set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des); void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle); bool use_pilot_yaw(void) const; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index f9e612a628..879f00f2d8 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,25 @@ void ModeGuided::pos_control_start() // set to position control mode guided_mode = SubMode::WP; - // initialise waypoint and spline controller - wp_nav->wp_and_spline_init(); + // initialise horizontal speed, acceleration + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); - // initialise wpnav to stopping point - Vector3f stopping_point; - wp_nav->get_wp_stopping_point(stopping_point); + // initialize vertical speeds and acceleration + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); - // no need to check return status because terrain data is not used - wp_nav->set_wp_destination(stopping_point, false); + // initialise velocity controller + pos_control->init_z_controller(); + pos_control->init_xy_controller(); // initialise yaw auto_yaw.set_mode_to_default(false); } // initialise guided mode's velocity controller -void ModeGuided::vel_control_start() +void ModeGuided::accel_control_start() { // set guided_mode to velocity controller - guided_mode = SubMode::Velocity; + guided_mode = SubMode::Accel; // initialise horizontal speed, acceleration pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); @@ -170,13 +173,36 @@ void ModeGuided::vel_control_start() // initialise the position controller pos_control->init_z_controller(); pos_control->init_xy_controller(); + + // pilot always controls yaw + auto_yaw.set_mode(AUTO_YAW_HOLD); +} + +// initialise guided mode's velocity controller +void ModeGuided::velaccel_control_start() +{ + // set guided_mode to velocity controller + guided_mode = SubMode::VelAccel; + + // initialise horizontal speed, acceleration + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + + // initialise the position controller + pos_control->init_z_controller(); + pos_control->init_xy_controller(); + + // pilot always controls yaw + auto_yaw.set_mode(AUTO_YAW_HOLD); } // initialise guided mode's posvel controller -void ModeGuided::posvel_control_start() +void ModeGuided::posvelaccel_control_start() { // set guided_mode to velocity controller - guided_mode = SubMode::PosVel; + guided_mode = SubMode::PosVelAccel; // initialise horizontal speed, acceleration pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); @@ -224,7 +250,7 @@ void ModeGuided::angle_control_start() auto_yaw.set_mode(AUTO_YAW_HOLD); } -// guided_set_destination - sets guided mode's target destination +// set_destination - sets guided mode's target destination // Returns true if the fence is enabled and guided waypoint is within the fence // else return false if the waypoint is outside the fence bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool terrain_alt) @@ -247,11 +273,14 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa // set yaw state set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); - // no need to check return status because terrain data is not used - wp_nav->set_wp_destination(destination, terrain_alt); + // set position target and zero velocity and acceleration + guided_pos_target_cm = destination.topostype(); + guided_vel_target_cms.zero(); + guided_accel_target_cmss.zero(); + update_time_ms = millis(); // log target - copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss); send_notification = true; @@ -286,47 +315,89 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y pos_control_start(); } - if (!wp_nav->set_wp_destination_loc(dest_loc)) { - // failure to set destination can only be because of missing terrain data - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); - // failure is propagated to GCS with NAK - return false; - } - // set yaw state set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + // set position target and zero velocity and acceleration + Vector3f pos_target_f; + if (!dest_loc.get_vector_from_origin_NEU(pos_target_f)) { + guided_pos_target_cm = pos_control->get_pos_target_cm(); + } else { + guided_pos_target_cm = pos_target_f.topostype(); + } + + guided_vel_target_cms.zero(); + guided_accel_target_cmss.zero(); + // log target - copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); + copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_vel_target_cms, guided_accel_target_cmss); send_notification = true; return true; } -// guided_set_velocity - sets guided mode's target velocity -void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) +// set_velaccel - sets guided mode's target velocity and acceleration +void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) { // check we are in velocity control mode - if (guided_mode != SubMode::Velocity) { - vel_control_start(); + if (guided_mode != SubMode::Accel) { + accel_control_start(); } // set yaw state set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); - // record velocity target - guided_vel_target_cms = velocity; - vel_update_time_ms = millis(); + // set velocity and acceleration targets and zero position + guided_pos_target_cm.zero(); + guided_vel_target_cms.zero(); + guided_accel_target_cmss = acceleration; + update_time_ms = millis(); // log target if (log_request) { - copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity); + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss); } } -// set guided mode posvel target +// set_velocity - sets guided mode's target velocity +void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) +{ + set_velaccel(velocity, Vector3f(), use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw, log_request); +} + +// set_velaccel - sets guided mode's target velocity and acceleration +void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) +{ + // check we are in velocity control mode + if (guided_mode != SubMode::VelAccel) { + velaccel_control_start(); + } + + // set yaw state + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + + // set velocity and acceleration targets and zero position + guided_pos_target_cm.zero(); + guided_vel_target_cms = velocity; + guided_accel_target_cmss = acceleration; + update_time_ms = millis(); + + // log target + if (log_request) { + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss); + } +} + +// set_destination_posvel - set guided mode position and velocity target bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +{ + set_destination_posvelaccel(destination, velocity, Vector3f(), use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + return true; +} + +// set_destination_posvelaccel - set guided mode position, velocity and acceleration target +bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { #if AC_FENCE == ENABLED // reject destination if outside the fence @@ -339,21 +410,20 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto #endif // check we are in velocity control mode - if (guided_mode != SubMode::PosVel) { - posvel_control_start(); + if (guided_mode != SubMode::PosVelAccel) { + posvelaccel_control_start(); } // set yaw state set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); - posvel_update_time_ms = millis(); + update_time_ms = millis(); guided_pos_target_cm = destination.topostype(); guided_vel_target_cms = velocity; - - copter.pos_control->set_pos_vel_accel(guided_pos_target_cm, guided_vel_target_cms, Vector3f()); + guided_accel_target_cmss = acceleration; // log target - copter.Log_Write_GuidedTarget(guided_mode, destination, velocity); + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_vel_target_cms, guided_accel_target_cmss); return true; } @@ -363,6 +433,18 @@ bool ModeGuided::set_attitude_target_provides_thrust() const return ((copter.g2.guided_options.get() & uint32_t(Options::SetAttitudeTarget_ThrustAsThrust)) != 0); } +// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate +bool ModeGuided:: stabilizing_pos_xy() const +{ + return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizePositionXY)) != 0); +} + +// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate +bool ModeGuided:: stabilizing_vel_xy() const +{ + return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizeVelocityXY)) != 0); +} + // set guided mode angle target and climbrate void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust) { @@ -393,10 +475,10 @@ void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, // log target copter.Log_Write_GuidedTarget(guided_mode, Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd), - Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust)); + Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust), Vector3f()); } -// guided_takeoff_run - takeoff in guided mode +// takeoff_run - takeoff in guided mode // called by guided_run at 100hz or more void ModeGuided::takeoff_run() { @@ -408,17 +490,18 @@ void ModeGuided::takeoff_run() #endif // switch to position control mode but maintain current target - const Vector3f target = wp_nav->get_wp_destination(); + const Vector3f target = pos_control->get_pos_target_cm().tofloat(); set_destination(target, false, 0, false, 0, false, wp_nav->origin_and_destination_are_terrain_alt()); } } -// guided_pos_control_run - runs the guided position controller +// pos_control_run - runs the guided position controller // called from guided_run void ModeGuided::pos_control_run() { // process pilot's yaw input float target_yaw_rate = 0; + if (!copter.failsafe.radio && use_pilot_yaw()) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); @@ -436,29 +519,31 @@ void ModeGuided::pos_control_run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // run waypoint controller - copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); + // send position and velocity targets to position controller + guided_accel_target_cmss.zero(); + guided_vel_target_cms.zero(); + pos_control->input_pos_xyz(guided_pos_target_cm); - // WP_Nav has set the vertical position control targets - // run the vertical position controller and set output throttle + // run position controllers + pos_control->update_xy_controller(); pos_control->update_z_controller(); // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate); + // roll & pitch from position controller, yaw rate from pilot + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); } else if (auto_yaw.mode() == AUTO_YAW_RATE) { - // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item - attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), auto_yaw.rate_cds()); + // roll & pitch from position controller, yaw rate from mavlink command or mission item + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { - // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() - attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw()); + // roll & pitch from position controller, yaw heading from GCS or auto_heading() + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw()); } } -// guided_vel_control_run - runs the guided velocity controller +// velaccel_control_run - runs the guided velocity controller // called from guided_run -void ModeGuided::vel_control_run() +void ModeGuided::accel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; @@ -492,37 +577,129 @@ void ModeGuided::vel_control_run() // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); - if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { - if (!pos_control->get_vel_desired_cms().is_zero()) { - set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f)); - } + if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { + guided_accel_target_cmss.zero(); if (auto_yaw.mode() == AUTO_YAW_RATE) { auto_yaw.set_rate(0.0f); } - } else { - set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms); } + // update position controller with new target + pos_control->input_accel_xy(guided_accel_target_cmss); + if (!stabilizing_vel_xy()) { + // set position and velocity errors to zero + pos_control->stop_vel_xy_stabilisation(); + } else if (!stabilizing_pos_xy()) { + // set position errors to zero + pos_control->stop_pos_xy_stabilisation(); + } + pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false); + // call velocity controller which includes z axis controller pos_control->update_xy_controller(); pos_control->update_z_controller(); // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { - // roll & pitch from waypoint controller, yaw rate from pilot + // roll & pitch from position controller, yaw rate from pilot attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); } else if (auto_yaw.mode() == AUTO_YAW_RATE) { - // roll & pitch from velocity controller, yaw rate from mavlink command or mission item + // roll & pitch from position controller, yaw rate from mavlink command or mission item attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { - // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() + // roll & pitch from position controller, yaw heading from GCS or auto_heading() attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw()); } } -// guided_posvel_control_run - runs the guided spline controller +// velaccel_control_run - runs the guided velocity and acceleration controller // called from guided_run -void ModeGuided::posvel_control_run() +void ModeGuided::velaccel_control_run() +{ + // process pilot's yaw input + float target_yaw_rate = 0; + if (!copter.failsafe.radio && use_pilot_yaw()) { + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!is_zero(target_yaw_rate)) { + auto_yaw.set_mode(AUTO_YAW_HOLD); + } + } + + // landed with positive desired climb rate, initiate takeoff + if (motors->armed() && copter.ap.auto_armed && copter.ap.land_complete && is_positive(guided_vel_target_cms.z)) { + zero_throttle_and_relax_ac(); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { + set_land_complete(false); + set_throttle_takeoff(); + } + return; + } + + // if not armed set throttle to zero and exit immediately + if (is_disarmed_or_landed()) { + make_safe_spool_down(); + return; + } + + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // set velocity to zero and stop rotating if no updates received for 3 seconds + uint32_t tnow = millis(); + if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { + guided_vel_target_cms.zero(); + guided_accel_target_cmss.zero(); + if (auto_yaw.mode() == AUTO_YAW_RATE) { + auto_yaw.set_rate(0.0f); + } + } + + bool do_avoid = false; +#if AC_AVOID_ENABLED + // limit the velocity for obstacle/fence avoidance + copter.avoid.adjust_velocity(guided_vel_target_cms, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt); + do_avoid = copter.avoid.limits_active(); +#endif + + // update position controller with new target + + if (!stabilizing_vel_xy() && !do_avoid) { + // set the current commanded xy vel to the desired vel + guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x; + guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y; + } + pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy()); + if (!stabilizing_vel_xy() && !do_avoid) { + // set position and velocity errors to zero + pos_control->stop_vel_xy_stabilisation(); + } else if (!stabilizing_pos_xy() && !do_avoid) { + // set position errors to zero + pos_control->stop_pos_xy_stabilisation(); + } + pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false); + + // call velocity controller which includes z axis controller + pos_control->update_xy_controller(); + pos_control->update_z_controller(); + + // call attitude controller + if (auto_yaw.mode() == AUTO_YAW_HOLD) { + // roll & pitch from position controller, yaw rate from pilot + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); + } else if (auto_yaw.mode() == AUTO_YAW_RATE) { + // roll & pitch from position controller, yaw rate from mavlink command or mission item + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); + } else { + // roll & pitch from position controller, yaw heading from GCS or auto_heading() + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw()); + } +} + +// posvelaccel_control_run - runs the guided position, velocity and acceleration controller +// called from guided_run +void ModeGuided::posvelaccel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; @@ -546,20 +723,37 @@ void ModeGuided::posvel_control_run() // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); - if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { + if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { guided_vel_target_cms.zero(); + guided_accel_target_cmss.zero(); if (auto_yaw.mode() == AUTO_YAW_RATE) { auto_yaw.set_rate(0.0f); } } - // advance position target using velocity target - guided_pos_target_cm += (guided_vel_target_cms * pos_control->get_dt()).topostype(); - // send position and velocity targets to position controller - pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), Vector2f()); + if (!stabilizing_vel_xy()) { + // set the current commanded xy pos to the target pos and xy vel to the desired vel + guided_pos_target_cm.x = pos_control->get_pos_target_cm().x; + guided_pos_target_cm.y = pos_control->get_pos_target_cm().y; + guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x; + guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y; + } else if (!stabilizing_pos_xy()) { + // set the current commanded xy pos to the target pos + guided_pos_target_cm.x = pos_control->get_pos_target_cm().x; + guided_pos_target_cm.y = pos_control->get_pos_target_cm().y; + } + pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), guided_accel_target_cmss.xy()); + if (!stabilizing_vel_xy()) { + // set position and velocity errors to zero + pos_control->stop_vel_xy_stabilisation(); + } else if (!stabilizing_pos_xy()) { + // set position errors to zero + pos_control->stop_pos_xy_stabilisation(); + } + float pz = guided_pos_target_cm.z; - pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, 0); + pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, guided_accel_target_cmss.z); guided_pos_target_cm.z = pz; // run position controllers @@ -568,18 +762,18 @@ void ModeGuided::posvel_control_run() // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { - // roll & pitch from waypoint controller, yaw rate from pilot + // roll & pitch from position controller, yaw rate from pilot attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); } else if (auto_yaw.mode() == AUTO_YAW_RATE) { - // roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item + // roll & pitch from position controller, yaw rate from mavlink command or mission item attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { - // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() + // roll & pitch from position controller, yaw heading from GCS or auto_heading() attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw()); } } -// guided_angle_control_run - runs the guided angle controller +// angle_control_run - runs the guided angle controller // called from guided_run void ModeGuided::angle_control_run() { @@ -663,17 +857,6 @@ void ModeGuided::angle_control_run() // helper function to update position controller's desired velocity while respecting acceleration limits void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des) { - // get current desired velocity - Vector3f curr_vel_des = vel_des; - -#if AC_AVOID_ENABLED - // limit the velocity for obstacle/fence avoidance - copter.avoid.adjust_velocity(curr_vel_des, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt); -#endif - - // update position controller with new target - pos_control->input_vel_accel_xy(curr_vel_des.xy(), Vector2f()); - pos_control->input_vel_accel_z(curr_vel_des.z, 0, false); } // helper function to set yaw state and targets @@ -694,7 +877,7 @@ bool ModeGuided::use_pilot_yaw(void) const // Guided Limit code -// guided_limit_clear - clear/turn off guided limits +// limit_clear - clear/turn off guided limits void ModeGuided::limit_clear() { guided_limit.timeout_ms = 0; @@ -703,7 +886,7 @@ void ModeGuided::limit_clear() guided_limit.horiz_max_cm = 0.0f; } -// guided_limit_set - set guided timeout and movement limits +// limit_set - set guided timeout and movement limits void ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) { guided_limit.timeout_ms = timeout_ms; @@ -712,7 +895,7 @@ void ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_ guided_limit.horiz_max_cm = horiz_max_cm; } -// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking +// limit_init_time_and_pos - initialise guided start time and position as reference for limit checking // only called from AUTO mode's auto_nav_guided_start function void ModeGuided::limit_init_time_and_pos() { @@ -723,7 +906,7 @@ void ModeGuided::limit_init_time_and_pos() guided_limit.start_pos = inertial_nav.get_position(); } -// guided_limit_check - returns true if guided mode has breached a limit +// limit_check - returns true if guided mode has breached a limit // used when guided is invoked from the NAV_GUIDED_ENABLE mission command bool ModeGuided::limit_check() { @@ -757,14 +940,28 @@ bool ModeGuided::limit_check() return false; } +const Vector3p &ModeGuided::get_target_pos() const +{ + return guided_pos_target_cm; +} + +const Vector3f& ModeGuided::get_target_vel() const +{ + return guided_vel_target_cms; +} + +const Vector3f& ModeGuided::get_target_accel() const +{ + return guided_accel_target_cmss; +} uint32_t ModeGuided::wp_distance() const { switch(guided_mode) { case SubMode::WP: - return wp_nav->get_wp_distance_to_destination(); + return norm(guided_pos_target_cm.x - inertial_nav.get_position().x, guided_pos_target_cm.y - inertial_nav.get_position().y); break; - case SubMode::PosVel: + case SubMode::PosVelAccel: return pos_control->get_pos_error_xy_cm(); break; default: @@ -776,13 +973,14 @@ int32_t ModeGuided::wp_bearing() const { switch(guided_mode) { case SubMode::WP: - return wp_nav->get_wp_bearing_to_destination(); + return get_bearing_cd(inertial_nav.get_position(), guided_pos_target_cm.tofloat()); break; - case SubMode::PosVel: + case SubMode::PosVelAccel: return pos_control->get_bearing_to_target_cd(); break; case SubMode::TakeOff: - case SubMode::Velocity: + case SubMode::Accel: + case SubMode::VelAccel: case SubMode::Angle: // these do not have bearings return 0; @@ -795,10 +993,10 @@ float ModeGuided::crosstrack_error() const { switch (guided_mode) { case SubMode::WP: - return wp_nav->crosstrack_error(); case SubMode::TakeOff: - case SubMode::Velocity: - case SubMode::PosVel: + case SubMode::Accel: + case SubMode::VelAccel: + case SubMode::PosVelAccel: case SubMode::Angle: // no track to have a crosstrack to return 0;