diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 9fde684cf3..d5ed4e39b7 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -322,7 +322,7 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo Quaternion q; q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg)); - mode_guided.set_angle(q,climb_rate_ms*100,use_yaw_rate,radians(yaw_rate_degs)); + mode_guided.set_angle(q, climb_rate_ms*100, use_yaw_rate, radians(yaw_rate_degs), false); return true; } diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 13e8d890f7..4c03b9da63 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -994,17 +994,25 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) break; } - // convert thrust to climb rate - packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f); - float climb_rate_cms = 0.0f; - if (is_equal(packet.thrust, 0.5f)) { - climb_rate_cms = 0.0f; - } else if (packet.thrust > 0.5f) { - // climb at up to WPNAV_SPEED_UP - climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_default_speed_up(); + // check if the message's thrust field should be interpreted as a climb rate or as thrust + const bool use_thrust = copter.g2.dev_options.get() & DevOptionSetAttitudeTarget_ThrustAsThrust; + + float climb_rate_or_thrust; + if (use_thrust) { + // interpret thrust as thrust + climb_rate_or_thrust = constrain_float(packet.thrust, -1.0f, 1.0f); } else { - // descend at up to WPNAV_SPEED_DN - climb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_default_speed_down()); + // convert thrust to climb rate + packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f); + if (is_equal(packet.thrust, 0.5f)) { + climb_rate_or_thrust = 0.0f; + } else if (packet.thrust > 0.5f) { + // climb at up to WPNAV_SPEED_UP + climb_rate_or_thrust = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_default_speed_up(); + } else { + // descend at up to WPNAV_SPEED_DN + climb_rate_or_thrust = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_default_speed_down()); + } } // if the body_yaw_rate field is ignored, use the commanded yaw position @@ -1015,7 +1023,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) } copter.mode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), - climb_rate_cms, use_yaw_rate, packet.body_yaw_rate); + climb_rate_or_thrust, use_yaw_rate, packet.body_yaw_rate, use_thrust); break; } diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index a03cc36b2a..fbabda32e6 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -776,7 +776,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: DEV_OPTIONS // @DisplayName: Development options // @Description: Bitmask of developer options. The meanings of the bit fields in this parameter may vary at any time. Developers should check the source code for current meaning - // @Bitmask: 0:ADSBMavlinkProcessing,1:DevOptionVFR_HUDRelativeAlt + // @Bitmask: 0:ADSBMavlinkProcessing,1:DevOptionVFR_HUDRelativeAlt,2:SetAttitudeTarget_ThrustAsThrust // @User: Advanced AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index db770dc626..4a901fb318 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -141,6 +141,7 @@ enum PayloadPlaceStateType { enum DevOptions { DevOptionADSBMAVLink = 1, DevOptionVFR_HUDRelativeAlt = 2, + DevOptionSetAttitudeTarget_ThrustAsThrust = 4, }; // Logging parameters diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index b04ad94647..0fc8ad549b 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -791,7 +791,7 @@ public: bool requires_terrain_failsafe() const override { return true; } - void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); + void set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust); 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; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 3fd01eacf6..a28f5d602e 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -24,8 +24,10 @@ struct { float pitch_cd; float yaw_cd; float yaw_rate_cds; - float climb_rate_cms; + float climb_rate_cms; // climb rate in cms. Used if use_thrust is false + float thrust; // thrust from -1 to 1. Used if use_thrust is true bool use_yaw_rate; + bool use_thrust; } static guided_angle_state; struct Guided_Limit { @@ -310,8 +312,8 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto return true; } -// set guided mode angle target -void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads) +// 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) { // check we are in velocity control mode if (guided_mode != Guided_Angle) { @@ -326,18 +328,26 @@ void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_y guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f; guided_angle_state.use_yaw_rate = use_yaw_rate; - guided_angle_state.climb_rate_cms = climb_rate_cms; + guided_angle_state.use_thrust = use_thrust; + if (use_thrust) { + guided_angle_state.thrust = climb_rate_cms_or_thrust; + guided_angle_state.climb_rate_cms = 0.0f; + } else { + guided_angle_state.thrust = 0.0f; + guided_angle_state.climb_rate_cms = climb_rate_cms_or_thrust; + } + guided_angle_state.update_time_ms = millis(); - // interpret positive climb rate as triggering take-off - if (motors->armed() && !copter.ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) { + // interpret positive climb rate or thrust as triggering take-off + if (motors->armed() && !copter.ap.auto_armed && is_positive(climb_rate_cms_or_thrust)) { copter.set_auto_armed(true); } // 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, guided_angle_state.climb_rate_cms)); + Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust)); } // guided_run - runs the guided controller @@ -567,11 +577,14 @@ void ModeGuided::angle_control_run() float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds); - // constrain climb rate - float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up()); + float climb_rate_cms = 0.0f; + if (!guided_angle_state.use_thrust) { + // constrain climb rate + climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up()); - // get avoidance adjusted climb rate - climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms); + // get avoidance adjusted climb rate + climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms); + } // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds uint32_t tnow = millis(); @@ -580,6 +593,7 @@ 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 not armed set throttle to zero and exit immediately @@ -611,8 +625,12 @@ void ModeGuided::angle_control_run() } // call position controller - pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false); - pos_control->update_z_controller(); + if (guided_angle_state.use_thrust) { + attitude_control->set_throttle_out(guided_angle_state.thrust, true, copter.g.throttle_filt); + } else { + pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false); + pos_control->update_z_controller(); + } } // helper function to update position controller's desired velocity while respecting acceleration limits