diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 917430afd50774..75a9e89570573c 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1,6 +1,7 @@ #pragma once #include "Copter.h" +#include #include class Parameters; class ParametersG2; @@ -1587,6 +1588,8 @@ class ModeSystemId : public Mode { DISTURB_VEL_LONG = 17, // longitudinal body axis measured velocity is being excited INPUT_LOITER_LAT = 18, // lateral body axis commanded velocity is being excited INPUT_LOITER_LONG = 19, // longitudinal body axis commanded velocity is being excited + LAT_REPOSITION = 20, // lateral body axis commanded velocity is being excited + DEPART_ABORT = 21, // longitudinal body axis commanded velocity is being excited }; AP_Int8 axis; // Controls which axis are being excited. Set to non-zero to display other parameters @@ -1596,6 +1599,8 @@ class ModeSystemId : public Mode { AP_Float time_fade_in; // Time to reach maximum amplitude of chirp AP_Float time_record; // Time taken to complete the chirp waveform AP_Float time_fade_out; // Time to reach zero amplitude after chirp finishes + AP_Float time_transit; // Time to complete MTE + AP_Float accel_max; // Max Accel used in MTE calculations bool att_bf_feedforward; // Setting of attitude_control->get_bf_feedforward float waveform_time; // Time reference for waveform @@ -1604,7 +1609,10 @@ class ModeSystemId : public Mode { float time_const_freq; // Time at constant frequency before chirp starts int8_t log_subsample; // Subsample multiple for logging. Vector2f target_vel; // target velocity for position controller modes - + Vector2f _target_pos; // target positon + Vector2f input_vel_last; // last cycle input velocity + float mte_heading; + // System ID states enum class SystemIDModeState { SYSTEMID_STATE_STOPPED, diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 6e98c17dd8aa1c..2c0f24e455560c 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -61,6 +61,22 @@ const AP_Param::GroupInfo ModeSystemId::var_info[] = { // @User: Standard AP_GROUPINFO("_T_FADE_OUT", 7, ModeSystemId, time_fade_out, 2), + // @Param: _T_TRANSIT + // @DisplayName: Transit Time for Lateral Reposition or Depart-Abort Axis types + // @Description: Time taken to complete the MTE + // @Range: 0 255 + // @Units: s + // @User: Standard + AP_GROUPINFO("_T_TRANSIT", 8, ModeSystemId, time_transit, 5.2), + + // @Param: _ACCEL_MAX + // @DisplayName: Maximum Acceleration used in Lateral Reposition and Depart Abort MTE calculations + // @Description: Maximum Acceleration used in Lateral Reposition and Depart Abort MTE calculations + // @Range: 0 10 + // @Units: m/s + // @User: Standard + AP_GROUPINFO("_ACCEL_MAX", 9, ModeSystemId, accel_max, 3.5), + AP_GROUPEND }; @@ -87,7 +103,8 @@ bool ModeSystemId::init(bool ignore_checks) if ((AxisType)axis.get() != AxisType::DISTURB_POS_LAT && (AxisType)axis.get() != AxisType::DISTURB_POS_LONG && (AxisType)axis.get() != AxisType::DISTURB_VEL_LAT && (AxisType)axis.get() != AxisType::DISTURB_VEL_LONG - && (AxisType)axis.get() != AxisType::INPUT_LOITER_LAT && (AxisType)axis.get() != AxisType::INPUT_LOITER_LONG) { + && (AxisType)axis.get() != AxisType::INPUT_LOITER_LAT && (AxisType)axis.get() != AxisType::INPUT_LOITER_LONG + && (AxisType)axis.get() != AxisType::LAT_REPOSITION && (AxisType)axis.get() != AxisType::DEPART_ABORT) { // System ID is being done on the attitude control loops @@ -126,6 +143,13 @@ bool ModeSystemId::init(bool ignore_checks) if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } + Vector3f target_pos; + target_pos = inertial_nav.get_position_neu_cm(); + _target_pos.x = target_pos.x; + _target_pos.y = target_pos.y; + + mte_heading = attitude_control->get_att_target_euler_rad().z; + } att_bf_feedforward = attitude_control->get_bf_feedforward(); @@ -162,7 +186,8 @@ void ModeSystemId::run() if ((AxisType)axis.get() != AxisType::DISTURB_POS_LAT && (AxisType)axis.get() != AxisType::DISTURB_POS_LONG && (AxisType)axis.get() != AxisType::DISTURB_VEL_LAT && (AxisType)axis.get() != AxisType::DISTURB_VEL_LONG - && (AxisType)axis.get() != AxisType::INPUT_LOITER_LAT && (AxisType)axis.get() != AxisType::INPUT_LOITER_LONG) { + && (AxisType)axis.get() != AxisType::INPUT_LOITER_LAT && (AxisType)axis.get() != AxisType::INPUT_LOITER_LONG + && (AxisType)axis.get() != AxisType::LAT_REPOSITION && (AxisType)axis.get() != AxisType::DEPART_ABORT) { // apply simple mode transform to pilot inputs update_simple_mode(); @@ -234,6 +259,19 @@ void ModeSystemId::run() waveform_sample = chirp_input.update(waveform_time - SYSTEM_ID_DELAY, waveform_magnitude); waveform_freq_rads = chirp_input.get_frequency_rads(); Vector2f disturb_state; + float mte_vel = 0.0f; + float mte_vel_const = 0.0f; + if (waveform_time < SYSTEM_ID_DELAY || waveform_time > time_transit + SYSTEM_ID_DELAY) { + mte_vel = 0.0f; + } else if (waveform_time >= SYSTEM_ID_DELAY && waveform_time <= SYSTEM_ID_DELAY + time_transit) { + float time_calc = waveform_time - SYSTEM_ID_DELAY; + if (waveform_time >= SYSTEM_ID_DELAY + 0.5f * time_transit) { + time_calc = SYSTEM_ID_DELAY + 0.5f * time_transit -waveform_time; + mte_vel_const = accel_max * time_transit * sinf(M_2PI) / (2.0f * M_2PI) - accel_max * 0.5f * time_transit; + } + mte_vel = accel_max * time_transit * sinf(2.0f * M_2PI * time_calc / time_transit) / (2.0f * M_2PI) - accel_max * time_calc + mte_vel_const; + } + switch (systemid_state) { case SystemIDModeState::SYSTEMID_STATE_STOPPED: attitude_control->bf_feedforward(att_bf_feedforward); @@ -337,13 +375,24 @@ void ModeSystemId::run() input_vel.y = 0.0f; input_vel.rotate(attitude_control->get_att_target_euler_rad().z); break; + case AxisType::LAT_REPOSITION: + input_vel.x = 0.0f; + input_vel.y = mte_vel * 100.0f; + input_vel.rotate(mte_heading); + break; + case AxisType::DEPART_ABORT: + input_vel.x = mte_vel * 100.0f; + input_vel.y = 0.0f; + input_vel.rotate(mte_heading); + break; } break; } if ((AxisType)axis.get() != AxisType::DISTURB_POS_LAT && (AxisType)axis.get() != AxisType::DISTURB_POS_LONG && (AxisType)axis.get() != AxisType::DISTURB_VEL_LAT && (AxisType)axis.get() != AxisType::DISTURB_VEL_LONG - && (AxisType)axis.get() != AxisType::INPUT_LOITER_LAT && (AxisType)axis.get() != AxisType::INPUT_LOITER_LONG) { + && (AxisType)axis.get() != AxisType::INPUT_LOITER_LAT && (AxisType)axis.get() != AxisType::INPUT_LOITER_LONG + && (AxisType)axis.get() != AxisType::LAT_REPOSITION && (AxisType)axis.get() != AxisType::DEPART_ABORT) { // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); @@ -362,13 +411,23 @@ void ModeSystemId::run() } Vector2f accel; - pos_control->input_vel_accel_xy(input_vel, accel); + _target_pos += input_vel * G_Dt; + if (is_positive(G_Dt)) { + accel = (input_vel - input_vel_last) / G_Dt; + input_vel_last = input_vel; + } + pos_control->set_pos_vel_accel_xy(_target_pos.topostype(), input_vel, accel); // run pos controller pos_control->update_xy_controller(); - // call attitude controller - attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate, false); + if ((AxisType)axis.get() == AxisType::LAT_REPOSITION || (AxisType)axis.get() == AxisType::DEPART_ABORT) { + // call attitude controller and specify heading to hold + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), ToDeg(mte_heading) * 100.0f); + } else { + // call attitude controller + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate, false); + } // Send the commanded climb rate to the position controller pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); @@ -413,7 +472,9 @@ void ModeSystemId::log_data() const if ((AxisType)axis.get() == AxisType::DISTURB_POS_LAT || (AxisType)axis.get() == AxisType::DISTURB_POS_LONG || (AxisType)axis.get() == AxisType::DISTURB_VEL_LAT || (AxisType)axis.get() == AxisType::DISTURB_VEL_LONG - || (AxisType)axis.get() == AxisType::INPUT_LOITER_LAT || (AxisType)axis.get() == AxisType::INPUT_LOITER_LONG) { + || (AxisType)axis.get() == AxisType::INPUT_LOITER_LAT || (AxisType)axis.get() == AxisType::INPUT_LOITER_LONG + || (AxisType)axis.get() == AxisType::LAT_REPOSITION || (AxisType)axis.get() == AxisType::DEPART_ABORT) { + pos_control->write_log(); copter.logger.Write_PID(LOG_PIDN_MSG, pos_control->get_vel_xy_pid().get_pid_info_x()); copter.logger.Write_PID(LOG_PIDE_MSG, pos_control->get_vel_xy_pid().get_pid_info_y());