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
bnsgeyer committed Feb 15, 2024
1 parent e855516 commit 1d25b27
Show file tree
Hide file tree
Showing 2 changed files with 107 additions and 2 deletions.
13 changes: 13 additions & 0 deletions ArduCopter/mode.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1589,6 +1589,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 @@ -1598,6 +1600,9 @@ 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
AP_Float hdg_transit;

bool att_bf_feedforward; // Setting of attitude_control->get_bf_feedforward
float waveform_time; // Time reference for waveform
Expand All @@ -1608,6 +1613,14 @@ class ModeSystemId : public Mode {
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;

// Lateral Reposition times
float mte_init_time; // time used to stabilize hover and turn to heading
float mte_end_first_time; // time used to stabilize hover and turn to heading
float mte_start_second_time; // time used to stabilize hover and turn to heading
float mte_end_second_time; // time used to stabilize hover and turn to heading

// System ID states
enum class SystemIDModeState {
SYSTEMID_STATE_STOPPED,
Expand Down
96 changes: 94 additions & 2 deletions ArduCopter/mode_systemid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,30 @@ 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),

// @Param: _HDG_TRANSIT
// @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("_HDG_TRANSIT", 10, ModeSystemId, hdg_transit, 0.0),

AP_GROUPEND
};

Expand Down Expand Up @@ -131,6 +155,20 @@ bool ModeSystemId::init(bool ignore_checks)
curr_pos = inertial_nav.get_position_neu_cm();
target_pos.x = curr_pos.x;
target_pos.y = curr_pos.y;

mte_heading = hdg_transit;

if ((AxisType)axis.get() == AxisType::LAT_REPOSITION) {
mte_init_time = 10.0f;
mte_end_first_time = 10.0f + time_transit;
mte_start_second_time = 20.0f + time_transit;
mte_end_second_time = 20.0f + 2.0f * time_transit;
} else if ((AxisType)axis.get() == AxisType::DEPART_ABORT) {
mte_init_time = 10.0f;
mte_end_first_time = 10.0f + time_transit;
mte_start_second_time = 40.0f + time_transit; // give extra time to turn around
mte_end_second_time = 40.0f + 2.0f * time_transit;
}
}

att_bf_feedforward = attitude_control->get_bf_feedforward();
Expand Down Expand Up @@ -237,6 +275,10 @@ 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;
float time_calc = 0.0f;

switch (systemid_state) {
case SystemIDModeState::SYSTEMID_STATE_STOPPED:
attitude_control->bf_feedforward(att_bf_feedforward);
Expand Down Expand Up @@ -340,6 +382,51 @@ void ModeSystemId::run()
input_vel.y = 0.0f;
input_vel.rotate(attitude_control->get_att_target_euler_rad().z);
break;
case AxisType::LAT_REPOSITION:
if (waveform_time < mte_init_time || (waveform_time >= mte_end_first_time && waveform_time < mte_start_second_time) || waveform_time > mte_end_second_time) {
mte_vel = 0.0f;
} else if (waveform_time >= mte_init_time && waveform_time < mte_end_first_time) {
time_calc = waveform_time - mte_init_time;
if (waveform_time >= mte_init_time + 0.5f * time_transit) {
time_calc = mte_init_time + 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;
} else if (waveform_time >= mte_start_second_time && waveform_time <= mte_end_second_time) {
time_calc = waveform_time - mte_start_second_time;
if (waveform_time >= mte_start_second_time + 0.5f * time_transit) {
time_calc = mte_start_second_time + 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;
}
input_vel.x = 0.0f;
input_vel.y = mte_vel * 100.0f;
input_vel.rotate(ToRad(mte_heading));
break;
case AxisType::DEPART_ABORT:
if (waveform_time < mte_init_time || (waveform_time >= mte_end_first_time && waveform_time < mte_start_second_time) || waveform_time > mte_end_second_time) {
mte_vel = 0.0f;
} else if (waveform_time >= mte_init_time && waveform_time < mte_end_first_time) {
time_calc = waveform_time - mte_init_time;
if (waveform_time >= mte_init_time + 0.5f * time_transit) {
time_calc = mte_init_time + 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;
} else if (waveform_time >= mte_start_second_time && waveform_time <= mte_end_second_time) {
time_calc = waveform_time - mte_start_second_time;
if (waveform_time >= mte_start_second_time + 0.5f * time_transit) {
time_calc = mte_start_second_time + 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;
}
if (waveform_time >= mte_end_first_time + 10.0f) { mte_heading = hdg_transit + 180.0f; }
input_vel.x = mte_vel * 100.0f;
input_vel.y = 0.0f;
input_vel.rotate(ToRad(mte_heading));
break;
}
break;
}
Expand Down Expand Up @@ -373,8 +460,13 @@ void ModeSystemId::run()
// 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(), 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

0 comments on commit 1d25b27

Please sign in to comment.