Skip to content

Commit

Permalink
Copter: add MTEs into system ID mode
Browse files Browse the repository at this point in the history
  • Loading branch information
bnsgeyer committed Feb 9, 2024
1 parent 940743e commit dad5306
Show file tree
Hide file tree
Showing 2 changed files with 77 additions and 8 deletions.
10 changes: 9 additions & 1 deletion ArduCopter/mode.h
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#pragma once

#include "Copter.h"
#include <AP_Math/control.h>
#include <AP_Math/chirp.h>
class Parameters;
class ParametersG2;
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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,
Expand Down
75 changes: 68 additions & 7 deletions ArduCopter/mode_systemid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
};

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

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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());
Expand Down

0 comments on commit dad5306

Please sign in to comment.