Copter: dev option so set-attitude-target thrust field used as thrust

This commit is contained in:
Randy Mackay 2020-07-27 18:18:34 +09:00
parent 8ae34a1977
commit 30c8d7bf40
6 changed files with 54 additions and 27 deletions

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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),

View File

@ -141,6 +141,7 @@ enum PayloadPlaceStateType {
enum DevOptions {
DevOptionADSBMAVLink = 1,
DevOptionVFR_HUDRelativeAlt = 2,
DevOptionSetAttitudeTarget_ThrustAsThrust = 4,
};
// Logging parameters

View File

@ -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;

View File

@ -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